3 files changed, 26 insertions, 22 deletions
diff --git a/sw/z80/drivers/include/pio.h b/sw/z80/drivers/include/pio.h
index a1137e4..e234b05 100644
--- a/sw/z80/drivers/include/pio.h
+++ b/sw/z80/drivers/include/pio.h
@@ -4,6 +4,9 @@
 #include "addresses.h"
 #include <stdint.h>
 
+// DEBUG
+#define PIO_ASM_INTERFACE
+
 // ports
 #define PIO_A       0
 #define PIO_B       1
@@ -12,10 +15,10 @@
 #define PIO_REG_DATA 0
 #define PIO_REG_CTRL 2
 
-// #define PIO_REG_DATA_A      (PIO_A + PIO_REG_PORT)
-// #define PIO_REG_DATA_B 1    (PIO_B + PIO_REG_PORT)
-// #define PIO_REG_CTRL_A 2    (PIO_A + PIO_REG_CTRL)
-// #define PIO_REG_CTRL_B 3    (PIO_B + PIO_REG_CTRL)
+#define PIO_REG_DATA_A      (PIO_A | PIO_REG_PORT)
+#define PIO_REG_DATA_B 1    (PIO_B | PIO_REG_PORT)
+#define PIO_REG_CTRL_A 2    (PIO_A | PIO_REG_CTRL)
+#define PIO_REG_CTRL_B 3    (PIO_B | PIO_REG_CTRL)
 
 #define PIO_MODE_BYTE_OUT  0 // mode 0
 #define PIO_MODE_BYTE_IN   1 // mode 1
diff --git a/sw/z80/drivers/include/usart.h b/sw/z80/drivers/include/usart.h
index fdc01e2..05aeb4a 100644
--- a/sw/z80/drivers/include/usart.h
+++ b/sw/z80/drivers/include/usart.h
@@ -6,7 +6,7 @@
 #include <stdint.h>
 #include <string.h>
 
-// baudrate clock divisors 
+// baudrate clock divisors
 // values from TL16C550C datasheet (table 9 for 1.8432 MHz crystal)
 #define USART_BAUDRATE_50       2304
 #define USART_BAUDRATE_75       1536
@@ -125,7 +125,7 @@ struct MSR
     volatile uint data_carrier_detect :1;
 };
 
-/* this structure is only for internal usage */
+/* this structure is only for internal use */
 struct _usart_device
 {
     register_t buffer;  // also used as LSB for divisor latch
diff --git a/sw/z80/drivers/pio.c b/sw/z80/drivers/pio.c
index 272e191..f396e43 100644
--- a/sw/z80/drivers/pio.c
+++ b/sw/z80/drivers/pio.c
@@ -1,33 +1,34 @@
 #include "pio.h"
 
 #ifdef PIO_ASM_INTERFACE
-/* old inline asm implementation */
-inline void _pio_write(uint8_t reg, uint8_t data)
+// TODO: set inline
+void _pio_write(uint8_t reg, uint8_t data)
 {
     __asm
-    ;; pop function arguments
+    ;;  pop function arguments data in h, reg in a (l)
     pop hl
-    ;; store data in A
-    ld a, h
-    ld h, #0
+    ld a, l
     ;; add ADDR_DEV_PIO to get the device address
-    ld bc, #ADDR_DEV_PIO
-    add hl, bc
+    add a, #ADDR_DEV_PIO
+    ld c, a
     ;; load data
-    ld (hl), a
+    out (c), h
     __endasm;
 }
 
-inline uint8_t _pio_read(uint8_t reg) __naked
+inline uint8_t _pio_read(uint8_t reg)
 {
+    // TODO: check "dec sp"
     __asm
     ;; pop function argument
-    pop l
-    ld h, #0
+    dec sp
+    pop hl
+    ld a, l
     ;; add ADDR_DEV_PIO to get the device address
-    ld bc, #ADDR_DEV_PIO
-    add hl, bc
-    ld l, (hl)
+    add a, #ADDR_DEV_PIO
+    ld c, a
+    ;; read data
+    in l, (c)
     ret
     __endasm;
 }
@@ -68,7 +69,7 @@ void pio_set_interrupts_mask(int port, int control, uint8_t mask)
 {
     // 0x17 is a control sequence to set interrupts
     // AND interpret the next byte as a bitmask
-    _pio_write((PIO_REG_CTRL + port),(control | 0x17));
+    _pio_write((PIO_REG_CTRL + port),(control | 0x97));
     _pio_write((PIO_REG_CTRL + port), mask);
 }