qemu/roms/seabios/src/hw/floppy.c

   1 // 16bit code to access floppy drives.
   2 //
   3 // Copyright (C) 2008,2009  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 LGPLv3 license.
   7
   8 #include "biosvar.h" // SET_BDA
   9 #include "block.h" // struct drive_s
  10 #include "bregs.h" // struct bregs
  11 #include "config.h" // CONFIG_FLOPPY
  12 #include "malloc.h" // malloc_fseg
  13 #include "output.h" // dprintf
  14 #include "pci.h" // pci_to_bdf
  15 #include "pci_ids.h" // PCI_CLASS_BRIDGE_ISA
  16 #include "pic.h" // pic_eoi1
  17 #include "romfile.h" // romfile_loadint
  18 #include "rtc.h" // rtc_read
  19 #include "stacks.h" // yield
  20 #include "std/disk.h" // DISK_RET_SUCCESS
  21 #include "string.h" // memset
  22 #include "util.h" // timer_calc
  23
  24 #define PORT_FD_BASE           0x03f0
  25 #define PORT_FD_DOR            0x03f2
  26 #define PORT_FD_STATUS         0x03f4
  27 #define PORT_FD_DATA           0x03f5
  28 #define PORT_FD_DIR            0x03f7
  29
  30 #define FLOPPY_SIZE_CODE 0x02 // 512 byte sectors
  31 #define FLOPPY_DATALEN 0xff   // Not used - because size code is 0x02
  32 #define FLOPPY_MOTOR_TICKS 37 // ~2 seconds
  33 #define FLOPPY_FILLBYTE 0xf6
  34 #define FLOPPY_GAPLEN 0x1B
  35 #define FLOPPY_FORMAT_GAPLEN 0x6c
  36 #define FLOPPY_PIO_TIMEOUT 1000
  37
  38 // New diskette parameter table adding 3 parameters from IBM
  39 // Since no provisions are made for multiple drive types, most
  40 // values in this table are ignored.  I set parameters for 1.44M
  41 // floppy here
  42 struct floppy_ext_dbt_s diskette_param_table2 VARFSEG = {
  43     .dbt = {
  44         .specify1       = 0xAF, // step rate 12ms, head unload 240ms
  45         .specify2       = 0x02, // head load time 4ms, DMA used
  46         .shutoff_ticks  = FLOPPY_MOTOR_TICKS, // ~2 seconds
  47         .bps_code       = FLOPPY_SIZE_CODE,
  48         .sectors        = 18,
  49         .interblock_len = FLOPPY_GAPLEN,
  50         .data_len       = FLOPPY_DATALEN,
  51         .gap_len        = FLOPPY_FORMAT_GAPLEN,
  52         .fill_byte      = FLOPPY_FILLBYTE,
  53         .settle_time    = 0x0F, // 15ms
  54         .startup_time   = 0x08, // 1 second
  55     },
  56     .max_track      = 79,   // maximum track
  57     .data_rate      = 0,    // data transfer rate
  58     .drive_type     = 4,    // drive type in cmos
  59 };
  60
  61 struct floppyinfo_s {
  62     struct chs_s chs;
  63     u8 floppy_size;
  64     u8 data_rate;
  65 };
  66
  67 #define FLOPPY_SIZE_525 0x01
  68 #define FLOPPY_SIZE_350 0x02
  69
  70 #define FLOPPY_RATE_500K 0x00
  71 #define FLOPPY_RATE_300K 0x01
  72 #define FLOPPY_RATE_250K 0x02
  73 #define FLOPPY_RATE_1M   0x03
  74
  75 struct floppyinfo_s FloppyInfo[] VARFSEG = {
  76     // Unknown
  77     { {0, 0, 0}, 0x00, 0x00},
  78     // 1 - 360KB, 5.25" - 2 heads, 40 tracks, 9 sectors
  79     { {2, 40, 9}, FLOPPY_SIZE_525, FLOPPY_RATE_300K},
  80     // 2 - 1.2MB, 5.25" - 2 heads, 80 tracks, 15 sectors
  81     { {2, 80, 15}, FLOPPY_SIZE_525, FLOPPY_RATE_500K},
  82     // 3 - 720KB, 3.5"  - 2 heads, 80 tracks, 9 sectors
  83     { {2, 80, 9}, FLOPPY_SIZE_350, FLOPPY_RATE_250K},
  84     // 4 - 1.44MB, 3.5" - 2 heads, 80 tracks, 18 sectors
  85     { {2, 80, 18}, FLOPPY_SIZE_350, FLOPPY_RATE_500K},
  86     // 5 - 2.88MB, 3.5" - 2 heads, 80 tracks, 36 sectors
  87     { {2, 80, 36}, FLOPPY_SIZE_350, FLOPPY_RATE_1M},
  88     // 6 - 160k, 5.25"  - 1 heads, 40 tracks, 8 sectors
  89     { {1, 40, 8}, FLOPPY_SIZE_525, FLOPPY_RATE_250K},
  90     // 7 - 180k, 5.25"  - 1 heads, 40 tracks, 9 sectors
  91     { {1, 40, 9}, FLOPPY_SIZE_525, FLOPPY_RATE_300K},
  92     // 8 - 320k, 5.25"  - 2 heads, 40 tracks, 8 sectors
  93     { {2, 40, 8}, FLOPPY_SIZE_525, FLOPPY_RATE_250K},
  94 };
  95
  96 struct drive_s *
  97 init_floppy(int floppyid, int ftype)
  98 {
  99     if (ftype <= 0 || ftype >= ARRAY_SIZE(FloppyInfo)) {
 100         dprintf(1, "Bad floppy type %d\n", ftype);
 101         return NULL;
 102     }
 103
 104     struct drive_s *drive = malloc_fseg(sizeof(*drive));
 105     if (!drive) {
 106         warn_noalloc();
 107         return NULL;
 108     }
 109     memset(drive, 0, sizeof(*drive));
 110     drive->cntl_id = floppyid;
 111     drive->type = DTYPE_FLOPPY;
 112     drive->blksize = DISK_SECTOR_SIZE;
 113     drive->floppy_type = ftype;
 114     drive->sectors = (u64)-1;
 115
 116     memcpy(&drive->lchs, &FloppyInfo[ftype].chs
 117            , sizeof(FloppyInfo[ftype].chs));
 118     return drive;
 119 }
 120
 121 static void
 122 addFloppy(int floppyid, int ftype)
 123 {
 124     struct drive_s *drive = init_floppy(floppyid, ftype);
 125     if (!drive)
 126         return;
 127     char *desc = znprintf(MAXDESCSIZE, "Floppy [drive %c]", 'A' + floppyid);
 128     struct pci_device *pci = pci_find_class(PCI_CLASS_BRIDGE_ISA); /* isa-to-pci bridge */
 129     int prio = bootprio_find_fdc_device(pci, PORT_FD_BASE, floppyid);
 130     boot_add_floppy(drive, desc, prio);
 131 }
 132
 133 void
 134 floppy_setup(void)
 135 {
 136     memcpy(&diskette_param_table, &diskette_param_table2
 137            , sizeof(diskette_param_table));
 138     SET_IVT(0x1E, SEGOFF(SEG_BIOS
 139                          , (u32)&diskette_param_table2 - BUILD_BIOS_ADDR));
 140
 141     if (! CONFIG_FLOPPY)
 142         return;
 143     dprintf(3, "init floppy drives\n");
 144
 145     if (CONFIG_QEMU) {
 146         u8 type = rtc_read(CMOS_FLOPPY_DRIVE_TYPE);
 147         if (type & 0xf0)
 148             addFloppy(0, type >> 4);
 149         if (type & 0x0f)
 150             addFloppy(1, type & 0x0f);
 151     } else {
 152         u8 type = romfile_loadint("etc/floppy0", 0);
 153         if (type)
 154             addFloppy(0, type);
 155         type = romfile_loadint("etc/floppy1", 0);
 156         if (type)
 157             addFloppy(1, type);
 158     }
 159
 160     enable_hwirq(6, FUNC16(entry_0e));
 161 }
 162
 163 // Find a floppy type that matches a given image size.
 164 int
 165 find_floppy_type(u32 size)
 166 {
 167     int i;
 168     for (i=1; i<ARRAY_SIZE(FloppyInfo); i++) {
 169         struct chs_s *c = &FloppyInfo[i].chs;
 170         if (c->cylinder * c->head * c->sector * DISK_SECTOR_SIZE == size)
 171             return i;
 172     }
 173     return -1;
 174 }
 175
 176
 177 /****************************************************************
 178  * Low-level floppy IO
 179  ****************************************************************/
 180
 181 u8 FloppyDOR VARLOW;
 182
 183 static inline void
 184 floppy_dor_write(u8 val)
 185 {
 186     outb(val, PORT_FD_DOR);
 187     SET_LOW(FloppyDOR, val);
 188 }
 189
 190 static void
 191 floppy_disable_controller(void)
 192 {
 193     dprintf(2, "Floppy_disable_controller\n");
 194     floppy_dor_write(0x00);
 195 }
 196
 197 static int
 198 floppy_wait_irq(void)
 199 {
 200     u8 frs = GET_BDA(floppy_recalibration_status);
 201     SET_BDA(floppy_recalibration_status, frs & ~FRS_IRQ);
 202     for (;;) {
 203         if (!GET_BDA(floppy_motor_counter)) {
 204             warn_timeout();
 205             floppy_disable_controller();
 206             return DISK_RET_ETIMEOUT;
 207         }
 208         frs = GET_BDA(floppy_recalibration_status);
 209         if (frs & FRS_IRQ)
 210             break;
 211         // Could use yield_toirq() here, but that causes issues on
 212         // bochs, so use yield() instead.
 213         yield();
 214     }
 215
 216     SET_BDA(floppy_recalibration_status, frs & ~FRS_IRQ);
 217     return DISK_RET_SUCCESS;
 218 }
 219
 220 // Floppy commands
 221 #define FCF_WAITIRQ 0x10000
 222 #define FC_CHECKIRQ    (0x08 | (0<<8) | (2<<12))
 223 #define FC_SEEK        (0x0f | (2<<8) | (0<<12) | FCF_WAITIRQ)
 224 #define FC_RECALIBRATE (0x07 | (1<<8) | (0<<12) | FCF_WAITIRQ)
 225 #define FC_READID      (0x4a | (1<<8) | (7<<12) | FCF_WAITIRQ)
 226 #define FC_READ        (0xe6 | (8<<8) | (7<<12) | FCF_WAITIRQ)
 227 #define FC_WRITE       (0xc5 | (8<<8) | (7<<12) | FCF_WAITIRQ)
 228 #define FC_FORMAT      (0x4d | (5<<8) | (7<<12) | FCF_WAITIRQ)
 229
 230 // Send the specified command and it's parameters to the floppy controller.
 231 static int
 232 floppy_pio(int command, u8 *param)
 233 {
 234     dprintf(9, "Floppy pio command %x\n", command);
 235     // Send command and parameters to controller.
 236     u32 end = timer_calc(FLOPPY_PIO_TIMEOUT);
 237     int send = (command >> 8) & 0xf;
 238     int i = 0;
 239     for (;;) {
 240         u8 sts = inb(PORT_FD_STATUS);
 241         if (!(sts & 0x80)) {
 242             if (timer_check(end)) {
 243                 warn_timeout();
 244                 floppy_disable_controller();
 245                 return DISK_RET_ETIMEOUT;
 246             }
 247             yield();
 248             continue;
 249         }
 250         if (sts & 0x40) {
 251             floppy_disable_controller();
 252             return DISK_RET_ECONTROLLER;
 253         }
 254         if (i == 0)
 255             outb(command & 0xff, PORT_FD_DATA);
 256         else
 257             outb(param[i-1], PORT_FD_DATA);
 258         if (i++ >= send)
 259             break;
 260     }
 261
 262     // Wait for command to complete.
 263     if (command & FCF_WAITIRQ) {
 264         int ret = floppy_wait_irq();
 265         if (ret)
 266             return ret;
 267     }
 268
 269     // Read response from controller.
 270     end = timer_calc(FLOPPY_PIO_TIMEOUT);
 271     int receive = (command >> 12) & 0xf;
 272     i = 0;
 273     for (;;) {
 274         u8 sts = inb(PORT_FD_STATUS);
 275         if (!(sts & 0x80)) {
 276             if (timer_check(end)) {
 277                 warn_timeout();
 278                 floppy_disable_controller();
 279                 return DISK_RET_ETIMEOUT;
 280             }
 281             yield();
 282             continue;
 283         }
 284         if (i >= receive) {
 285             if (sts & 0x40) {
 286                 floppy_disable_controller();
 287                 return DISK_RET_ECONTROLLER;
 288             }
 289             break;
 290         }
 291         if (!(sts & 0x40)) {
 292             floppy_disable_controller();
 293             return DISK_RET_ECONTROLLER;
 294         }
 295         param[i++] = inb(PORT_FD_DATA);
 296     }
 297
 298     return DISK_RET_SUCCESS;
 299 }
 300
 301 static int
 302 floppy_enable_controller(void)
 303 {
 304     dprintf(2, "Floppy_enable_controller\n");
 305     SET_BDA(floppy_motor_counter, FLOPPY_MOTOR_TICKS);
 306     floppy_dor_write(0x00);
 307     floppy_dor_write(0x0c);
 308     int ret = floppy_wait_irq();
 309     if (ret)
 310         return ret;
 311
 312     u8 param[2];
 313     return floppy_pio(FC_CHECKIRQ, param);
 314 }
 315
 316 // Activate a drive and send a command to it.
 317 static int
 318 floppy_drive_pio(u8 floppyid, int command, u8 *param)
 319 {
 320     // Enable controller if it isn't running.
 321     if (!(GET_LOW(FloppyDOR) & 0x04)) {
 322         int ret = floppy_enable_controller();
 323         if (ret)
 324             return ret;
 325     }
 326
 327     // reset the disk motor timeout value of INT 08
 328     SET_BDA(floppy_motor_counter, FLOPPY_MOTOR_TICKS);
 329
 330     // Turn on motor of selected drive, DMA & int enabled, normal operation
 331     floppy_dor_write((floppyid ? 0x20 : 0x10) | 0x0c | floppyid);
 332
 333     // Send command.
 334     int ret = floppy_pio(command, param);
 335     if (ret)
 336         return ret;
 337
 338     // Check IRQ command is needed after irq commands with no results
 339     if ((command & FCF_WAITIRQ) && ((command >> 12) & 0xf) == 0)
 340         return floppy_pio(FC_CHECKIRQ, param);
 341     return DISK_RET_SUCCESS;
 342 }
 343
 344
 345 /****************************************************************
 346  * Floppy media sense and seeking
 347  ****************************************************************/
 348
 349 static int
 350 floppy_drive_recal(u8 floppyid)
 351 {
 352     dprintf(2, "Floppy_drive_recal %d\n", floppyid);
 353     // send Recalibrate command to controller
 354     u8 param[2];
 355     param[0] = floppyid;
 356     int ret = floppy_drive_pio(floppyid, FC_RECALIBRATE, param);
 357     if (ret)
 358         return ret;
 359
 360     u8 frs = GET_BDA(floppy_recalibration_status);
 361     SET_BDA(floppy_recalibration_status, frs | (1<<floppyid));
 362     SET_BDA(floppy_track[floppyid], 0);
 363     return DISK_RET_SUCCESS;
 364 }
 365
 366 static int
 367 floppy_drive_readid(u8 floppyid, u8 data_rate, u8 head)
 368 {
 369     // Set data rate.
 370     outb(data_rate, PORT_FD_DIR);
 371
 372     // send Read Sector Id command
 373     u8 param[7];
 374     param[0] = (head << 2) | floppyid; // HD DR1 DR2
 375     int ret = floppy_drive_pio(floppyid, FC_READID, param);
 376     if (ret)
 377         return ret;
 378     if (param[0] & 0xc0)
 379         return -1;
 380     return 0;
 381 }
 382
 383 static int
 384 floppy_media_sense(struct drive_s *drive_gf)
 385 {
 386     u8 ftype = GET_GLOBALFLAT(drive_gf->floppy_type), stype = ftype;
 387     u8 floppyid = GET_GLOBALFLAT(drive_gf->cntl_id);
 388
 389     u8 data_rate = GET_GLOBAL(FloppyInfo[stype].data_rate);
 390     int ret = floppy_drive_readid(floppyid, data_rate, 0);
 391     if (ret) {
 392         // Attempt media sense.
 393         for (stype=1; ; stype++) {
 394             if (stype >= ARRAY_SIZE(FloppyInfo))
 395                 return DISK_RET_EMEDIA;
 396             if (stype==ftype
 397                 || (GET_GLOBAL(FloppyInfo[stype].floppy_size)
 398                     != GET_GLOBAL(FloppyInfo[ftype].floppy_size))
 399                 || (GET_GLOBAL(FloppyInfo[stype].chs.head)
 400                     > GET_GLOBAL(FloppyInfo[ftype].chs.head))
 401                 || (GET_GLOBAL(FloppyInfo[stype].chs.cylinder)
 402                     > GET_GLOBAL(FloppyInfo[ftype].chs.cylinder))
 403                 || (GET_GLOBAL(FloppyInfo[stype].chs.sector)
 404                     > GET_GLOBAL(FloppyInfo[ftype].chs.sector)))
 405                 continue;
 406             data_rate = GET_GLOBAL(FloppyInfo[stype].data_rate);
 407             ret = floppy_drive_readid(floppyid, data_rate, 0);
 408             if (!ret)
 409                 break;
 410         }
 411     }
 412     dprintf(2, "Floppy_media_sense on drive %d found rate %d\n"
 413             , floppyid, data_rate);
 414
 415     u8 old_data_rate = GET_BDA(floppy_media_state[floppyid]) >> 6;
 416     SET_BDA(floppy_last_data_rate, (old_data_rate<<2) | (data_rate<<6));
 417     u8 media = (stype == 1 ? 0x04 : (stype == 2 ? 0x05 : 0x07));
 418     u8 fms = (data_rate<<6) | FMS_MEDIA_DRIVE_ESTABLISHED | media;
 419     if (GET_GLOBAL(FloppyInfo[stype].chs.cylinder)
 420         < GET_GLOBAL(FloppyInfo[ftype].chs.cylinder))
 421         fms |= FMS_DOUBLE_STEPPING;
 422     SET_BDA(floppy_media_state[floppyid], fms);
 423
 424     return DISK_RET_SUCCESS;
 425 }
 426
 427 // Prepare a floppy for a data transfer.
 428 static int
 429 floppy_prep(struct drive_s *drive_gf, u8 cylinder)
 430 {
 431     u8 floppyid = GET_GLOBALFLAT(drive_gf->cntl_id);
 432     if (!(GET_BDA(floppy_recalibration_status) & (1<<floppyid)) ||
 433         !(GET_BDA(floppy_media_state[floppyid]) & FMS_MEDIA_DRIVE_ESTABLISHED)) {
 434         // Recalibrate drive.
 435         int ret = floppy_drive_recal(floppyid);
 436         if (ret)
 437             return ret;
 438
 439         // Sense media.
 440         ret = floppy_media_sense(drive_gf);
 441         if (ret)
 442             return ret;
 443     }
 444
 445     // Seek to cylinder if needed.
 446     u8 lastcyl = GET_BDA(floppy_track[floppyid]);
 447     if (cylinder != lastcyl) {
 448         u8 param[2];
 449         param[0] = floppyid;
 450         param[1] = cylinder;
 451         int ret = floppy_drive_pio(floppyid, FC_SEEK, param);
 452         if (ret)
 453             return ret;
 454         SET_BDA(floppy_track[floppyid], cylinder);
 455     }
 456
 457     return DISK_RET_SUCCESS;
 458 }
 459
 460
 461 /****************************************************************
 462  * Floppy DMA transfer
 463  ****************************************************************/
 464
 465 // Perform a floppy transfer command (setup DMA and issue PIO).
 466 static int
 467 floppy_dma_cmd(struct disk_op_s *op, int count, int command, u8 *param)
 468 {
 469     // Setup DMA controller
 470     int isWrite = command != FC_READ;
 471     int ret = dma_floppy((u32)op->buf_fl, count, isWrite);
 472     if (ret)
 473         return DISK_RET_EBOUNDARY;
 474
 475     // Invoke floppy controller
 476     u8 floppyid = GET_GLOBALFLAT(op->drive_gf->cntl_id);
 477     ret = floppy_drive_pio(floppyid, command, param);
 478     if (ret)
 479         return ret;
 480
 481     // Populate floppy_return_status in BDA
 482     int i;
 483     for (i=0; i<7; i++)
 484         SET_BDA(floppy_return_status[i], param[i]);
 485
 486     if (param[0] & 0xc0) {
 487         if (param[1] & 0x02)
 488             return DISK_RET_EWRITEPROTECT;
 489         dprintf(1, "floppy error: %02x %02x %02x %02x %02x %02x %02x\n"
 490                 , param[0], param[1], param[2], param[3]
 491                 , param[4], param[5], param[6]);
 492         return DISK_RET_ECONTROLLER;
 493     }
 494
 495     return DISK_RET_SUCCESS;
 496 }
 497
 498
 499 /****************************************************************
 500  * Floppy handlers
 501  ****************************************************************/
 502
 503 static struct chs_s
 504 lba2chs(struct disk_op_s *op)
 505 {
 506     struct chs_s res = { };
 507
 508     u32 tmp = op->lba;
 509     u16 nls = GET_GLOBALFLAT(op->drive_gf->lchs.sector);
 510     res.sector = (tmp % nls) + 1;
 511
 512     tmp /= nls;
 513     u16 nlh = GET_GLOBALFLAT(op->drive_gf->lchs.head);
 514     res.head = tmp % nlh;
 515
 516     tmp /= nlh;
 517     res.cylinder = tmp;
 518
 519     return res;
 520 }
 521
 522 // diskette controller reset
 523 static int
 524 floppy_reset(struct disk_op_s *op)
 525 {
 526     SET_BDA(floppy_recalibration_status, 0);
 527     SET_BDA(floppy_media_state[0], 0);
 528     SET_BDA(floppy_media_state[1], 0);
 529     SET_BDA(floppy_track[0], 0);
 530     SET_BDA(floppy_track[1], 0);
 531     SET_BDA(floppy_last_data_rate, 0);
 532     floppy_disable_controller();
 533     return floppy_enable_controller();
 534 }
 535
 536 // Read Diskette Sectors
 537 static int
 538 floppy_read(struct disk_op_s *op)
 539 {
 540     struct chs_s chs = lba2chs(op);
 541     int ret = floppy_prep(op->drive_gf, chs.cylinder);
 542     if (ret)
 543         return ret;
 544
 545     // send read-normal-data command to controller
 546     u8 floppyid = GET_GLOBALFLAT(op->drive_gf->cntl_id);
 547     u8 param[8];
 548     param[0] = (chs.head << 2) | floppyid; // HD DR1 DR2
 549     param[1] = chs.cylinder;
 550     param[2] = chs.head;
 551     param[3] = chs.sector;
 552     param[4] = FLOPPY_SIZE_CODE;
 553     param[5] = chs.sector + op->count - 1; // last sector to read on track
 554     param[6] = FLOPPY_GAPLEN;
 555     param[7] = FLOPPY_DATALEN;
 556     return floppy_dma_cmd(op, op->count * DISK_SECTOR_SIZE, FC_READ, param);
 557 }
 558
 559 // Write Diskette Sectors
 560 static int
 561 floppy_write(struct disk_op_s *op)
 562 {
 563     struct chs_s chs = lba2chs(op);
 564     int ret = floppy_prep(op->drive_gf, chs.cylinder);
 565     if (ret)
 566         return ret;
 567
 568     // send write-normal-data command to controller
 569     u8 floppyid = GET_GLOBALFLAT(op->drive_gf->cntl_id);
 570     u8 param[8];
 571     param[0] = (chs.head << 2) | floppyid; // HD DR1 DR2
 572     param[1] = chs.cylinder;
 573     param[2] = chs.head;
 574     param[3] = chs.sector;
 575     param[4] = FLOPPY_SIZE_CODE;
 576     param[5] = chs.sector + op->count - 1; // last sector to write on track
 577     param[6] = FLOPPY_GAPLEN;
 578     param[7] = FLOPPY_DATALEN;
 579     return floppy_dma_cmd(op, op->count * DISK_SECTOR_SIZE, FC_WRITE, param);
 580 }
 581
 582 // Verify Diskette Sectors
 583 static int
 584 floppy_verify(struct disk_op_s *op)
 585 {
 586     struct chs_s chs = lba2chs(op);
 587     int ret = floppy_prep(op->drive_gf, chs.cylinder);
 588     if (ret)
 589         return ret;
 590
 591     // This command isn't implemented - just return success.
 592     return DISK_RET_SUCCESS;
 593 }
 594
 595 // format diskette track
 596 static int
 597 floppy_format(struct disk_op_s *op)
 598 {
 599     struct chs_s chs = lba2chs(op);
 600     int ret = floppy_prep(op->drive_gf, chs.cylinder);
 601     if (ret)
 602         return ret;
 603
 604     // send format-track command to controller
 605     u8 floppyid = GET_GLOBALFLAT(op->drive_gf->cntl_id);
 606     u8 param[7];
 607     param[0] = (chs.head << 2) | floppyid; // HD DR1 DR2
 608     param[1] = FLOPPY_SIZE_CODE;
 609     param[2] = op->count; // number of sectors per track
 610     param[3] = FLOPPY_FORMAT_GAPLEN;
 611     param[4] = FLOPPY_FILLBYTE;
 612     return floppy_dma_cmd(op, op->count * 4, FC_FORMAT, param);
 613 }
 614
 615 int
 616 process_floppy_op(struct disk_op_s *op)
 617 {
 618     if (!CONFIG_FLOPPY)
 619         return 0;
 620
 621     switch (op->command) {
 622     case CMD_RESET:
 623         return floppy_reset(op);
 624     case CMD_READ:
 625         return floppy_read(op);
 626     case CMD_WRITE:
 627         return floppy_write(op);
 628     case CMD_VERIFY:
 629         return floppy_verify(op);
 630     case CMD_FORMAT:
 631         return floppy_format(op);
 632     default:
 633         return DISK_RET_EPARAM;
 634     }
 635 }
 636
 637
 638 /****************************************************************
 639  * HW irqs
 640  ****************************************************************/
 641
 642 // INT 0Eh Diskette Hardware ISR Entry Point
 643 void VISIBLE16
 644 handle_0e(void)
 645 {
 646     if (! CONFIG_FLOPPY)
 647         return;
 648     debug_isr(DEBUG_ISR_0e);
 649
 650     // diskette interrupt has occurred
 651     u8 frs = GET_BDA(floppy_recalibration_status);
 652     SET_BDA(floppy_recalibration_status, frs | FRS_IRQ);
 653
 654     pic_eoi1();
 655 }
 656
 657 // Called from int08 handler.
 658 void
 659 floppy_tick(void)
 660 {
 661     if (! CONFIG_FLOPPY)
 662         return;
 663
 664     // time to turn off drive(s)?
 665     u8 fcount = GET_BDA(floppy_motor_counter);
 666     if (fcount) {
 667         fcount--;
 668         SET_BDA(floppy_motor_counter, fcount);
 669         if (fcount == 0)
 670             // turn motor(s) off
 671             floppy_dor_write(GET_LOW(FloppyDOR) & ~0xf0);
 672     }
 673 }