From 4b8bde746e0caa27f96ff44beb1914d2b055b962 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 6 Feb 2018 09:47:02 +1100
Subject: [PATCH] HAL_ChibiOS: improved default USB strings

use board name and serial number to make IDs unique
---
 .../hwdef/common/chibios_board.mk             |  2 +-
 .../AP_HAL_ChibiOS/hwdef/common/usbcfg.c      | 58 ++++++++++++++++++-
 .../hwdef/scripts/chibios_hwdef.py            | 14 ++---
 3 files changed, 61 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk
index 50589311c2..57a67b36c3 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk
@@ -219,7 +219,7 @@ CPPWARN = -Wall -Wextra -Wundef
 #
 
 # List all user C define here, like -D_DEBUG=1
-UDEFS = $(FATFS_FLAGS)
+UDEFS = $(FATFS_FLAGS) -DHAL_BOARD_NAME=\"$(HAL_BOARD_NAME)\"
 
 # Define ASM defines here
 UADEFS =
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c
index 8bb27dc2ac..2c6b880cf3 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c
@@ -30,6 +30,7 @@
 #include "hwdef.h"
 
 #include <stdlib.h>
+#include <string.h>
 
 #pragma GCC optimize("O0")
 
@@ -210,22 +211,73 @@ static USBDescriptor vcom_strings[] = {
   {0, NULL}, // version
 };
 
+
+// start of 12 byte CPU ID
+#ifndef UDID_START
+#define UDID_START	0x1FFF7A10
+#endif
+
+/*
+  handle substitution of variables in strings for USB descriptors
+ */
+static char *string_substitute(const char *str)
+{
+    uint8_t new_len = strlen(str);
+    if (strstr(str, "%BOARD%")) {
+        new_len += strlen(HAL_BOARD_NAME) - 7;
+    }
+    if (strstr(str, "%SERIAL%")) {
+        new_len += 24 - 8;
+    }
+    char *str2 = malloc(new_len+1);
+    char *p = str2;
+    while (*str) {
+        char c = *str;
+        if (c == '%') {
+            if (strcmp(str, "%BOARD%") == 0) {
+                memcpy(p, HAL_BOARD_NAME, strlen(HAL_BOARD_NAME));
+                str += 7;
+                p += strlen(HAL_BOARD_NAME);
+                continue;
+            }
+            if (strcmp(str, "%SERIAL%") == 0) {
+                const char *hex = "0123456789ABCDEF";
+                const uint8_t *cpu_id = (const uint8_t *)UDID_START;
+                for (uint8_t i=0; i<12; i++) {
+                    *p++ = hex[(cpu_id[i]>>4)&0xF];
+                    *p++ = hex[cpu_id[i]&0xF];
+                }
+                str += 8;
+                continue;
+            }
+        }
+        *p++ = *str++;
+    }
+    *p = 0;
+    return str2;
+}
+
+
 /*
   dynamically allocate a USB descriptor string
  */
 static void setup_usb_string(USBDescriptor *desc, const char *str)
 {
-    uint8_t len = strlen(str);
+    char *str2 = string_substitute(str);
+    uint8_t len = strlen(str2);
     desc->ud_size = 2+2*len;
     uint8_t *b = (uint8_t *)calloc(1, desc->ud_size);
-    desc->ud_string = (const char *)b;
+    desc->ud_string = (const uint8_t *)b;
     b[0] = USB_DESC_BYTE(desc->ud_size);
     b[1] = USB_DESC_BYTE(USB_DESCRIPTOR_STRING);
     uint8_t i;
     for (i=0; i<len; i++) {
-        b[2+i*2] = str[i];
+        b[2+i*2] = str2[i];
         b[2+i*2+1] = 0;
     }
+    if (str2 != str) {
+        free(str2);
+    }
 }
 
 /*
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
index a170eb5582..dc93f2adc7 100755
--- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
+++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
@@ -355,16 +355,12 @@ INCLUDE common.ld
 
 def write_USB_config(f):
     '''write USB config defines'''
-    if not 'USB_VENDOR' in config:
-        return
     f.write('// USB configuration\n')
-    f.write('#define HAL_USB_VENDOR_ID %s\n' % get_config('USB_VENDOR'))
-    f.write('#define HAL_USB_PRODUCT_ID %s\n' % get_config('USB_PRODUCT'))
-    for s in [
-            'USB_STRING_MANUFACTURER', 'USB_STRING_PRODUCT',
-            'USB_STRING_SERIAL'
-    ]:
-        f.write('#define HAL_%s "%s"\n' % (s, get_config(s)))
+    f.write('#define HAL_USB_VENDOR_ID %s\n' % get_config('USB_VENDOR', default=0x0483)) # default to ST
+    f.write('#define HAL_USB_PRODUCT_ID %s\n' % get_config('USB_PRODUCT', default=0x5740))
+    f.write('#define HAL_USB_STRING_MANUFACTURER "%s"\n' % get_config("USB_STRING_MANUFACTURER", default="ArduPilot"))
+    f.write('#define HAL_USB_STRING_PRODUCT "%s"\n' % get_config("USB_STRING_PRODUCT", default="%BOARD%"))
+    f.write('#define HAL_USB_STRING_SERIAL "%s"\n' % get_config("USB_STRING_SERIAL", default="%SERIAL%"))
 
     f.write('\n\n')