palacios/src/devices/ramdisk.c

   1 /*
   2  * Zheng Cui
   3  * cuizheng@cs.unm.edu
   4  * July 2008
   5  */
   6
   7 #include <devices/ramdisk.h>
   8 #include <palacios/vmm.h>
   9 #include <devices/cdrom.h>
  10 #include <devices/ide.h>
  11 #include <devices/atapi.h>
  12
  13 #ifndef DEBUG_RAMDISK
  14 #undef PrintDebug
  15 #define PrintDebug(_f, _a...)
  16 #endif
  17
  18
  19 /*
  20  * Data type definitions
  21  *
  22  */
  23 #define INDEX_PULSE_CYCLE 10
  24
  25
  26
  27
  28 #define INTR_REASON_BIT_ERR           0x01
  29 #define UNABLE_FIND_TAT_CHANNEL_ERR   0x02
  30 #define DRQ_ERR                       0x03
  31 #define READ_BUF_GT_512               0x04
  32
  33
  34
  35 #define PRI_DATA_PORT         0x1f0
  36 #define PRI_FEATURES_PORT     0x1f1
  37 #define PRI_SECT_CNT_PORT     0x1f2
  38 #define PRI_SECT_ADDR1_PORT   0x1f3
  39 #define PRI_SECT_ADDR2_PORT   0x1f4
  40 #define PRI_SECT_ADDR3_PORT   0x1f5
  41 #define PRI_DRV_SEL_PORT      0x1f6
  42 #define PRI_CMD_PORT          0x1f7
  43 #define PRI_CTRL_PORT         0x3f6
  44 #define PRI_ADDR_REG_PORT     0x3f7
  45
  46 #define SEC_DATA_PORT         0x170
  47 #define SEC_FEATURES_PORT     0x171
  48 #define SEC_SECT_CNT_PORT     0x172
  49 #define SEC_SECT_ADDR1_PORT   0x173
  50 #define SEC_SECT_ADDR2_PORT   0x174
  51 #define SEC_SECT_ADDR3_PORT   0x175
  52 #define SEC_DRV_SEL_PORT      0x176
  53 #define SEC_CMD_PORT          0x177
  54 #define SEC_CTRL_PORT         0x376
  55 #define SEC_ADDR_REG_PORT     0x377
  56
  57
  58
  59
  60
  61 #define PACKET_SIZE 12
  62
  63
  64
  65 // FLAT MODE
  66 // Open a image. Returns non-negative if successful.
  67 //int open (const char* pathname);
  68
  69 // Open an image with specific flags. Returns non-negative if successful.
  70 int rd_open (const char* pathname, int flags);
  71
  72 // Close the image.
  73 void rd_close ();
  74
  75 // Position ourselves. Return the resulting offset from the
  76 // beginning of the file.
  77 off_t rd_lseek (off_t offset, int whence);
  78
  79 // Read count bytes to the buffer buf. Return the number of
  80 // bytes read (count).
  81 ssize_t rd_read (void* buf, size_t count);
  82
  83 // Write count bytes from buf. Return the number of bytes
  84 // written (count).
  85 ssize_t rd_write (const void* buf, size_t count);
  86
  87
  88
  89
  90
  91 /*
  92  * Debug facilities
  93  */
  94
  95 #define ATA_DETECT                     0xf0 //0X3E8
  96 #define ATA_RESET                      0xf1 //0X3E9
  97 #define ATA_CMD_DATA_IN                0xf2 //0X3EA
  98 #define ATA_CMD_DATA_OUT               0xf3 //0X3EB
  99 #define ATA_CMD_PACKET                 0xf4 //0X3EC
 100 #define ATAPI_GET_SENSE                0xf5 //0X3ED
 101 #define ATAPI_IS_READY                 0xf6 //0X3EE
 102 #define ATAPI_IS_CDROM                 0xf7 //0X3EF
 103
 104 #define CDEMU_INIT                     0xf8 //0X2E8
 105 #define CDEMU_ISACTIVE                 0xf9 //0X2E9
 106 #define CDEMU_EMULATED_DRIVE           0xfa //0X2EA
 107 #define CDROM_BOOT                     0xfb //0X2EB
 108
 109
 110 #define HARD_DRIVE_POST                0xfc //0X2EC
 111
 112
 113 #define ATA_DEVICE_NO                  0xfd //0X2ED
 114 #define ATA_DEVICE_TYPE                0xfe //0X2ED
 115
 116 #define INT13_HARDDISK                 0xff //0x2ef
 117 #define INT13_CDROM                    0xe0 //0x2f8
 118 #define INT13_CDEMU                    0xe1 //0x2f9
 119 #define INT13_ELTORITO                 0xe2 //0x2fa
 120 #define INT13_DISKETTE_FUNCTION        0xe3 //0x2fb
 121
 122
 123
 124
 125 static const char cdrom_str[] = "CD-ROM";
 126 static const char harddisk_str[] = "HARDDISK";
 127 static const char none_str[] = "NONE";
 128
 129
 130 static inline const char * device_type_to_str(device_type_t type) {
 131   switch (type) {
 132   case IDE_DISK:
 133     return harddisk_str;
 134   case IDE_CDROM:
 135     return cdrom_str;
 136   case IDE_NONE:
 137     return none_str;
 138   default:
 139     return NULL;
 140   }
 141 }
 142
 143
 144 static inline void write_features(struct channel_t * channel, uchar_t value) {
 145   channel->drives[0].controller.features = value;
 146   channel->drives[1].controller.features = value;
 147 }
 148
 149
 150 static inline void write_sector_count(struct channel_t * channel, uchar_t value) {
 151   channel->drives[0].controller.sector_count = value;
 152   channel->drives[1].controller.sector_count = value;
 153 }
 154
 155 static inline void write_sector_number(struct channel_t * channel, uchar_t value) {
 156   channel->drives[0].controller.sector_no = value;
 157   channel->drives[1].controller.sector_no = value;
 158 }
 159
 160
 161 static inline void write_cylinder_low(struct channel_t * channel, uchar_t value) {
 162   channel->drives[0].controller.cylinder_no &= 0xff00;
 163   channel->drives[0].controller.cylinder_no |= value;
 164   channel->drives[1].controller.cylinder_no &= 0xff00;
 165   channel->drives[1].controller.cylinder_no |= value;
 166 }
 167
 168 static inline void write_cylinder_high(struct channel_t * channel, uchar_t value) {
 169   ushort_t val2 = value;
 170   val2 = val2 << 8;
 171   channel->drives[0].controller.cylinder_no &= 0x00ff;
 172   channel->drives[0].controller.cylinder_no |= (val2 & 0xff00);
 173
 174   channel->drives[1].controller.cylinder_no &= 0x00ff;
 175   channel->drives[1].controller.cylinder_no |= (val2 & 0xff00);
 176 }
 177
 178 static inline void write_head_no(struct channel_t * channel, uchar_t value) {
 179   channel->drives[0].controller.head_no = value;
 180   channel->drives[1].controller.head_no = value;
 181 }
 182
 183 static inline void write_lba_mode(struct channel_t * channel, uchar_t value) {
 184   channel->drives[0].controller.lba_mode = value;
 185   channel->drives[1].controller.lba_mode = value;
 186 }
 187
 188
 189 static inline uint_t get_channel_no(struct ramdisk_t * ramdisk, struct channel_t * channel) {
 190   return (((uchar_t *)channel - (uchar_t *)(ramdisk->channels)) / sizeof(struct channel_t));
 191 }
 192
 193 static inline uint_t get_drive_no(struct channel_t * channel, struct drive_t * drive) {
 194   return (((uchar_t *)drive - (uchar_t*)(channel->drives)) /  sizeof(struct drive_t));
 195 }
 196
 197 static inline struct drive_t * get_selected_drive(struct channel_t * channel) {
 198   return &(channel->drives[channel->drive_select]);
 199 }
 200
 201
 202 static inline int is_primary_port(struct ramdisk_t * ramdisk, ushort_t port) {
 203   switch(port)
 204     {
 205     case PRI_DATA_PORT:
 206     case PRI_FEATURES_PORT:
 207     case PRI_SECT_CNT_PORT:
 208     case PRI_SECT_ADDR1_PORT:
 209     case PRI_SECT_ADDR2_PORT:
 210     case PRI_SECT_ADDR3_PORT:
 211     case PRI_DRV_SEL_PORT:
 212     case PRI_CMD_PORT:
 213     case PRI_CTRL_PORT:
 214       return 1;
 215     default:
 216       return 0;
 217     }
 218 }
 219
 220
 221
 222 static inline int is_secondary_port(struct ramdisk_t * ramdisk, ushort_t port) {
 223   switch(port)
 224     {
 225     case SEC_DATA_PORT:
 226     case SEC_FEATURES_PORT:
 227     case SEC_SECT_CNT_PORT:
 228     case SEC_SECT_ADDR1_PORT:
 229     case SEC_SECT_ADDR2_PORT:
 230     case SEC_SECT_ADDR3_PORT:
 231     case SEC_DRV_SEL_PORT:
 232     case SEC_CMD_PORT:
 233     case SEC_CTRL_PORT:
 234       return 1;
 235     default:
 236       return 0;
 237     }
 238 }
 239
 240 static inline int num_drives_on_channel(struct channel_t * channel) {
 241   if ((channel->drives[0].device_type == IDE_NONE) &&
 242       (channel->drives[1].device_type == IDE_NONE)) {
 243     return 0;
 244   } else if ((channel->drives[0].device_type != IDE_NONE) &&
 245              (channel->drives[1].device_type != IDE_NONE)) {
 246     return 2;
 247   } else {
 248     return 1;
 249   }
 250 }
 251
 252
 253
 254 static inline uchar_t extract_bits(uchar_t * buf, uint_t buf_offset, uint_t bit_offset, uint_t num_bits) {
 255   uchar_t val = buf[buf_offset];
 256   val = val >> bit_offset;
 257   val &= ((1 << num_bits) -1);
 258   return val;
 259 }
 260
 261
 262 static inline uchar_t get_packet_field(struct channel_t * channel, uint_t packet_offset, uint_t bit_offset, uint_t num_bits) {
 263   struct drive_t * drive = get_selected_drive(channel);
 264   return extract_bits(drive->controller.buffer, packet_offset, bit_offset, num_bits);
 265 }
 266
 267
 268 static inline uchar_t get_packet_byte(struct channel_t * channel, uint_t offset) {
 269   struct drive_t * drive = get_selected_drive(channel);
 270   return drive->controller.buffer[offset];
 271 }
 272
 273 static inline uint16_t get_packet_word(struct channel_t * channel, uint_t offset) {
 274   struct drive_t * drive = get_selected_drive(channel);
 275   uint16_t val = drive->controller.buffer[offset];
 276   val = val << 8;
 277   val |= drive->controller.buffer[offset + 1];
 278   return val;
 279 }
 280
 281
 282 static inline uint16_t rd_read_16bit(const uint8_t* buf) {
 283   return (buf[0] << 8) | buf[1];
 284 }
 285
 286
 287
 288 static inline uint32_t rd_read_32bit(const uint8_t* buf) {
 289   return (buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) | buf[3];
 290 }
 291
 292 ////////////////////////////////////////////////////////////////////////////
 293
 294
 295 /*
 296  * ATAPI routines
 297  */
 298
 299
 300 static void rd_init_mode_sense_single(struct vm_device * dev, struct channel_t * channel, const void * src, int size);
 301
 302 static void rd_command_aborted(struct vm_device * dev, struct channel_t * channel, unsigned value);
 303
 304
 305
 306
 307 static int handle_atapi_packet_command(struct vm_device * dev,
 308                                        struct channel_t * channel,
 309                                        ushort_t val);
 310
 311 static int rd_init_send_atapi_command(struct vm_device * dev,
 312                                       struct channel_t * channel,
 313                                       Bit8u command, int req_length,
 314                                       int alloc_length, bool lazy);
 315
 316 static void rd_ready_to_send_atapi(struct vm_device * dev,
 317                                    struct channel_t * channel);
 318
 319 static void rd_atapi_cmd_error(struct vm_device * dev,
 320                                struct channel_t * channel,
 321                                sense_t sense_key, asc_t asc);
 322
 323 static void rd_atapi_cmd_nop(struct vm_device * dev, struct channel_t * channel);
 324 static void rd_identify_ATAPI_drive(struct vm_device * dev, struct channel_t * channel);
 325
 326
 327
 328 /*
 329  * Interrupt handling
 330  */
 331 static void rd_raise_interrupt(struct vm_device * dev, struct channel_t * channel);
 332 static void rd_lower_irq(struct vm_device *dev, Bit32u irq);
 333
 334
 335
 336 /*
 337  * Helper routines
 338  */
 339
 340
 341
 342 #ifdef DEBUG_RAMDISK
 343 static void rd_print_state(struct ramdisk_t *ramdisk);
 344 static int check_bit_fields(struct controller_t * controller);
 345 #endif
 346 ////////////////////////////////////////////////////////////////////
 347
 348
 349
 350
 351
 352 static Bit32u rd_init_hardware(struct ramdisk_t *ramdisk) {
 353   uint_t channel_num;
 354   uint_t device;
 355   struct channel_t *channels = (struct channel_t *)(&(ramdisk->channels));
 356
 357   PrintDebug("[rd_init_harddrive]\n");
 358
 359   for (channel_num = 0; channel_num < MAX_ATA_CHANNEL; channel_num++) {
 360     memset((char *)(channels + channel_num), 0, sizeof(struct channel_t));
 361   }
 362
 363   for (channel_num = 0; channel_num < MAX_ATA_CHANNEL; channel_num++){
 364     struct channel_t * channel = &(channels[channel_num]);
 365
 366     channel->ioaddr1 = 0x0;
 367     channel->ioaddr2 = 0x0;
 368     channel->irq = 0;
 369
 370     for (device = 0; device < 2; device++){
 371       struct drive_t * drive = &(channel->drives[device]);
 372       struct controller_t * controller = &(drive->controller);
 373
 374       controller->status.busy = 0;
 375       controller->status.drive_ready = 1;
 376       controller->status.write_fault = 0;
 377       controller->status.seek_complete = 1;
 378       controller->status.drq = 0;
 379       controller->status.corrected_data = 0;
 380       controller->status.index_pulse = 0;
 381       controller->status.index_pulse_count = 0;
 382       controller->status.err = 0;
 383
 384       controller->error_register = 0x01; // diagnostic code: no error
 385       controller->head_no = 0;
 386       controller->sector_count = 1;
 387       controller->sector_no = 1;
 388       controller->cylinder_no = 0;
 389       controller->current_command = 0x00;
 390       controller->buffer_index = 0;
 391
 392       controller->control.reset = 0;
 393       controller->control.disable_irq = 0;
 394       controller->reset_in_progress = 0;
 395
 396       controller->sectors_per_block = 0x80;
 397       controller->lba_mode = 0;
 398
 399
 400       controller->features = 0;
 401
 402       // If not present
 403       drive->device_type = IDE_NONE;
 404
 405       // Make model string
 406       strncpy((char*)(drive->model_no), "", 40);
 407       while(strlen((char *)(drive->model_no)) < 40) {
 408         strcat ((char*)(drive->model_no), " ");
 409       }
 410
 411
 412
 413
 414       if (channel_num == 1) {
 415
 416         channel->ioaddr1 = 0x170;
 417         channel->ioaddr2 = 0x370;
 418         channel->irq =  15;
 419         channel->drive_select = 0;
 420
 421         if (device == 0) {
 422           // Make model string
 423           strncpy((char*)(drive->model_no), "V3VEE Ramdisk", 40);
 424           while (strlen((char *)(drive->model_no)) < 40) {
 425             strcat ((char*)(drive->model_no), " ");
 426           }
 427
 428           PrintDebug("CDROM on target %d/%d\n", channel_num, device);
 429
 430           drive->device_type = IDE_CDROM;
 431           drive->cdrom.locked = 0;
 432           drive->sense.sense_key = SENSE_NONE;
 433           drive->sense.asc = 0;
 434           drive->sense.ascq = 0;
 435
 436 #ifdef DEBUG_RAMDISK
 437           if (check_bit_fields(controller) == INTR_REASON_BIT_ERR) {
 438             PrintDebug("interrupt reason: bit field error\n");
 439             return INTR_REASON_BIT_ERR;
 440           }
 441 #endif
 442
 443           controller->sector_count = 0;
 444
 445           // allocate low level driver
 446           drive->cdrom.cd = (struct cdrom_interface *)V3_Malloc(sizeof(struct cdrom_interface));
 447           PrintDebug("cd = %x\n", drive->cdrom.cd);
 448           V3_ASSERT(drive->cdrom.cd != NULL);
 449
 450           struct cdrom_interface * cdif = drive->cdrom.cd;
 451           memset(cdif, 0, sizeof(struct cdrom_interface));
 452           init_cdrom(cdif);
 453           cdif->ops.init(cdif);
 454
 455           PrintDebug("\t\tCD on ata%d-%d: '%s'\n",
 456                      channel_num,
 457                      device, "");
 458
 459           if((drive->cdrom.cd->ops).insert_cdrom(cdif, NULL)) {
 460             PrintDebug("\t\tMedia present in CD-ROM drive\n");
 461             drive->cdrom.ready = 1;
 462             drive->cdrom.capacity = drive->cdrom.cd->ops.capacity(cdif);
 463           } else {
 464             PrintDebug("\t\tCould not locate CD-ROM, continuing with media not present\n");
 465             drive->cdrom.ready = 0;
 466           }
 467
 468         }//if device = 0
 469       }//if channel = 0
 470     }//for device
 471   }//for channel
 472
 473 #ifdef DEBUG_RAMDISK
 474   rd_print_state(ramdisk);
 475 #endif
 476   return 0;
 477 }
 478
 479
 480 /*
 481   static void rd_reset_harddrive(struct ramdisk_t *ramdisk, unsigned type) {
 482   return;
 483   }
 484
 485 */
 486 static void rd_close_harddrive(struct ramdisk_t *ramdisk) {
 487   return;
 488 }
 489
 490
 491 ////////////////////////////////////////////////////////////////////
 492
 493
 494
 495 static int read_data_port(ushort_t port, void * dst, uint_t length, struct vm_device * dev) {
 496   struct ramdisk_t * ramdisk  = (struct ramdisk_t *)(dev->private_data);
 497   struct channel_t * channel = NULL;
 498   struct drive_t * drive = NULL;
 499   struct controller_t * controller = NULL;
 500
 501
 502
 503   if (is_primary_port(ramdisk, port)) {
 504     channel = &(ramdisk->channels[0]);
 505   } else if (is_secondary_port(ramdisk, port)) {
 506     channel = &(ramdisk->channels[1]);
 507   } else {
 508     PrintError("Invalid Port: %d\n", port);
 509     return -1;
 510   }
 511
 512   drive = get_selected_drive(channel);
 513   controller = &(drive->controller);
 514
 515
 516   PrintDebug("[read_data_handler] IO Read at 0x%x, on drive %d/%d (current_cmd = 0x%02x)\n",
 517              port,
 518              get_channel_no(ramdisk, channel),
 519              get_drive_no(channel, drive),
 520              controller->current_command);
 521
 522   switch (controller->current_command) {
 523   case 0xec:    // IDENTIFY DEVICE
 524   case 0xa1:
 525     {
 526
 527
 528       controller->status.busy = 0;
 529       controller->status.drive_ready = 1;
 530       controller->status.write_fault = 0;
 531       controller->status.seek_complete = 1;
 532       controller->status.corrected_data = 0;
 533       controller->status.err = 0;
 534
 535       /*
 536         value32 = controller->buffer[index];
 537         index++;
 538
 539         if (io_len >= 2) {
 540         value32 |= (controller->buffer[index] << 8);
 541         index++;
 542         }
 543
 544         if (io_len == 4) {
 545         value32 |= (controller->buffer[index] << 16);
 546         value32 |= (controller->buffer[index+1] << 24);
 547         index += 2;
 548         }
 549
 550         controller->buffer_index = index;
 551       */
 552       /* JRL */
 553       memcpy(dst, controller->buffer + controller->buffer_index, length);
 554       controller->buffer_index += length;
 555
 556       if (controller->buffer_index >= 512) {
 557         controller->status.drq = 0;
 558       }
 559
 560       return length;
 561     }
 562   case 0xa0: //send packet cmd
 563     {
 564       uint_t index = controller->buffer_index;
 565
 566
 567       PrintDebug("\t\tatapi.command(%02x), index(%d), cdrom.remaining_blocks(%d)\n",
 568                     drive->atapi.command,
 569                     index,
 570                     drive->cdrom.remaining_blocks);
 571
 572       // Load block if necessary
 573       if (index >= 2048) {
 574
 575         if (index > 2048) {
 576           PrintError("\t\tindex > 2048 : 0x%x\n", index);
 577           return -1;
 578         }
 579
 580         switch (drive->atapi.command) {
 581         case 0x28: // read (10)
 582         case 0xa8: // read (12)
 583           {
 584             struct cdrom_interface * cdif = drive->cdrom.cd;
 585
 586             if (!(drive->cdrom.ready)) {
 587               PrintError("\t\tRead with CDROM not ready\n");
 588               return -1;
 589             }
 590
 591             drive->cdrom.cd->ops.read_block(cdif, controller->buffer,
 592                                             drive->cdrom.next_lba);
 593             drive->cdrom.next_lba++;
 594             drive->cdrom.remaining_blocks--;
 595
 596
 597             if (!(drive->cdrom.remaining_blocks)) {
 598               PrintDebug("\t\tLast READ block loaded {CDROM}\n");
 599             } else {
 600               PrintDebug("\t\tREAD block loaded (%d remaining) {CDROM}\n",
 601                          drive->cdrom.remaining_blocks);
 602             }
 603
 604             // one block transfered, start at beginning
 605             index = 0;
 606             break;
 607           }
 608         default: // no need to load a new block
 609           break;
 610         }
 611       }
 612
 613
 614       /*
 615         increment = 0;
 616         value32 = controller->buffer[index + increment];
 617         increment++;
 618
 619         if (io_len >= 2) {
 620         value32 |= (controller->buffer[index + increment] << 8);
 621         increment++;
 622         }
 623
 624         if (io_len == 4) {
 625         value32 |= (controller->buffer[index + increment] << 16);
 626         value32 |= (controller->buffer[index + increment + 1] << 24);
 627         increment += 2;
 628         }
 629
 630         controller->buffer_index = index + increment;
 631         controller->drq_index += increment;
 632
 633       */
 634       /* JRL: CHECK THAT there is enough data in the buffer to copy.... */
 635       {
 636         memcpy(dst, controller->buffer + index, length);
 637
 638         controller->buffer_index  = index + length;
 639         controller->drq_index += length;
 640       }
 641
 642       /* *** */
 643
 644       if (controller->drq_index >= (unsigned)drive->atapi.drq_bytes) {
 645         controller->status.drq = 0;
 646         controller->drq_index = 0;
 647
 648         drive->atapi.total_bytes_remaining -= drive->atapi.drq_bytes;
 649
 650         if (drive->atapi.total_bytes_remaining > 0) {
 651           // one or more blocks remaining (works only for single block commands)
 652
 653           PrintDebug("\t\tPACKET drq bytes read\n");
 654           controller->interrupt_reason.i_o = 1;
 655           controller->status.busy = 0;
 656           controller->status.drq = 1;
 657           controller->interrupt_reason.c_d = 0;
 658
 659           // set new byte count if last block
 660           if (drive->atapi.total_bytes_remaining < controller->byte_count) {
 661             controller->byte_count = drive->atapi.total_bytes_remaining;
 662           }
 663           drive->atapi.drq_bytes = controller->byte_count;
 664
 665           rd_raise_interrupt(dev, channel);
 666         } else {
 667           // all bytes read
 668           PrintDebug("\t\tPACKET all bytes read\n");
 669
 670           controller->interrupt_reason.i_o = 1;
 671           controller->interrupt_reason.c_d = 1;
 672           controller->status.drive_ready = 1;
 673           controller->interrupt_reason.rel = 0;
 674           controller->status.busy = 0;
 675           controller->status.drq = 0;
 676           controller->status.err = 0;
 677
 678           rd_raise_interrupt(dev, channel);
 679         }
 680       }
 681       return length;
 682       break;
 683     }
 684
 685   default:
 686     PrintDebug("\t\tread need support more command: %02x\n", controller->current_command);
 687     break;
 688   }
 689
 690   return -1;
 691 }
 692
 693
 694
 695
 696 static int write_data_port(ushort_t port, void * src, uint_t length, struct vm_device * dev) {
 697   struct ramdisk_t * ramdisk  = (struct ramdisk_t *)(dev->private_data);
 698   struct channel_t * channel = NULL;
 699   struct drive_t * drive = NULL;
 700   struct controller_t * controller = NULL;
 701
 702   if (is_primary_port(ramdisk, port)) {
 703     channel = &(ramdisk->channels[0]);
 704   } else if (is_secondary_port(ramdisk, port)) {
 705     channel = &(ramdisk->channels[1]);
 706   } else {
 707     PrintError("Invalid Port: %d\n", port);
 708     return -1;
 709   }
 710
 711   drive = get_selected_drive(channel);
 712   controller = &(drive->controller);
 713
 714   PrintDebug("[write_data_handler] IO write at 0x%x, current_cmd = 0x%02x\n",
 715              port, controller->current_command);
 716
 717   switch (controller->current_command) {
 718   case 0x30: // WRITE SECTORS
 719     PrintError("\t\tneed to implement 0x30(write sector) to port 0x%x\n", port);
 720     return -1;
 721
 722   case 0xa0: // PACKET
 723
 724     if (handle_atapi_packet_command(dev, channel, *(ushort_t *)src) == -1) {
 725       return -1;
 726     }
 727
 728     return length;
 729
 730   default:
 731     PrintError("\t\tIO write(0x%x): current command is %02xh\n",
 732                port, controller->current_command);
 733
 734     return -1;
 735   }
 736 }
 737
 738
 739
 740
 741
 742
 743
 744 static int read_status_port(ushort_t port, void * dst, uint_t length, struct vm_device * dev) {
 745   struct ramdisk_t * ramdisk  = (struct ramdisk_t *)(dev->private_data);
 746   struct channel_t * channel = NULL;
 747   struct drive_t * drive = NULL;
 748   struct controller_t * controller = NULL;
 749
 750
 751   if (length != 1) {
 752     PrintError("Invalid Status port read length: %d (port=%d)\n", length, port);
 753     return -1;
 754   }
 755
 756   if (is_primary_port(ramdisk, port)) {
 757     channel = &(ramdisk->channels[0]);
 758   } else if (is_secondary_port(ramdisk, port)) {
 759     channel = &(ramdisk->channels[1]);
 760   } else {
 761     PrintError("Invalid Port: %d\n", port);
 762     return -1;
 763   }
 764
 765   drive = get_selected_drive(channel);
 766   controller = &(drive->controller);
 767
 768   PrintDebug("[read_status_handler] IO read at 0x%x, on drive %d/%d\n",
 769              port, get_channel_no(ramdisk, channel),
 770              channel->drive_select);
 771
 772
 773   if (num_drives_on_channel(channel) == 0) {
 774     // (mch) Just return zero for these registers
 775     memset(dst, 0, length);
 776
 777   } else {
 778     uchar_t val = (
 779                    (controller->status.busy << 7)            |
 780                    (controller->status.drive_ready << 6)     |
 781                    (controller->status.write_fault << 5)     |
 782                    (controller->status.seek_complete << 4)   |
 783                    (controller->status.drq << 3)             |
 784                    (controller->status.corrected_data << 2)  |
 785                    (controller->status.index_pulse << 1)     |
 786                    (controller->status.err) );
 787
 788     memcpy(dst, &val, length);
 789
 790     controller->status.index_pulse_count++;
 791     controller->status.index_pulse = 0;
 792
 793     if (controller->status.index_pulse_count >= INDEX_PULSE_CYCLE) {
 794       controller->status.index_pulse = 1;
 795       controller->status.index_pulse_count = 0;
 796     }
 797   }
 798
 799   if ((port == SEC_CMD_PORT) || (port == PRI_CMD_PORT)) {
 800     rd_lower_irq(dev, channel->irq);
 801   }
 802
 803   return length;
 804
 805 }
 806
 807
 808 static int write_cmd_port(ushort_t port, void * src, uint_t length, struct vm_device * dev) {
 809   struct ramdisk_t * ramdisk  = (struct ramdisk_t *)(dev->private_data);
 810   struct channel_t * channel = NULL;
 811   struct drive_t * drive = NULL;
 812   struct controller_t * controller = NULL;
 813   uchar_t value = *(uchar_t *)src;
 814
 815   if (length != 1) {
 816     PrintError("Invalid Command port write length: %d (port=%d)\n", length, port);
 817     return -1;
 818   }
 819
 820   if (is_primary_port(ramdisk, port)) {
 821     channel = &(ramdisk->channels[0]);
 822   } else if (is_secondary_port(ramdisk, port)) {
 823     channel = &(ramdisk->channels[1]);
 824   } else {
 825     PrintError("Invalid Port: %d\n", port);
 826     return -1;
 827   }
 828
 829   drive = get_selected_drive(channel);
 830   controller = &(drive->controller);
 831
 832
 833   PrintDebug("[write_command_handler] IO write at 0x%x, on drive %d/%d (val = 0x%x)\n",
 834              port, get_channel_no(ramdisk, channel),
 835              channel->drive_select,
 836              value);
 837
 838   switch (value) {
 839     // ATAPI commands
 840   case 0xa1: // IDENTIFY PACKET DEVICE
 841     {
 842       if (drive->device_type == IDE_CDROM) {
 843         controller->current_command = value;
 844         controller->error_register = 0;
 845
 846         controller->status.busy = 0;
 847         controller->status.drive_ready = 1;
 848         controller->status.write_fault = 0;
 849         controller->status.drq   = 1;
 850         controller->status.err   = 0;
 851
 852         controller->status.seek_complete = 1;
 853         controller->status.corrected_data = 0;
 854
 855         controller->buffer_index = 0;
 856         rd_raise_interrupt(dev, channel);
 857         rd_identify_ATAPI_drive(dev, channel);
 858       } else {
 859         rd_command_aborted(dev, channel, 0xa1);
 860       }
 861       break;
 862     }
 863   case 0xa0: // SEND PACKET (atapi)
 864     {
 865       if (drive->device_type == IDE_CDROM) {
 866         // PACKET
 867
 868         if (controller->features & (1 << 0)) {
 869           PrintError("\t\tPACKET-DMA not supported");
 870           return -1;
 871         }
 872
 873         if (controller->features & (1 << 1)) {
 874           PrintError("\t\tPACKET-overlapped not supported");
 875           return -1;
 876         }
 877
 878         // We're already ready!
 879         controller->sector_count = 1;
 880         controller->status.busy = 0;
 881         controller->status.write_fault = 0;
 882
 883         // serv bit??
 884         controller->status.drq = 1;
 885         controller->status.err = 0;
 886
 887         // NOTE: no interrupt here
 888         controller->current_command = value;
 889         controller->buffer_index = 0;
 890       } else {
 891         rd_command_aborted (dev, channel, 0xa0);
 892       }
 893       break;
 894     }
 895   default:
 896     PrintError("\t\tneed translate command %2x\n", value);
 897     return -1;
 898
 899   }
 900   return length;
 901 }
 902
 903
 904 static int write_ctrl_port(ushort_t port, void * src, uint_t length, struct vm_device * dev) {
 905   struct ramdisk_t * ramdisk  = (struct ramdisk_t *)(dev->private_data);
 906   struct channel_t * channel = NULL;
 907   struct drive_t * master_drive = NULL;
 908   struct drive_t * slave_drive = NULL;
 909   struct controller_t * controller = NULL;
 910   uchar_t value = *(uchar_t *)src;
 911   rd_bool prev_control_reset;
 912
 913   if (length != 1) {
 914     PrintError("Invalid Status port read length: %d (port=%d)\n", length, port);
 915     return -1;
 916   }
 917
 918   if (is_primary_port(ramdisk, port)) {
 919     channel = &(ramdisk->channels[0]);
 920   } else if (is_secondary_port(ramdisk, port)) {
 921     channel = &(ramdisk->channels[1]);
 922   } else {
 923     PrintError("Invalid Port: %d\n", port);
 924     return -1;
 925   }
 926
 927   master_drive = &(channel->drives[0]);
 928   slave_drive = &(channel->drives[1]);
 929
 930   controller = &(get_selected_drive(channel)->controller);
 931
 932
 933   PrintDebug("[write_control_handler] IO write at 0x%x, on drive %d/%d (val = 0x%x)\n",
 934              port, get_channel_no(ramdisk, channel),
 935              channel->drive_select,
 936              value);
 937
 938   // (mch) Even if device 1 was selected, a write to this register
 939   // goes to device 0 (if device 1 is absent)
 940
 941   prev_control_reset = controller->control.reset;
 942
 943   master_drive->controller.control.reset         = value & 0x04;
 944   slave_drive->controller.control.reset         = value & 0x04;
 945
 946   // CGS: was: SELECTED_CONTROLLER(channel).control.disable_irq    = value & 0x02;
 947   master_drive->controller.control.disable_irq = value & 0x02;
 948   slave_drive->controller.control.disable_irq = value & 0x02;
 949
 950   PrintDebug("\t\tadpater control reg: reset controller = %d\n",
 951                 (unsigned) (controller->control.reset) ? 1 : 0);
 952   PrintDebug("\t\tadpater control reg: disable_irq(X) = %d\n",
 953                 (unsigned) (controller->control.disable_irq) ? 1 : 0);
 954
 955   if ((!prev_control_reset) && (controller->control.reset)) {
 956     uint_t id = 0;
 957
 958     // transition from 0 to 1 causes all drives to reset
 959     PrintDebug("\t\thard drive: RESET\n");
 960
 961     // (mch) Set BSY, drive not ready
 962     for (id = 0; id < 2; id++) {
 963       struct controller_t * ctrl = NULL;
 964
 965       if (id == 0) {
 966         ctrl = &(master_drive->controller);
 967       } else if (id == 1) {
 968         ctrl = &(slave_drive->controller);
 969       }
 970
 971       ctrl->status.busy           = 1;
 972       ctrl->status.drive_ready    = 0;
 973       ctrl->reset_in_progress     = 1;
 974
 975       ctrl->status.write_fault    = 0;
 976       ctrl->status.seek_complete  = 1;
 977       ctrl->status.drq            = 0;
 978       ctrl->status.corrected_data = 0;
 979       ctrl->status.err            = 0;
 980
 981       ctrl->error_register = 0x01; // diagnostic code: no error
 982
 983       ctrl->current_command = 0x00;
 984       ctrl->buffer_index = 0;
 985
 986       ctrl->sectors_per_block = 0x80;
 987       ctrl->lba_mode          = 0;
 988
 989       ctrl->control.disable_irq = 0;
 990     }
 991
 992     rd_lower_irq(dev, channel->irq);
 993
 994   } else if ((controller->reset_in_progress) &&
 995              (!controller->control.reset)) {
 996     uint_t id;
 997     // Clear BSY and DRDY
 998     PrintDebug("\t\tReset complete {%s}\n", device_type_to_str(get_selected_drive(channel)->device_type));
 999
1000     for (id = 0; id < 2; id++) {
1001       struct controller_t * ctrl = NULL;
1002       struct drive_t * drv = NULL;
1003
1004       if (id == 0) {
1005         ctrl = &(master_drive->controller);
1006         drv = master_drive;
1007       } else if (id == 1) {
1008         ctrl = &(slave_drive->controller);
1009         drv = slave_drive;
1010       }
1011
1012       ctrl->status.busy           = 0;
1013       ctrl->status.drive_ready    = 1;
1014       ctrl->reset_in_progress     = 0;
1015
1016       // Device signature
1017       if (drv->device_type == IDE_DISK) {
1018         PrintDebug("\t\tdrive %d/%d is harddrive\n", get_channel_no(ramdisk, channel), id);
1019         ctrl->head_no        = 0;
1020         ctrl->sector_count   = 1;
1021         ctrl->sector_no      = 1;
1022         ctrl->cylinder_no    = 0;
1023       } else {
1024         ctrl->head_no        = 0;
1025         ctrl->sector_count   = 1;
1026         ctrl->sector_no      = 1;
1027         ctrl->cylinder_no    = 0xeb14;
1028       }
1029     }
1030   }
1031
1032   PrintDebug("\t\ts[0].controller.control.disable_irq = %02x\n",
1033              master_drive->controller.control.disable_irq);
1034   PrintDebug("\t\ts[1].controller.control.disable_irq = %02x\n",
1035              slave_drive->controller.control.disable_irq);
1036   return length;
1037 }
1038
1039
1040 static int read_general_port(ushort_t port, void * dst, uint_t length, struct vm_device * dev) {
1041   struct ramdisk_t * ramdisk  = (struct ramdisk_t *)(dev->private_data);
1042   struct channel_t * channel = NULL;
1043   struct drive_t * drive = NULL;
1044   struct controller_t * controller = NULL;
1045
1046
1047   if (length != 1) {
1048     PrintError("Invalid Status port read length: %d (port=%d)\n", length, port);
1049     return -1;
1050   }
1051
1052   if (is_primary_port(ramdisk, port)) {
1053     channel = &(ramdisk->channels[0]);
1054   } else if (is_secondary_port(ramdisk, port)) {
1055     channel = &(ramdisk->channels[1]);
1056   } else {
1057     PrintError("Invalid Port: %d\n", port);
1058     return -1;
1059   }
1060
1061   drive = get_selected_drive(channel);
1062   controller = &(drive->controller);
1063
1064
1065   PrintDebug("[read_general_handler] IO read addr at %x, on drive %d/%d, curcmd = %02x\n",
1066              port, get_channel_no(ramdisk, channel),
1067              channel->drive_select,
1068              controller->current_command);
1069
1070
1071   switch (port) {
1072   case PRI_FEATURES_PORT:
1073   case SEC_FEATURES_PORT: // hard disk error register 0x1f1
1074     {
1075       uchar_t val = (drive->device_type == IDE_NONE) ? 0 : controller->error_register;
1076
1077       controller->status.err = 0;
1078
1079       *(uchar_t *)dst = val;
1080       return length;
1081
1082       break;
1083     }
1084
1085   case PRI_SECT_CNT_PORT:
1086   case SEC_SECT_CNT_PORT:  // hard disk sector count / interrupt reason 0x1f2
1087     {
1088       uchar_t val = (drive->device_type == IDE_NONE) ? 0 : controller->sector_count;
1089
1090       *(uchar_t *)dst = val;
1091       return length;
1092
1093       break;
1094     }
1095   case PRI_SECT_ADDR1_PORT:
1096   case SEC_SECT_ADDR1_PORT: // sector number 0x1f3
1097     {
1098       uchar_t val = (drive->device_type == IDE_NONE) ? 0 : controller->sector_no;
1099
1100       *(uchar_t *)dst = val;
1101       return length;
1102
1103       break;
1104     }
1105
1106   case PRI_SECT_ADDR2_PORT:
1107   case SEC_SECT_ADDR2_PORT:  // cylinder low 0x1f4
1108     {
1109       // -- WARNING : On real hardware the controller registers are shared between drives.
1110       // So we must respond even if the select device is not present. Some OS uses this fact
1111       // to detect the disks.... minix2 for example
1112       uchar_t val = (num_drives_on_channel(channel) == 0) ? 0 : (controller->cylinder_no & 0x00ff);
1113
1114       *(uchar_t *)dst = val;
1115       return length;
1116
1117       break;
1118   }
1119
1120   case PRI_SECT_ADDR3_PORT:
1121   case SEC_SECT_ADDR3_PORT: // cylinder high 0x1f5
1122     {
1123       // -- WARNING : On real hardware the controller registers are shared between drives.
1124       // So we must respond even if the select device is not present. Some OS uses this fact
1125       // to detect the disks.... minix2 for example
1126       uchar_t val = (num_drives_on_channel(channel) == 0) ? 0 : (controller->cylinder_no >> 8);
1127
1128       *(uchar_t *)dst = val;
1129       return length;
1130
1131       break;
1132     }
1133   case PRI_DRV_SEL_PORT:
1134   case SEC_DRV_SEL_PORT:  // hard disk drive and head register 0x1f6
1135     {
1136       // b7 Extended data field for ECC
1137       // b6/b5: Used to be sector size.  00=256,01=512,10=1024,11=128
1138       //   Since 512 was always used, bit 6 was taken to mean LBA mode:
1139       //     b6 1=LBA mode, 0=CHS mode
1140       //     b5 1
1141       // b4: DRV
1142       // b3..0 HD3..HD0
1143       uchar_t val = ((1 << 7)                          |
1144                      ((controller->lba_mode > 0) << 6) |
1145                      (1 << 5)                          |            // 01b = 512 sector size
1146                      (channel->drive_select << 4)      |
1147                      (controller->head_no << 0));
1148
1149       *(uchar_t *)dst = val;
1150       return length;
1151
1152       break;
1153     }
1154  case PRI_ADDR_REG_PORT:
1155  case SEC_ADDR_REG_PORT: // Hard Disk Address Register 0x3f7
1156    {
1157      // Obsolete and unsupported register.  Not driven by hard
1158      // disk controller.  Report all 1's.  If floppy controller
1159      // is handling this address, it will call this function
1160      // set/clear D7 (the only bit it handles), then return
1161      // the combined value
1162      *(uchar_t *)dst = 0xff;
1163      return length;
1164     }
1165
1166   default:
1167     PrintError("Invalid Port: %d\n", port);
1168     return -1;
1169   }
1170 }
1171
1172
1173
1174
1175 static int write_general_port(ushort_t port, void * src, uint_t length, struct vm_device * dev) {
1176   struct ramdisk_t * ramdisk  = (struct ramdisk_t *)(dev->private_data);
1177   struct channel_t * channel = NULL;
1178   struct drive_t * drive = NULL;
1179   struct controller_t * controller = NULL;
1180   uchar_t value = *(uchar_t *)src;
1181
1182   if (length != 1) {
1183     PrintError("Invalid Status port read length: %d (port=%d)\n", length, port);
1184     return -1;
1185   }
1186
1187   if (is_primary_port(ramdisk, port)) {
1188     channel = &(ramdisk->channels[0]);
1189   } else if (is_secondary_port(ramdisk, port)) {
1190     channel = &(ramdisk->channels[1]);
1191   } else {
1192     PrintError("Invalid Port: %d\n", port);
1193     return -1;
1194   }
1195
1196   drive = get_selected_drive(channel);
1197   controller = &(drive->controller);
1198
1199
1200   PrintDebug("[write_general_handler] IO write to port %x (val=0x%02x), channel = %d\n",
1201              port,  value, get_channel_no(ramdisk, channel));
1202
1203   switch (port) {
1204
1205   case PRI_FEATURES_PORT:
1206   case SEC_FEATURES_PORT: // hard disk write precompensation 0x1f1
1207     {
1208       write_features(channel, value);
1209       break;
1210     }
1211   case PRI_SECT_CNT_PORT:
1212   case SEC_SECT_CNT_PORT: // hard disk sector count 0x1f2
1213     {
1214       write_sector_count(channel, value);
1215       break;
1216     }
1217   case PRI_SECT_ADDR1_PORT:
1218   case SEC_SECT_ADDR1_PORT: // hard disk sector number 0x1f3
1219     {
1220       write_sector_number(channel, value);
1221       break;
1222     }
1223   case PRI_SECT_ADDR2_PORT:
1224   case SEC_SECT_ADDR2_PORT: // hard disk cylinder low 0x1f4
1225     {
1226       write_cylinder_low(channel, value);
1227       break;
1228     }
1229   case PRI_SECT_ADDR3_PORT:
1230   case SEC_SECT_ADDR3_PORT: // hard disk cylinder high 0x1f5
1231     {
1232       write_cylinder_high(channel, value);
1233       break;
1234     }
1235   case PRI_DRV_SEL_PORT:
1236   case SEC_DRV_SEL_PORT: // hard disk drive and head register 0x1f6
1237     {
1238       // b7 Extended data field for ECC
1239       // b6/b5: Used to be sector size.  00=256,01=512,10=1024,11=128
1240       //   Since 512 was always used, bit 6 was taken to mean LBA mode:
1241       //     b6 1=LBA mode, 0=CHS mode
1242       //     b5 1
1243       // b4: DRV
1244       // b3..0 HD3..HD0
1245
1246       // 1x1xxxxx
1247
1248       PrintDebug("\tDrive Select value=%x\n", value);
1249
1250       if ((value & 0xa0) != 0xa0) {
1251         PrintDebug("\t\tIO write 0x%x (%02x): not 1x1xxxxxb\n", port, (unsigned) value);
1252       }
1253
1254       write_head_no(channel, value & 0xf);
1255       if ((controller->lba_mode == 0) && (((value >> 6) & 1) == 1)) {
1256         PrintDebug("\t\tenabling LBA mode\n");
1257       }
1258
1259       write_lba_mode(channel, (value >> 6) & 1);
1260       drive->cdrom.cd->lba = (value >> 6) & 1;
1261
1262
1263       channel->drive_select = (value >> 4) & 0x01;
1264       drive = get_selected_drive(channel);
1265
1266       if (drive->device_type == IDE_NONE) {
1267         PrintDebug("\t\tError: device set to %d which does not exist! channel = 0x%x\n",
1268                    channel->drive_select, get_channel_no(ramdisk, channel));
1269
1270         controller->error_register = 0x04; // aborted
1271         controller->status.err = 1;
1272       }
1273
1274       break;
1275     }
1276   default:
1277     PrintError("\t\thard drive: io write to unhandled port 0x%x  (value = %c)\n", port, value);
1278     return -1;
1279   }
1280
1281   return length;
1282 }
1283
1284
1285
1286
1287
1288 static void rd_raise_interrupt(struct vm_device * dev, struct channel_t * channel) {
1289   Bit32u irq;
1290   //  struct ramdisk_t * ramdisk = (struct ramdisk_t *)(dev->private_data);
1291   struct drive_t * drive = get_selected_drive(channel);
1292   struct controller_t * controller = &(drive->controller);
1293
1294   PrintDebug("[raise_interrupt] disable_irq = %02x\n", controller->control.disable_irq);
1295
1296   if (!(controller->control.disable_irq)) {
1297     irq = channel->irq;
1298
1299     PrintDebug("\t\tRaising interrupt %d {%s}\n\n", irq, device_type_to_str(drive->device_type));
1300
1301     dev->vm->vm_ops.raise_irq(dev->vm, irq);
1302   } else {
1303     PrintDebug("\t\tirq is disabled\n");
1304   }
1305
1306   return;
1307 }
1308
1309 static void rd_lower_irq(struct vm_device *dev, Bit32u irq)  // __attribute__(regparm(1))
1310 {
1311   PrintDebug("[lower_irq] irq = %d\n", irq);
1312   dev->vm->vm_ops.lower_irq(dev->vm, irq);
1313 }
1314
1315
1316
1317
1318
1319
1320
1321 //////////////////////////////////////////////////////////////////////////
1322
1323 /*
1324  * ATAPI subroutines
1325  */
1326
1327
1328
1329 int handle_atapi_packet_command(struct vm_device * dev, struct channel_t * channel, ushort_t value) {
1330   //struct ramdisk_t * ramdisk  = (struct ramdisk_t *)(dev->private_data);
1331   struct drive_t * drive = get_selected_drive(channel);
1332   struct controller_t * controller = &(drive->controller);
1333
1334   if (controller->buffer_index >= PACKET_SIZE) {
1335     PrintError("ATAPI packet exceeded maximum length: buffer_index (%d) >= PACKET_SIZE\n",
1336                controller->buffer_index);
1337     return -1;
1338   }
1339
1340   controller->buffer[controller->buffer_index] = value;
1341   controller->buffer[controller->buffer_index + 1] = (value >> 8);
1342   controller->buffer_index += 2;
1343
1344
1345   /* if packet completely writtten */
1346   if (controller->buffer_index >= PACKET_SIZE) {
1347     // complete command received
1348     Bit8u atapi_command = controller->buffer[0];
1349
1350     PrintDebug("\t\tcdrom: ATAPI command 0x%x started\n", atapi_command);
1351
1352     switch (atapi_command) {
1353     case 0x00: // test unit ready
1354       {
1355         if (drive->cdrom.ready) {
1356           rd_atapi_cmd_nop(dev, channel);
1357         } else {
1358           rd_atapi_cmd_error(dev, channel, SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT);
1359         }
1360
1361         rd_raise_interrupt(dev, channel);
1362
1363         break;
1364       }
1365     case 0x03:  // request sense
1366       {
1367         int alloc_length = controller->buffer[4];
1368
1369         if (rd_init_send_atapi_command(dev, channel, atapi_command, 18, alloc_length, false) == -1) {
1370           return -1;
1371         }
1372
1373         // sense data
1374         controller->buffer[0] = 0x70 | (1 << 7);
1375         controller->buffer[1] = 0;
1376         controller->buffer[2] = drive->sense.sense_key;
1377         controller->buffer[3] = drive->sense.information.arr[0];
1378         controller->buffer[4] = drive->sense.information.arr[1];
1379         controller->buffer[5] = drive->sense.information.arr[2];
1380         controller->buffer[6] = drive->sense.information.arr[3];
1381         controller->buffer[7] = 17 - 7;
1382         controller->buffer[8] = drive->sense.specific_inf.arr[0];
1383         controller->buffer[9] = drive->sense.specific_inf.arr[1];
1384         controller->buffer[10] = drive->sense.specific_inf.arr[2];
1385         controller->buffer[11] = drive->sense.specific_inf.arr[3];
1386         controller->buffer[12] = drive->sense.asc;
1387         controller->buffer[13] = drive->sense.ascq;
1388         controller->buffer[14] = drive->sense.fruc;
1389         controller->buffer[15] = drive->sense.key_spec.arr[0];
1390         controller->buffer[16] = drive->sense.key_spec.arr[1];
1391         controller->buffer[17] = drive->sense.key_spec.arr[2];
1392
1393         rd_ready_to_send_atapi(dev, channel);
1394         break;
1395       }
1396     case 0x1b:  // start stop unit
1397       {
1398         //bx_bool Immed = (controller->buffer[1] >> 0) & 1;
1399         rd_bool LoEj = (controller->buffer[4] >> 1) & 1;
1400         rd_bool Start = (controller->buffer[4] >> 0) & 1;
1401
1402         // stop the disc
1403         if ((!LoEj) && (!Start)) {
1404           PrintError("FIXME: Stop disc not implemented\n");
1405
1406           rd_atapi_cmd_nop(dev, channel);
1407           rd_raise_interrupt(dev, channel);
1408         } else if (!LoEj && Start) { // start (spin up) the disc
1409
1410           drive->cdrom.cd->ops.start_cdrom(drive->cdrom.cd);
1411
1412           PrintError("FIXME: ATAPI start disc not reading TOC\n");
1413           rd_atapi_cmd_nop(dev, channel);
1414           rd_raise_interrupt(dev, channel);
1415         } else if (LoEj && !Start) { // Eject the disc
1416           rd_atapi_cmd_nop(dev, channel);
1417
1418           if (drive->cdrom.ready) {
1419
1420             drive->cdrom.cd->ops.eject_cdrom(drive->cdrom.cd);
1421
1422             drive->cdrom.ready = 0;
1423             //bx_options.atadevice[channel][SLAVE_SELECTED(channel)].Ostatus->set(EJECTED);
1424             //bx_gui->update_drive_status_buttons();
1425           }
1426           rd_raise_interrupt(dev, channel);
1427         } else { // Load the disc
1428           // My guess is that this command only closes the tray, that's a no-op for us
1429           rd_atapi_cmd_nop(dev, channel);
1430           rd_raise_interrupt(dev, channel);
1431         }
1432         break;
1433       }
1434     case 0xbd: // mechanism status
1435       {
1436         uint16_t alloc_length = rd_read_16bit(controller->buffer + 8);
1437
1438         if (alloc_length == 0) {
1439           PrintError("Zero allocation length to MECHANISM STATUS not impl.\n");
1440           return -1;
1441         }
1442
1443         if (rd_init_send_atapi_command(dev, channel, atapi_command, 8, alloc_length, false) == -1) {
1444           return -1;
1445         }
1446
1447         controller->buffer[0] = 0; // reserved for non changers
1448         controller->buffer[1] = 0; // reserved for non changers
1449
1450         controller->buffer[2] = 0; // Current LBA (TODO!)
1451         controller->buffer[3] = 0; // Current LBA (TODO!)
1452         controller->buffer[4] = 0; // Current LBA (TODO!)
1453
1454         controller->buffer[5] = 1; // one slot
1455
1456         controller->buffer[6] = 0; // slot table length
1457         controller->buffer[7] = 0; // slot table length
1458
1459         rd_ready_to_send_atapi(dev, channel);
1460         break;
1461       }
1462     case 0x5a:  // mode sense
1463       {
1464         uint16_t alloc_length = rd_read_16bit(controller->buffer + 7);
1465
1466         Bit8u PC = controller->buffer[2] >> 6;
1467         Bit8u PageCode = controller->buffer[2] & 0x3f;
1468
1469         switch (PC) {
1470         case 0x0: // current values
1471           {
1472             switch (PageCode) {
1473             case 0x01: // error recovery
1474               {
1475
1476                 if (rd_init_send_atapi_command(dev, channel, atapi_command, sizeof(struct error_recovery_t) + 8, alloc_length, false) == -1) {
1477                   return -1;
1478                 }
1479
1480                 rd_init_mode_sense_single(dev, channel, &(drive->cdrom.current.error_recovery),
1481                                           sizeof(struct error_recovery_t));
1482                 rd_ready_to_send_atapi(dev, channel);
1483                 break;
1484               }
1485             case 0x2a: // CD-ROM capabilities & mech. status
1486               {
1487
1488                 if (rd_init_send_atapi_command(dev, channel, atapi_command, 28, alloc_length, false) == -1) {
1489                   return -1;
1490                 }
1491
1492                 rd_init_mode_sense_single(dev, channel, &(controller->buffer[8]), 28);
1493
1494                 controller->buffer[8] = 0x2a;
1495                 controller->buffer[9] = 0x12;
1496                 controller->buffer[10] = 0x00;
1497                 controller->buffer[11] = 0x00;
1498                 // Multisession, Mode 2 Form 2, Mode 2 Form 1
1499                 controller->buffer[12] = 0x70;
1500                 controller->buffer[13] = (3 << 5);
1501                 controller->buffer[14] = (unsigned char) (1 |
1502                                                           (drive->cdrom.locked ? (1 << 1) : 0) |
1503                                                           (1 << 3) |
1504                                                           (1 << 5));
1505                 controller->buffer[15] = 0x00;
1506                 controller->buffer[16] = (706 >> 8) & 0xff;
1507                 controller->buffer[17] = 706 & 0xff;
1508                 controller->buffer[18] = 0;
1509                 controller->buffer[19] = 2;
1510                 controller->buffer[20] = (512 >> 8) & 0xff;
1511                 controller->buffer[21] = 512 & 0xff;
1512                 controller->buffer[22] = (706 >> 8) & 0xff;
1513                 controller->buffer[23] = 706 & 0xff;
1514                 controller->buffer[24] = 0;
1515                 controller->buffer[25] = 0;
1516                 controller->buffer[26] = 0;
1517                 controller->buffer[27] = 0;
1518                 rd_ready_to_send_atapi(dev, channel);
1519                 break;
1520               }
1521             case 0x0d: // CD-ROM
1522             case 0x0e: // CD-ROM audio control
1523             case 0x3f: // all
1524               {
1525                 PrintError("Ramdisk: cdrom: MODE SENSE (curr), code=%x not implemented yet\n",
1526                          PageCode);
1527                 rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST,
1528                                    ASC_INV_FIELD_IN_CMD_PACKET);
1529                 rd_raise_interrupt(dev, channel);
1530                 break;
1531               }
1532             default:
1533               {
1534                 // not implemeted by this device
1535                 PrintDebug("\t\tcdrom: MODE SENSE PC=%x, PageCode=%x, not implemented by device\n",
1536                               PC, PageCode);
1537                 rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST,
1538                                    ASC_INV_FIELD_IN_CMD_PACKET);
1539                 rd_raise_interrupt(dev, channel);
1540                 break;
1541               }
1542             }
1543             break;
1544           }
1545         case 0x1: // changeable values
1546           {
1547             switch (PageCode) {
1548             case 0x01: // error recovery
1549             case 0x0d: // CD-ROM
1550             case 0x0e: // CD-ROM audio control
1551             case 0x2a: // CD-ROM capabilities & mech. status
1552             case 0x3f: // all
1553               {
1554                 PrintError("cdrom: MODE SENSE (chg), code=%x not implemented yet\n",
1555                            PageCode);
1556                 rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST,
1557                                    ASC_INV_FIELD_IN_CMD_PACKET);
1558                 rd_raise_interrupt(dev, channel);
1559                 break;
1560               }
1561             default:
1562               {
1563                 // not implemeted by this device
1564                 PrintDebug("\t\tcdrom: MODE SENSE PC=%x, PageCode=%x, not implemented by device\n",
1565                            PC, PageCode);
1566                 rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST,
1567                                    ASC_INV_FIELD_IN_CMD_PACKET);
1568                 rd_raise_interrupt(dev, channel);
1569                 break;
1570               }
1571             }
1572             break;
1573           }
1574         case 0x2: // default values
1575           {
1576             switch (PageCode) {
1577             case 0x01: // error recovery
1578             case 0x0d: // CD-ROM
1579             case 0x0e: // CD-ROM audio control
1580             case 0x2a: // CD-ROM capabilities & mech. status
1581             case 0x3f: // all
1582               PrintError("cdrom: MODE SENSE (dflt), code=%x\n",
1583                        PageCode);
1584               return -1;
1585
1586             default:
1587               {
1588                 // not implemeted by this device
1589                 PrintDebug("\t\tcdrom: MODE SENSE PC=%x, PageCode=%x, not implemented by device\n",
1590                               PC, PageCode);
1591                 rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST,
1592                                    ASC_INV_FIELD_IN_CMD_PACKET);
1593                 rd_raise_interrupt(dev, channel);
1594                 break;
1595               }
1596             }
1597             break;
1598           }
1599         case 0x3: // saved values not implemented
1600           {
1601             rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST, ASC_SAVING_PARAMETERS_NOT_SUPPORTED);
1602             rd_raise_interrupt(dev, channel);
1603             break;
1604           }
1605         default:
1606           {
1607             PrintError("Should not get here!\n");
1608             return -1;
1609             break;
1610           }
1611         }
1612         break;
1613       }
1614     case 0x12: // inquiry
1615       {
1616         uint8_t alloc_length = controller->buffer[4];
1617
1618         if (rd_init_send_atapi_command(dev, channel, atapi_command, 36, alloc_length, false) == -1) {
1619           return -1;
1620         }
1621
1622         controller->buffer[0] = 0x05; // CD-ROM
1623         controller->buffer[1] = 0x80; // Removable
1624         controller->buffer[2] = 0x00; // ISO, ECMA, ANSI version
1625         controller->buffer[3] = 0x21; // ATAPI-2, as specified
1626         controller->buffer[4] = 31; // additional length (total 36)
1627         controller->buffer[5] = 0x00; // reserved
1628         controller->buffer[6] = 0x00; // reserved
1629         controller->buffer[7] = 0x00; // reserved
1630
1631         // Vendor ID
1632         const char* vendor_id = "VTAB    ";
1633         int i;
1634         for (i = 0; i < 8; i++) {
1635           controller->buffer[8+i] = vendor_id[i];
1636         }
1637
1638         // Product ID
1639         const char* product_id = "Turbo CD-ROM    ";
1640         for (i = 0; i < 16; i++) {
1641           controller->buffer[16+i] = product_id[i];
1642         }
1643
1644         // Product Revision level
1645         const char* rev_level = "1.0 ";
1646         for (i = 0; i < 4; i++) {
1647           controller->buffer[32 + i] = rev_level[i];
1648         }
1649
1650         rd_ready_to_send_atapi(dev, channel);
1651         break;
1652       }
1653     case 0x25:  // read cd-rom capacity
1654       {
1655         // no allocation length???
1656         if (rd_init_send_atapi_command(dev, channel, atapi_command, 8, 8, false) == -1) {
1657           return -1;
1658         }
1659
1660         if (drive->cdrom.ready) {
1661           uint32_t capacity = drive->cdrom.capacity;
1662
1663           PrintDebug("\t\tCapacity is %d sectors (%d bytes)\n", capacity, capacity * 2048);
1664
1665           controller->buffer[0] = (capacity >> 24) & 0xff;
1666           controller->buffer[1] = (capacity >> 16) & 0xff;
1667           controller->buffer[2] = (capacity >> 8) & 0xff;
1668           controller->buffer[3] = (capacity >> 0) & 0xff;
1669           controller->buffer[4] = (2048 >> 24) & 0xff;
1670           controller->buffer[5] = (2048 >> 16) & 0xff;
1671           controller->buffer[6] = (2048 >> 8) & 0xff;
1672           controller->buffer[7] = (2048 >> 0) & 0xff;
1673
1674           rd_ready_to_send_atapi(dev, channel);
1675         } else {
1676           rd_atapi_cmd_error(dev, channel, SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT);
1677           rd_raise_interrupt(dev, channel);
1678         }
1679         break;
1680       }
1681
1682
1683     case 0xbe:  // read cd
1684       {
1685         if (drive->cdrom.ready) {
1686           PrintError("Read CD with CD present not implemented\n");
1687           rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST, ASC_INV_FIELD_IN_CMD_PACKET);
1688           rd_raise_interrupt(dev, channel);
1689         } else {
1690           rd_atapi_cmd_error(dev, channel, SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT);
1691           rd_raise_interrupt(dev, channel);
1692         }
1693         break;
1694       }
1695     case 0x43: // read toc
1696       {
1697         if (drive->cdrom.ready) {
1698           int toc_length;
1699           bool msf = (controller->buffer[1] >> 1) & 1;
1700           uint8_t starting_track = controller->buffer[6];
1701
1702           uint16_t alloc_length = rd_read_16bit(controller->buffer + 7);
1703
1704           uint8_t format = (controller->buffer[9] >> 6);
1705           int i;
1706           switch (format) {
1707           case 0:
1708
1709             if (!(drive->cdrom.cd->ops.read_toc(drive->cdrom.cd, controller->buffer,
1710                                                 &toc_length, msf, starting_track))) {
1711               rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST,
1712                                  ASC_INV_FIELD_IN_CMD_PACKET);
1713               rd_raise_interrupt(dev, channel);
1714             } else {
1715               if (rd_init_send_atapi_command(dev, channel, atapi_command, toc_length, alloc_length, false) == -1) {
1716                 return -1;
1717               }
1718               rd_ready_to_send_atapi(dev, channel);
1719             }
1720             break;
1721
1722           case 1:
1723             // multi session stuff. we ignore this and emulate a single session only
1724             if (rd_init_send_atapi_command(dev, channel, atapi_command, 12, alloc_length, false) == -1) {
1725               return -1;
1726             }
1727
1728             controller->buffer[0] = 0;
1729             controller->buffer[1] = 0x0a;
1730             controller->buffer[2] = 1;
1731             controller->buffer[3] = 1;
1732
1733             for (i = 0; i < 8; i++) {
1734               controller->buffer[4 + i] = 0;
1735             }
1736
1737             rd_ready_to_send_atapi(dev, channel);
1738             break;
1739
1740           case 2:
1741           default:
1742             PrintError("(READ TOC) Format %d not supported\n", format);
1743             return -1;
1744           }
1745         } else {
1746           rd_atapi_cmd_error(dev, channel, SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT);
1747           rd_raise_interrupt(dev, channel);
1748         }
1749         break;
1750       }
1751     case 0x28: // read (10)
1752     case 0xa8: // read (12)
1753       {
1754
1755         uint32_t transfer_length;
1756         if (atapi_command == 0x28) {
1757           transfer_length = rd_read_16bit(controller->buffer + 7);
1758         } else {
1759           transfer_length = rd_read_32bit(controller->buffer + 6);
1760         }
1761
1762         uint32_t lba = rd_read_32bit(controller->buffer + 2);
1763
1764         if (!(drive->cdrom.ready)) {
1765           rd_atapi_cmd_error(dev, channel, SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT);
1766           rd_raise_interrupt(dev, channel);
1767           break;
1768         }
1769
1770         if (transfer_length == 0) {
1771           rd_atapi_cmd_nop(dev, channel);
1772           rd_raise_interrupt(dev, channel);
1773           PrintDebug("\t\tREAD(%d) with transfer length 0, ok\n", (atapi_command == 0x28) ? 10 : 12);
1774           break;
1775         }
1776
1777         if (lba + transfer_length > drive->cdrom.capacity) {
1778           rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST, ASC_LOGICAL_BLOCK_OOR);
1779           rd_raise_interrupt(dev, channel);
1780           break;
1781         }
1782
1783         PrintDebug("\t\tcdrom: READ (%d) LBA=%d LEN=%d\n",
1784                    (atapi_command == 0x28) ? 10 : 12,
1785                    lba, transfer_length);
1786
1787         // handle command
1788         if (rd_init_send_atapi_command(dev, channel, atapi_command, transfer_length * 2048,
1789                                        transfer_length * 2048, true) == -1) {
1790           return -1;
1791         }
1792         drive->cdrom.remaining_blocks = transfer_length;
1793         drive->cdrom.next_lba = lba;
1794         rd_ready_to_send_atapi(dev, channel);
1795         break;
1796       }
1797     case 0x2b:  // seek
1798       {
1799         uint32_t lba = rd_read_32bit(controller->buffer + 2);
1800
1801         if (!(drive->cdrom.ready)) {
1802           rd_atapi_cmd_error(dev, channel, SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT);
1803           rd_raise_interrupt(dev, channel);
1804           break;
1805         }
1806
1807         if (lba > drive->cdrom.capacity) {
1808           rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST, ASC_LOGICAL_BLOCK_OOR);
1809           rd_raise_interrupt(dev, channel);
1810           break;
1811         }
1812
1813         PrintDebug("\t\tcdrom: SEEK (ignored)\n");
1814
1815         rd_atapi_cmd_nop(dev, channel);
1816         rd_raise_interrupt(dev, channel);
1817
1818         break;
1819       }
1820     case 0x1e:  // prevent/allow medium removal
1821       {
1822
1823         if (drive->cdrom.ready) {
1824           drive->cdrom.locked = controller->buffer[4] & 1;
1825           rd_atapi_cmd_nop(dev, channel);
1826         } else {
1827           rd_atapi_cmd_error(dev, channel, SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT);
1828         }
1829
1830         rd_raise_interrupt(dev, channel);
1831
1832         break;
1833       }
1834     case 0x42:  // read sub-channel
1835       {
1836         //bool msf = get_packet_field(channel,1, 1, 1);
1837         bool sub_q = get_packet_field(channel,2, 6, 1);
1838         //uint8_t data_format = get_packet_byte(channel,3);
1839         //uint8_t track_number = get_packet_byte(channel,6);
1840         uint16_t alloc_length = get_packet_word(channel,7);
1841
1842
1843         /*
1844           UNUSED(msf);
1845           UNUSED(data_format);
1846           UNUSED(track_number);
1847         */
1848         if (!(drive->cdrom.ready)) {
1849           rd_atapi_cmd_error(dev, channel, SENSE_NOT_READY, ASC_MEDIUM_NOT_PRESENT);
1850           rd_raise_interrupt(dev, channel);
1851         } else {
1852           controller->buffer[0] = 0;
1853           controller->buffer[1] = 0; // audio not supported
1854           controller->buffer[2] = 0;
1855           controller->buffer[3] = 0;
1856
1857           int ret_len = 4; // header size
1858
1859           if (sub_q) { // !sub_q == header only
1860             PrintError("Read sub-channel with SubQ not implemented\n");
1861             rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST,
1862                                ASC_INV_FIELD_IN_CMD_PACKET);
1863             rd_raise_interrupt(dev, channel);
1864           }
1865
1866           if (rd_init_send_atapi_command(dev, channel, atapi_command, ret_len, alloc_length, false) == -1) {
1867             return -1;
1868           }
1869           rd_ready_to_send_atapi(dev, channel);
1870         }
1871         break;
1872       }
1873     case 0x51:  // read disc info
1874       {
1875         // no-op to keep the Linux CD-ROM driver happy
1876         rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST, ASC_INV_FIELD_IN_CMD_PACKET);
1877         rd_raise_interrupt(dev, channel);
1878         break;
1879       }
1880     case 0x55: // mode select
1881     case 0xa6: // load/unload cd
1882     case 0x4b: // pause/resume
1883     case 0x45: // play audio
1884     case 0x47: // play audio msf
1885     case 0xbc: // play cd
1886     case 0xb9: // read cd msf
1887     case 0x44: // read header
1888     case 0xba: // scan
1889     case 0xbb: // set cd speed
1890     case 0x4e: // stop play/scan
1891     case 0x46: // ???
1892     case 0x4a: // ???
1893       PrintError("ATAPI command 0x%x not implemented yet\n",
1894                atapi_command);
1895       rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST, ASC_INV_FIELD_IN_CMD_PACKET);
1896       rd_raise_interrupt(dev, channel);
1897       break;
1898     default:
1899       PrintError("Unknown ATAPI command 0x%x (%d)\n",
1900                  atapi_command, atapi_command);
1901       // We'd better signal the error if the user chose to continue
1902       rd_atapi_cmd_error(dev, channel, SENSE_ILLEGAL_REQUEST, ASC_INV_FIELD_IN_CMD_PACKET);
1903       rd_raise_interrupt(dev, channel);
1904       break;
1905     }
1906   }
1907   return 0;
1908 }
1909
1910
1911
1912
1913 int rd_init_send_atapi_command(struct vm_device * dev, struct channel_t * channel, Bit8u command, int req_length, int alloc_length, bool lazy)
1914 {
1915   struct drive_t * drive = &(channel->drives[channel->drive_select]);
1916   struct controller_t * controller = &(drive->controller);
1917
1918   // controller->byte_count is a union of controller->cylinder_no;
1919   // lazy is used to force a data read in the buffer at the next read.
1920
1921   PrintDebug("[rd_init_send_atapi_cmd]\n");
1922
1923   if (controller->byte_count == 0xffff) {
1924     controller->byte_count = 0xfffe;
1925   }
1926
1927   if ((controller->byte_count & 1) &&
1928       !(alloc_length <= controller->byte_count)) {
1929
1930     PrintDebug("\t\tOdd byte count (0x%04x) to ATAPI command 0x%02x, using 0x%x\n",
1931                   controller->byte_count,
1932                   command,
1933                   controller->byte_count - 1);
1934
1935     controller->byte_count -= 1;
1936   }
1937
1938   if (controller->byte_count == 0) {
1939     PrintError("\t\tATAPI command with zero byte count\n");
1940     return -1;
1941   }
1942
1943   if (alloc_length < 0) {
1944     PrintError("\t\tAllocation length < 0\n");
1945     return -1;
1946   }
1947
1948   if (alloc_length == 0) {
1949     alloc_length = controller->byte_count;
1950   }
1951
1952   controller->interrupt_reason.i_o = 1;
1953   controller->interrupt_reason.c_d = 0;
1954   controller->status.busy = 0;
1955   controller->status.drq = 1;
1956   controller->status.err = 0;
1957
1958   // no bytes transfered yet
1959   if (lazy) {
1960     controller->buffer_index = 2048;
1961   } else {
1962     controller->buffer_index = 0;
1963   }
1964
1965   controller->drq_index = 0;
1966
1967   if (controller->byte_count > req_length) {
1968     controller->byte_count = req_length;
1969   }
1970
1971   if (controller->byte_count > alloc_length) {
1972     controller->byte_count = alloc_length;
1973   }
1974
1975   drive->atapi.command = command;
1976   drive->atapi.drq_bytes = controller->byte_count;
1977   drive->atapi.total_bytes_remaining = (req_length < alloc_length) ? req_length : alloc_length;
1978
1979   // if (lazy) {
1980   // // bias drq_bytes and total_bytes_remaining
1981   // SELECTED_DRIVE(channel).atapi.drq_bytes += 2048;
1982   // SELECTED_DRIVE(channel).atapi.total_bytes_remaining += 2048;
1983   // }
1984
1985   return 0;
1986 }
1987
1988
1989
1990  void rd_ready_to_send_atapi(struct vm_device * dev, struct channel_t * channel) {
1991   PrintDebug("[rd_ready_to_send_atapi]\n");
1992
1993   rd_raise_interrupt(dev, channel);
1994 }
1995
1996
1997
1998
1999
2000 void rd_atapi_cmd_error(struct vm_device * dev, struct channel_t * channel, sense_t sense_key, asc_t asc)
2001 {
2002   struct ramdisk_t *ramdisk = (struct ramdisk_t *)(dev->private_data);
2003   struct drive_t * drive = &(channel->drives[channel->drive_select]);
2004   struct controller_t * controller = &(drive->controller);
2005
2006   PrintDebug("[rd_atapi_cmd_error]\n");
2007   PrintDebug("Error: atapi_cmd_error channel=%02x key=%02x asc=%02x\n",
2008              get_channel_no(ramdisk, channel), sense_key, asc);
2009
2010   controller->error_register = sense_key << 4;
2011   controller->interrupt_reason.i_o = 1;
2012   controller->interrupt_reason.c_d = 1;
2013   controller->interrupt_reason.rel = 0;
2014   controller->status.busy = 0;
2015   controller->status.drive_ready = 1;
2016   controller->status.write_fault = 0;
2017   controller->status.drq = 0;
2018   controller->status.err = 1;
2019
2020   drive->sense.sense_key = sense_key;
2021   drive->sense.asc = asc;
2022   drive->sense.ascq = 0;
2023 }
2024
2025
2026
2027 void rd_atapi_cmd_nop(struct vm_device * dev, struct channel_t * channel)
2028 {
2029   struct drive_t * drive = &(channel->drives[channel->drive_select]);
2030   struct controller_t * controller = &(drive->controller);
2031
2032   PrintDebug("[rd_atapi_cmd_nop]\n");
2033   controller->interrupt_reason.i_o = 1;
2034   controller->interrupt_reason.c_d = 1;
2035   controller->interrupt_reason.rel = 0;
2036   controller->status.busy = 0;
2037   controller->status.drive_ready = 1;
2038   controller->status.drq = 0;
2039   controller->status.err = 0;
2040 }
2041
2042
2043
2044
2045 void rd_identify_ATAPI_drive(struct vm_device * dev, struct channel_t * channel)
2046 {
2047   struct drive_t * drive = &(channel->drives[channel->drive_select]);
2048   struct controller_t * controller = &(drive->controller);
2049
2050
2051   uint_t i;
2052   const char* serial_number = " VT00001\0\0\0\0\0\0\0\0\0\0\0\0";
2053   const char* firmware = "ALPHA1  ";
2054
2055   drive->id_drive[0] = (2 << 14) | (5 << 8) | (1 << 7) | (2 << 5) | (0 << 0); // Removable CDROM, 50us response, 12 byte packets
2056
2057   for (i = 1; i <= 9; i++) {
2058     drive->id_drive[i] = 0;
2059   }
2060
2061   for (i = 0; i < 10; i++) {
2062     drive->id_drive[10 + i] = ((serial_number[i * 2] << 8) |
2063                                (serial_number[(i * 2) + 1]));
2064   }
2065
2066   for (i = 20; i <= 22; i++) {
2067     drive->id_drive[i] = 0;
2068   }
2069
2070   for (i = 0; i < strlen(firmware)/2; i++) {
2071     drive->id_drive[23 + i] = ((firmware[i * 2] << 8) |
2072                                (firmware[(i * 2) + 1]));
2073   }
2074   V3_ASSERT((23 + i) == 27);
2075
2076   for (i = 0; i < strlen((char *)(drive->model_no)) / 2; i++) {
2077     drive->id_drive[27 + i] = ((drive->model_no[i * 2] << 8) |
2078                                (drive->model_no[(i * 2) + 1]));
2079   }
2080   V3_ASSERT((27 + i) == 47);
2081
2082   drive->id_drive[47] = 0;
2083   drive->id_drive[48] = 1; // 32 bits access
2084
2085   drive->id_drive[49] = (1 << 9); // LBA supported
2086
2087   drive->id_drive[50] = 0;
2088   drive->id_drive[51] = 0;
2089   drive->id_drive[52] = 0;
2090
2091   drive->id_drive[53] = 3; // words 64-70, 54-58 valid
2092
2093   for (i = 54; i <= 62; i++) {
2094     drive->id_drive[i] = 0;
2095   }
2096
2097   // copied from CFA540A
2098   drive->id_drive[63] = 0x0103; // variable (DMA stuff)
2099   drive->id_drive[64] = 0x0001; // PIO
2100   drive->id_drive[65] = 0x00b4;
2101   drive->id_drive[66] = 0x00b4;
2102   drive->id_drive[67] = 0x012c;
2103   drive->id_drive[68] = 0x00b4;
2104
2105   drive->id_drive[69] = 0;
2106   drive->id_drive[70] = 0;
2107   drive->id_drive[71] = 30; // faked
2108   drive->id_drive[72] = 30; // faked
2109   drive->id_drive[73] = 0;
2110   drive->id_drive[74] = 0;
2111
2112   drive->id_drive[75] = 0;
2113
2114   for (i = 76; i <= 79; i++) {
2115     drive->id_drive[i] = 0;
2116   }
2117
2118   drive->id_drive[80] = 0x1e; // supports up to ATA/ATAPI-4
2119   drive->id_drive[81] = 0;
2120   drive->id_drive[82] = 0;
2121   drive->id_drive[83] = 0;
2122   drive->id_drive[84] = 0;
2123   drive->id_drive[85] = 0;
2124   drive->id_drive[86] = 0;
2125   drive->id_drive[87] = 0;
2126   drive->id_drive[88] = 0;
2127
2128   for (i = 89; i <= 126; i++) {
2129     drive->id_drive[i] = 0;
2130   }
2131
2132   drive->id_drive[127] = 0;
2133   drive->id_drive[128] = 0;
2134
2135   for (i = 129; i <= 159; i++) {
2136     drive->id_drive[i] = 0;
2137   }
2138
2139   for (i = 160; i <= 255; i++) {
2140     drive->id_drive[i] = 0;
2141   }
2142
2143   // now convert the id_drive array (native 256 word format) to
2144   // the controller buffer (512 bytes)
2145   Bit16u temp16;
2146   for (i = 0; i <= 255; i++) {
2147     temp16 = drive->id_drive[i];
2148     controller->buffer[i * 2] = temp16 & 0x00ff;
2149     controller->buffer[i * 2 + 1] = temp16 >> 8;
2150   }
2151
2152   return;
2153 }
2154
2155
2156
2157
2158
2159
2160
2161 static
2162 void rd_init_mode_sense_single(struct vm_device * dev,
2163                                struct channel_t * channel, const void* src, int size)
2164 {
2165   struct drive_t * drive = &(channel->drives[channel->drive_select]);
2166   struct controller_t * controller = &(drive->controller);
2167
2168   PrintDebug("[rd_init_mode_sense_single]\n");
2169
2170   // Header
2171   controller->buffer[0] = (size + 6) >> 8;
2172   controller->buffer[1] = (size + 6) & 0xff;
2173   controller->buffer[2] = 0x70; // no media present
2174   controller->buffer[3] = 0; // reserved
2175   controller->buffer[4] = 0; // reserved
2176   controller->buffer[5] = 0; // reserved
2177   controller->buffer[6] = 0; // reserved
2178   controller->buffer[7] = 0; // reserved
2179
2180   // Data
2181   memcpy(controller->buffer + 8, src, size);
2182 }
2183
2184
2185
2186 static void rd_command_aborted(struct vm_device * dev,
2187                                struct channel_t * channel, unsigned value) {
2188   struct drive_t * drive = &(channel->drives[channel->drive_select]);
2189   struct controller_t * controller = &(drive->controller);
2190
2191   PrintDebug("[rd_command_aborted]\n");
2192   PrintDebug("\t\taborting on command 0x%02x {%s}\n", value, device_type_to_str(drive->device_type));
2193
2194   controller->current_command = 0;
2195   controller->status.busy = 0;
2196   controller->status.drive_ready = 1;
2197   controller->status.err = 1;
2198   controller->error_register = 0x04; // command ABORTED
2199   controller->status.drq = 0;
2200   controller->status.seek_complete = 0;
2201   controller->status.corrected_data = 0;
2202   controller->buffer_index = 0;
2203
2204   rd_raise_interrupt(dev, channel);
2205 }
2206
2207
2208 static int ramdisk_init_device(struct vm_device *dev) {
2209   struct ramdisk_t *ramdisk= (struct ramdisk_t *)dev->private_data;
2210
2211   PrintDebug("Initializing Ramdisk\n");
2212
2213
2214   rd_init_hardware(ramdisk);
2215
2216
2217   dev_hook_io(dev, PRI_CTRL_PORT,
2218               &read_status_port, &write_ctrl_port);
2219
2220   dev_hook_io(dev, PRI_DATA_PORT,
2221               &read_data_port, &write_data_port);
2222   dev_hook_io(dev, PRI_FEATURES_PORT,
2223               &read_general_port, &write_general_port);
2224   dev_hook_io(dev, PRI_SECT_CNT_PORT,
2225               &read_general_port, &write_general_port);
2226   dev_hook_io(dev, PRI_SECT_ADDR1_PORT,
2227               &read_general_port, &write_general_port);
2228   dev_hook_io(dev, PRI_SECT_ADDR2_PORT,
2229               &read_general_port, &write_general_port);
2230   dev_hook_io(dev, PRI_SECT_ADDR3_PORT,
2231               &read_general_port, &write_general_port);
2232   dev_hook_io(dev, PRI_DRV_SEL_PORT,
2233               &read_general_port, &write_general_port);
2234   dev_hook_io(dev, PRI_CMD_PORT,
2235               &read_status_port, &write_cmd_port);
2236
2237
2238   dev_hook_io(dev, SEC_CTRL_PORT,
2239               &read_status_port, &write_ctrl_port);
2240
2241   dev_hook_io(dev, SEC_DATA_PORT,
2242               &read_data_port, &write_data_port);
2243   dev_hook_io(dev, SEC_FEATURES_PORT,
2244               &read_general_port, &write_general_port);
2245   dev_hook_io(dev, SEC_SECT_CNT_PORT,
2246               &read_general_port, &write_general_port);
2247   dev_hook_io(dev, SEC_SECT_ADDR1_PORT,
2248               &read_general_port, &write_general_port);
2249   dev_hook_io(dev, SEC_SECT_ADDR2_PORT,
2250               &read_general_port, &write_general_port);
2251   dev_hook_io(dev, SEC_SECT_ADDR3_PORT,
2252               &read_general_port, &write_general_port);
2253   dev_hook_io(dev, SEC_DRV_SEL_PORT,
2254               &read_general_port, &write_general_port);
2255   dev_hook_io(dev, SEC_CMD_PORT,
2256               &read_status_port, &write_cmd_port);
2257
2258
2259
2260   dev_hook_io(dev, SEC_ADDR_REG_PORT,
2261               &read_general_port, &write_general_port);
2262
2263   dev_hook_io(dev, PRI_ADDR_REG_PORT,
2264               &read_general_port, &write_general_port);
2265
2266
2267
2268   return 0;
2269
2270 }
2271
2272
2273 static int ramdisk_deinit_device(struct vm_device *dev) {
2274   struct ramdisk_t *ramdisk = (struct ramdisk_t *)(dev->private_data);
2275   rd_close_harddrive(ramdisk);
2276   return 0;
2277 }
2278
2279 static struct vm_device_ops dev_ops = {
2280   .init = ramdisk_init_device,
2281   .deinit = ramdisk_deinit_device,
2282   .reset = NULL,
2283   .start = NULL,
2284   .stop = NULL,
2285 };
2286
2287
2288
2289
2290 struct vm_device *create_ramdisk()
2291 {
2292
2293   struct ramdisk_t *ramdisk;
2294   ramdisk = (struct ramdisk_t *)V3_Malloc(sizeof(struct ramdisk_t));
2295   V3_ASSERT(ramdisk != NULL);
2296
2297   PrintDebug("[create_ramdisk]\n");
2298
2299   struct vm_device *device = create_device("RAMDISK", &dev_ops, ramdisk);
2300
2301   return device;
2302 }
2303
2304
2305
2306
2307 #ifdef DEBUG_RAMDISK
2308
2309 static void rd_print_state(struct ramdisk_t * ramdisk) {
2310   uchar_t channel;
2311   uchar_t device;
2312   struct channel_t * channels = (struct channel_t *)(&(ramdisk->channels));
2313
2314   /*
2315   for (channel = 0; channel < MAX_ATA_CHANNEL; channel++) {
2316     memset((char *)(channels + channel), 0, sizeof(struct channel_t));
2317   }
2318   */
2319   PrintDebug("sizeof(*channels) = %d\n", sizeof(*channels));
2320   PrintDebug("sizeof(channles->drives[0].controller) = %d\n", sizeof((channels->drives[0].controller)));
2321   PrintDebug("sizeof(channles->drives[0].cdrom) = %d\n", sizeof((channels->drives[0].cdrom)));
2322   PrintDebug("sizeof(channles->drives[0].sense) = %d\n", sizeof((channels->drives[0].sense)));
2323   PrintDebug("sizeof(channles->drives[0].atapi) = %d\n", sizeof((channels->drives[0].atapi)));
2324
2325
2326   PrintDebug("sizeof(channles->drives[0].controller.status) = %d\n",
2327                 sizeof((channels->drives[0].controller.status)));
2328   PrintDebug("sizeof(channles->drives[0].controller.sector_count) = %d\n",
2329                 sizeof((channels->drives[0].controller.sector_count)));
2330   PrintDebug("sizeof(channles->drives[0].controller.interrupt_reason) = %d\n",
2331                 sizeof((channels->drives[0].controller.interrupt_reason)));
2332
2333   PrintDebug("sizeof(channles->drives[0].controller.cylinder_no) = %d\n",
2334                 sizeof((channels->drives[0].controller.cylinder_no)));
2335   PrintDebug("sizeof(channles->drives[0].controller.byte_count) = %d\n",
2336                 sizeof((channels->drives[0].controller.byte_count)));
2337
2338
2339   PrintDebug("sizeof(channles->drives[0].controller.control) = %d\n",
2340                 sizeof((channels->drives[0].controller.control)));
2341
2342
2343   for (channel = 0; channel < MAX_ATA_CHANNEL; channel++){
2344
2345     for (device = 0; device < 2; device++){
2346
2347       // Initialize controller state, even if device is not present
2348       PrintDebug("channels[%d].drives[%d].controller.status.busy = %d\n",
2349                     channel, device,
2350                     channels[channel].drives[device].controller.status.busy);
2351       PrintDebug("channels[%d].drives[%d].controller.status.drive_ready = %d\n",
2352                     channel, device,
2353                     channels[channel].drives[device].controller.status.drive_ready);
2354       PrintDebug("channels[%d].drives[%d].controller.status.write_fault = %d\n",
2355                     channel, device,
2356                     channels[channel].drives[device].controller.status.write_fault);
2357       PrintDebug("channels[%d].drives[%d].controller.status.seek_complete = %d\n",
2358                     channel, device,
2359                     channels[channel].drives[device].controller.status.seek_complete);
2360       PrintDebug("channels[%d].drives[%d].controller.status.drq = %d\n",
2361                     channel, device,
2362                     channels[channel].drives[device].controller.status.drq);
2363       PrintDebug("channels[%d].drives[%d].controller.status.corrected_data = %d\n",
2364                     channel, device,
2365                     channels[channel].drives[device].controller.status.corrected_data);
2366       PrintDebug("channels[%d].drives[%d].controller.status.index_pulse = %d\n",
2367                     channel, device,
2368                     channels[channel].drives[device].controller.status.index_pulse);
2369       PrintDebug("channels[%d].drives[%d].controller.status.index_pulse_count = %d\n",
2370                     channel, device,
2371                     channels[channel].drives[device].controller.status.index_pulse_count);
2372       PrintDebug("channels[%d].drives[%d].controller.status.err = %d\n",
2373                     channel, device,
2374                     channels[channel].drives[device].controller.status.err);
2375
2376
2377       PrintDebug("channels[%d].drives[%d].controller.error_register = %d\n",
2378                     channel, device,
2379                     channels[channel].drives[device].controller.error_register);
2380       PrintDebug("channels[%d].drives[%d].controller.head_no = %d\n",
2381                     channel, device,
2382                     channels[channel].drives[device].controller.head_no);
2383       PrintDebug("channels[%d].drives[%d].controller.sector_count = %d\n",
2384                     channel, device,
2385                     channels[channel].drives[device].controller.sector_count);
2386       PrintDebug("channels[%d].drives[%d].controller.sector_no = %d\n",
2387                     channel, device,
2388                     channels[channel].drives[device].controller.sector_no);
2389       PrintDebug("channels[%d].drives[%d].controller.cylinder_no = %d\n",
2390                     channel, device,
2391                     channels[channel].drives[device].controller.cylinder_no);
2392       PrintDebug("channels[%d].drives[%d].controller.current_command = %02x\n",
2393                     channel, device,
2394                     channels[channel].drives[device].controller.current_command);
2395       PrintDebug("channels[%d].drives[%d].controller.buffer_index = %d\n",
2396                     channel, device,
2397                     channels[channel].drives[device].controller.buffer_index);
2398
2399
2400       PrintDebug("channels[%d].drives[%d].controller.control.reset = %d\n",
2401                     channel, device,
2402                     channels[channel].drives[device].controller.control.reset);
2403       PrintDebug("channels[%d].drives[%d].controller.control.disable_irq = %d\n",
2404                     channel, device,
2405                     channels[channel].drives[device].controller.control.disable_irq);
2406
2407
2408       PrintDebug("channels[%d].drives[%d].controller.reset_in_progress = %d\n",
2409                     channel, device,
2410                     channels[channel].drives[device].controller.reset_in_progress);
2411       PrintDebug("channels[%d].drives[%d].controller.sectors_per_block = %02x\n",
2412                     channel, device,
2413                     channels[channel].drives[device].controller.sectors_per_block);
2414       PrintDebug("channels[%d].drives[%d].controller.lba_mode = %d\n",
2415                     channel, device,
2416                     channels[channel].drives[device].controller.lba_mode);
2417       PrintDebug("channels[%d].drives[%d].controller.features = %d\n",
2418                     channel, device,
2419                     channels[channel].drives[device].controller.features);
2420
2421
2422       PrintDebug("channels[%d].drives[%d].model_no = %s\n",
2423                     channel, device,
2424                     channels[channel].drives[device].model_no);
2425       PrintDebug("channels[%d].drives[%d].device_type = %d\n",
2426                     channel, device,
2427                     channels[channel].drives[device].device_type);
2428       PrintDebug("channels[%d].drives[%d].cdrom.locked = %d\n",
2429                     channel, device,
2430                     channels[channel].drives[device].cdrom.locked);
2431       PrintDebug("channels[%d].drives[%d].sense.sense_key = %d\n",
2432                     channel, device,
2433                     channels[channel].drives[device].sense.sense_key);
2434       PrintDebug("channels[%d].drives[%d].sense.asc = %d\n",
2435                     channel, device,
2436                     channels[channel].drives[device].sense.asc);
2437       PrintDebug("channels[%d].drives[%d].sense.ascq = %d\n",
2438                     channel, device,
2439                     channels[channel].drives[device].sense.ascq);
2440
2441
2442
2443       PrintDebug("channels[%d].drives[%d].controller.interrupt_reason.c_d = %02x\n",
2444                     channel, device,
2445                     channels[channel].drives[device].controller.interrupt_reason.c_d);
2446
2447       PrintDebug("channels[%d].drives[%d].controller.interrupt_reason.i_o = %02x\n",
2448                     channel, device,
2449                     channels[channel].drives[device].controller.interrupt_reason.i_o);
2450
2451       PrintDebug("channels[%d].drives[%d].controller.interrupt_reason.rel = %02x\n",
2452                     channel, device,
2453                     channels[channel].drives[device].controller.interrupt_reason.rel);
2454
2455       PrintDebug("channels[%d].drives[%d].controller.interrupt_reason.tag = %02x\n",
2456                     channel, device,
2457                     channels[channel].drives[device].controller.interrupt_reason.tag);
2458
2459       PrintDebug("channels[%d].drives[%d].cdrom.ready = %d\n",
2460                     channel, device,
2461                     channels[channel].drives[device].cdrom.ready);
2462
2463     }  //for device
2464   }  //for channel
2465
2466   return;
2467 }
2468
2469 #if 0
2470 static void trace_info(ushort_t port, void *src, uint_t length) {
2471
2472   switch(port){
2473
2474   case 0x3e8:
2475     if (length == 1 && *((uchar_t*) src) == ATA_DETECT)
2476       PrintDebug("ata_detect()\n");
2477     break;
2478
2479   case 0x3e9:
2480     if (length == 1 && *((uchar_t*) src) == ATA_RESET)
2481       PrintDebug("ata_reset()\n");
2482     break;
2483
2484   case 0x3ea:
2485     if (length == 1 && *((uchar_t*) src) == ATA_CMD_DATA_IN)
2486       PrintDebug("ata_cmd_data_in()\n");
2487     break;
2488
2489   case 0x3eb:
2490     if (length == 1 && *((uchar_t*) src) == ATA_CMD_DATA_OUT)
2491       PrintDebug("ata_cmd_data_out()\n");
2492     break;
2493
2494   case 0x3ec:
2495     if (length == 1 && *((uchar_t*) src) == ATA_CMD_PACKET)
2496       PrintDebug("ata_cmd_packet()\n");
2497     break;
2498
2499   case 0x3ed:
2500     if (length == 1 && *((uchar_t*) src) == ATAPI_GET_SENSE)
2501       PrintDebug("atapi_get_sense()\n");
2502     break;
2503
2504   case 0x3ee:
2505     if (length == 1 && *((uchar_t*) src) == ATAPI_IS_READY)
2506       PrintDebug("atapi_is_ready()\n");
2507     break;
2508
2509   case 0x3ef:
2510     if (length == 1 && *((uchar_t*) src) == ATAPI_IS_CDROM)
2511       PrintDebug("atapi_is_cdrom()\n");
2512     break;
2513
2514
2515   case 0x2e8:
2516     if (length == 1 && *((uchar_t*) src) == CDEMU_INIT)
2517       PrintDebug("cdemu_init()\n");
2518     break;
2519
2520   case 0x2e9:
2521     if (length == 1 && *((uchar_t*) src) == CDEMU_ISACTIVE)
2522       PrintDebug("cdemu_isactive()\n");
2523     break;
2524
2525   case 0x2ea:
2526     if (length == 1 && *((uchar_t*) src) == CDEMU_EMULATED_DRIVE)
2527       PrintDebug("cdemu_emulated_drive()\n");
2528     break;
2529
2530   case 0x2eb:
2531     if (length == 1 && *((uchar_t*) src) == CDROM_BOOT)
2532       PrintDebug("cdrom_boot()\n");
2533     break;
2534
2535   case 0x2ec:
2536     if (length == 1 && *((uchar_t*) src) == HARD_DRIVE_POST)
2537       PrintDebug("ata_hard_drive_post()\n");
2538     break;
2539
2540   case 0x2ed:
2541     if (length == 1)
2542       PrintDebug("ata_device_no(%d)\n", *((uchar_t*) src));
2543     break;
2544
2545   case 0x2ee:
2546     if (length == 1)
2547       PrintDebug("ata_device_type(%d)\n", *((uchar_t*) src));
2548     break;
2549
2550   case 0x2ef:
2551     if (length == 1 && *((uchar_t*) src) == INT13_HARDDISK)
2552       PrintDebug("int13_harddrive()\n");
2553     break;
2554
2555   case 0x2f8:
2556     if (length == 1 && *((uchar_t*) src) == INT13_CDROM)
2557       PrintDebug("int13_cdrom()\n");
2558     break;
2559
2560   case 0x2f9:
2561     if (length == 1 && *((uchar_t*) src) == INT13_CDEMU)
2562       PrintDebug("int13_cdemu()\n");
2563     break;
2564
2565   case 0x2fa:
2566     if (length == 1 && *((uchar_t*) src) == INT13_ELTORITO)
2567       PrintDebug("int13_eltorito()\n");
2568     break;
2569
2570   case 0x2fb:
2571     if (length == 1 && *((uchar_t*) src) == INT13_DISKETTE_FUNCTION)
2572       PrintDebug("int13_diskette_function()\n");
2573     break;
2574
2575
2576   default:
2577     break;
2578   }
2579 }
2580
2581 #endif
2582
2583 static int check_bit_fields(struct controller_t * controller) {
2584   //Check bit fields
2585   controller->sector_count = 0;
2586   controller->interrupt_reason.c_d = 1;
2587   if (controller->sector_count != 0x01) {
2588     return INTR_REASON_BIT_ERR;
2589   }
2590
2591   controller->sector_count = 0;
2592   controller->interrupt_reason.i_o = 1;
2593   if (controller->sector_count != 0x02) {
2594     return INTR_REASON_BIT_ERR;
2595   }
2596
2597   controller->sector_count = 0;
2598   controller->interrupt_reason.rel = 1;
2599   if (controller->sector_count != 0x04) {
2600     return INTR_REASON_BIT_ERR;
2601   }
2602
2603   controller->sector_count = 0;
2604   controller->interrupt_reason.tag = 3;
2605   if (controller->sector_count != 0x18) {
2606     return INTR_REASON_BIT_ERR;
2607   }
2608
2609   return 0;
2610 }
2611 #endif