diff --git a/common.c b/common.c
index 48fe30b..1f5ba62 100644
--- a/common.c
+++ b/common.c
@@ -163,42 +163,42 @@ int listen_single_addr(struct addrinfo* addr, int keepalive, int udp)
    */
 int start_listen_sockets(struct listen_endpoint *sockfd[])
 {
-   struct addrinfo *addr, *start_addr;
-   char buf[NI_MAXHOST];
-   int i, res;
-   int num_addr = 0, keepalive = 0, udp = 0;
-   int sd_socks = 0;
+    struct addrinfo *addr, *start_addr;
+    char buf[NI_MAXHOST];
+    int i, res;
+    int num_addr = 0, keepalive = 0, udp = 0;
+    int sd_socks = 0;
 
-   sd_socks = get_fd_sockets(sockfd);
+    sd_socks = get_fd_sockets(sockfd);
 
-   if (sd_socks > 0) {
-       return sd_socks;
-   }
+    if (sd_socks > 0) {
+        return sd_socks;
+    }
 
-   *sockfd = NULL;
+    *sockfd = NULL;
 
-   if (cfg.verbose) fprintf(stderr, "Listening to:\n");
+    if (cfg.verbose) fprintf(stderr, "Listening to:\n");
+
+    for (i = 0; i < cfg.listen_len; i++) {
+        keepalive = cfg.listen[i].keepalive;
+        udp = cfg.listen[i].is_udp;
 
-   for (i = 0; i < cfg.listen_len; i++) {
         res = resolve_split_name(&start_addr, cfg.listen[i].host, cfg.listen[i].port);
         if (res) exit(4);
 
         for (addr = start_addr; addr; addr = addr->ai_next) {
-            keepalive = cfg.listen[i].keepalive;
-            udp = cfg.listen[i].is_udp;
-
             num_addr++;
             *sockfd = realloc(*sockfd, num_addr * sizeof(*sockfd));
             (*sockfd)[num_addr-1].socketfd = listen_single_addr(addr, keepalive, udp);
             (*sockfd)[num_addr-1].type = udp ? SOCK_DGRAM : SOCK_STREAM;
             if (cfg.verbose)
-                fprintf(stderr, "\t%s\t[%s]\n", sprintaddr(buf, sizeof(buf), addr),
-                    cfg.listen[i].keepalive ? "keepalive" : "");
+                fprintf(stderr, "%d:\t%s\t[%s]\n", (*sockfd)[num_addr-1].socketfd, sprintaddr(buf, sizeof(buf), addr),
+                        cfg.listen[i].keepalive ? "keepalive" : "");
         }
         freeaddrinfo(start_addr);
-   }
+    }
 
-   return num_addr;
+    return num_addr;
 }
 
 
diff --git a/sslh-fork.c b/sslh-fork.c
index 5997edd..f6e1572 100644
--- a/sslh-fork.c
+++ b/sslh-fork.c
@@ -1,7 +1,7 @@
 /*
    sslh-fork: forking server
 
-# Copyright (C) 2007-2012  Yves Rutschle
+# Copyright (C) 2007-2021  Yves Rutschle
 # 
 # This program is free software; you can redistribute it
 # and/or modify it under the terms of the GNU General Public
@@ -22,6 +22,7 @@
 
 #include "common.h"
 #include "probe.h"
+#include "sslh-conf.h"
 
 #ifdef LIBBSD
 #include <bsd/unistd.h>
@@ -168,7 +169,7 @@ void set_listen_procname(struct listen_endpoint *listen_socket)
  * IN: 
  *      endpoint: array of listening endpoint objects
  *      num_endpoints: size of endpoint array
- *      active_endpoint: which endpoint should be accepted
+ *      active_endpoint: which endpoint is this listener working on
  * Does not return
  * */
 void tcp_listener(struct listen_endpoint* endpoint, int num_endpoints, int active_endpoint)
@@ -197,6 +198,111 @@ void tcp_listener(struct listen_endpoint* endpoint, int num_endpoints, int activ
     }
 }
 
+/* UDP support types and stuff */
+struct known_udp_source {
+    int allocated;
+    struct sockaddr sockaddr;
+    socklen_t addrlen;
+    time_t last_active;
+
+    struct sslhcfg_protocols_item* proto; /* Where to connect it to */
+    /* We need one local socket for each target server, so we know where to
+     * forward server responses */
+    int target_sock;  
+};
+
+
+/* Find if the specified source has been seen before. -1 if not found
+ *
+ * TODO This is linear search and needs to be changed to something better for
+ * production if we have more than a dozen sources
+ * Also, this assumes src_addr from recvfrom() are repeatable for a specific
+ * source...
+ * */
+int known_source(struct known_udp_source* ks, int ks_len, struct sockaddr* addr, socklen_t addrlen)
+{
+    int i;
+
+    for (i = 0; i < ks_len; i++) {
+        if (ks[i].allocated) {
+            if (!memcmp(&ks[i].sockaddr, addr, addrlen)) {
+                return i;
+            }
+        }
+    }
+    return -1;
+}
+
+int get_empty_source(struct known_udp_source* ks, int ks_len)
+{
+    int i;
+    for (i = 0; i < ks_len; i++)
+        if (!ks[i].allocated) return i;
+    return -1;
+}
+
+/* TODO: Make that dynamic... */
+#define MAX_UDP_SRC 1024
+/* Array to keep the UDP sources we have seen before */
+struct known_udp_source udp_known_sources[MAX_UDP_SRC];
+
+/* UDP listener: upon incoming packet, find where it should go */
+void udp_listener(struct listen_endpoint* endpoint, int num_endpoints, int active_endpoint)
+{
+    char data[65536]; /* TODO what's right, here? */
+    char addr_str[NI_MAXHOST+1+NI_MAXSERV+1];
+    struct sockaddr src_addr;
+    struct addrinfo addrinfo;
+    struct known_udp_source* src;
+    ssize_t len;
+    socklen_t addrlen;
+    int i, res, target;
+
+    while (1) {
+        fprintf(stderr, "recvfrom(%d)\n", getpid());
+        addrlen = sizeof(src_addr);
+        len = recvfrom(endpoint[active_endpoint].socketfd, data, sizeof(data), 0, &src_addr, &addrlen);
+        if (len < 0) {
+            perror("recvfrom");
+            continue;
+        }
+        target = known_source(udp_known_sources, ARRAY_SIZE(udp_known_sources), 
+                              &src_addr, addrlen);
+        addrinfo.ai_addr = &src_addr;
+        addrinfo.ai_addrlen = addrlen;
+        if (cfg.verbose) 
+            fprintf(stderr, "received %d UDP from %d:%s\n", len, target, sprintaddr(addr_str, sizeof(addr_str), &addrinfo));
+        if (target == -1) {
+            target = get_empty_source(udp_known_sources, ARRAY_SIZE(udp_known_sources));
+            fprintf(stderr, "source target index %d\n", target);
+            if (target == -1) exit(0); /* TODO handle this properly */
+            /* A probe worked: save this as an active connection */
+            src = &udp_known_sources[target];
+            src->allocated = 1;
+            src->sockaddr = src_addr;
+            src->addrlen = addrlen;
+            /* TODO fill in time */
+
+
+            res = probe_buffer(data, len, &src->proto);
+            /* First version: if we can't work out the protocol from the first
+             * packet, drop it. Conceivably, we could store several packets to
+             * run probes on packet sets */
+            if (cfg.verbose) fprintf(stderr, "UDP probed: %d\n", res);
+            if (res != PROBE_MATCH) continue;
+
+            src->target_sock = socket(src->proto->saddr->ai_family, SOCK_DGRAM, 0);
+        }
+
+        src = &udp_known_sources[target];
+        /* at this point src is the UDP connection */
+        res = sendto(src->target_sock, data, len, 0, 
+               src->proto->saddr->ai_addr, src->proto->saddr->ai_addrlen);
+        fprintf(stderr, "sending %d to %s", 
+                res, sprintaddr(data, sizeof(data), src->proto->saddr));
+    }
+}
+
 void main_loop(struct listen_endpoint listen_sockets[], int num_addr_listen)
 {
     int i, res;
@@ -216,7 +322,10 @@ void main_loop(struct listen_endpoint listen_sockets[], int num_addr_listen)
         /* We're in the child, we have work to do  */
         case 0:
             set_listen_procname(&listen_sockets[i]);
-            tcp_listener(listen_sockets, num_addr_listen, i);
+            if (listen_sockets[i].type == SOCK_DGRAM)
+                udp_listener(listen_sockets, num_addr_listen, i);
+            else
+                tcp_listener(listen_sockets, num_addr_listen, i);
 	    break;
 
 	/* We're in the parent, we don't need to do anything */
@@ -233,6 +342,7 @@ void main_loop(struct listen_endpoint listen_sockets[], int num_addr_listen)
     res = sigaction(SIGTERM, &action, NULL);
     CHECK_RES_DIE(res, "sigaction");
 
+
     wait(NULL);
 }