emulator: open a second socket for debuglink, same as T2

emulator/emulator.h

@@ -30,8 +30,8 @@ void emulatorPoll(void);
 void emulatorRandom(void *buffer, size_t size);
 
 void emulatorSocketInit(void);
-size_t emulatorSocketRead(void *buffer, size_t size);
+size_t emulatorSocketRead(int *iface, void *buffer, size_t size);
-size_t emulatorSocketWrite(const void *buffer, size_t size);
+size_t emulatorSocketWrite(int iface, const void *buffer, size_t size);
 
 #endif
 

emulator/udp.c

@@ -26,33 +26,50 @@
 
 #define TREZOR_UDP_PORT 21324
 
-static int fd = -1;
+struct usb_socket {
-static struct sockaddr_in from;
+	int fd;
-static socklen_t fromlen;
+	struct sockaddr_in from;
+	socklen_t fromlen;
+};
 
-void emulatorSocketInit(void) {
+static struct usb_socket usb_main;
-	fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+static struct usb_socket usb_debug;
+
+static int socket_setup(int port) {
+	int fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
 	if (fd < 0) {
 		perror("Failed to create socket");
 		exit(1);
 	}
 
-	fromlen = 0;
-
 	struct sockaddr_in addr;
 	addr.sin_family = AF_INET;
-	addr.sin_port = htons(TREZOR_UDP_PORT);
+	addr.sin_port = htons(port);
 	addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
 
 	if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) != 0) {
 		perror("Failed to bind socket");
 		exit(1);
 	}
+
+	return fd;
 }
 
-size_t emulatorSocketRead(void *buffer, size_t size) {
+static size_t socket_write(struct usb_socket *sock, const void *buffer, size_t size) {
-	fromlen = sizeof(from);
+	if (sock->fromlen > 0) {
-	ssize_t n = recvfrom(fd, buffer, size, MSG_DONTWAIT, (struct sockaddr *) &from, &fromlen);
+		ssize_t n = sendto(sock->fd, buffer, size, MSG_DONTWAIT, (const struct sockaddr *) &sock->from, sock->fromlen);
+		if (n < 0 || ((size_t) n) != size) {
+			perror("Failed to write socket");
+			return 0;
+		}
+	}
+
+	return size;
+}
+
+static size_t socket_read(struct usb_socket *sock, void *buffer, size_t size) {
+	sock->fromlen = sizeof(sock->from);
+	ssize_t n = recvfrom(sock->fd, buffer, size, MSG_DONTWAIT, (struct sockaddr *) &sock->from, &sock->fromlen);
 
 	if (n < 0) {
 		if (errno != EAGAIN && errno != EWOULDBLOCK) {
@@ -65,21 +82,42 @@ size_t emulatorSocketRead(void *buffer, size_t size) {
 	static const char msg_pong[] = { 'P', 'O', 'N', 'G', 'P', 'O', 'N', 'G' };
 
 	if (n == sizeof(msg_ping) && memcmp(buffer, msg_ping, sizeof(msg_ping)) == 0) {
-		emulatorSocketWrite(msg_pong, sizeof(msg_pong));
+		socket_write(sock, msg_pong, sizeof(msg_pong));
 		return 0;
 	}
 
 	return n;
 }
 
-size_t emulatorSocketWrite(const void *buffer, size_t size) {
+void emulatorSocketInit(void) {
-	if (fromlen > 0) {
+	usb_main.fd = socket_setup(TREZOR_UDP_PORT);
-		ssize_t n = sendto(fd, buffer, size, MSG_DONTWAIT, (const struct sockaddr *) &from, fromlen);
+	usb_main.fromlen = 0;
-		if (n < 0 || ((size_t) n) != size) {
+	usb_debug.fd = socket_setup(TREZOR_UDP_PORT + 1);
-			perror("Failed to write socket");
+	usb_debug.fromlen = 0;
-			return 0;
+}
-		}
+
+size_t emulatorSocketRead(int *iface, void *buffer, size_t size) {
+	size_t n = socket_read(&usb_main, buffer, size);
+	if (n > 0) {
+		*iface = 0;
+		return n;
 	}
 
-	return size;
+	n = socket_read(&usb_debug, buffer, size);
+	if (n > 0) {
+		*iface = 1;
+		return n;
+	}
+
+	return 0;
+}
+
+size_t emulatorSocketWrite(int iface, const void *buffer, size_t size) {
+	if (iface == 0) {
+		return socket_write(&usb_main, buffer, size);
+	}
+	if (iface == 1) {
+		return socket_write(&usb_debug, buffer, size);
+	}
+	return 0;
 }

firmware/udp.c

@@ -23,6 +23,7 @@
 
 #include "messages.h"
 #include "timer.h"
+#include "debug.h"
 
 static volatile char tiny = 0;
 
@@ -30,29 +31,37 @@ void usbInit(void) {
 	emulatorSocketInit();
 }
 
+#if DEBUG_LINK
+#define _ISDBG (((iface == 1) ? 'd' : 'n'))
+#else
+#define _ISDBG ('n')
+#endif
+
 void usbPoll(void) {
 	emulatorPoll();
 
 	static uint8_t buffer[64];
-	if (emulatorSocketRead(buffer, sizeof(buffer)) > 0) {
+
+	int iface = 0;
+	if (emulatorSocketRead(&iface, buffer, sizeof(buffer)) > 0) {
 		if (!tiny) {
-			msg_read(buffer, sizeof(buffer));
+			msg_read_common(_ISDBG, buffer, sizeof(buffer));
 		} else {
 			msg_read_tiny(buffer, sizeof(buffer));
 		}
 	}
 
 	const uint8_t *data = msg_out_data();
+	if (data != NULL) {
+		emulatorSocketWrite(0, data, 64);
+	}
 
 #if DEBUG_LINK
-	if (data == NULL) {
+	data = msg_debug_out_data();
-		data = msg_debug_out_data();
+	if (data != NULL) {
+		emulatorSocketWrite(1, data, 64);
 	}
 #endif
-
-	if (data != NULL) {
-		emulatorSocketWrite(data, 64);
-	}
 }
 
 char usbTiny(char set) {