Palacios Public Git Repository

To checkout Palacios execute

  git clone http://v3vee.org/palacios/palacios.web/palacios.git
This will give you the master branch. You probably want the devel branch or one of the release branches. To switch to the devel branch, simply execute
  cd palacios
  git checkout --track -b devel origin/devel
The other branches are similar.


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