;;  Copyright ARM Ltd 2002. All rights reserved.
;;
;;  This code initializes the Integrator Core Module Core Clock
;;  for the following cores:
;;
;;  ARM966E-S
;;  ARM946E-S
;;  ARM926EJ-S
;;
;;  Core Module bus clock HCLK is derived by dividing the Core Clock (CLK) by HCLKDIV
;;  The Power On Default for HCLKDIV is 3


        AREA   CMclocks, CODE, READONLY     ; name this block of code

cm_base EQU    0x10000000
cm_osc  EQU    0x10000008
cm_lock EQU    0x10000014

        EXPORT clock_speed
clock_speed
;;
;; Enters with core clock value in MHz in LSB of R0
;;
        SUB     r0,r0,#8                    ; correction for MHz
        AND     r0,r0,#0xFF                 ; ensure byte value
;
        LDR     r2,=cm_base                 ; read CM base value
        LDR     r1,[r2,#8]                  ; read CM_OSC
;
        BIC     r1,r1,#0x0FF                ; clear bottom byte r1
        ORR     r1,r1,r0                    ; write in new clock values
;
        MOV     r0,#0xA000
        ORR     r0,r0, #0x5F                ; build 0xA05F in r0
        STR     r0,[r2,#0x14]               ; write to unlock CM_LOCK
        STR     r1,[r2,#8]                  ; write value back
        STR     r1,[r2,#0x14]               ; write in any value to relock CM_LOCK

        MOV     pc,lr                       ; return

        END