diff --git a/examples/ipforward/ipforward.c b/examples/ipforward/ipforward.c index 3f6487f72..be2782bae 100644 --- a/examples/ipforward/ipforward.c +++ b/examples/ipforward/ipforward.c @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -50,6 +51,8 @@ #include #include +#include +#include #include #include "netutils/netlib.h" @@ -58,12 +61,17 @@ * Pre-processor Definitions ****************************************************************************/ -#define MAX_DEVNAME 8 +#define MAX_DEVNAME 8 +#define IPFWD_BUFSIZE CONFIG_NET_TUN_MTU +#define NBYTES_PER_LINE 32 +#define IPFWD_NPACKETS 3 #ifdef CONFIG_NET_IPv6 -# define IPADDR_TYPE FAR const uint16_t * +# define IPADDR_TYPE FAR const uint16_t * +# define IP_HDRLEN IPv6_HDRLEN #else -# define IPADDR_TYPE uint32_t +# define IPADDR_TYPE uint32_t +# define IP_HDRLEN IPv4_HDRLEN #endif /**************************************************************************** @@ -72,14 +80,24 @@ struct ipfwd_tun_s { - int it_fd; - char it_devname[MAX_DEVNAME]; + int it_fd; + char it_devname[MAX_DEVNAME]; }; struct ipfwd_state_s { struct ipfwd_tun_s if_tun0; struct ipfwd_tun_s if_tun1; + pthread_t if_receiver; + pthread_t if_sender; +}; + +struct ipfwd_arg_s +{ + int ia_fd; + IPADDR_TYPE ia_srcipaddr; + IPADDR_TYPE ia_destipaddr; + uint8_t ia_buffer[IPFWD_BUFSIZE]; }; /**************************************************************************** @@ -128,6 +146,11 @@ static const uint32_t g_tun1_ipaddr = HTONL(0x0a000039); static const uint32_t g_netmask = HTONL(0xffffff00); #endif +static const char g_payload[] = "Hi there, TUN receiver!"; +#ifdef CONFIG_NET_IPv4 +static uint16_t g_ipid; +#endif + /**************************************************************************** * Private Functions ****************************************************************************/ @@ -221,14 +244,355 @@ static int ipfwd_netconfig(FAR struct ipfwd_tun_s *tun, IPADDR_TYPE ipaddr, return 0; } +/**************************************************************************** + * Name: Checksums + ****************************************************************************/ + +static uint16_t chksum(uint16_t sum, FAR const uint8_t *data, uint16_t len) +{ + FAR const uint8_t *dataptr; + FAR const uint8_t *last_byte; + uint16_t t; + + dataptr = data; + last_byte = data + len - 1; + + while (dataptr < last_byte) + { + /* At least two more bytes */ + + t = ((uint16_t)dataptr[0] << 8) + dataptr[1]; + sum += t; + if (sum < t) + { + sum++; /* carry */ + } + + dataptr += 2; + } + + if (dataptr == last_byte) + { + t = (dataptr[0] << 8) + 0; + sum += t; + if (sum < t) + { + sum++; /* carry */ + } + } + + /* Return sum in host byte order. */ + + return sum; +} + +#ifdef CONFIG_NET_IPv4 +static uint16_t ipv4_chksum(FAR const uint8_t *buffer) +{ + uint16_t sum; + + sum = chksum(0, buffer, IPv4_HDRLEN); + return (sum == 0) ? 0xffff : htons(sum); +} +#endif + +uint16_t tcp_chksum(FAR uint8_t *buffer) +{ +#ifdef CONFIG_NET_IPv6 + FAR struct ipv6_hdr_s *ipv6 = (FAR struct ipv6_hdr_s *)buffer; + uint16_t upperlen; + uint16_t sum; + + /* The length reported in the IPv6 header is the length of the payload + * that follows the header. + */ + + upperlen = ((uint16_t)ipv6->len[0] << 8) + ipv6->len[1]; + + /* The checksum is calculated starting with a pseudo-header of IPv6 header + * fields according to the IPv6 standard, which consists of the source + * and destination addresses, the packet length and the next header field. + */ + + sum = upperlen + IP_PROTO_TCP; + + /* Sum IP source and destination addresses. */ + + sum = chksum(sum, (FAR uint8_t *)&ipv6->srcipaddr, 2 * sizeof(net_ipv6addr_t)); + + /* Sum IP payload data. */ + + sum = chksum(sum, &buffer[IPv6_HDRLEN], upperlen); + return (sum == 0) ? 0xffff : htons(sum); +#else + FAR struct ipv4_hdr_s *ipv4 = (FAR struct ipv4_hdr_s *)buffer; + uint16_t upperlen; + uint16_t sum; + + /* The length reported in the IPv4 header is the length of both the IPv4 + * header and the payload that follows the header. We need to subtract + * the size of the IPv4 header to get the size of the payload. + */ + + upperlen = (((uint16_t)(ipv4->len[0]) << 8) + ipv4->len[1]) - IPv4_HDRLEN; + + /* First sum pseudo-header. */ + /* IP protocol and length fields. This addition cannot carry. */ + + sum = upperlen + IP_PROTO_TCP; + + /* Sum IP source and destination addresses. */ + + sum = chksum(sum, (FAR uint8_t *)&ipv4->srcipaddr, 2 * sizeof(in_addr_t)); + + /* Sum IP payload data. */ + + sum = chksum(sum, &buffer[IPv4_HDRLEN], upperlen); + return (sum == 0) ? 0xffff : htons(sum); +#endif +} + +/**************************************************************************** + * Name: ipfwd_dumppkt (and friends) + ****************************************************************************/ + +static char lib_nibble(unsigned char nibble) +{ + if (nibble < 10) + { + return '0' + nibble; + } + else + { + return 'a' + nibble - 10; + } +} + +static void ipfwd_dumpbuffer(FAR uint8_t *buffer, size_t buflen) +{ + unsigned int i; + unsigned int j; + unsigned int k; + + for (i = 0; i < buflen; i += NBYTES_PER_LINE) + { + /* Generate hex values: 2 * NBYTES_PER_LINE + 1 bytes */ + + for (j = 0; j < NBYTES_PER_LINE; j++) + { + k = i + j; + + putchar(' '); + putchar(' '); + + if (j == (NBYTES_PER_LINE / 2)) + { + putchar(' '); + } + + if (k < buflen) + { + putchar(lib_nibble((buffer[k] >> 4) & 0xf)); + putchar(lib_nibble(buffer[k] & 0xf)); + } + else + { + putchar(' '); + putchar(' '); + } + } + + putchar('\n'); + } +} + +static void ipfwd_dumppkt(FAR uint8_t *buffer, size_t buflen) +{ + size_t dumpsize; + + if (buflen <= 0) + { + return; + } + + dumpsize = IP_HDRLEN; + if (dumpsize > buflen) + { + dumpsize = buflen; + } + + printf("IP Header:\n"); + ipfwd_dumpbuffer(buffer, dumpsize); + + buffer += dumpsize; + buflen -= dumpsize; + if (buflen <= 0) + { + return; + } + + dumpsize = TCP_HDRLEN; + if (dumpsize > buflen) + { + dumpsize = buflen; + } + + printf("TCP Header:\n"); + ipfwd_dumpbuffer(buffer, dumpsize); + + buffer += dumpsize; + buflen -= dumpsize; + if (buflen <= 0) + { + return; + } + + printf("Payload:\n"); + ipfwd_dumpbuffer(buffer, buflen); +} + /**************************************************************************** * Name: ipfwd_receiver ****************************************************************************/ +static FAR void *ipfwd_receiver(FAR void *arg) +{ + FAR struct ipfwd_arg_s *fwd = (FAR struct ipfwd_arg_s *)arg; + ssize_t nread; + int errcode; + int i; + + for (i = 0; i < IPFWD_NPACKETS; i++) + { + nread = read(fwd->ia_fd, fwd->ia_buffer, IPFWD_BUFSIZE); + if (nread < 0) + { + errcode = errno; + fprintf(stderr, "ERROR: read() failed: %d\n", errcode); + break; + } + + printf("Received packet %d: size=%lu\n", i+1, (unsigned long)nread); + ipfwd_dumppkt(fwd->ia_buffer, nread); + } + + return NULL; +} + /**************************************************************************** * Name: ipfwd_sender ****************************************************************************/ +static FAR void *ipfwd_sender(FAR void *arg) +{ + FAR struct ipfwd_arg_s *fwd = (FAR struct ipfwd_arg_s *)arg; +#ifdef CONFIG_NET_IPv6 + FAR struct ipv6_hdr_s *ipv6; +#else + FAR struct ipv4_hdr_s *ipv4; +#endif + FAR struct tcp_hdr_s *tcp; + FAR char *payload; + size_t paysize; + size_t pktlen; + ssize_t nwritten; + int errcode; + int i; + + paysize = sizeof(g_payload); + + for (i = 0; i < IPFWD_NPACKETS; i++) + { +#ifdef CONFIG_NET_IPv6 + ipv6 = (FAR struct ipv6_hdr_s *)fwd->ia_buffer; + + /* Set up the IPv6 header */ + + ipv6->vtc = 0x60; /* Version/traffic class (MS) */ + ipv6->tcf = 0; /* Traffic class (LS)/Flow label (MS) */ + ipv6->flow = 0; /* Flow label (LS) */ + + /* Length excludes the IPv6 header */ + + pktlen = TCP_HDRLEN + paysize; + ipv6->len[0] = (pktlen >> 8); + ipv6->len[1] = (pktlen & 0xff); + + ipv6->proto = IP_PROTO_TCP; /* Next header */ + ipv6->ttl = 255; /* Hop limit */ + + /* Set source and destination address */ + + net_ipv6addr_copy(ipv6->srcipaddr, fwd->ia_srcipaddr); + net_ipv6addr_copy(ipv6->destipaddr, fwd->ia_destipaddr); + + pktlen = IPv6_HDRLEN + TCP_HDRLEN + paysize; + tcp = (FAR struct tcp_hdr_s *)&fwd->ia_buffer[IPv6_HDRLEN]; +#else + ipv4 = (FAR struct ipv4_hdr_s *)fwd->ia_buffer; + + /* Set up the IPv4 header */ + + ipv4->vhl = 0x45; + ipv4->tos = 0; + + pktlen = IPv4_HDRLEN + TCP_HDRLEN + paysize; + ipv4->len[0] = (pktlen >> 8); + ipv4->len[1] = (pktlen & 0xff); + + ++g_ipid; + ipv4->ipid[0] = g_ipid >> 8; + ipv4->ipid[1] = g_ipid & 0xff; + + ipv4->ipoffset[0] = IP_FLAG_DONTFRAG >> 8; + ipv4->ipoffset[1] = IP_FLAG_DONTFRAG & 0xff; + ipv4->ttl = IP_TTL; + ipv4->proto = IP_PROTO_TCP; + + net_ipv4addr_hdrcopy(ipv4->srcipaddr, fwd->ia_srcipaddr); + net_ipv4addr_hdrcopy(ipv4->destipaddr, fwd->ia_srcipaddr); + + /* Calculate IP checksum. */ + + ipv4->ipchksum = 0; + ipv4->ipchksum = ~(ipv4_chksum(fwd->ia_buffer)); + + tcp = (FAR struct tcp_hdr_s *)&fwd->ia_buffer[IPv4_HDRLEN]; +#endif + + /* Set up the TCP header. NOTE: Most of the elements are irrelevant + * in this test. The forwarding is L2 layer only and the L3 header + * content is not used in the forwarding. + */ + + memset(tcp, 0, sizeof(struct tcp_hdr_s)); + + tcp->srcport = HTONS(0x1234); + tcp->destport = HTONS(0xabcd); + tcp->tcpoffset = (TCP_HDRLEN / 4) << 4; + + payload = (FAR char *)tcp + TCP_HDRLEN; + memcpy(payload, g_payload, paysize); + + tcp->tcpchksum = ~tcp_chksum(fwd->ia_buffer); + + printf("Sending packet %d: size=%lu\n", i+1, (unsigned long)pktlen); + ipfwd_dumppkt(fwd->ia_buffer, pktlen); + + nwritten = write(fwd->ia_fd, fwd->ia_buffer, pktlen); + if (nwritten < 0) + { + errcode = errno; + fprintf(stderr, "ERROR: write() failed: %d\n", errcode); + break; + } + + printf(" %lu bytes sent\n", (unsigned long)nwritten); + } + + return NULL; +} + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -244,6 +608,9 @@ int ipfwd_main(int argc, char *argv[]) #endif { struct ipfwd_state_s fwd; + struct ipfwd_arg_s tun0arg; + struct ipfwd_arg_s tun1arg; + FAR void *value; int errcode = EXIT_SUCCESS; int ret; @@ -280,15 +647,52 @@ int ipfwd_main(int argc, char *argv[]) goto errout_with_tun1; } - /* Start receiver thread */ + /* Start receiver thread on tun0 */ - /* Start sender thread */ + tun0arg.ia_fd = fwd.if_tun0.it_fd; + tun0arg.ia_srcipaddr = g_tun0_ipaddr; + tun0arg.ia_destipaddr = g_tun1_ipaddr; + + ret = pthread_create(&fwd.if_receiver, NULL, ipfwd_receiver, &tun0arg); + if (ret != 0) + { + fprintf(stderr, "ERROR: pthread_create() failed for receiver: %d\n", ret); + errcode = EXIT_FAILURE; + goto errout_with_tun1; + } + + /* Start sender thread on tun1 */ + + tun1arg.ia_fd = fwd.if_tun1.it_fd; + tun1arg.ia_srcipaddr = g_tun1_ipaddr; + tun1arg.ia_destipaddr = g_tun0_ipaddr; + + ret = pthread_create(&fwd.if_sender, NULL, ipfwd_sender, &tun1arg); + if (ret != 0) + { + fprintf(stderr, "ERROR: pthread_create() failed for sender: %d\n", ret); + errcode = EXIT_FAILURE; + goto errout_with_receiver; + } /* Wait for sender thread to terminate */ + ret = pthread_join(fwd.if_sender, &value); + if (ret != OK) + { + fprintf(stderr, "ERROR: pthread_join() failed for sender: %d\n", ret); + } + errout_with_receiver: /* Wait for receiver thread to terminate */ + pthread_kill(fwd.if_receiver, 9); + ret = pthread_join(fwd.if_receiver, &value); + if (ret != OK) + { + fprintf(stderr, "ERROR: pthread_join() failed for receiver: %d\n", ret); + } + errout_with_tun1: close(fwd.if_tun1.it_fd); errout_with_tun0: