Server: add echo command and response

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index e0b763fa210934b17a1fe8b7db98ae1a6bf3e82d..ce264ae76ed8e691e0fb886f28df70e1a068e87e 100644 (file)
--- a/avr/z180-serv.c
+++ b/avr/z180-serv.c
@@ -52,6 +52,25 @@ uint32_t msg_to_addr(uint8_t *msg)
 }
 
 
+static int msg_xmit_header(uint8_t func, uint8_t subf, int len)
+{
+       z80_memfifo_putc(fifo_msgout, 0xAE);
+       z80_memfifo_putc(fifo_msgout, len+2);
+       z80_memfifo_putc(fifo_msgout, func);
+       z80_memfifo_putc(fifo_msgout, subf);
+
+       return 0;
+}
+
+int msg_xmit(uint8_t func, uint8_t subf, int len, uint8_t *msg)
+{
+       msg_xmit_header(func, subf, len);
+       while (len--)
+               z80_memfifo_putc(fifo_msgout, *msg++);
+
+       return 0;
+}
+
 void do_msg_ini_memfifo(uint8_t subf, int len, uint8_t * msg)
 {
        (void)len;
@@ -68,6 +87,15 @@ void do_msg_char_out(uint8_t subf, int len, uint8_t * msg)
                putchar(*msg++);
 }
 
+/* echo message */
+void do_msg_echo(uint8_t subf, int len, uint8_t * msg)
+{
+       (void)subf;
+
+       /* send re-echo */
+       msg_xmit(1, 3, len, msg);
+}
+
 
 const FLASH struct msg_item z80_messages[] =
 {
@@ -77,6 +105,9 @@ const FLASH struct msg_item z80_messages[] =
        { 1,
          1, 1,
          do_msg_char_out},
+       { 1,
+         2, 2,
+         do_msg_echo},
        { 0xff,                         /* end mark */
          0, 0,
          0},
@@ -178,7 +209,7 @@ int msg_handling(int state)
 
        if (pending) {
                switch (state) {
-               case 0:
+               case 0:                 /* need init */
                        z80_bus_cmd(Request);
                        uint32_t addr = z80_read(0x40) +
                                ((uint16_t) z80_read(0x41) << 8) +
@@ -189,7 +220,7 @@ int msg_handling(int state)
                                state = 1;
                        }
                        break;
-               case 1:
+               case 1:                 /* awaiting messages */
                        check_msg_fifo();
                        break;
                }