X-Git-Url: http://cloudbase.mooo.com/gitweb/z180-stamp.git/blobdiff_plain/c79c80b49dcde54add2e9ba323c9d07033723f52..e4c4b148ced1347935dff6200380e90cb3bfcde3:/avr/z180-serv.c?ds=sidebyside

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index e3bca16..ce264ae 100644
--- a/avr/z180-serv.c
+++ b/avr/z180-serv.c
@@ -1,3 +1,9 @@
+/*
+ * (C) Copyright 2014 Leo C. <erbl259-lmu@yahoo.de>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
 #include "common.h"
 #include <util/atomic.h>
 
@@ -7,8 +13,6 @@
 #include "debug.h"
 #include "z180-serv.h"
 
-
-
 /*--------------------------------------------------------------------------*/
 
 
@@ -48,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;
@@ -64,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[] =
 {
@@ -73,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},
@@ -174,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) +
@@ -185,7 +220,7 @@ int msg_handling(int state)
 				state = 1;
 			}
 			break;
-		case 1:
+		case 1:			/* awaiting messages */
 			check_msg_fifo();
 			break;
 		}