ssmp: unpacking 'sequence' field

uhppoted · Aug 20, 2024 · 2d5d3e6 · 2d5d3e6
1 parent 2379a08
commit 2d5d3e6
Show file tree

Hide file tree

Showing 6 changed files with 112 additions and 14 deletions.
diff --git a/Rev.0/firmware/core/src/cli.c b/Rev.0/firmware/core/src/cli.c
@@ -409,7 +409,7 @@ void debug() {
         1, 0, 48, 13, 48, 11, 6, 7, 43, 6, 167, 254, 32,
         1, 1, 5, 0,
 
-        48, 37, 2, 1, 0, 4, 6, 112, 117, 98, 108, 105, 99,
+        48, 37, 2, 1, 13, 4, 6, 112, 117, 98, 108, 105, 99,
         160, 24, 2, 1, 1, 2, 1, 0, 2,
         1, 0, 48, 13, 48, 11, 6, 7, 43, 6, 167, 254, 32,
         1, 1, 5, 0,

diff --git a/Rev.0/firmware/lib/include/encoding/BER/BER.h b/Rev.0/firmware/lib/include/encoding/BER/BER.h
@@ -10,9 +10,29 @@ struct packet BER_decode(const uint8_t *message, int N);
 
 /* internal use only */
 
-typedef struct field {
-
-} field;
+typedef enum {
+    FIELD_UNKNOWN = 0,
+    FIELD_BOOLEAN = 1,
+    FIELD_INTEGER = 2,
+    FIELD_NULL = 5,
+    FIELD_OID = 6,
+    FIELD_SEQUENCE = 48,
+} FIELD;
 
+typedef struct field field;
 typedef field *fieldp;
 typedef fieldp *fields;
+
+typedef struct field {
+    FIELD tag;
+    union {
+        struct {
+            int64_t value;
+        } integer;
+
+        struct {
+            fields fields;
+        } sequence;
+    };
+
+} field;
diff --git a/Rev.0/firmware/lib/src/encoding/BER/decode.c b/Rev.0/firmware/lib/src/encoding/BER/decode.c
@@ -9,6 +9,7 @@ extern fields fields_add(fields, field *);
 
 fields unpack(const uint8_t *message, int N);
 void unpack_boolean(const uint8_t *message, int N, int *ix);
+field *unpack_integer(const uint8_t *message, int N, int *ix);
 void unpack_null(const uint8_t *message, int N, int *ix);
 void unpack_OID(const uint8_t *message, int N, int *ix);
 field *unpack_sequence(const uint8_t *message, int N, int *ix);
@@ -49,19 +50,23 @@ fields unpack(const uint8_t *bytes, int N) {
         ix += 1;
 
         switch (tag) {
-            // case 1:
+            // case FIELD_BOOLEAN:
             //     unpack_boolean(bytes, N, &ix);
             //     break;
 
-            // case 5:
+        case FIELD_INTEGER:
+            unpack_integer(bytes, N, &ix);
+            break;
+
+            // case FIELD_NULL:
             //     unpack_null(bytes, N, &ix);
             //     break;
 
-            // case 6:
+            // case FIELD_OID:
             //     unpack_OID(bytes, N, &ix);
             //     break;
 
-        case 48:
+        case FIELD_SEQUENCE:
             if ((f = unpack_sequence(bytes, N, &ix)) != NULL) {
                 list = fields_add(list, f);
             }
@@ -102,6 +107,48 @@ void unpack_boolean(const uint8_t *message, int N, int *ix) {
     *ix += length;
 }
 
+field *unpack_integer(const uint8_t *message, int N, int *ix) {
+    // ... length
+    uint32_t length = 0;
+
+    if (*ix < N) {
+        uint8_t b = message[*ix];
+        *ix += 1;
+
+        if (b & 0x80 == 0x80) {
+            length = (uint32_t)(b & 0x7f);
+        } else {
+            int len = b & 0x7f;
+            for (int i = 0; i < len && *ix < N; i++) {
+                length <<= 8;
+                length += (uint32_t)(message[*ix]);
+                *ix += 1;
+            }
+        }
+    }
+
+    // .. value
+    int64_t value = 0;
+
+    if (length <= 8) {
+        for (int i = 0; i < length; i++) {
+            value <<= 8;
+            value |= message[*ix + i] & 0x00ff;
+        }
+    }
+
+    printf("::integer     N:%d  ix:%-3d length:%lu  value:%lld\n", N, *ix, length, value);
+
+    // ... compose field
+    field *f = (field *)calloc(1, sizeof(field));
+    f->tag = FIELD_INTEGER;
+    f->integer.value = value;
+
+    *ix += length;
+
+    return f;
+}
+
 void unpack_null(const uint8_t *message, int N, int *ix) {
     // ... length
     uint32_t length = 0;
@@ -151,7 +198,7 @@ void unpack_OID(const uint8_t *message, int N, int *ix) {
 }
 
 field *unpack_sequence(const uint8_t *message, int N, int *ix) {
-    // ... length
+    // ... get length
     uint32_t length = 0;
     if (*ix < N) {
         uint8_t b = message[*ix];
@@ -171,9 +218,19 @@ field *unpack_sequence(const uint8_t *message, int N, int *ix) {
 
     printf("::sequence    N:%d  ix:%-3d length:%lu end:%d\n", N, *ix, length, *ix + (int)length);
 
+    // ... unpack sequence
+    fields list = unpack(&message[*ix], length);
+
+    printf("::sequence/fields %p\n", list);
+    fields_free(list);
+
+    // ... 'k, done'
     *ix += length;
 
     field *f = (field *)calloc(1, sizeof(field));
 
+    f->tag = FIELD_SEQUENCE;
+    f->sequence.fields = NULL;
+
     return f;
 }
diff --git a/Rev.0/firmware/lib/src/encoding/BER/fields.c b/Rev.0/firmware/lib/src/encoding/BER/fields.c
@@ -4,16 +4,29 @@
 
 #include <encoding/BER/BER.h>
 
+void field_free(field *f);
+void fields_free(fields list);
+
 void field_free(field *f) {
-    free(f);
+    if (f->tag == FIELD_SEQUENCE) {
+        fields_free(f->sequence.fields);
+    } else {
+        free(f);
+    }
 }
 
 void fields_free(fields list) {
-    fieldp *p = list;
+    if (list != NULL) {
+        fieldp *p = list;
 
-    while (*p != NULL) {
-        field_free(*p);
-        p++;
+        while (*p != NULL) {
+            printf(">>>> FFREE  tag:%d\n", (*p)->tag);
+
+            field_free(*p);
+            p++;
+        }
+
+        free(list);
     }
 }
 

diff --git a/TODO.md b/TODO.md
@@ -40,13 +40,18 @@
          - [x] require STX-ETX framing
          - [ ] CRC
          - [ ] BER
+               - [ ] not unpacking multiple fields with different lengths correctly
+               - [ ] commonalise decode length
                - https://www.oss.com/asn1/resources/asn1-made-simple/asn1-quick-reference/basic-encoding-rules.html
 ```
 seq   48
       37
       2  1 0 4 6 112 117 98 108 105 99 160 24 2  1   1
       2  1 0 2 1 0   48  13 48  11  6  7   43 6  167 254
       32 1 1 5 0
+
+int 2 1 0
+
 ```
    - [x] SSMP idle - revert to MODE_UNKNOWN
    - [ ] Enable FIFO

diff --git a/design-notes.md b/design-notes.md
@@ -29,3 +29,6 @@
 - Vʜɪɢʜ = 0.7Vᴅᴅ (3.5V)
 - Vʜ    = 3.5V - 1.5V = 2V
 
+## SSMP
+1. Does not handle bignums.
+