diff options
Diffstat (limited to 'avr/z180-serv.c')
| -rw-r--r-- | avr/z180-serv.c | 44 |
1 files changed, 42 insertions, 2 deletions
diff --git a/avr/z180-serv.c b/avr/z180-serv.c index ec3db7d..6973908 100644 --- a/avr/z180-serv.c +++ b/avr/z180-serv.c @@ -106,6 +106,45 @@ void do_msg_get_timer(uint8_t subf, int len, uint8_t * msg) /* ---------------------------------------------------------------------------*/ +/* measure f_cpu */ +void do_msg_m_fcpu(uint8_t subf, int len UNUSED, uint8_t * msg) +{ + uint32_t freq; + uint8_t eimsk_save; + + /* Save state and disable INT5/INT6 */ + ATOMIC_BLOCK(ATOMIC_FORCEON) { + eimsk_save = EIMSK; + EIMSK &= ~_BV(INT6); + EIMSK &= ~_BV(INT5); + } + EIFR = _BV(INTF5); /* Reset pending int */ + z80_bus_cmd(Request); + z80_write(0x3f, 0xff); + z80_bus_cmd(Release); + + freq = z80_measure_phi(*msg); + + z80_bus_cmd(Request); + z80_write(0x3f, 0xff); + z80_bus_cmd(Release); + + /* Restore INT5/INT6 */ + ATOMIC_BLOCK(ATOMIC_FORCEON) { + if ((eimsk_save & _BV(INT5)) != 0) + EIMSK |= _BV(INT5); + if ((eimsk_save & _BV(INT6)) != 0) + EIMSK |= _BV(INT6); + /* Reset pending int */ + EIFR = _BV(INTF5); + EIFR = _BV(INTF6); + } + + msg_xmit(4, subf, sizeof(freq), (uint8_t *) &freq); +} + +/* ---------------------------------------------------------------------------*/ + #define CPM_DAY_OFFSET ((1978-1900) * 365 + 19) /* 19 leap years */ /* @@ -622,6 +661,9 @@ const FLASH struct msg_item z80_messages[] = { 3, 2, 3, /* 2: get, 3: set time and date */ do_msg_get_set_time}, + { 4, + 1, 1, + do_msg_m_fcpu}, { 0xff, /* end mark */ 0, 0, 0}, @@ -629,8 +671,6 @@ const FLASH struct msg_item z80_messages[] = }; - - void do_message(int len, uint8_t *msg) { uint8_t fct, sub_fct; |