From ec5ac64bdca473ed3de63f588bebbd1f9c59a633 Mon Sep 17 00:00:00 2001
From: dragonmux <git@dragonmux.network>
Date: Mon, 11 Jul 2022 14:16:37 -0400
Subject: [PATCH] hosted/cmsis_dap: Done a first pass at rewriting the HID
 serial number logic to be unicode-safe

---
 src/platforms/hosted/cmsis_dap.c | 35 ++++++++++++++++++++++++++------
 1 file changed, 29 insertions(+), 6 deletions(-)

diff --git a/src/platforms/hosted/cmsis_dap.c b/src/platforms/hosted/cmsis_dap.c
index cce8d3f..b7948e7 100644
--- a/src/platforms/hosted/cmsis_dap.c
+++ b/src/platforms/hosted/cmsis_dap.c
@@ -63,6 +63,23 @@ static uint8_t buffer[1024 + 1];
 static int report_size = 64 + 1; // TODO: read actual report size
 static bool has_swd_sequence = false;
 
+static size_t mbslen(const char *str)
+{
+	const char *const end = str + strlen(str);
+	size_t result = 0;
+	// Reset conversion state
+	mblen(NULL, 0);
+	while (str < end) {
+		const int next = mblen(str, end - str);
+		// If an error occurs, bail out with whatever we got so far.
+		if (next == -1)
+			break;
+		str += next;
+		++result;
+	}
+	return result;
+}
+
 /* LPC845 Breakout Board Rev. 0 report invalid response with > 65 bytes */
 int dap_init(bmp_info_t *info)
 {
@@ -73,11 +90,17 @@ int dap_init(bmp_info_t *info)
 		if (hid_init())
 			return -1;
 
-		const size_t size = strlen(info->serial);
-		wchar_t serial[64] = {0}, *wc = serial;
-		for (size_t i = 0; i < size; i++)
-			*wc++ = info->serial[i];
-		*wc = 0;
+		const size_t size = mbslen(info->serial);
+		if (size > 64) {
+			PRINT_INFO("Serial number invalid, aborting\n");
+			return -1;
+		}
+		wchar_t serial[65] = {0};
+		if (mbstowcs(serial, info->serial, size) != size) {
+			PRINT_INFO("Serial number conversion failed, aborting\n");
+			return -1;
+		}
+		serial[size] = 0;
 		/* Blacklist devices that do not work with 513 byte report length
 		* FIXME: Find a solution to decipher from the device.
 		*/
@@ -87,7 +110,7 @@ int dap_init(bmp_info_t *info)
 		}
 		handle = hid_open(info->vid, info->pid,  (serial[0]) ? serial : NULL);
 		if (!handle) {
-			DEBUG_WARN("hid_open failed: %ls\n", hid_error(NULL));
+			PRINT_INFO("hid_open failed: %ls\n", hid_error(NULL));
 			return -1;
 		}
 	} else if (type == CMSIS_TYPE_BULK) {