command: Support 2-byte message ids

Allow command ids, response ids, and output ids to be either 1 or 2
bytes long.  This increases the total number of message types from 128
to 16384.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

3 files changed, 33 insertions, 8 deletions

diff --git a/src/command.c b/src/command.c
index 39c09458..d2d05aff 100644
--- a/src/command.c
+++ b/src/command.c
@@ -1,6 +1,6 @@
 // Code for parsing incoming commands and encoding outgoing messages
 //
-// Copyright (C) 2016,2017  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2016-2024  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
@@ -69,6 +69,28 @@ parse_int(uint8_t **pp)
     return v;
 }
 
+// Write an encoded msgid (optimized 2-byte VLQ encoder)
+static uint8_t *
+encode_msgid(uint8_t *p, uint_fast16_t encoded_msgid)
+{
+    if (encoded_msgid >= 0x80)
+        *p++ = (encoded_msgid >> 7) | 0x80;
+    *p++ = encoded_msgid & 0x7f;
+    return p;
+}
+
+// Parse an encoded msgid (optimized 2-byte parser, return as positive number)
+uint_fast16_t
+command_parse_msgid(uint8_t **pp)
+{
+    uint8_t *p = *pp;
+    uint_fast16_t encoded_msgid = *p++;
+    if (encoded_msgid & 0x80)
+        encoded_msgid = ((encoded_msgid & 0x7f) << 7) | (*p++);
+    *pp = p;
+    return encoded_msgid;
+}
+
 // Parse an incoming command into 'args'
 uint8_t *
 command_parsef(uint8_t *p, uint8_t *maxend
@@ -119,7 +141,7 @@ command_encodef(uint8_t *buf, const struct command_encoder *ce, va_list args)
     uint8_t *maxend = &p[max_size - MESSAGE_MIN];
     uint_fast8_t num_params = READP(ce->num_params);
     const uint8_t *param_types = READP(ce->param_types);
-    *p++ = READP(ce->msg_id);
+    p = encode_msgid(p, READP(ce->encoded_msgid));
     while (num_params--) {
         if (p > maxend)
             goto error;
@@ -227,7 +249,7 @@ DECL_SHUTDOWN(sendf_shutdown);
 
 // Find the command handler associated with a command
 static const struct command_parser *
-command_lookup_parser(uint_fast8_t cmdid)
+command_lookup_parser(uint_fast16_t cmdid)
 {
     if (!cmdid || cmdid >= READP(command_index_size))
         shutdown("Invalid command");
@@ -309,7 +331,7 @@ command_dispatch(uint8_t *buf, uint_fast8_t msglen)
     uint8_t *p = &buf[MESSAGE_HEADER_SIZE];
     uint8_t *msgend = &buf[msglen-MESSAGE_TRAILER_SIZE];
     while (p < msgend) {
-        uint_fast8_t cmdid = *p++;
+        uint_fast16_t cmdid = command_parse_msgid(&p);
         const struct command_parser *cp = command_lookup_parser(cmdid);
         uint32_t args[READP(cp->num_args)];
         p = command_parsef(p, msgend, cp, args);
diff --git a/src/command.h b/src/command.h
index 894114d7..21b3f79b 100644
--- a/src/command.h
+++ b/src/command.h
@@ -57,11 +57,13 @@
 #define MESSAGE_SYNC 0x7E
 
 struct command_encoder {
-    uint8_t msg_id, max_size, num_params;
+    uint16_t encoded_msgid;
+    uint8_t max_size, num_params;
     const uint8_t *param_types;
 };
 struct command_parser {
-    uint8_t msg_id, num_args, flags, num_params;
+    uint16_t encoded_msgid;
+    uint8_t num_args, flags, num_params;
     const uint8_t *param_types;
     void (*func)(uint32_t *args);
 };
@@ -72,6 +74,7 @@ enum {
 
 // command.c
 void *command_decode_ptr(uint32_t v);
+uint_fast16_t command_parse_msgid(uint8_t **pp);
 uint8_t *command_parsef(uint8_t *p, uint8_t *maxend
                         , const struct command_parser *cp, uint32_t *args);
 uint_fast8_t command_encode_and_frame(
@@ -86,7 +89,7 @@ int_fast8_t command_find_and_dispatch(uint8_t *buf, uint_fast8_t buf_len
 
 // out/compile_time_request.c (auto generated file)
 extern const struct command_parser command_index[];
-extern const uint8_t command_index_size;
+extern const uint16_t command_index_size;
 extern const uint8_t command_identify_data[];
 extern const uint32_t command_identify_size;
 const struct command_encoder *ctr_lookup_encoder(const char *str);
diff --git a/src/pru/pru0.c b/src/pru/pru0.c
index 57d55d27..8a11e140 100644
--- a/src/pru/pru0.c
+++ b/src/pru/pru0.c
@@ -141,7 +141,7 @@ do_dispatch(uint8_t *buf, uint32_t msglen)
     uint8_t *msgend = &buf[msglen-MESSAGE_TRAILER_SIZE];
     while (p < msgend) {
         // Parse command
-        uint_fast8_t cmdid = *p++;
+        uint_fast16_t cmdid = command_parse_msgid(&p);
         const struct command_parser *cp = &SHARED_MEM->command_index[cmdid];
         if (!cmdid || cmdid >= SHARED_MEM->command_index_size
             || cp->num_args > ARRAY_SIZE(SHARED_MEM->next_command_args)) {