Proper port support for UDP

etc/make/conf.mk

@@ -111,7 +111,8 @@ OBJS+=$(O)/sys/sys_net.o \
 	  $(O)/net/tcp.o \
 	  $(O)/net/if.o \
 	  $(O)/net/socket.o \
-	  $(O)/net/util.o
+	  $(O)/net/util.o \
+	  $(O)/net/ports.o
 DIRS+=$(O)/net
 
 ifeq ($(DEBUG_COUNTERS),1)

include/net/net.h

@@ -4,4 +4,5 @@
 struct netdev;
 
 int net_receive(struct netdev *dev, const void *data, size_t len);
+void net_init(void);
 void net_daemon_start(void);

include/net/ports.h

@@ -0,0 +1,20 @@
+#pragma once
+#include "sys/types.h"
+#include "sys/list.h"
+
+#define PORT_ARRAY_BUCKET_COUNT             64
+
+struct port_array_entry {
+    uint16_t port_no;
+    void *socket;
+    struct list_head list;
+};
+
+struct port_array {
+    struct list_head buckets[PORT_ARRAY_BUCKET_COUNT];
+};
+
+void port_array_init(struct port_array *arr);
+int port_array_lookup(struct port_array *arr, uint16_t port_no, void **port);
+void port_array_insert(struct port_array *arr, uint16_t port_no, void *port);
+void *port_array_delete(struct port_array *arr, uint16_t key);

include/net/udp.h

@@ -13,3 +13,4 @@ struct inet_frame;
 struct packet;
 
 void udp_handle_frame(struct packet *p, struct eth_frame *eth, struct inet_frame *ip, void *data, size_t len);
+void udp_init(void);

net/net.c

@@ -139,6 +139,10 @@ int net_receive(struct netdev *dev, const void *data, size_t len) {
     return 0;
 }
 
+void net_init(void) {
+    udp_init();
+}
+
 void net_daemon_start(void) {
     packet_queue_init(&g_rxq);
 

net/ports.c

@@ -0,0 +1,63 @@
+#include "sys/mem/slab.h"
+#include "sys/assert.h"
+#include "net/ports.h"
+#include <stddef.h>
+
+static struct slab_cache *g_ent_cache = NULL;
+
+void port_array_insert(struct port_array *arr, uint16_t key, void *value) {
+    if (!g_ent_cache) {
+        g_ent_cache = slab_cache_get(sizeof(struct port_array_entry));
+        _assert(g_ent_cache);
+    }
+
+    size_t hash = key % PORT_ARRAY_BUCKET_COUNT;
+    struct port_array_entry *ent;
+    list_for_each_entry(ent, &arr->buckets[hash], list) {
+        assert(ent->port_no != key, "Tried to insert a duplicate port mapping\n");
+    }
+
+    ent = slab_calloc(g_ent_cache);
+    _assert(ent);
+
+    ent->port_no = key;
+    ent->socket = value;
+    list_head_init(&ent->list);
+    list_add(&ent->list, &arr->buckets[hash]);
+}
+
+void *port_array_delete(struct port_array *arr, uint16_t key) {
+    size_t hash = key % PORT_ARRAY_BUCKET_COUNT;
+    struct port_array_entry *ent;
+    list_for_each_entry(ent, &arr->buckets[hash], list) {
+        if (ent->port_no == key) {
+            _assert(g_ent_cache);
+            list_del(&ent->list);
+            void *r = ent->socket;
+            slab_free(g_ent_cache, ent);
+
+            return r;
+        }
+    }
+
+    return NULL;
+}
+
+int port_array_lookup(struct port_array *arr, uint16_t key, void **value) {
+    size_t hash = key % PORT_ARRAY_BUCKET_COUNT;
+    struct port_array_entry *ent;
+    list_for_each_entry(ent, &arr->buckets[hash], list) {
+        if (ent->port_no == key) {
+            *value = ent->socket;
+            return 0;
+        }
+    }
+
+    return -1;
+}
+
+void port_array_init(struct port_array *arr) {
+    for (size_t i = 0; i < PORT_ARRAY_BUCKET_COUNT; ++i) {
+        list_head_init(&arr->buckets[i]);
+    }
+}

net/udp.c

@@ -7,6 +7,7 @@
 #include "sys/string.h"
 #include "net/class.h"
 #include "sys/sched.h"
+#include "net/ports.h"
 #include "user/inet.h"
 #include "sys/debug.h"
 #include "sys/wait.h"
@@ -37,6 +38,7 @@ static ssize_t udp_socket_sendto(struct socket *s,
                                  size_t salen);
 static int udp_socket_bind(struct socket *s, struct sockaddr *sa, size_t len);
 static int udp_socket_setsockopt(struct socket *s, int optname, void *optval, size_t optlen);
+static int udp_socket_count_pending(struct socket *s);
 
 static struct sockops udp_socket_ops = {
     .open = udp_socket_open,
@@ -47,6 +49,8 @@ static struct sockops udp_socket_ops = {
 
     .bind = udp_socket_bind,
     .setsockopt = udp_socket_setsockopt,
+
+    .count_pending = udp_socket_count_pending
 };
 static struct socket_class udp_socket_class = {
     .name = "udp",
@@ -74,12 +78,13 @@ struct udp_socket {
     struct packet_queue pending;
 };
 
-static uint16_t udp_eph_port = 32768;
+#define UDP_EPH_PORT_BASE           32768
+static uint16_t udp_eph_port = UDP_EPH_PORT_BASE;
+static struct port_array udp_port_array;
 
-// Only allowed for binding now
-#define UDP_BIND_COUNT          16
-#define UDP_BIND_START          60
-static struct udp_socket *udp_ports[UDP_BIND_COUNT] = {0};
+void udp_init(void) {
+    port_array_init(&udp_port_array);
+}
 
 struct udp_socket *udp_socket_create(void) {
     struct udp_socket *sock = kmalloc(sizeof(struct udp_socket));
@@ -109,17 +114,15 @@ void udp_handle_frame(struct packet *p, struct eth_frame *eth, struct inet_frame
 
     uint16_t dpt = ntohs(udp->dst_port);
 
-    if (dpt >= UDP_BIND_START && (dpt - UDP_BIND_START) < UDP_BIND_COUNT) {
-        // Check if it's a packet for one of "listening" sockets
-        struct udp_socket *sock = udp_ports[dpt - UDP_BIND_START];
+    if (dpt < UDP_EPH_PORT_BASE) {
+        struct udp_socket *sock;
+
+        if (port_array_lookup(&udp_port_array, dpt, (void **) &sock) == 0) {
+            _assert(sock);
 
-        if (sock) {
-            // Add "pending" packet for this socket
             packet_ref(p);
             packet_queue_push(&sock->pending, p);
             thread_notify_io(&sock->wait);
-        } else {
-            kdebug("Packet is destined to unbound port: %u\n", dpt);
         }
     }
 }
@@ -165,6 +168,20 @@ static ssize_t udp_socket_recvfrom(struct socket *s,
         panic("Not implemented: partial packet reading\n");
     }
 
+    // Fill sockaddr from packet
+    if (salen != NULL) {
+        _assert(sa);
+        struct sockaddr_in *sin = (struct sockaddr_in *) sa;
+        struct inet_frame *in = (struct inet_frame *) (p->data + sizeof(struct eth_frame));
+        struct udp_frame *udp = (struct udp_frame *) ((void *) in + sizeof(struct inet_frame));
+
+        sin->sin_family = AF_INET;
+        sin->sin_addr = in->src_inaddr;
+        sin->sin_port = udp->src_port;
+
+        *salen = sizeof(struct sockaddr_in);
+    }
+
     memcpy(buf, p_data, p_size);
     packet_unref(p);
 
@@ -233,10 +250,16 @@ static int udp_socket_bind(struct socket *s, struct sockaddr *sa, size_t len) {
     _assert(!(sock->flags));
 
     uint16_t port = ntohs(sin->sin_port);
-    if (port < UDP_BIND_START || (port - UDP_BIND_START) >= UDP_BIND_COUNT) {
+
+    if (port >= UDP_EPH_PORT_BASE) {
         return -EINVAL;
     }
-    udp_ports[port - UDP_BIND_START] = sock;
+
+    if (port_array_lookup(&udp_port_array, port, NULL) == 0) {
+        return -EEXIST;
+    }
+
+    port_array_insert(&udp_port_array, port, sock);
 
     sock->flags |= UDP_SOCKET_ANY;
     sock->recv_port = port;
@@ -266,7 +289,7 @@ static void udp_socket_close(struct socket *s) {
     _assert(sock);
 
     if (sock->flags & UDP_SOCKET_ANY) {
-        udp_ports[sock->recv_port - UDP_BIND_START] = NULL;
+        _assert(port_array_delete(&udp_port_array, sock->recv_port) == sock);
     }
 
     sock->flags &= ~UDP_SOCKET_ANY;
@@ -274,3 +297,9 @@ static void udp_socket_close(struct socket *s) {
     kfree(sock);
     s->data = NULL;
 }
+
+static int udp_socket_count_pending(struct socket *s) {
+    struct udp_socket *sock = s->data;
+    _assert(sock);
+    return !!sock->pending.head;
+}

sys/kernel.c

@@ -20,6 +20,7 @@ void main(void) {
     syscall_init();
     sched_init();
 
+    net_init();
     usb_daemon_start();
     net_daemon_start();
     user_init_start();