Add drive letter (0,1,2,A,B) to gw.py read/write commands.

Allows use of multiple drives with new F7 GW hardware.

inc/cdc_acm_protocol.h

@@ -16,37 +16,48 @@
 
 /* CMD_GET_INFO, length=3, 0. Returns 32 bytes after ACK. */
 #define CMD_GET_INFO        0
-/* CMD_SEEK, length=3, cyl# */
-#define CMD_SEEK            1
+/* [BOOTLOADER] CMD_UPDATE, length=6, <update_len>. 
+ * Host follows with <update_len> bytes.
+ * Bootloader finally returns a status byte, 0 on success. */
+#define CMD_UPDATE          1
+/* CMD_SEEK, length=3, cyl#. Seek to cyl# on selected drive. */
+#define CMD_SEEK            2
 /* CMD_SIDE, length=3, side# (0=bottom) */
-#define CMD_SIDE            2
+#define CMD_SIDE            3
 /* CMD_SET_PARAMS, length=3+nr, idx, <nr bytes> */
-#define CMD_SET_PARAMS      3
+#define CMD_SET_PARAMS      4
 /* CMD_GET_PARAMS, length=4, idx, nr_bytes. Returns nr_bytes after ACK. */
-#define CMD_GET_PARAMS      4
-/* CMD_MOTOR, length=3, motor_mask */
-#define CMD_MOTOR           5
+#define CMD_GET_PARAMS      5
+/* CMD_MOTOR, length=4, drive#, on/off. Turn on/off a drive motor. */
+#define CMD_MOTOR           6
 /* CMD_READ_FLUX, length=2-3. Optionally include all or part of gw_read_flux.
  * Returns flux readings until EOStream. */
-#define CMD_READ_FLUX       6
+#define CMD_READ_FLUX       7
 /* CMD_WRITE_FLUX, length=2-7. Optionally include all or part of gw_write_flux.
  * Host follows with flux readings until EOStream. */
-#define CMD_WRITE_FLUX      7
+#define CMD_WRITE_FLUX      8
 /* CMD_GET_FLUX_STATUS, length=2. Last read/write status returned in ACK. */
-#define CMD_GET_FLUX_STATUS 8
+#define CMD_GET_FLUX_STATUS 9
 /* CMD_GET_INDEX_TIMES, length=4, first, nr.
  * Returns nr*4 bytes after ACK. */
-#define CMD_GET_INDEX_TIMES 9
-/* CMD_SELECT, length=3, select_mask */
-#define CMD_SELECT         10
+#define CMD_GET_INDEX_TIMES 10
 /* CMD_SWITCH_FW_MODE, length=3, <mode> */
 #define CMD_SWITCH_FW_MODE 11
-#define CMD_MAX            11
+/* CMD_SELECT, length=3, drive#. Select drive# as current unit. */
+#define CMD_SELECT         12
+/* CMD_DESELECT, length=2. Deselect current unit (if any). */
+#define CMD_DESELECT       13
+/* CMD_SET_BUS_TYPE, length=3, bus_type. Set the bus type. */
+#define CMD_SET_BUS_TYPE   14
+#define CMD_MAX            14
 
-/* [BOOTLOADER] CMD_UPDATE, length=6, <update_len>. 
- * Host follows with <update_len> bytes.
- * Bootloader finally returns a status byte, 0 on success. */
-#define CMD_UPDATE          1
+
+/*
+ * CMD_SET_BUS CODES
+ */
+#define BUS_NONE            0
+#define BUS_IBMPC           1
+#define BUS_SHUGART         2
 
 
 /*
@@ -59,6 +70,9 @@
 #define ACK_FLUX_OVERFLOW   4
 #define ACK_FLUX_UNDERFLOW  5
 #define ACK_WRPROT          6
+#define ACK_NO_UNIT         7
+#define ACK_NO_BUS          8
+#define ACK_BAD_UNIT        9
 
 
 /*

scripts/greaseweazle/tools/read.py

@@ -38,6 +38,8 @@ def main(argv):
 
     parser = argparse.ArgumentParser(
         formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    parser.add_argument("--drive", type=util.drive_letter, default='A',
+                        help="drive to read (A,B,0,1,2)")
     parser.add_argument("--revs", type=int, default=3,
                         help="number of revolutions to read per track")
     parser.add_argument("--scyl", type=int, default=0,

scripts/greaseweazle/tools/util.py

@@ -7,7 +7,7 @@
 # This is free and unencumbered software released into the public domain.
 # See the file COPYING for more details, or visit <http://unlicense.org>.
 
-import os, sys, serial, struct, time
+import argparse, os, sys, serial, struct, time
 
 from greaseweazle import version
 from greaseweazle import usb as USB
@@ -15,6 +15,19 @@ from greaseweazle.image.scp import SCP
 from greaseweazle.image.hfe import HFE
 
 
+def drive_letter(letter):
+    types = {
+        'A': (USB.BusType.IBMPC, 0),
+        'B': (USB.BusType.IBMPC, 1),
+        '0': (USB.BusType.Shugart, 0),
+        '1': (USB.BusType.Shugart, 1),
+        '2': (USB.BusType.Shugart, 2)
+    }
+    if not letter.upper() in types:
+        raise argparse.ArgumentTypeError("invalid drive letter: '%s'" % letter)
+    return types[letter.upper()]
+
+
 def get_image_class(name):
     image_types = { '.scp': SCP, '.hfe': HFE }
     _, ext = os.path.splitext(name)
@@ -25,9 +38,10 @@ def get_image_class(name):
 
 
 def with_drive_selected(fn, usb, args):
+    usb.set_bus_type(args.drive[0])
     try:
-        usb.drive_select(True)
-        usb.drive_motor(True)
+        usb.drive_select(args.drive[1])
+        usb.drive_motor(args.drive[1], True)
         fn(usb, args)
     except KeyboardInterrupt:
         print()
@@ -35,8 +49,8 @@ def with_drive_selected(fn, usb, args):
         usb.ser.close()
         usb.ser.open()
     finally:
-        usb.drive_motor(False)
-        usb.drive_select(False)
+        usb.drive_motor(args.drive[1], False)
+        usb.drive_deselect()
 
 
 def usb_reopen(usb, is_update):

scripts/greaseweazle/tools/write.py

@@ -67,6 +67,8 @@ def main(argv):
 
     parser = argparse.ArgumentParser(
         formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    parser.add_argument("--drive", type=util.drive_letter, default='A',
+                        help="drive to write (A,B,0,1,2)")
     parser.add_argument("--scyl", type=int, default=0,
                         help="first cylinder to write")
     parser.add_argument("--ecyl", type=int, default=81,

scripts/greaseweazle/usb.py

@@ -18,19 +18,37 @@ class ControlCmd:
 ## Command set
 class Cmd:
     GetInfo         =  0
-    Seek            =  1
-    Side            =  2
-    SetParams       =  3
-    GetParams       =  4
-    Motor           =  5
-    ReadFlux        =  6
-    WriteFlux       =  7
-    GetFluxStatus   =  8
-    GetIndexTimes   =  9
-    Select          = 10
-    SwitchFwMode    = 11
-    # Bootloader specific:
     Update          =  1
+    Seek            =  2
+    Side            =  3
+    SetParams       =  4
+    GetParams       =  5
+    Motor           =  6
+    ReadFlux        =  7
+    WriteFlux       =  8
+    GetFluxStatus   =  9
+    GetIndexTimes   = 10
+    SwitchFwMode    = 11
+    Select          = 12
+    Deselect        = 13
+    SetBusType      = 14
+    str = {
+        GetInfo: "GetInfo",
+        Update: "Update",
+        Seek: "Seek",
+        Side: "Side",
+        SetParams: "SetParams",
+        GetParams: "GetParams",
+        Motor: "Motor",
+        ReadFlux: "ReadFlux",
+        WriteFlux: "WriteFlux",
+        GetFluxStatus: "GetFluxStatus",
+        GetIndexTimes: "GetIndexTimes",
+        SwitchFwMode: "SwitchFwMode",
+        Select: "Select",
+        Deselect: "Deselect",
+        SetBusType: "SetBusType"
+    }
 
 
 ## Command responses/acknowledgements
@@ -42,7 +60,22 @@ class Ack:
     FluxOverflow    = 4
     FluxUnderflow   = 5
     Wrprot          = 6
-    Max             = 6
+    NoUnit          = 7
+    NoBus           = 8
+    BadUnit         = 9
+    str = {
+        Okay: "Okay",
+        BadCommand: "Bad Command",
+        NoIndex: "No Index",
+        NoTrk0: "Track 0 not found",
+        FluxOverflow: "Flux Overflow",
+        FluxUnderflow: "Flux Underflow",
+        Wrprot: "Disk is Write Protected",
+        NoUnit: "No drive unit selected",
+        NoBus: "No bus type (eg. Shugart, IBM/PC) specified",
+        BadUnit: "Bad unit number"
+    }
+
 
 
 ## Cmd.{Get,Set}Params indexes
@@ -50,20 +83,24 @@ class Params:
     Delays          = 0
 
 
+## Cmd.SetBusType values
+class BusType:
+    Invalid         = 0
+    IBMPC           = 1
+    Shugart         = 2
+
+
 ## CmdError: Encapsulates a command acknowledgement.
 class CmdError(Exception):
 
-    str = [ "Okay", "Bad Command", "No Index", "Track 0 not found",
-            "Flux Overflow", "Flux Underflow", "Disk is Write Protected" ]
-
     def __init__(self, cmd, code):
         self.cmd = cmd
         self.code = code
 
     def __str__(self):
-        if self.code <= Ack.Max:
-            return self.str[self.code]
-        return "Unknown Error (%u)" % self.code
+        return "%s: %s" % (Cmd.str.get(self.cmd, "UnknownCmd"),
+                           Ack.str.get(self.code, "Unknown Error (%u)"
+                                       % self.code))
 
 
 class Unit:
@@ -134,16 +171,28 @@ class Unit:
         self._send_cmd(struct.pack("3B", Cmd.Side, 3, side))
 
 
+    ## set_bus_type:
+    ## Set the floppy bus type.
+    def set_bus_type(self, type):
+        self._send_cmd(struct.pack("3B", Cmd.SetBusType, 3, type))
+
+
     ## drive_select:
-    ## Select/deselect the drive.
-    def drive_select(self, state):
-        self._send_cmd(struct.pack("3B", Cmd.Select, 3, int(state)))
+    ## Select the specified drive unit.
+    def drive_select(self, unit):
+        self._send_cmd(struct.pack("3B", Cmd.Select, 3, unit))
+
+
+    ## drive_deselect:
+    ## Deselect currently-selected drive unit (if any).
+    def drive_deselect(self):
+        self._send_cmd(struct.pack("2B", Cmd.Deselect, 2))
 
 
     ## drive_motor:
-    ## Turn the selected drive's motor on/off.
-    def drive_motor(self, state):
-        self._send_cmd(struct.pack("3B", Cmd.Motor, 3, int(state)))
+    ## Turn the specified drive's motor on/off.
+    def drive_motor(self, unit, state):
+        self._send_cmd(struct.pack("4B", Cmd.Motor, 4, unit, int(state)))
 
 
     ## _get_index_times:

scripts/scp_info.py

@@ -0,0 +1,43 @@
+import struct, sys
+
+trknr = int(sys.argv[2])
+
+with open(sys.argv[1], "rb") as f:
+    dat = f.read()
+
+header = struct.unpack("<3s9BI", dat[0:16])
+(sig, _, _, nr_revs, s_trk, e_trk, flags, _, ss, _, _) = header
+assert sig == b"SCP"
+nr_sides = 1 if ss else 2
+        
+trk_offs = struct.unpack("<168I", dat[16:0x2b0])
+
+print("Revolutions: %u" % nr_revs)
+print("Track %u:" % trknr)
+
+trk_off = trk_offs[trknr]
+if trk_off == 0:
+    print("Empty")
+    sys.exit(0)
+    
+# Parse the SCP track header and extract the flux data.
+thdr = dat[trk_off:trk_off+4+12*nr_revs]
+sig, tnr, _, _, s_off = struct.unpack("<3sB3I", thdr[:16])
+assert sig == b"TRK"
+assert tnr == trknr
+for i in range(nr_revs):
+    t,n,_ = struct.unpack("<3I", thdr[4+i*12:4+(i+1)*12])
+    print("Rev %u: time=%uus flux=%u" % (i, t//40, n))
+_, e_nr, e_off = struct.unpack("<3I", thdr[-12:])
+tdat = dat[trk_off+s_off:trk_off+e_off+e_nr*2]
+fluxl = []
+while tdat:
+    flux, = struct.unpack(">H", tdat[:2])
+    tdat = tdat[2:]
+    fluxl.append(flux / 40)
+tot = 0.0
+for x in fluxl:
+    print(x)
+    tot += x
+print("Total: %uus (%uus per rev)" % (int(tot), tot//nr_revs))
+

src/floppy.c

@@ -24,27 +24,30 @@
 #define sample_us(x) ((x) * SAMPLE_MHZ)
 #define time_from_samples(x) ((x) * TIME_MHZ / SAMPLE_MHZ)
 
+#define write_pin(pin, level) \
+    gpio_write_pin(gpio_##pin, pin_##pin, level ? O_TRUE : O_FALSE)
+
+static int bus_type = -1;
+static int unit_nr = -1;
+static struct {
+    int cyl;
+    bool_t motor;
+} unit[3];
+
+static struct gw_delay delay_params = {
+    .select_delay = 10,
+    .step_delay = 3000,
+    .seek_settle = 15,
+    .motor_delay = 750,
+    .auto_off = 10000
+};
+
 #if STM32F == 1
 #include "floppy_f1.c"
 #elif STM32F == 7
 #include "floppy_f7.c"
 #endif
 
-/* Track and modify states of output pins. */
-static struct {
-    bool_t densel;
-    bool_t sel0;
-    bool_t mot0;
-    bool_t dir;
-    bool_t step;
-    bool_t wgate;
-    bool_t side;
-} pins;
-#define read_pin(pin) pins.pin
-#define write_pin(pin, level) ({                                        \
-    gpio_write_pin(gpio_##pin, pin_##pin, level ? O_TRUE : O_FALSE);    \
-    pins.pin = level; })
-
 static struct index {
     /* Main code can reset this at will. */
     volatile unsigned int count;
@@ -91,14 +94,6 @@ static uint8_t u_buf[8192];
 static uint32_t u_cons, u_prod;
 #define U_MASK(x) ((x)&(sizeof(u_buf)-1))
 
-static struct gw_delay delay_params = {
-    .select_delay = 10,
-    .step_delay = 3000,
-    .seek_settle = 15,
-    .motor_delay = 750,
-    .auto_off = 10000
-};
-
 static void step_one_out(void)
 {
     write_pin(dir, FALSE);
@@ -119,19 +114,35 @@ static void step_one_in(void)
     delay_us(delay_params.step_delay);
 }
 
-static int cur_cyl = -1;
-
-static void drive_select(bool_t on)
+static bool_t set_bus_type(uint8_t type)
 {
-    if (read_pin(sel0) == on)
-        return;
-    write_pin(sel0, on);
-    if (on)
-        delay_us(delay_params.select_delay);
+    int i;
+
+    if (type == bus_type)
+        return TRUE;
+
+    if (type > BUS_SHUGART)
+        return FALSE;
+
+    bus_type = type;
+    unit_nr = -1;
+    for (i = 0; i < ARRAY_SIZE(unit); i++) {
+        unit[i].cyl = -1;
+        unit[i].motor = FALSE;
+    }
+    reset_bus();
+
+    return TRUE;
 }
 
-static bool_t floppy_seek(unsigned int cyl)
+static uint8_t floppy_seek(unsigned int cyl)
 {
+    int cur_cyl;
+
+    if (unit_nr < 0)
+        return ACK_NO_UNIT;
+    cur_cyl = unit[unit_nr].cyl;
+
     if ((cyl == 0) || (cur_cyl < 0)) {
 
         unsigned int i;
@@ -142,8 +153,8 @@ static bool_t floppy_seek(unsigned int cyl)
         }
         cur_cyl = 0;
         if (get_trk0() == HIGH) {
-            cur_cyl = -1;
-            return FALSE;
+            unit[unit_nr].cyl = -1;
+            return ACK_NO_TRK0;
         }
 
     }
@@ -165,18 +176,9 @@ static bool_t floppy_seek(unsigned int cyl)
     }
 
     delay_ms(delay_params.seek_settle);
-    cur_cyl = cyl;
-
-    return TRUE;
-}
+    unit[unit_nr].cyl = cyl;
 
-static void drive_motor(bool_t on)
-{
-    if (read_pin(mot0) == on)
-        return;
-    write_pin(mot0, on);
-    if (on)
-        delay_ms(delay_params.motor_delay);
+    return ACK_OKAY;
 }
 
 static void floppy_flux_end(void)
@@ -207,14 +209,7 @@ static void floppy_reset(void)
 
     floppy_flux_end();
 
-    /* Turn off all output pins. */
-    write_pin(densel, FALSE);
-    write_pin(sel0,   FALSE);
-    write_pin(mot0,   FALSE);
-    write_pin(dir,    FALSE);
-    write_pin(step,   FALSE);
-    write_pin(wgate,  FALSE);
-    write_pin(side,   FALSE);
+    drive_deselect();
 
     act_led(FALSE);
 }
@@ -225,8 +220,6 @@ void floppy_init(void)
 
     /* Output pins, unbuffered. */
     configure_pin(densel, GPO_bus);
-    configure_pin(sel0,   GPO_bus);
-    configure_pin(mot0,   GPO_bus);
     configure_pin(dir,    GPO_bus);
     configure_pin(step,   GPO_bus);
     configure_pin(wgate,  GPO_bus);
@@ -246,6 +239,8 @@ void floppy_init(void)
     exti->imr = exti->ftsr = m(pin_index);
     IRQx_set_prio(irq_index, INDEX_IRQ_PRI);
     IRQx_enable(irq_index);
+
+    set_bus_type(BUS_NONE);
 }
 
 static struct gw_info gw_info = {
@@ -774,7 +769,7 @@ static void process_command(void)
         uint8_t cyl = u_buf[2];
         if ((len != 3) || (cyl > 85))
             goto bad_command;
-        u_buf[1] = floppy_seek(cyl) ? ACK_OKAY : ACK_NO_TRK0;
+        u_buf[1] = floppy_seek(cyl);
         goto out;
     }
     case CMD_SIDE: {
@@ -803,11 +798,11 @@ static void process_command(void)
         break;
     }
     case CMD_MOTOR: {
-        uint8_t mask = u_buf[2];
-        if ((len != 3) || (mask & ~1))
+        uint8_t unit = u_buf[2], on_off = u_buf[3];
+        if ((len != 4) || (on_off & ~1))
             goto bad_command;
-        drive_motor(mask & 1);
-        break;
+        u_buf[1] = drive_motor(unit, on_off & 1);
+        goto out;
     }
     case CMD_READ_FLUX: {
         struct gw_read_flux rf = { .nr_idx = 2 };
@@ -841,10 +836,22 @@ static void process_command(void)
         break;
     }
     case CMD_SELECT: {
-        uint8_t mask = u_buf[2];
-        if ((len != 3) || (mask & ~1))
+        uint8_t unit = u_buf[2];
+        if (len != 3)
+            goto bad_command;
+        u_buf[1] = drive_select(unit);
+        goto out;
+    }
+    case CMD_DESELECT: {
+        if (len != 2)
+            goto bad_command;
+        drive_deselect();
+        break;
+    }
+    case CMD_SET_BUS_TYPE: {
+        uint8_t type = u_buf[2];
+        if ((len != 3) || !set_bus_type(type))
             goto bad_command;
-        drive_select(mask & 1);
         break;
     }
     case CMD_SWITCH_FW_MODE: {
@@ -885,12 +892,15 @@ static void floppy_configure(void)
 
 void floppy_process(void)
 {
-    int len;
+    int i, len;
 
     if (auto_off.armed && (time_since(auto_off.deadline) >= 0)) {
         floppy_flux_end();
-        drive_motor(FALSE);
-        drive_select(FALSE);
+        for (i = 0; i < ARRAY_SIZE(unit); i++) {
+            if (unit[i].motor)
+                drive_motor(i, FALSE);
+        }
+        drive_deselect();
         auto_off.armed = FALSE;
     }
 

src/floppy_f1.c

@@ -27,10 +27,10 @@ static unsigned int GPI_bus;
 /* Output pins. */
 #define gpio_densel gpiob
 #define pin_densel  9 /* PB9 */
-#define gpio_sel0  gpiob
-#define pin_sel0   10 /* PB10 */
-#define gpio_mot0  gpiob
-#define pin_mot0   11 /* PB11 */
+#define gpio_sel   gpiob
+#define pin_sel    10 /* PB10 */
+#define gpio_mot   gpiob
+#define pin_mot    11 /* PB11 */
 #define gpio_dir   gpiob
 #define pin_dir    12 /* PB12 */
 #define gpio_step  gpiob
@@ -77,6 +77,10 @@ static void floppy_mcu_init(void)
     afio->exticr1 = afio->exticr2 = afio->exticr3 = afio->exticr4 = 0x1111;
 
     configure_pin(rdata, GPI_bus);
+
+    /* Configure SELECT/MOTOR lines. */
+    configure_pin(sel, GPO_bus);
+    configure_pin(mot, GPO_bus);
 }
 
 static void rdata_prep(void)
@@ -138,6 +142,39 @@ static void dma_wdata_start(void)
                     DMA_CR_EN);
 }
 
+static void drive_deselect(void)
+{
+    write_pin(sel, FALSE);
+    unit_nr = -1;
+}
+
+static uint8_t drive_select(uint8_t nr)
+{
+    write_pin(sel, TRUE);
+    unit_nr = 0;
+    delay_us(delay_params.select_delay);
+    return ACK_OKAY;
+}
+
+static uint8_t drive_motor(uint8_t nr, bool_t on)
+{
+    if (unit[0].motor == on)
+        return ACK_OKAY;
+
+    write_pin(mot, on);
+    unit[0].motor = on;
+    if (on)
+        delay_ms(delay_params.motor_delay);
+
+    return ACK_OKAY;
+}
+
+static void reset_bus(void)
+{
+    write_pin(sel, FALSE);
+    write_pin(mot, FALSE);
+}
+
 /*
  * Local variables:
  * mode: C

src/floppy_f7.c

@@ -27,14 +27,14 @@
 /* Output pins. */
 #define gpio_densel gpiob
 #define pin_densel 12 /* PB12 */
-#define gpio_sel0  gpiob
-#define pin_sel0   11 /* PB11 */
-#define gpio_mot0  gpiob
-#define pin_mot0   1  /* PB1 */
-#define gpio_sel1  gpiob
-#define pin_sel1   0  /* PB0 */
-#define gpio_mot1  gpiob
-#define pin_mot1   10 /* PB10 */
+#define gpio_pin10 gpiob
+#define pin_pin10  1  /* PB1 */
+#define gpio_pin12 gpiob
+#define pin_pin12  0  /* PB0 */
+#define gpio_pin14 gpiob
+#define pin_pin14  11 /* PB11 */
+#define gpio_pin16 gpiob
+#define pin_pin16  10 /* PB10 */
 #define gpio_dir   gpioc
 #define pin_dir    4  /* PC4 */
 #define gpio_step  gpioa
@@ -75,6 +75,12 @@ static void floppy_mcu_init(void)
     gpio_set_af(gpio_wdata, pin_wdata, 1);
     configure_pin(rdata, AFI(PUPD_none));
 
+    /* Configure SELECT/MOTOR lines. */
+    configure_pin(pin10, GPO_bus);
+    configure_pin(pin12, GPO_bus);
+    configure_pin(pin14, GPO_bus);
+    configure_pin(pin16, GPO_bus);
+
     /* Set up EXTI mapping for INDEX: PB[3:0] -> EXT[3:0] */
     syscfg->exticr1 = 0x1111;
 }
@@ -140,6 +146,105 @@ static void dma_wdata_start(void)
     dma_wdata.cr |= DMA_CR_EN;
 }
 
+static void drive_deselect(void)
+{
+    if (unit_nr == -1)
+        return;
+
+    switch (bus_type) {
+    case BUS_IBMPC:
+        switch (unit_nr) {
+        case 0: write_pin(pin14, FALSE); break;
+        case 1: write_pin(pin12, FALSE); break;
+        }
+        break;
+    case BUS_SHUGART:
+        switch (unit_nr) {
+        case 0: write_pin(pin10, FALSE); break;
+        case 1: write_pin(pin12, FALSE); break;
+        case 2: write_pin(pin14, FALSE); break;
+        }
+        break;
+    }
+
+    unit_nr = -1;
+}
+
+static uint8_t drive_select(uint8_t nr)
+{
+    if (nr == unit_nr)
+        return ACK_OKAY;
+
+    drive_deselect();
+
+    switch (bus_type) {
+    case BUS_IBMPC:
+        switch (nr) {
+        case 0: write_pin(pin14, TRUE); break;
+        case 1: write_pin(pin12, TRUE); break;
+        default: return ACK_BAD_UNIT;
+        }
+        break;
+    case BUS_SHUGART:
+        switch (nr) {
+        case 0: write_pin(pin10, TRUE); break;
+        case 1: write_pin(pin12, TRUE); break;
+        case 2: write_pin(pin14, TRUE); break;
+        default: return ACK_BAD_UNIT;
+        }
+        break;
+    default:
+        return ACK_NO_BUS;
+    }
+
+    unit_nr = nr;
+    delay_us(delay_params.select_delay);
+
+    return ACK_OKAY;
+}
+
+static uint8_t drive_motor(uint8_t nr, bool_t on)
+{
+    switch (bus_type) {
+    case BUS_IBMPC:
+        if (nr >= 2) 
+            return ACK_BAD_UNIT;
+        if (unit[nr].motor == on)
+            return ACK_OKAY;
+        switch (nr) {
+        case 0: write_pin(pin10, on); break;
+        case 1: write_pin(pin16, on); break;
+        }
+        break;
+    case BUS_SHUGART:
+        if (nr >= 3)
+            return ACK_BAD_UNIT;
+        /* All shugart units share one motor line. Alias them all to unit 0. */
+        nr = 0;
+        if (unit[nr].motor == on)
+            return ACK_OKAY;
+        write_pin(pin16, on);
+        break;
+    default:
+        return ACK_NO_BUS;
+    }
+
+    unit[nr].motor = on;
+    if (on)
+        delay_ms(delay_params.motor_delay);
+
+    return ACK_OKAY;
+
+}
+
+static void reset_bus(void)
+{
+    write_pin(pin10, FALSE);
+    write_pin(pin12, FALSE);
+    write_pin(pin14, FALSE);
+    write_pin(pin16, FALSE);
+}
+
 /*
  * Local variables:
  * mode: C