Completes code for apps/examples/ipforward. Still untested.
This commit is contained in:
Gregory Nutt
parent
47a42852e5
commit
37bb767089
@ -41,6 +41,7 @@
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
@ -50,6 +51,8 @@
|
||||
#include <net/if.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
#include <nuttx/net/ip.h>
|
||||
#include <nuttx/net/tcp.h>
|
||||
#include <nuttx/net/tun.h>
|
||||
|
||||
#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:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user