Alternative way of supporting the USB mode protocol

include/tkey/debug.h

@@ -29,7 +29,7 @@ void tkey_lf();
 void tkey_putinthex(const uint32_t n);
 void tkey_puts(const char *s);
 void tkey_puthex(const uint8_t ch);
-void tkey_hexdump(const uint8_t *buf, int len);
+void tkey_hexdump(const void *buf, int len);
 
 #define debug_putchar tkey_putchar
 #define debug_lf tkey_lf

include/tkey/proto.h

@@ -8,6 +8,7 @@
 #define TKEY_PROTO_H
 
 enum mode {
+	MODE_QEMU = 0x10,
 	MODE_TKEYCTRL = 0x20,
 	MODE_CDC = 0x40,
 	MODE_HID = 0x80,
@@ -42,10 +43,6 @@ struct frame_header {
 
 uint8_t genhdr(uint8_t id, uint8_t endpoint, uint8_t status, enum cmdlen len);
 int parseframe(uint8_t b, struct frame_header *hdr);
-void writebyte(uint8_t b);
-void write(const uint8_t *buf, size_t nbytes);
-uint8_t readbyte(uint8_t *mode, uint8_t *mode_bytes_left);
-int read(uint8_t *buf, size_t bufsize, size_t nbytes, uint8_t *mode,
-	 uint8_t *mode_bytes_left);
-
+void write(const uint8_t *buf, size_t nbytes, enum mode mode);
+int read(uint8_t *buf, size_t bufsize, size_t nbytes, enum mode expect_mode);
 #endif

libcommon/proto.c

@@ -7,13 +7,27 @@
 #include <tkey/proto.h>
 #include <tkey/tk1_mem.h>
 
+// USB Mode Protocol:
+//   1 byte mode
+//   1 byte length
+//
+// Our USB Mode Protocol packets has room for 255 bytes according to
+// the header but we use a packet size of 64 which neatly fits in with
+// the USB packet size from the CH552 to the client.
+#define USBMODE_PACKET_SIZE 64
+
 // clang-format off
 static volatile uint32_t* const can_rx = (volatile uint32_t *)TK1_MMIO_UART_RX_STATUS;
 static volatile uint32_t* const rx =     (volatile uint32_t *)TK1_MMIO_UART_RX_DATA;
 static volatile uint32_t* const can_tx = (volatile uint32_t *)TK1_MMIO_UART_TX_STATUS;
 static volatile uint32_t* const tx =     (volatile uint32_t *)TK1_MMIO_UART_TX_DATA;
 // clang-format on
 
+static void writebyte(uint8_t b);
+static uint8_t readbyte(void);
+static void write_with_header(const uint8_t *buf, size_t nbytes, enum mode mode);
+static void qemu_write(const uint8_t *buf, size_t nbytes);
+
 uint8_t genhdr(uint8_t id, uint8_t endpoint, uint8_t status, enum cmdlen len)
 {
 	return (id << 5) | (endpoint << 3) | (status << 2) | len;
@@ -56,7 +70,7 @@ int parseframe(uint8_t b, struct frame_header *hdr)
 	return 0;
 }
 
-void writebyte(uint8_t b)
+static void writebyte(uint8_t b)
 {
 	for (;;) {
 		if (*can_tx) {
@@ -66,48 +80,106 @@ void writebyte(uint8_t b)
 	}
 }
 
-void write(const uint8_t *buf, size_t nbytes)
+static void write_with_header(const uint8_t *buf, size_t nbytes, enum mode mode)
 {
+	// Append USB Mode Protocol header:
+	//   1 byte mode
+	//   1 byte length
+
+	writebyte(mode);
+	writebyte(nbytes);
+
 	for (int i = 0; i < nbytes; i++) {
 		writebyte(buf[i]);
 	}
 }
 
-uint8_t readbyte_(void)
+static void qemu_write(const uint8_t *buf, size_t nbytes)
 {
-	for (;;) {
-		if (*can_rx) {
-			return *rx;
-		}
+	static volatile uint8_t *const debugtx =
+	    (volatile uint8_t *)TK1_MMIO_QEMU_DEBUG;
+
+	for (int i = 0; i < nbytes; i++) {
+		*debugtx = buf[i];
 	}
 }
 
-uint8_t readbyte(uint8_t *mode, uint8_t *mode_bytes_left)
+// write blockingly writes nbytes bytes of data from buf to the UART,
+// framing the data in USB Mode Protocol with mode mode, either
+// MODE_CDC or MODE_HID.
+void write(const uint8_t *buf, size_t nbytes, enum mode mode)
 {
-	if (*mode_bytes_left == 0) {
-		*mode = readbyte_();
-		if ((*mode == MODE_CDC) || (*mode == MODE_HID) ||
-		    (*mode == MODE_TKEYCTRL)) {
-			*mode_bytes_left = readbyte_();
-		} else {
-			debug_puts("We only support MODE_CDC, MODE_HID and "
-				   "MODE_TKEYCTRL\n");
+	if (mode == MODE_QEMU) {
+		qemu_write(buf, nbytes);
+
+		return;
+	}
+
+	while (nbytes > 0) {
+		// We split the data into chunks that will fit in the
+		// USB Mode Protocol with some spare change.
+		uint8_t len = (nbytes < (USBMODE_PACKET_SIZE))
+				  ? nbytes
+				  : (USBMODE_PACKET_SIZE);
+
+		write_with_header((const uint8_t *)buf, len, mode);
+
+		buf += len;
+		nbytes -= len;
+	}
+}
+
+static uint8_t readbyte(void)
+{
+	for (;;) {
+		if (*can_rx) {
+			return *rx;
 		}
 	}
-	uint8_t b = readbyte_();
-	*mode_bytes_left -= 1;
-	return b;
 }
 
-int read(uint8_t *buf, size_t bufsize, size_t nbytes, uint8_t *mode,
-	 uint8_t *mode_bytes_left)
+// read blockingly reads nbytes bytes of data into buffer buf, a
+// maximum bufsize bytes.
+//
+// Caller asks for the expected USB mode expect_mode: MODE_CDC or
+// MODE_HID, which represents different endpoints on the USB
+// controller.
+//
+// If data is readable but with another mode set, it is silently
+// discarded and we keep on reading until nbytes bytes have appeared.
+//
+int read(uint8_t *buf, size_t bufsize, size_t nbytes, enum mode expect_mode)
 {
+	static uint8_t mode = 0;
+	static uint8_t mode_bytes_left = 0;
+
 	if (nbytes > bufsize) {
 		return -1;
 	}
 
-	for (int n = 0; n < nbytes; n++) {
-		buf[n] = readbyte(mode, mode_bytes_left);
+	int n = 0;
+	while (n < nbytes) {
+		if (mode_bytes_left == 0) {
+			// Read USB Mode Protocol header:
+			//   1 byte mode
+			//   1 byte length
+			mode = readbyte();
+			mode_bytes_left = readbyte();
+		}
+
+		if (mode == expect_mode) {
+			// Reading payload.
+			buf[n] = readbyte();
+			n++;
+			mode_bytes_left--;
+		} else {
+			// Not the USB mode caller asked for. Eat the rest.
+			for (int i = 0; i < mode_bytes_left; i++) {
+				(void)readbyte();
+			}
+
+			mode_bytes_left = 0;
+		}
 	}
 
 	return 0;

libcommon/tkey_debug.c

@@ -12,55 +12,14 @@
 
 #include <tkey/debug.h>
 
-#define PACKET_SIZE 64
-#define HEADER_SIZE 2
-
-// clang-format off
-static volatile uint32_t *can_tx = (volatile uint32_t *)TK1_MMIO_UART_TX_STATUS;
-static volatile uint32_t *tx =     (volatile uint32_t *)TK1_MMIO_UART_TX_DATA;
-// clang-format on
-
-void tkey_writebyte(uint8_t b)
-{
-	for (;;) {
-		if (*can_tx) {
-			*tx = b;
-			return;
-		}
-	}
-}
-
-void tkey_write(const uint8_t *buf, size_t nbytes)
-{
-	for (int i = 0; i < nbytes; i++) {
-		tkey_writebyte(buf[i]);
-	}
-}
-
-void tkey_write_with_header(const uint8_t *buf, size_t nbytes)
-{
-	tkey_writebyte(MODE_TKEYCTRL);
-	tkey_writebyte(nbytes);
-
-	for (int i = 0; i < nbytes; i++) {
-		tkey_writebyte(buf[i]);
-	}
-}
-
 void tkey_putchar(const uint8_t ch)
 {
-	tkey_writebyte(MODE_TKEYCTRL);
-	tkey_writebyte(1);
-
-	tkey_writebyte(ch);
+	write(&ch, 1, MODE_TKEYCTRL);
 }
 
 void tkey_lf()
 {
-	tkey_writebyte(MODE_TKEYCTRL);
-	tkey_writebyte(1);
-
-	tkey_writebyte('\n');
+	tkey_putchar('\n');
 }
 
 static char hexnibble(const uint8_t ch)
@@ -103,60 +62,71 @@ static char hexnibble(const uint8_t ch)
 	return '0';
 }
 
-void tkey_puthex(const uint8_t ch)
+uint8_t *hex(const uint8_t ch)
 {
-	tkey_writebyte(MODE_TKEYCTRL);
-	tkey_writebyte(2);
+	static uint8_t buf[2];
+
+	buf[0] = hexnibble(ch >> 4 & 0x0f);
+	buf[1] = hexnibble(ch & 0x0f);
+
+	return buf;
+}
 
-	tkey_writebyte(hexnibble(ch >> 4 & 0x0f));
-	tkey_writebyte(hexnibble(ch & 0x0f));
+void tkey_puthex(const uint8_t ch)
+{
+	write(hex(ch), 2, MODE_TKEYCTRL);
 }
 
 void tkey_putinthex(const uint32_t n)
 {
-	uint8_t buf[4];
+	uint8_t buf[10];
+	uint8_t *intbuf = (uint8_t *)&n;
 
-	tkey_writebyte(MODE_TKEYCTRL);
-	tkey_writebyte(2);
+	buf[0] = '0';
+	buf[1] = 'x';
 
-	memcpy(buf, &n, 4);
-	tkey_write((uint8_t *)"0x", 2);
+	int j = 2;
 	for (int i = 3; i > -1; i--) {
-		tkey_puthex(buf[i]);
+		memcpy(&buf[j], hex(intbuf[i]), 2);
+		j += 2;
 	}
+
+	write(buf, 10, MODE_TKEYCTRL);
 }
 
 void tkey_puts(const char *s)
 {
-	size_t remaining = strlen(s);
-
-	while (remaining > 0) {
-		uint8_t len = (remaining < (PACKET_SIZE - HEADER_SIZE))
-				  ? remaining
-				  : (PACKET_SIZE - HEADER_SIZE);
-
-		// Send chunk data
-		tkey_write_with_header((const uint8_t *)s, len);
-
-		s += len;
-		remaining -= len;
-	}
+	write((const uint8_t *)s, strlen(s), MODE_TKEYCTRL);
 }
 
-void tkey_hexdump(const uint8_t *buf, int len)
+// Size of a hex line. 16 bytes printed 3 chars + final newline.
+#define LINEBUFSIZE (16 * 3 + 1)
+
+void tkey_hexdump(const void *buf, int len)
 {
+	uint8_t rowbuf[LINEBUFSIZE] = {0};
+	uint8_t *hexbuf = {0}; // A byte represented as 2 chars and a space
 	uint8_t *byte_buf = (uint8_t *)buf;
 
+	int rowpos = 0;
 	for (int i = 0; i < len; i++) {
-		tkey_puthex(byte_buf[i]);
-		if (i % 2 == 1) {
-			tkey_putchar(' ');
-		}
-
-		if ((i + 1) % 16 == 0) {
-			tkey_lf();
+		hexbuf = hex(byte_buf[i]);
+		rowbuf[rowpos++] = hexbuf[0];
+		rowbuf[rowpos++] = hexbuf[1];
+		rowbuf[rowpos++] = ' ';
+
+		// A finished row is 3 chars * 16. If the row is full,
+		// print it now.
+		if (rowpos == LINEBUFSIZE - 1) {
+			rowbuf[rowpos++] = '\n';
+			write(rowbuf, rowpos, MODE_TKEYCTRL);
+			rowpos = 0;
 		}
 	}
 
-	tkey_lf();
+	// If final row wasn't full, print it now.
+	if (rowpos != 0) {
+		rowbuf[rowpos++] = '\n';
+		write(rowbuf, rowpos, MODE_TKEYCTRL);
+	}
 }