command: Refactor message block generation

Separate out the buffer management, message encoding, and message
framing code.

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

src/command.c

@@ -104,27 +104,16 @@ error:
     shutdown("Command parser error");
 }
 
-static uint8_t in_sendf;
-
-// Encode a message and transmit it
-void
-_sendf(const struct command_encoder *ce, ...)
+// Encode a message
+static uint8_t
+command_encodef(char *buf, uint8_t buf_len
+                , const struct command_encoder *ce, va_list args)
 {
-    if (readb(&in_sendf))
-        // This sendf call was made from an irq handler while the main
-        // code was already in sendf - just drop this sendf request.
-        return;
-    writeb(&in_sendf, 1);
-
-    uint8_t max_size = READP(ce->max_size);
-    char *buf = console_get_output(max_size + MESSAGE_MIN);
-    if (!buf)
-        goto done;
+    if (buf_len <= MESSAGE_MIN)
+        // Ack/Nak message
+        return buf_len;
     char *p = &buf[MESSAGE_HEADER_SIZE];
-    if (max_size) {
-        char *maxend = &p[max_size];
-        va_list args;
-        va_start(args, ce);
+    char *maxend = &p[buf_len - MESSAGE_MIN];
     uint8_t num_params = READP(ce->num_params);
     const uint8_t *param_types = READP(ce->param_types);
     *p++ = READP(ce->msg_id);
@@ -174,23 +163,48 @@ _sendf(const struct command_encoder *ce, ...)
             goto error;
         }
     }
-        va_end(args);
-    }
+    return p - buf + MESSAGE_TRAILER_SIZE;
+error:
+    shutdown("Message encode error");
+}
 
-    // Send message to serial port
-    uint8_t msglen = p+MESSAGE_TRAILER_SIZE - buf;
+// Add header and trailer bytes to a message block
+static void
+command_add_frame(char *buf, uint8_t msglen)
+{
     buf[MESSAGE_POS_LEN] = msglen;
     buf[MESSAGE_POS_SEQ] = next_sequence;
-    uint16_t crc = crc16_ccitt(buf, p-buf);
-    *p++ = crc>>8;
-    *p++ = crc;
-    *p++ = MESSAGE_SYNC;
+    uint16_t crc = crc16_ccitt(buf, msglen - MESSAGE_TRAILER_SIZE);
+    buf[msglen - MESSAGE_TRAILER_CRC + 0] = crc >> 8;
+    buf[msglen - MESSAGE_TRAILER_CRC + 1] = crc;
+    buf[msglen - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC;
+}
+
+static uint8_t in_sendf;
+
+// Encode and transmit a "response" message
+void
+_sendf(const struct command_encoder *ce, ...)
+{
+    if (readb(&in_sendf))
+        // This sendf call was made from an irq handler while the main
+        // code was already in sendf - just drop this sendf request.
+        return;
+    writeb(&in_sendf, 1);
+
+    uint8_t buf_len = READP(ce->max_size) + MESSAGE_MIN;
+    char *buf = console_get_output(buf_len);
+    if (!buf)
+        goto done;
+    va_list args;
+    va_start(args, ce);
+    uint8_t msglen = command_encodef(buf, buf_len, ce, args);
+    va_end(args);
+    command_add_frame(buf, msglen);
     console_push_output(msglen);
 done:
     writeb(&in_sendf, 0);
     return;
-error:
-    shutdown("Message encode error");
 }
 
 void