diff --git a/core/timer.c b/core/timer.c index 06f2d11..892f5da 100644 --- a/core/timer.c +++ b/core/timer.c @@ -7,11 +7,11 @@ /* Compute clocks until the next decrement to zero */ static uint32_t tmrGetUntil(VB *sim) { - uint32_t full = (sim->tmr.reload + 1) * (sim->tmr.t_clk_sel ? 400 : 2000); - uint32_t cur = sim->tmr.clocks + + uint32_t fullTick = sim->tmr.t_clk_sel ? 400 : 2000; + uint32_t thisTick = sim->tmr.clocks + (sim->tmr.t_clk_sel ? 0 : 400 * (4 - sim->tmr.tick20)); - return sim->tmr.counter == 0 ? - sim->tmr.reload * full : (sim->tmr.counter - 1) * full + cur; + return thisTick + fullTick * + (sim->tmr.counter == 0 ? sim->tmr.reload : sim->tmr.counter - 1); } /* Update the counter to a new value */ @@ -142,12 +142,14 @@ static void tmrWriteControl(VB *sim, uint8_t value) { static void tmrWriteHigh(VB *sim, uint8_t value) { sim->tmr.reload = (uint16_t) value << 8 | (sim->tmr.reload & 0x00FF); tmrUpdate(sim, sim->tmr.reload); + sim->tmr.until = tmrGetUntil(sim); } /* Write to the low data register */ static void tmrWriteLow(VB *sim, uint8_t value) { sim->tmr.reload = (sim->tmr.reload & 0xFF00) | value; tmrUpdate(sim, sim->tmr.reload); + sim->tmr.until = tmrGetUntil(sim); }