git.sesse.net Git - pistorm/blob - platforms/amiga/piscsi/piscsi.c

   1 #include <stdio.h>
   2 #include <stdint.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <fcntl.h>
   6 #include <unistd.h>
   7 #include <endian.h>
   8 #include "../hunk-reloc.h"
   9 #include "piscsi.h"
  10 #include "piscsi-enums.h"
  11 #include "../../../config_file/config_file.h"
  12 #include "../../../gpio/ps_protocol.h"
  13
  14 #define BE(val) be32toh(val)
  15 #define BE16(val) be16toh(val)
  16
  17 // Comment these lines to restore debug output:
  18 #define DEBUG(...)
  19 //#define DEBUG printf
  20 #define DEBUG_TRIVIAL(...)
  21 //#define DEBUG_TRIVIAL printf
  22 #define stop_cpu_emulation(...)
  23
  24 #ifdef FAKESTORM
  25 #define lseek64 lseek
  26 #endif
  27
  28 extern struct emulator_config *cfg;
  29 extern void stop_cpu_emulation(uint8_t disasm_cur);
  30
  31 struct piscsi_dev devs[8];
  32 struct piscsi_fs filesystems[NUM_FILESYSTEMS];
  33
  34 uint8_t piscsi_num_fs = 0;
  35
  36 uint8_t piscsi_cur_drive = 0;
  37 uint32_t piscsi_u32[4];
  38 uint32_t piscsi_dbg[8];
  39 uint32_t piscsi_rom_size = 0;
  40 uint8_t *piscsi_rom_ptr;
  41
  42 uint32_t rom_partitions[128];
  43 uint32_t rom_partition_prio[128];
  44 uint32_t rom_partition_dostype[128];
  45 uint32_t rom_cur_partition = 0, rom_cur_fs = 0;
  46
  47
  48
  49 extern unsigned char ac_piscsi_rom[];
  50
  51 static const char *op_type_names[4] = {
  52     "BYTE",
  53     "WORD",
  54     "LONGWORD",
  55     "MEM",
  56 };
  57
  58 //static const char *partition_marker = "PART";
  59
  60 struct hunk_info piscsi_hinfo;
  61 struct hunk_reloc piscsi_hreloc[256];
  62
  63 void piscsi_init() {
  64     for (int i = 0; i < 8; i++) {
  65         devs[i].fd = -1;
  66         devs[i].lba = 0;
  67         devs[i].c = devs[i].h = devs[i].s = 0;
  68     }
  69
  70     FILE *in = fopen("./platforms/amiga/piscsi/piscsi.rom", "rb");
  71     if (in == NULL) {
  72         printf("[PISCSI] Could not open PISCSI Boot ROM file for reading!\n");
  73         // Zero out the boot ROM offset from the autoconfig ROM.
  74         ac_piscsi_rom[20] = 0;
  75         ac_piscsi_rom[21] = 0;
  76         ac_piscsi_rom[22] = 0;
  77         ac_piscsi_rom[23] = 0;
  78         return;
  79     }
  80     fseek(in, 0, SEEK_END);
  81     piscsi_rom_size = ftell(in);
  82     fseek(in, 0, SEEK_SET);
  83     piscsi_rom_ptr = malloc(piscsi_rom_size);
  84     fread(piscsi_rom_ptr, piscsi_rom_size, 1, in);
  85
  86     fseek(in, PISCSI_DRIVER_OFFSET, SEEK_SET);
  87     process_hunks(in, &piscsi_hinfo, piscsi_hreloc, PISCSI_DRIVER_OFFSET);
  88
  89     fclose(in);
  90     printf("[PISCSI] Loaded Boot ROM.\n");
  91 }
  92
  93 void piscsi_find_partitions(struct piscsi_dev *d) {
  94     int fd = d->fd;
  95     int cur_partition = 0;
  96     uint8_t tmp;
  97
  98     for (int i = 0; i < 16; i++) {
  99         if (d->pb[i]) {
 100             free(d->pb[i]);
 101             d->pb[i] = NULL;
 102         }
 103     }
 104
 105     if (!d->rdb || d->rdb->rdb_PartitionList == 0) {
 106         DEBUG("[PISCSI] No partitions on disk.\n");
 107         return;
 108     }
 109
 110     char *block = malloc(512);
 111
 112     lseek(fd, BE(d->rdb->rdb_PartitionList) * 512, SEEK_SET);
 113 next_partition:;
 114     read(fd, block, 512);
 115
 116     uint32_t first = be32toh(*((uint32_t *)&block[0]));
 117     if (first != PART_IDENTIFIER) {
 118         DEBUG("Entry at block %d is not a valid partition. Aborting.\n", BE(d->rdb->rdb_PartitionList));
 119         return;
 120     }
 121
 122     struct PartitionBlock *pb = (struct PartitionBlock *)block;
 123     tmp = pb->pb_DriveName[0];
 124     pb->pb_DriveName[tmp + 1] = 0x00;
 125     DEBUG("[PISCSI] Partition %d: %s\n", cur_partition, pb->pb_DriveName + 1);
 126     DEBUG("Checksum: %.8X HostID: %d\n", BE(pb->pb_ChkSum), BE(pb->pb_HostID));
 127     DEBUG("Flags: %d (%.8X) Devflags: %d (%.8X)\n", BE(pb->pb_Flags), BE(pb->pb_Flags), BE(pb->pb_DevFlags), BE(pb->pb_DevFlags));
 128     d->pb[cur_partition] = pb;
 129
 130     if (d->pb[cur_partition]->pb_Next != 0xFFFFFFFF) {
 131         uint64_t next = be32toh(pb->pb_Next);
 132         block = malloc(512);
 133         lseek64(fd, next * 512, SEEK_SET);
 134         cur_partition++;
 135         DEBUG("[PISCSI] Next partition at block %d.\n", be32toh(pb->pb_Next));
 136         goto next_partition;
 137     }
 138     DEBUG("[PISCSI] No more partitions on disk.\n");
 139     d->num_partitions = cur_partition + 1;
 140     d->fshd_offs = lseek64(fd, 0, SEEK_CUR);
 141
 142     return;
 143 }
 144
 145 int piscsi_parse_rdb(struct piscsi_dev *d) {
 146     int fd = d->fd;
 147     int i = 0;
 148     uint8_t *block = malloc(512);
 149
 150     lseek(fd, 0, SEEK_SET);
 151     for (i = 0; i < RDB_BLOCK_LIMIT; i++) {
 152         read(fd, block, 512);
 153         uint32_t first = be32toh(*((uint32_t *)&block[0]));
 154         if (first == RDB_IDENTIFIER)
 155             goto rdb_found;
 156     }
 157     goto no_rdb_found;
 158 rdb_found:;
 159     struct RigidDiskBlock *rdb = (struct RigidDiskBlock *)block;
 160     DEBUG("[PISCSI] RDB found at block %d.\n", i);
 161     d->c = be32toh(rdb->rdb_Cylinders);
 162     d->h = be32toh(rdb->rdb_Heads);
 163     d->s = be32toh(rdb->rdb_Sectors);
 164     d->num_partitions = 0;
 165     DEBUG("[PISCSI] RDB - first partition at block %d.\n", be32toh(rdb->rdb_PartitionList));
 166     if (d->rdb)
 167         free(d->rdb);
 168     d->rdb = rdb;
 169     sprintf(d->rdb->rdb_DriveInitName, "pi-scsi.device");
 170     return 0;
 171
 172 no_rdb_found:;
 173     if (block)
 174         free(block);
 175
 176     return -1;
 177 }
 178
 179 void piscsi_refresh_drives() {
 180     piscsi_num_fs = 0;
 181
 182     for (int i = 0; i < NUM_FILESYSTEMS; i++) {
 183         if (filesystems[i].binary_data) {
 184             free(filesystems[i].binary_data);
 185             filesystems[i].binary_data = NULL;
 186         }
 187         if (filesystems[i].fhb) {
 188             free(filesystems[i].fhb);
 189             filesystems[i].fhb = NULL;
 190         }
 191         filesystems[i].h_info.current_hunk = 0;
 192         filesystems[i].h_info.reloc_hunks = 0;
 193         filesystems[i].FS_ID = 0;
 194         filesystems[i].handler = 0;
 195     }
 196
 197     rom_cur_fs = 0;
 198
 199     for (int i = 0; i < NUM_UNITS; i++) {
 200         if (devs[i].fd != -1) {
 201             piscsi_parse_rdb(&devs[i]);
 202             piscsi_find_partitions(&devs[i]);
 203             piscsi_find_filesystems(&devs[i]);
 204         }
 205     }
 206 }
 207
 208 void piscsi_find_filesystems(struct piscsi_dev *d) {
 209     if (!d->num_partitions)
 210         return;
 211
 212     uint8_t fs_found = 0;
 213
 214     uint8_t *fhb_block = malloc(512);
 215
 216     lseek64(d->fd, d->fshd_offs, SEEK_SET);
 217
 218     struct FileSysHeaderBlock *fhb = (struct FileSysHeaderBlock *)fhb_block;
 219     read(d->fd, fhb_block, 512);
 220
 221     while (BE(fhb->fhb_ID) == FS_IDENTIFIER) {
 222         char *dosID = (char *)&fhb->fhb_DosType;
 223         uint16_t *fsVer = (uint16_t *)&fhb->fhb_Version;
 224
 225         DEBUG("[FSHD] FSHD Block found.\n");
 226         DEBUG("[FSHD] HostID: %d Next: %d Size: %d\n", BE(fhb->fhb_HostID), BE(fhb->fhb_Next), BE(fhb->fhb_SummedLongs));
 227         DEBUG("[FSHD] Flags: %.8X DOSType: %c%c%c/%d\n", BE(fhb->fhb_Flags), dosID[0], dosID[1], dosID[2], dosID[3]);
 228         DEBUG("[FSHD] Version: %d.%d\n", BE16(fsVer[0]), BE16(fsVer[1]));
 229         DEBUG("[FSHD] Patchflags: %d Type: %d\n", BE(fhb->fhb_PatchFlags), BE(fhb->fhb_Type));
 230         DEBUG("[FSHD] Task: %d Lock: %d\n", BE(fhb->fhb_Task), BE(fhb->fhb_Lock));
 231         DEBUG("[FSHD] Handler: %d StackSize: %d\n", BE(fhb->fhb_Handler), BE(fhb->fhb_StackSize));
 232         DEBUG("[FSHD] Prio: %d Startup: %d\n", BE(fhb->fhb_Priority), BE(fhb->fhb_Startup));
 233         DEBUG("[FSHD] SegListBlocks: %d GlobalVec: %d\n", BE(fhb->fhb_Priority), BE(fhb->fhb_Startup));
 234         DEBUG("[FSHD] FileSysName: %s\n", fhb->fhb_FileSysName + 1);
 235
 236         for (int i = 0; i < NUM_FILESYSTEMS; i++) {
 237             if (filesystems[i].FS_ID == fhb->fhb_DosType) {
 238                 DEBUG("[FSHD] File system %c%c%c/%d already loaded. Skipping.\n", dosID[0], dosID[1], dosID[2], dosID[3]);
 239                 if (BE(fhb->fhb_Next) == 0xFFFFFFFF)
 240                     goto fs_done;
 241
 242                 goto skip_fs_load_lseg;
 243             }
 244         }
 245
 246         if (load_lseg(d->fd, &filesystems[piscsi_num_fs].binary_data, &filesystems[piscsi_num_fs].h_info, filesystems[piscsi_num_fs].relocs) != -1) {
 247             filesystems[piscsi_num_fs].FS_ID = fhb->fhb_DosType;
 248             filesystems[piscsi_num_fs].fhb = fhb;
 249             printf("[FSHD] Loaded and set up file system %d: %c%c%c/%d\n", piscsi_num_fs + 1, dosID[0], dosID[1], dosID[2], dosID[3]);
 250             piscsi_num_fs++;
 251         }
 252
 253 skip_fs_load_lseg:;
 254         fs_found++;
 255         lseek64(d->fd, BE(fhb->fhb_Next) * 512, SEEK_SET);
 256         fhb_block = malloc(512);
 257         fhb = (struct FileSysHeaderBlock *)fhb_block;
 258         read(d->fd, fhb_block, 512);
 259     }
 260
 261     if (!fs_found)
 262         DEBUG("[!!!FSHD] No file systems found on hard drive!\n");
 263
 264 fs_done:;
 265     if (fhb_block)
 266         free(fhb_block);
 267 }
 268
 269 void piscsi_map_drive(char *filename, uint8_t index) {
 270     if (index > 7) {
 271         printf("[PISCSI] Drive index %d out of range.\nUnable to map file %s to drive.\n", index, filename);
 272         return;
 273     }
 274
 275     int32_t tmp_fd = open(filename, O_RDWR);
 276     if (tmp_fd == -1) {
 277         printf("[PISCSI] Failed to open file %s, could not map drive %d.\n", filename, index);
 278         return;
 279     }
 280
 281     struct piscsi_dev *d = &devs[index];
 282
 283     uint64_t file_size = lseek(tmp_fd, 0, SEEK_END);
 284     d->fs = file_size;
 285     d->fd = tmp_fd;
 286     lseek(tmp_fd, 0, SEEK_SET);
 287     printf("[PISCSI] Map %d: [%s] - %llu bytes.\n", index, filename, file_size);
 288
 289     if (piscsi_parse_rdb(d) == -1) {
 290         DEBUG("[PISCSI] No RDB found on disk, making up some CHS values.\n");
 291         d->h = 16;
 292         d->s = 63;
 293         d->c = (file_size / 512) / (d->s * d->h);
 294     }
 295     printf("[PISCSI] CHS: %d %d %d\n", d->c, d->h, d->s);
 296
 297     piscsi_find_partitions(d);
 298     piscsi_find_filesystems(d);
 299 }
 300
 301 void piscsi_unmap_drive(uint8_t index) {
 302     if (devs[index].fd != -1) {
 303         DEBUG("[PISCSI] Unmapped drive %d.\n", index);
 304         close (devs[index].fd);
 305         devs[index].fd = -1;
 306     }
 307 }
 308
 309 char *io_cmd_name(int index) {
 310     switch (index) {
 311         case CMD_INVALID: return "INVALID";
 312         case CMD_RESET: return "RESET";
 313         case CMD_READ: return "READ";
 314         case CMD_WRITE: return "WRITE";
 315         case CMD_UPDATE: return "UPDATE";
 316         case CMD_CLEAR: return "CLEAR";
 317         case CMD_STOP: return "STOP";
 318         case CMD_START: return "START";
 319         case CMD_FLUSH: return "FLUSH";
 320         case TD_MOTOR: return "TD_MOTOR";
 321         case TD_SEEK: return "SEEK";
 322         case TD_FORMAT: return "FORMAT";
 323         case TD_REMOVE: return "REMOVE";
 324         case TD_CHANGENUM: return "CHANGENUM";
 325         case TD_CHANGESTATE: return "CHANGESTATE";
 326         case TD_PROTSTATUS: return "PROTSTATUS";
 327         case TD_RAWREAD: return "RAWREAD";
 328         case TD_RAWWRITE: return "RAWWRITE";
 329         case TD_GETDRIVETYPE: return "GETDRIVETYPE";
 330         case TD_GETNUMTRACKS: return "GETNUMTRACKS";
 331         case TD_ADDCHANGEINT: return "ADDCHANGEINT";
 332         case TD_REMCHANGEINT: return "REMCHANGEINT";
 333         case TD_GETGEOMETRY: return "GETGEOMETRY";
 334         case TD_EJECT: return "EJECT";
 335         case TD_LASTCOMM: return "LASTCOMM/READ64";
 336         case TD_WRITE64: return "WRITE64";
 337         case HD_SCSICMD: return "HD_SCSICMD";
 338         case NSCMD_DEVICEQUERY: return "NSCMD_DEVICEQUERY";
 339         case NSCMD_TD_READ64: return "NSCMD_TD_READ64";
 340         case NSCMD_TD_WRITE64: return "NSCMD_TD_WRITE64";
 341         case NSCMD_TD_FORMAT64: return "NSCMD_TD_FORMAT64";
 342
 343         default:
 344             return "[!!!PISCSI] Unhandled IO command";
 345     }
 346 }
 347
 348 #define GETSCSINAME(a) case a: return ""#a"";
 349 #define SCSIUNHANDLED(a) return "[!!!PISCSI] Unhandled SCSI command "#a"";
 350
 351 char *scsi_cmd_name(int index) {
 352     switch(index) {
 353         GETSCSINAME(SCSICMD_TEST_UNIT_READY);
 354         GETSCSINAME(SCSICMD_INQUIRY);
 355         GETSCSINAME(SCSICMD_READ_6);
 356         GETSCSINAME(SCSICMD_WRITE_6);
 357         GETSCSINAME(SCSICMD_READ_10);
 358         GETSCSINAME(SCSICMD_WRITE_10);
 359         GETSCSINAME(SCSICMD_READ_CAPACITY_10);
 360         GETSCSINAME(SCSICMD_MODE_SENSE_6);
 361         GETSCSINAME(SCSICMD_READ_DEFECT_DATA_10);
 362         default:
 363             return "[!!!PISCSI] Unhandled SCSI command";
 364     }
 365 }
 366
 367 void print_piscsi_debug_message(int index) {
 368     int32_t r = 0;
 369
 370     switch (index) {
 371         case DBG_INIT:
 372             DEBUG("[PISCSI] Initializing devices.\n");
 373             break;
 374         case DBG_OPENDEV:
 375             if (piscsi_dbg[0] != 255)
 376                 DEBUG("[PISCSI] Opening device %d (%d). Flags: %d (%.2X)\n", piscsi_dbg[0], piscsi_dbg[2], piscsi_dbg[1], piscsi_dbg[1]);
 377             break;
 378         case DBG_CLEANUP:
 379             DEBUG("[PISCSI] Cleaning up.\n");
 380             break;
 381         case DBG_CHS:
 382             DEBUG("[PISCSI] C/H/S: %d / %d / %d\n", piscsi_dbg[0], piscsi_dbg[1], piscsi_dbg[2]);
 383             break;
 384         case DBG_BEGINIO:
 385             DEBUG("[PISCSI] BeginIO: io_Command: %d (%s) - io_Flags = %d - quick: %d\n", piscsi_dbg[0], io_cmd_name(piscsi_dbg[0]), piscsi_dbg[1], piscsi_dbg[2]);
 386             break;
 387         case DBG_ABORTIO:
 388             DEBUG("[PISCSI] AbortIO!\n");
 389             break;
 390         case DBG_SCSICMD:
 391             DEBUG("[PISCSI] SCSI Command %d (%s)\n", piscsi_dbg[1], scsi_cmd_name(piscsi_dbg[1]));
 392             DEBUG("Len: %d - %.2X %.2X %.2X - Command Length: %d\n", piscsi_dbg[0], piscsi_dbg[1], piscsi_dbg[2], piscsi_dbg[3], piscsi_dbg[4]);
 393             break;
 394         case DBG_SCSI_UNKNOWN_MODESENSE:
 395             DEBUG("[!!!PISCSI] SCSI: Unknown modesense %.4X\n", piscsi_dbg[0]);
 396             break;
 397         case DBG_SCSI_UNKNOWN_COMMAND:
 398             DEBUG("[!!!PISCSI] SCSI: Unknown command %.4X\n", piscsi_dbg[0]);
 399             break;
 400         case DBG_SCSIERR:
 401             DEBUG("[!!!PISCSI] SCSI: An error occured: %.4X\n", piscsi_dbg[0]);
 402             break;
 403         case DBG_IOCMD:
 404             DEBUG_TRIVIAL("[PISCSI] IO Command %d (%s)\n", piscsi_dbg[0], io_cmd_name(piscsi_dbg[0]));
 405             break;
 406         case DBG_IOCMD_UNHANDLED:
 407             DEBUG("[!!!PISCSI] WARN: IO command %.4X (%s) is unhandled by driver.\n", piscsi_dbg[0], io_cmd_name(piscsi_dbg[0]));
 408             break;
 409         case DBG_SCSI_FORMATDEVICE:
 410             DEBUG("[PISCSI] Get SCSI FormatDevice MODE SENSE.\n");
 411             break;
 412         case DBG_SCSI_RDG:
 413             DEBUG("[PISCSI] Get SCSI RDG MODE SENSE.\n");
 414             break;
 415         case DBG_SCSI_DEBUG_MODESENSE_6:
 416             DEBUG_TRIVIAL("[PISCSI] SCSI ModeSense debug. Data: %.8X\n", piscsi_dbg[0]);
 417             r = get_mapped_item_by_address(cfg, piscsi_dbg[0]);
 418             if (r != -1) {
 419                 uint32_t addr = piscsi_dbg[0] - cfg->map_offset[r];
 420                 struct SCSICmd_ModeSense6 *sense = (struct SCSICmd_ModeSense6 *)(&cfg->map_data[r][addr]);
 421                 DEBUG_TRIVIAL("[SenseData] CMD: %.2X\n", sense->opcode);
 422                 DEBUG_TRIVIAL("[SenseData] DBD: %d\n", sense->reserved_dbd & 0x04);
 423                 DEBUG_TRIVIAL("[SenseData] PC: %d\n", (sense->pc_pagecode & 0xC0 >> 6));
 424                 DEBUG_TRIVIAL("[SenseData] PageCodes: %.2X %.2X\n", (sense->pc_pagecode & 0x3F), sense->subpage_code);
 425                 DEBUG_TRIVIAL("[SenseData] AllocLen: %d\n", sense->alloc_len);
 426                 DEBUG_TRIVIAL("[SenseData] Control: %.2X (%d)\n", sense->control, sense->control);
 427             }
 428             else {
 429                 DEBUG("[!!!PISCSI] ModeSense data not immediately available.\n");
 430             }
 431             break;
 432         default:
 433             DEBUG("[!!!PISCSI] No debug message available for index %d.\n", index);
 434             break;
 435     }
 436 }
 437
 438 #define DEBUGME_SIMPLE(i, s) case i: DEBUG(s); break;
 439
 440 void piscsi_debugme(uint32_t index) {
 441     switch (index) {
 442         DEBUGME_SIMPLE(1, "[PISCSI-DEBUGME] Arrived at DiagEntry.\n");
 443         DEBUGME_SIMPLE(3, "[PISCSI-DEBUGME] Init: Interrupt disable.\n");
 444         DEBUGME_SIMPLE(4, "[PISCSI-DEBUGME] Init: Copy/reloc driver.\n");
 445         DEBUGME_SIMPLE(5, "[PISCSI-DEBUGME] Init: InitResident.\n");
 446         DEBUGME_SIMPLE(7, "[PISCSI-DEBUGME] Init: Begin partition loop.\n");
 447         DEBUGME_SIMPLE(8, "[PISCSI-DEBUGME] Init: Partition loop done. Cleaning up and returning to Exec.\n");
 448         DEBUGME_SIMPLE(9, "[PISCSI-DEBUGME] Init: Load file systems.\n");
 449         DEBUGME_SIMPLE(10, "[PISCSI-DEBUGME] Init: AllocMem for resident.\n");
 450         DEBUGME_SIMPLE(11, "[PISCSI-DEBUGME] Init: Checking if resident is loaded.\n");
 451         DEBUGME_SIMPLE(22, "[PISCSI-DEBUGME] Arrived at BootEntry.\n");
 452         DEBUGME_SIMPLE(30, "[PISCSI-DEBUGME] LoadFileSystems: Opening FileSystem.resource.\n");
 453         DEBUGME_SIMPLE(33, "[PISCSI-DEBUGME] FileSystem.resource not available, creating.\n");
 454         case 31:
 455             DEBUG("[PISCSI-DEBUGME] OpenResource result: %d\n", piscsi_u32[0]);
 456             break;
 457         case 32:
 458             DEBUG("AAAAHH!\n");
 459             break;
 460         default:
 461             DEBUG("[!!!PISCSI-DEBUGME] No debugme message for index %d!\n", index);
 462             break;
 463     }
 464
 465     if (index == 8) {
 466         stop_cpu_emulation(1);
 467     }
 468 }
 469
 470 void handle_piscsi_write(uint32_t addr, uint32_t val, uint8_t type) {
 471     int32_t r;
 472
 473     struct piscsi_dev *d = &devs[piscsi_cur_drive];
 474
 475     uint16_t cmd = (addr & 0xFFFF);
 476
 477     switch (cmd) {
 478         case PISCSI_CMD_READ64:
 479         case PISCSI_CMD_READ:
 480             d = &devs[val];
 481             if (d->fd == -1) {
 482                 DEBUG("[!!!PISCSI] BUG: Attempted read from unmapped drive %d.\n", val);
 483                 break;
 484             }
 485
 486             if (cmd == PISCSI_CMD_READ) {
 487                 DEBUG("[PISCSI-%d] %d byte READ from block %d to address %.8X\n", val, piscsi_u32[1], piscsi_u32[0], piscsi_u32[2]);
 488                 d->lba = piscsi_u32[0];
 489                 lseek(d->fd, (piscsi_u32[0] * 512), SEEK_SET);
 490             }
 491             else {
 492                 uint64_t src = piscsi_u32[3];
 493                 src = (src << 32) | piscsi_u32[0];
 494                 DEBUG("[PISCSI-%d] %d byte READ64 from block %lld to address %.8X\n", val, piscsi_u32[1], (src / 512), piscsi_u32[2]);
 495                 d->lba = (src / 512);
 496                 lseek64(d->fd, src, SEEK_SET);
 497             }
 498
 499             r = get_mapped_item_by_address(cfg, piscsi_u32[2]);
 500             if (r != -1 && cfg->map_type[r] == MAPTYPE_RAM) {
 501                 DEBUG_TRIVIAL("[PISCSI-%d] \"DMA\" Read goes to mapped range %d.\n", val, r);
 502                 read(d->fd, cfg->map_data[r] + piscsi_u32[2] - cfg->map_offset[r], piscsi_u32[1]);
 503             }
 504             else {
 505                 DEBUG_TRIVIAL("[PISCSI-%d] No mapped range found for read.\n", val);
 506                 uint8_t c = 0;
 507                 for (uint32_t i = 0; i < piscsi_u32[1]; i++) {
 508                     read(d->fd, &c, 1);
 509                     write8(piscsi_u32[2] + i, (uint32_t)c);
 510                 }
 511             }
 512             break;
 513         case PISCSI_CMD_WRITE64:
 514         case PISCSI_CMD_WRITE:
 515             d = &devs[val];
 516             if (d->fd == -1) {
 517                 DEBUG ("[PISCSI] BUG: Attempted write to unmapped drive %d.\n", val);
 518                 break;
 519             }
 520
 521             if (cmd == PISCSI_CMD_WRITE) {
 522                 DEBUG("[PISCSI-%d] %d byte WRITE to block %d from address %.8X\n", val, piscsi_u32[1], piscsi_u32[0], piscsi_u32[2]);
 523                 d->lba = piscsi_u32[0];
 524                 lseek(d->fd, (piscsi_u32[0] * 512), SEEK_SET);
 525             }
 526             else {
 527                 uint64_t src = piscsi_u32[3];
 528                 src = (src << 32) | piscsi_u32[0];
 529                 DEBUG("[PISCSI-%d] %d byte WRITE64 to block %lld from address %.8X\n", val, piscsi_u32[1], (src / 512), piscsi_u32[2]);
 530                 d->lba = (src / 512);
 531                 lseek64(d->fd, src, SEEK_SET);
 532             }
 533
 534             r = get_mapped_item_by_address(cfg, piscsi_u32[2]);
 535             if (r != -1) {
 536                 DEBUG_TRIVIAL("[PISCSI-%d] \"DMA\" Write comes from mapped range %d.\n", val, r);
 537                 write(d->fd, cfg->map_data[r] + piscsi_u32[2] - cfg->map_offset[r], piscsi_u32[1]);
 538             }
 539             else {
 540                 DEBUG_TRIVIAL("[PISCSI-%d] No mapped range found for write.\n", val);
 541                 uint8_t c = 0;
 542                 for (uint32_t i = 0; i < piscsi_u32[1]; i++) {
 543                     c = read8(piscsi_u32[2] + i);
 544                     write(d->fd, &c, 1);
 545                 }
 546             }
 547             break;
 548         case PISCSI_CMD_ADDR1: case PISCSI_CMD_ADDR2: case PISCSI_CMD_ADDR3: case PISCSI_CMD_ADDR4: {
 549             int i = ((addr & 0xFFFF) - PISCSI_CMD_ADDR1) / 4;
 550             piscsi_u32[i] = val;
 551             break;
 552         }
 553         case PISCSI_CMD_DRVNUM:
 554             //printf("%d ", val);
 555             if (val % 10 != 0)
 556                 piscsi_cur_drive = 255;
 557             else
 558                 piscsi_cur_drive = val / 10;
 559             if (piscsi_cur_drive > NUM_UNITS)
 560                 piscsi_cur_drive = 255;
 561
 562             if (piscsi_cur_drive != 255)
 563                 DEBUG("[PISCSI] (%s) Drive number set to %d (%d)\n", op_type_names[type], piscsi_cur_drive, val);
 564             break;
 565         case PISCSI_CMD_DEBUGME:
 566             piscsi_debugme(val);
 567             break;
 568         case PISCSI_CMD_DRIVER: {
 569             DEBUG("[PISCSI] Driver copy/patch called, destination address %.8X.\n", val);
 570             int r = get_mapped_item_by_address(cfg, val);
 571             if (r != -1) {
 572                 uint32_t addr = val - cfg->map_offset[r];
 573                 uint8_t *dst_data = cfg->map_data[r];
 574                 uint8_t cur_partition = 0;
 575                 memcpy(dst_data + addr, piscsi_rom_ptr + PISCSI_DRIVER_OFFSET, 0x4000 - PISCSI_DRIVER_OFFSET);
 576
 577                 piscsi_hinfo.base_offset = val;
 578
 579                 reloc_hunks(piscsi_hreloc, dst_data + addr, &piscsi_hinfo);
 580
 581                 #define PUTNODELONG(val) *(uint32_t *)&dst_data[p_offs] = htobe32(val); p_offs += 4;
 582                 #define PUTNODELONGBE(val) *(uint32_t *)&dst_data[p_offs] = val; p_offs += 4;
 583
 584                 for (int i = 0; i < 128; i++) {
 585                     rom_partitions[i] = 0;
 586                     rom_partition_prio[i] = 0;
 587                     rom_partition_dostype[i] = 0;
 588                 }
 589                 rom_cur_partition = 0;
 590
 591                 uint32_t data_addr = addr + 0x3F00;
 592                 sprintf((char *)dst_data + data_addr, "pi-scsi.device");
 593                 uint32_t addr2 = addr + 0x4000;
 594                 for (int i = 0; i < NUM_UNITS; i++) {
 595                     if (devs[i].fd != -1)
 596                         piscsi_find_partitions(&devs[i]);
 597                     else
 598                         goto skip_disk;
 599
 600                     if (devs[i].num_partitions) {
 601                         uint32_t p_offs = addr2;
 602                         DEBUG("[PISCSI] Adding %d partitions for unit %d\n", devs[i].num_partitions, i);
 603                         for (uint32_t j = 0; j < devs[i].num_partitions; j++) {
 604                             DEBUG("Partition %d: %s\n", j, devs[i].pb[j]->pb_DriveName + 1);
 605                             sprintf((char *)dst_data + p_offs, "%s", devs[i].pb[j]->pb_DriveName + 1);
 606                             p_offs += 0x20;
 607                             PUTNODELONG(addr2 + cfg->map_offset[r]);
 608                             PUTNODELONG(data_addr + cfg->map_offset[r]);
 609                             PUTNODELONG((i * 10));
 610                             PUTNODELONG(0);
 611                             uint32_t nodesize = (be32toh(devs[i].pb[j]->pb_Environment[0]) + 1) * 4;
 612                             memcpy(dst_data + p_offs, devs[i].pb[j]->pb_Environment, nodesize);
 613
 614                             struct pihd_dosnode_data *dat = (struct pihd_dosnode_data *)(&dst_data[addr2+0x20]);
 615
 616                             if (BE(devs[i].pb[j]->pb_Flags) & 0x01) {
 617                                 DEBUG("Partition is bootable.\n");
 618                                 rom_partition_prio[cur_partition] = 0;
 619                                 dat->priority = 0;
 620                             }
 621                             else {
 622                                 DEBUG("Partition is not bootable.\n");
 623                                 rom_partition_prio[cur_partition] = -128;
 624                                 dat->priority = htobe32(-128);
 625                             }
 626
 627                             DEBUG("DOSNode Data:\n");
 628                             DEBUG("Name: %s Device: %s\n", dst_data + addr2, dst_data + data_addr);
 629                             DEBUG("Unit: %d Flags: %d Pad1: %d\n", BE(dat->unit), BE(dat->flags), BE(dat->pad1));
 630                             DEBUG("Node len: %d Block len: %d\n", BE(dat->node_len) * 4, BE(dat->block_len) * 4);
 631                             DEBUG("H: %d SPB: %d BPS: %d\n", BE(dat->surf), BE(dat->secs_per_block), BE(dat->blocks_per_track));
 632                             DEBUG("Reserved: %d Prealloc: %d\n", BE(dat->reserved_blocks), BE(dat->pad2));
 633                             DEBUG("Interleaved: %d Buffers: %d Memtype: %d\n", BE(dat->interleave), BE(dat->buffers), BE(dat->mem_type));
 634                             DEBUG("Lowcyl: %d Highcyl: %d Prio: %d\n", BE(dat->lowcyl), BE(dat->highcyl), BE(dat->priority));
 635                             DEBUG("Maxtransfer: %.8X Mask: %.8X\n", BE(dat->maxtransfer), BE(dat->transfer_mask));
 636                             DEBUG("DOSType: %.8X\n", BE(dat->dostype));
 637
 638                             rom_partitions[cur_partition] = addr2 + 0x20 + cfg->map_offset[r];
 639                             rom_partition_dostype[cur_partition] = dat->dostype;
 640                             cur_partition++;
 641                             addr2 += 0x100;
 642                             p_offs = addr2;
 643                         }
 644                     }
 645 skip_disk:;
 646                 }
 647             }
 648
 649             break;
 650         }
 651         case PISCSI_CMD_NEXTPART:
 652             DEBUG("[PISCSI] Switch partition %d -> %d\n", rom_cur_partition, rom_cur_partition + 1);
 653             rom_cur_partition++;
 654             break;
 655         case PISCSI_CMD_NEXTFS:
 656             DEBUG("[PISCSI] Switch file file system %d -> %d\n", rom_cur_fs, rom_cur_fs + 1);
 657             rom_cur_fs++;
 658             break;
 659         case PISCSI_CMD_COPYFS:
 660             DEBUG("[PISCSI] Copy file system %d to %.8X and reloc.\n", rom_cur_fs, piscsi_u32[2]);
 661             r = get_mapped_item_by_address(cfg, piscsi_u32[2]);
 662             if (r != -1) {
 663                 uint32_t addr = piscsi_u32[2] - cfg->map_offset[r];
 664                 memset(cfg->map_data[r] + addr, 0x00, filesystems[rom_cur_fs].h_info.alloc_size);
 665                 memcpy(cfg->map_data[r] + addr, filesystems[rom_cur_fs].binary_data, filesystems[rom_cur_fs].h_info.byte_size);
 666                 filesystems[rom_cur_fs].h_info.base_offset = piscsi_u32[2];
 667                 reloc_hunks(filesystems[rom_cur_fs].relocs, cfg->map_data[r] + addr, &filesystems[rom_cur_fs].h_info);
 668                 filesystems[rom_cur_fs].handler = piscsi_u32[2];
 669             }
 670             break;
 671         case PISCSI_CMD_SETFSH: {
 672             int i = 0;
 673             DEBUG("[PISCSI] Set handler for partition %d (DeviceNode: %.8X)\n", rom_cur_partition, val);
 674             r = get_mapped_item_by_address(cfg, val);
 675             if (r != -1) {
 676                 uint32_t addr = val - cfg->map_offset[r];
 677                 struct DeviceNode *node = (struct DeviceNode *)(cfg->map_data[r] + addr);
 678                 char *dosID = (char *)&rom_partition_dostype[rom_cur_partition];
 679                 DEBUG("[PISCSI] Partition DOSType is %c%c%c/%d\n", dosID[0], dosID[1], dosID[2], dosID[3]);
 680                 for (i = 0; i < piscsi_num_fs; i++) {
 681                     if (rom_partition_dostype[rom_cur_partition] == filesystems[i].FS_ID) {
 682                         node->dn_SegList = htobe32(filesystems[i].handler);
 683                         goto fs_found;
 684                     }
 685                 }
 686                 DEBUG("[!!!PISCSI] Found no handler for file system!\n");
 687 fs_found:;
 688                 DEBUG("[FS-HANDLER] Next: %d Type: %d\n", BE(node->dn_Next), BE(node->dn_Type));
 689                 DEBUG("[FS-HANDLER] Task: %d Lock: %d\n", BE(node->dn_Task), BE(node->dn_Lock));
 690                 DEBUG("[FS-HANDLER] Handler: %d Stacksize: %d\n", BE((uint32_t)node->dn_Handler), BE(node->dn_StackSize));
 691                 DEBUG("[FS-HANDLER] Priority: %d Startup: %d\n", BE((uint32_t)node->dn_Priority), BE(node->dn_Startup));
 692                 DEBUG("[FS-HANDLER] SegList: %d GlobalVec: %d\n", BE((uint32_t)node->dn_SegList), BE(node->dn_GlobalVec));
 693                 DEBUG("[PISCSI] Handler for partition %.8X set to %.8X (%.8X).\n", BE((uint32_t)node->dn_Name), filesystems[i].FS_ID, filesystems[i].handler);
 694             }
 695             break;
 696         }
 697         case PISCSI_DBG_VAL1: case PISCSI_DBG_VAL2: case PISCSI_DBG_VAL3: case PISCSI_DBG_VAL4:
 698         case PISCSI_DBG_VAL5: case PISCSI_DBG_VAL6: case PISCSI_DBG_VAL7: case PISCSI_DBG_VAL8: {
 699             int i = ((addr & 0xFFFF) - PISCSI_DBG_VAL1) / 4;
 700             piscsi_dbg[i] = val;
 701             break;
 702         }
 703         case PISCSI_DBG_MSG:
 704             print_piscsi_debug_message(val);
 705             break;
 706         default:
 707             DEBUG("[!!!PISCSI] WARN: Unhandled %s register write to %.8X: %d\n", op_type_names[type], addr, val);
 708             break;
 709     }
 710 }
 711
 712 #define PIB 0x00
 713
 714 uint32_t handle_piscsi_read(uint32_t addr, uint8_t type) {
 715     if (type) {}
 716
 717     if ((addr & 0xFFFF) >= PISCSI_CMD_ROM) {
 718         uint32_t romoffs = (addr & 0xFFFF) - PISCSI_CMD_ROM;
 719         if (romoffs < (piscsi_rom_size + PIB)) {
 720             //DEBUG("[PISCSI] %s read from Boot ROM @$%.4X (%.8X): ", op_type_names[type], romoffs, addr);
 721             uint32_t v = 0;
 722             switch (type) {
 723                 case OP_TYPE_BYTE:
 724                     v = piscsi_rom_ptr[romoffs - PIB];
 725                     //DEBUG("%.2X\n", v);
 726                     break;
 727                 case OP_TYPE_WORD:
 728                     v = be16toh(*((uint16_t *)&piscsi_rom_ptr[romoffs - PIB]));
 729                     //DEBUG("%.4X\n", v);
 730                     break;
 731                 case OP_TYPE_LONGWORD:
 732                     v = be32toh(*((uint32_t *)&piscsi_rom_ptr[romoffs - PIB]));
 733                     //DEBUG("%.8X\n", v);
 734                     break;
 735             }
 736             return v;
 737         }
 738         return 0;
 739     }
 740
 741     switch (addr & 0xFFFF) {
 742         case PISCSI_CMD_ADDR1: case PISCSI_CMD_ADDR2: case PISCSI_CMD_ADDR3: case PISCSI_CMD_ADDR4: {
 743             int i = ((addr & 0xFFFF) - PISCSI_CMD_ADDR1) / 4;
 744             return piscsi_u32[i];
 745             break;
 746         }
 747         case PISCSI_CMD_DRVTYPE:
 748             if (devs[piscsi_cur_drive].fd == -1) {
 749                 DEBUG("[PISCSI] %s Read from DRVTYPE %d, drive not attached.\n", op_type_names[type], piscsi_cur_drive);
 750                 return 0;
 751             }
 752             DEBUG("[PISCSI] %s Read from DRVTYPE %d, drive attached.\n", op_type_names[type], piscsi_cur_drive);
 753             return 1;
 754             break;
 755         case PISCSI_CMD_DRVNUM:
 756             return piscsi_cur_drive;
 757             break;
 758         case PISCSI_CMD_CYLS:
 759             DEBUG("[PISCSI] %s Read from CYLS %d: %d\n", op_type_names[type], piscsi_cur_drive, devs[piscsi_cur_drive].c);
 760             return devs[piscsi_cur_drive].c;
 761             break;
 762         case PISCSI_CMD_HEADS:
 763             DEBUG("[PISCSI] %s Read from HEADS %d: %d\n", op_type_names[type], piscsi_cur_drive, devs[piscsi_cur_drive].h);
 764             return devs[piscsi_cur_drive].h;
 765             break;
 766         case PISCSI_CMD_SECS:
 767             DEBUG("[PISCSI] %s Read from SECS %d: %d\n", op_type_names[type], piscsi_cur_drive, devs[piscsi_cur_drive].s);
 768             return devs[piscsi_cur_drive].s;
 769             break;
 770         case PISCSI_CMD_BLOCKS: {
 771             uint32_t blox = devs[piscsi_cur_drive].fs / 512;
 772             DEBUG("[PISCSI] %s Read from BLOCKS %d: %d\n", op_type_names[type], piscsi_cur_drive, (uint32_t)(devs[piscsi_cur_drive].fs / 512));
 773             DEBUG("fs: %lld (%d)\n", devs[piscsi_cur_drive].fs, blox);
 774             return blox;
 775             break;
 776         }
 777         case PISCSI_CMD_GETPART: {
 778             DEBUG("[PISCSI] Get ROM partition %d offset: %.8X\n", rom_cur_partition, rom_partitions[rom_cur_partition]);
 779             return rom_partitions[rom_cur_partition];
 780             break;
 781         }
 782         case PISCSI_CMD_GETPRIO:
 783             DEBUG("[PISCSI] Get partition %d boot priority: %d\n", rom_cur_partition, rom_partition_prio[rom_cur_partition]);
 784             return rom_partition_prio[rom_cur_partition];
 785             break;
 786         case PISCSI_CMD_CHECKFS:
 787             DEBUG("[PISCSI] Get current loaded file system: %.8X\n", filesystems[rom_cur_fs].FS_ID);
 788             return filesystems[rom_cur_fs].FS_ID;
 789         case PISCSI_CMD_FSSIZE:
 790             DEBUG("[PISCSI] Get alloc size of loaded file system: %d\n", filesystems[rom_cur_fs].h_info.alloc_size);
 791             return filesystems[rom_cur_fs].h_info.alloc_size;
 792         default:
 793             DEBUG("[!!!PISCSI] WARN: Unhandled %s register read from %.8X\n", op_type_names[type], addr);
 794             break;
 795     }
 796
 797     return 0;
 798 }