/* Test if main oscillator is enabled */
   ldr    r0,=AT91C_PMC_SR
   ldr    r1, [r0]
   ldr    r2,=AT91C_PMC_MOSCS
   ands    r1, r1, r2
   bne     _switch_to_mosc


/* Enable the main oscillator */
_enable_mosc:
   ldr    r0,=AT91C_PMC_MOR
   mov    r1, #(0x40 << 8)
   ldr   r2,=AT91C_CKGR_MOSCEN
   orr   r1, r1, r2
   str   r1, [r0]
   ldr    r0,=AT91C_PMC_SR
1:
   ldr    r1, [r0]
   ldr    r2,=AT91C_PMC_MOSCS
   ands    r1, r1, r2
   beq   1b
   
/* Test if MCK == SLOW CLOCK */
_switch_to_mosc:
   ldr    r0,=AT91C_PMC_MCKR
   ldr    r1,=AT91C_PMC_CSS
   ldr    r2, [r0]
   and    r2, r2, r1
   mov   r1, #0
   cmp   r1, r2
   bne   _init_bss
   ldr    r1,=AT91C_PMC_CSS_MAIN_CLK
   ldr    r2,=AT91C_PMC_PRES_CLK
   orr   r1, r1, r2
   str    r1, [r0]
   ldr    r0,=AT91C_PMC_SR
1:
   ldr     r1, [r0]
   ldr   r2,=AT91C_PMC_MCKRDY
   ands    r1, r1, r2
   beq     1b

       /* Check whether the main oscillator is working */
       ldr   r0, = PMC_SR
       ldr   r1, [r0]
       ands  r1, r1, #PMC_MOSCS
       bne   _switch_to_mosc

       /* Enabling the main oscillator */
       ldr   r3, = PMC_MOR
       ldr   r4, = (PMC_MOR_MOSCEN + (64 << 8 ))
       str   r4, [r3]

       /* Waiting for the main clock to stabilize */
1:     ldr   r1, [r0]
       ands  r1, r1, #PMC_MOSCS
       beq   1b

_switch_to_mosc:

       /* Check whether the main oscillator is used */
       ldr   r3, = PMC_MCKR
       ldr   r4, [r3]
       ands  r4, r4, #PMC_CSS
       bne   _init_bss

       /* Switching to the main oscillator */
       mov   r4, #(PMC_CSS_MAIN + PMC_PRES_1 + PMC_MDIV_1)
       str   r4, [r3]

       /* Waiting for the main oscillator to be ready */
1:     ldr   r1, [r0]
       ands  r1, r1, #PMC_MCKRDY
       beq   1b