diff --git a/devices/floppy/swim3.cpp b/devices/floppy/swim3.cpp index cdf9cd9..39c38c6 100644 --- a/devices/floppy/swim3.cpp +++ b/devices/floppy/swim3.cpp @@ -80,6 +80,8 @@ uint8_t Swim3Ctrl::read(uint8_t reg_offset) uint8_t status_addr, rddata_val, old_int_flags, old_error; switch(reg_offset) { + case Swim3Reg::Timer: + return this->calc_timer_val(); case Swim3Reg::Error: old_error = this->error; this->error = 0; @@ -127,7 +129,7 @@ void Swim3Ctrl::write(uint8_t reg_offset, uint8_t value) switch(reg_offset) { case Swim3Reg::Timer: - LOG_F(INFO, "SWIM3: writing %d to the Timer register", value); + this->init_timer(value); break; case Swim3Reg::Param_Data: this->pram = value; @@ -357,6 +359,44 @@ void Swim3Ctrl::stop_disk_access() this->access_timer_id = 0; } +void Swim3Ctrl::init_timer(const uint8_t start_val) +{ + if (this->timer_val) { + LOG_F(WARNING, "SWIM3: attempt to re-arm the timer"); + } + this->timer_val = start_val; + if (!this->timer_val) { + this->one_us_timer_start = 0; + return; + } + + this->one_us_timer_start = TimerManager::get_instance()->current_time_ns(); + + this->one_us_timer_id = TimerManager::get_instance()->add_oneshot_timer( + this->timer_val * NS_PER_USEC, + [this]() { + this->timer_val = 0; + this->int_flags |= INT_TIMER_DONE; + update_irq(); + } + ); +} + +uint8_t Swim3Ctrl::calc_timer_val() +{ + if (!this->timer_val) { + return 0; + } + + uint64_t time_now = TimerManager::get_instance()->current_time_ns(); + uint64_t us_elapsed = (time_now - this->one_us_timer_start) / NS_PER_USEC; + if (us_elapsed > this->timer_val) { + return 0; + } else { + return (this->timer_val - us_elapsed) & 0xFFU; + } +} + // floppy disk formats properties for the cases // where disk format needs to be specified manually static const std::vector FloppyFormats = { diff --git a/devices/floppy/swim3.h b/devices/floppy/swim3.h index ed235a4..156dfb6 100644 --- a/devices/floppy/swim3.h +++ b/devices/floppy/swim3.h @@ -64,9 +64,10 @@ enum { /** Interrupt flags. */ enum { - INT_STEP_DONE = 0x02, - INT_ID_READ = 0x04, - INT_SECT_DONE = 0x08, + INT_TIMER_DONE = 0x01, + INT_STEP_DONE = 0x02, + INT_ID_READ = 0x04, + INT_SECT_DONE = 0x08, }; // SWIM3 internal states. @@ -96,19 +97,22 @@ public: }; protected: - void update_irq(); - void start_stepping(); - void do_step(); - void stop_stepping(); - void start_disk_access(); - void disk_access(); - void stop_disk_access(); + void update_irq(); + void start_stepping(); + void do_step(); + void stop_stepping(); + void start_disk_access(); + void disk_access(); + void stop_disk_access(); + void init_timer(const uint8_t start_val); + uint8_t calc_timer_val(); private: std::unique_ptr int_drive; DmaBidirChannel* dma_ch; + uint8_t timer_val = 0; // internal timer that decrements at a 1 us rate uint8_t setup_reg; uint8_t mode_reg; uint8_t error; @@ -128,9 +132,12 @@ private: uint8_t rd_line; int cur_state; + int one_us_timer_id = 0; int step_timer_id = 0; int access_timer_id = 0; + uint64_t one_us_timer_start = 0; + // Interrupt related stuff InterruptCtrl* int_ctrl = nullptr; uint32_t irq_id = 0;