IDE.cpp

//
//  IDE.c
//  Omega2 - Gale IDE Emulation (Refactored)
//
//  Key improvements:
//  - LBA mode support (0xC8/0xCA commands)
//  - Multi-sector read/write
//  - Better state tracking
//  - Fixed endianness and block addressing
//
//  Created by Matt Parsons on 10/01/2024
//  Refactored: 2025
//


#include "IDE.hpp"
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include "m68k.h"

// ============================================================================
// IDE REGISTER DEFINITIONS
// ============================================================================

#define IDE_DATA     0x00   // Data port (16-bit)
#define IDE_ERROR    0x01   // Error flags (read) / Features (write)
#define IDE_NSECTOR  0x02   // Sector count
#define IDE_SECTOR   0x03   // Sector number (CHS) / LBA0-7
#define IDE_LCYL     0x04   // Cylinder low (CHS) / LBA8-15
#define IDE_HCYL     0x05   // Cylinder high (CHS) / LBA16-23
#define IDE_SELECT   0x06   // Device/Head select
#define IDE_STATUS   0x07   // Status (read) / Command (write)

// ============================================================================
// STATUS BIT FLAGS
// ============================================================================

#define IDE_STATUS_ERR  0x01  // Error occurred
#define IDE_STATUS_IDX  0x02  // Index pulse
#define IDE_STATUS_DRQ  0x08  // Data request (ready for I/O)
#define IDE_STATUS_DSC  0x10  // Drive seek complete
#define IDE_STATUS_DRDY 0x40  // Drive ready
#define IDE_STATUS_BSY  0x80  // Controller busy

// ============================================================================
// IDE COMMANDS
// ============================================================================

#define IDE_CMD_RECALIBRATE        0x10
#define IDE_CMD_READ_SECTORS_CHS   0x20
#define IDE_CMD_WRITE_SECTORS_CHS  0x30
#define IDE_CMD_READ_SECTORS_LBA   0xC8
#define IDE_CMD_WRITE_SECTORS_LBA  0xCA
#define IDE_CMD_IDENTIFY_DEVICE    0xEC
#define IDE_CMD_INITIALIZE_PARAMS  0x91
#define IDE_CMD_SET_MULTIPLE_MODE  0xC6

// ============================================================================
// OPERATION STATES
// ============================================================================

typedef enum {
    IDE_STATE_IDLE = 0,
    IDE_STATE_READING = 1,
    IDE_STATE_WRITING = 2,
    IDE_STATE_DATA_READY = 3
} IDEState_t;

// ============================================================================
// IDE DRIVE STRUCTURE
// ============================================================================

typedef struct {
    // IDE Registers (standard ATA interface)
    uint16_t sectorCount;      // How many sectors to read/write
    uint16_t sector;           // Sector number (CHS mode) or LBA[7:0]
    uint16_t cylinderLow;      // Cylinder low (CHS) or LBA[15:8]
    uint16_t cylinderHigh;     // Cylinder high (CHS) or LBA[23:16]
    uint16_t head;             // Device/Head (bit 6=LBA mode, bits 3:0=head, bit 4=device)
    uint16_t status;           // Status register
    
    // Identify data (word 0-255)
    uint16_t ID[256];
    
    // Data pointer and state
    uint8_t* dataPtr;          // Points into mappedFile or (uint8_t*)ID
    uint32_t dataIndex;        // Position within current 512-byte sector
    uint32_t sectorsRemaining; // How many sectors left to read/write
    uint8_t  state;            // Current operation state
    
    // Block addressing
    uint64_t currentBlock;     // Current block address (LBA, in sectors)
    uint64_t fileSize;         // Total file size in bytes
    
    // Host filesystem
    char* path;
    int fd;
    uint8_t* mappedFile;       // mmap'd hard file pointer

} IDEDrive_t;

// ============================================================================
// GAYLE CHIPSET STRUCTURE (A600 IDE Controller)
// ============================================================================

typedef struct {
    uint8_t status;    // Gayle status (bit 7 = IDE interrupt)
    uint8_t intreq;    // Interrupt request
    uint8_t intena;    // Interrupt enable
    uint8_t config;    // Configuration
} Gayle_t;

// ============================================================================
// GLOBAL STATE
// ============================================================================

static IDEDrive_t* IDEDrive = NULL;
static Gayle_t gayle = {0};
static CIA* cia = NULL;

// ============================================================================
// HELPER FUNCTIONS
// ============================================================================

/// Trigger IDE interrupt to CPU
void IDEInterrupt(void) {
    if (gayle.intena & 0x80) {
        //m68k_set_irq(2);  // Level 2 interrupt
        cia->interrupt(true);
    }
    gayle.status |= 0x80;   // Set IDE interrupt bit
    gayle.intreq |= 0x80;   // Set IDE interrupt request
}

/// Convert CHS address to LBA block number
/// @return Block address in sectors
uint64_t CHS_to_LBA(uint16_t cylinder, uint8_t head, uint8_t sector) {
    uint64_t block = 0;

    // CHS → LBA formula: LBA = (C * H + H) * S + (S - 1)
    // Where H = heads per cylinder, S = sectors per track
    block = cylinder * IDEDrive->ID[3];        // C * heads
    block += (head & 0x0F);                     // + H
    block *= IDEDrive->ID[6];                   // * sectors_per_track
    block += (sector - 1);                      // + (S - 1)

    return block;
}

/// Update task-file registers from currentBlock so the scsi.device can
/// read back the correct position after a transfer completes.
void UpdateTaskFileRegisters() {
    if (IDEDrive->head & 0x40) {
        // LBA mode
        IDEDrive->sector      = IDEDrive->currentBlock & 0xFF;
        IDEDrive->cylinderLow = (IDEDrive->currentBlock >> 8) & 0xFF;
        IDEDrive->cylinderHigh= (IDEDrive->currentBlock >> 16) & 0xFF;
        IDEDrive->head = (IDEDrive->head & 0xF0) | ((IDEDrive->currentBlock >> 24) & 0x0F);
    } else {
        // CHS mode — convert LBA back to CHS
        uint16_t spt   = IDEDrive->ID[6];   // sectors per track
        uint16_t heads = IDEDrive->ID[3];    // heads
        IDEDrive->sector = (IDEDrive->currentBlock % spt) + 1;
        uint64_t tmp = IDEDrive->currentBlock / spt;
        IDEDrive->head = (IDEDrive->head & 0xF0) | (tmp % heads);
        uint16_t cylinder = (uint16_t)(tmp / heads);
        IDEDrive->cylinderLow  = cylinder & 0xFF;
        IDEDrive->cylinderHigh = (cylinder >> 8) & 0xFF;
    }
    IDEDrive->sectorCount = IDEDrive->sectorsRemaining;
}

/// Set dataPtr to point at a sector in the mmap'd file
/// Returns false if the block is out of range
bool SetSectorPtr(uint64_t block) {
    uint64_t byteOffset = block * 512;

    if (byteOffset + 512 > IDEDrive->fileSize) {
        printf("[IDE] WARNING: Access past end of drive! Block %llu\n", block);
        IDEDrive->status |= IDE_STATUS_ERR;
        return false;
    }

    IDEDrive->dataPtr = IDEDrive->mappedFile + byteOffset;
    return true;
}

// ============================================================================
// IDE READ/WRITE HANDLERS
// ============================================================================

int ReadIDE(int size, uint32_t address) {
    address &= 0xFFFFFFFC;  // Mask alignment bits
    uint16_t value = 0;
    
    // ========== GAYLE REGISTERS ==========
    
    if (address == 0xDA8000) {
        // Gayle status (bit 7 = IDE interrupt pending)
        return gayle.status & 0xBE;  // 0xBE masks out PCMCIA bits
    }
    
    if (address == 0xDA9000) {
        // Gayle interrupt request
        return gayle.intreq | 0x3C;   // OR with fixed bits
    }
    
    if (address == 0xDAA000) {
        // Gayle interrupt enable
        return gayle.intena;
    }
    
    if (address == 0xDAB000) {
        // Gayle configuration
        return gayle.config;
    }
    
    // ========== IDE DATA PORT ==========
    // A1200 Gayle: bit 13 is not decoded, so 0xDA0000 and 0xDA2000 are aliases
    if (address == 0xDA2000 || address == 0xDA0000) {
        // Data port - return words directly from mmap'd memory
        if (IDEDrive == NULL || IDEDrive->dataPtr == NULL) return 0;

        // Log first word of each new sector for data verification
        if (IDEDrive->dataIndex == 0 && IDEDrive->dataPtr != (uint8_t*)IDEDrive->ID) {
           // printf("[IDE] Sector %llu data: %02X %02X %02X %02X %02X %02X %02X %02X\n",IDEDrive->currentBlock,IDEDrive->dataPtr[0], IDEDrive->dataPtr[1],IDEDrive->dataPtr[2], IDEDrive->dataPtr[3],IDEDrive->dataPtr[4], IDEDrive->dataPtr[5],IDEDrive->dataPtr[6], IDEDrive->dataPtr[7]);
        }

        switch (size) {
            case 0:  // Byte access
                value = IDEDrive->dataPtr[IDEDrive->dataIndex++];
                break;

            case 1:  // Word access (standard for IDE)
                value = *(uint16_t*)&IDEDrive->dataPtr[IDEDrive->dataIndex];
                IDEDrive->dataIndex += 2;
                // CRITICAL: IDE sends little-endian, 68K expects big-endian
                value = (value << 8) | (value >> 8);  // Byte swap!
                break;
        }

        // Check if we've exhausted this sector
        if (IDEDrive->dataIndex >= 512) {
            IDEDrive->sectorsRemaining--;

            if (IDEDrive->sectorsRemaining > 0) {
                // Advance dataPtr to next sector in mmap'd region
                IDEDrive->currentBlock++;
                if (!SetSectorPtr(IDEDrive->currentBlock)) {
                    IDEDrive->sectorsRemaining = 0;
                    IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_ERR;
                    cia->HDLED = false;
                } else {
                    IDEDrive->dataIndex = 0;
                    IDEDrive->status |= IDE_STATUS_DRQ;
                    IDEInterrupt();  // ATA PIO: interrupt after each sector
                }
                UpdateTaskFileRegisters();

            } else {
                // All sectors transferred
                IDEDrive->status &= ~IDE_STATUS_DRQ;
                IDEDrive->status |= IDE_STATUS_DRDY;
                UpdateTaskFileRegisters();
                IDEInterrupt();  // ATA PIO: interrupt on transfer complete
                cia->HDLED = false;
            }
        }

        return value;
    }
    
    // ========== IDE REGISTERS (A600 alternate addresses) ==========
    
    if (IDEDrive == NULL) return 0;
    
    int index = (address >> 2) & 0xFF;

    switch (index) {
        case IDE_ERROR:
            return 0;  // No error info yet

        case IDE_NSECTOR:
            return IDEDrive->sectorCount;

        case IDE_SECTOR:
            return IDEDrive->sector;

        case IDE_LCYL:
            return IDEDrive->cylinderLow;

        case IDE_HCYL:
            return IDEDrive->cylinderHigh;

        case IDE_SELECT:
            return IDEDrive->head;

        case IDE_STATUS:
            if ((IDEDrive->head & 0xB0) == 0xB0) {
                return 0;  // No slave drive
            }
            return IDEDrive->status;

        default:
            return 0;
    }
}

void WriteIDE(int size, uint32_t address, uint32_t value) {
    address &= 0xFFFFFFFC;
    
    // ========== GAYLE REGISTERS ==========
    
    if (address == 0xDA8000) {
        // Gayle status write
        gayle.status = (value & 0x7F) | (gayle.status & 0x80);
        return;
    }
    
    if (address == 0xDA9000) {
        // Gayle interrupt request (write 0 to clear)
        if ((value & 0x80) == 0) {
            gayle.status &= 0x7F;
            gayle.intreq &= 0x7F;
        } else {
            gayle.intreq = value;
        }
        return;
    }
    
    if (address == 0xDAA000) {
        // Gayle interrupt enable
        gayle.intena = value;
        return;
    }
    
    if (address == 0xDAB000) {
        // Gayle configuration
        gayle.config = value;
        return;
    }
    
    // ========== IDE DATA PORT ==========
    // A1200 Gayle: bit 13 is not decoded, so 0xDA0000 and 0xDA2000 are aliases
    if (address == 0xDA2000 || address == 0xDA0000) {
        if (IDEDrive == NULL || IDEDrive->dataPtr == NULL) return;

        // Write data directly into mmap'd region
        switch (size) {
            case 0:  // Byte
                IDEDrive->dataPtr[IDEDrive->dataIndex++] = value;                                 // ALLOW WRITES AT THIS TIME - Danger
                break;

            case 1:  // Word (standard for IDE)
                value = (value << 8) | (value >> 8);  // Byte swap!
                *(uint16_t*)&IDEDrive->dataPtr[IDEDrive->dataIndex] = value;                       // ALLOW WRITES AT THIS TIME - Danger
                IDEDrive->dataIndex += 2;
                break;
        }

        // Check if sector is full
        if (IDEDrive->dataIndex >= 512) {
            IDEDrive->sectorsRemaining--;

            if (IDEDrive->sectorsRemaining > 0) {
                // Advance dataPtr to next sector
                IDEDrive->currentBlock++;
                if (!SetSectorPtr(IDEDrive->currentBlock)) {
                    IDEDrive->sectorsRemaining = 0;
                    IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_ERR;
                    cia->HDLED = false;
                } else {
                    IDEDrive->dataIndex = 0;
                    IDEInterrupt();  // ATA PIO: interrupt after each sector
                }
                UpdateTaskFileRegisters();
            } else {
                // All sectors written
                IDEDrive->status &= ~IDE_STATUS_DRQ;
                IDEDrive->status |= IDE_STATUS_DRDY;
                UpdateTaskFileRegisters();
                IDEInterrupt();  // ATA PIO: interrupt on transfer complete
                cia->HDLED = false;
            }
        }

        return;
    }
    
    // ========== IDE REGISTERS ==========
    
    if (IDEDrive == NULL) return;
    
    int index = (address >> 2) & 0xFF;
    
    switch (index) {
        // ===== ADDRESS REGISTERS =====
        
        case IDE_NSECTOR:
            IDEDrive->sectorCount = (value & 0xFF) ? (value & 0xFF) : 256;
            break;
            
        case IDE_SECTOR:
            IDEDrive->sector = value & 0xFF;
            break;
            
        case IDE_LCYL:
            IDEDrive->cylinderLow = value & 0xFF;
            break;
            
        case IDE_HCYL:
            IDEDrive->cylinderHigh = value & 0xFF;
            break;
            
        case IDE_SELECT:
            IDEDrive->head = value & 0xFF;
            break;
            
        // ===== COMMAND REGISTER =====
        
        case IDE_STATUS:
        {
            uint8_t command = value & 0xFF;
            cia->HDLED = true;
            switch (command) {
                // ===== CHS MODE COMMANDS =====
                
                case IDE_CMD_RECALIBRATE:
                    // Reset head to 0
                    IDEDrive->head &= 0xF0;
                    IDEDrive->status = IDE_STATUS_DRDY;
                    break;
                    
                case IDE_CMD_READ_SECTORS_CHS:
                {
                    if (IDEDrive->head & 0x40) {
                        // LBA mode (bit 6 set in device/head register)
                        IDEDrive->currentBlock = IDEDrive->sector |
                                                (IDEDrive->cylinderLow << 8) |
                                                (IDEDrive->cylinderHigh << 16) |
                                                ((IDEDrive->head & 0x0F) << 24);

                       // printf("[IDE] Read LBA (0x20): Block %llu (%d sectors)\n", IDEDrive->currentBlock, IDEDrive->sectorCount); // Useful Debugging output
                    } else {
                        // CHS mode
                        uint16_t cylinder = (IDEDrive->cylinderHigh << 8) | IDEDrive->cylinderLow;
                        IDEDrive->currentBlock = CHS_to_LBA(cylinder, IDEDrive->head, IDEDrive->sector);

                        //printf("[IDE] Read CHS: C=%d H=%d S=%d → Block %llu (%d sectors)\n",cylinder, IDEDrive->head & 0x0F, IDEDrive->sector,IDEDrive->currentBlock, IDEDrive->sectorCount);
                    }

                    IDEDrive->sectorsRemaining = IDEDrive->sectorCount;
                    IDEDrive->dataIndex = 0;

                    if (!SetSectorPtr(IDEDrive->currentBlock)) {
                        IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_ERR;
                        break;
                    }
                    IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_DRQ;
                    IDEInterrupt();
                    break;
                }

                case IDE_CMD_WRITE_SECTORS_CHS:
                {
                    if (IDEDrive->head & 0x40) {
                        // LBA mode (bit 6 set in device/head register)
                        IDEDrive->currentBlock = IDEDrive->sector |
                                                (IDEDrive->cylinderLow << 8) |
                                                (IDEDrive->cylinderHigh << 16) |
                                                ((IDEDrive->head & 0x0F) << 24);

                       // printf("[IDE] Write LBA (0x30): Block %llu (%d sectors)\n", IDEDrive->currentBlock, IDEDrive->sectorCount);
                    } else {
                        // CHS mode
                        uint16_t cylinder = (IDEDrive->cylinderHigh << 8) | IDEDrive->cylinderLow;
                        IDEDrive->currentBlock = CHS_to_LBA(cylinder, IDEDrive->head, IDEDrive->sector);

                       // printf("[IDE] Write CHS: C=%d H=%d S=%d → Block %llu (%d sectors)\n", cylinder, IDEDrive->head & 0x0F, IDEDrive->sector, IDEDrive->currentBlock, IDEDrive->sectorCount);
                    }

                    IDEDrive->sectorsRemaining = IDEDrive->sectorCount;
                    IDEDrive->dataIndex = 0;

                    if (!SetSectorPtr(IDEDrive->currentBlock)) {
                        IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_ERR;
                        break;
                    }
                    IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_DRQ;
                    IDEInterrupt();
                    break;
                }
                
                // ===== LBA MODE COMMANDS =====
                
                case IDE_CMD_READ_SECTORS_LBA:
                {
                    IDEDrive->currentBlock = IDEDrive->sector |
                                            (IDEDrive->cylinderLow << 8) |
                                            (IDEDrive->cylinderHigh << 16) |
                                            ((IDEDrive->head & 0x0F) << 24);

                    IDEDrive->sectorsRemaining = IDEDrive->sectorCount;
                    IDEDrive->dataIndex = 0;

                    printf("[IDE] Read LBA: Block %llu (%d sectors)\n",
                           IDEDrive->currentBlock, IDEDrive->sectorCount);

                    if (!SetSectorPtr(IDEDrive->currentBlock)) {
                        IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_ERR;
                        break;
                    }
                    IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_DRQ;
                    IDEInterrupt();
                    break;
                }

                case IDE_CMD_WRITE_SECTORS_LBA:
                {
                    IDEDrive->currentBlock = IDEDrive->sector |
                                            (IDEDrive->cylinderLow << 8) |
                                            (IDEDrive->cylinderHigh << 16) |
                                            ((IDEDrive->head & 0x0F) << 24);

                    IDEDrive->sectorsRemaining = IDEDrive->sectorCount;
                    IDEDrive->dataIndex = 0;

                    printf("[IDE] Write LBA: Block %llu (%d sectors)\n",
                           IDEDrive->currentBlock, IDEDrive->sectorCount);

                    if (!SetSectorPtr(IDEDrive->currentBlock)) {
                        IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_ERR;
                        break;
                    }
                    IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_DRQ;
                    IDEInterrupt();
                    break;
                }
                
                // ===== OTHER COMMANDS =====
                
                case IDE_CMD_IDENTIFY_DEVICE:
                {
                    // Point dataPtr directly at the ID array (512 bytes)
                    IDEDrive->dataPtr = (uint8_t*)IDEDrive->ID;
                    IDEDrive->dataIndex = 0;
                    IDEDrive->sectorsRemaining = 1;
                    IDEDrive->status = IDE_STATUS_DRDY | IDE_STATUS_DRQ;
                    IDEInterrupt();
                    break;
                }
                
                case IDE_CMD_INITIALIZE_PARAMS:
                case IDE_CMD_SET_MULTIPLE_MODE:
                    // Accept but do nothing
                    IDEDrive->status = IDE_STATUS_DRDY;
                    break;
                    
                default:
                    printf("[IDE] Unknown command: 0x%02X\n", command);
                    IDEDrive->status |= IDE_STATUS_ERR;
                    break;
            }
            break;
        }
        
        default:
            // printf("[IDE] Write to register %d (0x%X): 0x%X\n", index, address, value);
            break;
    }
}

// ============================================================================
// RIGID DISK BLOCK (RDB) PARSING
// ============================================================================

typedef struct {
    uint32_t rdb_ID;
    uint32_t rdb_SummedLongs;
    uint32_t rdb_ChkSum;
    uint32_t rdb_HostID;
    uint32_t rdb_BlockBytes;
    uint32_t rdb_Flags;
    uint32_t rdb_BadBlockList;
    uint32_t rdb_PartitionList;
    uint32_t rdb_FileSysHeaderList;
    uint32_t rdb_DriveInit;
    uint32_t rdb_Reserved1[6];
    uint32_t rdb_Cylinders;
    uint32_t rdb_Sectors;
    uint32_t rdb_Heads;
    uint32_t rdb_Interleave;
    uint32_t rdb_Park;
    uint32_t rdb_Reserved2[3];
    uint32_t rdb_WritePreComp;
    uint32_t rdb_ReducedWrite;
    uint32_t rdb_StepRate;
    uint32_t rdb_Reserved3[5];
    uint32_t rdb_RDBBlocksLo;
    uint32_t rdb_RDBBlocksHi;
    uint32_t rdb_LoCylinder;
    uint32_t rdb_HiCylinder;
    uint32_t rdb_CylBlocks;
    uint32_t rdb_AutoParkSeconds;
    uint32_t rdb_HighRDSKBlock;
    uint32_t rdb_Reserved4;
    char rdb_DiskVendor[8];
    char rdb_DiskProduct[16];
    char rdb_DiskRevision[4];
    char rdb_ControllerVendor[8];
    char rdb_ControllerProduct[16];
    char rdb_ControllerRevision[4];
    uint32_t rdb_Reserved5[10];
} RigidDiskBlock_t;

uint32_t readRDBField(uint32_t value) {
    uint8_t* t = (uint8_t*)&value;
    return (t[0] << 24) | (t[1] << 16) | (t[2] << 8) | t[3];
}

// ============================================================================
// INITIALIZATION
// ============================================================================

void InitIDE(char* path) {
    IDEDrive = (IDEDrive_t*) malloc(sizeof(IDEDrive_t));
    memset(IDEDrive, 0, sizeof(IDEDrive_t));

    IDEDrive->path = path;
    IDEDrive->fd = open(path, O_RDWR);

    if (IDEDrive->fd < 0) {
        printf("[IDE] ERROR: Cannot open %s\n", path);
        IDEDrive->status = 0;
        return;
    }

    // Get file size via stat
    struct stat st;
    if (fstat(IDEDrive->fd, &st) < 0) {
        printf("[IDE] ERROR: Cannot stat %s\n", path);
        close(IDEDrive->fd);
        IDEDrive->fd = -1;
        IDEDrive->status = 0;
        return;
    }
    IDEDrive->fileSize = st.st_size;

    // mmap the entire hard file into memory
    IDEDrive->mappedFile = (uint8_t*)mmap(NULL, IDEDrive->fileSize, PROT_READ | PROT_WRITE, MAP_SHARED, IDEDrive->fd, 0);
    if (IDEDrive->mappedFile == MAP_FAILED) {
        printf("[IDE] ERROR: Cannot mmap %s\n", path);
        close(IDEDrive->fd);
        IDEDrive->fd = -1;
        IDEDrive->mappedFile = NULL;
        IDEDrive->status = 0;
        return;
    }

    printf("[IDE] Opened and mmap'd: %s (%llu bytes)\n", path, IDEDrive->fileSize);

    // Dump first 3 blocks looking for RDB
    printf("=== RDB SEARCH ===\n");
    for (int block = 0; block < 3; block++) {
        uint32_t rdb_id;
        memcpy(&rdb_id, IDEDrive->mappedFile + (block * 512), 4);

        printf("Block %d: ID = 0x%08X", block, rdb_id);
        if (rdb_id == 0x4B534452) printf(" ✓ RDSK (little-endian)");
        else if (rdb_id == 0x52445343) printf(" ✓ RDSK (big-endian?)");
        printf("\n");
    }

    // Try to parse RDB
    RigidDiskBlock_t rdb;
    memcpy(&rdb, IDEDrive->mappedFile, sizeof(RigidDiskBlock_t));
    
    uint32_t cylinders, heads, sectors, blockBytes;
    uint64_t totalBlocks;
    
    if (rdb.rdb_ID == 0x4B534452) {  // "RDSK" = 0x52445343 (big-endian)
        printf("[IDE] Valid RDB found\n");
        blockBytes = readRDBField(rdb.rdb_BlockBytes);
        cylinders = readRDBField(rdb.rdb_Cylinders);
        heads = readRDBField(rdb.rdb_Heads);
        sectors = readRDBField(rdb.rdb_Sectors);
        totalBlocks = IDEDrive->fileSize / blockBytes;
    } else {
        printf("[IDE] No RDB found, using defaults\n");
        blockBytes = 512;
        sectors = 63;
        heads = 4;
        totalBlocks = IDEDrive->fileSize / blockBytes;
        cylinders = (uint32_t) (totalBlocks / (heads * sectors));
    }
    
    printf("[IDE] Geometry: C=%u H=%u S=%u\n", cylinders, heads, sectors);

    // Initialize Identify Device data
    IDEDrive->ID[0] = 0x807F;              // Fixed drive, non-removable
    IDEDrive->ID[1] = cylinders;            // Cylinders
    IDEDrive->ID[3] = heads;                // Heads
    IDEDrive->ID[5] = blockBytes;           // Bytes per sector (usually 512)
    IDEDrive->ID[6] = sectors;              // Sectors per track

    // Serial number
    const char *serial = "OmegaIDE0001        ";
    for (int i = 0; i < 20; i += 2) {
        IDEDrive->ID[10 + i/2] = (serial[i] << 8) | serial[i+1];
    }

    IDEDrive->ID[20] = 1;                   // Buffer type
    IDEDrive->ID[21] = 1;                   // Buffer size (512-byte increments)

    // Firmware version
    for (int i = 0; i < 8; i += 2) {
        IDEDrive->ID[23 + i/2] = ('F' << 8) | ('1' + i);
    }

    // Model string
    const char *model = "OmegaHARDDRIVE                          ";
    for (int i = 0; i < 40; i += 2) {
        IDEDrive->ID[27 + i/2] = (model[i] << 8) | model[i+1];
    }

    IDEDrive->ID[49] = 0x200;               // Capabilities: LBA supported!
    IDEDrive->ID[54] = cylinders;           // Current cylinders
    IDEDrive->ID[55] = heads;               // Current heads
    IDEDrive->ID[56] = sectors;             // Current sectors

    uint32_t* sectorCount = (uint32_t*) &IDEDrive->ID[57];
    *sectorCount = (uint32_t)totalBlocks;   // CHS capacity in sectors

    uint32_t* lbaSectors = (uint32_t*) &IDEDrive->ID[60];
    *lbaSectors = (uint32_t)totalBlocks;    // LBA total user-addressable sectors
    
    // Initialize drive state
    IDEDrive->status = IDE_STATUS_DRDY;
    IDEDrive->state = IDE_STATE_IDLE;
    IDEDrive->dataPtr = NULL;
    IDEDrive->dataIndex = 0;
    IDEDrive->sectorsRemaining = 0;
    
    printf("[IDE] Initialization complete\n");
}

void CloseIDE(void) {
    if (IDEDrive) {
        if (IDEDrive->mappedFile && IDEDrive->mappedFile != MAP_FAILED) {
            msync(IDEDrive->mappedFile, IDEDrive->fileSize, MS_SYNC);
            munmap(IDEDrive->mappedFile, IDEDrive->fileSize);
        }
        if (IDEDrive->fd >= 0) {
            close(IDEDrive->fd);
        }
        free(IDEDrive);
        IDEDrive = NULL;
    }
}

void IDESetCIA(CIA* cia_ptr){
    cia = cia_ptr;
}

// ============================================================================
// GAYLE CHECK (A600 detection sequence)
// ============================================================================

int gayleCheckCounter = 0;

// A600 Gayle ID sequence (shift register simulation)
int gayleIdArray[] = {128, 128, 0, 128, 0, 0, 0, 0};

int GayleCheck(int value) {
    if (IDEDrive == NULL) {
        return 0;
    }
    
    if (value == 0) {
        printf("Gayle: Reset ID\n");
        gayleCheckCounter = 0;
        return 0;
    }
    printf("Gayle: Check Version\n");
    uint8_t c = gayleIdArray[gayleCheckCounter];
    gayleCheckCounter += 1;
    return c;
}

// ============================================================================
// DIAGNOSTICS
// ============================================================================

int GetIDEIndex(void) {
    return IDEDrive ? (int)IDEDrive->currentBlock : 0;
}

void IDECycle(void) {
    // Reserved for future async operations
}





























/*
 0xda0000    // Data
 0xda0006    // Error | Feature
 0xda000a    // Sector Count
 0xda000e    // Sector Number
 0xda0012    // Cylinder Low
 0xda0016    // Cylinder High
 0xda001a    // Device / Head
 0xda001e    // Status | Command
 0xda101a    // Control
 0xda8000    // Gayle Status
 0xda9000    // Gayle INTREQ
 0xdaa000    // Gayle INTENA
 0xdab000    // Gayle Config
 
 The "Gayle-check" is:

 write 0x00 to 0xde1000
 read byte 0xde1000 with bit 7 set
 read byte 0xde1000 with bit 7 set
 read byte 0xde1000 with bit 7 cleard
 read byte 0xde1000 with bit 7 set

 Then the Kickstart accepts Gayle and accesses HD. Works even with 37.300 (A600).
 
 
 */