grml...

[seabios.git] / src / floppy.c

diff --git a/src/floppy.c b/src/floppy.c
index 311fc4b304edb24f54734931a5cc14b146267d5b..383744a22d9d8fa5debc025053d4d2af2463de5c 100644 (file)
--- a/src/floppy.c
+++ b/src/floppy.c
@@ -9,10 +9,13 @@
 #include "disk.h" // DISK_RET_SUCCESS
 #include "config.h" // CONFIG_FLOPPY
 #include "biosvar.h" // SET_BDA
-#include "util.h" // irq_disable
+#include "util.h" // wait_irq
 #include "cmos.h" // inb_cmos
 #include "pic.h" // eoi_pic1
 #include "bregs.h" // struct bregs
+#include "boot.h" // boot_add_floppy
+#include "pci.h" // pci_to_bdf
+#include "pci_ids.h" // PCI_CLASS_BRIDGE_ISA
 
 #define FLOPPY_SIZE_CODE 0x02 // 512 byte sectors
 #define FLOPPY_DATALEN 0xff   // Not used - because size code is 0x02
@@ -89,37 +92,44 @@ struct floppyinfo_s FloppyInfo[] VAR16VISIBLE = {
 };
 
 struct drive_s *
-addFloppy(int floppyid, int ftype, int driver)
+init_floppy(int floppyid, int ftype)
 {
     if (ftype <= 0 || ftype >= ARRAY_SIZE(FloppyInfo)) {
         dprintf(1, "Bad floppy type %d\n", ftype);
         return NULL;
     }
 
-    struct drive_s *drive_g = allocDrive();
-    if (!drive_g)
+    struct drive_s *drive_g = malloc_fseg(sizeof(*drive_g));
+    if (!drive_g) {
+        warn_noalloc();
         return NULL;
+    }
+    memset(drive_g, 0, sizeof(*drive_g));
     drive_g->cntl_id = floppyid;
-    drive_g->type = driver;
+    drive_g->type = DTYPE_FLOPPY;
     drive_g->blksize = DISK_SECTOR_SIZE;
     drive_g->floppy_type = ftype;
     drive_g->sectors = (u64)-1;
 
     memcpy(&drive_g->lchs, &FloppyInfo[ftype].chs
            , sizeof(FloppyInfo[ftype].chs));
-
-    map_floppy_drive(drive_g);
     return drive_g;
 }
 
-void
-describe_floppy(struct drive_s *drive_g)
+static void
+addFloppy(int floppyid, int ftype)
 {
-    printf("drive %c", 'A' + drive_g->cntl_id);
+    struct drive_s *drive_g = init_floppy(floppyid, ftype);
+    if (!drive_g)
+        return;
+    char *desc = znprintf(MAXDESCSIZE, "Floppy [drive %c]", 'A' + floppyid);
+    struct pci_device *pci = pci_find_class(PCI_CLASS_BRIDGE_ISA); /* isa-to-pci bridge */
+    int prio = bootprio_find_fdc_device(pci, PORT_FD_BASE, floppyid);
+    boot_add_floppy(drive_g, desc, prio);
 }
 
 void
-floppy_setup()
+floppy_setup(void)
 {
     if (! CONFIG_FLOPPY)
         return;
@@ -130,14 +140,14 @@ floppy_setup()
     } else {
         u8 type = inb_cmos(CMOS_FLOPPY_DRIVE_TYPE);
         if (type & 0xf0)
-            addFloppy(0, type >> 4, DTYPE_FLOPPY);
+            addFloppy(0, type >> 4);
         if (type & 0x0f)
-            addFloppy(1, type & 0x0f, DTYPE_FLOPPY);
+            addFloppy(1, type & 0x0f);
     }
 
     outb(0x02, PORT_DMA1_MASK_REG);
 
-    enable_hwirq(6, entry_0e);
+    enable_hwirq(6, FUNC16(entry_0e));
 }
 
 // Find a floppy type that matches a given image size.
@@ -159,7 +169,7 @@ find_floppy_type(u32 size)
  ****************************************************************/
 
 static void
-floppy_reset_controller()
+floppy_reset_controller(void)
 {
     // Reset controller
     u8 val8 = inb(PORT_FD_DOR);
@@ -172,18 +182,19 @@ floppy_reset_controller()
 }
 
 static int
-wait_floppy_irq()
+wait_floppy_irq(void)
 {
+    ASSERT16();
     u8 v;
     for (;;) {
-        if (!GET_BDA(floppy_motor_counter)) {
-            irq_disable();
+        if (!GET_BDA(floppy_motor_counter))
             return -1;
-        }
         v = GET_BDA(floppy_recalibration_status);
         if (v & FRS_TIMEOUT)
             break;
-        wait_irq();
+        // Could use wait_irq() here, but that causes issues on
+        // bochs, so use yield() instead.
+        yield();
     }
 
     v &= ~FRS_TIMEOUT;
@@ -571,7 +582,7 @@ process_floppy_op(struct disk_op_s *op)
 
 // INT 0Eh Diskette Hardware ISR Entry Point
 void VISIBLE16
-handle_0e()
+handle_0e(void)
 {
     debug_isr(DEBUG_ISR_0e);
     if (! CONFIG_FLOPPY)
@@ -594,7 +605,7 @@ done:
 
 // Called from int08 handler.
 void
-floppy_tick()
+floppy_tick(void)
 {
     if (! CONFIG_FLOPPY)
         return;