SWIM3: implement head stepping.

devices/floppy/superdrive.cpp

@@ -40,6 +40,7 @@ MacSuperDrive::MacSuperDrive()
     this->eject_latch = 0; // eject latch is off
     this->drive_mode  = RecMethod::MFM; // assume MFM mode by default
     this->motor_stat  = 0; // spindle motor is off
+    this->head_pos    = 0; // current head position
     this->is_ready    = 0; // drive not ready
 }
 
@@ -53,6 +54,14 @@ void MacSuperDrive::command(uint8_t addr, uint8_t value)
     case CommandAddr::Step_Direction:
         this->step_dir = value ? -1 : 1;
         break;
+    case CommandAddr::Do_Step:
+        if (!value) {
+            this->head_pos += this->step_dir;
+            if (this->head_pos < 0)
+                this->head_pos = 0;
+            this->track_zero = this->head_pos == 0;
+        }
+        break;
     case CommandAddr::Motor_On_Off:
         new_motor_stat = value ^ 1;
         if (this->motor_stat != new_motor_stat) {

devices/floppy/superdrive.h

@@ -51,6 +51,7 @@ enum StatusAddr : uint8_t {
 /** Apple Drive command addresses. */
 enum CommandAddr : uint8_t {
     Step_Direction    = 0,
+    Do_Step           = 1,
     Motor_On_Off      = 2,
     Reset_Eject_Latch = 4,
     Switch_Drive_Mode = 5,
@@ -87,7 +88,9 @@ private:
     uint8_t motor_stat;  // spindle motor status: 1 - on, 0 - off
     uint8_t drive_mode;  // drive mode: 0 - GCR, 1 - MFM
     uint8_t is_ready;
+    uint8_t track_zero;  // 1 - if head is at track zero
     int     step_dir;    // step direction -1/+1
+    int     head_pos;    // track number the head is currently at
 
     // physical parameters of the currently inserted disk
     uint8_t media_kind;

devices/floppy/swim3.cpp

@@ -21,6 +21,7 @@ along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
 /** @file Sander-Wozniak Machine 3 (SWIM3) emulation. */
 
+#include <core/timermanager.h>
 #include <devices/floppy/superdrive.h>
 #include <devices/floppy/swim3.h>
 #include <loguru.hpp>
@@ -36,6 +37,7 @@ Swim3Ctrl::Swim3Ctrl()
     this->setup_reg = 0;
     this->mode_reg  = 0;
     this->int_reg   = 0;
+    this->int_flags = 0;
     this->int_mask  = 0;
     this->xfer_cnt  = 0;
     this->first_sec = 0xFF;
@@ -49,7 +51,7 @@ Swim3Ctrl::Swim3Ctrl()
 
 uint8_t Swim3Ctrl::read(uint8_t reg_offset)
 {
-    uint8_t status_addr;
+    uint8_t status_addr, old_int_flags;
 
     switch(reg_offset) {
     case Swim3Reg::Phase:
@@ -60,9 +62,15 @@ uint8_t Swim3Ctrl::read(uint8_t reg_offset)
         if (this->mode_reg & 2) { // internal drive?
             status_addr = ((this->mode_reg & 0x20) >> 2) | (this->phase_lines & 7);
             return ((this->int_drive->status(status_addr) & 1) << 2);
-        } else {
-            return 4;
         }
+        return 4;
+    case Swim3Reg::Interrupt_Flags:
+        old_int_flags = this->int_flags;
+        this->int_flags = 0; // read from this register clears all flags
+        update_irq();
+        return old_int_flags;
+    case Swim3Reg::Interrupt_Mask:
+        return this->int_mask;
     default:
         LOG_F(INFO, "SWIM3: reading from 0x%X register", reg_offset);
     }
@@ -71,6 +79,8 @@ uint8_t Swim3Ctrl::read(uint8_t reg_offset)
 
 void Swim3Ctrl::write(uint8_t reg_offset, uint8_t value)
 {
+    uint8_t old_mode_reg;
+
     switch(reg_offset) {
     case Swim3Reg::Param_Data:
         this->pram = value;
@@ -91,12 +101,29 @@ void Swim3Ctrl::write(uint8_t reg_offset, uint8_t value)
         break;
     case Swim3Reg::Status_Mode0:
         // ones in value clear the corresponding bits in the mode register
+        if ((this->mode_reg & value) & (SWIM3_GO | SWIM3_GO_STEP)) {
+            if (value & SWIM3_GO_STEP) {
+                stop_stepping();
+            } else {
+                stop_action();
+            }
+        }
         this->mode_reg &= ~value;
         break;
     case Swim3Reg::Handshake_Mode1:
         // ones in value set the corresponding bits in the mode register
+        if ((this->mode_reg ^ value) & (SWIM3_GO | SWIM3_GO_STEP)) {
+            if (value & SWIM3_GO_STEP) {
+                start_stepping();
+            } else {
+                start_action();
+            }
+        }
         this->mode_reg |= value;
         break;
+    case Swim3Reg::Step:
+        this->step_count = value;
+        break;
     case Swim3Reg::Interrupt_Mask:
         this->int_mask = value;
         break;
@@ -104,3 +131,70 @@ void Swim3Ctrl::write(uint8_t reg_offset, uint8_t value)
         LOG_F(INFO, "SWIM3: writing 0x%X to register 0x%X", value, reg_offset);
     }
 }
+
+void Swim3Ctrl::update_irq()
+{
+}
+
+void Swim3Ctrl::do_step()
+{
+    if (this->mode_reg & SWIM3_GO_STEP && this->step_count) { // are we still stepping?
+        // instruct the drive to perform single step in current direction
+        this->int_drive->command(MacSuperdrive::CommandAddr::Do_Step, 0);
+        if (--this->step_count == 0) {
+            if (this->step_timer_id) {
+                this->stop_stepping();
+            }
+            this->int_flags |= INT_STEP_DONE;
+            update_irq();
+        }
+    }
+}
+
+void Swim3Ctrl::start_stepping()
+{
+    if (!this->step_count) {
+        LOG_F(WARNING, "SWIM3: step_count is zero while go_step is active!");
+        return;
+    }
+
+    if ((((this->mode_reg & 0x20) >> 3) | (this->phase_lines & 3))
+        != MacSuperdrive::CommandAddr::Do_Step) {
+        LOG_F(WARNING, "SWIM3: invalid command address on the phase lines!");
+        return;
+    }
+
+    this->mode_reg |= SWIM3_GO_STEP;
+
+    // step count > 1 requires periodic task
+    if (this->step_count > 1) {
+        this->step_timer_id = TimerManager::get_instance()->add_cyclic_timer(
+            USECS_TO_NSECS(80),
+            [this]() {
+                this->do_step();
+            }
+        );
+    }
+
+    // perform the first step immediately
+    do_step();
+}
+
+void Swim3Ctrl::stop_stepping()
+{
+    // cancel stepping task
+    if (this->step_timer_id) {
+        TimerManager::get_instance()->cancel_timer(this->step_timer_id);
+    }
+    this->step_timer_id = 0;
+    this->step_count = 0; // not sure this one is required
+}
+
+void Swim3Ctrl::start_action()
+{
+    LOG_F(INFO, "SWIM3: action started!");
+}
+
+void Swim3Ctrl::stop_action()
+{
+}

devices/floppy/swim3.h

@@ -41,7 +41,7 @@ enum Swim3Reg : uint8_t {
     Setup           = 5,
     Status_Mode0    = 6,  // read: Status, write: zeroes to the mode register
     Handshake_Mode1 = 7,  // read: Handshake, write: ones to the mode register
-    Interrupt       = 8,
+    Interrupt_Flags = 8,
     Step            = 9,
     Current_Track   = 10,
     Current_Sector  = 11,
@@ -51,6 +51,17 @@ enum Swim3Reg : uint8_t {
     Interrupt_Mask  = 15
 };
 
+/** Mode register bits. */
+enum {
+    SWIM3_GO      = 0x08,
+    SWIM3_GO_STEP = 0x80,
+};
+
+/** Interrupt flags. */
+enum {
+    INT_STEP_DONE = 0x02,
+};
+
 class Swim3Ctrl {
 public:
     Swim3Ctrl();
@@ -60,6 +71,14 @@ public:
     uint8_t read(uint8_t reg_offset);
     void    write(uint8_t reg_offset, uint8_t value);
 
+protected:
+    void update_irq();
+    void start_stepping();
+    void do_step();
+    void stop_stepping();
+    void start_action();
+    void stop_action();
+
 private:
     std::unique_ptr<MacSuperdrive::MacSuperDrive> int_drive;
 
@@ -67,10 +86,14 @@ private:
     uint8_t mode_reg;
     uint8_t phase_lines;
     uint8_t int_reg;
+    uint8_t int_flags;  // interrupt flags
     uint8_t int_mask;
     uint8_t pram;       // parameter RAM: two nibbles = {late_time, early_time}
+    uint8_t step_count;
     uint8_t first_sec;
     uint8_t xfer_cnt;
+
+    int     step_timer_id = 0;
 };
 
 }; // namespace Swim3