Disable all peripheral functions globally. Enable used functions when needed.

[z180-stamp.git] / avr / z80-if.c

diff --git a/avr/z80-if.c b/avr/z80-if.c
index c0f128e1a4870005d543becf1adf5a47b1c8776b..dd0dd96414e2a91a1494987c8b6e703f2f57a954 100644 (file)
--- a/avr/z80-if.c
+++ b/avr/z80-if.c
@@ -144,6 +144,7 @@ struct bits {
 #define MASK(n)        ((1<<(n))-1)
 #define SMASK(w,s) (MASK(w) << (s))
 
+#define LOWSPEED       50000
 
 
 typedef union {
@@ -157,6 +158,13 @@ static zstate_t zstate;
 
 /*--------------------------------------------------------------------------*/
 
+static
+uint8_t is_lowspeed()
+{
+       return (TCCR1B & 7) < 2 &&
+               OCR1A > (F_CPU / 2 / LOWSPEED);
+}
+
 static
 void z80_setup_clock(void)
 {
@@ -206,12 +214,10 @@ int z80_clock_set(unsigned long freq)
        TCCR1A = (0b01 << COM1C0) | (0b00 << WGM10);
        TCCR1B = (0b01 << WGM12)  | ((prescale+1) << CS10);
 
-       if (ocrval == 0) {
-//             TCCR1C |= _BV(FOC1C);
+       if (ocrval == 0)
                ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
                        TCNT1 = 0xFFFF;
                }
-       }
 
        PINB |= _BV(6);         /* Debug */
 
@@ -229,7 +235,6 @@ uint32_t z80_clock_get(void)
                else
                        count *= 8;
                pre--;
-
        }
 
        return F_CPU/count;