altosui: write USB serial number string while flashing

[fw/altos] / ao-tools / altosui / AltosRomconfig.java

diff --git a/ao-tools/altosui/AltosRomconfig.java b/ao-tools/altosui/AltosRomconfig.java
index 844da7c4b7aad021b969453a01616b2ed86fb0f1..8c9cb27aa1207971ee21218ce058c9b02bc02652 100644 (file)
--- a/ao-tools/altosui/AltosRomconfig.java
+++ b/ao-tools/altosui/AltosRomconfig.java
@@ -47,14 +47,57 @@ public class AltosRomconfig {
                }
        }
 
+       static void put_string(String value, byte[] bytes, int start) {
+               for (int i = 0; i < value.length(); i++)
+                       bytes[start + i] = (byte) value.charAt(i);
+       }
+
+       static final int AO_USB_DESC_STRING     = 3;
+
+       static void put_usb_serial(int value, byte[] bytes, int start) {
+               int offset = start + 0xa;
+               int string_num = 0;
+               System.out.printf("Put usb serial %d\n", value);
+
+               while (offset < bytes.length && bytes[offset] != 0) {
+                       if (bytes[offset + 1] == AO_USB_DESC_STRING) {
+                               ++string_num;
+                               if (string_num == 4)
+                                       break;
+                       }
+                       offset += ((int) bytes[offset]) & 0xff;
+               }
+               System.out.printf("offset %d content %d\n",
+                                 offset, bytes[offset]);
+               if (offset >= bytes.length || bytes[offset] == 0)
+                       return;
+               int len = ((((int) bytes[offset]) & 0xff) - 2) / 2;
+               String fmt = String.format("%%0%dd", len);
+               System.out.printf("existing serial length %d format %s\n", len, fmt);
+
+               String s = String.format(fmt, value);
+               if (s.length() != len) {
+                       System.out.printf("weird usb length issue %s isn't %d\n",
+                                         s, len);
+                       return;
+               }
+               for (int i = 0; i < len; i++) {
+                       bytes[offset + 2 + i*2] = (byte) s.charAt(i);
+                       bytes[offset + 2 + i*2+1] = 0;
+               }
+       }
+
        public AltosRomconfig(byte[] bytes, int offset) {
                version = get_int(bytes, offset + 0, 2);
                check = get_int(bytes, offset + 2, 2);
+               System.out.printf("version %d check %d\n", version, check);
                if (check == (~version & 0xffff)) {
                        switch (version) {
+                       case 2:
                        case 1:
                                serial_number = get_int(bytes, offset + 4, 2);
                                radio_calibration = get_int(bytes, offset + 6, 4);
+                               System.out.printf("serial %d cal %d\n", serial_number, radio_calibration);
                                valid = true;
                                break;
                        }
@@ -77,6 +120,8 @@ public class AltosRomconfig {
                        throw new IOException("image does not contain existing rom config");
 
                switch (existing.version) {
+               case 2:
+                       put_usb_serial(serial_number, bytes, offset);
                case 1:
                        put_int(serial_number, bytes, offset + 4, 2);
                        put_int(radio_calibration, bytes, offset + 6, 4);
@@ -86,7 +131,9 @@ public class AltosRomconfig {
 
        public void write (AltosHexfile hexfile) throws IOException {
                write(hexfile.data, 0xa0 - hexfile.address);
-               new AltosRomconfig(hexfile);
+               AltosRomconfig check = new AltosRomconfig(hexfile);
+               if (!check.valid())
+                       throw new IOException("writing new rom config failed\n");
        }
 
        public AltosRomconfig(int in_serial_number, int in_radio_calibration) {