change message buffer initialization

1 files changed, 24 insertions, 23 deletions

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index 929cd97..b5d8140 100644
--- a/avr/z180-serv.c
+++ b/avr/z180-serv.c
@@ -581,35 +581,38 @@ int msg_handling(int state)
 		pending = (Stat & S_MSG_PENDING) != 0;
 		Stat &= ~S_MSG_PENDING;
 	}
-/*
- * TODO: if pending but no message chr --> special condition. ie init,...
- */
 
 	if (pending) {
-		switch (state) {
-		case 0:			/* need init */
+		uint8_t init_request;
+		z80_bus_cmd(Request);
+		init_request = z80_read(0x43);
+		z80_bus_cmd(Release);
+		if ( init_request != 0) {
 			/* Get address of fifo_list */
+			uint32_t fifo_addr = 0;
 			z80_bus_cmd(Request);
 			uint32_t fifo_list = z80_read(0x40) +
-				((uint16_t) z80_read(0x41) << 8) +
-				((uint32_t) z80_read(0x42) << 16);
-			z80_bus_cmd(Release);
+						((uint16_t) z80_read(0x41) << 8) +
+						((uint32_t) z80_read(0x42) << 16);
 			if (fifo_list != 0) {
 				/* Get address of fifo 0 */
-				z80_bus_cmd(Request);
-				uint32_t fifo_addr = z80_read(fifo_list) +
-					((uint16_t) z80_read(fifo_list+1) << 8) +
-					((uint32_t) z80_read(fifo_list+2) << 16);
-				z80_bus_cmd(Release);
-				if (fifo_addr != 0) {
-					z80_memfifo_init(fifo_msgin, fifo_addr);
-					state = 1;
-				}
+				fifo_addr = z80_read(fifo_list) +
+							((uint16_t) z80_read(fifo_list+1) << 8);
+							/* TODO: log to phys addr translation
+							((uint32_t) z80_read(fifo_list+2) << 16);
+							*/
 			}
-			break;
-		case 1:			/* awaiting messages */
+			z80_write(0x43, 0);
+			z80_bus_cmd(Release);
+
+			if (fifo_addr != 0) {
+				z80_memfifo_init(fifo_msgin, fifo_addr);
+				state = 1;
+			} else
+				state = 0;
+
+		} else {
 			check_msg_fifo();
-			break;
 		}
 	}
 
@@ -629,9 +632,7 @@ void setup_z180_serv(void)
 void restart_z180_serv(void)
 {
 	z80_bus_cmd(Request);
-	z80_write(0x40, 0);
-	z80_write(0x41, 0);
-	z80_write(0x42, 0);
+	z80_memset(0x40, 0, 4);
 	z80_bus_cmd(Release);
 
 	for (int i = 0; i < NUM_FIFOS; i++)