/* ****************************************************************************** * INTERPEAK SOURCE FILE * * Document no: @(#) $Name: VXWORKS_ITER40_2019102207 $ $RCSfile: ipmip4_pkt.c,v $ $Revision: 1.28 $ * $Source: /home/interpeak/CVSRoot/ipmip4/src/ipmip4_pkt.c,v $ * $Author: ulf $ * $State: Exp $ $Locker: $ * * INTERPEAK_COPYRIGHT_STRING * Design and implementation by Joel Hortelius ****************************************************************************** */ /* **************************************************************************** * 1 DESCRIPTION **************************************************************************** */ /* **************************************************************************** * 2 CONFIGURATION **************************************************************************** */ #include "ipmip4_config.h" /* **************************************************************************** * 3 INCLUDE FILES **************************************************************************** */ #define IPCOM_USE_CLIB_PROTO #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "ipmip4_h.h" #include #include #include #include /* **************************************************************************** * 4 DEFINES **************************************************************************** */ #define IPMIP4_PKT_CLEN 128 #define IPMIP4_PKT_VLEN 512 /* **************************************************************************** * 5 TYPES **************************************************************************** */ /* **************************************************************************** * 6 EXTERN PROTOTYPES **************************************************************************** */ /* **************************************************************************** * 7 LOCAL PROTOTYPES **************************************************************************** */ /* **************************************************************************** * 8 DATA **************************************************************************** */ static Ip_u16 ipmip4_pkt_ipid = 1; /* **************************************************************************** * 9 STATIC FUNCTIONS **************************************************************************** */ /* *=========================================================================== * ipmip4_pkt_udp_recv *=========================================================================== * Description: Receive an UDP (MIP packet). * Parameters: ctx - The context * fd - File descriptor * Returns: * */ IP_STATIC void ipmip4_pkt_udp_recv(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ipmip4_pkt_udp_register_t *udpr) { Ipmip4_pktbuf_t *pbuf = (Ipmip4_pktbuf_t *)ipmip_inet_udp_recvmsg(&ctx->ipmip, proc, IPMIP4_PKT_VLEN, udpr->fd); /* Invoke callback */ if (pbuf) (*udpr->udp_callback) (ctx, proc, pbuf, udpr->opaque); } /* *=========================================================================== * ipmip4_pkt_icmp_recv *=========================================================================== * Description: Receive ICMP agent advertisement/solicitation * Parameters: ctx - The packet context * fd - The file descriptor * Returns: * */ IP_STATIC void ipmip4_pkt_icmp_recv(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ip_fd fd) { struct Ip_cmsghdr *cmsghdr; int len; /* Destination Option & Packet Info */ struct Ip_msghdr msg; struct Ip_iovec vec; Ipmip4_pktbuf_t *pbuf = IP_NULL; struct Ip_sockaddr_in src; /* Allocate pktbuf */ pbuf = (Ipmip4_pktbuf_t *)ipmip_pktbuf_alloc(&ctx->ipmip, proc, IPMIP4_PKT_VLEN + IPMIP4_PKT_CLEN); /* Failed to allocate buffer? */ if (pbuf == IP_NULL) return; /* Initialize vector */ vec.iov_base = IPMIP_BUFFER_DATA(&pbuf->ipmip.packet); vec.iov_len = IPMIP4_PKT_VLEN; /* Initialize name */ msg.msg_name = &src; msg.msg_namelen = sizeof(src); /* Initialize io */ msg.msg_iov = &vec; msg.msg_iovlen = 1; /* Initialize control */ msg.msg_control = ipmip_buffer_offset(&pbuf->ipmip.packet, IPMIP4_PKT_VLEN); msg.msg_controllen = IPMIP4_PKT_CLEN; /* Clear flags */ msg.msg_flags = 0; /* Read and process a Mobile Header message */ if ((len = ipcom_recvmsg(fd, &msg, 0)) <= 0) { /* Failure */ IPCOM_LOG1(ERR, "Failed to read ADV message error=%u", ipcom_errno); ipmip_util_sock_clear(fd); goto error_out; } /* We've gotten something */ if ((msg.msg_flags & (IP_MSG_TRUNC | IP_MSG_CTRUNC)) != 0) { /* Failure */ IPCOM_LOG0(ERR, "ADV TRUNC or CTRUNC set, not all data read. Discarding"); goto error_out; } /* Store value */ ipmip_util_dup(&pbuf->ipmip.srcinfo.src, &src, sizeof(src)); /* Move past the read length */ ipmip_buffer_append(&pbuf->ipmip.packet, IP_NULL, len); /* Did we get any ancillary data? */ if (msg.msg_controllen > 0) { /* Loop the ancillary data, see what we find */ for (cmsghdr = IP_CMSG_FIRSTHDR(&msg); cmsghdr != 0; cmsghdr = IP_CMSG_NXTHDR(&msg, cmsghdr)) { /* Correct level? */ if (cmsghdr->cmsg_level == IP_IPPROTO_IP) { /* Type? */ switch (cmsghdr->cmsg_type) { case IP_IP_X_SENDERLINKADDR: { struct Ip_sockaddr_dl *dl = IP_CMSG_DATA(cmsghdr); ipmip_util_dup(&pbuf->ipmip.srcinfo.dl, dl, IPCOM_SA_LEN_GET(dl)); } break; case IP_IP_PKTINFO: { struct Ip_in_pktinfo *pktinfo = IP_CMSG_DATA(cmsghdr); ipmip_pktbuf_srcinfo_dst_inet(&pbuf->ipmip, pktinfo); } break; case IP_IP_TTL: { int *ttl = IP_CMSG_DATA(cmsghdr); pbuf->ipmip.srcinfo.ttl = IP_GET_32ON8(ttl); } break; default: break; } } } } if (pbuf->ipmip.srcinfo.dst == IP_NULL) { /* Failure */ IPCOM_LOG0(ERR, "NO PKTINFO"); goto error_out; } pbuf->ipmip.srcinfo.lladdr = ipmip_lladdr_dl_to_lladdr(pbuf->ipmip.srcinfo.dl, pbuf->ipmip.srcinfo.dst->ifindex); /* Time to remove the IP header */ { Ipcom_ip_header *iph = (Ipcom_ip_header *)IPMIP_BUFFER_DATA(&pbuf->ipmip.packet); int ip_hdr_len = (int)((iph->vers_len & 0x0F) << 2); if (ip_hdr_len < IPCOM_IP_HEADER_LENGTH) goto error_out; if (IP_GET_NTOHS(&iph->length) > IPMIP_BUFFER_SIZE(&pbuf->ipmip.packet)) goto error_out; /* Won't do, we're looking for ICMP's here */ if (IPMIP_BUFFER_SIZE(&pbuf->ipmip.packet) < (unsigned) (ip_hdr_len + IPCOM_ICMP_HEADER_LENGTH)) goto error_out; /* */ ipmip_buffer_delete_header(&pbuf->ipmip.packet, ip_hdr_len); } (*ctx->pkt->icmp) (ctx, proc, pbuf); return; error_out: ipmip_pktbuf_release(&ctx->ipmip, proc, &pbuf->ipmip); } /* *=========================================================================== * ipmip4_pkt_packet_recv *=========================================================================== * Description: Receive a RAW packet * Parameters: ctx - The context * fd - The file descriptor * Returns: * */ IP_STATIC void ipmip4_pkt_packet_recv(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ip_fd fd) { struct Ip_cmsghdr *cmsghdr; int len; /* Destination Option & Packet Info */ struct Ip_msghdr msg; struct Ip_iovec vec; Ipmip4_pktbuf_t *pbuf = IP_NULL; struct Ip_sockaddr_ll ll; /* Allocate pktbuf */ pbuf = (Ipmip4_pktbuf_t *)ipmip_pktbuf_alloc(&ctx->ipmip, proc, IPMIP4_PKT_VLEN + IPMIP4_PKT_CLEN); /* Failed to allocate buffer? */ if (pbuf == IP_NULL) return; /* Initialize vector */ vec.iov_base = IPMIP_BUFFER_DATA(&pbuf->ipmip.packet); vec.iov_len = IPMIP4_PKT_VLEN; /* Initialize name */ msg.msg_name = ≪ msg.msg_namelen = sizeof(struct Ip_sockaddr_ll); /* Initialize io */ msg.msg_iov = &vec; msg.msg_iovlen = 1; /* Initialize control */ msg.msg_control = ipmip_buffer_offset(&pbuf->ipmip.packet, IPMIP4_PKT_VLEN); msg.msg_controllen = IPMIP4_PKT_CLEN; /* Clear flags */ msg.msg_flags = 0; /* Read and process a Mobile Header message */ if ((len = ipcom_recvmsg(fd, &msg, 0)) <= 0) { /* Failure */ IPCOM_LOG1(ERR, "Failed to read ADV message error=%u", ipcom_errno); ipmip_util_sock_clear(fd); goto error_out; } /* We've gotten something */ if ((msg.msg_flags & (IP_MSG_TRUNC | IP_MSG_CTRUNC)) != 0) { /* Failure */ IPCOM_LOG0(ERR, "ADV TRUNC or CTRUNC set, not all data read. Discarding"); goto error_out; } /* Store value */ ipmip_util_dup(&pbuf->ipmip.srcinfo.ll, &ll, sizeof(ll)); /* Move past the read length */ ipmip_buffer_append(&pbuf->ipmip.packet, IP_NULL, len); /* Did we get any ancillary data? */ if (msg.msg_controllen > 0) { /* Loop the ancillary data, see what we find */ for (cmsghdr = IP_CMSG_FIRSTHDR(&msg); cmsghdr != 0; cmsghdr = IP_CMSG_NXTHDR(&msg, cmsghdr)) { /* Correct level? */ if (cmsghdr->cmsg_level == IP_IPPROTO_IP) { /* Type? */ switch (cmsghdr->cmsg_type) { case IP_IP_X_SENDERLINKADDR: { struct Ip_sockaddr_dl *dl = IP_CMSG_DATA(cmsghdr); ipmip_util_dup(&pbuf->ipmip.srcinfo.dl, dl, IPCOM_SA_LEN_GET(dl)); } break; case IP_IP_PKTINFO: { struct Ip_in_pktinfo *pktinfo = IP_CMSG_DATA(cmsghdr); ipmip_pktbuf_srcinfo_dst_inet(&pbuf->ipmip, pktinfo); } break; default: break; } } } } /**/ pbuf->ipmip.srcinfo.lladdr = ipmip_lladdr_dl_to_lladdr(pbuf->ipmip.srcinfo.dl, pbuf->ipmip.srcinfo.dst? pbuf->ipmip.srcinfo.dst->ifindex : 0); (*ctx->pkt->packet) (ctx, proc, pbuf); return; error_out: ipmip_pktbuf_release(&ctx->ipmip, proc, &pbuf->ipmip); } /* *=========================================================================== * ipmip4_pkt_loop *=========================================================================== * Description: * */ IP_STATIC void ipmip4_pkt_loop(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ipmip4_pkt_context_t *pkt) { IPMIP_UNUSED_PARAMETER(pkt); /*********** tick loop ***********/ for (; proc->quit != IP_TRUE;) { int maxfd = 0; Ip_fd_set rset; int ret; struct Ip_timeval tv; Ipmip4_pkt_udp_register_t *udpr; int num_socks = 0; /* Clear it */ IP_FD_ZERO(&rset); /* Register socket */ for (udpr = IPCOM_LIST_FIRST(&pkt->udp_sockets); udpr != IP_NULL; udpr = IPCOM_LIST_NEXT(&udpr->next)) { if (udpr->fd != IP_INVALID_SOCKET) { maxfd = IP_MAX(maxfd, udpr->fd); IP_FD_SET(udpr->fd, &rset); num_socks++; } } if (ctx->pkt->fd_icmp_in != IP_INVALID_SOCKET) { maxfd = IP_MAX(maxfd, ctx->pkt->fd_icmp_in); IP_FD_SET(ctx->pkt->fd_icmp_in, &rset); num_socks++; } /* If we've request inbound packet; add it */ if (IP_BIT_ISTRUE(ctx->pkt->flags, IPMIP4_PKT_PKT_IN)) { if (ctx->pkt->fd_pkt != IP_INVALID_SOCKET) { maxfd = IP_MAX(maxfd, ctx->pkt->fd_pkt); IP_FD_SET(ctx->pkt->fd_pkt, &rset); num_socks++; } } if (ctx->ipmip.arp != IP_NULL && ctx->ipmip.arp->fd_arp_in != IP_INVALID_SOCKET) { maxfd = IP_MAX(maxfd, ctx->ipmip.arp->fd_arp_in); IP_FD_SET(ctx->ipmip.arp->fd_arp_in, &rset); num_socks++; } if (ctx->ipmip.dhcp != IP_NULL && ctx->ipmip.dhcp->fd_udp != IP_INVALID_SOCKET) { maxfd = IP_MAX(maxfd, ctx->ipmip.dhcp->fd_udp); IP_FD_SET(ctx->ipmip.dhcp->fd_udp, &rset); num_socks++; } if (num_socks == 0) { /* Sleep */ ipcom_millisleep(200); } else { tv.tv_sec = 0; tv.tv_usec = 200 * 1000; ret = ipcom_socketselect(maxfd + 1, &rset, IP_NULL, IP_NULL, &tv); /* Timeout */ if (ret == 0) continue; else if (ret < 0) { IPCOM_LOG2(WARNING, "packet context socketselect() failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); } else { for (udpr = IPCOM_LIST_FIRST(&pkt->udp_sockets); udpr != IP_NULL; udpr = IPCOM_LIST_NEXT(&udpr->next)) { if (udpr->fd != IP_INVALID_SOCKET && IP_FD_ISSET(udpr->fd, &rset)) ipmip4_pkt_udp_recv(ctx, proc, udpr); } /* Advert socket input */ if (ctx->pkt->fd_icmp_in != IP_INVALID_SOCKET && IP_FD_ISSET(ctx->pkt->fd_icmp_in, &rset)) ipmip4_pkt_icmp_recv(ctx, proc, ctx->pkt->fd_icmp_in); /* If we've request inbound packet; add it */ if (IP_BIT_ISTRUE(ctx->pkt->flags, IPMIP4_PKT_PKT_IN)) { /* Advert socket input */ if (IP_FD_ISSET(ctx->pkt->fd_pkt, &rset)) ipmip4_pkt_packet_recv(ctx, proc, ctx->pkt->fd_pkt); } if (ctx->ipmip.arp != IP_NULL && ctx->ipmip.arp->fd_arp_in != IP_INVALID_SOCKET && IP_FD_ISSET(ctx->ipmip.arp->fd_arp_in, &rset)) ipmip_inet_arp_process(&ctx->ipmip, proc); if (ctx->ipmip.dhcp != IP_NULL && ctx->ipmip.dhcp->fd_udp != IP_INVALID_SOCKET && IP_FD_ISSET(ctx->ipmip.dhcp->fd_udp, &rset)) ipmip_dhcp_process(&ctx->ipmip, proc); } } } } /* **************************************************************************** * 10 GLOBAL FUNCTIONS **************************************************************************** */ IP_GLOBAL void ipmip4_pkt_udp_register(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ipmip4_pkt_udp_register_t *udpr) { IPMIP_UNUSED_PARAMETER(proc); ipcom_list_insert_last(&ctx->pkt->udp_sockets, &udpr->next); } IP_GLOBAL void ipmip4_pkt_udp_unregister(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ipmip4_pkt_udp_register_t *udpr) { IPMIP_UNUSED_PARAMETER(ctx); IPMIP_UNUSED_PARAMETER(proc); if (udpr->next.head != IP_NULL) ipcom_list_remove(&udpr->next); } /* *=========================================================================== * ipmip4_pkt_icmp_write *=========================================================================== * Description: Write an ICMP message to a link local host. * Parameters: ctx - The context * proc - Process specific context * payload - The payload packet * from - The source * to - The destination * iff - Outgoing interface * lla - Link level address * Returns: IP_TRUE if successful. * */ IP_GLOBAL Ip_bool ipmip4_pkt_icmp_write(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ipmip_buffer_t *payload, Ip_u32 from, Ip_u32 to, Ipmip4_if_t *iff, Ipmip_lladdr_t *lladdr) { struct Ip_msghdr m; Ipcom_ip_header iph; struct Ip_sockaddr_in to_in; Ip_size_t tot_size; struct Ip_iovec iov[3]; int ioventry = 0; int ret = -1; Ip_u32 chk; if (!lladdr) return ipmip4_pkt_icmp_sendmsg(ctx, proc, payload, from, to, iff, 1); ipcom_memset(&m, 0, sizeof(m)); ipcom_memset(&iph, 0, sizeof(iph)); tot_size = IPMIP_BUFFER_SIZE(payload); tot_size += sizeof(iph); /* Setup the IP header */ iph.vers_len = 0x45; iph.length = (Ip_u16) tot_size; iph.id = ip_htons(ipmip4_pkt_ipid); iph.ttl = 1; iph.protocol = IP_IPPROTO_ICMP; iph.src.addr32 = from; if (to == 0) { /* Send to world wide broadcast */ iph.dst.addr32 = IP_INADDR_BROADCAST; } else { iph.dst.addr32 = to; } ipmip4_pkt_ipid++; /* Local; Force the binding of an interface and an LLA */ /**/ iph.length = ip_htons(iph.length); /* Got an LLA */ /* Only deal with MAC's right now */ if (lladdr->type == IPMIP_LLADDR_TYPE_MAC) { struct Ip_sockaddr_ll to_ll; IPCOM_SA_LEN_SET(&to_ll, sizeof(to_ll)); to_ll.sll_family = IP_AF_PACKET; to_ll.sll_protocol = ip_htons(0x800); to_ll.sll_ifindex = iff->ipmip.ifindex; to_ll.sll_halen = 6; ipcom_memcpy(to_ll.sll_addr, lladdr->lla, IPMIP_ETH_ADDR_SIZE); m.msg_name = &to_ll; m.msg_namelen = sizeof(to_ll); iov[ioventry].iov_base = &iph; iov[ioventry].iov_len = sizeof(iph); ioventry++; iov[ioventry].iov_base = IPMIP_BUFFER_DATA(payload); iov[ioventry].iov_len = IPMIP_BUFFER_SIZE(payload); ioventry++; /* Remote; do not bind this to anything */ m.msg_iov = iov; m.msg_iovlen = ioventry; /* Local; build IP & UDP header */ chk = ipcom_in_checksum_update(&iph, sizeof(iph)); iph.checksum = ipcom_in_checksum_finish(chk); /* Send */ if ((ret = ipcom_sendmsg(ctx->pkt->fd_pkt, &m, 0)) < 0) ipmip_util_sock_clear(ctx->pkt->fd_pkt); return ret >= 0; } else if (lladdr->type == IPMIP_LLADDR_TYPE_INET) { Ipcom_ip_header outer_iph; Ip_u32 rdst; /* Need to increase it by one to deal with encap */ iph.ttl = 2; /* Increase total size */ tot_size += sizeof(outer_iph); ipcom_memset(&outer_iph, 0, sizeof(outer_iph)); /* Get outer destination */ rdst = IP_GET_32ON8(lladdr->lla); /* Setup the IP header */ outer_iph.vers_len = 0x45; outer_iph.length = (Ip_u16) tot_size; outer_iph.id = ip_htons(ipmip4_pkt_ipid); outer_iph.ttl = 64; outer_iph.protocol = IP_IPPROTO_IPIP; outer_iph.src.addr32 = from; outer_iph.dst.addr32 = rdst; ipmip4_pkt_ipid++; iov[ioventry].iov_base = &outer_iph; iov[ioventry].iov_len = sizeof(outer_iph); ioventry++; iov[ioventry].iov_base = &iph; iov[ioventry].iov_len = sizeof(iph); ioventry++; iov[ioventry].iov_base = IPMIP_BUFFER_DATA(payload); iov[ioventry].iov_len = IPMIP_BUFFER_SIZE(payload); ioventry++; chk = ipcom_in_checksum_update(&iph, sizeof(iph)); iph.checksum = ipcom_in_checksum_finish(chk); /* Fall through and send */ /* */ ipcom_memset(&to_in, 0, sizeof(to_in)); to_in.sin_family = IP_AF_INET; IPCOM_SA_LEN_SET(&to_in, sizeof(to_in)); to_in.sin_addr.s_addr = rdst; m.msg_name = &to_in; m.msg_namelen = sizeof(to_in); /* Remote; do not bind this to anything */ m.msg_iov = iov; m.msg_iovlen = ioventry; /* Send advertisement. */ if ((ret = ipcom_sendmsg(ctx->pkt->fd_raw, &m, 0)) < 0) { IPCOM_LOG1(WARNING, " ipcom_sendmsg() returned %u", ipcom_errno); ipmip_util_sock_clear(ctx->pkt->fd_raw); } return ret >= 0; } return IP_FALSE; } /* *=========================================================================== * ipmip4_pkt_icmp_sendmsg *=========================================================================== * Description: Send an ICMP message * Parameters: ctx - The context * proc - Process context * payload - The payload packet * from - The source * to - The destination * iff - Outgoing interface * ttl - TTL * Returns: IP_TRUE if successful. * */ IP_GLOBAL Ip_bool ipmip4_pkt_icmp_sendmsg(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ipmip_buffer_t *payload, Ip_u32 from, Ip_u32 to, Ipmip4_if_t *iff, Ip_u8 ttl) { struct Ip_msghdr m; Ipcom_ip_header iph; struct Ip_sockaddr_in to_in; Ip_size_t tot_size; struct Ip_iovec iov[3]; int ioventry = 0; Ip_u8 cmsg_buf[IP_CMSG_SPACE(sizeof(struct Ip_in_pktinfo))]; int ret = -1; IPMIP_UNUSED_PARAMETER(proc); ipcom_memset(&m, 0, sizeof(m)); ipcom_memset(&iph, 0, sizeof(iph)); tot_size = IPMIP_BUFFER_SIZE(payload); tot_size += sizeof(iph); /* Setup the IP header */ iph.vers_len = 0x45; iph.length = (Ip_u16) tot_size; iph.id = ip_htons(ipmip4_pkt_ipid); iph.ttl = ttl; iph.protocol = IP_IPPROTO_ICMP; iph.src.addr32 = from; if (to == 0) { /* Send to world wide broadcast */ iph.dst.addr32 = IP_INADDR_BROADCAST; } else { iph.dst.addr32 = to; } ipmip4_pkt_ipid++; iov[ioventry].iov_base = &iph; iov[ioventry].iov_len = sizeof(iph); ioventry++; iov[ioventry].iov_base = IPMIP_BUFFER_DATA(payload); iov[ioventry].iov_len = IPMIP_BUFFER_SIZE(payload); ioventry++; /* Local but no LLA; bind to interface */ if (iff) { struct Ip_cmsghdr *c; struct Ip_in_pktinfo *pktinfo; /* Use ancillary data to specify outgoing interface. */ c = (struct Ip_cmsghdr *) cmsg_buf; c->cmsg_len = IP_CMSG_LEN(sizeof(struct Ip_in_pktinfo)); c->cmsg_level = IP_IPPROTO_IP; c->cmsg_type = IP_IP_PKTINFO; pktinfo = IP_CMSG_DATA(c); ipcom_memset(pktinfo, 0, sizeof(*pktinfo)); pktinfo->ipi_addr.s_addr = 0; pktinfo->ipi_ifindex = iff->ipmip.ifindex; m.msg_control = c; m.msg_controllen = sizeof(cmsg_buf); } /* */ ipcom_memset(&to_in, 0, sizeof(to_in)); to_in.sin_family = IP_AF_INET; IPCOM_SA_LEN_SET(&to_in, sizeof(to_in)); to_in.sin_addr.s_addr = iph.dst.addr32; m.msg_name = &to_in; m.msg_namelen = sizeof(to_in); /* Remote; do not bind this to anything */ m.msg_iov = iov; m.msg_iovlen = ioventry; /* Send advertisement. */ if ((ret = ipcom_sendmsg(ctx->pkt->fd_raw, &m, 0)) < 0) { IPCOM_LOG1(WARNING, " ipcom_sendmsg() returned %u", ipcom_errno); ipmip_util_sock_clear(ctx->pkt->fd_raw); } return ret >= 0; } /* *=========================================================================== * ipmip4_pkt_icmp_echo_request *=========================================================================== * Description: Send an echo request message * Parameters: ctx - Context * proc - Process context * from - Source IP * to - Destination IP * iff - Outgoing interface * Returns: IP_TRUE if successful. * */ IP_GLOBAL Ip_bool ipmip4_pkt_icmp_echo_request(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ip_u32 from, Ip_u32 to, Ipmip4_if_t *iff) { static Ip_u16 seq = 0; Ipmip_buffer_t echo; Ip_bool ret; struct { Ip_u8 icmp_type; Ip_u8 icmp_code; Ip_u16 icmp_checksum; /* Set to 0 then calc */ Ip_u16 icmp_id; Ip_u16 icmp_seq; } icmp; /* Do send icmp echo request */ icmp.icmp_type = 8; icmp.icmp_code = 0; icmp.icmp_checksum = 0; icmp.icmp_id = ip_htons(1973); icmp.icmp_seq = ip_htons(seq); /* Calculate the ICMP checksum */ icmp.icmp_checksum = ipcom_in_checksum(&icmp, sizeof(icmp)); /* Up the sequence */ seq++; ipmip_buffer_init(&echo); ipmip_buffer_append(&echo, &icmp, sizeof(icmp)); ret = ipmip4_pkt_icmp_sendmsg(ctx, proc, &echo, from, to, iff, 64); ipmip_buffer_release(&echo); /* Return result */ return ret; } /* *=========================================================================== * ipmip4_pkt_udp_sendmsg *=========================================================================== * Description: Send a UDP message to a remote host * Parameters: ctx - The context * proc - Process context * payload - The payload buffer * from - The source address * from_port - The source port in network byte order * to - The destination address * to_port - The destination port in network byte order * iff - If set; the interface to force the output on. * Returns: The resulting return code. * */ IP_GLOBAL int ipmip4_pkt_udp_sendmsg(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ipmip_buffer_t *payload, Ip_u32 from, Ip_u16 from_port, Ip_u32 to, Ip_u16 to_port, Ipmip4_if_t *iff) { return ipmip_inet_udp_sendmsg(&ctx->ipmip, proc, ctx->pkt->fd_raw, payload, from, from_port, to, to_port, &iff->ipmip) >= 0; } /* *=========================================================================== * ipmip4_pkt_udp_write *=========================================================================== * Description: Send a UDP message to a link local host with the * indicated MAC address. * Parameters: ctx - The context * proc - Process context * payload - The payload buffer * from - The source address * from_port - The source port in netwrok byte order * to - The destination address * to_port - The destination port in network byte order * iff - Outgoing interface * lla - MAC address * Returns: The resulting return code. * */ IP_GLOBAL int ipmip4_pkt_udp_write(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ipmip_buffer_t *payload, Ip_u32 from, Ip_u16 from_port, Ip_u32 to, Ip_u16 to_port, Ipmip4_if_t *iff, Ipmip_lladdr_t *lladdr) { return ipmip_inet_udp_write(&ctx->ipmip, proc, ctx->pkt->fd_pkt, ctx->pkt->fd_raw, payload, from, from_port, to, to_port, &iff->ipmip, lladdr) >= 0; } /* *=========================================================================== * ipmip4_pkt_arp_write *=========================================================================== * Description: Send an ARP message to specific destination * Parameters: ctx - The context * proc - Process context * from - From IP address * to - To IP address * iff - Outgoing interface * lla - LLA destination * Returns: * */ IP_GLOBAL void ipmip4_pkt_arp_write(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ip_u32 from, Ip_u32 to, Ipmip4_if_t *iff, Ipmip_lladdr_t *lladdr) { IPMIP_UNUSED_PARAMETER(proc); /* Only do MAC */ if (lladdr->type == IPMIP_LLADDR_TYPE_MAC && iff->ipmip.lladdr != IP_NULL && iff->ipmip.lladdr->type == lladdr->type) { struct Ip_iovec iov; struct Ip_msghdr m; Ipmip4_pkt_arp_t arp; struct Ip_sockaddr_ll to_ll; ipcom_memset(&m, 0, sizeof(m)); /* Set the base values */ arp.hrd = IPMIP_ARP_HRD_ETHER; arp.pro = IPMIP_ARP_PRO_IP; arp.hln_pln = IPMIP_ARP_HLN_PLN; arp.op = IPMIP_ARP_OP_REPLY; /* Setup the structures */ ipcom_memcpy(arp.eth_src, iff->ipmip.lladdr->lla, IPMIP_ETH_ADDR_SIZE); ipcom_memcpy(arp.eth_dst, lladdr->lla, IPMIP_ETH_ADDR_SIZE); IP_SET_32ON8(arp.ip_src, from); IP_SET_32ON8(arp.ip_dst, to); IPCOM_SA_LEN_SET(&to_ll, sizeof(to_ll)); to_ll.sll_family = IP_AF_PACKET; to_ll.sll_protocol = ip_htons(IPMIP_ETH_P_ARP); to_ll.sll_ifindex = iff->ipmip.ifindex; to_ll.sll_halen = IPMIP_ETH_ADDR_SIZE; ipcom_memcpy(to_ll.sll_addr, lladdr->lla, IPMIP_ETH_ADDR_SIZE); iov.iov_base = &arp; iov.iov_len = sizeof(arp); m.msg_iov = &iov; m.msg_iovlen = 1; m.msg_name = &to_ll; m.msg_namelen = sizeof(to_ll); /* */ if (ipcom_sendmsg(ctx->pkt->fd_pkt, &m, 0) < 0) ipmip_util_sock_clear(ctx->pkt->fd_pkt); } } /* *=========================================================================== * ipmip4_pkt_context_create *=========================================================================== * Description: Create the packet context * Parameters: ctx - The context * proc - Process context * flags - Flags what packet context features we need. * udp - UDP callback * icmp - ICMP callback * packet - Packet callback * proc_block - The process block * Returns: IP_TRUE on success * */ IP_GLOBAL Ip_bool ipmip4_pkt_context_create(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ip_u32 flags, Ipmip4_pkt_udp_cb_t udp, Ipmip4_pkt_icmp_cb_t icmp, Ipmip4_pkt_packet_cb_t packet) { /* Ipmip4_pkt_context_t *ctx = ipcom_calloc(1, sizeof(*ctx)); */ int i; ctx->pkt = ipcom_calloc(1, sizeof(*ctx->pkt)); if (ctx->pkt == IP_NULL) return IP_NULL; ipcom_list_init(&ctx->pkt->udp_sockets); /* Store the allocation flags */ ctx->pkt->flags = flags; ctx->pkt->icmp = icmp; ctx->pkt->packet = packet; ctx->pkt->fd_icmp_in = IP_INVALID_SOCKET; ctx->pkt->fd_pkt = IP_INVALID_SOCKET; ctx->pkt->fd_raw = IP_INVALID_SOCKET; ctx->pkt->udp_in.fd = IP_INVALID_SOCKET; ctx->pkt->udp_out.fd = IP_INVALID_SOCKET; if (udp) { ipcom_memset(&ctx->pkt->s_udp_in, 0, sizeof(ctx->pkt->s_udp_in)); ctx->pkt->s_udp_in.sin_family = IP_AF_INET; IPCOM_SA_LEN_SET(&ctx->pkt->s_udp_in, sizeof(ctx->pkt->s_udp_in)); if (IP_BIT_ISSET(ctx->pkt->flags, IPMIP4_PKT_UDP_IN_434)) ctx->pkt->s_udp_in.sin_port = ip_htons(IPMIP4_PORT); /* Create raw IP socket for all IPv4 input on this interface */ if ((ctx->pkt->udp_in.fd = ipmip_inet_udp_socket(&ctx->pkt->s_udp_in)) == IP_INVALID_SOCKET) { IPCOM_LOG2(ERR, "ipcom_socket(UDP/IN) failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); goto fail; } ctx->pkt->udp_in.opaque = IP_NULL; ctx->pkt->udp_in.udp_callback = udp; ipmip4_pkt_udp_register(ctx, proc, &ctx->pkt->udp_in); /* */ if (IP_BIT_ISTRUE(ctx->pkt->flags, IPMIP4_PKT_UDP_IN_OUT)) { /* Shutdown write */ ipcom_shutdown(ctx->pkt->udp_in.fd, IP_SHUT_WR); ipcom_memset(&ctx->pkt->s_udp_out, 0, sizeof(ctx->pkt->s_udp_out)); ctx->pkt->s_udp_out.sin_family = IP_AF_INET; IPCOM_SA_LEN_SET(&ctx->pkt->s_udp_out, sizeof(ctx->pkt->s_udp_out)); ctx->pkt->s_udp_out.sin_port = 0; if ((ctx->pkt->udp_out.fd = ipmip_inet_udp_socket(&ctx->pkt->s_udp_out)) == IP_INVALID_SOCKET) { IPCOM_LOG2(ERR, "ipcom_socket(UDP/IN) failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); goto fail; } ctx->pkt->udp_out.opaque = IP_NULL; ctx->pkt->udp_out.udp_callback = udp; ipmip4_pkt_udp_register(ctx, proc, &ctx->pkt->udp_out); /* Shutdown write */ ipcom_shutdown(ctx->pkt->udp_out.fd, IP_SHUT_WR); i = IP_UDP_NAT_T_RFC3519; if (ipcom_setsockopt(ctx->pkt->udp_out.fd, IP_IPPROTO_UDP, IP_UDP_NAT_T, &i, sizeof(i))) { IPCOM_LOG2(WARNING, "ipcom_setsockopt(UDP/OUT/IP_UDP_NAT_T) failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); goto fail; } } else { /* Allow NAT-T according to RFC3519 */ i = IP_UDP_NAT_T_RFC3519; if (ipcom_setsockopt(ctx->pkt->udp_in.fd, IP_IPPROTO_UDP, IP_UDP_NAT_T, &i, sizeof(i))) { IPCOM_LOG2(WARNING, "ipcom_setsockopt(UDP/OUT/IP_UDP_NAT_T) failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); goto fail; } ipcom_memcpy(&ctx->pkt->s_udp_out, &ctx->pkt->s_udp_in, sizeof(ctx->pkt->s_udp_out)); } } /* Create raw IP socket for all IPv4 input on this interface */ if ((ctx->pkt->fd_raw = ipcom_socket(IP_AF_INET, IP_SOCK_RAW, 0)) == IP_INVALID_SOCKET) { IPCOM_LOG2(ERR, "ipcom_socket(RAW/OUT) failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); goto fail; } /* Enable IP header include */ i = 1; if (ipcom_setsockopt(ctx->pkt->fd_raw, IP_IPPROTO_IP, IP_IP_HDRINCL, &i, sizeof(i))) { IPCOM_LOG2(ERR, "ipcom_setsockopt(RAW/HDRINCL) failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); goto fail; } /* Shutdown read */ ipcom_shutdown(ctx->pkt->fd_raw, IP_SHUT_RD); /* */ if (IP_BIT_ISTRUE(ctx->pkt->flags, IPMIP4_PKT_ICMP_IN)) { /* Create socket for sending ICMP ADV/SOL */ if ((ctx->pkt->fd_icmp_in = ipcom_socket(IP_AF_INET, IP_SOCK_RAW, IP_IPPROTO_ICMP)) == IP_INVALID_SOCKET) { IPCOM_LOG2(ERR, "ipcom_socket(ICMP/IN) failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); goto fail; } /* I want to receive PACKET INFO as ancillary data */ i = 1; if (ipcom_setsockopt(ctx->pkt->fd_icmp_in, IP_IPPROTO_IP, IP_IP_PKTINFO, &i, sizeof(i)) < 0) { IPCOM_LOG2(ERR, "ipcom_setsockopt(ICMP/IN/IP_IP_RECVPKTINFO) failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); goto fail; } i = 1; if (ipcom_setsockopt(ctx->pkt->fd_icmp_in, IP_IPPROTO_IP, IP_IP_RECVTTL, &i, sizeof(i)) < 0) { IPCOM_LOG2(ERR, "ipcom_setsockopt(ICMP/IN/IP_IP_RECVTTL) failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); goto fail; } /* Enable IP_X_SENDERLINKADDR to get source link address */ i= 1; if (ipcom_setsockopt(ctx->pkt->fd_icmp_in, IP_IPPROTO_IP, IP_IP_X_SENDERLINKADDR, &i, sizeof(i)) < 0) { IPCOM_LOG1(ERR, "ipcom_setsockopt(ICMP/IN/IP_IP_X_SENDERLINKADDR) failed, errno = %d", ipcom_errno); goto fail; } /* Shutdown write */ ipcom_shutdown(ctx->pkt->fd_icmp_in, IP_SHUT_WR); } if (IP_BIT_ISSET(ctx->pkt->flags, IPMIP4_PKT_PKT_OUT|IPMIP4_PKT_PKT_IN)) { /* Create raw IP socket for all IPv4 input on this interface */ ctx->pkt->fd_pkt = ipcom_socket(IP_AF_PACKET, IP_SOCK_DGRAM, ip_htons(0x0806)); if (ctx->pkt->fd_pkt == IP_INVALID_SOCKET) { IPCOM_LOG2(ERR, "ipcom_socket(PACKET,ARP) failed : %s [%d]", ipcom_strerror(ipcom_errno), ipcom_errno); goto fail; } /* Shutdown read */ if (IP_BIT_ISFALSE(ctx->pkt->flags, IPMIP4_PKT_PKT_IN)) ipcom_shutdown(ctx->pkt->fd_pkt, IP_SHUT_RD); if (IP_BIT_ISFALSE(ctx->pkt->flags, IPMIP4_PKT_PKT_OUT)) ipcom_shutdown(ctx->pkt->fd_pkt, IP_SHUT_WR); } return IP_TRUE; fail: ipmip4_pkt_context_destroy(ctx, proc); return IP_FALSE; } /* *=========================================================================== * ipmip4_pkt_context_destroy *=========================================================================== * Description: Destroy the packet context * Parameters: ctx - The context * proc - Process context * Returns: * */ IP_GLOBAL void ipmip4_pkt_context_destroy(Ipmip4_context_t *ctx, Ipmip_proc_t *proc) { IPMIP_UNUSED_PARAMETER(proc); if (ctx->pkt != IP_NULL) { if (ctx->pkt->fd_icmp_in != IP_INVALID_SOCKET) ipcom_socketclose(ctx->pkt->fd_icmp_in); if (ctx->pkt->fd_pkt != IP_INVALID_SOCKET) ipcom_socketclose(ctx->pkt->fd_pkt); if (ctx->pkt->fd_raw != IP_INVALID_SOCKET) ipcom_socketclose(ctx->pkt->fd_raw); if (ctx->pkt->udp_in.fd != IP_INVALID_SOCKET) ipcom_socketclose(ctx->pkt->udp_in.fd); if (ctx->pkt->udp_out.fd != IP_INVALID_SOCKET) ipcom_socketclose(ctx->pkt->udp_out.fd); ipcom_free(ctx->pkt); ctx->pkt = IP_NULL; } } /* *=========================================================================== * ipmip4_pkt_context_start *=========================================================================== * Description: Start the packet context * Parameters: ctx - The context * proc - Process context * Returns: IP_TRUE on success. * */ IP_GLOBAL Ip_bool ipmip4_pkt_context_start(Ipmip4_context_t *ctx, Ipmip_proc_t *proc) { /* Lock the global mutex */ if (ctx->pkt != IP_NULL && ctx->pkt->proc == IP_NULL) { ctx->pkt->proc = ipmip_proc_start(&ctx->ipmip, proc, "packet", (Ipmip_proc_func_t) ipmip4_pkt_loop, ctx->pkt); } return ctx->pkt != IP_NULL && ctx->pkt->proc != IP_NULL; } /* *=========================================================================== * ipmip4_pkt_context_stop *=========================================================================== * Description: Stops the packet thread. * Parameters: ctx - The timeout process * proc - The process * Returns: * */ IP_GLOBAL void ipmip4_pkt_context_stop(Ipmip4_context_t *ctx, Ipmip_proc_t *proc) { /* Lock the global mutex */ if (ctx->pkt != IP_NULL && ctx->pkt->proc != IP_NULL) { ipmip_proc_stop(&ctx->ipmip, proc, ctx->pkt->proc, IP_NULL, IP_NULL); ctx->pkt->proc = IP_NULL; } } /* *=========================================================================== * ipmip4_pkt_icmp_join *=========================================================================== * Description: Join a specific multicast group * Parameters: ctx - The context * proc - Process context * group - The group * ifindex - The ifindex * Returns: * */ IP_GLOBAL Ip_bool ipmip4_pkt_icmp_join(Ipmip4_context_t *ctx, Ipmip_proc_t *proc, Ip_u32 group, Ip_u32 ifindex) { struct Ip_ip_mreqn mreq; IPMIP_UNUSED_PARAMETER(proc); mreq.imr_multiaddr.s_addr = group; mreq.imr_interface.s_addr = 0; mreq.imr_ifindex = ifindex; /* */ if (ipcom_setsockopt(ctx->pkt->fd_icmp_in, IP_IPPROTO_IP, IP_IP_JOIN_GROUP, &mreq, sizeof(mreq)) >= 0 || ipcom_errno == IP_ERRNO_EADDRINUSE) return IP_TRUE; return IP_FALSE; } /* **************************************************************************** * 11 PUBLIC FUNCTIONS **************************************************************************** */ /* **************************************************************************** * END OF FILE **************************************************************************** */