Initial cd emulation code.
[people/mcb30/legacybios.git] / src / disk.c
1 // 16bit code to access hard drives.
2 //
3 // Copyright (C) 2008  Kevin O'Connor <kevin@koconnor.net>
4 // Copyright (C) 2002  MandrakeSoft S.A.
5 //
6 // This file may be distributed under the terms of the GNU GPLv3 license.
7
8 #include "disk.h" // floppy_13
9 #include "biosvar.h" // struct bregs
10 #include "config.h" // CONFIG_*
11 #include "cmos.h" // inb_cmos
12 #include "util.h" // debug_enter
13 #include "ata.h" // ATA_*
14
15
16 /****************************************************************
17  * Helper functions
18  ****************************************************************/
19
20 static inline void
21 disk_ret(struct bregs *regs, u8 code)
22 {
23     regs->ah = code;
24     SET_BDA(disk_last_status, code);
25     set_cf(regs, code);
26 }
27
28 #define DISK_STUB(regs) do {                    \
29         struct bregs *__regs = (regs);          \
30         debug_stub(__regs);                     \
31         disk_ret(__regs, DISK_RET_SUCCESS);     \
32     } while (0)
33
34 static u8
35 checksum_seg(u16 seg, u16 offset, u32 len)
36 {
37     u32 i;
38     u8 sum = 0;
39     for (i=0; i<len; i++)
40         sum += GET_FARVAR(seg, *(u8*)(offset+i));
41     return sum;
42 }
43
44 static void
45 basic_access(struct bregs *regs, u8 device, u16 command)
46 {
47     u16 count       = regs->al;
48     u16 cylinder    = regs->ch | ((((u16) regs->cl) << 2) & 0x300);
49     u16 sector      = regs->cl & 0x3f;
50     u16 head        = regs->dh;
51
52     if ((count > 128) || (count == 0) || (sector == 0)) {
53         BX_INFO("int13_harddisk: function %02x, parameter out of range!\n"
54                 , regs->ah);
55         disk_ret(regs, DISK_RET_EPARAM);
56         return;
57     }
58
59     u16 nlc   = GET_EBDA(ata.devices[device].lchs.cylinders);
60     u16 nlh   = GET_EBDA(ata.devices[device].lchs.heads);
61     u16 nlspt = GET_EBDA(ata.devices[device].lchs.spt);
62     u16 nph   = GET_EBDA(ata.devices[device].pchs.heads);
63     u16 npspt = GET_EBDA(ata.devices[device].pchs.spt);
64
65     // sanity check on cyl heads, sec
66     if ( (cylinder >= nlc) || (head >= nlh) || (sector > nlspt )) {
67         BX_INFO("int13_harddisk: function %02x, parameters out of"
68                 " range %04x/%04x/%04x!\n"
69                 , regs->ah, cylinder, head, sector);
70         disk_ret(regs, DISK_RET_EPARAM);
71         return;
72     }
73
74     u32 lba = 0;
75     // if needed, translate lchs to lba, and execute command
76     if ( (nph != nlh) || (npspt != nlspt)) {
77         lba = (((((u32)cylinder * (u32)nlh) + (u32)head) * (u32)nlspt)
78                + (u32)sector - 1);
79         sector = 0; // this forces the command to be lba
80     }
81
82     u16 segment = regs->es;
83     u16 offset  = regs->bx;
84
85     u8 status;
86     switch (command) {
87     case ATA_CMD_READ_SECTORS:
88         status = ata_cmd_data_in(device, ATA_CMD_READ_SECTORS
89                                  , count, cylinder, head, sector
90                                  , lba, segment, offset);
91         break;
92     case ATA_CMD_WRITE_SECTORS:
93         status = ata_cmd_data_out(device, ATA_CMD_WRITE_SECTORS
94                                   , count, cylinder, head, sector
95                                   , lba, segment, offset);
96         break;
97     default:
98         disk_ret(regs, DISK_RET_SUCCESS);
99         return;
100     }
101
102     // Set nb of sector transferred
103     regs->al = GET_EBDA(ata.trsfsectors);
104
105     if (status != 0) {
106         BX_INFO("int13_harddisk: function %02x, error %02x !\n",regs->ah,status);
107         disk_ret(regs, DISK_RET_EBADTRACK);
108     }
109     disk_ret(regs, DISK_RET_SUCCESS);
110 }
111
112 static void
113 emu_access(struct bregs *regs, u8 device, u16 command)
114 {
115     u16 nbsectors   = regs->al;
116     u16 cylinder    = regs->ch | ((((u16) regs->cl) << 2) & 0x300);
117     u16 sector      = regs->cl & 0x3f;
118     u16 head        = regs->dh;
119
120     if ((nbsectors > 128) || (nbsectors == 0) || (sector == 0)) {
121         BX_INFO("int13_harddisk: function %02x, parameter out of range!\n"
122                 , regs->ah);
123         disk_ret(regs, DISK_RET_EPARAM);
124         return;
125     }
126
127     u16 nlc   = GET_EBDA(cdemu.vdevice.cylinders);
128     u16 nlh   = GET_EBDA(cdemu.vdevice.heads);
129     u16 nlspt = GET_EBDA(cdemu.vdevice.spt);
130
131     // sanity check on cyl heads, sec
132     if ( (cylinder >= nlc) || (head >= nlh) || (sector > nlspt )) {
133         BX_INFO("int13_harddisk: function %02x, parameters out of"
134                 " range %04x/%04x/%04x!\n"
135                 , regs->ah, cylinder, head, sector);
136         disk_ret(regs, DISK_RET_EPARAM);
137         return;
138     }
139
140     if (!command) {
141         // If verify or seek
142         disk_ret(regs, DISK_RET_SUCCESS);
143         return;
144     }
145
146     u32 ilba = GET_EBDA(cdemu.ilba);
147     // calculate the virtual lba inside the image
148     u32 vlba= (((((u32)cylinder*(u32)nlh)+(u32)head)*(u32)nlspt)
149                +((u32)(sector-1)));
150     // start lba on cd
151     u32 slba  = (u32)vlba/4;
152     u16 before= (u16)vlba%4;
153     // end lba on cd
154     u32 elba = (u32)(vlba+nbsectors-1)/4;
155     u32 count = elba-slba+1;
156     u32 lba = ilba+slba;
157
158     u16 segment = regs->es;
159     u16 offset  = regs->bx;
160
161     u8 atacmd[12];
162     memset(atacmd, 0, sizeof(atacmd));
163     atacmd[0]=0x28;                      // READ command
164     atacmd[7]=(count & 0xff00) >> 8;     // Sectors
165     atacmd[8]=(count & 0x00ff);          // Sectors
166     atacmd[2]=(lba & 0xff000000) >> 24;  // LBA
167     atacmd[3]=(lba & 0x00ff0000) >> 16;
168     atacmd[4]=(lba & 0x0000ff00) >> 8;
169     atacmd[5]=(lba & 0x000000ff);
170
171     u8 status = ata_cmd_packet(device, (u32)atacmd, sizeof(atacmd)
172                                , before*512, count*2048L
173                                , ATA_DATA_IN, segment, offset);
174
175     if (status != 0) {
176         BX_INFO("int13_harddisk: function %02x, error %02x !\n",regs->ah,status);
177         regs->al = 0;
178         disk_ret(regs, DISK_RET_EBADTRACK);
179     }
180     regs->al = nbsectors;
181     disk_ret(regs, DISK_RET_SUCCESS);
182 }
183
184 static void
185 extended_access(struct bregs *regs, u8 device, u16 command)
186 {
187     u16 count = GET_INT13EXT(regs, count);
188     u16 segment = GET_INT13EXT(regs, segment);
189     u16 offset = GET_INT13EXT(regs, offset);
190
191     // Can't use 64 bits lba
192     u32 lba = GET_INT13EXT(regs, lba2);
193     if (lba != 0L) {
194         BX_PANIC("int13_harddisk: function %02x. Can't use 64bits lba\n"
195                  , regs->ah);
196         disk_ret(regs, DISK_RET_EPARAM);
197         return;
198     }
199
200     u8 type = GET_EBDA(ata.devices[device].type);
201
202     // Get 32 bits lba and check
203     lba = GET_INT13EXT(regs, lba1);
204     if (type == ATA_TYPE_ATA
205         && lba >= GET_EBDA(ata.devices[device].sectors)) {
206         BX_INFO("int13_harddisk: function %02x. LBA out of range\n", regs->ah);
207         disk_ret(regs, DISK_RET_EPARAM);
208         return;
209     }
210
211     u8 status;
212     switch (command) {
213     case ATA_CMD_READ_SECTORS:
214         if (type == ATA_TYPE_ATA) {
215             status = ata_cmd_data_in(device, ATA_CMD_READ_SECTORS
216                                      , count, 0, 0, 0
217                                      , lba, segment, offset);
218         } else {
219             u8 atacmd[12];
220             memset(atacmd, 0, sizeof(atacmd));
221             atacmd[0]=0x28;                      // READ command
222             atacmd[7]=(count & 0xff00) >> 8;     // Sectors
223             atacmd[8]=(count & 0x00ff);          // Sectors
224             atacmd[2]=(lba & 0xff000000) >> 24;  // LBA
225             atacmd[3]=(lba & 0x00ff0000) >> 16;
226             atacmd[4]=(lba & 0x0000ff00) >> 8;
227             atacmd[5]=(lba & 0x000000ff);
228
229             status = ata_cmd_packet(device, (u32)atacmd, sizeof(atacmd)
230                                     , 0, count*2048L
231                                     , ATA_DATA_IN, segment, offset);
232         }
233         break;
234     case ATA_CMD_WRITE_SECTORS:
235         status = ata_cmd_data_out(device, ATA_CMD_WRITE_SECTORS
236                                   , count, 0, 0, 0
237                                   , lba, segment, offset);
238         break;
239     default:
240         // If verify or seek
241         disk_ret(regs, DISK_RET_SUCCESS);
242         return;
243     }
244
245     SET_INT13EXT(regs, count, GET_EBDA(ata.trsfsectors));
246
247     if (status != 0) {
248         BX_INFO("int13_harddisk: function %02x, error %02x !\n"
249                 , regs->ah, status);
250         disk_ret(regs, DISK_RET_EBADTRACK);
251         return;
252     }
253     disk_ret(regs, DISK_RET_SUCCESS);
254 }
255
256
257 /****************************************************************
258  * Hard Drive functions
259  ****************************************************************/
260
261 // disk controller reset
262 static void
263 disk_1300(struct bregs *regs, u8 device)
264 {
265     ata_reset(device);
266 }
267
268 // read disk status
269 static void
270 disk_1301(struct bregs *regs, u8 device)
271 {
272     regs->ah = GET_BDA(disk_last_status);
273     disk_ret(regs, DISK_RET_SUCCESS);
274 }
275
276 // read disk sectors
277 static void
278 disk_1302(struct bregs *regs, u8 device)
279 {
280     basic_access(regs, device, ATA_CMD_READ_SECTORS);
281 }
282
283 // write disk sectors
284 static void
285 disk_1303(struct bregs *regs, u8 device)
286 {
287     basic_access(regs, device, ATA_CMD_WRITE_SECTORS);
288 }
289
290 // verify disk sectors
291 static void
292 disk_1304(struct bregs *regs, u8 device)
293 {
294     basic_access(regs, device, 0);
295     // FIXME verify
296 }
297
298 // format disk track
299 static void
300 disk_1305(struct bregs *regs, u8 device)
301 {
302     DISK_STUB(regs);
303 }
304
305 // read disk drive parameters
306 static void
307 disk_1308(struct bregs *regs, u8 device)
308 {
309     // Get logical geometry from table
310     u16 nlc = GET_EBDA(ata.devices[device].lchs.cylinders);
311     u16 nlh = GET_EBDA(ata.devices[device].lchs.heads);
312     u16 nlspt = GET_EBDA(ata.devices[device].lchs.spt);
313     u16 count = GET_EBDA(ata.hdcount);
314
315     nlc = nlc - 2; /* 0 based , last sector not used */
316     regs->al = 0;
317     regs->ch = nlc & 0xff;
318     regs->cl = ((nlc >> 2) & 0xc0) | (nlspt & 0x3f);
319     regs->dh = nlh - 1;
320     regs->dl = count; /* FIXME returns 0, 1, or n hard drives */
321
322     // FIXME should set ES & DI
323     disk_ret(regs, DISK_RET_SUCCESS);
324 }
325
326 // initialize drive parameters
327 static void
328 disk_1309(struct bregs *regs, u8 device)
329 {
330     DISK_STUB(regs);
331 }
332
333 // seek to specified cylinder
334 static void
335 disk_130c(struct bregs *regs, u8 device)
336 {
337     DISK_STUB(regs);
338 }
339
340 // alternate disk reset
341 static void
342 disk_130d(struct bregs *regs, u8 device)
343 {
344     DISK_STUB(regs);
345 }
346
347 // check drive ready
348 static void
349 disk_1310(struct bregs *regs, u8 device)
350 {
351     // should look at 40:8E also???
352
353     // Read the status from controller
354     u8 status = inb(GET_EBDA(ata.channels[device/2].iobase1) + ATA_CB_STAT);
355     if ( (status & ( ATA_CB_STAT_BSY | ATA_CB_STAT_RDY )) == ATA_CB_STAT_RDY )
356         disk_ret(regs, DISK_RET_SUCCESS);
357     else
358         disk_ret(regs, DISK_RET_ENOTREADY);
359 }
360
361 // recalibrate
362 static void
363 disk_1311(struct bregs *regs, u8 device)
364 {
365     DISK_STUB(regs);
366 }
367
368 // controller internal diagnostic
369 static void
370 disk_1314(struct bregs *regs, u8 device)
371 {
372     DISK_STUB(regs);
373 }
374
375 // read disk drive size
376 static void
377 disk_1315(struct bregs *regs, u8 device)
378 {
379     // Get logical geometry from table
380     u16 nlc   = GET_EBDA(ata.devices[device].lchs.cylinders);
381     u16 nlh   = GET_EBDA(ata.devices[device].lchs.heads);
382     u16 nlspt = GET_EBDA(ata.devices[device].lchs.spt);
383
384     // Compute sector count seen by int13
385     u32 lba = (u32)(nlc - 1) * (u32)nlh * (u32)nlspt;
386     regs->cx = lba >> 16;
387     regs->dx = lba & 0xffff;
388
389     disk_ret(regs, 0);
390     regs->ah = 3; // hard disk accessible
391 }
392
393 // IBM/MS installation check
394 static void
395 disk_1341(struct bregs *regs, u8 device)
396 {
397     regs->bx = 0xaa55;  // install check
398     regs->cx = 0x0007;  // ext disk access and edd, removable supported
399     disk_ret(regs, DISK_RET_SUCCESS);
400     regs->ah = 0x30;    // EDD 3.0
401 }
402
403 // IBM/MS extended read
404 static void
405 disk_1342(struct bregs *regs, u8 device)
406 {
407     extended_access(regs, device, ATA_CMD_READ_SECTORS);
408 }
409
410 // IBM/MS extended write
411 static void
412 disk_1343(struct bregs *regs, u8 device)
413 {
414     extended_access(regs, device, ATA_CMD_WRITE_SECTORS);
415 }
416
417 // IBM/MS verify
418 static void
419 disk_1344(struct bregs *regs, u8 device)
420 {
421     extended_access(regs, device, 0);
422 }
423
424 // IBM/MS lock/unlock drive
425 static void
426 disk_1345(struct bregs *regs, u8 device)
427 {
428     // Always success for HD
429     disk_ret(regs, DISK_RET_SUCCESS);
430 }
431
432 // IBM/MS eject media
433 static void
434 disk_1346(struct bregs *regs, u8 device)
435 {
436     // Volume Not Removable
437     disk_ret(regs, DISK_RET_ENOTREMOVABLE);
438 }
439
440 // IBM/MS extended seek
441 static void
442 disk_1347(struct bregs *regs, u8 device)
443 {
444     extended_access(regs, device, 0);
445 }
446
447 // IBM/MS get drive parameters
448 static void
449 disk_1348(struct bregs *regs, u8 device)
450 {
451     u16 size = GET_INT13DPT(regs, size);
452
453     // Buffer is too small
454     if (size < 0x1a) {
455         disk_ret(regs, DISK_RET_EPARAM);
456         return;
457     }
458
459     // EDD 1.x
460
461     u8  type    = GET_EBDA(ata.devices[device].type);
462     u16 npc     = GET_EBDA(ata.devices[device].pchs.cylinders);
463     u16 nph     = GET_EBDA(ata.devices[device].pchs.heads);
464     u16 npspt   = GET_EBDA(ata.devices[device].pchs.spt);
465     u32 lba     = GET_EBDA(ata.devices[device].sectors);
466     u16 blksize = GET_EBDA(ata.devices[device].blksize);
467
468     SET_INT13DPT(regs, size, 0x1a);
469     if (type == ATA_TYPE_ATA) {
470         if ((lba/npspt)/nph > 0x3fff) {
471             SET_INT13DPT(regs, infos, 0x00); // geometry is invalid
472             SET_INT13DPT(regs, cylinders, 0x3fff);
473         } else {
474             SET_INT13DPT(regs, infos, 0x02); // geometry is valid
475             SET_INT13DPT(regs, cylinders, (u32)npc);
476         }
477         SET_INT13DPT(regs, heads, (u32)nph);
478         SET_INT13DPT(regs, spt, (u32)npspt);
479         SET_INT13DPT(regs, sector_count1, lba);  // FIXME should be Bit64
480         SET_INT13DPT(regs, sector_count2, 0L);
481     } else {
482         // ATAPI
483         // 0x74 = removable, media change, lockable, max values
484         SET_INT13DPT(regs, infos, 0x74);
485         SET_INT13DPT(regs, cylinders, 0xffffffff);
486         SET_INT13DPT(regs, heads, 0xffffffff);
487         SET_INT13DPT(regs, spt, 0xffffffff);
488         SET_INT13DPT(regs, sector_count1, 0xffffffff);  // FIXME should be Bit64
489         SET_INT13DPT(regs, sector_count2, 0xffffffff);
490     }
491     SET_INT13DPT(regs, blksize, blksize);
492
493     if (size < 0x1e) {
494         disk_ret(regs, DISK_RET_SUCCESS);
495         return;
496     }
497
498     // EDD 2.x
499
500     SET_INT13DPT(regs, size, 0x1e);
501
502     SET_INT13DPT(regs, dpte_segment, EBDA_SEG);
503     SET_INT13DPT(regs, dpte_offset
504                  , offsetof(struct extended_bios_data_area_s, ata.dpte));
505
506     // Fill in dpte
507     u8 channel = device / 2;
508     u16 iobase1 = GET_EBDA(ata.channels[channel].iobase1);
509     u16 iobase2 = GET_EBDA(ata.channels[channel].iobase2);
510     u8 irq = GET_EBDA(ata.channels[channel].irq);
511     u8 mode = GET_EBDA(ata.devices[device].mode);
512
513     u16 options;
514     if (type == ATA_TYPE_ATA) {
515         u8 translation = GET_EBDA(ata.devices[device].translation);
516         options  = (translation==ATA_TRANSLATION_NONE?0:1)<<3; // chs translation
517         options |= (translation==ATA_TRANSLATION_LBA?1:0)<<9;
518         options |= (translation==ATA_TRANSLATION_RECHS?3:0)<<9;
519     } else {
520         // ATAPI
521         options  = (1<<5); // removable device
522         options |= (1<<6); // atapi device
523     }
524     options |= (1<<4); // lba translation
525     options |= (mode==ATA_MODE_PIO32?1:0)<<7;
526
527     SET_EBDA(ata.dpte.iobase1, iobase1);
528     SET_EBDA(ata.dpte.iobase2, iobase2 + ATA_CB_DC);
529     SET_EBDA(ata.dpte.prefix, (0xe | (device % 2))<<4 );
530     SET_EBDA(ata.dpte.unused, 0xcb );
531     SET_EBDA(ata.dpte.irq, irq );
532     SET_EBDA(ata.dpte.blkcount, 1 );
533     SET_EBDA(ata.dpte.dma, 0 );
534     SET_EBDA(ata.dpte.pio, 0 );
535     SET_EBDA(ata.dpte.options, options);
536     SET_EBDA(ata.dpte.reserved, 0);
537     if (size >= 0x42)
538         SET_EBDA(ata.dpte.revision, 0x11);
539     else
540         SET_EBDA(ata.dpte.revision, 0x10);
541
542     u8 sum = checksum_seg(EBDA_SEG
543                           , offsetof(struct extended_bios_data_area_s, ata.dpte)
544                           , 15);
545     SET_EBDA(ata.dpte.checksum, ~sum);
546
547     if (size < 0x42) {
548         disk_ret(regs, DISK_RET_SUCCESS);
549         return;
550     }
551
552     // EDD 3.x
553     channel = device / 2;
554     u8 iface = GET_EBDA(ata.channels[channel].iface);
555     iobase1 = GET_EBDA(ata.channels[channel].iobase1);
556
557     SET_INT13DPT(regs, size, 0x42);
558     SET_INT13DPT(regs, key, 0xbedd);
559     SET_INT13DPT(regs, dpi_length, 0x24);
560     SET_INT13DPT(regs, reserved1, 0);
561     SET_INT13DPT(regs, reserved2, 0);
562
563     if (iface==ATA_IFACE_ISA) {
564         SET_INT13DPT(regs, host_bus[0], 'I');
565         SET_INT13DPT(regs, host_bus[1], 'S');
566         SET_INT13DPT(regs, host_bus[2], 'A');
567         SET_INT13DPT(regs, host_bus[3], 0);
568     } else {
569         // FIXME PCI
570     }
571     SET_INT13DPT(regs, iface_type[0], 'A');
572     SET_INT13DPT(regs, iface_type[1], 'T');
573     SET_INT13DPT(regs, iface_type[2], 'A');
574     SET_INT13DPT(regs, iface_type[3], 0);
575
576     if (iface==ATA_IFACE_ISA) {
577         SET_INT13DPT(regs, iface_path[0], iobase1);
578         SET_INT13DPT(regs, iface_path[2], 0);
579         SET_INT13DPT(regs, iface_path[4], 0L);
580     } else {
581         // FIXME PCI
582     }
583     SET_INT13DPT(regs, device_path[0], device%2);
584     SET_INT13DPT(regs, device_path[1], 0);
585     SET_INT13DPT(regs, device_path[2], 0);
586     SET_INT13DPT(regs, device_path[4], 0L);
587
588     sum = checksum_seg(regs->ds, 30, 34);
589     SET_INT13DPT(regs, checksum, ~sum);
590 }
591
592 // IBM/MS extended media change
593 static void
594 disk_1349(struct bregs *regs, u8 device)
595 {
596     // Always success for HD
597     disk_ret(regs, DISK_RET_SUCCESS);
598 }
599
600 static void
601 disk_134e01(struct bregs *regs, u8 device)
602 {
603     disk_ret(regs, DISK_RET_SUCCESS);
604 }
605
606 static void
607 disk_134e03(struct bregs *regs, u8 device)
608 {
609     disk_ret(regs, DISK_RET_SUCCESS);
610 }
611
612 static void
613 disk_134e04(struct bregs *regs, u8 device)
614 {
615     disk_ret(regs, DISK_RET_SUCCESS);
616 }
617
618 static void
619 disk_134e06(struct bregs *regs, u8 device)
620 {
621     disk_ret(regs, DISK_RET_SUCCESS);
622 }
623
624 static void
625 disk_134eXX(struct bregs *regs, u8 device)
626 {
627     debug_stub(regs);
628     disk_ret(regs, DISK_RET_EPARAM);
629 }
630
631 // IBM/MS set hardware configuration
632 static void
633 disk_134e(struct bregs *regs, u8 device)
634 {
635     switch (regs->al) {
636     case 0x01: disk_134e01(regs, device); break;
637     case 0x03: disk_134e03(regs, device); break;
638     case 0x04: disk_134e04(regs, device); break;
639     case 0x06: disk_134e06(regs, device); break;
640     default:   disk_134eXX(regs, device); break;
641     }
642 }
643
644 static void
645 disk_13XX(struct bregs *regs, u8 device)
646 {
647     debug_stub(regs);
648     disk_ret(regs, DISK_RET_EPARAM);
649 }
650
651 static void
652 disk_13(struct bregs *regs, u8 device)
653 {
654     //debug_stub(regs);
655
656     // clear completion flag
657     SET_BDA(disk_interrupt_flag, 0);
658
659     switch (regs->ah) {
660     case 0x00: disk_1300(regs, device); break;
661     case 0x01: disk_1301(regs, device); break;
662     case 0x02: disk_1302(regs, device); break;
663     case 0x03: disk_1303(regs, device); break;
664     case 0x04: disk_1304(regs, device); break;
665     case 0x05: disk_1305(regs, device); break;
666     case 0x08: disk_1308(regs, device); break;
667     case 0x09: disk_1309(regs, device); break;
668     case 0x0c: disk_130c(regs, device); break;
669     case 0x0d: disk_130d(regs, device); break;
670     case 0x10: disk_1310(regs, device); break;
671     case 0x11: disk_1311(regs, device); break;
672     case 0x14: disk_1314(regs, device); break;
673     case 0x15: disk_1315(regs, device); break;
674     case 0x41: disk_1341(regs, device); break;
675     case 0x42: disk_1342(regs, device); break;
676     case 0x43: disk_1343(regs, device); break;
677     case 0x44: disk_1344(regs, device); break;
678     case 0x45: disk_1345(regs, device); break;
679     case 0x46: disk_1346(regs, device); break;
680     case 0x47: disk_1347(regs, device); break;
681     case 0x48: disk_1348(regs, device); break;
682     case 0x49: disk_1349(regs, device); break;
683     case 0x4e: disk_134e(regs, device); break;
684     default:   disk_13XX(regs, device); break;
685     }
686 }
687
688
689 /****************************************************************
690  * CDROM functions
691  ****************************************************************/
692
693 // read disk drive size
694 static void
695 cdrom_1315(struct bregs *regs, u8 device)
696 {
697     disk_ret(regs, DISK_RET_EADDRNOTFOUND);
698 }
699
700 // lock
701 static void
702 cdrom_134500(struct bregs *regs, u8 device)
703 {
704     u8 locks = GET_EBDA(ata.devices[device].lock);
705     if (locks == 0xff) {
706         regs->al = 1;
707         disk_ret(regs, DISK_RET_ETOOMANYLOCKS);
708         return;
709     }
710     SET_EBDA(ata.devices[device].lock, locks + 1);
711     regs->al = 1;
712     disk_ret(regs, DISK_RET_SUCCESS);
713 }
714
715 // unlock
716 static void
717 cdrom_134501(struct bregs *regs, u8 device)
718 {
719     u8 locks = GET_EBDA(ata.devices[device].lock);
720     if (locks == 0x00) {
721         regs->al = 0;
722         disk_ret(regs, DISK_RET_ENOTLOCKED);
723         return;
724     }
725     locks--;
726     SET_EBDA(ata.devices[device].lock, locks);
727     regs->al = (locks ? 1 : 0);
728     disk_ret(regs, DISK_RET_SUCCESS);
729 }
730
731 // status
732 static void
733 cdrom_134502(struct bregs *regs, u8 device)
734 {
735     u8 locks = GET_EBDA(ata.devices[device].lock);
736     regs->al = (locks ? 1 : 0);
737     disk_ret(regs, DISK_RET_SUCCESS);
738 }
739
740 static void
741 cdrom_1345XX(struct bregs *regs, u8 device)
742 {
743     disk_ret(regs, DISK_RET_EPARAM);
744 }
745
746 // IBM/MS lock/unlock drive
747 static void
748 cdrom_1345(struct bregs *regs, u8 device)
749 {
750     switch (regs->al) {
751     case 0x00: cdrom_134500(regs, device); break;
752     case 0x01: cdrom_134501(regs, device); break;
753     case 0x02: cdrom_134502(regs, device); break;
754     default:   cdrom_1345XX(regs, device); break;
755     }
756 }
757
758 // IBM/MS eject media
759 static void
760 cdrom_1346(struct bregs *regs, u8 device)
761 {
762     u8 locks = GET_EBDA(ata.devices[device].lock);
763     if (locks != 0) {
764         disk_ret(regs, DISK_RET_ELOCKED);
765         return;
766     }
767
768     // FIXME should handle 0x31 no media in device
769     // FIXME should handle 0xb5 valid request failed
770
771     // Call removable media eject
772     struct bregs br;
773     memset(&br, 0, sizeof(br));
774     br.ah = 0x52;
775     call16_int(0x15, &br);
776
777     if (br.ah || br.flags & F_CF) {
778         disk_ret(regs, DISK_RET_ELOCKED);
779         return;
780     }
781     disk_ret(regs, DISK_RET_SUCCESS);
782 }
783
784 // IBM/MS extended media change
785 static void
786 cdrom_1349(struct bregs *regs, u8 device)
787 {
788     // always send changed ??
789     regs->ah = DISK_RET_ECHANGED;
790     set_cf(regs, 1);
791 }
792
793 static void
794 cdrom_ok(struct bregs *regs, u8 device)
795 {
796     disk_ret(regs, DISK_RET_SUCCESS);
797 }
798
799 static void
800 cdrom_wp(struct bregs *regs, u8 device)
801 {
802     disk_ret(regs, DISK_RET_EWRITEPROTECT);
803 }
804
805 static void
806 cdrom_13(struct bregs *regs, u8 device)
807 {
808     //debug_stub(regs);
809
810     switch (regs->ah) {
811     case 0x15: cdrom_1315(regs, device); break;
812     case 0x45: cdrom_1345(regs, device); break;
813     case 0x46: cdrom_1346(regs, device); break;
814     case 0x49: cdrom_1349(regs, device); break;
815
816     // These functions are the same as for hard disks
817     case 0x01: disk_1301(regs, device); break;
818     case 0x41: disk_1341(regs, device); break;
819     case 0x42: disk_1342(regs, device); break;
820     case 0x44: disk_1344(regs, device); break;
821     case 0x47: disk_1347(regs, device); break;
822     case 0x48: disk_1348(regs, device); break;
823     case 0x4e: disk_134e(regs, device); break;
824
825     // all these functions return SUCCESS
826     case 0x00: cdrom_ok(regs, device); break; // disk controller reset
827     case 0x09: cdrom_ok(regs, device); break; // initialize drive parameters
828     case 0x0c: cdrom_ok(regs, device); break; // seek to specified cylinder
829     case 0x0d: cdrom_ok(regs, device); break; // alternate disk reset
830     case 0x10: cdrom_ok(regs, device); break; // check drive ready
831     case 0x11: cdrom_ok(regs, device); break; // recalibrate
832     case 0x14: cdrom_ok(regs, device); break; // controller internal diagnostic
833     case 0x16: cdrom_ok(regs, device); break; // detect disk change
834
835     // all these functions return disk write-protected
836     case 0x03: cdrom_wp(regs, device); break; // write disk sectors
837     case 0x05: cdrom_wp(regs, device); break; // format disk track
838     case 0x43: cdrom_wp(regs, device); break; // IBM/MS extended write
839
840     default:   disk_13XX(regs, device); break;
841     }
842 }
843
844
845 /****************************************************************
846  * CD emulation
847  ****************************************************************/
848
849 // read disk sectors
850 static void
851 cdemu_1302(struct bregs *regs, u8 device)
852 {
853     emu_access(regs, device, ATA_CMD_READ_SECTORS);
854 }
855
856 // verify disk sectors
857 static void
858 cdemu_1304(struct bregs *regs, u8 device)
859 {
860     emu_access(regs, device, 0);
861 }
862
863 // read disk drive parameters
864 static void
865 cdemu_1308(struct bregs *regs, u8 device)
866 {
867     u16 nlc   = GET_EBDA(cdemu.vdevice.cylinders) - 1;
868     u16 nlh   = GET_EBDA(cdemu.vdevice.heads) - 1;
869     u16 nlspt = GET_EBDA(cdemu.vdevice.spt);
870
871     regs->al = 0x00;
872     regs->bl = 0x00;
873     regs->ch = nlc & 0xff;
874     regs->cl = ((nlc >> 2) & 0xc0) | (nlspt  & 0x3f);
875     regs->dh = nlh;
876     // FIXME ElTorito Various. should send the real count of drives 1 or 2
877     // FIXME ElTorito Harddisk. should send the HD count
878     regs->dl = 0x02;
879     u8 media = GET_EBDA(cdemu.media);
880     if (media <= 3)
881         regs->bl = media * 2;
882
883     regs->es = SEG_BIOS;
884     regs->di = (u16)&diskette_param_table2;
885
886     disk_ret(regs, DISK_RET_SUCCESS);
887 }
888
889 static void
890 cdemu_13(struct bregs *regs)
891 {
892     //debug_stub(regs);
893
894     u8 device  = GET_EBDA(cdemu.controller_index) * 2;
895     device += GET_EBDA(cdemu.device_spec);
896
897     switch (regs->ah) {
898     case 0x02: cdemu_1302(regs, device); break;
899     case 0x04: cdemu_1304(regs, device); break;
900     case 0x08: cdemu_1308(regs, device); break;
901     // XXX - All other calls get passed to standard CDROM functions.
902     default: cdrom_13(regs, device); break;
903     }
904 }
905
906 struct eltorito_s {
907     u8 size;
908     u8 media;
909     u8 emulated_drive;
910     u8 controller_index;
911     u32 ilba;
912     u16 device_spec;
913     u16 buffer_segment;
914     u16 load_segment;
915     u16 sector_count;
916     u8 cylinders;
917     u8 sectors;
918     u8 heads;
919 };
920
921 #define SET_INT13ET(regs,var,val)                                      \
922     SET_FARVAR((regs)->ds, ((struct eltorito_s*)((regs)->si+0))->var, (val))
923
924 // ElTorito - Terminate disk emu
925 static void
926 cdemu_134b(struct bregs *regs)
927 {
928     // FIXME ElTorito Hardcoded
929     SET_INT13ET(regs, size, 0x13);
930     SET_INT13ET(regs, media, GET_EBDA(cdemu.media));
931     SET_INT13ET(regs, emulated_drive, GET_EBDA(cdemu.emulated_drive));
932     SET_INT13ET(regs, controller_index, GET_EBDA(cdemu.controller_index));
933     SET_INT13ET(regs, ilba, GET_EBDA(cdemu.ilba));
934     SET_INT13ET(regs, device_spec, GET_EBDA(cdemu.device_spec));
935     SET_INT13ET(regs, buffer_segment, GET_EBDA(cdemu.buffer_segment));
936     SET_INT13ET(regs, load_segment, GET_EBDA(cdemu.load_segment));
937     SET_INT13ET(regs, sector_count, GET_EBDA(cdemu.sector_count));
938     SET_INT13ET(regs, cylinders, GET_EBDA(cdemu.vdevice.cylinders));
939     SET_INT13ET(regs, sectors, GET_EBDA(cdemu.vdevice.spt));
940     SET_INT13ET(regs, heads, GET_EBDA(cdemu.vdevice.heads));
941
942     // If we have to terminate emulation
943     if (regs->al == 0x00) {
944         // FIXME ElTorito Various. Should be handled accordingly to spec
945         SET_EBDA(cdemu.active, 0x00); // bye bye
946     }
947
948     disk_ret(regs, DISK_RET_SUCCESS);
949 }
950
951
952 /****************************************************************
953  * Entry points
954  ****************************************************************/
955
956 static u8
957 get_device(struct bregs *regs, u8 drive)
958 {
959     // basic check : device has to be defined
960     if (drive >= CONFIG_MAX_ATA_DEVICES) {
961         disk_ret(regs, DISK_RET_EPARAM);
962         return CONFIG_MAX_ATA_DEVICES;
963     }
964
965     // Get the ata channel
966     u8 device = GET_EBDA(ata.hdidmap[drive]);
967
968     // basic check : device has to be valid
969     if (device >= CONFIG_MAX_ATA_DEVICES) {
970         disk_ret(regs, DISK_RET_EPARAM);
971         return CONFIG_MAX_ATA_DEVICES;
972     }
973
974     return device;
975 }
976
977 static void
978 handle_legacy_disk(struct bregs *regs, u8 drive)
979 {
980     if (drive < 0x80) {
981         floppy_13(regs, drive);
982         return;
983     }
984
985     if (! CONFIG_ATA) {
986         // XXX - old code had other disk access method.
987         disk_ret(regs, DISK_RET_EPARAM);
988         return;
989     }
990
991     if (drive >= 0xe0) {
992         u8 device = get_device(regs, drive - 0xe0);
993         if (device >= CONFIG_MAX_ATA_DEVICES)
994             return;
995         cdrom_13(regs, device);
996         return;
997     }
998
999     u8 device = get_device(regs, drive - 0x80);
1000     if (device >= CONFIG_MAX_ATA_DEVICES)
1001         return;
1002     disk_13(regs, device);
1003 }
1004
1005 void VISIBLE
1006 handle_40(struct bregs *regs)
1007 {
1008     debug_enter(regs);
1009     handle_legacy_disk(regs, regs->dl);
1010     debug_exit(regs);
1011 }
1012
1013 // INT 13h Fixed Disk Services Entry Point
1014 void VISIBLE
1015 handle_13(struct bregs *regs)
1016 {
1017     debug_enter(regs);
1018     u8 drive = regs->dl;
1019
1020     if (CONFIG_ELTORITO_BOOT) {
1021         if (regs->ah == 0x4b) {
1022             cdemu_134b(regs);
1023             goto done;
1024         }
1025         if (GET_EBDA(cdemu.active)) {
1026             if (drive == GET_EBDA(cdemu.emulated_drive)) {
1027                 cdemu_13(regs);
1028                 goto done;
1029             }
1030             drive--;
1031         }
1032     }
1033     handle_legacy_disk(regs, drive);
1034 done:
1035     debug_exit(regs);
1036 }
1037
1038 // record completion in BIOS task complete flag
1039 void VISIBLE
1040 handle_76(struct bregs *regs)
1041 {
1042     debug_isr(regs);
1043     SET_BDA(floppy_harddisk_info, 0xff);
1044     eoi_both_pics();
1045 }