2 files changed, 34 insertions, 5 deletions
diff --git a/src/avr/serial.c b/src/avr/serial.c
index 213b10ed..87067c7a 100644
--- a/src/avr/serial.c
+++ b/src/avr/serial.c
@@ -10,8 +10,9 @@
 #include "board/io.h" // readb
 #include "board/misc.h" // console_get_input
 #include "command.h" // DECL_CONSTANT
-#include "sched.h" // DECL_INIT
 #include "irq.h" // irq_save
+#include "pgm.h" // READP
+#include "sched.h" // DECL_INIT
 
 static char receive_buf[192];
 static uint8_t receive_pos;
@@ -139,7 +140,7 @@ console_task(void)
 DECL_TASK(console_task);
 
 // Return an output buffer that the caller may fill with transmit messages
-char *
+static char *
 console_get_output(uint8_t len)
 {
     uint8_t tpos = readb(&transmit_pos), tmax = readb(&transmit_max);
@@ -164,9 +165,22 @@ console_get_output(uint8_t len)
 }
 
 // Accept the given number of bytes added to the transmit buffer
-void
+static void
 console_push_output(uint8_t len)
 {
     writeb(&transmit_max, readb(&transmit_max) + len);
     enable_tx_irq();
 }
+
+// Encode and transmit a "response" message
+void
+console_sendf(const struct command_encoder *ce, va_list args)
+{
+    uint8_t buf_len = READP(ce->max_size);
+    char *buf = console_get_output(buf_len);
+    if (!buf)
+        return;
+    uint8_t msglen = command_encodef(buf, buf_len, ce, args);
+    command_add_frame(buf, msglen);
+    console_push_output(msglen);
+}
diff --git a/src/avr/usbserial.c b/src/avr/usbserial.c
index 78ef719e..7be94a67 100644
--- a/src/avr/usbserial.c
+++ b/src/avr/usbserial.c
@@ -7,6 +7,8 @@
 #include <avr/interrupt.h> // USART0_RX_vect
 #include <string.h> // memmove
 #include "../lib/pjrc_usb_serial/usb_serial.h"
+#include "command.h" // command_dispatch
+#include "pgm.h" // READP
 #include "sched.h" // DECL_INIT
 
 #define USBSERIAL_BUFFER_SIZE 64
@@ -62,7 +64,7 @@ console_task(void)
 DECL_TASK(console_task);
 
 // Return an output buffer that the caller may fill with transmit messages
-char *
+static char *
 console_get_output(uint8_t len)
 {
     if (len > sizeof(transmit_buf))
@@ -71,9 +73,22 @@ console_get_output(uint8_t len)
 }
 
 // Accept the given number of bytes added to the transmit buffer
-void
+static void
 console_push_output(uint8_t len)
 {
     usb_serial_write((void*)transmit_buf, len);
     usb_serial_flush_output();
 }
+
+// Encode and transmit a "response" message
+void
+console_sendf(const struct command_encoder *ce, va_list args)
+{
+    uint8_t buf_len = READP(ce->max_size);
+    char *buf = console_get_output(buf_len);
+    if (!buf)
+        return;
+    uint8_t msglen = command_encodef(buf, buf_len, ce, args);
+    command_add_frame(buf, msglen);
+    console_push_output(msglen);
+}