From 77073309775a03b4a3f6e700ea1d8ed073752c36 Mon Sep 17 00:00:00 2001
From: David Kuder <davidk@thewaffleiron.net>
Date: Fri, 14 Apr 2023 01:46:32 -0400
Subject: [PATCH] Build for 20230414 - Fixed config interface

---
 build.sh           |   1 +
 common/cfgtoken.h  |   4 +-
 common/config.c    | 165 +++++++++++++++++++++--------
 common/config.h    |   2 +-
 common/flash.c     |  14 +--
 common/main.c      |  40 ++++---
 vga/businterface.c |  10 +-
 vga/render.c       |   6 +-
 vga/render_80col.c |   8 +-
 vga/render_dgr.c   |  22 +++-
 vga/render_dhgr.c  |   8 +-
 vga/render_hires.c |   8 +-
 vga/render_lores.c |  20 +++-
 vga/render_text.c  |  16 ++-
 z80/businterface.c |  79 +++++++++++---
 z80/z80buf.h       |   7 ++
 z80/z80main.c      | 258 +++++++++++++++++++++++++++++++--------------
 17 files changed, 482 insertions(+), 186 deletions(-)

diff --git a/build.sh b/build.sh
index eee356d..7020972 100644
--- a/build.sh
+++ b/build.sh
@@ -10,6 +10,7 @@ build_firmware() {
     ( mkdir -p build/$1 && cd build/$1 && cmake ../.. )
   fi
   make -C build/$1
+  #tools/mksparse build/$1/$1.bin build/$1/$1.ota
 }
 
 build_firmware v2-analog-lc-4ns-z80
diff --git a/common/cfgtoken.h b/common/cfgtoken.h
index bd04c89..ba6a469 100644
--- a/common/cfgtoken.h
+++ b/common/cfgtoken.h
@@ -1,7 +1,7 @@
 
 #define NEWCONFIG_MAGIC       0x0001434E // "NC\x01\x00"
 #define NEWCONFIG_EOF_MARKER  0x00464F45 // "EOF\x00"
-#define CFGTOKEN_REVISION     0x0001434E // "RV\xXX\x00"
+#define CFGTOKEN_REVISION     0x00015652 // "RV\xXX\x00"
 
 #define CFGTOKEN_MODE_VGA     0x0000564D // "MV\x00\x00" VGA
 #define CFGTOKEN_MODE_PCPI    0x00005A4D // "MZ\x00\x00" PCPI Applicard
@@ -41,6 +41,8 @@
 #define CFGTOKEN_JD_HOST      0x0000484A // "JH\x00\x01" JetDirect Hostname
 #define CFGTOKEN_JD_PORT      0x0200444A // "JD\x00\x01" JetDirect Port
 
+#define CFGTOKEN_FONT_00      0x00004656 // "VF\xXX\x00" Custom default font
+
 #define CFGTOKEN_MONO_00      0x00005056 // "VP\x00\x00" Full Color Video
 #define CFGTOKEN_MONO_80      0x00805056 // "VP\x80\x00" B&W Video
 #define CFGTOKEN_MONO_90      0x00905056 // "VP\x90\x00" B&W Inverse
diff --git a/common/config.c b/common/config.c
index f939f26..2bce47d 100644
--- a/common/config.c
+++ b/common/config.c
@@ -6,6 +6,8 @@
 #ifdef FUNCTION_VGA
 #include "vga/render.h"
 #include "vga/vgaout.h"
+extern volatile uint8_t romx_textbank;
+extern volatile uint8_t romx_changed;
 #endif
 #ifdef FUNCTION_Z80
 #include "z80/z80buf.h"
@@ -83,6 +85,10 @@ bool DELAYED_COPY_CODE(parse_config)(uint32_t address) {
                 cfg_machine = MACHINE_BASIS;
                 break;
 #ifdef FUNCTION_VGA
+            case CFGTOKEN_FONT_00:
+                romx_textbank = (config[i] >> 16) & 0x2F;
+                romx_changed = 1;
+                break;
             case CFGTOKEN_MONO_00:
                 mono_palette = (config[i] >> 16) & 0xF;
                 break;
@@ -128,11 +134,11 @@ bool DELAYED_COPY_CODE(parse_config)(uint32_t address) {
                 break;
             case CFGTOKEN_WIFI_SSID:
                 memset((char*)wifi_ssid, 0, sizeof(wifi_ssid));
-                strncpy((char*)wifi_ssid, (char*)(config+i+1), (config[i] >> 24));
+                strncpy((char*)wifi_ssid, (char*)(&config[i+1]), (config[i] >> 24));
                 break;
             case CFGTOKEN_WIFI_PSK:
                 memset((char*)wifi_psk, 0, sizeof(wifi_psk));
-                strncpy((char*)wifi_psk, (char*)(config+i+1), (config[i] >> 24));
+                strncpy((char*)wifi_psk, (char*)(&config[i+1]), (config[i] >> 24));
                 break;
             case CFGTOKEN_WIFI_IP:
                 wifi_address = config[i+1];
@@ -142,7 +148,7 @@ bool DELAYED_COPY_CODE(parse_config)(uint32_t address) {
                 break;
             case CFGTOKEN_JD_HOST:
                 memset((uint8_t*)jd_host, 0, sizeof(jd_host));
-                strncpy(jd_host, (char*)(config+i+1), (config[i] >> 24));
+                strncpy(jd_host, (char*)(&config[i+1]), (config[i] >> 24));
                 break;
             case CFGTOKEN_JD_PORT:
                 jd_port = config[i+1];
@@ -169,13 +175,13 @@ void DELAYED_COPY_CODE(default_config)() {
     cfg_machine = MACHINE_AUTO;
 }
 
-int DELAYED_COPY_CODE(make_config)(uint8_t rev) {
+int DELAYED_COPY_CODE(make_config)(uint32_t rev) {
     int i = 0;
 
     memset(config_temp, 0, sizeof(config_temp));
 
     config_temp[i++] = NEWCONFIG_MAGIC;
-    config_temp[i++] = CFGTOKEN_REVISION | (((uint16_t)rev) << 16);
+    config_temp[i++] = CFGTOKEN_REVISION | ((rev & 0xff) << 16);
 
     switch(cfg_machine) {
     default:
@@ -258,21 +264,19 @@ int DELAYED_COPY_CODE(make_config)(uint8_t rev) {
     }
 
     config_temp[i] = CFGTOKEN_WIFI_SSID | (((uint32_t)strlen((char*)wifi_ssid)+1) << 24);
-    strcpy((char*)(config_temp+i+1), (char*)wifi_ssid);
+    strcpy((char*)(&config_temp[i+1]), (char*)wifi_ssid);
     i += 1 + (((config_temp[i] >> 24) + 3) >> 2);
 
     config_temp[i] = CFGTOKEN_WIFI_PSK | (((uint32_t)strlen((char*)wifi_psk)+1) << 24);
-    strcpy((char*)(config_temp+i+1), (char*)wifi_psk);
+    strcpy((char*)(&config_temp[i+1]), (char*)wifi_psk);
     i += 1 + (((config_temp[i] >> 24) + 3) >> 2);
 #endif
 
 #ifdef FUNCTION_VGA
+    config_temp[i++] = CFGTOKEN_FONT_00 | ((romx_textbank & 0x2F) << 20);
     config_temp[i++] = CFGTOKEN_MONO_00 | ((mono_palette & 0xF) << 20);
     config_temp[i++] = CFGTOKEN_TBCOLOR | ((terminal_tbcolor & 0xFF) << 16);
     config_temp[i++] = CFGTOKEN_BORDER | ((terminal_border & 0xF) << 16);
-
-    // TODO: Font
-    //config_temp[i++] = CFGTOKEN_FONTSLOT | ((terminal_font & 0x1F) << 16);
 #endif
     config_temp[i++] = NEWCONFIG_EOF_MARKER;
 
@@ -323,6 +327,7 @@ bool DELAYED_COPY_CODE(is_primary_config_newer)() {
     return (a == ((b+1) & 0xff));
 }
 
+
 bool DELAYED_COPY_CODE(read_config)() {
     if(is_config_valid(FLASH_CONFIG_ONETIME)) {
         internal_flags &= ~IFLAGS_TEST;
@@ -390,38 +395,33 @@ bool DELAYED_COPY_CODE(write_config)(bool onetime) {
     return retval;
 }
 
-#ifdef FUNCTION_VGA
-uint8_t DELAYED_COPY_CODE(test_font)(uint16_t param0) {
-    int i;
+uint8_t DELAYED_COPY_CODE(get_config_block)() {
+    uint16_t last_config_block;
+    uint16_t next_config_block;
 
-    for(i = 0; i < 4096; i++) {
-        character_rom[i] = apple_memory[param0+i];
+    last_config_block = (FLASH_CONFIG_PRIMARY-XIP_BASE) / 4096;
+    next_config_block = last_config_block;
+
+    if(is_config_valid(FLASH_CONFIG_PRIMARY)) {
+        if(!is_config_valid(FLASH_CONFIG_SECONDARY) || (is_primary_config_newer())) {
+            last_config_block = (FLASH_CONFIG_PRIMARY-XIP_BASE) / 4096;
+            next_config_block = (FLASH_CONFIG_SECONDARY-XIP_BASE) / 4096;
+        } else {
+            last_config_block = (FLASH_CONFIG_SECONDARY-XIP_BASE) / 4096;
+            next_config_block = (FLASH_CONFIG_PRIMARY-XIP_BASE) / 4096;
+        }
+    } else if(is_config_valid(FLASH_CONFIG_SECONDARY)) {
+        last_config_block = (FLASH_CONFIG_SECONDARY-XIP_BASE) / 4096;
+        next_config_block = (FLASH_CONFIG_PRIMARY-XIP_BASE) / 4096;
     }
-
-    return REPLY_OK;
-}
-
-uint8_t DELAYED_COPY_CODE(write_font)(uint16_t param0, uint16_t param1) {
-    int i;
     
-    if(param1 > 0x27) return REPLY_EPARAM;
-
-    // Disable video output to stop DMA and IRQs
-    vga_dpms_sleep();
-
-    for(i = 0; i < 4096; i++) {
-        character_rom[i] = apple_memory[param0+i];
-    }
-
-    flash_range_erase((FLASH_FONT(param1)-XIP_BASE), FONT_SIZE);
-    flash_range_program((FLASH_FONT(param1)-XIP_BASE), character_rom, FONT_SIZE);
-
-    // Enable video output now that we are done
-    vga_dpms_wake();
+    config_rpybuf[5] = (next_config_block >> 8) & 0xFF;
+    config_rpybuf[4] = (next_config_block >> 0) & 0xFF;
+    config_rpybuf[3] = (last_config_block >> 8) & 0xFF;
+    config_rpybuf[2] = (last_config_block >> 0) & 0xFF;
 
     return REPLY_OK;
 }
-#endif
 
 uint8_t DELAYED_COPY_CODE(cf_readblock)(uint16_t param0) {
     if(param0 >= (FLASH_SIZE/4096)) return REPLY_EPARAM;
@@ -454,7 +454,7 @@ uint8_t DELAYED_COPY_CODE(cf_writeblock)(uint16_t param0) {
     vga_dpms_sleep();
 #endif
 
-    flash_range_program(param0 * 4096, (void*)cfbuf, 4096);
+    flash_range_program(((uint32_t)param0) * 4096, (void*)cfbuf, 4096);
 
 #ifdef FUNCTION_VGA
     // Enable video output now that we are done
@@ -477,7 +477,66 @@ uint8_t DELAYED_COPY_CODE(cf_eraseblock)(uint16_t param0) {
     vga_dpms_sleep();
 #endif
 
-    flash_range_erase(param0 * 4096, 4096);
+    flash_range_erase(((uint32_t)param0) * 4096, 4096);
+
+#ifdef FUNCTION_VGA
+    // Enable video output now that we are done
+    vga_dpms_wake();
+#endif
+
+    return REPLY_OK;
+}
+
+#ifdef FUNCTION_VGA
+uint8_t DELAYED_COPY_CODE(test_font)() {
+    int i;
+
+    for(i = 0; i < 2048; i++) {
+        character_rom[i] = cfbuf[i];
+    }
+    userfont = true;
+
+    return REPLY_OK;
+}
+#endif
+
+uint8_t DELAYED_COPY_CODE(read_font)(uint16_t param0) {
+    if(param0 > 0x27) return REPLY_EPARAM;
+
+    return cf_readblock((FLASH_FONT(param0)-XIP_BASE) / 4096);
+}
+
+uint8_t DELAYED_COPY_CODE(erase_font)(uint16_t param0) {
+    int i;
+    
+    if(param0 > 0x27) return REPLY_EPARAM;
+
+#ifdef FUNCTION_VGA
+    // Disable video output to stop DMA and IRQs
+    vga_dpms_sleep();
+#endif
+
+    flash_range_erase((FLASH_FONT(param0)-XIP_BASE), FONT_SIZE);
+
+#ifdef FUNCTION_VGA
+    // Enable video output now that we are done
+    vga_dpms_wake();
+#endif
+
+    return REPLY_OK;
+}
+
+uint8_t DELAYED_COPY_CODE(write_font)(uint16_t param0) {
+    int i;
+    
+    if(param0 > 0x27) return REPLY_EPARAM;
+
+#ifdef FUNCTION_VGA
+    // Disable video output to stop DMA and IRQs
+    vga_dpms_sleep();
+#endif
+
+    flash_range_program((FLASH_FONT(param0)-XIP_BASE), (void*)cfbuf, FONT_SIZE);
 
 #ifdef FUNCTION_VGA
     // Enable video output now that we are done
@@ -516,22 +575,37 @@ void DELAYED_COPY_CODE(config_handler)() {
     config_rpybuf[0] = REPLY_BUSY;
 
     switch(config_cmdbuf[0]) {
-#ifdef FUNCTION_VGA
         case 'C':
             switch(config_cmdbuf[1]) {
                 default:
                     retval = REPLY_ECMD;
                     break;
+#ifdef FUNCTION_VGA
                 case 'T':
                     // One-time load of font data (lost at reboot)
-                    retval = test_font(param0);
-                case 'I':
+                    retval = test_font();
+                case 'S':
+                    romx_textbank = param0 & 0x2F;
+                    romx_changed = 1;
+                    retval = REPLY_OK;
+#endif
+                case 'r':
+                    // Read font from flash
+                    retval = read_font(param0);
+                    cfptr = 0;
+                    break;
+                case 'w':
                     // Save font to flash
-                    retval = write_font(param0, param1);
+                    retval = write_font(param0);
+                    cfptr = 0;
+                    break;
+                case 'e':
+                    // Erase font from flash
+                    retval = erase_font(param0);
+                    cfptr = 0;
                     break;
             }
             break;
-#endif
         case 'H':
             retval = REPLY_OK;
             switch(config_cmdbuf[1]) {
@@ -579,6 +653,11 @@ void DELAYED_COPY_CODE(config_handler)() {
                 default:
                     retval = REPLY_ECMD;
                     break;
+                case 'c':
+                    // Current Config Block
+                    retval = get_config_block();
+                    cfptr = 0;
+                    break;
                 case 'r':
                     // Read block
                     retval = cf_readblock(param0);
diff --git a/common/config.h b/common/config.h
index bf3b03e..d219f21 100644
--- a/common/config.h
+++ b/common/config.h
@@ -101,7 +101,7 @@ extern volatile compat_t current_machine;
 
 
 void default_config();
-int make_config(uint8_t rev);
+int make_config(uint32_t rev);
 bool read_config();
 bool write_config(bool onetime);
 
diff --git a/common/flash.c b/common/flash.c
index 232eeaf..2f7f8cd 100644
--- a/common/flash.c
+++ b/common/flash.c
@@ -4,8 +4,10 @@
 #include <hardware/resets.h>
 #include <hardware/dma.h>
 #include <hardware/flash.h>
-#include "config.h"
-#include "flash.h"
+#include "common/config.h"
+#include "common/buffers.h"
+#include "common/flash.h"
+#include <string.h>
 
 #ifdef RASPBERRYPI_PICO_W
 #include <pico/cyw43_arch.h>
@@ -62,14 +64,14 @@ void __noinline __time_critical_func(flash_ota)() {
 
     uint32_t sniffed_crc = dma_sniffer_get_data_accumulator();
     if (0ul == sniffed_crc) {
-        uint8_t *ptr = (const uint8_t *)FLASH_OTA_AREA
+        uint8_t *ptr = (uint8_t *)FLASH_OTA_AREA;
         uint32_t offset;
         flash_range_erase(0, FLASH_OTA_SIZE);
 
         // Copy from OTA area to Boot
-        for(offset = 0; offset < FLASH_OTA_SIZE; offset+=65536);
-            memcpy(private_memory, ptr+offset, 65536);
-            flash_range_program(offset, private_memory, 65536);
+        for(offset = 0; offset < FLASH_OTA_SIZE; offset+=65536) {
+            memcpy((uint8_t *)private_memory, ptr+offset, 65536);
+            flash_range_program(offset, (const uint8_t *)private_memory, 65536);
         }
 
         flash_range_erase((uint32_t)(FLASH_OTA_AREA-XIP_BASE), FLASH_OTA_SIZE);
diff --git a/common/main.c b/common/main.c
index a6bcf1e..6212cb6 100644
--- a/common/main.c
+++ b/common/main.c
@@ -43,37 +43,45 @@ static void __noinline __time_critical_func(core1_loop)() {
 
                 // Config memory in card slot-rom address space
                 if(ACCESS_WRITE) {
-                    if((address & 0xFF) == 0xED) {
+                    if((address & 0xFF) == 0xEC) {
                         apple_memory[address] = value;
-                        cfptr = (cfptr & 0x1F00) | value;
-                        apple_memory[address] = cfbuf[cfptr];
+                        cfptr = (cfptr & 0x0F00) | value;
+                        apple_memory[address+2] = cfbuf[cfptr]; // $CnEE
+                        apple_memory[address+3] = cfbuf[cfptr]; // $CnEF
                     }
-                    if((address & 0xFF) == 0xEE) {
-                        apple_memory[address] = value & 0x1F;
-                        cfptr = ((cfptr & 0xFF) | (value << 8)) & 0x1FFF;
-                        apple_memory[address] = cfbuf[cfptr];
+                    if((address & 0xFF) == 0xED) {
+                        apple_memory[address] = value & 0x0F;
+                        cfptr = ((cfptr & 0xFF) | (((uint16_t)value) << 8)) & 0xFFF;
+                        apple_memory[address+1] = cfbuf[cfptr]; // $CnEE
+                        apple_memory[address+2] = cfbuf[cfptr]; // $CnEF
                     }
                     if((address & 0xFF) == 0xEF) {
-                        apple_memory[address] = value;
                         cfbuf[cfptr] = value;
+                        cfptr = (cfptr + 1) & 0x0FFF;
+                        apple_memory[address-1] = cfbuf[cfptr]; // $CnEE
+                        apple_memory[address]   = cfbuf[cfptr]; // $CnEF
                     }
                     if((address & 0xFF) >= 0xF0) {
                         apple_memory[address] = value;
                     }
-                }
-                if((address & 0xFF) == 0xEF) {
-                    cfptr = (cfptr + 1) & 0x1FFF;
-                    apple_memory[address] = cfbuf[cfptr];
+                } else if((address & 0xFF) == 0xEE) {
+                    cfptr = (cfptr + 1) & 0x0FFF;
+                    apple_memory[address]   = cfbuf[cfptr]; // $CnEE
+                    apple_memory[address+1] = cfbuf[cfptr]; // $CnEF
                     apple_memory[address-1] = (cfptr >> 8) & 0xff;
                     apple_memory[address-2] = cfptr & 0xff;
                 }
-
-                // Stop further processing by businterface
-                continue;
             }
 #ifdef FUNCTION_VGA
         } else if(current_machine == MACHINE_AUTO) {
-            if((apple_memory[0x0417] == 0xE7) && (apple_memory[0x416] == 0xC9)) { // Apple IIgs
+            if((apple_memory[0x0403] == 0xD8) && (apple_memory[0x404] == 0xE5)) { // ROMXe
+                current_machine = MACHINE_IIE;
+                internal_flags |= IFLAGS_IIE_REGS;
+                internal_flags &= ~IFLAGS_IIGS_REGS;
+            } else if((apple_memory[0x0412] == 0xC5) && (apple_memory[0x0413] == 0xD8)) { // ROMX
+                current_machine = MACHINE_II;
+                internal_flags &= ~(IFLAGS_IIE_REGS | IFLAGS_IIGS_REGS);
+            } else if((apple_memory[0x0417] == 0xE7) && (apple_memory[0x416] == 0xC9)) { // Apple IIgs
                 current_machine = MACHINE_IIGS;
                 internal_flags &= ~IFLAGS_IIE_REGS;
                 internal_flags |= IFLAGS_IIGS_REGS;
diff --git a/vga/businterface.c b/vga/businterface.c
index 463af66..fd8494b 100644
--- a/vga/businterface.c
+++ b/vga/businterface.c
@@ -157,7 +157,7 @@ void __time_critical_func(vga_businterface)(uint32_t address, uint32_t value) {
             }
         } else if(romx_unlocked == 3) {
             if(romx_type == 0xFA) {
-                if((address >> 4) == 0xF81) {
+                if((address & 0xFFF0) == 0xF810) {
                     romx_textbank = address & 0xF;
                 }
                 if(address == 0xF851) {
@@ -165,10 +165,10 @@ void __time_critical_func(vga_businterface)(uint32_t address, uint32_t value) {
                     romx_unlocked = 0;
                 }
             } else if(romx_type == 0xCA) {
-                if((address >> 4) == 0xCFD) {
+                if((address & 0xFFF0) == 0xCFD0) {
                     romx_textbank = address & 0xF;
                 }
-                if((address >> 4) == 0xCFE) {
+                if((address & 0xFFF0) == 0xCFE0) {
                     romx_changed = 1;
                     romx_unlocked = 0;
                 }
@@ -237,8 +237,8 @@ void __time_critical_func(vga_businterface)(uint32_t address, uint32_t value) {
                 terminal_memory[terminal_row*80+terminal_col] = value;
                 break;
             case 0x0B:
-                if(value <= 0x27) {
-                    romx_textbank = value;
+                if((value & 0xFF) <= 0x27) {
+                    romx_textbank = (value & 0xFF);
                     romx_changed = 1;
                 }
                 break;
diff --git a/vga/render.c b/vga/render.c
index 2497e7f..b2c1014 100644
--- a/vga/render.c
+++ b/vga/render.c
@@ -27,10 +27,10 @@ uint16_t DELAYED_COPY_DATA(mono_colors)[14] = {
 
 // Initialize the character generator ROM
 static void DELAYED_COPY_CODE(switch_font)() {
-    if(userfont) {
-        return;
-    } else if(romx_changed) {
+    if(romx_changed) {
         memcpy32(character_rom, (void*)FLASH_FONT(romx_textbank), 4096);
+    } else if(userfont) {
+        return;
     } else if(current_machine != machinefont) {
         switch(current_machine) {
         default:
diff --git a/vga/render_80col.c b/vga/render_80col.c
index 9cdc8be..1a433dc 100644
--- a/vga/render_80col.c
+++ b/vga/render_80col.c
@@ -37,7 +37,9 @@ void DELAYED_COPY_CODE(render_terminal_line)(unsigned int line) {
         uint sl_pos = 0;
 
         // Pad 40 pixels on the left to center horizontally
-        sl->data[sl_pos++] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 16 pixels per word
 
         for(uint col=0; col < 80; ) {
             // Grab 14 pixels from the next two characters
@@ -62,7 +64,9 @@ void DELAYED_COPY_CODE(render_terminal_line)(unsigned int line) {
         }
 
         // Pad 40 pixels on the right to center horizontally
-        sl->data[sl_pos++] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 16 pixels per word
 
         sl->length = sl_pos;
         sl->repeat_count = 1;
diff --git a/vga/render_dgr.c b/vga/render_dgr.c
index 5ef9a38..3aac52e 100644
--- a/vga/render_dgr.c
+++ b/vga/render_dgr.c
@@ -46,9 +46,16 @@ static void DELAYED_COPY_CODE(render_dgr_line)(bool p2, uint line) {
     const uint8_t *line_bufb = (const uint8_t *)((p2 ? text_p4 : text_p3) + ((line & 0x7) << 7) + (((line >> 3) & 0x3) * 40));
 
     // Pad 40 pixels on the left to center horizontally
-    sl1->data[sl_pos] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
-    sl2->data[sl_pos] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
     sl_pos++;
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl_pos++;
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
+    sl_pos++;
+
 
     if((soft_switches & SOFTSW_MONOCHROME) || (mono_palette & 0x8)) {
         for(i = 0; i < 40; i++) {
@@ -88,9 +95,16 @@ static void DELAYED_COPY_CODE(render_dgr_line)(bool p2, uint line) {
     }
 
     // Pad 40 pixels on the right to center horizontally
-    sl1->data[sl_pos] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
-    sl2->data[sl_pos] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
     sl_pos++;
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl_pos++;
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
+    sl_pos++;
+
 
     sl1->length = sl_pos;
     sl1->repeat_count = 7;
diff --git a/vga/render_dhgr.c b/vga/render_dhgr.c
index 4a4b26d..386291d 100644
--- a/vga/render_dhgr.c
+++ b/vga/render_dhgr.c
@@ -55,7 +55,9 @@ static void DELAYED_COPY_CODE(render_dhgr_line)(bool p2, uint line) {
     const uint8_t *line_memb = (const uint8_t *)((p2 ? hgr_p4 : hgr_p3) + dhgr_line_to_mem_offset(line));
 
     // Pad 40 pixels on the left to center horizontally
-    sl->data[sl_pos++] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 16 pixels per word
 
     // DHGR is weird. Nuff said.
     uint32_t dots = 0;
@@ -123,7 +125,9 @@ static void DELAYED_COPY_CODE(render_dhgr_line)(bool p2, uint line) {
     }
 
     // Pad 40 pixels on the right to center horizontally
-    sl->data[sl_pos++] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 16 pixels per word
 
     sl->length = sl_pos;
     sl->repeat_count = 1;
diff --git a/vga/render_hires.c b/vga/render_hires.c
index 55ad8ad..987fd7e 100644
--- a/vga/render_hires.c
+++ b/vga/render_hires.c
@@ -45,7 +45,9 @@ static void DELAYED_COPY_CODE(render_hires_line)(bool p2, uint line) {
     const uint8_t *line_mem = (const uint8_t *)((p2 ? hgr_p2 : hgr_p1) + hires_line_to_mem_offset(line));
 
     // Pad 40 pixels on the left to center horizontally
-    sl->data[sl_pos++] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 16 pixels per word
 
     // Each hires byte contains 7 pixels which may be shifted right 1/2 a pixel. That is
     // represented here by 14 'dots' to precisely describe the half-pixel positioning.
@@ -105,7 +107,9 @@ static void DELAYED_COPY_CODE(render_hires_line)(bool p2, uint line) {
     }
 
     // Pad 40 pixels on the right to center horizontally
-    sl->data[sl_pos++] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl->data[sl_pos++] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 16 pixels per word
 
     sl->length = sl_pos;
     sl->repeat_count = 1;
diff --git a/vga/render_lores.c b/vga/render_lores.c
index 2ac43c0..8f14e5a 100644
--- a/vga/render_lores.c
+++ b/vga/render_lores.c
@@ -69,8 +69,14 @@ static void DELAYED_COPY_CODE(render_lores_line)(bool p2, uint line) {
     const uint8_t *line_buf = (const uint8_t *)((p2 ? text_p2 : text_p1) + ((line & 0x7) << 7) + (((line >> 3) & 0x3) * 40));
 
     // Pad 40 pixels on the left to center horizontally
-    sl1->data[sl_pos] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
-    sl2->data[sl_pos] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl_pos++;
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl_pos++;
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
     sl_pos++;
 
     if((soft_switches & SOFTSW_MONOCHROME) || (mono_palette & 0x8)) {
@@ -109,8 +115,14 @@ static void DELAYED_COPY_CODE(render_lores_line)(bool p2, uint line) {
     }
 
     // Pad 40 pixels on the right to center horizontally
-    sl1->data[sl_pos] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
-    sl2->data[sl_pos] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl_pos++;
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+    sl_pos++;
+    sl1->data[sl_pos] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
+    sl2->data[sl_pos] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
     sl_pos++;
 
     sl1->length = sl_pos;
diff --git a/vga/render_text.c b/vga/render_text.c
index 216f835..579a52b 100644
--- a/vga/render_text.c
+++ b/vga/render_text.c
@@ -66,7 +66,9 @@ void DELAYED_COPY_CODE(render_text40_line)(bool p2, unsigned int line) {
         uint sl_pos = 0;
 
         // Pad 40 pixels on the left to center horizontally
-        sl->data[sl_pos++] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
 
         for(uint col=0; col < 40; ) {
             // Grab 14 pixels from the next two characters
@@ -91,7 +93,9 @@ void DELAYED_COPY_CODE(render_text40_line)(bool p2, unsigned int line) {
         }
 
         // Pad 40 pixels on the right to center horizontally
-        sl->data[sl_pos++] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
 
         sl->length = sl_pos;
         sl->repeat_count = 1;
@@ -110,7 +114,9 @@ void DELAYED_COPY_CODE(render_text80_line)(bool p2, unsigned int line) {
         uint sl_pos = 0;
 
         // Pad 40 pixels on the left to center horizontally
-        sl->data[sl_pos++] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
 
         for(uint col=0; col < 40; ) {
             // Grab 14 pixels from the next two characters
@@ -134,7 +140,9 @@ void DELAYED_COPY_CODE(render_text80_line)(bool p2, unsigned int line) {
         }
 
         // Pad 40 pixels on the right to center horizontally
-        sl->data[sl_pos++] = (text_border|THEN_EXTEND_31) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_7) | ((text_border|THEN_EXTEND_7) << 16); // 16 pixels per word
+        sl->data[sl_pos++] = (text_border|THEN_EXTEND_3) | ((text_border|THEN_EXTEND_3) << 16); // 8 pixels per word
 
         sl->length = sl_pos;
         sl->repeat_count = 1;
diff --git a/z80/businterface.c b/z80/businterface.c
index f0fd52a..a78a49c 100644
--- a/z80/businterface.c
+++ b/z80/businterface.c
@@ -9,39 +9,88 @@
 volatile uint8_t *pcpi_reg = apple_memory + 0xC0C0;
 
 static inline void __time_critical_func(pcpi_read)(uint32_t address) {
-    switch(address & 0x7) {
-    case 0: // Read Data from Z80
+    switch(address & 0xF) {
+    case 0x0: // Read Data from Z80
         clr_6502_stat;
         break;
-    case 5: // Z80 Reset
+    case 0x5: // Z80 Reset
         z80_res = 1;
         break;
-    case 6: // Z80 INT
+    case 0x6: // Z80 INT
         //z80_irq = 1;
         break;
-    case 7: // Z80 NMI
+    case 0x7: // Z80 NMI
         z80_nmi = 1;
         break;
+    case 0x8: // 6551 A Data Register
+        pcpi_reg[0x08] = auart_read(0);
+        pcpi_reg[0x09] = auart_status(0);
+        break;
+    case 0x9: // 6551 A Status Register
+        pcpi_reg[0x08] = zuart_peek(0);
+        pcpi_reg[0x09] = auart_status(0);
+        break;
+    case 0xA: // 6551 A Command Register
+        break;
+    case 0xB: // 6551 A Control Register
+        break;
+    case 0xC: // 6551 B Data Register
+        pcpi_reg[0x0C] = auart_read(1);
+        pcpi_reg[0x0D] = auart_status(1);
+        break;
+    case 0xD: // 6551 B Status Register
+        pcpi_reg[0x0C] = zuart_peek(1);
+        pcpi_reg[0x0D] = auart_status(1);
+        break;
+    case 0xE: // 6551 B Command Register
+        break;
+    case 0xF: // 6551 B Control Register
+        break;
     }
 }
 
 
 static inline void __time_critical_func(pcpi_write)(uint32_t address, uint32_t value) {
-    switch(address & 0x7) {
-    case 0:
-    case 2:
-    case 3:
-    case 4:
-        break;
-    case 1: // Write Data to Z80
+    switch(address & 0xF) {
+    case 0x1: // Write Data to Z80
         pcpi_reg[1] = value & 0xff;
         set_z80_stat;
         break;
-    case 5:
-    case 6:
-    case 7:
+    case 0x5: // Z80 Reset
+    case 0x6: // Z80 INT
+    case 0x7: // Z80 NMI
         pcpi_read(address);
         break;
+    case 0x8: // 6551 A Data Register
+        zuart_write(0, value);
+        pcpi_reg[0x08] = zuart_peek(0);
+        pcpi_reg[0x09] = auart_status(0);
+        break;
+    case 0x9: // 6551 A Reset Register
+        pcpi_reg[0x08] = zuart_peek(0);
+        pcpi_reg[0x09] = auart_status(0);
+        break;
+    case 0xA: // 6551 A Command Register
+        pcpi_reg[0x0A] = auart_command(0, value);
+        break;
+    case 0xB: // 6551 A Control Register
+        pcpi_reg[0x0B] = auart_control(0, value);
+        break;
+    case 0xC: // 6551 B Data Register
+        zuart_write(1, value);
+        pcpi_reg[0x0C] = zuart_peek(1);
+        pcpi_reg[0x0D] = auart_status(1);
+        break;
+    case 0xD: // 6551 B Reset Register
+        pcpi_reg[0x0C] = zuart_peek(1);
+        pcpi_reg[0x0D] = auart_status(1);
+        break;
+    case 0xE: // 6551 B Command Register
+        pcpi_reg[0x0E] = auart_command(1, value);
+        break;
+    case 0xF: // 6551 B Control Register
+        pcpi_reg[0x0F] = auart_control(1, value);
+        break;
     }
 }
 
diff --git a/z80/z80buf.h b/z80/z80buf.h
index ad81235..667face 100644
--- a/z80/z80buf.h
+++ b/z80/z80buf.h
@@ -38,3 +38,10 @@ typedef struct sio_s {
 
 extern volatile sio_t sio[2];
 extern volatile uint8_t sio_vector;
+
+extern uint8_t auart_read(bool port);
+extern uint8_t zuart_peek(bool port);
+extern uint8_t auart_status(bool port);
+extern uint8_t auart_command(bool port, uint8_t value);
+extern uint8_t auart_control(bool port, uint8_t value);
+extern uint8_t zuart_write(bool port, uint8_t value);
diff --git a/z80/z80main.c b/z80/z80main.c
index f11a43d..c3723d6 100644
--- a/z80/z80main.c
+++ b/z80/z80main.c
@@ -27,6 +27,183 @@ volatile uint8_t sio_vector;
 
 #define Z80break (z80_res || (config_cmdbuf[7] == 0) || (!z80_cycle++))
 
+uint8_t DELAYED_COPY_CODE(zuart_read)(bool port) {
+    uint8_t rv = 0;
+
+    if(sio[port].datavalid) {
+        rv = sio[port].data;
+        sio[port].datavalid = 0;
+    }
+
+    switch(serialmux[port]) {
+    case SERIAL_LOOP:
+        break;
+    case SERIAL_USB:
+        if(tud_cdc_n_available(port)) {
+            sio[port].data = tud_cdc_n_read_char(port);
+            sio[port].datavalid = 1;
+        }
+        break;
+    case SERIAL_UART:
+        if(port) {
+            if(uart_is_readable(uart1)) {
+                sio[port].data = uart_getc(uart1);
+                sio[port].datavalid = 1;
+            }
+        } else {
+            if(uart_is_readable(uart0)) {
+                sio[port].data = uart_getc(uart0);
+                sio[port].datavalid = 1;
+            }
+        }
+        break;
+    }
+    
+    return rv;
+}
+
+uint8_t DELAYED_COPY_CODE(zuart_peek)(bool port) {
+    uint8_t rv = 0;
+
+    if(sio[port].datavalid) {
+        rv = sio[port].data;
+    }
+
+    return rv;
+}
+
+uint8_t DELAYED_COPY_CODE(auart_read)(bool port) {
+    zuart_read(port);
+    return zuart_peek(port);
+}
+
+uint8_t DELAYED_COPY_CODE(auart_status)(bool port) {
+    uint8_t rv;
+
+    if(!sio[port].datavalid) {
+        zuart_read(port);
+    }
+
+    rv = sio[port].datavalid ? 0x08 : 0x00;
+
+    switch(serialmux[port]) {
+    case SERIAL_LOOP:
+        rv |= ((sio[port].control[5] & 0x02) ? 0x40 : 0x00) |
+             ((sio[port].control[5] & 0x80) ? 0x20 : 0x00) |
+             (sio[port].datavalid ? 0x00 : 0x10);
+        break;
+    case SERIAL_USB:
+        rv |= ((tud_cdc_n_get_line_state(port) & 2) ? 0x00 : 0x40) | 
+             (tud_cdc_n_connected(port) ? 0x00 : 0x20) |
+             (tud_cdc_n_write_available(port) ? 0x10 : 0x00);
+        break;
+    case SERIAL_UART:
+        if(port) {
+            rv |= (uart_is_writable(uart1) ? 0x10 : 0x00);
+        } else {
+            rv |= (uart_is_writable(uart0) ? 0x10 : 0x00);
+        }
+        break;
+    }
+
+    return rv;
+}
+
+uint8_t DELAYED_COPY_CODE(auart_control)(bool port, uint8_t value) {
+    return value;
+}
+
+uint8_t DELAYED_COPY_CODE(auart_command)(bool port, uint8_t value) {
+    if(value & 0x1) {
+        sio[port].control[5] |= 0x80;
+    } else {
+        sio[port].control[5] &= ~0x80;
+    }
+    if(value & 0xC) {
+        sio[port].control[5] |= 0x02;
+    } else {
+        sio[port].control[5] &= ~0x02;
+    }
+
+    return value;
+}
+
+uint8_t DELAYED_COPY_CODE(zuart_write)(bool port, uint8_t value) {
+    switch(serialmux[port]) {
+    case SERIAL_LOOP:
+        if(sio[port].datavalid) {
+            sio[port].status[1] |= 0x20;
+        }
+        sio[port].datavalid = 1;
+        sio[port].data = value;
+        break;
+    case SERIAL_UART:
+        if(tud_cdc_n_write_available(port)) {
+            tud_cdc_n_write_char(port, value);
+        }
+        break;
+    case SERIAL_USB:
+        if(port) {
+            if(uart_is_writable(uart1)) {
+                uart_putc(uart1, value);
+            }
+        } else {
+            if(uart_is_writable(uart0)) {
+                uart_putc(uart0, value);
+            }
+        }
+        break;
+    }
+    return value;
+}
+
+uint8_t DELAYED_COPY_CODE(zuart_status)(bool port) {
+    uint8_t rv = 0;
+
+    
+    switch(sio[port].control[0] & 0x7) {
+    case 0:
+        if(!sio[port].datavalid) {
+            zuart_read(port);
+        }
+
+        rv = sio[port].datavalid ? 0x01 : 0x00;
+
+        switch(serialmux[port]) {
+        case SERIAL_LOOP:
+            rv |= ((sio[port].control[5] & 0x02) ? 0x20 : 0x00) |
+                 ((sio[port].control[5] & 0x80) ? 0x08 : 0x00) |
+                 (sio[port].datavalid ? 0x00 : 0x04);
+            break;
+        case SERIAL_USB:
+            rv |= ((tud_cdc_n_get_line_state(port) & 2) ? 0x20 : 0x00) | 
+                 (tud_cdc_n_connected(port) ? 0x08 : 0x00) |
+                 (tud_cdc_n_write_available(port) ? 0x04 : 0x00);
+            break;
+        case SERIAL_UART:
+            if(port) {
+                rv |= 0x20 | 
+                     (uart_is_writable(uart1) ? 0x00 : 0x04);
+            } else {
+                rv |= 0x20 | 
+                     (uart_is_writable(uart0) ? 0x00 : 0x04);
+            }
+            break;
+        }
+        break;
+    case 1:
+        rv = sio[(port)].status[1];
+        break;
+    case 2:
+        if(port)
+            rv = sio_vector;
+        break;
+    }
+    sio[(port)].control[0] &= 0xF8;
+
+    return rv;
+}
+
 uint8_t DELAYED_COPY_CODE(cpu_in)(uint16_t address) {
     uint8_t rv = 0;
     if(address & 0x80) {
@@ -42,69 +219,11 @@ uint8_t DELAYED_COPY_CODE(cpu_in)(uint16_t address) {
             break;
         case 0xFC:
         case 0xFE:
-            switch(serialmux[(address & 0x01)]) {
-            case SERIAL_LOOP:
-                if(sio[(address & 0x01)].datavalid) {
-                    sio[(address & 0x01)].datavalid = 0;
-                    rv = sio[(address & 0x01)].data;
-                }
-                break;
-            case SERIAL_USB:
-                if(tud_cdc_n_available((address & 0x01))) {
-                    rv = tud_cdc_n_read_char((address & 0x01));
-                }
-                break;
-            case SERIAL_UART:
-                if(address & 0x01) {
-                    if(uart_is_readable(uart1)) {
-                        rv = uart_getc(uart1);
-                    }
-                } else {
-                    if(uart_is_readable(uart0)) {
-                        rv = uart_getc(uart0);
-                    }
-                }
-                break;
-            }
+            rv = zuart_read(address & 0x02);
             break;
         case 0xFD:
         case 0xFF:
-            switch(sio[(address & 0x01)].control[0] & 0x7) {
-            case 0:
-                switch(serialmux[(address & 0x01)]) {
-                case SERIAL_LOOP:
-                    rv = ((sio[(address & 0x01)].control[5] & 0x02) ? 0x20 : 0x00) |
-                         ((sio[(address & 0x01)].control[5] & 0x80) ? 0x08 : 0x00) |
-                         (sio[(address & 0x01)].datavalid ? 0x01 : 0x04);
-                    break;
-                case SERIAL_USB:
-                    rv = ((tud_cdc_n_get_line_state(address & 0x01) & 2) ? 0x20 : 0x00) | 
-                         (tud_cdc_n_connected(address & 0x01) ? 0x08 : 0x00) |
-                         (tud_cdc_n_write_available(address & 0x01) ? 0x04 : 0x00) |
-                         (tud_cdc_n_available(address & 0x01) ? 0x01 : 0x00);
-                    break;
-                case SERIAL_UART:
-                    if(address & 0x01) {
-                        rv = 0x20 | 
-                             (uart_is_writable(uart1) ? 0x00 : 0x04) |
-                             (uart_is_readable(uart1) ? 0x01 : 0x00);
-                    } else {
-                        rv = 0x20 | 
-                             (uart_is_writable(uart0) ? 0x00 : 0x04) |
-                             (uart_is_readable(uart0) ? 0x01 : 0x00);
-                    }
-                    break;
-                }
-                break;
-            case 1:
-                rv = sio[(address & 0x01)].status[1];
-                break;
-            case 2:
-                if(address & 0x01)
-                    rv = sio_vector;
-                break;
-            }
-            sio[(address & 0x01)].control[0] &= 0xF8;
+            rv = zuart_status(address & 0x02);
             break;
         }
     } else {
@@ -161,24 +280,7 @@ void DELAYED_COPY_CODE(cpu_out)(uint16_t address, uint8_t value) {
             break;
         case 0xFC:
         case 0xFE:
-            switch(serialmux[(address & 0x01)]) {
-            case SERIAL_LOOP:
-                if(sio[(address & 0x01)].datavalid) {
-                    sio[(address & 0x01)].status[1] |= 0x20;
-                }
-                sio[(address & 0x01)].datavalid = 1;
-                sio[(address & 0x01)].data = value;
-                break;
-            case SERIAL_UART:
-                if(tud_cdc_n_write_available(address & 0x01))
-                    tud_cdc_n_write_char(address & 0x01, value);
-                break;
-            case SERIAL_USB:
-                if(uart_is_writable(uart0)) {
-                    uart_putc(uart0, value);
-                }
-                break;
-            }
+            zuart_write((address & 0x02), value);
             break;
         case 0xFD:
         case 0xFF: