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

   1 /*
   2  *      IDE Emulation Layer for retro-style PIO interfaces
   3  *
   4  *      (c) Copyright Alan Cox, 2015-2019
   5  *
   6  *      IDE-emu is free software: you can redistribute it and/or modify
   7  *      it under the terms of the GNU General Public License as published by
   8  *      the Free Software Foundation, either version 2 of the License, or
   9  *      (at your option) any later version.
  10  *
  11  *      IDE-emu is distributed in the hope that it will be useful,
  12  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *      GNU General Public License for more details.
  15  *
  16  *      You should have received a copy of the GNU General Public License
  17  *      along with IDE-emu.  If not, see <http://www.gnu.org/licenses/>.
  18  */
  19
  20 #include <stdio.h>
  21 #include <stdint.h>
  22 #include <unistd.h>
  23 #include <string.h>
  24 #include <stdlib.h>
  25 #include <errno.h>
  26 #include <time.h>
  27 #include <arpa/inet.h>
  28 #include "../../../config_file/config_file.h"
  29
  30 #include "ide.h"
  31
  32 #define IDE_IDLE        0
  33 #define IDE_CMD         1
  34 #define IDE_DATA_IN     2
  35 #define IDE_DATA_OUT    3
  36
  37 #define DCR_NIEN        2
  38 #define DCR_SRST        4
  39
  40 #define DEVH_HEAD       15
  41 #define DEVH_DEV        16
  42 #define DEVH_LBA        64
  43
  44 #define ERR_AMNF        1
  45 #define ERR_TKNONF      2
  46 #define ERR_ABRT        4
  47 #define ERR_MCR         8
  48 #define ERR_IDNF        16
  49 #define ERR_MC          32
  50 #define ERR_UNC         64
  51
  52 #define ST_ERR          1
  53 #define ST_IDX          2
  54 #define ST_CORR         4
  55 #define ST_DRQ          8
  56 #define ST_DSC          16
  57 #define ST_DF           32
  58 #define ST_DRDY         64
  59 #define ST_BSY          128
  60
  61 #define DCL_SRST        4
  62 #define DCL_NIEN        2
  63
  64 #define IDE_CMD_CALIB           0x10
  65 #define IDE_CMD_READ            0x20
  66 #define IDE_CMD_READ_NR         0x21
  67 #define IDE_CMD_WRITE           0x30
  68 #define IDE_CMD_WRITE_NR        0x31
  69 #define IDE_CMD_VERIFY          0x40
  70 #define IDE_CMD_VERIFY_NR       0x41
  71 #define IDE_CMD_SEEK            0x70
  72 #define IDE_CMD_EDD             0x90
  73 #define IDE_CMD_INTPARAMS       0x91
  74 #define IDE_CMD_IDENTIFY        0xEC
  75 #define IDE_CMD_SETFEATURES     0xEF
  76
  77 const uint8_t ide_magic[9] = {
  78   '1','D','E','D','1','5','C','0',0x00
  79 };
  80
  81 static char *charmap(uint8_t v)
  82 {
  83   static char cbuf[3];
  84   if (v < 32)
  85     sprintf(cbuf, "^%c", '@'+v);
  86   else if (v < 127)
  87     sprintf(cbuf, " %c", v);
  88   else if (v == 127)
  89     sprintf(cbuf, "DL");
  90   else if (v < 160)
  91     sprintf(cbuf, ":%c", '@' + v - 128);
  92   else if (v < 255)
  93     sprintf(cbuf, "~%c", v - 128);
  94   else
  95     sprintf(cbuf, "!D");
  96   return cbuf;
  97 }
  98
  99 static void hexdump(uint8_t *bp)
 100 {
 101   int i,j;
 102   for (i = 0; i < 512; i+= 16) {
 103     for(j = 0; j < 16; j++)
 104       fprintf(stderr, "%02X ", bp[i+j]);
 105     fprintf(stderr, "|");
 106     for(j = 0; j < 16; j++)
 107       fprintf(stderr, "%2s", charmap(bp[i+j]));
 108     fprintf(stderr, "\n");
 109   }
 110 }
 111
 112 /* FIXME: use proper endian convertors! */
 113 static uint16_t le16(uint16_t v)
 114 {
 115   uint8_t *p = (uint8_t *)&v;
 116   return p[0] | (p[1] << 8);
 117 }
 118
 119 static void ide_xlate_errno(struct ide_taskfile *t, int len)
 120 {
 121   t->status |= ST_ERR;
 122   if (len == -1) {
 123     if (errno == EIO)
 124       t->error = ERR_UNC;
 125     else
 126       t->error = ERR_AMNF;
 127   } else
 128     t->error = ERR_AMNF;
 129 }
 130
 131 static void ide_fault(struct ide_drive *d, const char *p)
 132 {
 133   fprintf(stderr, "ide: %s: %d: %s\n", d->controller->name,
 134                         (int)(d - d->controller->drive), p);
 135 }
 136
 137 /* Disk translation */
 138 static off_t xlate_block(struct ide_taskfile *t)
 139 {
 140   struct ide_drive *d = t->drive;
 141   uint16_t cyl;
 142
 143   if (d->controller->lba4 & DEVH_LBA) {
 144 /*    fprintf(stderr, "XLATE LBA %02X:%02X:%02X:%02X\n",
 145       t->lba4, t->lba3, t->lba2, t->lba1);*/
 146     if (d->lba)
 147       return ((d->header_present) ? 2 : 0) + (((t->drive->controller->lba4 & DEVH_HEAD) << 24) | (t->drive->controller->lba3 << 16) | (t->drive->controller->lba2 << 8) | t->drive->controller->lba1);
 148     ide_fault(d, "LBA on non LBA drive");
 149   }
 150
 151   /* Some well known software asks for 0/0/0 when it means 0/0/1. Drives appear
 152      to interpret sector 0 as sector 1 */
 153   if (t->drive->controller->lba1 == 0) {
 154     fprintf(stderr, "[Bug: request for sector offset 0].\n");
 155     t->drive->controller->lba1 = 1;
 156   }
 157   cyl = (t->drive->controller->lba3 << 8) | t->drive->controller->lba2;
 158   /* fprintf(stderr, "(H %d C %d S %d)\n", t->lba4 & DEVH_HEAD, cyl, t->lba1); */
 159   if (t->drive->controller->lba1 == 0 || t->drive->controller->lba1 > d->sectors || t->drive->controller->lba4 >= d->heads || cyl >= d->cylinders) {
 160     return -1;
 161   }
 162   /* Sector 1 is first */
 163   /* Images generally go cylinder/head/sector. This also matters if we ever
 164      implement more advanced geometry setting */
 165   //off_t ret = ((d->header_present) ? 1 : -1) + ((cyl * d->heads) + (t->drive->controller->lba4 & DEVH_HEAD)) * d->sectors + t->drive->controller->lba1;
 166   //printf("Non-LBA xlate block %lX.\n", ret);
 167   //printf("Cyl: %d Heads: %d Sectors: %d\n", cyl, d->heads, d->sectors);
 168   //printf("LBA1: %.2X LBA2: %.2X LBA3: %.2X LBA4: %.2X\n", t->drive->controller->lba1, t->drive->controller->lba2, t->drive->controller->lba3, t->drive->controller->lba4);
 169
 170   return ((d->header_present) ? 1 : -1) + ((cyl * d->heads) + (t->drive->controller->lba4 & DEVH_HEAD)) * d->sectors + t->drive->controller->lba1;
 171 }
 172
 173 /* Indicate the drive is ready */
 174 static void ready(struct ide_taskfile *tf)
 175 {
 176   tf->status &= ~(ST_BSY|ST_DRQ);
 177   tf->status |= ST_DRDY;
 178   tf->drive->state = IDE_IDLE;
 179 }
 180
 181 /* Return to idle state, completing a command */
 182 static void completed(struct ide_taskfile *tf)
 183 {
 184   ready(tf);
 185   tf->drive->intrq = 1;
 186 }
 187
 188 static void drive_failed(struct ide_taskfile *tf)
 189 {
 190   tf->status |= ST_ERR;
 191   tf->error = ERR_IDNF;
 192   ready(tf);
 193 }
 194
 195 static void data_in_state(struct ide_taskfile *tf)
 196 {
 197   struct ide_drive *d = tf->drive;
 198   d->state = IDE_DATA_IN;
 199   d->dptr = d->data + 512;
 200   /* We don't clear DRDY here, drives may well accept a command at this
 201      point and at least one firmware for RC2014 assumes this */
 202   tf->status &= ~ST_BSY;
 203   tf->status |= ST_DRQ;
 204   d->intrq = 1;                 /* Double check */
 205 }
 206
 207 static void data_out_state(struct ide_taskfile *tf)
 208 {
 209   struct ide_drive *d = tf->drive;
 210   d->state = IDE_DATA_OUT;
 211   d->dptr = d->data;
 212   tf->status &= ~ (ST_BSY|ST_DRDY);
 213   tf->status |= ST_DRQ;
 214   d->intrq = 1;                 /* Double check */
 215 }
 216
 217 static void edd_setup(struct ide_taskfile *tf)
 218 {
 219   tf->error = 0x01;             /* All good */
 220   tf->drive->controller->lba1 = 0x01;           /* EDD always updates drive 0 */
 221   tf->drive->controller->lba2 = 0x00;
 222   tf->drive->controller->lba3 = 0x00;
 223   tf->drive->controller->lba4 = 0x00;
 224   tf->count = 0x01;
 225   ready(tf);
 226 }
 227
 228 void ide_reset(struct ide_controller *c)
 229 {
 230   if (c->drive[0].present) {
 231     edd_setup(&c->drive[0].taskfile);
 232     /* A drive could clear busy then set DRDY up to 2 minutes later if its
 233        mindnumbingly slow to start up ! We don't emulate any of that */
 234     c->drive[0].taskfile.status = ST_DRDY;
 235     c->drive[0].eightbit = 0;
 236   }
 237   if (c->drive[1].present) {
 238     edd_setup(&c->drive[1].taskfile);
 239     c->drive[1].taskfile.status = ST_DRDY;
 240     c->drive[1].eightbit = 0;
 241   }
 242   if (c->selected != 0) {
 243   }
 244   c->selected = 0;
 245 }
 246
 247 void ide_reset_begin(struct ide_controller *c)
 248 {
 249   if (c->drive[0].present)
 250     c->drive[0].taskfile.status |= ST_BSY;
 251   if (c->drive[1].present)
 252     c->drive[1].taskfile.status |= ST_BSY;
 253   /* Ought to be a time delay relative to reset or power on */
 254   ide_reset(c);
 255 }
 256
 257 static void ide_srst_begin(struct ide_controller *c)
 258 {
 259   ide_reset(c);
 260   if (c->drive[0].present)
 261     c->drive[0].taskfile.status |= ST_BSY;
 262   if (c->drive[1].present)
 263     c->drive[1].taskfile.status |= ST_BSY;
 264 }
 265
 266 static void ide_srst_end(struct ide_controller *c)
 267 {
 268   /* Could be time delays here */
 269   ready(&c->drive[0].taskfile);
 270   ready(&c->drive[1].taskfile);
 271 }
 272
 273 static void cmd_edd_complete(struct ide_taskfile *tf)
 274 {
 275   struct ide_controller *c = tf->drive->controller;
 276   if (c->drive[0].present)
 277     edd_setup(&c->drive[0].taskfile);
 278   if (c->drive[1].present)
 279     edd_setup(&c->drive[1].taskfile);
 280   c->selected = 0;
 281 }
 282
 283 static void cmd_identify_complete(struct ide_taskfile *tf)
 284 {
 285   struct ide_drive *d = tf->drive;
 286   memcpy(d->data, d->identify, 512);
 287   data_in_state(tf);
 288   /* Arrange to copy just the identify buffer */
 289   d->dptr = d->data;
 290   d->length = 1;
 291 }
 292
 293 static void cmd_initparam_complete(struct ide_taskfile *tf)
 294 {
 295   struct ide_drive *d = tf->drive;
 296   /* We only support the current mapping */
 297   if (tf->count != d->sectors || (tf->drive->controller->lba4 & DEVH_HEAD) + 1 != d->heads) {
 298     tf->status |= ST_ERR;
 299     tf->error |= ERR_ABRT;
 300     tf->drive->failed = 1;              /* Report ID NF until fixed */
 301 /*    fprintf(stderr, "geo is %d %d, asked for %d %d\n",
 302       d->sectors, d->heads, tf->count, (tf->lba4 & DEVH_HEAD) + 1); */
 303     ide_fault(d, "invalid geometry");
 304   } else if (tf->drive->failed == 1)
 305     tf->drive->failed = 0;              /* Valid translation */
 306   completed(tf);
 307 }
 308
 309 static void cmd_readsectors_complete(struct ide_taskfile *tf)
 310 {
 311   struct ide_drive *d = tf->drive;
 312   /* Move to data xfer */
 313   if (d->failed) {
 314     drive_failed(tf);
 315     return;
 316   }
 317   d->offset = xlate_block(tf);
 318   /* DRDY is not guaranteed here but at least one buggy RC2014 firmware
 319      expects it */
 320   tf->status |= ST_DRQ | ST_DSC | ST_DRDY;
 321   tf->status &= ~ST_BSY;
 322   /* 0 = 256 sectors */
 323   d->length = tf->count ? tf->count : 256;
 324   /* fprintf(stderr, "READ %d SECTORS @ %ld\n", d->length, d->offset); */
 325   if (d->offset == -1 ||  lseek(d->fd, 512 * d->offset, SEEK_SET) == -1) {
 326     tf->status |= ST_ERR;
 327     tf->status &= ~ST_DSC;
 328     tf->error |= ERR_IDNF;
 329     /* return null data */
 330     completed(tf);
 331     return;
 332   }
 333   /* do the xfer */
 334   data_in_state(tf);
 335 }
 336
 337 static void cmd_verifysectors_complete(struct ide_taskfile *tf)
 338 {
 339   struct ide_drive *d = tf->drive;
 340   /* Move to data xfer */
 341   if (d->failed) {
 342     drive_failed(tf);
 343     return;
 344   }
 345   d->offset = xlate_block(tf);
 346   /* 0 = 256 sectors */
 347   d->length = tf->count ? tf->count : 256;
 348   if (d->offset == -1 || lseek(d->fd, 512 * (d->offset + d->length - 1), SEEK_SET) == -1) {
 349     tf->status &= ~ST_DSC;
 350     tf->status |= ST_ERR;
 351     tf->error |= ERR_IDNF;
 352   }
 353   tf->status |= ST_DSC;
 354   completed(tf);
 355 }
 356
 357 static void cmd_recalibrate_complete(struct ide_taskfile *tf)
 358 {
 359   struct ide_drive *d = tf->drive;
 360   if (d->failed)
 361     drive_failed(tf);
 362   if (d->offset == -1 || xlate_block(tf) != 0L) {
 363     tf->status &= ~ST_DSC;
 364     tf->status |= ST_ERR;
 365     tf->error |= ERR_ABRT;
 366   }
 367   tf->status |= ST_DSC;
 368   completed(tf);
 369 }
 370
 371 static void cmd_seek_complete(struct ide_taskfile *tf)
 372 {
 373   struct ide_drive *d = tf->drive;
 374   if (d->failed)
 375     drive_failed(tf);
 376   d->offset = xlate_block(tf);
 377   if (d->offset == -1 || lseek(d->fd, 512 * d->offset, SEEK_SET) == -1) {
 378     tf->status &= ~ST_DSC;
 379     tf->status |= ST_ERR;
 380     tf->error |= ERR_IDNF;
 381   }
 382   tf->status |= ST_DSC;
 383   completed(tf);
 384 }
 385
 386 static void cmd_setfeatures_complete(struct ide_taskfile *tf)
 387 {
 388   struct ide_drive *d = tf->drive;
 389   switch(tf->feature) {
 390     case 0x01:
 391       d->eightbit = 1;
 392       break;
 393     case 0x03:
 394       if ((tf->count & 0xF0) >= 0x20) {
 395         tf->status |= ST_ERR;
 396         tf->error |= ERR_ABRT;
 397       }
 398       /* Silently accept PIO mode settings */
 399       break;
 400     case 0x81:
 401       d->eightbit = 0;
 402       break;
 403     default:
 404       tf->status |= ST_ERR;
 405       tf->error |= ERR_ABRT;
 406   }
 407   completed(tf);
 408 }
 409
 410 static void cmd_writesectors_complete(struct ide_taskfile *tf)
 411 {
 412   struct ide_drive *d = tf->drive;
 413   /* Move to data xfer */
 414   if (d->failed) {
 415     drive_failed(tf);
 416     return;
 417   }
 418   d->offset = xlate_block(tf);
 419   tf->status |= ST_DRQ;
 420   /* 0 = 256 sectors */
 421   d->length = tf->count ? tf->count : 256;
 422 /*  fprintf(stderr, "WRITE %d SECTORS @ %ld\n", d->length, d->offset); */
 423   if (d->offset == -1 ||  lseek(d->fd, 512 * d->offset, SEEK_SET) == -1) {
 424     tf->status |= ST_ERR;
 425     tf->error |= ERR_IDNF;
 426     tf->status &= ~ST_DSC;
 427     /* return null data */
 428     completed(tf);
 429     return;
 430   }
 431   /* do the xfer */
 432   data_out_state(tf);
 433 }
 434
 435 static void ide_set_error(struct ide_drive *d)
 436 {
 437   d->controller->lba4 &= ~DEVH_HEAD;
 438
 439   if (d->controller->lba4 & DEVH_LBA) {
 440     d->controller->lba1 = d->offset & 0xFF;
 441     d->controller->lba2 = (d->offset >> 8) & 0xFF;
 442     d->controller->lba3 = (d->offset >> 16) & 0xFF;
 443     d->controller->lba4 |= (d->offset >> 24) & DEVH_HEAD;
 444   } else {
 445     d->controller->lba1 = d->offset % d->sectors + 1;
 446     d->offset /= d->sectors;
 447     d->controller->lba4 |= d->offset / (d->cylinders * d->sectors);
 448     d->offset %= (d->cylinders * d->sectors);
 449     d->controller->lba2 = d->offset & 0xFF;
 450     d->controller->lba3 = (d->offset >> 8) & 0xFF;
 451   }
 452   d->taskfile.count = d->length;
 453   d->taskfile.status |= ST_ERR;
 454   d->state = IDE_IDLE;
 455   completed(&d->taskfile);
 456 }
 457
 458 static int ide_read_sector(struct ide_drive *d)
 459 {
 460   int len;
 461
 462   d->dptr = d->data;
 463   if ((len = read(d->fd, d->data, 512)) != 512) {
 464     perror("ide_read_sector");
 465     d->taskfile.status |= ST_ERR;
 466     d->taskfile.status &= ~ST_DSC;
 467     ide_xlate_errno(&d->taskfile, len);
 468     return -1;
 469   }
 470 //  hexdump(d->data);
 471   d->offset += 512;
 472   return 0;
 473 }
 474
 475 static int ide_write_sector(struct ide_drive *d)
 476 {
 477   int len;
 478
 479   d->dptr = d->data;
 480   if ((len = write(d->fd, d->data, 512)) != 512) {
 481     d->taskfile.status |= ST_ERR;
 482     d->taskfile.status &= ~ST_DSC;
 483     ide_xlate_errno(&d->taskfile, len);
 484     return -1;
 485   }
 486 //  hexdump(d->data);
 487   d->offset += 512;
 488   return 0;
 489 }
 490
 491 static uint16_t ide_data_in(struct ide_drive *d, int len)
 492 {
 493   uint16_t v;
 494   if (d->state == IDE_DATA_IN) {
 495     if (d->dptr == d->data + 512) {
 496       if (ide_read_sector(d) < 0) {
 497         ide_set_error(d);       /* Set the LBA or CHS etc */
 498         return 0xFFFF;          /* and error bits set by read_sector */
 499       }
 500     }
 501     v = *d->dptr;
 502     if (!d->eightbit) {
 503       if (len == 2)
 504         v |= (d->dptr[1] << 8);
 505       d->dptr+=2;
 506     } else
 507       d->dptr++;
 508     d->taskfile.data = v;
 509     if (d->dptr == d->data + 512) {
 510       d->length--;
 511       d->intrq = 1;             /* we don't yet emulate multimode */
 512       if (d->length == 0) {
 513         d->state = IDE_IDLE;
 514         completed(&d->taskfile);
 515       }
 516     }
 517   } else
 518     ide_fault(d, "bad data read");
 519
 520   if (len == 1)
 521     return d->taskfile.data & 0xFF;
 522   return d->taskfile.data;
 523 }
 524
 525 static void ide_data_out(struct ide_drive *d, uint16_t v, int len)
 526 {
 527   if (d->state != IDE_DATA_OUT) {
 528     ide_fault(d, "bad data write");
 529     d->taskfile.data = v;
 530   } else {
 531     if (d->eightbit)
 532       v &= 0xFF;
 533     *d->dptr++ = v;
 534     d->taskfile.data = v;
 535     if (!d->eightbit) {
 536       *d->dptr++ = v >> 8;
 537       d->taskfile.data = v >> 8;
 538     }
 539     if (d->dptr == d->data + 512) {
 540       if (ide_write_sector(d) < 0) {
 541         ide_set_error(d);
 542         return;
 543       }
 544       d->length--;
 545       d->intrq = 1;
 546       if (d->length == 0) {
 547         d->state = IDE_IDLE;
 548         d->taskfile.status |= ST_DSC;
 549         completed(&d->taskfile);
 550       }
 551     }
 552   }
 553 }
 554
 555 static void ide_issue_command(struct ide_taskfile *t)
 556 {
 557   t->status &= ~(ST_ERR|ST_DRDY);
 558   t->status |= ST_BSY;
 559   t->error = 0;
 560   t->drive->state = IDE_CMD;
 561
 562   /* We could complete with delays but don't do so yet */
 563   switch(t->command) {
 564     case IDE_CMD_EDD:   /* 0x90 */
 565       cmd_edd_complete(t);
 566       break;
 567     case IDE_CMD_IDENTIFY:      /* 0xEC */
 568       cmd_identify_complete(t);
 569       break;
 570     case IDE_CMD_INTPARAMS:     /* 0x91 */
 571       cmd_initparam_complete(t);
 572       break;
 573     case IDE_CMD_READ:          /* 0x20 */
 574     case IDE_CMD_READ_NR:       /* 0x21 */
 575       cmd_readsectors_complete(t);
 576       break;
 577     case IDE_CMD_SETFEATURES:   /* 0xEF */
 578       cmd_setfeatures_complete(t);
 579       break;
 580     case IDE_CMD_VERIFY:        /* 0x40 */
 581     case IDE_CMD_VERIFY_NR:     /* 0x41 */
 582       cmd_verifysectors_complete(t);
 583       break;
 584     case IDE_CMD_WRITE:         /* 0x30 */
 585     case IDE_CMD_WRITE_NR:      /* 0x31 */
 586       cmd_writesectors_complete(t);
 587       break;
 588     default:
 589       if ((t->command & 0xF0) == IDE_CMD_CALIB) /* 1x */
 590         cmd_recalibrate_complete(t);
 591       else if ((t->command & 0xF0) == IDE_CMD_SEEK) /* 7x */
 592         cmd_seek_complete(t);
 593       else {
 594         /* Unknown */
 595         t->status |= ST_ERR;
 596         t->error |= ERR_ABRT;
 597         completed(t);
 598       }
 599   }
 600 }
 601
 602 /*
 603  *      8bit IDE controller emulation
 604  */
 605
 606 uint8_t ide_read8(struct ide_controller *c, uint8_t r)
 607 {
 608   struct ide_drive *d = &c->drive[c->selected];
 609   struct ide_taskfile *t = &d->taskfile;
 610   switch(r) {
 611     case ide_data:
 612       return ide_data_in(d, 1);
 613     case ide_error_r:
 614       return t->error;
 615     case ide_sec_count:
 616       return t->count;
 617     case ide_lba_low:
 618       return c->lba1;
 619     case ide_lba_mid:
 620       return c->lba2;
 621     case ide_lba_hi:
 622       return c->lba3;
 623     case ide_lba_top:
 624       return c->lba4 | ((c->selected) ? 0x10 : 0x00);
 625     case ide_status_r:
 626       d->intrq = 0;             /* Acked */
 627     case ide_altst_r:
 628       return t->status;
 629     default:
 630       ide_fault(d, "bogus register");
 631       return 0xFF;
 632   }
 633 }
 634
 635 void ide_write8(struct ide_controller *c, uint8_t r, uint8_t v)
 636 {
 637   struct ide_drive *d = &c->drive[c->selected];
 638   struct ide_taskfile *t = &d->taskfile;
 639
 640   if (r != ide_devctrl_w) {
 641     if (t->status & ST_BSY) {
 642       ide_fault(d, "command written while busy");
 643       return;
 644     }
 645     /* Not clear this is the right emulation */
 646     if (d->present == 0 && r != ide_lba_top) {
 647       ide_fault(d, "not present");
 648       return;
 649     }
 650   }
 651
 652   uint8_t ve;
 653
 654   switch(r) {
 655     case ide_data:
 656       ide_data_out(d, v, 1);
 657       break;
 658     case ide_feature_w:
 659       t->feature = v;
 660       break;
 661     case ide_sec_count:
 662       t->count = v;
 663       break;
 664     case ide_lba_low:
 665       c->lba1 = v;
 666       break;
 667     case ide_lba_mid:
 668       c->lba2 = v;
 669       break;
 670     case ide_lba_hi:
 671       c->lba3 = v;
 672       break;
 673     case ide_lba_top:
 674       c->selected = (v & DEVH_DEV) ? 1 : 0;
 675       c->lba4 = v & (DEVH_HEAD|/*DEVH_DEV|*/DEVH_LBA);
 676       break;
 677     case ide_command_w:
 678       t->command = v;
 679       ide_issue_command(t);
 680       break;
 681     case ide_devctrl_w:
 682       /* ATA: "When the Device Control register is written, both devices
 683          respond to the write regardless of which device is selected" */
 684       if ((v ^ t->devctrl) & DCL_SRST) {
 685         if (v & DCL_SRST)
 686           ide_srst_begin(c);
 687         else
 688           ide_srst_end(c);
 689       }
 690       c->drive[0].taskfile.devctrl = v; /* Check versus real h/w does this end up cleared */
 691       c->drive[1].taskfile.devctrl = v;
 692       break;
 693   }
 694 }
 695
 696 /*
 697  *      16bit IDE controller emulation
 698  */
 699
 700 uint16_t ide_read16(struct ide_controller *c, uint8_t r)
 701 {
 702   struct ide_drive *d = &c->drive[c->selected];
 703   if (r == ide_data)
 704     return htons(ide_data_in(d,2));
 705   return ide_read8(c, r);
 706 }
 707
 708 void ide_write16(struct ide_controller *c, uint8_t r, uint16_t v)
 709 {
 710   struct ide_drive *d = &c->drive[c->selected];
 711   struct ide_taskfile *t = &d->taskfile;
 712
 713   if (r != ide_devctrl_w && (t->status & ST_BSY)) {
 714     ide_fault(d, "command written while busy");
 715     return;
 716   }
 717   if (r == ide_data)
 718     ide_data_out(d, ntohs(v), 2);
 719   else
 720     ide_write8(c, r, v);
 721 }
 722
 723 /*
 724  *      Allocate a new IDE controller emulation
 725  */
 726 struct ide_controller *ide_allocate(const char *name)
 727 {
 728   struct ide_controller *c = calloc(1, sizeof(*c));
 729   if (c == NULL)
 730     return NULL;
 731   c->name = strdup(name);
 732   if (c->name == NULL) {
 733     free(c);
 734     return NULL;
 735   }
 736   c->drive[0].controller = c;
 737   c->drive[1].controller = c;
 738   c->drive[0].taskfile.drive = &c->drive[0];
 739   c->drive[1].taskfile.drive = &c->drive[1];
 740   return c;
 741 }
 742
 743 /*
 744  *      Attach a file to a device on the controller
 745  */
 746 int ide_attach(struct ide_controller *c, int drive, int fd)
 747 {
 748   struct ide_drive *d = &c->drive[drive];
 749   if (d->present) {
 750     ide_fault(d, "double attach");
 751     return -1;
 752   }
 753   d->fd = fd;
 754   if (read(d->fd, d->data, 512) != 512 ||
 755       read(d->fd, d->identify, 512) != 512) {
 756     ide_fault(d, "i/o error on attach");
 757     return -1;
 758   }
 759   if (memcmp(d->data, ide_magic, 8)) {
 760     ide_fault(d, "bad magic");
 761     return -1;
 762   }
 763   d->fd = fd;
 764   d->present = 1;
 765   d->heads = d->identify[3];
 766   d->sectors = d->identify[6];
 767   d->cylinders = le16(d->identify[1]);
 768   d->header_present = 1;
 769   if (d->identify[49] & le16(1 << 9))
 770     d->lba = 1;
 771   else
 772     d->lba = 0;
 773   return 0;
 774 }
 775
 776 // Attach a headerless HDD image to the controller
 777 int ide_attach_hdf(struct ide_controller *c, int drive, int fd)
 778 {
 779   struct ide_drive *d = &c->drive[drive];
 780   if (d->present) {
 781     printf("[IDE/HDL] Drive already attached.\n");
 782     return -1;
 783   }
 784
 785   d->fd = fd;
 786   d->present = 1;
 787   d->lba = 0;
 788
 789   d->heads = 255;
 790   d->sectors = 63;
 791   d->header_present = 0;
 792
 793   uint64_t file_size = lseek(fd, 0, SEEK_END);
 794   lseek(fd, 1024, SEEK_SET);
 795
 796   if (file_size < 500 * SIZE_MEGA) {
 797     d->heads = 16;
 798   }
 799   else if (file_size < 1000 * SIZE_MEGA) {
 800     d->heads = 32;
 801   }
 802   else if (file_size < 2000 * SIZE_MEGA) {
 803     d->heads = 64;
 804   }
 805   else if (file_size < (uint64_t)4000 * SIZE_MEGA) {
 806     d->heads = 128;
 807   }
 808
 809   d->cylinders = (file_size / 512) / (d->sectors * d->heads);
 810
 811   printf("[IDE/HDL] Cylinders: %d Heads: %d Sectors: %d\n", d->cylinders, d->heads, d->sectors);
 812
 813   if (file_size >= 4 * 1000 * 1000) {
 814     d->lba = 1;
 815   }
 816
 817   ide_make_ident(d->cylinders, d->heads, d->sectors, "PISTORM HDD IMAGE v0.1", d->identify);
 818
 819   return 0;
 820 }
 821
 822 /*
 823  *      Detach an IDE device from the interface (not hot pluggable)
 824  */
 825 void ide_detach(struct ide_drive *d)
 826 {
 827   close(d->fd);
 828   d->fd = -1;
 829   d->present = 0;
 830 }
 831
 832 /*
 833  *      Free up and release and IDE controller
 834  */
 835 void ide_free(struct ide_controller *c)
 836 {
 837   if (c->drive[0].present)
 838     ide_detach(&c->drive[0]);
 839   if (c->drive[1].present)
 840     ide_detach(&c->drive[1]);
 841   free((void *)c->name);
 842   free(c);
 843 }
 844
 845 /*
 846  *      Emulation interface for an 8bit controller using latches on the
 847  *      data register
 848  */
 849 uint8_t ide_read_latched(struct ide_controller *c, uint8_t reg)
 850 {
 851   uint16_t v;
 852   if (reg == ide_data_latch)
 853     return c->data_latch;
 854   v = ide_read16(c, reg);
 855   if (reg == ide_data) {
 856     c->data_latch = v >> 8;
 857     v &= 0xFF;
 858   }
 859   return v;
 860 }
 861
 862 void ide_write_latched(struct ide_controller *c, uint8_t reg, uint8_t v)
 863 {
 864   uint16_t d = v;
 865
 866   if (reg == ide_data_latch) {
 867     c->data_latch = v;
 868     return;
 869   }
 870   if (reg == ide_data)
 871     d |=  (c->data_latch << 8);
 872   ide_write16(c, reg, d);
 873 }
 874
 875 static void make_ascii(uint16_t *p, const char *t, int len)
 876 {
 877   int i;
 878   char *d = (char *)p;
 879   strncpy(d, t, len);
 880
 881   for (i = 0; i < len; i += 2) {
 882     char c = *d;
 883     *d = d[1];
 884     d[1] = c;
 885     d += 2;
 886   }
 887 }
 888
 889 static void make_serial(uint16_t *p)
 890 {
 891   char buf[21];
 892   srand(getpid()^time(NULL));
 893   snprintf(buf, 21, "%08d%08d%04d", rand(), rand(), rand());
 894   make_ascii(p, buf, 20);
 895 }
 896
 897 int ide_make_ident(uint16_t c, uint8_t h, uint8_t s, char *name, uint16_t *target)
 898 {
 899   uint16_t *ident = target;
 900   uint32_t sectors;
 901
 902   memset(ident, 0, 512);
 903   memcpy(ident, ide_magic, 8);
 904
 905   memset(ident, 0, 8);
 906   ident[0] = le16((1 << 15) | (1 << 6));        /* Non removable */
 907   make_serial(ident + 10);
 908   ident[47] = 0; /* no read multi for now */
 909   ident[51] = le16(240 /* PIO2 */ << 8);        /* PIO cycle time */
 910   ident[53] = le16(1);          /* Geometry words are valid */
 911
 912   make_ascii(ident + 23, "A001.001", 8);
 913   make_ascii(ident + 27, name, 40);
 914   ident[49] = le16(1 << 9); /* LBA */
 915
 916   ident[1] = le16(c);
 917   ident[3] = le16(h);
 918   ident[6] = le16(s);
 919   ident[54] = ident[1];
 920   ident[55] = ident[3];
 921   ident[56] = ident[6];
 922   sectors = c * h * s;
 923   ident[57] = le16(sectors & 0xFFFF);
 924   ident[58] = le16(sectors >> 16);
 925   ident[60] = ident[57];
 926   ident[61] = ident[58];
 927
 928   return 0;
 929 }
 930
 931 int ide_make_drive(uint8_t type, int fd)
 932 {
 933   uint8_t s, h;
 934   uint16_t c;
 935   uint32_t sectors;
 936   uint16_t ident[256];
 937
 938   if (type < 1 || type > MAX_DRIVE_TYPE)
 939     return -2;
 940
 941   memset(ident, 0, 512);
 942   memcpy(ident, ide_magic, 8);
 943   if (write(fd, ident, 512) != 512)
 944     return -1;
 945
 946   memset(ident, 0, 8);
 947   ident[0] = le16((1 << 15) | (1 << 6));        /* Non removable */
 948   make_serial(ident + 10);
 949   ident[47] = 0; /* no read multi for now */
 950   ident[51] = le16(240 /* PIO2 */ << 8);        /* PIO cycle time */
 951   ident[53] = le16(1);          /* Geometry words are valid */
 952
 953   switch(type) {
 954     case ACME_ROADRUNNER:
 955       /* 504MB drive with LBA support */
 956       c = 1024;
 957       h = 16;
 958       s = 63;
 959       make_ascii(ident + 23, "A001.001", 8);
 960       make_ascii(ident + 27, "ACME ROADRUNNER v0.1", 40);
 961       ident[49] = le16(1 << 9); /* LBA */
 962       break;
 963     case ACME_ULTRASONICUS:
 964       /* 40MB drive with LBA support */
 965       c = 977;
 966       h = 5;
 967       s = 16;
 968       ident[49] = le16(1 << 9); /* LBA */
 969       make_ascii(ident + 23, "A001.001", 8);
 970       make_ascii(ident + 27, "ACME ULTRASONICUS AD INFINITUM v0.1", 40);
 971       break;
 972     case ACME_NEMESIS:
 973       /* 20MB drive with LBA support */
 974       c = 615;
 975       h = 4;
 976       s = 16;
 977       ident[49] = le16(1 << 9); /* LBA */
 978       make_ascii(ident + 23, "A001.001", 8);
 979       make_ascii(ident + 27, "ACME NEMESIS RIDICULII v0.1", 40);
 980       break;
 981     case ACME_COYOTE:
 982       /* 20MB drive without LBA support */
 983       c = 615;
 984       h = 4;
 985       s = 16;
 986       make_ascii(ident + 23, "A001.001", 8);
 987       make_ascii(ident + 27, "ACME COYOTE v0.1", 40);
 988       break;
 989     case ACME_ACCELLERATTI:
 990       c = 1024;
 991       h = 16;
 992       s = 16;
 993       ident[49] = le16(1 << 9); /* LBA */
 994       make_ascii(ident + 23, "A001.001", 8);
 995       make_ascii(ident + 27, "ACME ACCELLERATTI INCREDIBILUS v0.1", 40);
 996       break;
 997     case ACME_ZIPPIBUS:
 998       c = 1024;
 999       h = 16;
1000       s = 32;
1001       ident[49] = le16(1 << 9); /* LBA */
1002       make_ascii(ident + 23, "A001.001", 8);
1003       make_ascii(ident + 27, "ACME ZIPPIBUS v0.1", 40);
1004       break;
1005   }
1006   ident[1] = le16(c);
1007   ident[3] = le16(h);
1008   ident[6] = le16(s);
1009   ident[54] = ident[1];
1010   ident[55] = ident[3];
1011   ident[56] = ident[6];
1012   sectors = c * h * s;
1013   ident[57] = le16(sectors & 0xFFFF);
1014   ident[58] = le16(sectors >> 16);
1015   ident[60] = ident[57];
1016   ident[61] = ident[58];
1017   if (write(fd, ident, 512) != 512)
1018     return -1;
1019
1020   memset(ident, 0xE5, 512);
1021   while(sectors--)
1022     if (write(fd, ident, 512) != 512)
1023       return -1;
1024   return 0;
1025 }