Server: add echo command and response

1 files changed, 33 insertions, 2 deletions

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index e0b763f..ce264ae 100644
--- 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;
 		}