From f76cb92b15afbb5022696bf20c7a0172e3c6f834 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Mon, 25 Nov 2019 00:12:56 -0500
Subject: [PATCH] usb_cdc: Add a usb_fill_serial() helper function

Add a helper function to fill the usb serial string descriptor.  Use
it in the lpc176x code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 src/generic/usb_cdc.c   | 17 +++++++++++++++++
 src/generic/usb_cdc.h   |  1 +
 src/lpc176x/usbserial.c | 37 ++++++++++++++-----------------------
 3 files changed, 32 insertions(+), 23 deletions(-)

diff --git a/src/generic/usb_cdc.c b/src/generic/usb_cdc.c
index e213a07c..9adc0aa9 100644
--- a/src/generic/usb_cdc.c
+++ b/src/generic/usb_cdc.c
@@ -284,10 +284,27 @@ static const struct descriptor_s {
       &cdc_string_manufacturer, SIZE_cdc_string_manufacturer },
     { (USB_DT_STRING<<8) | USB_STR_ID_PRODUCT, USB_LANGID_ENGLISH_US,
       &cdc_string_product, SIZE_cdc_string_product },
+#if !CONFIG_USB_SERIAL_NUMBER_CHIPID
     { (USB_DT_STRING<<8) | USB_STR_ID_SERIAL, USB_LANGID_ENGLISH_US,
       &cdc_string_serial, SIZE_cdc_string_serial },
+#endif
 };
 
+// Fill in a USB serial string descriptor from a chip id
+void
+usb_fill_serial(struct usb_string_descriptor *desc, int strlen, void *id)
+{
+    desc->bLength = sizeof(*desc) + strlen * sizeof(desc->data[0]);
+    desc->bDescriptorType = USB_DT_STRING;
+
+    uint8_t *src = id;
+    int i;
+    for (i = 0; i < strlen; i++) {
+        uint8_t c = i & 1 ? src[i/2] & 0x0f : src[i/2] >> 4;
+        desc->data[i] = c < 10 ? c + '0' : c - 10 + 'A';
+    }
+}
+
 
 /****************************************************************
  * USB endpoint 0 control message handling
diff --git a/src/generic/usb_cdc.h b/src/generic/usb_cdc.h
index 279c486c..f955c840 100644
--- a/src/generic/usb_cdc.h
+++ b/src/generic/usb_cdc.h
@@ -25,6 +25,7 @@ void usb_request_bootloader(void);
 struct usb_string_descriptor *usbserial_get_serialid(void);
 
 // usb_cdc.c
+void usb_fill_serial(struct usb_string_descriptor *desc, int strlen, void *id);
 void usb_notify_bulk_in(void);
 void usb_notify_bulk_out(void);
 void usb_notify_ep0(void);
diff --git a/src/lpc176x/usbserial.c b/src/lpc176x/usbserial.c
index 54c28a31..6e1cd77b 100644
--- a/src/lpc176x/usbserial.c
+++ b/src/lpc176x/usbserial.c
@@ -111,16 +111,6 @@ sie_select_and_clear(uint32_t idx)
 /****************************************************************
  * Interface
  ****************************************************************/
-#define USB_STR_SERIAL_CHIPID u"0123456789ABCDEF0123456789ABCDEF"
-
-#define SIZE_cdc_string_serial_chipid \
-    (sizeof(cdc_string_serial_chipid) + sizeof(USB_STR_SERIAL_CHIPID) - 2)
-
-static struct usb_string_descriptor cdc_string_serial_chipid = {
-    .bLength = SIZE_cdc_string_serial_chipid,
-    .bDescriptorType = USB_DT_STRING,
-    .data = USB_STR_SERIAL_CHIPID,
-};
 
 static int_fast8_t
 usb_write_packet(uint32_t ep, const void *data, uint_fast8_t len)
@@ -277,35 +267,37 @@ usb_request_bootloader(void)
     NVIC_SystemReset();
 }
 
+static struct {
+    struct usb_string_descriptor desc;
+    uint16_t data[IAP_UID_LEN * 2];
+} cdc_chipid;
+
 struct usb_string_descriptor *
 usbserial_get_serialid(void)
 {
-   return &cdc_string_serial_chipid;
+   return &cdc_chipid.desc;
 }
 
+
 /****************************************************************
  * Setup and interrupts
  ****************************************************************/
+
 static void
 usb_set_serial(void)
 {
+    if (!CONFIG_USB_SERIAL_NUMBER_CHIPID)
+        return;
+
     uint32_t iap_cmd_uid[5] = {IAP_CMD_READ_UID, 0, 0, 0, 0};
     uint32_t iap_resp[5];
-
     IAP iap_entry = (IAP)IAP_LOCATION;
     __disable_irq();
     iap_entry(iap_cmd_uid, iap_resp);
     __enable_irq();
 
-    uint8_t *chipid = (uint8_t *)&iap_resp[1];
-    uint8_t i, j, c;
-    for (i = 0; i < IAP_UID_LEN; i++) {
-        for (j = 0; j < 2; j++) {
-            c = (chipid[i] >> 4*j) & 0xF;
-            c = (c < 10) ? '0'+c : 'A'-10+c;
-            cdc_string_serial_chipid.data[2*i+((j)?0:1)] = c;
-        }
-    }
+    usb_fill_serial(&cdc_chipid.desc, ARRAY_SIZE(cdc_chipid.data)
+                    , &iap_resp[1]);
 }
 
 void
@@ -343,8 +335,7 @@ DECL_CONSTANT_STR("RESERVE_PINS_USB", "P0.30,P0.29,P2.9");
 void
 usbserial_init(void)
 {
-    if (CONFIG_USB_SERIAL_NUMBER_CHIPID)
-        usb_set_serial();
+    usb_set_serial();
 
     usb_irq_disable();
     // enable power