change message buffer initialization

[z180-stamp.git] / avr / z180-serv.c

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index 929cd97148472d060afc2675f7bcdc4ae6bbf602..b5d8140dcee1ab3f327dd54e6d06bce83c11d35e 100644 (file)
--- 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++)