3 files changed, 31 insertions, 25 deletions
diff --git a/sw/z80/kernel/include/pio.h b/sw/z80/kernel/include/pio.h
index 8d72ab0..5d289ca 100644
--- a/sw/z80/kernel/include/pio.h
+++ b/sw/z80/kernel/include/pio.h
@@ -1,14 +1,31 @@
 #ifndef __PIO_H__
 #define __PIO_H__
 
-#define PIO_A   0
-#define PIO_B   1
+#include "devices.h"
+#include "types.h"
 
+#define PIO_A       0
+#define PIO_B       1
 
-void pio_data(int port, uint8_t data);
-void pio_command(int port, uint8_t cmd);
+#define PIO_MODE_0  0
+#define PIO_MODE_1  1
+#define PIO_MODE_2  2
+#define PIO_MODE_3  3
 
-uint8_t pio_read_data(int port);
+#define PIO_INT_ACTIVE_HIGH     (1<<5)
+#define PIO_INT_AND_MODE        (1<<6)
+#define PIO_INT_ENABLE          (1<<7)
+
+
+void _pio_data(int port, uint8_t data);
+void _pio_command(int port, uint8_t cmd);
+
+void pio_set_mode(int port, int mode);
+void pio_set_interrupts(int port, int control);
+void pio_set_interrupts_mask(int port, uint8_t mask);
+void pio_set_io(int port, uint8_t io);
+
+// uint8_t pio_read_data(int port);
 
 inline int pio_read_pin(int port, uint8_t pin);
 inline void pio_write_pin(int port, uint8_t pin);
diff --git a/sw/z80/kernel/pio.c b/sw/z80/kernel/pio.c
index e8c7e24..4b9caee 100644
--- a/sw/z80/kernel/pio.c
+++ b/sw/z80/kernel/pio.c
@@ -1,29 +1,18 @@
 #include "pio.h"
 
-static uint8_t *pio_port_a = (uint8_t *) ADDR_DEV_PIO;
-static uint8_t *pio_port_b = (uint8_t *) ADDR_DEV_PIO + 1;
-static uint8_t *pio_ctrl_a = (uint8_t *) ADDR_DEV_PIO + 2;
-static uint8_t *pio_ctrl_b = (uint8_t *) ADDR_DEV_PIO + 3;
+static uint8_t *pio_port = (uint8_t *) ADDR_DEV_PIO;
+static uint8_t *pio_ctrl = (uint8_t *) (ADDR_DEV_PIO + 2);
 
-void pio_data(int port, uint8_t data)
+void _pio_data(int port, uint8_t data)
 {
-    if (port == PIO_A)
-        *pio_port_a = cmd;
-    else
-        *pio_port_b = cmd;
+    *(pio_port + port) = data;
 }
 
-void pio_command(int port, uint8_t cmd)
+void _pio_command(int port, uint8_t cmd)
 {
-    if (port == PIO_A)
-        *pio_ctrl_a = cmd;
-    else
-        *pio_ctrl_b = cmd;
+    *(pio_ctrl + port) = cmd;
 }
 
-
-
-inline int pio_read_pin(int port, uint8_t pin)
+void pio_set_mode(int port, int mode)
 {
-
 }
diff --git a/sw/z80/kernel/usart.c b/sw/z80/kernel/usart.c
index 4c2fdaf..9ec6dbd 100644
--- a/sw/z80/kernel/usart.c
+++ b/sw/z80/kernel/usart.c
@@ -65,8 +65,8 @@ int usart_write(uint8_t *data, size_t size)
 
     while (size--) {
         _usart->buffer = *(dp++);
+        while (_usart->LSR.transmitter_empty);
     }
-    while (_usart->LSR.transmitter_empty);
 
     // TODO: do something that actually counts for sent bytes 
     return size;
@@ -83,7 +83,7 @@ int usart_read(uint8_t *buffer, size_t count)
         if (_usart->LSR.framing_error || _usart->LSR.parity_error) {
             bp--; // delete last byte (?)
         } else {
-           read_count++;
+            read_count++;
         }
     }