ndd disks working (retail and dev)

sw/n64/src/main.c

@@ -65,7 +65,7 @@ void main (void) {
         }
     }
 
-    LOG_I("Booting IPL3\r\n\r\n");
+    LOG_I("Booting IPL3\033[0m\r\n\r\n");
 
     boot(&boot_info);
 }

sw/pc/sc64.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 
+from io import TextIOWrapper
 from serial import Serial, SerialException
 from serial.tools import list_ports
 import argparse
@@ -7,6 +8,7 @@ import filecmp
 import os
 import progressbar
 import re
+import struct
 import sys
 import time
 
@@ -18,9 +20,6 @@ class SC64Exception(Exception):
 
 
 class SC64:
-    __CFG_ID_SCR = 0
-    __CFG_ID_SDRAM_SWITCH = 1
-    __CFG_ID_SDRAM_WRITABLE = 2
     __CFG_ID_DD_ENABLE = 3
     __CFG_ID_SAVE_TYPE = 4
     __CFG_ID_CIC_SEED = 5
@@ -33,6 +32,7 @@ class SC64:
     __CFG_ID_FLASH_PROGRAM = 12
     __CFG_ID_RECONFIGURE = 13
     __CFG_ID_DD_SETTING = 14
+    __CFG_ID_DD_THB_TABLE_OFFSET = 15
 
     __SC64_VERSION_V2 = 0x53437632
 
@@ -49,13 +49,12 @@ class SC64:
     __DEBUG_ID_FSD_SECTOR = 0xF3
     __DEBUG_ID_DD_BLOCK = 0xF5
 
-    __DD_USER_SECTORS_IN_BLOCK = 85
-
     __DD_SETTING_DISK_EJECTED = 0
     __DD_SETTING_DISK_INSERTED = 1
     __DD_SETTING_DISK_CHANGED = 2
     __DD_SETTING_DRIVE_RETAIL = 3
     __DD_SETTING_DRIVE_DEVELOPMENT = 4
+    __DD_SETTING_SET_BLOCK_READY = 5
 
     def __init__(self) -> None:
         self.__serial = None
@@ -97,8 +96,11 @@ class SC64:
         return re.sub(b"\x1B", b"\x1B\x1B", data)
 
 
-    def __reset_link(self) -> None:
+    def reset_link(self) -> None:
         self.__serial.write(b"\x1BR")
+        while (self.__serial.in_waiting):
+            self.__serial.read_all()
+            time.sleep(0.1)
 
 
     def __read(self, bytes: int) -> bytes:
@@ -151,10 +153,7 @@ class SC64:
                 try:
                     self.__serial = Serial(p.device, timeout=1.0, write_timeout=1.0)
                     self.__serial.flushOutput()
-                    self.__reset_link()
+                    self.reset_link()
-                    while (self.__serial.in_waiting):
-                        self.__serial.read_all()
-                        time.sleep(0.1)
                     self.__probe_device()
                 except (SerialException, SC64Exception):
                     if (self.__serial):
@@ -390,6 +389,153 @@ class SC64:
             raise SC64Exception("DD drive type outside of supported values")
 
 
+    def __dd_process_disk(self, handle: TextIOWrapper) -> tuple[str, bytes]:
+        DISK_TRACKS = 1175
+        DISK_HEADS = 2
+        DISK_BLOCKS_PER_TRACK = 2
+        DISK_SECTORS_PER_BLOCK = 85
+        DISK_SYSTEM_SECTOR_SIZE = 232
+
+        DISK_PZONES = [
+            (0, 0, 158, 232, 0,    0),
+            (1, 0, 158, 216, 158,  1),
+            (2, 0, 149, 208, 316,  2),
+            (3, 0, 149, 192, 465,  3),
+            (4, 0, 149, 176, 614,  4),
+            (5, 0, 149, 160, 763,  5),
+            (6, 0, 149, 144, 912,  6),
+            (7, 0, 114, 128, 1061, 7),
+            (1, 1, 158, 216, 157,  8),
+            (2, 1, 158, 208, 315,  9),
+            (3, 1, 149, 192, 464,  10),
+            (4, 1, 149, 176, 613,  11),
+            (5, 1, 149, 160, 762,  12),
+            (6, 1, 149, 144, 911,  13),
+            (7, 1, 149, 128, 1060, 14),
+            (8, 1, 114, 112, 1174, 15),
+        ] 
+
+        DISK_VZONE_TO_PZONE = [
+            [0, 1, 2, 9, 8, 3, 4, 5, 6, 7, 15, 14, 13, 12, 11, 10],
+            [0, 1, 2, 3, 10, 9, 8, 4, 5, 6, 7, 15, 14, 13, 12, 11],
+            [0, 1, 2, 3, 4, 11, 10, 9, 8, 5, 6, 7, 15, 14, 13, 12],
+            [0, 1, 2, 3, 4, 5, 12, 11, 10, 9, 8, 6, 7, 15, 14, 13],
+            [0, 1, 2, 3, 4, 5, 6, 13, 12, 11, 10, 9, 8, 7, 15, 14],
+            [0, 1, 2, 3, 4, 5, 6, 7, 14, 13, 12, 11, 10, 9, 8, 15],
+            [0, 1, 2, 3, 4, 5, 6, 7, 15, 14, 13, 12, 11, 10, 9, 8],
+        ]
+
+        DRIVE_TYPES = [{
+            "drive_type": "development",
+            "system_lbas": [11, 10, 3, 2],
+            "sector_size": 192,
+        }, {
+            "drive_type": "retail",
+            "system_lbas": [9, 8, 1, 0],
+            "sector_size": 232,
+        }]
+
+        block_valid = False
+        system_data = None
+        disk_type = None
+        drive_type = None
+
+        for drive in DRIVE_TYPES:
+            if (block_valid):
+                break
+
+            for system_lba in drive["system_lbas"]:
+                block_valid = True
+                sector_size = drive["sector_size"]
+
+                handle.seek(system_lba * DISK_SYSTEM_SECTOR_SIZE * DISK_SECTORS_PER_BLOCK)
+                block_data = handle.read(sector_size * DISK_SECTORS_PER_BLOCK)
+
+                system_data = block_data[:sector_size]
+
+                for sector in range(1, DISK_SECTORS_PER_BLOCK):
+                    sector_data = block_data[(sector * sector_size):][:sector_size]
+                    if (system_data != sector_data):
+                        block_valid = False
+
+                if (system_data[4] != 0x10):
+                    block_valid = False
+
+                if ((system_data[5] & 0xF0) != 0x10):
+                    block_valid = False
+
+                if (block_valid):
+                    disk_type = system_data[5] & 0x0F
+                    drive_type = drive["drive_type"]
+
+        if (not block_valid):
+            raise SC64Exception("Provided 64DD disk file is not valid")
+
+        disk_zone_bad_tracks = []
+
+        for pzone in range(16):
+            tracks = []
+            start = 0 if pzone == 0 else system_data[0x07 + pzone]
+            stop = system_data[0x07 + pzone + 1]
+            for offset in range(start, stop):
+                tracks.append(system_data[0x20 + offset])
+            if (drive_type == "development"):
+                tracks.append(DISK_PZONES[pzone][2] - 2)
+                tracks.append(DISK_PZONES[pzone][2] - 1)
+            disk_zone_bad_tracks.append(tracks)
+
+        zones = []
+
+        for vzone in range(16):
+            zones.append(DISK_PZONES[DISK_VZONE_TO_PZONE[disk_type][vzone]])
+
+        thb_table = [0xFFFFFFFF] * (DISK_TRACKS * DISK_HEADS * DISK_BLOCKS_PER_TRACK)
+
+        starting_block = 0
+        disk_file_offset = 0
+        lba = 0
+
+        for (vzone, head, tracks, sector_size, track, pzone) in zones:
+            processed_tracks = 0
+            for zone_track in range(tracks):
+                current_zone_track = ((tracks - 1) - zone_track) if head else zone_track
+                if (current_zone_track in disk_zone_bad_tracks[pzone]):
+                    track += (-1) if head else 1 
+                    continue
+
+                if (processed_tracks >= (tracks - 12)):
+                    break
+
+                for block in range(2):
+                    if (not (drive_type == "retail" and lba == 12)):
+                        thb_table_entry = (track << 2) | (head << 1) | (starting_block ^ block)
+                        thb_table[thb_table_entry] = disk_file_offset
+                    disk_file_offset += sector_size * 85
+                    lba += 1
+
+                track += (-1) if head else 1
+                starting_block ^= 1
+                processed_tracks += 1
+
+        return (drive_type, thb_table)
+
+
+    def set_dd_disk_info(self, file: str = None):
+        if (file):
+            with open(file, "rb+") as handle:
+                (drive_type, thb_table) = self.__dd_process_disk(handle)
+                thb_table_offset = self.__query_config(self.__CFG_ID_DD_THB_TABLE_OFFSET)            
+                data = bytearray()
+                for value in thb_table:
+                    data += struct.pack(">I", value)
+                self.__write_cmd("W", thb_table_offset, len(data))
+                self.__write(data)
+                self.__read_cmd_status("W")
+                self.set_dd_drive_type(drive_type)
+        else:
+            raise SC64Exception("No DD disk file provided for disk info creation")
+
+
     def __debug_process_fsd_set_sector(self, data: bytes) -> None:
         sector = int.from_bytes(data[0:4], byteorder='big')
         if (self.__fsd_file):
@@ -409,112 +555,31 @@ class SC64:
             self.__fsd_file.write(data)
 
 
-    def __dd_calculate_file_offset(self, head_track: int, starting_block: int) -> int:
-        # shamelessly stolen from MAME implementation, to be rewritten
-        head = (head_track & 0x1000) >> 9
-        track = head_track & 0xFFF
-        dd_zone = 0
-        tr_off = 0
-
-        if (track >= 0x425):
-            dd_zone = 7 + head
-            tr_off = track - 0x425
-        elif (track >= 0x390):
-            dd_zone = 6 + head
-            tr_off = track - 0x390
-        elif (track >= 0x2FB):
-            dd_zone = 5 + head
-            tr_off = track - 0x2FB
-        elif (track >= 0x266):
-            dd_zone = 4 + head
-            tr_off = track - 0x266
-        elif (track >= 0x1D1):
-            dd_zone = 3 + head
-            tr_off = track - 0x1D1
-        elif (track >= 0x13C):
-            dd_zone = 2 + head
-            tr_off = track - 0x13C
-        elif (track >= 0x9E):
-            dd_zone = 1 + head
-            tr_off = track - 0x9E
-        else:
-            dd_zone = 0 + head
-            tr_off = track
-
-        ddStartOffset = [
-            0x0000000,
-            0x05F15E0,
-            0x0B79D00,
-            0x10801A0,
-            0x1523720,
-            0x1963D80,
-            0x1D414C0,
-            0x20BBCE0,
-            0x23196E0,
-            0x28A1E00,
-            0x2DF5DC0,
-            0x3299340,
-            0x36D99A0,
-            0x3AB70E0,
-            0x3E31900,
-            0x4149200
-        ]
-
-        ddZoneSecSize = [
-            232,
-            216,
-            208,
-            192,
-            176,
-            160,
-            144,
-            128,
-            216,
-            208,
-            192,
-            176,
-            160,
-            144,
-            128,
-            112
-        ]
-
-        offset = ddStartOffset[dd_zone] + tr_off * ddZoneSecSize[dd_zone] * self.__DD_USER_SECTORS_IN_BLOCK * 2
-        offset += starting_block * self.__DD_USER_SECTORS_IN_BLOCK * ddZoneSecSize[dd_zone]
-        return offset
-
-
     def __debug_process_dd_block(self, data: bytes) -> None:
-        transfer_mode = int.from_bytes(data[0:4], byteorder='little')
+        transfer_mode = int.from_bytes(data[0:4], byteorder='big')
-        head_track = int.from_bytes(data[4:6], byteorder='little')
+        sdram_offset = int.from_bytes(data[4:8], byteorder='big')
-        starting_block = int.from_bytes(data[6:7], byteorder='little')
+        disk_file_offset = int.from_bytes(data[8:12], byteorder='big')
-        sector_size = int.from_bytes(data[7:8], byteorder='little')
+        block_length = int.from_bytes(data[12:16], byteorder='big')
-
-        block_data = None
 
         if (self.__disk_file):
-            file_offset = self.__dd_calculate_file_offset(head_track, starting_block)
+            self.__disk_file.seek(disk_file_offset)
-            self.__disk_file.seek(file_offset)
             if (transfer_mode):
-                block_data = self.__disk_file.read(sector_size * self.__DD_USER_SECTORS_IN_BLOCK)
+                self.__write_cmd("W", sdram_offset, block_length)
-                self.__debug_write(self.__DEBUG_ID_DD_BLOCK, block_data)
+                self.__write(self.__disk_file.read(block_length))
+                self.__read_cmd_status("W")
             else:
-                block_data = self.__read_long(sector_size * self.__DD_USER_SECTORS_IN_BLOCK)
+                self.__write_cmd("R", sdram_offset, block_length)
-                self.__disk_file.write(block_data)
+                self.__disk_file.write(self.__read_long(block_length))
-            print(f"{'R'if transfer_mode else 'W'} - H: {1 if (head_track & 0x2000) else 0} | T: {head_track & 0x1FFF} | S: {starting_block} = [{hex(int.from_bytes(block_data[0:4], byteorder='big'))}]")
+                self.__read_cmd_status("R")
-        else:
+
-            if (transfer_mode):
+        self.__change_config(self.__CFG_ID_DD_SETTING, self.__DD_SETTING_SET_BLOCK_READY)
-                self.__debug_write(self.__DEBUG_ID_DD_BLOCK, bytes(4))
 
 
-    def debug_loop(self, file: str = None, disk_file: str = None) -> None:
+    def debug_loop(self, fsd_file: str = None, disk_file: str = None) -> None:
         print("\r\n\033[34m --- Debug server started --- \033[0m\r\n")
 
-        self.__serial.timeout = 0.01
+        if (fsd_file):
-        self.__serial.write_timeout = 1
+            self.__fsd_file = open(fsd_file, "rb+")
-
-        if (file):
-            self.__fsd_file = open(file, "rb+")
 
         if (disk_file):
             self.__disk_file = open(disk_file, "rb+")
@@ -537,10 +602,13 @@ class SC64:
             header = self.__read_long(4)
             id = int(header[0])
             length = int.from_bytes(header[1:4], byteorder="big")
-
-            if (length > 0):
             data = self.__read_long(length)
+            self.__read_long(self.__align(length, 4) - length)
+            end_indicator = self.__read_long(4)
 
+            if (end_indicator != b"CMPH"):
+                print(f"\033[35mGot unknown end indicator: {end_indicator.decode(encoding='ascii', errors='backslashreplace')}\033[0m", file=sys.stderr)
+            else:
                 if (id == self.__DEBUG_ID_TEXT):
                     print(data.decode(encoding="ascii", errors="backslashreplace"), end="")
                 elif (id == self.__DEBUG_ID_FSD_READ):
@@ -554,11 +622,6 @@ class SC64:
                 else:
                     print(f"\033[35mGot unknown id: {id}, length: {length}\033[0m", file=sys.stderr)
 
-            self.__read_long(self.__align(length, 4) - length)
-            end_indicator = self.__read_long(4)
-            if (end_indicator != b"CMPH"):
-                print(f"\033[35mGot unknown end indicator: {end_indicator.decode(encoding='ascii', errors='backslashreplace')}\033[0m", file=sys.stderr)
-
 
 
 class SC64ProgressBar:
@@ -633,7 +696,6 @@ if __name__ == "__main__":
     parser.add_argument("-q", default=None, action="store_true", required=False, help="start debug server")
     parser.add_argument("-f", metavar="sd_path", default=None, required=False, help="path to disk or file for fake SD card emulation")
     parser.add_argument("-k", metavar="disk_path", default=None, required=False, help="path to 64DD disk file")
-    parser.add_argument("-x", default=False, action="store_true", required=False, help="set 64DD drive type to development")
     parser.add_argument("rom", metavar="rom_path", default=None, help="path to ROM file", nargs="?")
 
     if (len(sys.argv) <= 1):
@@ -659,7 +721,6 @@ if __name__ == "__main__":
         debug_server = args.q
         sd_file = args.f
         disk_file = args.k
-        development_64dd = args.x
 
         firmware_backup_file = "sc64firmware.bin.bak"
 
@@ -697,14 +758,6 @@ if __name__ == "__main__":
                     print(f"Setting 64DD emulation to [{'Enabled' if dd_enable else 'Disabled'}]")
                 sc64.set_dd_enable(dd_enable)
 
-                if (development_64dd):
-                    print(f"Setting 64DD drive type to [Development]")
-                sc64.set_dd_drive_type("development" if development_64dd else "retail")
-
-                if (disk_file):
-                    print(f"Setting 64DD disk state to [Inserted]")
-                sc64.set_dd_disk_state("inserted" if disk_file else "ejected")
-
             if (rom_file):
                 if (is_read):            
                     if (rom_length > 0):
@@ -727,13 +780,24 @@ if __name__ == "__main__":
                     sc64.upload_save(save_file)
 
             if (debug_server):
+                if (sd_file):
+                    print(f"Using fake SD emulation file [{sd_file}]")
+                if (disk_file):
+                    print(f"Using 64DD disk image file [{disk_file}]")
+                    sc64.set_dd_disk_info(disk_file)
+                if (disk_file):
+                    print(f"Setting 64DD disk state to [Inserted]")
+                sc64.set_dd_disk_state("changed" if disk_file else "ejected")
                 sc64.debug_loop(sd_file, disk_file)
 
     except SC64Exception as e:
         print(f"Error: {e}")
         parser.exit(1)
     except KeyboardInterrupt:
-        if (disk_file):
+        pass
-            sc64.set_dd_disk_state("ejected")
     finally:
+        sc64.reset_link()
+        if (disk_file):
+            print(f"Setting 64DD disk state to [Ejected]")
+            sc64.set_dd_disk_state("ejected")
         sys.stdout.write("\033[0m")

sw/riscv/src/cfg.c

@@ -33,6 +33,7 @@ enum cfg_id {
     CFG_ID_FLASH_PROGRAM,
     CFG_ID_RECONFIGURE,
     CFG_ID_DD_SETTING,
+    CFG_ID_DD_THB_TABLE_OFFSET,
 };
 
 enum save_type {
@@ -53,11 +54,12 @@ enum boot_mode {
 };
 
 enum dd_setting {
-    DD_SETTING_DISK_EJECTED,
+    DD_SETTING_DISK_EJECTED = 0,
-    DD_SETTING_DISK_INSERTED,
+    DD_SETTING_DISK_INSERTED = 1,
-    DD_SETTING_DISK_CHANGED,
+    DD_SETTING_DISK_CHANGED = 2,
-    DD_SETTING_DRIVE_RETAIL,
+    DD_SETTING_DRIVE_RETAIL = 3,
-    DD_SETTING_DRIVE_DEVELOPMENT,
+    DD_SETTING_DRIVE_DEVELOPMENT = 4,
+    DD_SETTING_SET_BLOCK_READY = 5,
 };
 
 
@@ -139,6 +141,9 @@ static void set_dd_setting (enum dd_setting setting) {
         case DD_SETTING_DRIVE_DEVELOPMENT:
             dd_set_drive_type_development(true);
             break;
+        case DD_SETTING_SET_BLOCK_READY:
+            dd_set_block_ready(true);
+            break;
     }
 }
 
@@ -238,6 +243,9 @@ void cfg_query (uint32_t *args) {
         case CFG_ID_RECONFIGURE:
             args[1] = CFG->RECONFIGURE;
             break;
+        case CFG_ID_DD_THB_TABLE_OFFSET:
+            args[1] = dd_get_thb_table_offset();
+            break;
     }
 }
 

sw/riscv/src/dd.c

@@ -3,13 +3,22 @@
 #include "usb.h"
 
 
+#define DD_USER_SECTORS_PER_BLOCK   (85)
+
+#define DD_BUFFERS_OFFSET           (SDRAM_BASE + 0x03BD0000UL)
+#define DD_THB_TABLE_OFFSET         (DD_BUFFERS_OFFSET + 0x0000)
+#define DD_USB_BUFFER_OFFSET        (DD_BUFFERS_OFFSET + 0x5000)
+#define DD_BLOCK_BUFFER_OFFSET      (DD_BUFFERS_OFFSET + 0x6000)
+
+#define USB_DEBUG_ID_DD_BLOCK       (0xF5)
+
 #define DD_DRIVE_ID_RETAIL          (0x0003)
 #define DD_DRIVE_ID_DEVELOPMENT     (0x0004)
 #define DD_VERSION_RETAIL           (0x0114)
-#define DD_POWER_UP_DELAY_TICKS     (320000000UL)   // 3.2 s
+
+#define DD_POWER_UP_DELAY_TICKS     (200000000UL)   // 2 s
 #define DD_TRACK_SEEK_TIME_TICKS    (10000)         // 0.1 ms
-#define DEFAULT_DD_BUFFER_OFFSET    (0x03BD8000UL)
+
-#define USB_DEBUG_ID_DD_BLOCK       (0xF5)
 
 typedef enum {
     DD_CMD_SEEK_READ                = 0x01,
@@ -55,57 +64,66 @@ struct process {
     uint8_t current_sector;
     bool is_dev_disk;
     rtc_time_t time;
-    io32_t *buffer;
+    io32_t *thb_table;
+    io32_t *usb_buffer;
+    io32_t *block_buffer;
+    bool block_ready;
 };
 
 static struct process p;
 
-enum {
-    READ_PHASE_IDLE,
-    READ_PHASE_STARTED,
-    READ_PHASE_WAIT,
-} read_phase = READ_PHASE_IDLE;
 
-static void dd_block_read (void) {
+static uint16_t dd_track_head_block (void) {
-    read_phase = READ_PHASE_STARTED;
+    uint32_t head_track = DD->HEAD_TRACK;
+    uint16_t track = ((head_track & DD_TRACK_MASK) << 2);
+    uint16_t head = (((head_track & DD_HEAD_MASK) ? 1 : 0) << 1);
+    uint16_t block = (p.starting_block ? 1 : 0);
 
-    io32_t *dst = p.buffer;
+    return (track | head | block);
+}
+
+static bool dd_block_valid (void) {
+    return (p.thb_table[dd_track_head_block()] != 0xFFFFFFFF);
+}
+
+static bool dd_block_request (void) {
+    if (!usb_debug_tx_ready()) {
+        return false;
+    }
+
+    if (!(DD->SCR & DD_SCR_DISK_INSERTED)) {
+        return true;
+    }
 
     const char *dma = "DMA@";
     const char *cmp = "CMPH";
 
+    io32_t offset = p.thb_table[dd_track_head_block()];
+    uint32_t length = ((DD->SECTOR_SIZE + 1) * DD_USER_SECTORS_PER_BLOCK);
+
+    io32_t *dst = p.usb_buffer;
+
     *dst++ = *((uint32_t *) (dma));
-    *dst++ = (((2 * 4) << 24) | USB_DEBUG_ID_DD_BLOCK);
+    *dst++ = swap32((USB_DEBUG_ID_DD_BLOCK << 24) | 16);
-    *dst++ = p.transfer_mode;
+    *dst++ = swap32(p.transfer_mode);
-    *dst++ = (((DD->SECTOR_SIZE + 1) << 24) | (p.starting_block << 16) | DD->HEAD_TRACK);
+    *dst++ = swap32((uint32_t) (p.block_buffer));
+    *dst++ = (offset);
+    *dst++ = swap32(length);
     *dst++ = *((uint32_t *) (cmp));
 
-    usb_debug_tx_data((uint32_t) (p.buffer), (5 * 4));
+    usb_debug_tx_data((uint32_t) (p.usb_buffer), 28);
-}
 
-static bool dd_block_read_done (void) {
+    p.block_ready = false;
-    uint32_t type;
-    size_t length;
-
-    if (read_phase == READ_PHASE_STARTED) {
-        if (usb_debug_rx_ready(&type, &length)) {
-            usb_debug_rx_data((uint32_t) (p.buffer), length);
-            read_phase = READ_PHASE_WAIT;
-        }
-        return false;
-    } else if (read_phase == READ_PHASE_WAIT) {
-        if (!usb_debug_rx_busy()) {
-            read_phase = READ_PHASE_IDLE;
-            return true;
-        }
-        return false;
-    }
 
     return true;
 }
 
+static bool dd_block_ready (void) {
+    return p.block_ready || (!(DD->SCR & DD_SCR_DISK_INSERTED));
+}
+
 static void dd_sector_read (void) {
-    io32_t *src = p.buffer;
+    io32_t *src = p.block_buffer;
     io32_t *dst = DD->SECTOR_BUFFER;
 
     uint8_t sector_size = ((DD->SECTOR_SIZE + 1) / sizeof(io32_t));
@@ -119,7 +137,7 @@ static void dd_sector_read (void) {
 
 static void dd_sector_write (void) {
     io32_t *src = DD->SECTOR_BUFFER;
-    io32_t *dst = p.buffer + 4;
+    io32_t *dst = p.block_buffer;
 
     uint8_t sector_size = ((DD->SECTOR_SIZE + 1) / sizeof(io32_t));
 
@@ -130,27 +148,6 @@ static void dd_sector_write (void) {
     }
 }
 
-static void dd_block_write (void) {
-    io32_t *dst = p.buffer;
-
-    const char *dma = "DMA@";
-    const char *cmp = "CMPH";
-
-    uint32_t block_length = ((DD->SECTOR_SIZE + 1) * 85);
-
-    *dst++ = *((uint32_t *) (dma));
-    *dst++ = (((2 * 4) << 24) | USB_DEBUG_ID_DD_BLOCK);
-    *dst++ = p.transfer_mode;
-    *dst++ = (((DD->SECTOR_SIZE + 1) << 24) | (p.starting_block << 16) | DD->HEAD_TRACK);
-    dst += block_length / 4;
-    *dst++ = *((uint32_t *) (cmp));
-    usb_debug_tx_data((uint32_t) (p.buffer), ((5 * 4) + block_length));
-}
-
-static bool dd_block_write_done (void) {
-    return usb_debug_tx_ready();
-}
-
 
 void dd_set_disk_state (disk_state_t disk_state) {
     uint32_t scr = (DD->SCR & (~(DD_SCR_DISK_CHANGED | DD_SCR_DISK_INSERTED)));
@@ -179,6 +176,14 @@ void dd_set_drive_type_development (bool value) {
     }
 }
 
+void dd_set_block_ready (bool value) {
+    p.block_ready = value;
+}
+
+uint32_t dd_get_thb_table_offset (void) {
+    return (((uint32_t) (p.thb_table)) & 0x0FFFFFFF);
+}
+
 
 void dd_init (void) {
     DD->SCR = 0;
@@ -190,7 +195,9 @@ void dd_init (void) {
     p.deffered_cmd_ready = false;
     p.bm_running = false;
     p.is_dev_disk = false;
-    p.buffer = (io32_t *) (SDRAM_BASE + DEFAULT_DD_BUFFER_OFFSET);
+    p.thb_table = (io32_t *) (DD_THB_TABLE_OFFSET);
+    p.usb_buffer = (io32_t *) (DD_USB_BUFFER_OFFSET);
+    p.block_buffer = (io32_t *) (DD_BLOCK_BUFFER_OFFSET);
 }
 
 
@@ -268,16 +275,16 @@ void process_dd (void) {
                     break;
 
                 case DD_CMD_GET_RTC_YEAR_MONTH:
-                    DD->DATA = (p.time.year << 8) | p.time.month;
+                    DD->DATA = ((p.time.year << 8) | p.time.month);
                     break;
 
                 case DD_CMD_GET_RTC_DAY_HOUR:
-                    DD->DATA = (p.time.day << 8) | p.time.hour;
+                    DD->DATA = ((p.time.day << 8) | p.time.hour);
                     break;
 
                 case DD_CMD_GET_RTC_MINUTE_SECOND:
                     p.time = *rtc_get_time();
-                    DD->DATA = (p.time.minute << 8) | p.time.second;
+                    DD->DATA = ((p.time.minute << 8) | p.time.second);
                     break;
 
                 case DD_CMD_READ_PROGRAM_VERSION:
@@ -349,28 +356,31 @@ void process_dd (void) {
             p.bm_running = true;
             p.current_sector = 0;
             if (p.transfer_mode) {
-                if (!p.is_dev_disk && ((DD->HEAD_TRACK & DD_HEAD_TRACK_MASK) == 6) && !p.starting_block) {
+                if (dd_block_valid()) {
-                    p.state = STATE_SECTOR_READ;
-                    DD->SCR |= DD_SCR_BM_MICRO_ERROR;
-                } else {
                     p.state = STATE_BLOCK_READ;
                     DD->SCR |= DD_SCR_BM_TRANSFER_DATA;
+                } else {
+                    p.state = STATE_SECTOR_READ;
+                    DD->SCR |= DD_SCR_BM_MICRO_ERROR;
                 }
             } else {
                 p.state = STATE_IDLE;
+                if (dd_block_valid()) {
                     DD->SCR |= DD_SCR_BM_TRANSFER_DATA | DD_SCR_BM_READY;
+                } else {
+                    DD->SCR |= DD_SCR_BM_MICRO_ERROR | DD_SCR_BM_READY;
+                }
             }
             break;
 
         case STATE_BLOCK_READ:
-            if (dd_block_read_done()) {
+            if (dd_block_request()) {
-                dd_block_read();
                 p.state = STATE_BLOCK_READ_WAIT;
             }
             break;
 
         case STATE_BLOCK_READ_WAIT:
-            if (dd_block_read_done()) {
+            if (dd_block_ready()) {
                 p.state = STATE_SECTOR_READ;
             }
             break;
@@ -394,14 +404,13 @@ void process_dd (void) {
             break;
 
         case STATE_BLOCK_WRITE:
-            if (dd_block_write_done()) {
+            if (dd_block_request()) {
-                dd_block_write();
                 p.state = STATE_BLOCK_WRITE_WAIT;
             }
             break;
 
         case STATE_BLOCK_WRITE_WAIT:
-            if (dd_block_write_done()) {
+            if (dd_block_ready()) {
                 p.state = STATE_NEXT_BLOCK;
             }
             break;
@@ -417,7 +426,7 @@ void process_dd (void) {
                     p.state = STATE_STOP;
                 } else {
                     p.state = STATE_IDLE;
-                    DD->SCR &= ~(DD_SCR_BM_TRANSFER_DATA);
+                    DD->SCR &= ~(DD_SCR_BM_TRANSFER_C2 | DD_SCR_BM_TRANSFER_DATA);
                     DD->SCR |= DD_SCR_BM_READY;
                 }
             }

sw/riscv/src/dd.h

@@ -14,6 +14,8 @@ typedef enum {
 
 void dd_set_disk_state (disk_state_t disk_state);
 void dd_set_drive_type_development (bool value);
+void dd_set_block_ready (bool value);
+uint32_t dd_get_thb_table_offset (void);
 void dd_init (void);
 void process_dd (void);
 

sw/riscv/src/sys.h

@@ -8,6 +8,11 @@
 #include <stdbool.h>
 
 
+#define swap32(x)                   ((((x) & 0xFF000000UL) >> 24) | \
+                                    (((x) & 0x00FF0000UL) >> 8) | \
+                                    (((x) & 0x0000FF00UL) << 8) | \
+                                    (((x) & 0x000000FFUL) << 24))
+
 typedef volatile uint8_t            io8_t;
 typedef volatile uint16_t           io16_t;
 typedef volatile uint32_t           io32_t;
@@ -235,7 +240,8 @@ typedef volatile struct dd_regs {
 #define DD_SCR_SEEK_TIMER_RESET     (1 << 20)
 
 #define DD_TRACK_MASK               (0x0FFF)
-#define DD_HEAD_TRACK_MASK          (0x1FFF)
+#define DD_HEAD_MASK                (0x1000)
+#define DD_HEAD_TRACK_MASK          (DD_HEAD_MASK | DD_TRACK_MASK)
 #define DD_HEAD_TRACK_INDEX_LOCK    (1 << 13)