"master drive" read stream logic

inc/util.h

@@ -195,7 +195,6 @@ void EXC_unused(void);
 #define FLOPPY_IRQ_WGATE_PRI  2
 #define FLOPPY_IRQ_STEP_PRI   3
 #define FLOPPY_IRQ_SIDE_PRI   4
-#define FLOPPY_IRQ_HI_PRI     3
 #define TIMER_IRQ_PRI         4
 #define WDATA_IRQ_PRI         7
 #define RDATA_IRQ_PRI         8

src/floppy.c

@@ -59,7 +59,6 @@ static struct dma_ring *dma_wr; /* WDATA DMA buffer */
 static struct drive {
     uint8_t cyl, head, nr_sides;
     bool_t writing;
-    bool_t sel;
     bool_t index_suppressed; /* disable IDX while writing to USB stick */
     uint8_t outp;
     struct {
@@ -74,7 +73,9 @@ static struct drive {
     } step;
     uint32_t restart_pos;
     struct image *image;
-} drive;
+    struct sel *sel;
+} drive[2], *master;
+static unsigned int nr_drive = 1;
 
 static struct image *image;
 static time_t sync_time, sync_pos;
@@ -134,9 +135,9 @@ static always_inline void drive_change_pin(
 
     /* Logically assert or deassert the pin. */
     if (assert)
-        gpio_out_active |= pin_mask;
+        sel_A.gpio_active |= pin_mask;
     else
-        gpio_out_active &= ~pin_mask;
+        sel_A.gpio_active &= ~pin_mask;
 
     /* Update the physical output pin, if the drive is selected. */
     if (drv->sel)
@@ -187,7 +188,7 @@ static void drive_change_output(
     drive_change_pin(drv, pin, assert);
 }
 
-static void update_amiga_id(bool_t amiga_hd_id)
+static void update_amiga_id(struct drive *drv, bool_t amiga_hd_id)
 {
     /* Only for the Amiga interface, with hacked RDY (pin 34) signal. */
     if (fintf_mode != FINTF_AMIGA)
@@ -197,7 +198,7 @@ static void update_amiga_id(bool_t amiga_hd_id)
 
     /* If mounting an HD image then we signal to the host by toggling pin 34 
      * every time the drive is selected. */
-    update_SELA_irq(amiga_hd_id);
+    update_SEL_irq(drv, amiga_hd_id);
 
     /* DD-ID: !!HACK!! We permanently assert pin 34, even when no disk is
      * inserted. Properly we should only do this when MTR is asserted. */
@@ -205,8 +206,8 @@ static void update_amiga_id(bool_t amiga_hd_id)
      * the HD-ID sequence 101010... with the host poll loop. It turns out that
      * starting with pin 34 asserted when the HD image is mounted seems to
      * generally work! */
-    gpio_out_active |= m(pin_34);
-    if (drive.sel)
+    drv->sel->gpio_active |= m(pin_34);
+    if (drv->sel->active)
         gpio_write_pins(gpio_out, m(pin_34), O_TRUE);
 
     IRQ_global_enable();
@@ -214,7 +215,7 @@ static void update_amiga_id(bool_t amiga_hd_id)
 
 void floppy_cancel(void)
 {
-    struct drive *drv = &drive;
+    struct drive *drv = &drive[0];
 
     /* Initialised? Bail if not. */
     if (!dma_rd)
@@ -225,7 +226,7 @@ void floppy_cancel(void)
     drive_change_output(drv, outp_rdy, FALSE);
     drive_change_output(drv, outp_wrprot, TRUE);
     drive_change_output(drv, outp_hden, FALSE);
-    update_amiga_id(FALSE);
+    update_amiga_id(drv, FALSE);
 
     /* Stop DMA + timer work. */
     IRQx_disable(dma_rdata_irq);
@@ -275,7 +276,7 @@ void floppy_set_fintf_mode(void)
         [outp_hden] = "dens",
         [outp_unused] = "high"
     };
-    struct drive *drv = &drive;
+    struct drive *drv = &drive[0];
     uint32_t old_active;
     uint8_t mode = ff_cfg.interface;
 
@@ -298,25 +299,25 @@ void floppy_set_fintf_mode(void)
     pin02 &= ~PIN_invert;
     pin34 &= ~PIN_invert;
 
-    old_active = gpio_out_active;
-    gpio_out_active &= ~(m(pin_02) | m(pin_34));
+    old_active = sel_A.gpio_active;
+    sel_A.gpio_active &= ~(m(pin_02) | m(pin_34));
     if (((drv->outp >> pin02) ^ pin02_inverted) & 1)
-        gpio_out_active |= m(pin_02);
+        sel_A.gpio_active |= m(pin_02);
     if (((drv->outp >> pin34) ^ pin34_inverted) & 1)
-        gpio_out_active |= m(pin_34);
+        sel_A.gpio_active |= m(pin_34);
 
-    /* Default handler for IRQ_SELA_changed */
-    update_SELA_irq(FALSE);
+    /* Default handler for IRQ_SEL_changed */
+    update_SEL_irq(&drive[0], FALSE);
 
     if (drv->sel) {
-        gpio_write_pins(gpio_out, old_active & ~gpio_out_active, O_FALSE);
-        gpio_write_pins(gpio_out, ~old_active & gpio_out_active, O_TRUE);
+        gpio_write_pins(gpio_out, old_active & ~sel_A.gpio_active, O_FALSE);
+        gpio_write_pins(gpio_out, ~old_active & sel_A.gpio_active, O_TRUE);
     }
 
     IRQ_global_enable();
 
     /* Default to Amiga-DD identity until HD image is mounted. */
-    update_amiga_id(FALSE);
+    update_amiga_id(drv, FALSE);
 
     printk("Interface: %s (pin2=%s%s, pin34=%s%s)\n",
            fintf_name[mode],
@@ -326,10 +327,19 @@ void floppy_set_fintf_mode(void)
 
 void floppy_init(void)
 {
-    struct drive *drv = &drive;
+    struct drive *drv = &drive[0];
     const struct exti_irq *e;
     unsigned int i;
 
+    master = &drive[0];
+    drive[0].sel = &sel_A;
+    sel_A.drive = &drive[0];
+    sel_A.sel_other = &sel_B;
+    sel_A.is_master = 1;
+    drive[1].sel = &sel_B;
+    sel_B.drive = &drive[1];
+    sel_B.sel_other = &sel_A;
+
     floppy_set_fintf_mode();
 
     board_floppy_init();
@@ -379,7 +389,7 @@ void floppy_init(void)
 
 void floppy_insert(unsigned int unit, struct slot *slot)
 {
-    struct drive *drv = &drive;
+    struct drive *drv = &drive[0];
 
     arena_init();
 
@@ -499,7 +509,7 @@ void floppy_insert(unsigned int unit, struct slot *slot)
 
     /* Drive is ready. Set output signals appropriately. */
     drive_change_output(drv, outp_rdy, TRUE);
-    update_amiga_id(image->stk_per_rev > stk_ms(300));
+    update_amiga_id(drv, image->stk_per_rev > stk_ms(300));
     if (!(slot->attributes & AM_RDO))
         drive_change_output(drv, outp_wrprot, FALSE);
 }
@@ -523,7 +533,7 @@ static void drive_set_restart_pos(struct drive *drv)
 static void wdata_stop(void)
 {
     struct write *write;
-    struct drive *drv = &drive;
+    struct drive *drv = &drive[0];
     uint8_t prev_state = dma_wr->state;
 
     /* Already inactive? Nothing to do. */
@@ -592,17 +602,17 @@ static void wdata_start(void)
 
     /* Find rotational start position of the write, in SYSCLK ticks. */
     start_pos = max_t(int32_t, 0, time_diff(index.prev_time, time_now()));
-    start_pos %= drive.image->stk_per_rev;
+    start_pos %= drive[0].image->stk_per_rev;
     start_pos *= SYSCLK_MHZ / STK_MHZ;
     write = get_write(image, image->wr_prod);
     write->start = start_pos;
-    write->track = drive_calc_track(&drive);
+    write->track = drive_calc_track(&drive[0]);
 
     /* Allow IDX pulses while handling a write. */
-    drive.index_suppressed = FALSE;
+    drive[0].index_suppressed = FALSE;
 
     /* Exit head-settling state. Ungates INDEX signal. */
-    cmpxchg(&drive.step.state, STEP_settling, 0);
+    cmpxchg(&drive[0].step.state, STEP_settling, 0);
 }
 
 /* Called from IRQ context to stop the read stream. */
@@ -629,9 +639,9 @@ static void rdata_stop(void)
 
     /* track-change = instant: Restart read stream where we left off. */
     if ((ff_cfg.track_change == TRKCHG_instant)
-        && !drive.index_suppressed
+        && !drive[0].index_suppressed
         && ff_cfg.index_suppression)
-        drive_set_restart_pos(&drive);
+        drive_set_restart_pos(&drive[0]);
 }
 
 /* Called from user context to start the read stream. */
@@ -651,11 +661,11 @@ static void rdata_start(void)
     tim_rdata->cr1 = TIM_CR1_CEN;
 
     /* Enable output. */
-    if (drive.sel)
+    if (drive[0].sel->active)
         gpio_configure_pin(gpio_data, pin_rdata, AFO_bus);
 
     /* Exit head-settling state. Ungates INDEX signal. */
-    cmpxchg(&drive.step.state, STEP_settling, 0);
+    cmpxchg(&drive[0].step.state, STEP_settling, 0);
 
 out:
     IRQ_global_enable();
@@ -664,7 +674,7 @@ out:
 static void floppy_sync_flux(void)
 {
     const uint16_t buf_mask = ARRAY_SIZE(dma_rd->buf) - 1;
-    struct drive *drv = &drive;
+    struct drive *drv = &drive[0];
     uint16_t nr_to_wrap, nr_to_cons, nr;
     int32_t ticks;
 
@@ -769,7 +779,7 @@ static bool_t dma_rd_handle(struct drive *drv)
         /* Work out where in new track to start reading data from. */
         index_time = index.prev_time;
         read_start_pos = drv->index_suppressed
-            ? drive.restart_pos /* start read exactly where write ended */
+            ? drive[0].restart_pos /* start read exactly where write ended */
             : time_since(index_time) + delay;
         read_start_pos %= drv->image->stk_per_rev;
         /* Seek to the new track. */
@@ -882,7 +892,7 @@ static bool_t dma_wr_handle(struct drive *drv)
 void floppy_set_cyl(uint8_t unit, uint8_t cyl)
 {
     if (unit == 0) {
-        struct drive *drv = &drive;
+        struct drive *drv = &drive[0];
         drv->cyl = cyl;
         if (cyl == 0)
             drive_change_output(drv, outp_trk0, TRUE);
@@ -892,15 +902,15 @@ void floppy_set_cyl(uint8_t unit, uint8_t cyl)
 void floppy_get_track(uint8_t *p_cyl, uint8_t *p_side, uint8_t *p_sel,
                       uint8_t *p_writing)
 {
-    *p_cyl = drive.cyl;
-    *p_side = drive.head & (drive.nr_sides - 1);
-    *p_sel = drive.sel;
+    *p_cyl = drive[0].cyl;
+    *p_side = drive[0].head & (drive[0].nr_sides - 1);
+    *p_sel = drive[0].sel->active;
     *p_writing = (dma_wr && dma_wr->state != DMA_inactive);
 }
 
 bool_t floppy_handle(void)
 {
-    struct drive *drv = &drive;
+    struct drive *drv = &drive[0];
 
     return ((dma_wr->state == DMA_inactive)
             ? dma_rd_handle : dma_wr_handle)(drv);
@@ -908,7 +918,7 @@ bool_t floppy_handle(void)
 
 static void index_assert(void *dat)
 {
-    struct drive *drv = &drive;
+    struct drive *drv = master;
     index.prev_time = index.timer.deadline;
     if (!drv->index_suppressed
         && !(drv->step.state && ff_cfg.index_suppression)) {
@@ -921,7 +931,7 @@ static void index_assert(void *dat)
 
 static void index_deassert(void *dat)
 {
-    struct drive *drv = &drive;
+    struct drive *drv = &drive[0];
     drive_change_output(drv, outp_index, FALSE);
 }
 
@@ -955,9 +965,34 @@ static void drive_step_timer(void *_drv)
 
 static void IRQ_soft(void)
 {
-    struct drive *drv = &drive;
+    struct drive *drv;
+    unsigned int i;
 
-    if (drv->step.state == STEP_started) {
+    drv = master;
+    if (!drv->sel->active && drv->sel->sel_other->active) {
+
+        bool_t new_master;
+
+        /* Atomically re-check that we should switch master, and swizzle the
+         * flags that are tested by the SEL-changed IRQs. This ensures that 
+         * any relevant concurrent changes to the SEL lines will cause another 
+         * call to this function. */
+        IRQ_global_disable();
+        new_master = !drv->sel->active && drv->sel->sel_other->active;
+        if (new_master) {
+            drv->sel->is_master = 0;
+            drv->sel->sel_other->is_master = 1;
+        }
+        IRQ_global_enable();
+
+        /* Stop read data stream from old master drive. */
+        if (new_master)
+            rdata_stop();
+    }
+
+    for (i = 0, drv = drive; i < nr_drive; i++, drv++) {
+        if (drv->step.state != STEP_started)
+            continue;
         timer_cancel(&drv->step.timer);
         drv->step.state = STEP_latched;
         timer_set(&drv->step.timer, drv->step.start + time_ms(1));
@@ -975,7 +1010,7 @@ static void IRQ_rdata_dma(void)
     uint32_t prev_ticks_since_index, ticks, i;
     uint16_t nr_to_wrap, nr_to_cons, nr, dmacons, done;
     time_t now;
-    struct drive *drv = &drive;
+    struct drive *drv = &drive[0];
 
     /* Clear DMA peripheral interrupts. */
     dma1->ifcr = DMA_IFCR_CGIF(dma_rdata_ch);

src/gotek/floppy.c

@@ -17,7 +17,7 @@
 #define pin_dir     0 /* PB0 */
 #define pin_step    1 /* PA1 */
 #define pin_sel0    0 /* PA0 */
-#define pin_sel1    3 /* PA3 */
+#define pin_sel1    3 /* PA3 (or PA2) */
 #define pin_wgate   9 /* PB9 */
 #define pin_side    4 /* PB4 */
 #define pin_motor  15 /* PA15 */
@@ -49,15 +49,33 @@ void IRQ_13(void) __attribute__((alias("IRQ_rdata_dma")));
 /* EXTI IRQs. */
 /*void IRQ_6(void) __attribute__((alias("IRQ_SELA_changed")));*/ /* EXTI0 */
 void IRQ_7(void) __attribute__((alias("IRQ_STEP_changed"))); /* EXTI1 */
+/*void IRQ_8(void) __attribute__((alias("IRQ_SELB_changed")));*/ /* EXTI2 */
+/*void IRQ_9(void) __attribute__((alias("IRQ_SELB_changed")));*/ /* EXTI3 */
 void IRQ_10(void) __attribute__((alias("IRQ_SIDE_changed"))); /* EXTI4 */
 void IRQ_23(void) __attribute__((alias("IRQ_WGATE_changed"))); /* EXTI9_5 */
 static const struct exti_irq exti_irqs[] = {
     {  6, FLOPPY_IRQ_SEL_PRI, 0 }, 
     {  7, FLOPPY_IRQ_STEP_PRI, m(pin_step) },
+    {  8, FLOPPY_IRQ_SEL_PRI, 0 },
+    {  9, FLOPPY_IRQ_SEL_PRI, 0 },
     { 10, FLOPPY_IRQ_SIDE_PRI, 0 }, 
     { 23, FLOPPY_IRQ_WGATE_PRI, 0 } 
 };
 
+extern struct sel {
+    uint32_t _unused;
+    uint16_t b_op, nop;
+    /* Subset of output pins which are active (O_TRUE). */
+    uint32_t gpio_active;
+    /* GPIO register to either assert or deassert active output pins. */
+    uint32_t gpio_setreset;
+    struct drive *drive;
+    uint32_t pin_mask;
+    uint32_t active;
+    struct sel *sel_other;
+    uint32_t is_master;
+} sel_A, sel_B;
+
 bool_t floppy_ribbon_is_reversed(void)
 {
     time_t t_start = time_now();
@@ -77,6 +95,11 @@ bool_t floppy_ribbon_is_reversed(void)
 
 static void board_floppy_init(void)
 {
+    uint32_t trigger_mask;
+
+    sel_A.pin_mask = m(pin_sel0);
+    sel_B.pin_mask = m(pin_sel1);
+
     gpio_configure_pin(gpiob, pin_dir,   GPI_bus);
     gpio_configure_pin(gpioa, pin_step,  GPI_bus);
     gpio_configure_pin(gpioa, pin_sel0,  GPI_bus);
@@ -87,104 +110,123 @@ static void board_floppy_init(void)
         gpio_configure_pin(gpioa, pin_motor, GPI_bus);
     }
 
-    /* PB[15:2] -> EXT[15:2], PA[1:0] -> EXT[1:0] */
+    /* PB[15:4] -> EXT[15:4], PA[3:0] -> EXT[3:0] */
     afio->exticr2 = afio->exticr3 = afio->exticr4 = 0x1111;
-    afio->exticr1 = 0x1100;
+    afio->exticr1 = 0x0000;
 
-    exti->imr = exti->rtsr = exti->ftsr =
-        m(pin_wgate) | m(pin_side) | m(pin_step) | m(pin_sel0);
+    trigger_mask = m(pin_wgate) | m(pin_side) | m(pin_step) | m(pin_sel0);
+    if (nr_drive == 2)
+        trigger_mask |= m(pin_sel1);
+    exti->imr = exti->rtsr = exti->ftsr = trigger_mask;
 }
 
-/* Fast speculative entry point for SELA-changed IRQ. We assume SELA has 
+/* Fast speculative entry point for SEL-changed IRQ. We assume SEL has 
  * changed to the opposite of what we observed on the previous interrupt. This
  * is always the case unless we missed an edge (fast transitions). 
- * Note that the entirety of the SELA handler is in SRAM (.data) -- not only 
- * is this faster to execute, but allows us to co-locate gpio_out_active for 
+ * Note that the entirety of the SEL handler is in SRAM (.data) -- not only 
+ * is this faster to execute, but allows us to co-locate sel_{A,B} for 
  * even faster access in the time-critical speculative entry point. */
 void IRQ_SELA_changed(void);
+void IRQ_SELB_changed(void);
+#define BUILD_IRQ_SEL(x)                                                \
+"    .align 4\n"                                                        \
+"    .thumb_func\n"                                                     \
+"    .type IRQ_SEL"#x"_changed,%function\n"                             \
+"IRQ_SEL"#x"_changed:\n"                                                \
+"    mov  r0, pc\n"                                                     \
+"    ldr  r2, [r0, #12]\n" /* r1 = &gpio_out->b[s]rr */                 \
+"sel_"#x":\n"                                                           \
+"    ldr  r1, [r0, #8]\n"  /* r0 = sel.gpio_active   */                 \
+"    str  r1, [r2, #0]\n"  /* gpio_out->b[s]rr = sel.gpio_active */     \
+"    b.n  _IRQ_SEL_changed\n" /* branch to the main ISR entry point */  \
+"    nop\n"                                                             \
+"   .word 0\n"          /* sel.gpio_active */                           \
+"   .word 0x40010c10\n" /* sel.gpio_setreset (=&gpio_out->b[s]rr) */    \
+"   .word 0\n"          /* sel.drive */                                 \
+"   .word 0\n"          /* sel.pin_mask */                              \
+"   .word 0\n"          /* sel.active */                                \
+"   .word 0\n"          /* sel.sel_other */                             \
+"   .word 0\n"          /* sel.is_master */
 asm (
 "    .data\n"
-"    .align 4\n"
-"    .thumb_func\n"
-"    .type IRQ_SELA_changed,%function\n"
-"IRQ_SELA_changed:\n"
-"    ldr  r0, [pc, #4]\n" /* r0 = gpio_out_active */
-"    ldr  r1, [pc, #8]\n" /* r1 = &gpio_out->b[s]rr */
-"    str  r0, [r1, #0]\n" /* gpio_out->b[s]rr = gpio_out_active */
-"    b.n  _IRQ_SELA_changed\n" /* branch to the main ISR entry point */
-"gpio_out_active:   .word 0\n"
-"gpio_out_setreset: .word 0x40010c10\n" /* gpio_out->b[s]rr */
-"    .global IRQ_6\n"
-"    .thumb_set IRQ_6,IRQ_SELA_changed\n"
+BUILD_IRQ_SEL(A)
+BUILD_IRQ_SEL(B)
+"    .global IRQ_6 ; .thumb_set IRQ_6,IRQ_SELA_changed\n"
+"    .global IRQ_8\n"
+"    .thumb_set IRQ_8,IRQ_SELB_changed\n"
+"    .global IRQ_9\n"
+"    .thumb_set IRQ_9,IRQ_SELB_changed\n"
 "    .previous\n"
 );
 
-/* Subset of output pins which are active (O_TRUE). */
-extern uint32_t gpio_out_active;
-
-/* GPIO register to either assert or deassert active output pins. */
-extern uint32_t gpio_out_setreset;
-
-static void Amiga_HD_ID(uint32_t _gpio_out_active, uint32_t _gpio_out_setreset)
+static void Amiga_HD_ID(struct sel *sel)
     __attribute__((used)) __attribute__((section(".data@")));
-static void _IRQ_SELA_changed(uint32_t _gpio_out_active)
+static void _IRQ_SEL_changed(struct sel *sel)
     __attribute__((used)) __attribute__((section(".data@")));
 
-/* Intermediate SELA-changed handler for generating the Amiga HD RDY signal. */
-static void Amiga_HD_ID(uint32_t _gpio_out_active, uint32_t _gpio_out_setreset)
+/* Intermediate SEL-changed handler for generating the Amiga HD RDY signal. */
+static void Amiga_HD_ID(struct sel *sel)
 {
     /* If deasserting the bus, toggle pin 34 for next time we take the bus. */
-    if (!(_gpio_out_setreset & 4))
-        gpio_out_active ^= m(pin_34);
+    if (!(sel->gpio_setreset & 4))
+        sel->gpio_active ^= m(pin_34);
 
-    /* Continue to the main SELA-changed IRQ entry point. */
-    _IRQ_SELA_changed(_gpio_out_active);
+    /* Continue to the main SEL-changed IRQ entry point. */
+    _IRQ_SEL_changed(sel);
 }
 
-/* Main entry point for SELA-changed IRQ. This fixes up GPIO pins if we 
+/* Main entry point for SEL-changed IRQ. This fixes up GPIO pins if we 
  * mis-speculated, also handles the timer-driver RDATA pin, and sets up the 
  * speculative entry point for the next interrupt. */
-static void _IRQ_SELA_changed(uint32_t _gpio_out_active)
+static void _IRQ_SEL_changed(struct sel *sel)
 {
-    /* Clear SELA-changed flag. */
-    exti->pr = m(pin_sel0);
+    uint32_t idr_a;
 
-    if (!(gpioa->idr & m(pin_sel0))) {
-        /* SELA is asserted (this drive is selected). 
+    /* Clear SEL-changed flag. */
+    exti->pr = sel->pin_mask;
+
+    idr_a = gpioa->idr;
+
+    if (!(idr_a & sel->pin_mask)) {
+    assert_other:
+        /* SEL is asserted (this drive is selected). 
          * Immediately re-enable all our asserted outputs. */
-        gpio_out->brr = _gpio_out_active;
+        gpio_out->brr = sel->gpio_active;
         /* Set pin_rdata as timer output (AFO_bus). */
         if (dma_rd && (dma_rd->state == DMA_active))
             gpio_data->crl = (gpio_data->crl & ~(0xfu<<(pin_rdata<<2)))
                 | ((AFO_bus&0xfu)<<(pin_rdata<<2));
         /* Let main code know it can drive the bus until further notice. */
-        drive.sel = 1;
+        sel->active = 1;
+        sel->gpio_setreset &= ~4; /* gpio_out->bsrr */
+        if (unlikely(!sel->is_master))
+            IRQx_set_pending(FLOPPY_SOFTIRQ);
     } else {
-        /* SELA is deasserted (this drive is not selected).
+        /* SEL is deasserted (this drive is not selected).
          * Relinquish the bus by disabling all our asserted outputs. */
-        gpio_out->bsrr = _gpio_out_active;
+        gpio_out->bsrr = sel->gpio_active;
         /* Set pin_rdata as quiescent (GPO_bus). */
         if (dma_rd && (dma_rd->state == DMA_active))
             gpio_data->crl = (gpio_data->crl & ~(0xfu<<(pin_rdata<<2)))
                 | ((GPO_bus&0xfu)<<(pin_rdata<<2));
         /* Tell main code to leave the bus alone. */
-        drive.sel = 0;
+        sel->active = 0;
+        sel->gpio_setreset |= 4; /* gpio_out->brr */
+        /* If other emulated drive is active, assert it on the bus. */
+        sel = sel->sel_other;
+        if (unlikely(sel->active) && !(idr_a & sel->pin_mask))
+            goto assert_other;
     }
-
-    /* Set up the speculative fast path for the next interrupt. */
-    if (drive.sel)
-        gpio_out_setreset &= ~4; /* gpio_out->bsrr */
-    else
-        gpio_out_setreset |= 4; /* gpio_out->brr */
 }
 
-/* Update the SELA handler. Used for switching in the Amiga HD-ID "magic". 
+/* Update the SEL handler. Used for switching in the Amiga HD-ID "magic". 
  * Must be called with interrupts disabled. */
-static void update_SELA_irq(bool_t amiga_hd_id)
+static void update_SEL_irq(struct drive *drv, bool_t amiga_hd_id)
 {
     uint32_t handler = amiga_hd_id ? (uint32_t)Amiga_HD_ID
-        : (uint32_t)_IRQ_SELA_changed;
-    uint32_t entry = (uint32_t)IRQ_SELA_changed;
+        : (uint32_t)_IRQ_SEL_changed;
+    uint32_t entry = (drv == &drive[0]) ? (uint32_t)IRQ_SELA_changed
+        : (uint32_t)IRQ_SELB_changed;
     uint16_t opcode;
 
     /* Strip the Thumb LSB from the function addresses. */
@@ -192,12 +234,12 @@ static void update_SELA_irq(bool_t amiga_hd_id)
     entry &= ~1;
 
     /* Create a new tail-call instruction for the entry stub. */
-    opcode = handler - (entry + 6 + 4);
+    opcode = handler - (entry + 8 + 4);
     opcode = 0xe000 | (opcode >> 1);
 
     /* If the tail-call instruction has changed, modify the entry stub. */
-    if (unlikely(((uint16_t *)entry)[3] != opcode)) {
-        ((uint16_t *)entry)[3] = opcode;
+    if (unlikely(((uint16_t *)entry)[4] != opcode)) {
+        ((uint16_t *)entry)[4] = opcode;
         cpu_sync(); /* synchronise self-modifying code */
     }
 }
@@ -216,8 +258,9 @@ static bool_t drive_is_writing(void)
 
 static void IRQ_STEP_changed(void)
 {
-    struct drive *drv = &drive;
+    struct drive *drv;
     uint8_t idr_a, idr_b;
+    unsigned int i;
 
     /* Clear STEP-changed flag. */
     exti->pr = m(pin_step);
@@ -226,45 +269,47 @@ static void IRQ_STEP_changed(void)
     idr_a = gpioa->idr;
     idr_b = gpiob->idr;
 
-    /* Bail if drive not selected. */
-    if (idr_a & m(pin_sel0))
-        return;
+    for (i = 0, drv = drive; i < nr_drive; i++, drv++) {
+        /* Bail if drive not selected. */
+        if (idr_a & drv->sel->pin_mask)
+            continue;
 
-    /* DSKCHG asserts on any falling edge of STEP. We deassert on any edge. */
-    if ((drv->outp & m(outp_dskchg)) && (dma_rd != NULL))
-        drive_change_output(drv, outp_dskchg, FALSE);
+        /* DSKCHG asserts on any falling edge of STEP. Deassert on any edge. */
+        if ((drv->outp & m(outp_dskchg)) && (dma_rd != NULL)) /* XXX */
+            drive_change_output(drv, outp_dskchg, FALSE);
 
-    if (!(idr_a & m(pin_step))   /* Not rising edge on STEP? */
-        || (drv->step.state & STEP_active) /* Already mid-step? */
-        || drive_is_writing())   /* Write in progress? */
-        return;
+        if (!(idr_a & m(pin_step))   /* Not rising edge on STEP? */
+            || (drv->step.state & STEP_active) /* Already mid-step? */
+            || drive_is_writing())   /* Write in progress? */
+            return;
 
-    /* Latch the step direction and check bounds (0 <= cyl <= 255). */
-    drv->step.inward = !(idr_b & m(pin_dir));
-    if (drv->cyl == (drv->step.inward ? 255 : 0))
-        return;
+        /* Latch the step direction and check bounds (0 <= cyl <= 255). */
+        drv->step.inward = !(idr_b & m(pin_dir));
+        if (drv->cyl == (drv->step.inward ? 255 : 0))
+            return;
 
-    /* Valid step request for this drive: start the step operation. */
-    drv->step.start = time_now();
-    drv->step.state = STEP_started;
-    if (drv->outp & m(outp_trk0))
-        drive_change_output(drv, outp_trk0, FALSE);
-    if (dma_rd != NULL) {
-        rdata_stop();
-        if (!ff_cfg.index_suppression) {
-            /* Opportunistically insert an INDEX pulse ahead of seek op. */
-            drive_change_output(drv, outp_index, TRUE);
-            index.fake_fired = TRUE;
+        /* Valid step request for this drive: start the step operation. */
+        drv->step.start = time_now();
+        drv->step.state = STEP_started;
+        if (drv->outp & m(outp_trk0))
+            drive_change_output(drv, outp_trk0, FALSE);
+        if ((drv == master) && (dma_rd != NULL)) {
+            rdata_stop();
+            if (!ff_cfg.index_suppression) {
+                /* Opportunistically insert an INDEX pulse ahead of seek op. */
+                drive_change_output(drv, outp_index, TRUE);
+                index.fake_fired = TRUE;
+            }
         }
+        IRQx_set_pending(FLOPPY_SOFTIRQ);
     }
-    IRQx_set_pending(FLOPPY_SOFTIRQ);
 }
 
 static void IRQ_SIDE_changed(void)
 {
     stk_time_t t = stk_now();
     unsigned int filter = stk_us(ff_cfg.side_select_glitch_filter);
-    struct drive *drv = &drive;
+    struct drive *drv = &drive[0];
     uint8_t hd;
 
     do {
@@ -279,16 +324,18 @@ static void IRQ_SIDE_changed(void)
         /* If configured to do so, wait a few microseconds to ensure this isn't
          * a glitch (eg. signal is mistaken for the archaic Fault-Reset line by
          * old CP/M loaders, and pulsed LOW when starting a read). */
-    } while (stk_diff(t, stk_now()) < filter);
+    } while (stk_timesince(t) < filter);
 
-    drv->head = hd;
-    if ((dma_rd != NULL) && (drv->nr_sides == 2))
+    /* Update head number on both drives. */
+    drv->head = drive[1].head = hd;
+
+    if ((dma_rd != NULL) && (master->nr_sides == 2))
         rdata_stop();
 }
 
 static void IRQ_WGATE_changed(void)
 {
-    struct drive *drv = &drive;
+    struct drive *drv = master; /* XXX check sel.pin_mask directly */
 
     /* Clear WGATE-changed flag. */
     exti->pr = m(pin_wgate);
@@ -297,8 +344,8 @@ static void IRQ_WGATE_changed(void)
     if (drv->outp & m(outp_wrprot))
         return;
 
-    if ((gpiob->idr & m(pin_wgate))      /* WGATE off? */
-        || (gpioa->idr & m(pin_sel0))) { /* Not selected? */
+    if ((gpiob->idr & m(pin_wgate)) /* WGATE off? */
+        || (gpioa->idr & drv->sel->pin_mask)) { /* Not selected? */
         wdata_stop();
     } else {
         rdata_stop();