--- a/megawifi.c	Sun Apr 19 00:59:49 2020 -0700
+++ b/megawifi.c	Sat May 09 23:39:44 2020 -0700
@@ -6,15 +6,28 @@
 #define WINVER 0x501
 #include <winsock2.h>
 #include <ws2tcpip.h>
+#include <sys/param.h>
 #else
 #include <sys/socket.h>
+#include <arpa/inet.h>
 #include <unistd.h>
 #include <netinet/in.h>
+#include <netdb.h>
 #endif
 #include <errno.h>
 #include <fcntl.h>
+#include <time.h>
 #include "genesis.h"
 #include "net.h"
+#include "util.h"
+
+#ifdef _WIN32
+#  if BYTE_ORDER == LITTLE_ENDIAN
+#define htobe64(val)   ((((uint64_t)htonl((val)&0xFFFFFFFF))<<32) | htonl((val)>>32))
+#  else
+#define htobe64(val)	(val)
+#  endif
+#endif
 
 enum {
 	TX_IDLE,
@@ -25,7 +38,7 @@
 };
 #define STX 0x7E
 #define ETX 0x7E
-#define MAX_RECV_SIZE 1440
+#define MAX_RECV_SIZE 1460
 
 #define E(N) N
 enum {
@@ -43,7 +56,7 @@
 #define MSG_NOSIGNAL 0
 #endif
 
-enum {
+enum mw_state {
 	STATE_IDLE=1,
 	STATE_AP_JOIN,
 	STATE_SCAN,
@@ -51,6 +64,21 @@
 	STATE_TRANSPARENT
 };
 
+enum {
+	SOCKST_NONE = 0,
+	SOCKST_TCP_LISTEN,
+	SOCKST_TCP_EST,
+	SOCKST_UDP_READY
+};
+
+// TCP/UDP address message
+struct mw_addr_msg {
+	char dst_port[6];
+	char src_port[6];
+	uint8_t channel;
+	char host[];
+};
+
 #define FLAG_ONLINE 
 
 typedef struct {
@@ -68,6 +96,7 @@
 	uint8_t  flags;
 	uint8_t  transmit_buffer[4096];
 	uint8_t  receive_buffer[4096];
+	struct sockaddr_in remote_addr[15];	// Needed for UDP sockets
 } megawifi;
 
 static megawifi *get_megawifi(void *context)
@@ -75,11 +104,12 @@
 	m68k_context *m68k = context;
 	genesis_context *gen = m68k->system;
 	if (!gen->extra) {
+		socket_init();
 		gen->extra = calloc(1, sizeof(megawifi));
 		megawifi *mw = gen->extra;
 		mw->module_state = STATE_IDLE;
-		for (int i = 0; i < 15; i++)
-		{
+		mw->flags = 0xE0; // cfg_ok, dt_ok, online
+		for (int i = 0; i < 15; i++) {
 			mw->sock_fds[i] = -1;
 		}
 	}
@@ -112,14 +142,88 @@
 	mw->receive_bytes += count;
 }
 
-static void mw_puts(megawifi *mw, char *s)
+static void mw_puts(megawifi *mw, const char *s)
+{
+	size_t len = strlen(s);
+	mw_copy(mw, (uint8_t*)s, len);
+}
+
+static void udp_recv(megawifi *mw, uint8_t idx)
 {
-	uint32_t len = strlen(s);
-	if ((mw->receive_bytes + len) > sizeof(mw->receive_buffer)) {
-		return;
+	ssize_t recvd;
+	int s = mw->sock_fds[idx];
+	struct sockaddr_in remote;
+	socklen_t addr_len = sizeof(struct sockaddr_in);
+
+	if (mw->remote_addr[idx].sin_addr.s_addr != htonl(INADDR_ANY)) {
+		// Receive only from specified address
+		recvd = recvfrom(s, (char*)mw->receive_buffer + 3, MAX_RECV_SIZE, 0,
+				(struct sockaddr*)&remote, &addr_len);
+		if (recvd > 0) {
+			if (remote.sin_addr.s_addr != mw->remote_addr[idx].sin_addr.s_addr) {
+				printf("Discarding UDP packet from unknown addr %s:%d\n",
+						inet_ntoa(remote.sin_addr), ntohs(remote.sin_port));
+				recvd = 0;
+			}
+		}
+	} else {
+		// Reuse mode, data is preceded by remote IPv4 and port
+		recvd = recvfrom(s, (char*)mw->receive_buffer + 9, MAX_RECV_SIZE - 6,
+				0, (struct sockaddr*)&remote, &addr_len);
+		if (recvd > 0) {
+			mw->receive_buffer[3] = remote.sin_addr.s_addr;
+			mw->receive_buffer[4] = remote.sin_addr.s_addr>>8;
+			mw->receive_buffer[5] = remote.sin_addr.s_addr>>16;
+			mw->receive_buffer[6] = remote.sin_addr.s_addr>>24;
+			mw->receive_buffer[7] = remote.sin_port;
+			mw->receive_buffer[8] = remote.sin_port>>8;
+			recvd += 6;
+		}
 	}
-	memcpy(mw->receive_buffer + mw->receive_bytes, s, len);
-	mw->receive_bytes += len;
+
+	if (recvd > 0) {
+		mw_putc(mw, STX);
+		mw_putc(mw, (recvd >> 8) | ((idx+1) << 4));
+		mw_putc(mw, recvd);
+		mw->receive_bytes += recvd;
+		mw_putc(mw, ETX);
+		//should this set the channel flag?
+	} else if (recvd < 0 && !socket_error_is_wouldblock()) {
+		socket_close(mw->sock_fds[idx]);
+		mw->channel_state[idx] = SOCKST_NONE;
+		mw->channel_flags |= 1 << (idx + 1);
+	}
+}
+
+static void udp_send(megawifi *mw, uint8_t idx)
+{
+	struct sockaddr_in remote;
+	int s = mw->sock_fds[idx];
+	int sent;
+	char *data = (char*)mw->transmit_buffer;
+
+	if (mw->remote_addr[idx].sin_addr.s_addr != htonl(INADDR_ANY)) {
+		sent = sendto(s, data, mw->transmit_bytes, 0, (struct sockaddr*)&mw->remote_addr[idx],
+				sizeof(struct sockaddr_in));
+	} else {
+		// Reuse mode, extract address from leading bytes
+		// NOTE: mw->remote_addr[idx].sin_addr.s_addr == INADDR_ANY
+		remote.sin_addr.s_addr = *((int32_t*)data);
+		remote.sin_port = *((int16_t*)(data + 4));
+		remote.sin_family = AF_INET;
+		memset(remote.sin_zero, 0, sizeof(remote.sin_zero));
+		sent = sendto(s, data + 6, mw->transmit_bytes - 6, 0, (struct sockaddr*)&remote,
+				sizeof(struct sockaddr_in)) + 6;
+	}
+	if (sent < 0 && !socket_error_is_wouldblock()) {
+		socket_close(s);
+		mw->sock_fds[idx] = -1;
+		mw->channel_state[idx] = SOCKST_NONE;
+		mw->channel_flags |= 1 << (idx + 1);
+	} else if (sent < mw->transmit_bytes) {
+		//TODO: save this data somewhere so it can be sent in poll_socket
+		printf("Sent %d bytes on channel %d, but %d were requested\n", sent, idx + 1, mw->transmit_bytes);
+	}
 }
 
 static void poll_socket(megawifi *mw, uint8_t channel)
@@ -127,28 +231,25 @@
 	if (mw->sock_fds[channel] < 0) {
 		return;
 	}
-	if (mw->channel_state[channel] == 1) {
+	if (mw->channel_state[channel] == SOCKST_TCP_LISTEN) {
 		int res = accept(mw->sock_fds[channel], NULL, NULL);
 		if (res >= 0) {
-			close(mw->sock_fds[channel]);
-#ifndef _WIN32
-//FIXME: Set nonblocking on Windows too
-			fcntl(res, F_SETFL, O_NONBLOCK);
-#endif
+			socket_close(mw->sock_fds[channel]);
+			socket_blocking(res, 0);
 			mw->sock_fds[channel] = res;
-			mw->channel_state[channel] = 2;
+			mw->channel_state[channel] = SOCKST_TCP_EST;
 			mw->channel_flags |= 1 << (channel + 1);
 		} else if (errno != EAGAIN && errno != EWOULDBLOCK) {
-			close(mw->sock_fds[channel]);
-			mw->channel_state[channel] = 0;
+			socket_close(mw->sock_fds[channel]);
+			mw->channel_state[channel] = SOCKST_NONE;
 			mw->channel_flags |= 1 << (channel + 1);
 		}
-	} else if (mw->channel_state[channel] == 2 && mw->receive_bytes < sizeof(mw->receive_buffer) - 4) {
+	} else if (mw->channel_state[channel] == SOCKST_TCP_EST && mw->receive_bytes < (sizeof(mw->receive_buffer) - 4)) {
 		size_t max = sizeof(mw->receive_buffer) - 4 - mw->receive_bytes;
 		if (max > MAX_RECV_SIZE) {
 			max = MAX_RECV_SIZE;
 		}
-		int bytes = recv(mw->sock_fds[channel], mw->receive_buffer + mw->receive_bytes + 3, max, 0);
+		int bytes = recv(mw->sock_fds[channel], (char*)(mw->receive_buffer + mw->receive_bytes + 3), max, 0);
 		if (bytes > 0) {
 			mw_putc(mw, STX);
 			mw_putc(mw, bytes >> 8 | (channel+1) << 4);
@@ -156,11 +257,13 @@
 			mw->receive_bytes += bytes;
 			mw_putc(mw, ETX);
 			//should this set the channel flag?
-		} else if (bytes < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
-			close(mw->sock_fds[channel]);
-			mw->channel_state[channel] = 0;
+		} else if (bytes < 0 && !socket_error_is_wouldblock()) {
+			socket_close(mw->sock_fds[channel]);
+			mw->channel_state[channel] = SOCKST_NONE;
 			mw->channel_flags |= 1 << (channel + 1);
 		}
+	} else if (mw->channel_state[channel] == SOCKST_UDP_READY && !mw->receive_bytes) {
+		udp_recv(mw, channel);
 	}
 }
 
@@ -172,6 +275,7 @@
 	}
 }
 
+
 static void start_reply(megawifi *mw, uint8_t cmd)
 {
 	mw_putc(mw, STX);
@@ -197,152 +301,443 @@
 	mw_putc(mw, ETX);
 }
 
-static void process_packet(megawifi *mw)
+static void cmd_ap_cfg_get(megawifi *mw)
+{
+	char ssid[32] = {0};
+	char pass[64] = {0};
+	uint8_t slot = mw->transmit_buffer[4];
+
+	sprintf(ssid, "BLASTEM! SSID %d", slot + 1);
+	sprintf(pass, "BLASTEM! PASS %d", slot + 1);
+	start_reply(mw, CMD_OK);
+	mw_putc(mw, slot);
+	mw_putc(mw, 7);	/// 11bgn
+	mw_copy(mw, (uint8_t*)ssid, 32);
+	mw_copy(mw, (uint8_t*)pass, 64);
+	end_reply(mw);
+}
+
+static void cmd_ip_cfg_get(megawifi *mw)
+{
+	uint32_t ipv4s[5] = {0};
+
+	start_reply(mw, CMD_OK);
+	mw_putc(mw, mw->transmit_buffer[4]);
+	mw_putc(mw, 0);
+	mw_putc(mw, 0);
+	mw_putc(mw, 0);
+	mw_copy(mw, (uint8_t*)ipv4s, sizeof(ipv4s));
+	end_reply(mw);
+}
+
+static void cmd_tcp_con(megawifi *mw, uint32_t size)
+{
+	struct mw_addr_msg *addr = (struct mw_addr_msg*)(mw->transmit_buffer + 4);
+	struct addrinfo hints;
+	struct addrinfo *res = NULL;
+	int s;
+	int err;
+
+	uint8_t channel = addr->channel;
+	if (!channel || channel > 15 || mw->sock_fds[channel - 1] >= 0) {
+		start_reply(mw, CMD_ERROR);
+		end_reply(mw);
+		return;
+	}
+	channel--;
+
+	memset(&hints, 0, sizeof(hints));
+	hints.ai_family = AF_INET;
+#ifndef _WIN32
+	hints.ai_flags = AI_NUMERICSERV;
+#endif
+	hints.ai_socktype = SOCK_STREAM;
+
+	if ((err = getaddrinfo(addr->host, addr->dst_port, &hints, &res)) != 0) {
+		printf("getaddrinfo failed: %s\n", gai_strerror(err));
+		start_reply(mw, CMD_ERROR);
+		end_reply(mw);
+		return;
+	}
+
+	s = socket(AF_INET, SOCK_STREAM, 0);
+	if (s < 0) {
+		goto err;
+	}
+
+	// Should this be handled in a separate thread to avoid blocking emulation?
+	if (connect(s, res->ai_addr, res->ai_addrlen) != 0) {
+		goto err;
+	}
+
+	socket_blocking(s, 0);
+	mw->sock_fds[channel] = s;
+	mw->channel_state[channel] = SOCKST_TCP_EST;
+	mw->channel_flags |= 1 << (channel + 1);
+	printf("Connection established on ch %d with %s:%s\n", channel + 1,
+			addr->host, addr->dst_port);
+
+	if (res) {
+		freeaddrinfo(res);
+	}
+	start_reply(mw, CMD_OK);
+	end_reply(mw);
+	return;
+
+err:
+	freeaddrinfo(res);
+	printf("Connection to %s:%s failed, %s\n", addr->host, addr->dst_port, strerror(errno));
+	start_reply(mw, CMD_ERROR);
+	end_reply(mw);
+}
+
+static void cmd_close(megawifi *mw)
+{
+	int channel = mw->transmit_buffer[4] - 1;
+
+	if (channel >= 15 || mw->sock_fds[channel] < 0) {
+		start_reply(mw, CMD_ERROR);
+		end_reply(mw);
+		return;
+	}
+
+	socket_close(mw->sock_fds[channel]);
+	mw->sock_fds[channel] = -1;
+	mw->channel_state[channel] = SOCKST_NONE;
+	mw->channel_flags |= 1 << (channel + 1);
+	start_reply(mw, CMD_OK);
+	end_reply(mw);
+}
+
+static void cmd_udp_set(megawifi *mw)
 {
-	if (mw->transmit_channel == 0) {
-		uint32_t command = mw->transmit_buffer[0] << 8 | mw->transmit_buffer[1];
-		uint32_t size = mw->transmit_buffer[2] << 8 | mw->transmit_buffer[3];
-		if (size > mw->transmit_bytes - 4) {
-			size = mw->transmit_bytes - 4;
+	struct mw_addr_msg *addr = (struct mw_addr_msg*)(mw->transmit_buffer + 4);
+	unsigned int local_port, remote_port;
+	int s;
+	struct addrinfo *raddr;
+	struct addrinfo hints;
+	struct sockaddr_in local;
+	int err;
+
+	uint8_t channel = addr->channel;
+	if (!channel || channel > 15 || mw->sock_fds[channel - 1] >= 0) {
+		goto err;
+	}
+	channel--;
+	local_port = atoi(addr->src_port);
+	remote_port = atoi(addr->dst_port);
+
+	if ((s = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
+		printf("Datagram socket creation failed\n");
+		goto err;
+	}
+
+	memset(local.sin_zero, 0, sizeof(local.sin_zero));
+	local.sin_family = AF_INET;
+	local.sin_addr.s_addr = htonl(INADDR_ANY);
+	local.sin_port = htons(local_port);
+	if (remote_port && addr->host[0]) {
+		// Communication with remote peer
+		printf("Set UDP ch %d, port %d to addr %s:%d\n", addr->channel,
+				local_port, addr->host, remote_port);
+
+		memset(&hints, 0, sizeof(hints));
+		hints.ai_family = AF_INET;
+#ifndef _WIN32
+		hints.ai_flags = AI_NUMERICSERV;
+#endif
+		hints.ai_socktype = SOCK_DGRAM;
+
+		if ((err = getaddrinfo(addr->host, addr->dst_port, &hints, &raddr)) != 0) {
+			printf("getaddrinfo failed: %s\n", gai_strerror(err));
+			goto err;
 		}
-		int orig_receive_bytes = mw->receive_bytes;
-		switch (command)
-		{
-		case CMD_VERSION:
+		mw->remote_addr[channel] = *((struct sockaddr_in*)raddr->ai_addr);
+		freeaddrinfo(raddr);
+	} else if (local_port) {
+		// Server in reuse mode
+		printf("Set UDP ch %d, src port %d\n", addr->channel, local_port);
+		mw->remote_addr[channel] = local;
+	} else {
+		printf("Invalid UDP socket data\n");
+		goto err;
+	}
+
+	if (bind(s, (struct sockaddr*)&local, sizeof(struct sockaddr_in)) < 0) {
+		printf("bind to port %d failed\n", local_port);
+		goto err;
+	}
+
+	socket_blocking(s, 0);
+	mw->sock_fds[channel] = s;
+	mw->channel_state[channel] = SOCKST_UDP_READY;
+	mw->channel_flags |= 1 << (channel + 1);
+
+	start_reply(mw, CMD_OK);
+	end_reply(mw);
+
+	return;
+
+err:
+	start_reply(mw, CMD_ERROR);
+	end_reply(mw);
+}
+
+#define AVATAR_BYTES	(32 * 48 / 2)
+static void cmd_gamertag_get(megawifi *mw)
+{
+	uint32_t id = htonl(1);
+	char buf[AVATAR_BYTES];
+
+	start_reply(mw, CMD_OK);
+	// TODO Get items from config file
+	mw_copy(mw, (uint8_t*)&id, 4);
+	strncpy(buf, "doragasu on Blastem!", 32);
+	mw_copy(mw, (uint8_t*)buf, 32);
+	strncpy(buf, "My cool password", 32);
+	mw_copy(mw, (uint8_t*)buf, 32);
+	strncpy(buf, "All your WiFi are belong to me!", 32);
+	mw_copy(mw, (uint8_t*)buf, 32);
+	memset(buf, 0, 64); // Telegram token
+	mw_copy(mw, (uint8_t*)buf, 64);
+	mw_copy(mw, (uint8_t*)buf, AVATAR_BYTES); // Avatar tiles
+	mw_copy(mw, (uint8_t*)buf, 32); // Avatar palette
+	end_reply(mw);
+}
+
+static void cmd_hrng_get(megawifi *mw)
+{
+	uint16_t len = (mw->transmit_buffer[4]<<8) + mw->transmit_buffer[5];
+	if (len > (MAX_RECV_SIZE - 4)) {
+		start_reply(mw, CMD_ERROR);
+		end_reply(mw);
+		return;
+	}
+	// Pseudo-random, but who cares
+	start_reply(mw, CMD_OK);
+	srand(time(NULL));
+	for (uint16_t i = 0; i < len; i++) {
+		mw_putc(mw, rand());
+	}
+	end_reply(mw);
+}
+
+static void cmd_datetime(megawifi *mw)
+{
+	start_reply(mw, CMD_OK);
+#ifdef _WIN32
+	__time64_t t = _time64(NULL);
+	int64_t t_be = htobe64(t);
+	mw_copy(mw, (uint8_t*)&t_be, sizeof(int64_t));
+	mw_puts(mw, _ctime64(&t));
+#else
+	time_t t = time(NULL);
+	int64_t t_be = htobe64(t);
+	mw_copy(mw, (uint8_t*)&t_be, sizeof(int64_t));
+	mw_puts(mw, ctime(&t));
+#endif
+
+	mw_putc(mw, '\0');
+	end_reply(mw);
+}
+
+static void process_command(megawifi *mw)
+{
+	uint32_t command = mw->transmit_buffer[0] << 8 | mw->transmit_buffer[1];
+	uint32_t size = mw->transmit_buffer[2] << 8 | mw->transmit_buffer[3];
+	if (size > mw->transmit_bytes - 4) {
+		size = mw->transmit_bytes - 4;
+	}
+	int orig_receive_bytes = mw->receive_bytes;
+	switch (command)
+	{
+	case CMD_VERSION:
+		start_reply(mw, CMD_OK);
+		mw_putc(mw, 1);
+		mw_putc(mw, 3);
+		mw_putc(mw, 0);
+		mw_puts(mw, "blastem");
+		mw_putc(mw, '\0');
+		end_reply(mw);
+		break;
+	case CMD_ECHO:
+		mw->receive_bytes = mw->transmit_bytes;
+		memcpy(mw->receive_buffer, mw->transmit_buffer, mw->transmit_bytes);
+		break;
+	case CMD_AP_CFG_GET:
+		cmd_ap_cfg_get(mw);
+		break;
+	case CMD_IP_CURRENT: {
+		iface_info i;
+		if (get_host_address(&i)) {
 			start_reply(mw, CMD_OK);
-			mw_putc(mw, 1);
-			mw_putc(mw, 0);
-			mw_puts(mw, "blastem");
-			end_reply(mw);
-			break;
-		case CMD_ECHO:
-			mw->receive_bytes = mw->transmit_bytes;
-			memcpy(mw->receive_buffer, mw->transmit_buffer, mw->transmit_bytes);
-			break;
-		case CMD_IP_CURRENT: {
-			iface_info i;
-			if (get_host_address(&i)) {
-				start_reply(mw, CMD_OK);
-				//config number and reserved bytes
-				mw_set(mw, 0, 4);
-				//ip
-				mw_copy(mw, i.ip, sizeof(i.ip));
-				//net mask
-				mw_copy(mw, i.net_mask, sizeof(i.net_mask));
-				//gateway guess
-				mw_putc(mw, i.ip[0] & i.net_mask[0]);
-				mw_putc(mw, i.ip[1] & i.net_mask[1]);
-				mw_putc(mw, i.ip[2] & i.net_mask[2]);
-				mw_putc(mw, (i.ip[3] & i.net_mask[3]) + 1);
-				//dns
-				static const uint8_t localhost[] = {127,0,0,1};
-				mw_copy(mw, localhost, sizeof(localhost));
-				mw_copy(mw, localhost, sizeof(localhost));
-				
-			} else {
-				start_reply(mw, CMD_ERROR);
-			}
+			//config number and reserved bytes
+			mw_set(mw, 0, 4);
+			//ip
+			mw_copy(mw, i.ip, sizeof(i.ip));
+			//net mask
+			mw_copy(mw, i.net_mask, sizeof(i.net_mask));
+			//gateway guess
+			mw_putc(mw, i.ip[0] & i.net_mask[0]);
+			mw_putc(mw, i.ip[1] & i.net_mask[1]);
+			mw_putc(mw, i.ip[2] & i.net_mask[2]);
+			mw_putc(mw, (i.ip[3] & i.net_mask[3]) + 1);
+			//dns
+			static const uint8_t localhost[] = {127,0,0,1};
+			mw_copy(mw, localhost, sizeof(localhost));
+			mw_copy(mw, localhost, sizeof(localhost));
+			
+		} else {
+			start_reply(mw, CMD_ERROR);
+		}
+		end_reply(mw);
+		break;
+	}
+	case CMD_IP_CFG_GET:
+		cmd_ip_cfg_get(mw);
+		break;
+	case CMD_DEF_AP_CFG_GET:
+		start_reply(mw, CMD_OK);
+		mw_putc(mw, 0);
+		end_reply(mw);
+		break;
+	case CMD_AP_JOIN:
+		mw->module_state = STATE_READY;
+		start_reply(mw, CMD_OK);
+		end_reply(mw);
+		break;
+	case CMD_TCP_CON:
+		cmd_tcp_con(mw, size);
+		break;
+	case CMD_TCP_BIND:{
+		if (size < 7){
+			start_reply(mw, CMD_ERROR);
 			end_reply(mw);
 			break;
 		}
-		case CMD_AP_JOIN:
-			mw->module_state = STATE_READY;
-			start_reply(mw, CMD_OK);
+		uint8_t channel = mw->transmit_buffer[10];
+		if (!channel || channel > 15) {
+			start_reply(mw, CMD_ERROR);
+			end_reply(mw);
+			break;
+		}
+		channel--;
+		if (mw->sock_fds[channel] >= 0) {
+			socket_close(mw->sock_fds[channel]);
+		}
+		mw->sock_fds[channel] = socket(AF_INET, SOCK_STREAM, 0);
+		if (mw->sock_fds[channel] < 0) {
+			start_reply(mw, CMD_ERROR);
 			end_reply(mw);
 			break;
-		case CMD_TCP_BIND:{
-			if (size < 7){
-				start_reply(mw, CMD_ERROR);
-				end_reply(mw);
-				break;
-			}
-			uint8_t channel = mw->transmit_buffer[10];
-			if (!channel || channel > 15) {
-				start_reply(mw, CMD_ERROR);
-				end_reply(mw);
-				break;
-			}
-			channel--;
-			if (mw->sock_fds[channel] >= 0) {
-				close(mw->sock_fds[channel]);
-			}
-			mw->sock_fds[channel] = socket(AF_INET, SOCK_STREAM, 0);
-			if (mw->sock_fds[channel] < 0) {
-				start_reply(mw, CMD_ERROR);
-				end_reply(mw);
-				break;
-			}
-			int value = 1;
-			setsockopt(mw->sock_fds[channel], SOL_SOCKET, SO_REUSEADDR, &value, sizeof(value));
-			struct sockaddr_in bind_addr;
-			memset(&bind_addr, 0, sizeof(bind_addr));
-			bind_addr.sin_family = AF_INET;
-			bind_addr.sin_port = htons(mw->transmit_buffer[8] << 8 | mw->transmit_buffer[9]);
-			if (bind(mw->sock_fds[channel], (struct sockaddr *)&bind_addr, sizeof(bind_addr)) != 0) {
-				close(mw->sock_fds[channel]);
-				mw->sock_fds[channel] = -1;
-				start_reply(mw, CMD_ERROR);
-				end_reply(mw);
-				break;
-			}
-			int res = listen(mw->sock_fds[channel], 2);
-			start_reply(mw, res ? CMD_ERROR : CMD_OK);
-			if (res) {
-				close(mw->sock_fds[channel]);
-				mw->sock_fds[channel] = -1;
-			} else {
-				mw->channel_flags |= 1 << (channel + 1);
-				mw->channel_state[channel] = 1;
-#ifndef _WIN32
-//FIXME: Set nonblocking on Windows too
-				fcntl(mw->sock_fds[channel], F_SETFL, O_NONBLOCK);
-#endif
-			}
+		}
+		int value = 1;
+		setsockopt(mw->sock_fds[channel], SOL_SOCKET, SO_REUSEADDR, (char*)&value, sizeof(value));
+		struct sockaddr_in bind_addr;
+		memset(&bind_addr, 0, sizeof(bind_addr));
+		bind_addr.sin_family = AF_INET;
+		bind_addr.sin_port = htons(mw->transmit_buffer[8] << 8 | mw->transmit_buffer[9]);
+		if (bind(mw->sock_fds[channel], (struct sockaddr *)&bind_addr, sizeof(bind_addr)) != 0) {
+			socket_close(mw->sock_fds[channel]);
+			mw->sock_fds[channel] = -1;
+			start_reply(mw, CMD_ERROR);
+			end_reply(mw);
+			break;
+		}
+		int res = listen(mw->sock_fds[channel], 2);
+		start_reply(mw, res ? CMD_ERROR : CMD_OK);
+		if (res) {
+			socket_close(mw->sock_fds[channel]);
+			mw->sock_fds[channel] = -1;
+		} else {
+			mw->channel_flags |= 1 << (channel + 1);
+			mw->channel_state[channel] = SOCKST_TCP_LISTEN;
+			socket_blocking(mw->sock_fds[channel], 0);
+		}
+		end_reply(mw);
+		break;
+	}
+	case CMD_CLOSE:
+		cmd_close(mw);
+		break;
+	case CMD_UDP_SET:
+		cmd_udp_set(mw);
+		break;
+	case CMD_SOCK_STAT: {
+		uint8_t channel = mw->transmit_buffer[4];
+		if (!channel || channel > 15) {
+			start_reply(mw, CMD_ERROR);
 			end_reply(mw);
 			break;
 		}
-		case CMD_SOCK_STAT: {
-			uint8_t channel = mw->transmit_buffer[4];
-			if (!channel || channel > 15) {
-				start_reply(mw, CMD_ERROR);
-				end_reply(mw);
-				break;
+		mw->channel_flags &= ~(1 << channel);
+		channel--;
+		poll_socket(mw, channel);
+		start_reply(mw, CMD_OK);
+		mw_putc(mw, mw->channel_state[channel]);
+		end_reply(mw);
+		break;
+	}
+	case CMD_DATETIME:
+		cmd_datetime(mw);
+		break;
+	case CMD_SYS_STAT:
+		poll_all_sockets(mw);
+		start_reply(mw, CMD_OK);
+		mw_putc(mw, mw->module_state);
+		mw_putc(mw, mw->flags);
+		mw_putc(mw, mw->channel_flags >> 8);
+		mw_putc(mw, mw->channel_flags);
+		end_reply(mw);
+		break;
+	case CMD_GAMERTAG_GET:
+		cmd_gamertag_get(mw);
+		break;
+	case CMD_LOG:
+		start_reply(mw, CMD_OK);
+		puts((char*)&mw->transmit_buffer[4]);
+		end_reply(mw);
+		break;
+	case CMD_HRNG_GET:
+		cmd_hrng_get(mw);
+		break;
+	case CMD_SERVER_URL_GET:
+		start_reply(mw, CMD_OK);
+		// FIXME: This should be get from config file
+		mw_puts(mw, "doragasu.com");
+		mw_putc(mw,'\0');
+		end_reply(mw);
+		break;
+	default:
+		printf("Unhandled MegaWiFi command %s(%d) with length %X\n", cmd_names[command], command, size);
+		break;
+	}
+}
+
+static void process_packet(megawifi *mw)
+{
+	if (mw->transmit_channel == 0) {
+		process_command(mw);
+	} else {
+		uint8_t channel = mw->transmit_channel - 1;
+		int channel_state = mw->channel_state[channel];
+		int sock_fd = mw->sock_fds[channel];
+		if (sock_fd >= 0 && channel_state == SOCKST_TCP_EST) {
+			int sent = send(sock_fd, (char*)mw->transmit_buffer, mw->transmit_bytes, 0);
+			if (sent < 0 && !socket_error_is_wouldblock()) {
+				socket_close(sock_fd);
+				mw->sock_fds[channel] = -1;
+				mw->channel_state[channel] = SOCKST_NONE;
+				mw->channel_flags |= 1 << mw->transmit_channel;
+			} else if (sent < mw->transmit_bytes) {
+				//TODO: save this data somewhere so it can be sent in poll_socket
+				printf("Sent %d bytes on channel %d, but %d were requested\n", sent, mw->transmit_channel, mw->transmit_bytes);
 			}
-			mw->channel_flags &= ~(1 << channel);
-			channel--;
-			poll_socket(mw, channel);
-			start_reply(mw, CMD_OK);
-			mw_putc(mw, mw->channel_state[channel]);
-			end_reply(mw);
-			break;
+		} else if (sock_fd >= 0 && channel_state == SOCKST_UDP_READY) {
+			udp_send(mw, channel);
+		} else {
+			printf("Unhandled receive of MegaWiFi data on channel %d\n", mw->transmit_channel);
 		}
-		case CMD_SYS_STAT:
-			poll_all_sockets(mw);
-			start_reply(mw, CMD_OK);
-			mw_putc(mw, mw->module_state);
-			mw_putc(mw, mw->flags);
-			mw_putc(mw, mw->channel_flags >> 8);
-			mw_putc(mw, mw->channel_flags);
-			end_reply(mw);
-			break;
-		default:
-			printf("Unhandled MegaWiFi command %s(%d) with length %X\n", cmd_names[command], command, size);
-			break;
-		}
-	} else if (mw->sock_fds[mw->transmit_channel - 1] >= 0 && mw->channel_state[mw->transmit_channel - 1] == 2) {
-		uint8_t channel = mw->transmit_channel - 1;
-		int sent = send(mw->sock_fds[channel], mw->transmit_buffer, mw->transmit_bytes, MSG_NOSIGNAL);
-		if (sent < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
-			close(mw->sock_fds[channel]);
-			mw->sock_fds[channel] = -1;
-			mw->channel_state[channel] = 0;
-			mw->channel_flags |= 1 << mw->transmit_channel;
-		} else if (sent < mw->transmit_bytes) {
-			//TODO: save this data somewhere so it can be sent in poll_socket
-			printf("Sent %d bytes on channel %d, but %d were requested\n", sent, mw->transmit_channel, mw->transmit_bytes);
-		}
-	} else {
-		printf("Unhandled receive of MegaWiFi data on channel %d\n", mw->transmit_channel);
 	}
 	mw->transmit_bytes = mw->expected_bytes = 0;
 }