Add command to set PIO mode

ata.c

@@ -231,12 +231,16 @@ void ata_set_xfer(struct IDEUnit *unit, enum xfer method) {
             unit->read_unaligned  = &ata_read_unaligned_long;
             unit->write_fast      = &ata_write_long_movem;
             unit->write_unaligned = &ata_write_unaligned_long;
+
+            unit->xferMethod = longword_movem;
             break;
         case longword_move:
             unit->read_fast       = &ata_read_long_move;
             unit->read_unaligned  = &ata_read_unaligned_long;
             unit->write_fast      = &ata_write_long_move;
             unit->write_unaligned = &ata_write_unaligned_long;
+
+            unit->xferMethod = longword_move;
             break;
     }
 }
@@ -377,7 +381,7 @@ ident_failed:
     unit->present = true;
 
     Info("INIT: LBAs %ld Blocksize: %ld\n",unit->logicalSectors,unit->blockSize);
-
+    
     if (buf) FreeMem(buf,512);
     return true;
 }
@@ -711,3 +715,34 @@ static BYTE write_taskfile_lba48(struct IDEUnit *unit, UBYTE command, ULONG lba,
 
     return 0;
 }
+
+
+/**
+ * ata_set_pio
+ * 
+ * @param unit Pointer t oan IDEUnit struct
+ * @param pio pio mode
+*/
+BYTE ata_set_pio(struct IDEUnit *unit, UBYTE pio) {
+    
+    if (pio > 4) return IOERR_BADADDRESS;
+    
+    if (pio > 0) pio |= 0x80;
+
+    UBYTE drvSel = (unit->primary) ? 0xE0 : 0xF0;
+
+    ata_select(unit,drvSel,true);
+
+    if (!ata_wait_ready(unit,ATA_RDY_WAIT_COUNT)) {
+        ata_save_error(unit);
+        return HFERR_SelTimeout;
+    }
+
+    *unit->drive->error_features = 0x03; // Set Transfer Mode
+    *unit->drive->sectorCount    = pio;
+    *unit->drive->status_command = ATA_CMD_SET_FEATURES;
+
+    if (ata_check_error(unit)) return IOERR_BADLENGTH;
+
+    return 0;
+}

ata.h

@@ -50,6 +50,7 @@
 #define ATA_CMD_WRITE_MULTIPLE     0xC5
 #define ATA_CMD_WRITE_MULTIPLE_EXT 0x39
 #define ATA_CMD_SET_MULTIPLE       0xC6
+#define ATA_CMD_SET_FEATURES       0xEF
 
 // Identify data word offsets
 #define ata_identify_cylinders       1
@@ -96,6 +97,7 @@ void ata_set_xfer(struct IDEUnit *unit, enum xfer method);
 
 BYTE ata_read(void *buffer, ULONG lba, ULONG count, ULONG *actual, struct IDEUnit *unit);
 BYTE ata_write(void *buffer, ULONG lba, ULONG count, ULONG *actual, struct IDEUnit *unit);
+BYTE ata_set_pio(struct IDEUnit *unit, UBYTE pio);
 
 void ata_read_unaligned_long(void *source, void *destination);
 void ata_write_unaligned_long(void *source, void *destination);

driver.c

@@ -794,6 +794,7 @@ static void __attribute__((used, saveds)) begin_io(struct DeviceBase *dev asm("a
             case NSCMD_ETD_WRITE64:
             case NSCMD_ETD_FORMAT64:
             case CMD_XFER:
+            case CMD_PIO:
             case HD_SCSICMD:
                 // Send all of these to ide_task
                 ioreq->io_Flags &= ~IOF_QUICK;

idetask.c

@@ -710,6 +710,14 @@ transfer:
                     }
                     break;
 
+                case CMD_PIO:
+                    if (ioreq->io_Length <= 4) {
+                        error = ata_set_pio(unit,ioreq->io_Length);
+                    } else {
+                        error = IOERR_BADADDRESS;
+                    }
+                    break;
+
                 /* CMD_DIE: Shut down this task and clean up */
                 case CMD_DIE:
                     Info("Task: CMD_DIE: Shutting down IDE Task\n");

idetask.h

@@ -11,6 +11,7 @@
 
 #define CMD_DIE  0x1000
 #define CMD_XFER (CMD_DIE + 1)
+#define CMD_PIO  (CMD_XFER + 1)
 
 void ide_task();
 void diskchange_task();