diff -Naru orig/driver/at91_slowclk.c new/driver/at91_slowclk.c --- orig/driver/at91_slowclk.c 2019-03-18 16:56:37.362723190 -0500 +++ new/driver/at91_slowclk.c 2019-03-18 17:06:16.138706056 -0500 @@ -35,12 +35,15 @@ #if !defined(SAMA5D4) && !defined(SAMA5D2) unsigned int reg; - /* - * Enable the 32768 Hz oscillator by setting the bit OSC32EN to 1 - */ reg = readl(AT91C_BASE_SCKCR); - reg |= AT91C_SLCKSEL_OSC32EN; - writel(reg, AT91C_BASE_SCKCR); + /* Only enable 32768 Hz oscillator if needed */ + if ( !(reg & AT91C_SLCKSEL_OSC32EN) ) { + /* + * Enable the 32768 Hz oscillator by setting the bit OSC32EN to 1 + */ + reg |= AT91C_SLCKSEL_OSC32EN; + writel(reg, AT91C_BASE_SCKCR); + } #endif /* #if !defined(SAMA5D4) && !defined(SAMA5D2) */ /* start a internal timer */ @@ -67,12 +70,15 @@ { unsigned int reg; - /* - * Disable the 32kHz RC oscillator by setting the bit RCEN to 0 - */ + /* Only disable internal RC oscillator if needed */ reg = readl(AT91C_BASE_SCKCR); - reg &= ~AT91C_SLCKSEL_RCEN; - writel(reg, AT91C_BASE_SCKCR); + if (reg | AT91C_SLCKSEL_RCEN) { + /* + * Disable the 32kHz RC oscillator by setting the bit RCEN to 0 + */ + reg &= ~AT91C_SLCKSEL_RCEN; + writel(reg, AT91C_BASE_SCKCR); + } } #endif /* #if !defined(SAMA5D4) && !defined(SAMA5D2) */ @@ -90,22 +96,32 @@ if (reg & AT91C_SLCKSEL_OSCSEL) return 0; - reg |= AT91C_SLCKSEL_OSCSEL; - writel(reg, AT91C_BASE_SCKCR); - - /* - * Waiting 5 slow clock cycles for internal resynchronization - * 5 slow clock cycles = ~153 us (5 / 32768) - */ - udelay(153); - + if ( !(reg & AT91C_SLCKSEL_OSCSEL) ) { + dbg_printf("Switching slow clock to external oscillator...\n"); + /* + * Wait 32768 Hz Startup Time for clock stabilization (software loop) + * wait about ~1s (1300ms) + */ + slowclk_wait_osc32_stable(); + + /* + * Switching from internal 32kHz RC oscillator to 32768 Hz oscillator + * by setting the bit OSCSEL to 1 + */ + reg |= AT91C_SLCKSEL_OSCSEL; + writel(reg, AT91C_BASE_SCKCR); + + /* + * Waiting 5 slow clock cycles for internal resynchronization + * 5 slow clock cycles = ~153 us (5 / 32768) + */ + udelay(153); + } return 0; } int slowclk_switch_osc32(void) { - slowclk_wait_osc32_stable(); - slowclk_select_osc32(); #if !defined(SAMA5D4) && !defined(SAMA5D2)