complete though slightly messy

diff --git a/rrqnet.c b/rrqnet.c
index cf0617ed2b198d2d6124c12ddb10bf1a979ec245..2711039cfddaccd81fc4fcd0bda51bdffda3f6ff 100644 (file)
--- a/rrqnet.c
+++ b/rrqnet.c
@@ -66,7 +66,7 @@ struct Allowed {
 // Details of actualized connections.
 struct Remote {
     struct SockAddr uaddr;     // The remote IP address
-    struct SockAddr iaddr;     // The receiving IP address
+    struct SockAddr laddr;     // The local IP address
     struct Allowed *spec;      // Rule being instantiated
     struct timeval rec_when;   // Last received packet time, in seconds
 };
@@ -84,8 +84,8 @@ struct Interface  {
 typedef struct _PacketItem {
     QueueItem base;
     int fd;
-    struct SockAddr src;
-    struct SockAddr dst;
+    struct SockAddr src; // the remote IP for this packet
+    struct SockAddr dst; // the local IP for this packet
     ssize_t len;
     unsigned char buffer[ BUFSIZE ];
 } PacketItem;
@@ -921,9 +921,9 @@ static void write_remote(unsigned char *buf, int n,struct Remote *r) {
     // A packet buffer
     unsigned char output[ BUFSIZE ];
     if ( n < 12 ) {
-       VERBOSE2OUT( "SEND %d bytes to %s\n", n, inet_stoa( &r->uaddr ) );
+       VERBOSE2OUT( "SENDing %d bytes to %s\n", n, inet_stoa( &r->uaddr ) );
     } else {
-       VERBOSE2OUT( "SEND %d bytes %s -> %s to %s\n", n,
+       VERBOSE2OUT( "SENDing %d bytes %s -> %s to %s\n", n,
                     inet_mtoa( buf+6 ), inet_mtoa( buf ),
                     inet_stoa( &r->uaddr ) );
     }
@@ -950,28 +950,68 @@ static void write_remote(unsigned char *buf, int n,struct Remote *r) {
     } else if ( r->spec->psk.keyfile ) {
        encrypt( output, n, &r->spec->psk );
     }
-    struct sockaddr *sock = &r->uaddr.in;
+    // Setup the packet addressing
+    struct in_pktinfo pkt4info = {
+       .ipi_ifindex = 0,  /* Interface index */
+       .ipi_spec_dst.s_addr = 0, /* Local address */
+       .ipi_addr.s_addr = 0,     /* Header Destination address */
+    };
+    struct in6_pktinfo pkt6info = {
+       .ipi6_addr.s6_addr32 = { 0, 0, 0, 0 },
+       .ipi6_ifindex = 0,
+    };
+    void *pktinfo = 0;
+    int pktinfosize = 0;
+    struct sockaddr_in *sock4 = &r->uaddr.in4;
+    struct sockaddr_in6 *sock6 = &r->uaddr.in6;
+    void *sock;
     size_t size;
-    if ( sock->sa_family == AF_INET6 ) {
+    if ( udp6 ) {
        // Note that the size of +struct sockaddr_in6+ is actually
        // larger than the size of +struct sockaddr+ (due to the
        // addition of the +sin6_flowinfo+ field). It results in the
        // following cuteness for passing arguments to +sendto+.
+       sock = sock6;
        size = sizeof( struct sockaddr_in6 );
        VERBOSE2OUT( "IPv6 UDP %d %s\n",
-                    udp_fd, inet_stoa( (struct SockAddr*) sock ) );
+                    udp_fd, inet_stoa( &r->laddr ) );
+       if ( r->laddr.in.sa_family ) {
+           memcpy( &pkt6info.ipi6_addr, &sock6->sin6_addr, 16 );
+           pktinfo = &pkt6info;
+           pktinfosize = sizeof( pkt6info );
+       }
     } else {
+       sock = sock4;
        size = sizeof( struct sockaddr_in );
        VERBOSE2OUT( "IPv4 UDP %d %s\n",
-                    udp_fd, inet_stoa( (struct SockAddr*) sock ) );
+                    udp_fd, inet_stoa( &r->laddr ) );
+       if ( r->laddr.in.sa_family ) {
+           memcpy( &pkt4info.ipi_spec_dst, &sock4->sin_addr, 4 );
+           pktinfo = &pkt4info;
+           pktinfosize = sizeof( pkt4info );
+       }
     }
-    VERBOSE2OUT( "SEND %d bytes to %s [%s -> %s]\n",
-                n, inet_stoa( (struct SockAddr*) sock ),
+    VERBOSE2OUT( "SEND %d bytes from %s to %s [%s -> %s]\n",
+                n,
+                inet_stoa( &r->laddr ),
+                inet_stoa( &r->uaddr ),
                 ( n < 12 )? "" : inet_mtoa( buf+6 ),
                 ( n < 12 )? "" : inet_mtoa( buf )
              );
-    // IS sendto thread safe??
-    if ( sendto( udp_fd, output, n, 0, sock, size ) < n ) {
+    // IS sendmsg thread safe??
+    struct iovec data[1] = {{ output, n }};
+    (void)pktinfo;
+    (void)pktinfosize;
+    struct msghdr msg = {
+       .msg_name = sock,
+       .msg_namelen = size,
+       .msg_iov = data,
+       .msg_iovlen = 1,
+       .msg_control = 0, //pktinfo,
+       .msg_controllen = 0, //pktinfosize,
+       .msg_flags = 0 // unused
+    };
+    if ( sendmsg( udp_fd, &msg, 0 ) < n ) {
        perror( "Writing socket" );
        // Invalidate remote temporarily instead? But if it's an
        // "uplink" it should be retried eventually...
@@ -1141,7 +1181,10 @@ static struct Interface *input_check(
 }
 
 // Check packet and deliver out
-static void route_packet(unsigned char *buf,int len,struct SockAddr *src) {
+static void route_packet(PacketItem *pi) {
+    unsigned char *buf = pi->buffer;
+    int len = pi->len;
+    struct SockAddr *src = &pi->src;
     struct Interface *x = input_check( buf, len, src );
     if ( x == 0 ) {
        return; // not a nice packet
@@ -1167,6 +1210,8 @@ static void route_packet(unsigned char *buf,int len,struct SockAddr *src) {
        VERBOSE2OUT( "RECV route %s -> %s to %s\n",
                     inet_mtoa( buf+6 ), inet_mtoa( buf ),
                     inet_stoa( &y->remote->uaddr ) );
+       // Set the local address for the remote
+       memcpy( &y->remote->laddr, &pi->dst, sizeof( pi->dst ) );
        write_remote( buf, len, y->remote );
        return;
     }
@@ -1224,13 +1269,14 @@ static void *packet_handler(void *data) {
     for ( ;; ) {
        PacketItem *todo = (PacketItem *) Queue_getItem( &todolist.full );
        if ( todo->fd == mcast.fd ) {
-           // Patch multicast address as source for multicast packet
-           route_packet( todo->buffer, todo->len, &mcast.sock );
+           // Patch in the multicast address as source for multicast packet
+           memcpy( &todo->src, &mcast.sock, sizeof( todo->src ) );
+           route_packet( todo );
        } else {
            if ( udp6 ) {
                unmap_if_mapped( &todo->src );
            }
-           route_packet( todo->buffer, todo->len, &todo->src );
+           route_packet( todo );
        }
        Queue_addItem( &todolist.free, (QueueItem*) todo );
     }
@@ -1260,7 +1306,7 @@ void todolist_initialize(int nbuf,int nthr) {
 }
 
 static ssize_t recvpacket(int fd,PacketItem *p) {
-    int flags = udp6? IPV6_PKTINFO : IP_PKTINFO;
+    int flags = 0;
     socklen_t addrlen =
        udp6? sizeof( p->src.in6 ) : sizeof( p->src.in4 );
     struct iovec buffer = {
@@ -1275,7 +1321,7 @@ static ssize_t recvpacket(int fd,PacketItem *p) {
        .msg_iovlen = 1,
        .msg_control = data,
        .msg_controllen = sizeof( data ),
-       .msg_flags = flags // Return value
+       .msg_flags = 0 // Return value
     };
     p->len = recvmsg( fd, &msg, flags );
     struct cmsghdr *cmsg = CMSG_FIRSTHDR( &msg );
@@ -1286,8 +1332,13 @@ static ssize_t recvpacket(int fd,PacketItem *p) {
     cmsg = CMSG_FIRSTHDR( &msg );
     if ( cmsg ) {
        if ( udp6 ) {
+           struct in6_pktinfo *pinf = (struct in6_pktinfo*) CMSG_DATA( cmsg );
+           p->dst.in6.sin6_family = AF_INET6;
+           memcpy( &p->dst.in6.sin6_addr, &pinf->ipi6_addr, 16 );
+           VERBOSE3OUT( "DEST= udp6 %d %s\n",
+                        pinf->ipi6_ifindex, inet_stoa( &p->dst ) );
        } else {
-           struct in_pktinfo* pinf = (struct in_pktinfo*) CMSG_DATA( cmsg );
+           struct in_pktinfo *pinf = (struct in_pktinfo*) CMSG_DATA( cmsg );
            p->dst.in4.sin_family = AF_INET;
            p->dst.in4.sin_addr = pinf->ipi_addr;
            VERBOSE3OUT( "DEST= %d %s\n",
@@ -1309,16 +1360,10 @@ static void *doreadUDP(void *data) {
     int fd = ((ReaderData *) data)->fd;
     while ( 1 ) {
        PacketItem *todo = (PacketItem *) Queue_getItem( &todolist.free );
-#if 0 
-       socklen_t addrlen =
-           udp6? sizeof( todo->src.in6 ) : sizeof( todo->src.in4 );
-#endif
-       memset( &todo->src, 0, sizeof( todo->src ) );
        todo->fd = fd;
-#if 0
-       todo->len = recvfrom(
-           fd, todo->buffer, BUFSIZE, 0, &todo->src.in, &addrlen );
-#endif
+       memset( &todo->src, 0, sizeof( struct SockAddr ) );
+       memset( &todo->dst, 0, sizeof( struct SockAddr ) );
+       todo->len = 0;
        VERBOSE3OUT( "Reading packet\n" );
        ssize_t len = recvpacket( fd, todo );
        if ( len == -1) {
@@ -1682,6 +1727,12 @@ int main(int argc, char *argv[]) {
            exit(1);
        }
        VERBOSEOUT( "Using ipv6 UDP at %d\n", udp_fd );
+       int opt = 1;
+       if ( setsockopt(
+                udp_fd, IPPROTO_IPV6, IPV6_RECVPKTINFO, &opt, sizeof(opt)) ) {
+           fprintf( stderr, "Error configuring socket!\n");
+            exit(1);
+       }
     }
     // If not using stdio for local traffic, then stdin and stdout are
     // closed here, so as to avoid that any other traffic channel gets