Implement optional support for SOCKS5-UDP

This change adds a new option, --socks5-udp.  If this option is
present, and no UDP gateway is specified, UDP packets will no
longer be dropped.  Instead, the client will use the SOCKS5
UDP ASSOCIATE command to route UDP packets through the proxy
server.

This implementation is intended for use with any UDP data, and it
includes an optimization for packets containing DNS queries.
However, this implementation is currently limited to localhost
SOCKS5 servers. SOCKS5-UDP does not perform well over actual
network links, as it requires several roundtrips to the server and
is not compatible with NAT.

This implementation is currently in use in a fork of tun2socks used
by Outline (https://getoutline.org) and Intra (https://getintra.org).

Fixes https://github.com/ambrop72/badvpn/issues/56

17 files changed, 869 insertions, 83 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 02c6ddc..6751e13 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -332,6 +332,7 @@ endif ()
 if (BUILD_TUN2SOCKS)
     add_subdirectory(socksclient)
     add_subdirectory(udpgw_client)
+    add_subdirectory(socks_udp_client)
     add_subdirectory(lwip)
 endif ()
 
diff --git a/blog_channels.txt b/blog_channels.txt
index 8daa1e3..18657d5 100644
--- a/blog_channels.txt
+++ b/blog_channels.txt
@@ -89,6 +89,7 @@ NCDRfkillMonitor 4
 udpgw 4
 UdpGwClient 4
 SocksUdpGwClient 4
+SocksUdpClient 4
 BNetwork 4
 BConnection 4
 BSSLConnection 4
diff --git a/generated/blog_channel_SocksUdpClient.h b/generated/blog_channel_SocksUdpClient.h
new file mode 100644
index 0000000..25779f4
--- /dev/null
+++ b/generated/blog_channel_SocksUdpClient.h
@@ -0,0 +1,4 @@
+#ifdef BLOG_CURRENT_CHANNEL
+#undef BLOG_CURRENT_CHANNEL
+#endif
+#define BLOG_CURRENT_CHANNEL BLOG_CHANNEL_SocksUdpClient
diff --git a/generated/blog_channels_defines.h b/generated/blog_channels_defines.h
index 4f554e4..804c914 100644
--- a/generated/blog_channels_defines.h
+++ b/generated/blog_channels_defines.h
@@ -89,60 +89,61 @@
 #define BLOG_CHANNEL_udpgw 88
 #define BLOG_CHANNEL_UdpGwClient 89
 #define BLOG_CHANNEL_SocksUdpGwClient 90
-#define BLOG_CHANNEL_BNetwork 91
-#define BLOG_CHANNEL_BConnection 92
-#define BLOG_CHANNEL_BSSLConnection 93
-#define BLOG_CHANNEL_BDatagram 94
-#define BLOG_CHANNEL_PeerChat 95
-#define BLOG_CHANNEL_BArpProbe 96
-#define BLOG_CHANNEL_NCDModuleIndex 97
-#define BLOG_CHANNEL_NCDModuleProcess 98
-#define BLOG_CHANNEL_NCDValGenerator 99
-#define BLOG_CHANNEL_ncd_from_string 100
-#define BLOG_CHANNEL_ncd_to_string 101
-#define BLOG_CHANNEL_ncd_value 102
-#define BLOG_CHANNEL_ncd_try 103
-#define BLOG_CHANNEL_ncd_sys_request_server 104
-#define BLOG_CHANNEL_NCDRequest 105
-#define BLOG_CHANNEL_ncd_net_ipv6_wait_dynamic_addr 106
-#define BLOG_CHANNEL_NCDRequestClient 107
-#define BLOG_CHANNEL_ncd_request 108
-#define BLOG_CHANNEL_ncd_sys_request_client 109
-#define BLOG_CHANNEL_ncd_exit 110
-#define BLOG_CHANNEL_ncd_getargs 111
-#define BLOG_CHANNEL_ncd_arithmetic 112
-#define BLOG_CHANNEL_ncd_parse 113
-#define BLOG_CHANNEL_ncd_valuemetic 114
-#define BLOG_CHANNEL_ncd_file 115
-#define BLOG_CHANNEL_ncd_netmask 116
-#define BLOG_CHANNEL_ncd_implode 117
-#define BLOG_CHANNEL_ncd_call2 118
-#define BLOG_CHANNEL_ncd_assert 119
-#define BLOG_CHANNEL_ncd_reboot 120
-#define BLOG_CHANNEL_ncd_explode 121
-#define BLOG_CHANNEL_NCDPlaceholderDb 122
-#define BLOG_CHANNEL_NCDVal 123
-#define BLOG_CHANNEL_ncd_net_ipv6_addr 124
-#define BLOG_CHANNEL_ncd_net_ipv6_route 125
-#define BLOG_CHANNEL_ncd_net_ipv4_addr_in_network 126
-#define BLOG_CHANNEL_ncd_net_ipv6_addr_in_network 127
-#define BLOG_CHANNEL_dostest_server 128
-#define BLOG_CHANNEL_dostest_attacker 129
-#define BLOG_CHANNEL_ncd_timer 130
-#define BLOG_CHANNEL_ncd_file_open 131
-#define BLOG_CHANNEL_ncd_backtrack 132
-#define BLOG_CHANNEL_ncd_socket 133
-#define BLOG_CHANNEL_ncd_depend_scope 134
-#define BLOG_CHANNEL_ncd_substr 135
-#define BLOG_CHANNEL_ncd_sys_start_process 136
-#define BLOG_CHANNEL_NCDBuildProgram 137
-#define BLOG_CHANNEL_ncd_log 138
-#define BLOG_CHANNEL_ncd_log_msg 139
-#define BLOG_CHANNEL_ncd_buffer 140
-#define BLOG_CHANNEL_ncd_getenv 141
-#define BLOG_CHANNEL_BThreadSignal 142
-#define BLOG_CHANNEL_BLockReactor 143
-#define BLOG_CHANNEL_ncd_load_module 144
-#define BLOG_CHANNEL_ncd_basic_functions 145
-#define BLOG_CHANNEL_ncd_objref 146
-#define BLOG_NUM_CHANNELS 147
+#define BLOG_CHANNEL_SocksUdpClient 91
+#define BLOG_CHANNEL_BNetwork 92
+#define BLOG_CHANNEL_BConnection 93
+#define BLOG_CHANNEL_BSSLConnection 94
+#define BLOG_CHANNEL_BDatagram 95
+#define BLOG_CHANNEL_PeerChat 96
+#define BLOG_CHANNEL_BArpProbe 97
+#define BLOG_CHANNEL_NCDModuleIndex 98
+#define BLOG_CHANNEL_NCDModuleProcess 99
+#define BLOG_CHANNEL_NCDValGenerator 100
+#define BLOG_CHANNEL_ncd_from_string 101
+#define BLOG_CHANNEL_ncd_to_string 102
+#define BLOG_CHANNEL_ncd_value 103
+#define BLOG_CHANNEL_ncd_try 104
+#define BLOG_CHANNEL_ncd_sys_request_server 105
+#define BLOG_CHANNEL_NCDRequest 106
+#define BLOG_CHANNEL_ncd_net_ipv6_wait_dynamic_addr 107
+#define BLOG_CHANNEL_NCDRequestClient 108
+#define BLOG_CHANNEL_ncd_request 109
+#define BLOG_CHANNEL_ncd_sys_request_client 110
+#define BLOG_CHANNEL_ncd_exit 111
+#define BLOG_CHANNEL_ncd_getargs 112
+#define BLOG_CHANNEL_ncd_arithmetic 113
+#define BLOG_CHANNEL_ncd_parse 114
+#define BLOG_CHANNEL_ncd_valuemetic 115
+#define BLOG_CHANNEL_ncd_file 116
+#define BLOG_CHANNEL_ncd_netmask 117
+#define BLOG_CHANNEL_ncd_implode 118
+#define BLOG_CHANNEL_ncd_call2 119
+#define BLOG_CHANNEL_ncd_assert 120
+#define BLOG_CHANNEL_ncd_reboot 121
+#define BLOG_CHANNEL_ncd_explode 122
+#define BLOG_CHANNEL_NCDPlaceholderDb 123
+#define BLOG_CHANNEL_NCDVal 124
+#define BLOG_CHANNEL_ncd_net_ipv6_addr 125
+#define BLOG_CHANNEL_ncd_net_ipv6_route 126
+#define BLOG_CHANNEL_ncd_net_ipv4_addr_in_network 127
+#define BLOG_CHANNEL_ncd_net_ipv6_addr_in_network 128
+#define BLOG_CHANNEL_dostest_server 129
+#define BLOG_CHANNEL_dostest_attacker 130
+#define BLOG_CHANNEL_ncd_timer 131
+#define BLOG_CHANNEL_ncd_file_open 132
+#define BLOG_CHANNEL_ncd_backtrack 133
+#define BLOG_CHANNEL_ncd_socket 134
+#define BLOG_CHANNEL_ncd_depend_scope 135
+#define BLOG_CHANNEL_ncd_substr 136
+#define BLOG_CHANNEL_ncd_sys_start_process 137
+#define BLOG_CHANNEL_NCDBuildProgram 138
+#define BLOG_CHANNEL_ncd_log 139
+#define BLOG_CHANNEL_ncd_log_msg 140
+#define BLOG_CHANNEL_ncd_buffer 141
+#define BLOG_CHANNEL_ncd_getenv 142
+#define BLOG_CHANNEL_BThreadSignal 143
+#define BLOG_CHANNEL_BLockReactor 144
+#define BLOG_CHANNEL_ncd_load_module 145
+#define BLOG_CHANNEL_ncd_basic_functions 146
+#define BLOG_CHANNEL_ncd_objref 147
+#define BLOG_NUM_CHANNELS 148
diff --git a/generated/blog_channels_list.h b/generated/blog_channels_list.h
index d099b2b..930bd8b 100644
--- a/generated/blog_channels_list.h
+++ b/generated/blog_channels_list.h
@@ -89,6 +89,7 @@
 {"udpgw", 4},
 {"UdpGwClient", 4},
 {"SocksUdpGwClient", 4},
+{"SocksUdpClient", 4},
 {"BNetwork", 4},
 {"BConnection", 4},
 {"BSSLConnection", 4},
diff --git a/misc/socks_proto.h b/misc/socks_proto.h
index 41f5a1f..89e2775 100644
--- a/misc/socks_proto.h
+++ b/misc/socks_proto.h
@@ -112,7 +112,15 @@ B_START_PACKED
 struct socks_addr_ipv6 {
     uint8_t addr[16];
     uint16_t port;
-} B_PACKED;    
+} B_PACKED;
+B_END_PACKED
+
+B_START_PACKED
+struct socks_udp_header {
+    uint16_t rsv;
+    uint8_t frag;
+    uint8_t atyp;
+} B_PACKED;
 B_END_PACKED
 
 #endif
diff --git a/socks_udp_client/CMakeLists.txt b/socks_udp_client/CMakeLists.txt
new file mode 100644
index 0000000..166eea5
--- /dev/null
+++ b/socks_udp_client/CMakeLists.txt
@@ -0,0 +1 @@
+badvpn_add_library(socks_udp_client "system;flow;flowextra" "" SocksUdpClient.c)
diff --git a/socks_udp_client/SocksUdpClient.c b/socks_udp_client/SocksUdpClient.c
new file mode 100644
index 0000000..430cb17
--- /dev/null
+++ b/socks_udp_client/SocksUdpClient.c
@@ -0,0 +1,529 @@
+/*
+ * Copyright (C) 2018 Jigsaw Operations LLC
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the author nor the
+ *    names of its contributors may be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+
+#include <misc/balloc.h>
+#include <misc/offset.h>
+#include <misc/byteorder.h>
+#include <misc/compare.h>
+#include <base/BLog.h>
+
+#include <socks_udp_client/SocksUdpClient.h>
+
+#include <generated/blog_channel_SocksUdpClient.h>
+
+#define DNS_PORT 53
+
+static int addr_comparator (void *unused, BAddr *v1, BAddr *v2);
+static struct SocksUdpClient_connection * find_connection_by_addr (SocksUdpClient *o, BAddr addr);
+static void init_localhost4(uint32_t *ip4);
+static void init_localhost6(uint8_t ip6[16]);
+static void socks_state_handler(struct SocksUdpClient_connection *con, int event);
+static void datagram_state_handler(struct SocksUdpClient_connection *con, int event);
+static void send_monitor_handler (struct SocksUdpClient_connection *con);
+static void recv_if_handler_send (struct SocksUdpClient_connection *con, uint8_t *data, int data_len);
+static struct SocksUdpClient_connection * connection_init(SocksUdpClient *o, BAddr local_addr,
+                                                          BAddr first_remote_addr,
+                                                          const uint8_t *first_data,
+                                                          int first_data_len);
+static void connection_free (struct SocksUdpClient_connection *con);
+static void connection_send (struct SocksUdpClient_connection *con, BAddr remote_addr, const uint8_t *data, int data_len);
+static void first_job_handler(struct SocksUdpClient_connection *con);
+static int compute_mtu(int udp_mtu);
+static int get_dns_id(BAddr *remote_addr, const uint8_t *data, int data_len);
+
+int addr_comparator (void *unused, BAddr *v1, BAddr *v2)
+{
+    return BAddr_CompareOrder(v1, v2);
+}
+
+struct SocksUdpClient_connection * find_connection_by_addr (SocksUdpClient *o, BAddr addr)
+{
+    BAVLNode *tree_node = BAVL_LookupExact(&o->connections_tree, &addr);
+    if (!tree_node) {
+        return NULL;
+    }
+    
+    return UPPER_OBJECT(tree_node, struct SocksUdpClient_connection, connections_tree_node);
+}
+
+void init_localhost4(uint32_t *ip4)
+{
+    *ip4 = 1<<24 | 127;
+}
+
+void init_localhost6(uint8_t ip6[16])
+{
+    memset(ip6, 0, 16);
+    ip6[15] = 1;
+}
+
+void socks_state_handler(struct SocksUdpClient_connection *con, int event)
+{
+    switch (event) {
+        case BSOCKSCLIENT_EVENT_UP: {
+            BIPAddr localhost;
+            localhost.type = con->client->server_addr.type;
+            if (localhost.type == BADDR_TYPE_IPV4) {
+                init_localhost4(&localhost.ipv4);
+            } else if (localhost.type == BADDR_TYPE_IPV6) {
+                init_localhost6(localhost.ipv6);
+            } else {
+                BLog(BLOG_ERROR, "Bad address type");
+            }
+            // This will unblock the queue of pending packets.
+            BDatagram_SetSendAddrs(&con->socket, con->socks.bind_addr, localhost);
+        } break;
+        case BSOCKSCLIENT_EVENT_ERROR: {
+            BLog(BLOG_ERROR, "Socks error event");
+        } // Fallthrough
+        case BSOCKSCLIENT_EVENT_ERROR_CLOSED: {
+            connection_free(con);
+        } break;
+        default: {
+            BLog(BLOG_ERROR, "Unknown event");
+        }
+    }
+}
+
+void datagram_state_handler(struct SocksUdpClient_connection *con, int event)
+{
+    if (event == BDATAGRAM_EVENT_ERROR) {
+        char local_buffer[BADDR_MAX_PRINT_LEN];
+        BAddr_Print(&con->local_addr, local_buffer);
+        BLog(BLOG_ERROR, "Failing connection for %s due to a datagram send error", local_buffer);
+        connection_free(con);
+    }
+}
+
+void send_monitor_handler (struct SocksUdpClient_connection *con)
+{
+    // The connection has passed its idle timeout.  Remove it.
+    connection_free(con);
+}
+
+void recv_if_handler_send(struct SocksUdpClient_connection *con, uint8_t *data, int data_len)
+{
+    SocksUdpClient *o = con->client;
+    DebugObject_Access(&con->client->d_obj);
+    ASSERT(data_len >= 0)
+    ASSERT(data_len <= compute_mtu(o->udp_mtu))
+    
+    // accept packet
+    PacketPassInterface_Done(&con->recv_if);
+    
+    // check header
+    if (data_len < sizeof(struct socks_udp_header)) {
+        BLog(BLOG_ERROR, "missing header");
+        return;
+    }
+    struct socks_udp_header *header = (struct socks_udp_header *)data;
+    uint8_t *addr_data = data + sizeof(struct socks_udp_header);
+    
+    // parse address
+    BAddr remote_addr;
+    size_t addr_size;
+    switch (header->atyp) {
+        case SOCKS_ATYP_IPV4: {
+            remote_addr.type = BADDR_TYPE_IPV4;
+            struct socks_addr_ipv4 *addr_ipv4 = (struct socks_addr_ipv4 *)addr_data;
+            remote_addr.ipv4.ip = addr_ipv4->addr;
+            remote_addr.ipv4.port = addr_ipv4->port;
+            addr_size = sizeof(*addr_ipv4);
+        } break;
+        case SOCKS_ATYP_IPV6: {
+            remote_addr.type = BADDR_TYPE_IPV6;
+            struct socks_addr_ipv6 *addr_ipv6 = (struct socks_addr_ipv6 *)addr_data;
+            memcpy(remote_addr.ipv6.ip, addr_ipv6->addr, sizeof(remote_addr.ipv6.ip));
+            remote_addr.ipv6.port = addr_ipv6->port;
+            addr_size = sizeof(*addr_ipv6);
+        } break;
+        default: {
+            BLog(BLOG_ERROR, "Bad address type");
+            return;
+        }
+    }
+    
+    uint8_t *body_data = addr_data + addr_size;
+    size_t body_len = data_len - (body_data - data);
+    
+    // check remaining data
+    if (body_len > o->udp_mtu) {
+        BLog(BLOG_ERROR, "too much data");
+        return;
+    }
+    
+    // pass packet to user
+    SocksUdpClient *client = con->client;
+    client->handler_received(client->user, con->local_addr, remote_addr, body_data, body_len);
+
+    if (con->dns_id >= 0) {
+        // This connection has only been used for a single DNS query.
+        int recv_dns_id = get_dns_id(&remote_addr, body_data, body_len);
+        if (recv_dns_id == con->dns_id) {
+            // We have now forwarded the response, so this connection is no longer needed.
+            connection_free(con);
+        } else {
+            BLog(BLOG_INFO,
+                 "DNS client port received an unexpected non-DNS packet.  "
+                 "Disabling DNS optimization.");
+            con->dns_id = -1;
+        }
+    }
+}
+
+struct SocksUdpClient_connection *connection_init(SocksUdpClient *o, BAddr local_addr,
+                                                  BAddr first_remote_addr,
+                                                  const uint8_t *first_data,
+                                                  int first_data_len)
+{
+    DebugObject_Access(&o->d_obj);
+    ASSERT(o->num_connections <= o->max_connections)
+    ASSERT(!find_connection_by_addr(o, local_addr))
+    
+    char buffer[BADDR_MAX_PRINT_LEN];
+    BAddr_Print(&local_addr, buffer);
+    BLog(BLOG_DEBUG, "Creating new connection for %s", buffer);
+    
+    // allocate structure
+    struct SocksUdpClient_connection *con = (struct SocksUdpClient_connection *)malloc(sizeof(*con));
+    if (!con) {
+        BLog(BLOG_ERROR, "malloc failed");
+        goto fail0;
+    }
+    
+    // init arguments
+    con->client = o;
+    con->local_addr = local_addr;
+    con->first_data = BAlloc(first_data_len);
+    con->first_data_len = first_data_len;
+    con->first_remote_addr = first_remote_addr;
+    memcpy(con->first_data, first_data, first_data_len);
+    
+    con->dns_id = get_dns_id(&first_remote_addr, first_data, first_data_len);
+    
+    BPendingGroup *pg = BReactor_PendingGroup(o->reactor);
+    
+    // init first job, to send the first packet asynchronously.  This has to happen asynchronously
+    // because con->send_writer (a BufferWriter) cannot accept writes until after it is linked with
+    // its PacketBuffer (con->send_buffer), which happens asynchronously.
+    BPending_Init(&con->first_job, pg, (BPending_handler)first_job_handler, con);
+    // Add the first job to the pending set.  BPending acts as a LIFO stack, and first_job_handler
+    // needs to run after async actions that occur in PacketBuffer_Init, so we need to put first_job
+    // on the stack first.
+    BPending_Set(&con->first_job);
+    
+    // Create a datagram socket
+    if (!BDatagram_Init(&con->socket, con->local_addr.type, o->reactor, con,
+                        (BDatagram_handler)datagram_state_handler)) {
+        BLog(BLOG_ERROR, "Failed to create a UDP socket");
+        goto fail1;
+    }
+    
+    // Bind to 127.0.0.1:0 (or [::1]:0).  Port 0 signals the kernel to choose an open port.
+    BAddr socket_addr;
+    socket_addr.type = local_addr.type;
+    if (local_addr.type == BADDR_TYPE_IPV4) {
+        init_localhost4(&socket_addr.ipv4.ip);
+        socket_addr.ipv4.port = 0;
+    } else if (local_addr.type == BADDR_TYPE_IPV6) {
+        init_localhost6(socket_addr.ipv6.ip);
+        socket_addr.ipv6.port = 0;
+    } else {
+        BLog(BLOG_ERROR, "Unknown local address type");
+        goto fail2;
+    }
+    if (!BDatagram_Bind(&con->socket, socket_addr)) {
+        BLog(BLOG_ERROR, "Bind to localhost failed");
+        goto fail2;
+    }
+    
+    // Bind succeeded, so the kernel has found an open port.
+    // Update socket_addr to the actual port that was bound.
+    uint16_t port;
+    if (!BDatagram_GetLocalPort(&con->socket, &port)) {
+        BLog(BLOG_ERROR, "Failed to get bound port");
+        goto fail2;
+    }
+    if (socket_addr.type == BADDR_TYPE_IPV4) {
+        socket_addr.ipv4.port = port;
+    } else {
+        socket_addr.ipv6.port = port;
+    }
+    
+    // Initiate connection to socks server
+    if (!BSocksClient_Init(&con->socks, o->server_addr, o->auth_info, o->num_auth_info, socket_addr,
+                           true, (BSocksClient_handler)socks_state_handler, con, o->reactor)) {
+        BLog(BLOG_ERROR, "Failed to initialize SOCKS client");
+        goto fail2;
+    }
+    
+    // Ensure that the UDP handling pipeline can handle queries big enough to include
+    // all data plus the SOCKS-UDP header.
+    int socks_mtu = compute_mtu(o->udp_mtu);
+    
+    // Send pipeline: send_writer -> send_buffer -> send_monitor -> send_if -> socket.
+    BDatagram_SendAsync_Init(&con->socket, socks_mtu);
+    PacketPassInterface *send_if = BDatagram_SendAsync_GetIf(&con->socket);
+    PacketPassInactivityMonitor_Init(&con->send_monitor, send_if, o->reactor, o->keepalive_time,
+                                     (PacketPassInactivityMonitor_handler)send_monitor_handler, con);
+    BufferWriter_Init(&con->send_writer, compute_mtu(o->udp_mtu), pg);
+    if (!PacketBuffer_Init(&con->send_buffer, BufferWriter_GetOutput(&con->send_writer),
+                           PacketPassInactivityMonitor_GetInput(&con->send_monitor),
+                           SOCKS_UDP_SEND_BUFFER_PACKETS, pg)) {
+        BLog(BLOG_ERROR, "Send buffer init failed");
+        goto fail3;
+    }
+    
+    // Receive pipeline: socket -> recv_buffer -> recv_if
+    BDatagram_RecvAsync_Init(&con->socket, socks_mtu);
+    PacketPassInterface_Init(&con->recv_if, socks_mtu,
+                            (PacketPassInterface_handler_send)recv_if_handler_send, con, pg);
+    if (!SinglePacketBuffer_Init(&con->recv_buffer, BDatagram_RecvAsync_GetIf(&con->socket),
+                                &con->recv_if, pg)) {
+        BLog(BLOG_ERROR, "Receive buffer init failed");
+        goto fail4;
+    }
+    
+    // insert to connections tree
+    ASSERT_EXECUTE(BAVL_Insert(&o->connections_tree, &con->connections_tree_node, NULL))
+    
+    o->num_connections++;
+    
+    return con;
+    
+fail4:
+    PacketPassInterface_Free(&con->recv_if);
+    BDatagram_RecvAsync_Free(&con->socket);
+    PacketBuffer_Free(&con->send_buffer);
+fail3:
+    BufferWriter_Free(&con->send_writer);
+    PacketPassInactivityMonitor_Free(&con->send_monitor);
+    BDatagram_SendAsync_Free(&con->socket);
+fail2:
+    BDatagram_Free(&con->socket);
+fail1:
+    BPending_Free(&con->first_job);
+    BFree(con->first_data);
+    free(con);
+fail0:
+    return NULL;
+}
+
+void connection_free (struct SocksUdpClient_connection *con)
+{
+    SocksUdpClient *o = con->client;
+    DebugObject_Access(&o->d_obj);
+    
+    // decrement number of connections
+    o->num_connections--;
+    
+    // remove from connections tree
+    BAVL_Remove(&o->connections_tree, &con->connections_tree_node);
+    
+    // Free UDP send pipeline components
+    PacketBuffer_Free(&con->send_buffer);
+    BufferWriter_Free(&con->send_writer);
+    PacketPassInactivityMonitor_Free(&con->send_monitor);
+    BDatagram_SendAsync_Free(&con->socket);
+    
+    // Free UDP receive pipeline components
+    SinglePacketBuffer_Free(&con->recv_buffer);
+    PacketPassInterface_Free(&con->recv_if);
+    BDatagram_RecvAsync_Free(&con->socket);
+    
+    // Free UDP socket
+    BDatagram_Free(&con->socket);
+    
+    // Free SOCKS client
+    BSocksClient_Free(&con->socks);
+    
+    BPending_Free(&con->first_job);
+    if (con->first_data) {
+      BFree(con->first_data);
+    }
+    // free structure
+    free(con);
+}
+
+void connection_send (struct SocksUdpClient_connection *con, BAddr remote_addr,
+                      const uint8_t *data, int data_len)
+{
+    SocksUdpClient *o = con->client;
+    DebugObject_Access(&o->d_obj);
+    ASSERT(data_len >= 0)
+    ASSERT(data_len <= o->udp_mtu)
+    
+    if (con->dns_id >= 0) {
+        // So far, this connection has only sent a single DNS query.
+        int new_dns_id = get_dns_id(&remote_addr, data, data_len);
+        if (new_dns_id != con->dns_id) {
+            BLog(BLOG_DEBUG, "Client reused DNS query port.  Disabling DNS optimization.");
+            con->dns_id = -1;
+        }
+    }
+    
+    // Check if we're sending to an IPv4 or IPv6 destination.
+    int atyp;
+    size_t address_size;
+    // write address
+    switch (remote_addr.type) {
+        case BADDR_TYPE_IPV4: {
+            atyp = SOCKS_ATYP_IPV4;
+            address_size = sizeof(struct socks_addr_ipv4);
+        } break;
+        case BADDR_TYPE_IPV6: {
+            atyp = SOCKS_ATYP_IPV6;
+            address_size = sizeof(struct socks_addr_ipv6);
+        } break;
+        default: {
+          BLog(BLOG_ERROR, "bad address type");
+          return;
+        }
+    }
+    
+    // Wrap the payload in a UDP SOCKS header.
+    size_t socks_data_len = sizeof(struct socks_udp_header) + address_size + data_len;
+    if (socks_data_len > compute_mtu(o->udp_mtu)) {
+        BLog(BLOG_ERROR, "Packet is too big: %d > %d", socks_data_len, compute_mtu(o->udp_mtu));
+        return;
+    }
+    uint8_t *socks_data;
+    if (!BufferWriter_StartPacket(&con->send_writer, &socks_data)) {
+        BLog(BLOG_ERROR, "Send buffer is full");
+        return;
+    }
+    // Write header
+    struct socks_udp_header *header = (struct socks_udp_header *)socks_data;
+    header->rsv = 0;
+    header->frag = 0;
+    header->atyp = atyp;
+    uint8_t *addr_data = socks_data + sizeof(struct socks_udp_header);
+    switch (atyp) {
+        case SOCKS_ATYP_IPV4: {
+            struct socks_addr_ipv4 *addr_ipv4 = (struct socks_addr_ipv4 *)addr_data;
+            addr_ipv4->addr = remote_addr.ipv4.ip;
+            addr_ipv4->port = remote_addr.ipv4.port;
+        } break;
+        case SOCKS_ATYP_IPV6: {
+            struct socks_addr_ipv6 *addr_ipv6 = (struct socks_addr_ipv6 *)addr_data;
+            memcpy(addr_ipv6->addr, remote_addr.ipv6.ip, sizeof(addr_ipv6->addr));
+            addr_ipv6->port = remote_addr.ipv6.port;
+        } break;
+    }
+    // write packet to buffer
+    memcpy(addr_data + address_size, data, data_len);
+    BufferWriter_EndPacket(&con->send_writer, socks_data_len);
+}
+
+void first_job_handler(struct SocksUdpClient_connection *con)
+{
+    connection_send(con, con->first_remote_addr, con->first_data, con->first_data_len);
+    BFree(con->first_data);
+    con->first_data = NULL;
+    con->first_data_len = 0;
+}
+
+int compute_mtu(int udp_mtu)
+{
+    return udp_mtu + sizeof(struct socks_udp_header) + sizeof(struct socks_addr_ipv6);
+}
+
+int get_dns_id(BAddr *remote_addr, const uint8_t *data, int data_len)
+{
+    if (BAddr_GetPort(remote_addr) == htons(DNS_PORT) && data_len >= 2) {
+        return (data[0] << 8) + data[1];
+    }
+    return -1;
+}
+
+void SocksUdpClient_Init (SocksUdpClient *o, int udp_mtu, int max_connections, btime_t keepalive_time,
+                          BAddr server_addr, const struct BSocksClient_auth_info *auth_info, size_t num_auth_info,
+                          BReactor *reactor, void *user,
+                          SocksUdpClient_handler_received handler_received)
+{
+    ASSERT(udp_mtu >= 0)
+    ASSERT(compute_mtu(udp_mtu) >= 0)
+    ASSERT(max_connections > 0)
+    
+    // init arguments
+    o->server_addr = server_addr;
+    o->auth_info = auth_info;
+    o->num_auth_info = num_auth_info;
+    o->udp_mtu = udp_mtu;
+    o->max_connections = max_connections;
+    o->num_connections = 0;
+    o->keepalive_time = keepalive_time;
+    o->reactor = reactor;
+    o->user = user;
+    o->handler_received = handler_received;
+    
+    // limit max connections to number of conid's
+    if (o->max_connections > UINT16_MAX + 1) {
+        o->max_connections = UINT16_MAX + 1;
+    }
+    
+    // init connections tree
+    BAVL_Init(&o->connections_tree, OFFSET_DIFF(struct SocksUdpClient_connection, local_addr, connections_tree_node), (BAVL_comparator)addr_comparator, NULL);
+    
+    DebugObject_Init(&o->d_obj);
+}
+
+void SocksUdpClient_Free (SocksUdpClient *o)
+{
+    // free connections
+    while (!BAVL_IsEmpty(&o->connections_tree)) {
+        struct SocksUdpClient_connection *con = UPPER_OBJECT(BAVL_GetFirst(&o->connections_tree), struct SocksUdpClient_connection, connections_tree_node);
+        connection_free(con);
+    }
+
+    DebugObject_Free(&o->d_obj);
+}
+
+void SocksUdpClient_SubmitPacket (SocksUdpClient *o, BAddr local_addr, BAddr remote_addr, const uint8_t *data, int data_len)
+{
+    DebugObject_Access(&o->d_obj);
+    ASSERT(local_addr.type == BADDR_TYPE_IPV4 || local_addr.type == BADDR_TYPE_IPV6)
+    ASSERT(remote_addr.type == BADDR_TYPE_IPV4 || remote_addr.type == BADDR_TYPE_IPV6)
+    ASSERT(data_len >= 0)
+    
+    // lookup connection
+    struct SocksUdpClient_connection *con = find_connection_by_addr(o, local_addr);
+    if (!con) {
+        if (o->num_connections == o->max_connections) {
+            // Drop the packet.
+            BLog(BLOG_ERROR, "Dropping UDP packet, reached max number of connections.");
+            return;
+        }
+        // create new connection and enqueue the packet
+        connection_init(o, local_addr, remote_addr, data, data_len);
+    } else {
+      // send packet
+      connection_send(con, remote_addr, data, data_len);
+    }
+}
diff --git a/socks_udp_client/SocksUdpClient.h b/socks_udp_client/SocksUdpClient.h
new file mode 100644
index 0000000..4e0409e
--- /dev/null
+++ b/socks_udp_client/SocksUdpClient.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2018 Jigsaw Operations LLC
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the author nor the
+ *    names of its contributors may be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef BADVPN_SOCKS_UDP_CLIENT_SOCKSUDPCLIENT_H
+#define BADVPN_SOCKS_UDP_CLIENT_SOCKSUDPCLIENT_H
+
+#include <stdint.h>
+
+#include <base/BPending.h>
+#include <base/DebugObject.h>
+#include <flow/BufferWriter.h>
+#include <flow/PacketBuffer.h>
+#include <flow/SinglePacketBuffer.h>
+#include <flowextra/PacketPassInactivityMonitor.h>
+#include <misc/debug.h>
+#include <misc/socks_proto.h>
+#include <socksclient/BSocksClient.h>
+#include <structure/BAVL.h>
+#include <system/BAddr.h>
+#include <system/BDatagram.h>
+#include <system/BReactor.h>
+#include <system/BTime.h>
+
+// This sets the number of packets to accept while waiting for SOCKS server to authenticate and
+// connect.  A slow or far-away SOCKS server could require 300 ms to connect, and a chatty
+// client (e.g. STUN) could send a packet every 20 ms, so a limit of 16 seems reasonable.
+#define SOCKS_UDP_SEND_BUFFER_PACKETS 16
+
+typedef void (*SocksUdpClient_handler_received) (void *user, BAddr local_addr, BAddr remote_addr, const uint8_t *data, int data_len);
+
+typedef struct {
+    BAddr server_addr;
+    const struct BSocksClient_auth_info *auth_info;
+    size_t num_auth_info;
+    int num_connections;
+    int max_connections;
+    int udp_mtu;
+    btime_t keepalive_time;
+    BReactor *reactor;
+    void *user;
+    SocksUdpClient_handler_received handler_received;
+    BAVL connections_tree;  // By local_addr
+    DebugObject d_obj;
+} SocksUdpClient;
+
+struct SocksUdpClient_connection {
+    SocksUdpClient *client;
+    BAddr local_addr;
+    BSocksClient socks;
+    BufferWriter send_writer;
+    PacketBuffer send_buffer;
+    PacketPassInactivityMonitor send_monitor;
+    PacketPassInterface send_if;
+    BDatagram socket;
+    PacketPassInterface recv_if;
+    SinglePacketBuffer recv_buffer;
+    // The first_* members represent the initial packet, which has to be stored so it can wait for
+    // send_writer to become ready.
+    uint8_t *first_data;
+    int first_data_len;
+    BAddr first_remote_addr;
+    // If all packets sent so far have been sent to the same IP, port 53, with the
+    // same DNS ID, then this is that ID.  Otherwise, it is -1.  This is used to
+    // close ephemeral DNS query connections once a response is received.
+    int dns_id;
+    BPending first_job;
+    BAVLNode connections_tree_node;
+};
+
+/**
+ * Initializes the SOCKS5-UDP client object.
+ * This function does not perform network access, so it will always succeed if the arguments
+ * are valid.
+ * 
+ * Currently, this function only supports connection to a SOCKS5 server that is routable from
+ * localhost (i.e. running on the local machine).  It may be possible to add support for remote
+ * servers, but SOCKS5 does not support UDP if there is a NAT or firewall between the client
+ * and the proxy.
+ * 
+ * @param o the object
+ * @param udp_mtu the maximum size of packets that will be sent through the tunnel
+ * @param max_connections how many local ports to track before dropping packets
+ * @param keepalive_time how long to track an idle local port before forgetting it
+ * @param server_addr SOCKS5 server address.  MUST BE ON LOCALHOST.
+ * @param reactor reactor we live in
+ * @param user value passed to handler
+ * @param handler_received handler for incoming UDP packets
+ */
+void SocksUdpClient_Init (SocksUdpClient *o, int udp_mtu, int max_connections, btime_t keepalive_time,
+                          BAddr server_addr, const struct BSocksClient_auth_info *auth_info, size_t num_auth_info,
+                          BReactor *reactor, void *user, SocksUdpClient_handler_received handler_received);
+void SocksUdpClient_Free (SocksUdpClient *o);
+
+/**
+ * Submit a packet to be sent through the proxy.
+ *
+ * This will reuse an existing connection for packets from local_addr, or create one if
+ * there is none.  If the number of live connections exceeds max_connections, or if the number of
+ * buffered packets from this port exceeds a limit, packets will be dropped silently.
+ * 
+ * As a resource optimization, if a connection has only been used to send one DNS query, then
+ * the connection will be closed and freed once the reply is received.
+ * 
+ * @param o the object
+ * @param local_addr the UDP packet's source address, and the expected destination for replies
+ * @param remote_addr the destination of the packet after it exits the proxy
+ * @param data the packet contents.  Caller retains ownership.
+ */
+void SocksUdpClient_SubmitPacket (SocksUdpClient *o, BAddr local_addr, BAddr remote_addr, const uint8_t *data, int data_len);
+
+#endif
diff --git a/socksclient/BSocksClient.c b/socksclient/BSocksClient.c
index 21415af..a0cae06 100644
--- a/socksclient/BSocksClient.c
+++ b/socksclient/BSocksClient.c
@@ -329,9 +329,11 @@ void recv_handler_done (BSocksClient *o, int data_len)
             int addr_len;
             switch (ntoh8(imsg.atyp)) {
                 case SOCKS_ATYP_IPV4:
+                    o->bind_addr.type = BADDR_TYPE_IPV4;
                     addr_len = sizeof(struct socks_addr_ipv4);
                     break;
                 case SOCKS_ATYP_IPV6:
+                    o->bind_addr.type = BADDR_TYPE_IPV6;
                     addr_len = sizeof(struct socks_addr_ipv6);
                     break;
                 default:
@@ -365,6 +367,26 @@ void recv_handler_done (BSocksClient *o, int data_len)
         case STATE_RECEIVED_REPLY_HEADER: {
             BLog(BLOG_DEBUG, "received reply rest");
             
+            // Record the address of the new socket bound by the server.
+            // For a CONNECT command, this is the address of the TCP client socket to dest_addr.
+            // Knowing this address is usually not important.
+            // For a UDP_ASSOCIATE command, this is the UDP address to which to send SOCKS UDP.
+            // Recording this address is a prerequisite to send traffic on a SOCKS-UDP association.
+            void *addr_buffer = o->buffer + sizeof(struct socks_reply_header);
+            switch (o->bind_addr.type) {
+                case BADDR_TYPE_IPV4: {
+                    struct socks_addr_ipv4 *ip4 = addr_buffer;
+                    o->bind_addr.ipv4.ip = ip4->addr;
+                    o->bind_addr.ipv4.port = ip4->port;
+                } break;
+                case BADDR_TYPE_IPV6: {
+                    struct socks_addr_ipv6 *ip6 = addr_buffer;
+                    memcpy(o->bind_addr.ipv6.ip, ip6->addr, sizeof(ip6->addr));
+                    o->bind_addr.ipv6.port = ip6->port;
+                } break;
+                default: ASSERT(0);
+            }
+            
             // free buffer
             BFree(o->buffer);
             o->buffer = NULL;
@@ -476,7 +498,7 @@ void auth_finished (BSocksClient *o)
     // write request
     struct socks_request_header header;
     header.ver = hton8(SOCKS_VERSION);
-    header.cmd = hton8(SOCKS_CMD_CONNECT);
+    header.cmd = hton8(o->udp ? SOCKS_CMD_UDP_ASSOCIATE : SOCKS_CMD_CONNECT);
     header.rsv = hton8(0);
     switch (o->dest_addr.type) {
         case BADDR_TYPE_IPV4: {
@@ -525,7 +547,7 @@ struct BSocksClient_auth_info BSocksClient_auth_password (const char *username,
 
 int BSocksClient_Init (BSocksClient *o,
                        BAddr server_addr, const struct BSocksClient_auth_info *auth_info, size_t num_auth_info,
-                       BAddr dest_addr, BSocksClient_handler handler, void *user, BReactor *reactor)
+                       BAddr dest_addr, bool udp, BSocksClient_handler handler, void *user, BReactor *reactor)
 {
     ASSERT(!BAddr_IsInvalid(&server_addr))
     ASSERT(dest_addr.type == BADDR_TYPE_IPV4 || dest_addr.type == BADDR_TYPE_IPV6)
@@ -540,6 +562,7 @@ int BSocksClient_Init (BSocksClient *o,
     o->auth_info = auth_info;
     o->num_auth_info = num_auth_info;
     o->dest_addr = dest_addr;
+    o->udp = udp;
     o->handler = handler;
     o->user = user;
     o->reactor = reactor;
diff --git a/socksclient/BSocksClient.h b/socksclient/BSocksClient.h
index f19b3a8..7bb7754 100644
--- a/socksclient/BSocksClient.h
+++ b/socksclient/BSocksClient.h
@@ -35,6 +35,7 @@
 #define BADVPN_SOCKS_BSOCKSCLIENT_H
 
 #include <stdint.h>
+#include <stdbool.h>
 
 #include <misc/debug.h>
 #include <misc/debugerror.h>
@@ -78,6 +79,8 @@ typedef struct {
     const struct BSocksClient_auth_info *auth_info;
     size_t num_auth_info;
     BAddr dest_addr;
+    bool udp;
+    BAddr bind_addr;
     BSocksClient_handler handler;
     void *user;
     BReactor *reactor;
@@ -117,7 +120,7 @@ struct BSocksClient_auth_info BSocksClient_auth_password (const char *username,
  */
 int BSocksClient_Init (BSocksClient *o,
                        BAddr server_addr, const struct BSocksClient_auth_info *auth_info, size_t num_auth_info,
-                       BAddr dest_addr, BSocksClient_handler handler, void *user, BReactor *reactor) WARN_UNUSED;
+                       BAddr dest_addr, bool udp, BSocksClient_handler handler, void *user, BReactor *reactor) WARN_UNUSED;
 
 /**
  * Frees the object.
diff --git a/system/BDatagram.h b/system/BDatagram.h
index 33efb45..63e55ec 100644
--- a/system/BDatagram.h
+++ b/system/BDatagram.h
@@ -123,6 +123,16 @@ void BDatagram_SetSendAddrs (BDatagram *o, BAddr remote_addr, BIPAddr local_addr
  */
 int BDatagram_GetLastReceiveAddrs (BDatagram *o, BAddr *remote_addr, BIPAddr *local_addr);
 
+/**
+ * Returns the bound port.
+ * Fails if and only if a port is not yet bound.
+ * 
+ * @param o the object
+ * @param local_port returns the local bound port.
+ * @return 1 on success, 0 on failure
+ */
+int BDatagram_GetLocalPort (BDatagram *o, uint16_t *local_port);
+
 #ifndef BADVPN_USE_WINAPI
 /**
  * Returns the underlying socket file descriptor of the datagram object.
diff --git a/system/BDatagram_unix.c b/system/BDatagram_unix.c
index b49b865..a6e42e3 100644
--- a/system/BDatagram_unix.c
+++ b/system/BDatagram_unix.c
@@ -734,6 +734,30 @@ int BDatagram_GetLastReceiveAddrs (BDatagram *o, BAddr *remote_addr, BIPAddr *lo
     return 1;
 }
 
+int BDatagram_GetLocalPort (BDatagram *o, uint16_t *local_port)
+{
+    DebugObject_Access(&o->d_obj);
+    
+    struct sys_addr sysaddr;
+    BAddr addr;
+    sysaddr.len = sizeof(sysaddr.addr);
+    if (getsockname(o->fd, &sysaddr.addr.generic, &sysaddr.len) != 0) {
+        BLog(BLOG_ERROR, "getsockname failed");
+        return 0;
+    }
+    addr_sys_to_socket(&addr, sysaddr);
+    if (addr.type == BADDR_TYPE_IPV4) {
+        *local_port = addr.ipv4.port;
+        return 1;
+    }
+    if (addr.type == BADDR_TYPE_IPV6) {
+        *local_port = addr.ipv6.port;
+        return 1;
+    }
+    BLog(BLOG_ERROR, "Unknown address type from getsockname: %d", addr.type);
+    return 0;
+}
+
 int BDatagram_GetFd (BDatagram *o)
 {
     DebugObject_Access(&o->d_obj);
diff --git a/system/BDatagram_win.c b/system/BDatagram_win.c
index b8b9b1f..2cdc9c8 100644
--- a/system/BDatagram_win.c
+++ b/system/BDatagram_win.c
@@ -635,6 +635,31 @@ int BDatagram_GetLastReceiveAddrs (BDatagram *o, BAddr *remote_addr, BIPAddr *lo
     return 1;
 }
 
+int BDatagram_GetLocalPort (BDatagram *o, uint16_t *local_port)
+{
+    DebugObject_Access(&o->d_obj);
+    
+    struct BDatagram_sys_addr sysaddr;
+    BAddr addr;
+    socklen_t addr_size = sizeof(sysaddr.addr.generic);
+    if (getsockname(o->sock, &sysaddr.addr.generic, &addr_size) != 0) {
+        BLog(BLOG_ERROR, "getsockname failed");
+        return 0;
+    }
+    
+    addr_sys_to_socket(&addr, sysaddr);
+    if (addr.type == BADDR_TYPE_IPV4) {
+        *local_port = addr.ipv4.port;
+        return 1;
+    }
+    if (addr.type == BADDR_TYPE_IPV6) {
+        *local_port = addr.ipv6.port;
+        return 1;
+    }
+    BLog(BLOG_ERROR, "Unknown address type from getsockname: %d", addr.type);
+    return 0;
+}
+
 int BDatagram_SetReuseAddr (BDatagram *o, int reuse)
 {
     DebugObject_Access(&o->d_obj);
diff --git a/tun2socks/CMakeLists.txt b/tun2socks/CMakeLists.txt
index 4246fd0..3d5ec32 100644
--- a/tun2socks/CMakeLists.txt
+++ b/tun2socks/CMakeLists.txt
@@ -2,7 +2,7 @@ add_executable(badvpn-tun2socks
     tun2socks.c
     SocksUdpGwClient.c
 )
-target_link_libraries(badvpn-tun2socks system flow tuntap lwip socksclient udpgw_client)
+target_link_libraries(badvpn-tun2socks system flow tuntap lwip socksclient udpgw_client socks_udp_client)
 
 install(
     TARGETS badvpn-tun2socks
diff --git a/tun2socks/SocksUdpGwClient.c b/tun2socks/SocksUdpGwClient.c
index 949d114..5ce68d9 100644
--- a/tun2socks/SocksUdpGwClient.c
+++ b/tun2socks/SocksUdpGwClient.c
@@ -62,7 +62,7 @@ static void try_connect (SocksUdpGwClient *o)
     ASSERT(!BTimer_IsRunning(&o->reconnect_timer))
     
     // init SOCKS client
-    if (!BSocksClient_Init(&o->socks_client, o->socks_server_addr, o->auth_info, o->num_auth_info, o->remote_udpgw_addr, (BSocksClient_handler)socks_client_handler, o, o->reactor)) {
+    if (!BSocksClient_Init(&o->socks_client, o->socks_server_addr, o->auth_info, o->num_auth_info, o->remote_udpgw_addr, false, (BSocksClient_handler)socks_client_handler, o, o->reactor)) {
         BLog(BLOG_ERROR, "BSocksClient_Init failed");
         goto fail0;
     }
diff --git a/tun2socks/tun2socks.c b/tun2socks/tun2socks.c
index 36c72c7..bf951ec 100644
--- a/tun2socks/tun2socks.c
+++ b/tun2socks/tun2socks.c
@@ -65,6 +65,7 @@
 #include <lwip/nd6.h>
 #include <lwip/ip6_frag.h>
 #include <tun2socks/SocksUdpGwClient.h>
+#include <socks_udp_client/SocksUdpClient.h>
 
 #ifndef BADVPN_USE_WINAPI
 #include <base/BLog_syslog.h>
@@ -115,6 +116,7 @@ struct {
     int udpgw_max_connections;
     int udpgw_connection_buffer_size;
     int udpgw_transparent_dns;
+    int socks5_udp;
 } options;
 
 // TCP client
@@ -183,6 +185,9 @@ PacketPassInterface device_read_interface;
 SocksUdpGwClient udpgw_client;
 int udp_mtu;
 
+// SOCKS5-UDP client
+SocksUdpClient socks_udp_client;
+
 // TCP timer
 BTimer tcp_timer;
 int tcp_timer_mod4;
@@ -242,7 +247,8 @@ static void client_socks_recv_initiate (struct tcp_client *client);
 static void client_socks_recv_handler_done (struct tcp_client *client, int data_len);
 static int client_socks_recv_send_out (struct tcp_client *client);
 static err_t client_sent_func (void *arg, struct tcp_pcb *tpcb, u16_t len);
-static void udpgw_client_handler_received (void *unused, BAddr local_addr, BAddr remote_addr, const uint8_t *data, int data_len);
+static void udp_send_packet_to_device (void *unused, BAddr local_addr, BAddr remote_addr, const uint8_t *data, int data_len);
+
 
 int main (int argc, char **argv)
 {
@@ -351,19 +357,19 @@ int main (int argc, char **argv)
         goto fail4;
     }
     
-    if (options.udpgw_remote_server_addr) {
-        // compute maximum UDP payload size we need to pass through udpgw
-        udp_mtu = BTap_GetMTU(&device) - (int)(sizeof(struct ipv4_header) + sizeof(struct udp_header));
-        if (options.netif_ip6addr) {
-            int udp_ip6_mtu = BTap_GetMTU(&device) - (int)(sizeof(struct ipv6_header) + sizeof(struct udp_header));
-            if (udp_mtu < udp_ip6_mtu) {
-                udp_mtu = udp_ip6_mtu;
-            }
-        }
-        if (udp_mtu < 0) {
-            udp_mtu = 0;
+    // compute maximum UDP payload size we need to pass through udpgw
+    udp_mtu = BTap_GetMTU(&device) - (int)(sizeof(struct ipv4_header) + sizeof(struct udp_header));
+    if (options.netif_ip6addr) {
+        int udp_ip6_mtu = BTap_GetMTU(&device) - (int)(sizeof(struct ipv6_header) + sizeof(struct udp_header));
+        if (udp_mtu < udp_ip6_mtu) {
+            udp_mtu = udp_ip6_mtu;
         }
-        
+    }
+    if (udp_mtu < 0) {
+        udp_mtu = 0;
+    }
+
+    if (options.udpgw_remote_server_addr) {
         // make sure our UDP payloads aren't too large for udpgw
         int udpgw_mtu = udpgw_compute_mtu(udp_mtu);
         if (udpgw_mtu < 0 || udpgw_mtu > PACKETPROTO_MAXPAYLOAD) {
@@ -374,11 +380,15 @@ int main (int argc, char **argv)
         // init udpgw client
         if (!SocksUdpGwClient_Init(&udpgw_client, udp_mtu, DEFAULT_UDPGW_MAX_CONNECTIONS, options.udpgw_connection_buffer_size, UDPGW_KEEPALIVE_TIME,
                                    socks_server_addr, socks_auth_info, socks_num_auth_info,
-                                   udpgw_remote_server_addr, UDPGW_RECONNECT_TIME, &ss, NULL, udpgw_client_handler_received
+                                   udpgw_remote_server_addr, UDPGW_RECONNECT_TIME, &ss, NULL, udp_send_packet_to_device
         )) {
             BLog(BLOG_ERROR, "SocksUdpGwClient_Init failed");
             goto fail4a;
         }
+    } else if (options.socks5_udp) {
+        SocksUdpClient_Init(&socks_udp_client, udp_mtu, DEFAULT_UDPGW_MAX_CONNECTIONS,
+                            UDPGW_KEEPALIVE_TIME, socks_server_addr, socks_auth_info,
+                            socks_num_auth_info, &ss, NULL, udp_send_packet_to_device);
     }
     
     // init lwip init job
@@ -440,6 +450,8 @@ fail5:
     BPending_Free(&lwip_init_job);
     if (options.udpgw_remote_server_addr) {
         SocksUdpGwClient_Free(&udpgw_client);
+    } else if (options.socks5_udp) {
+        SocksUdpClient_Free(&socks_udp_client);
     }
 fail4a:
     SinglePacketBuffer_Free(&device_read_buffer);
@@ -502,6 +514,7 @@ void print_help (const char *name)
         "        [--udpgw-max-connections <number>]\n"
         "        [--udpgw-connection-buffer-size <number>]\n"
         "        [--udpgw-transparent-dns]\n"
+        "        [--socks5-udp]\n"
         "Address format is a.b.c.d:port (IPv4) or [addr]:port (IPv6).\n",
         name
     );
@@ -542,6 +555,7 @@ int parse_arguments (int argc, char *argv[])
     options.udpgw_max_connections = DEFAULT_UDPGW_MAX_CONNECTIONS;
     options.udpgw_connection_buffer_size = DEFAULT_UDPGW_CONNECTION_BUFFER_SIZE;
     options.udpgw_transparent_dns = 0;
+    options.socks5_udp = 0;
     
     int i;
     for (i = 1; i < argc; i++) {
@@ -719,6 +733,9 @@ int parse_arguments (int argc, char *argv[])
         else if (!strcmp(arg, "--udpgw-transparent-dns")) {
             options.udpgw_transparent_dns = 1;
         }
+        else if (!strcmp(arg, "--socks5-udp")) {
+            options.socks5_udp = 1;
+        }
         else {
             fprintf(stderr, "unknown option: %s\n", arg);
             return 0;
@@ -1050,7 +1067,7 @@ int process_device_udp_packet (uint8_t *data, int data_len)
     ASSERT(data_len >= 0)
     
     // do nothing if we don't have udpgw
-    if (!options.udpgw_remote_server_addr) {
+    if (!options.udpgw_remote_server_addr && !options.socks5_udp) {
         goto fail;
     }
     
@@ -1155,8 +1172,13 @@ int process_device_udp_packet (uint8_t *data, int data_len)
         goto fail;
     }
     
-    // submit packet to udpgw
-    SocksUdpGwClient_SubmitPacket(&udpgw_client, local_addr, remote_addr, is_dns, data, data_len);
+    if (options.udpgw_remote_server_addr) {
+        // submit packet to udpgw
+        SocksUdpGwClient_SubmitPacket(&udpgw_client, local_addr, remote_addr,
+                                      is_dns, data, data_len);
+    } else if (options.socks5_udp) {
+        SocksUdpClient_SubmitPacket(&socks_udp_client, local_addr, remote_addr, data, data_len);
+    }
     
     return 1;
     
@@ -1305,7 +1327,7 @@ err_t listener_accept_func (void *arg, struct tcp_pcb *newpcb, err_t err)
     
     // init SOCKS
     if (!BSocksClient_Init(&client->socks_client, socks_server_addr, socks_auth_info, socks_num_auth_info,
-                           addr, (BSocksClient_handler)client_socks_handler, client, &ss)) {
+                           addr, false, (BSocksClient_handler)client_socks_handler, client, &ss)) {
         BLog(BLOG_ERROR, "listener accept: BSocksClient_Init failed");
         goto fail1;
     }
@@ -1816,9 +1838,8 @@ out:
     return (DEAD_KILLED > 0) ? ERR_ABRT : ERR_OK;
 }
 
-void udpgw_client_handler_received (void *unused, BAddr local_addr, BAddr remote_addr, const uint8_t *data, int data_len)
+void udp_send_packet_to_device (void *unused, BAddr local_addr, BAddr remote_addr, const uint8_t *data, int data_len)
 {
-    ASSERT(options.udpgw_remote_server_addr)
     ASSERT(local_addr.type == BADDR_TYPE_IPV4 || local_addr.type == BADDR_TYPE_IPV6)
     ASSERT(local_addr.type == remote_addr.type)
     ASSERT(data_len >= 0)