CPUClk

; Copyright 2011 Castle Technology Ltd
;
; Licensed under the Apache License, Version 2.0 (the "License");
; you may not use this file except in compliance with the License.
; You may obtain a copy of the License at
;
;     http://www.apache.org/licenses/LICENSE-2.0
;
; Unless required by applicable law or agreed to in writing, software
; distributed under the License is distributed on an "AS IS" BASIS,
; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
; See the License for the specific language governing permissions and
; limitations under the License.
;
        GET     Hdr:ListOpts
        GET     Hdr:Macros
        GET     Hdr:System
        GET     Hdr:Machine.<Machine>
        GET     Hdr:ImageSize.<ImageSize>
        $GetIO

        GET     Hdr:OSEntries
        GET     Hdr:HALEntries
        GET     Hdr:Proc

        GET     hdr.omap3530
        GET     hdr.StaticWS
        GET     hdr.PRCM

        AREA    |Asm$$Code|, CODE, READONLY, PIC

        EXPORT  CPUClk_Init

        IMPORT  TPSRead
        IMPORT  TPSWrite
        IMPORT  memcpy
        IMPORT  __rt_udiv

        MACRO
        CallOS  $entry
        ASSERT  $entry <= HighestOSEntry
        MOV     lr, pc
        LDR     pc, OSentries + 4*$entry
        MEND

        MACRO
        myCPSIDi
        DCI     &F10C0080 ; CPSID i
        MEND

        MACRO
        myUBFX  $rd,$rn,$lsb,$width
        DCI     &E7E00050+($rd<<12)+($rn)+($lsb<<7)+(($width-1)<<16)
        MEND

        MACRO
        myBFI   $rd,$rn,$lsb,$width
        DCI     &E7C00010+($rd<<12)+($rn)+($lsb<<7)+(($lsb+$width-1)<<16)
        MEND

        GBLL    DebugCPUClk
DebugCPUClk SETL {FALSE}

 [ DebugCPUClk
        IMPORT  DebugHALPrint
        IMPORT  DebugHALPrintReg
        IMPORT  DebugHALPrintByte
 ]


CPUClk_Init
        Entry   "v1-v4"
        ; First identify the OMAP type
        LDR     a1, L4_Wakeup_Log
        LDR     a2, =L4_CONTROL_IDCODE-L4_Wakeup
        LDR     a1, [a2, a1]!
        myUBFX  a1, a1, 12, 16
        ADR     a3, CPUList
10
        LDMIA   a3!,{a4,v1,v2}
        CMP     a4,#0
        EXIT    EQ ; Unrecognised CPU!
        CMP     a1, a4
        BNE     %BT10
        ; Extra logic for detecting 720MHz OMAP3's
        MOV     v4, #0
        LDR     a3, =HAWKEYE_OMAP35x_ES10
        CMP     a3, a1
        LDRNE   a3, =HAWKEYE_OMAP35x
        CMPNE   a3, a1
        LDREQ   a2, [a2, #L4_PRODID-L4_CONTROL_IDCODE]
        ANDEQ   v4, a2, #&F ; 0x0 for 600MHz, 0x8 for 720MHz
 [ DebugCPUClk
        DebugReg a4, "CPU="
        DebugReg v1, "OPPTbl size="
        DebugReg v2, "OPPTbl ptr="
        DebugReg v4, "720MHz="
 ]
        ; v1 = OPPTbl size
        ; v2 = OPPTbl ptr
        ; Init the HAL device descriptor
        ADRL    v3, CPUClkWS
        MOV     a1, v3
        ADR     a2, CPUClkDevTemplate
        MOV     a3, #HALDevice_CPUClk_Size
        BL      memcpy
        STR     sb, [v3, #:INDEX: CPUClkWorkspace]
        STR     v1, [v3, #:INDEX: CPUClkOPPTblSize]
        ADD     a1, v3, #:INDEX: CPUClkOPPTbl
        MOV     a2, v2
        ASSERT  OPPTbl_Size = 4
        MOV     a3, v1, LSL #2
        BL      memcpy
        ; Poke the last entry in the table if this was a 720MHz model
        CMP     v4, #8
        MOVEQ   v4, #720
        STREQH  v4, [v3, #:INDEX: CPUClkOPPTbl + End_OPPTbl_OMAP35x-OPPTbl_OMAP35x - OPPTbl_Size + OPPTbl_MHz]
        ; Make sure VDD1_VMODE_CFG is set correctly
        ; We want READ_REG set, ENABLE_VMODE clear
        SUB     sp, sp, #4
        MOV     a1, #TPSPM_IIC*2
        MOV     a2, sp
        MOV     a3, #1
        MOV     a4, #VDD1_VMODE_CFG
        LDR     v1, OSentries+4*OS_IICOpV
        BL      TPSRead
        CMP     a1, #0
        BNE     %FT10
        LDRB    a1, [a2]
 [ DebugCPUClk
        DebugReg a1, "VDD1_VMODE_CFG="
 ]
        BIC     a1, a1, #1
        ORR     a1, a1, #2
        STRB    a1, [a2]
        MOV     a1, #TPSPM_IIC*2
        BL      TPSWrite
        CMP     a1, #0
10
        ADD     sp, sp, #4
        EXIT    NE
        ; TODO - Extra setup steps:
        ; - Ensure DSP is off
        ; - Ensure SmartReflex is off
        ; - Ensure VMODE is off (CPU side)
        ; Go to max speed
        MOV     a1, v3
        MVN     a2, #0
        STR     a2, CPUClkNewSpeed
        LDR     a2, CPUClkOPPTblSize
        BL      CPUClk_Set
        ; Register device with OS
        MOV     a1, #0
        MOV     a2, v3
        CallOS  OS_AddDevice
 [ DebugCPUClk
        DebugTX "CPUClk_Init done"
 ]
        EXIT

        MACRO
        OPPTblEntry $mhz,$vdd1,$clkout_m2
        ; VDD1 specified in mV
        DCW $mhz
        DCB ((($vdd1-600)*10)+124)/125 ; VDD1_VSEL = ((V-0.6)+0.0124)/0.0125
        DCB $clkout_m2
        MEND

OPPTbl_OMAP35x
        OPPTblEntry  125, 0985, 1 ; OPP1
        OPPTblEntry  250, 1060, 1 ; OPP2
        OPPTblEntry  500, 1200, 1 ; OPP3
        OPPTblEntry  550, 1270, 1 ; OPP4
        OPPTblEntry  600, 1350, 1 ; OPP5
End_OPPTbl_OMAP35x

OPPTbl_AMDM37x
        OPPTblEntry  300, 0970, 2 ; OPP60
        OPPTblEntry  600, 1140, 1 ; OPP100
        OPPTblEntry  800, 1270, 1 ; OPP130
End_OPPTbl_AMDM37x

        MACRO
        CPUListEntry $hawkeye, $list
        DCD $hawkeye
        ASSERT (End_$list-$list)/OPPTbl_Size <= OPPTbl_Max
        DCD (End_$list-$list)/OPPTbl_Size
        DCD $list
        MEND

CPUList
        CPUListEntry HAWKEYE_OMAP35x_ES10, OPPTbl_OMAP35x
        CPUListEntry HAWKEYE_OMAP35x,      OPPTbl_OMAP35x
        CPUListEntry HAWKEYE_AMDM37x,      OPPTbl_AMDM37x
        DCD 0

CPUClkDevTemplate
        DCW     HALDeviceType_SysPeri + HALDeviceSysPeri_CPUClk
        DCW     HALDeviceID_CPUClk_OMAP3
        DCD     HALDeviceBus_Interconnect + HALDeviceInterconnectBus_L4
        DCD     0                     ; API version
        DCD     CPUClk_Desc           ; Description
        DCD     0                     ; Address - unused
        %       12                    ; Unused
        DCD     CPUClk_Activate
        DCD     CPUClk_Deactivate
        DCD     CPUClk_Reset
        DCD     CPUClk_Sleep
        DCD     -1                    ; Device - unused
        DCD     0                     ; TestIRQ
        DCD     0                     ; ClearIRQ
        %       4
        ASSERT  (.-CPUClkDevTemplate) = HALDeviceSize
        DCD     CPUClk_NumSpeeds
        DCD     CPUClk_Info
        DCD     CPUClk_Get
        DCD     CPUClk_Set
        DCD     CPUClk_Override
        ASSERT  (.-CPUClkDevTemplate) = HALDevice_CPUClk_Size

CPUClk_Desc
        =       "OMAP3 CPU clock generator",0
        ALIGN

CPUClk_Activate
        ; Do nothing
        MOV     a1, #1
CPUClk_Deactivate
CPUClk_Reset
        MOV     pc, lr

CPUClk_Sleep
        MOV     a1, #0
        MOV     pc, lr

CPUClk_NumSpeeds
        ; Out: a1 = num entries in table
        LDR     a1, CPUClkOPPTblSize
        MOV     pc, lr

CPUClk_Info
        ; In: a2 = table index
        ; Out: a1 = MHz
        ADR     a3, CPUClkOPPTbl
        ASSERT  OPPTbl_Size = 4
        ADD     a3, a3, a2, LSL #2
        LDRH    a1, [a3]
        MOV     pc, lr

CPUClk_Get
        ; Return current table index
        EntryS  "sb"
        myCPSIDi ; Prevent speed changing while we're reading it
        LDR     a2, CPUClkNewSpeed
        LDR     sb, CPUClkWorkspace
        CMP     a2, #-1 ; Are we changing speed?
        BLNE    CPUClk_Set ; Yes, complete the change so that the returned value is accurate
        ADRL    a2, CPUClkWS
        LDR     a1, CPUClkCurSpeed
        EXITS

CPUClk_Set
        ; a2 = new table index
        ; Return 0 on success, -1 on failure
        EntryS  "v1-v5,sb", 4
        ; Turn IRQs off so that we only have to worry about being re-entered during IIC ops
        ; Keeping IRQs off will also help reduce the time spent with the DPLL unlocked
        myCPSIDi
        ; Clamp a2, get table entry
        LDR     sb, CPUClkWorkspace
        CMP     a2, #0
        LDR     v3, CPUClkOPPTblSize
        MOVLT   a2, #0
        ADR     v2, CPUClkOPPTbl
        CMP     a2, v3
        SUBGE   a2, v3, #1
        ASSERT  OPPTbl_Size = 4
        ADD     v2, v2, a2, LSL #2
        LDRH    v3, [v2, #OPPTbl_MHz]
        ; a2 = new idx
        ; v2 = OPP table entry ptr
        ; v3 = new clock rate
 [ DebugCPUClk
        DebugReg a2,"CPUClk_Set: Idx="
        DebugReg v2,"OPPTbl ptr="
        DebugReg v3,"New rate="
        LDR     a3, CPUClkNewSpeed
        DebugReg a3,"Re-entrancy flag="
 ]
        ; Set the re-entrancy flag
        STR     a2, CPUClkNewSpeed
        ; Get current VDD1
        MOV     a1, #TPSPM_IIC*2
        MOV     a2, sp
        MOV     a3, #1
        MOV     a4, #VDD1_VSEL
        LDR     v1, OSentries+4*OS_IICOpV
        BL      TPSRead
        CMP     a1, #0
        BNE     %FT80
        ; Check if this change has been completed/cancelled by us being re-entered
        ADRL    a1, CPUClkWS
        LDR     a1, CPUClkNewSpeed
        CMP     a1, #-1
        EXITS   EQ,c
        LDRB    a1, [a2]
        LDRB    ip, [v2, #OPPTbl_VDD1]
 [ DebugCPUClk
        DebugReg a1,"VDD1 currently "
        DebugReg ip,"VDD1 needed "
 ]
        CMP     a1, ip
        BHS     %FT30
        ; Pre-increment VDD1
 [ DebugCPUClk
        DebugReg ip,"Increasing VDD1 to "
 ]
        STRB    ip, [a2]
        MOV     a1, #TPSPM_IIC*2
        BL      TPSWrite
        CMP     a1, #0
        BNE     %FT80
        ; Check if this change has been completed/cancelled by us being re-entered
        ADRL    a1, CPUClkWS
        LDR     a1, CPUClkNewSpeed
        CMP     a1, #-1
        EXITS   EQ,c
30
        ; Adjust DPLL registers
        LDRB    a1, [v2, #OPPTbl_CLKOUT_M2]
        MUL     v3, a1, v3
        LDR     ip, L4_ClockMan_Log
        LDR     lr, [ip, #CM_CLKSEL1_PLL_MPU]
        MOV     v4, lr
        myBFI   v4, v3, 8, 11 ; Update MPU_DPLL_MULT
        LDR     v5, [ip, #CM_CLKSEL2_PLL_MPU]
        LDR     v3, [ip, #CM_CLKEN_PLL_MPU]
        ; v4 = new CM_CLKSEL1_PLL_MPU
        ; a1 = new CM_CLKSEL2_PLL_MPU
        ; lr = old CM_CLKSEL1_PLL_MPU
        ; v5 = old CM_CLKSEL2_PLL_MPU
 [ DebugCPUClk
        Push    "lr"
        DebugReg v4, "New CM_CLKSEL1_PLL_MPU="
        DebugReg a1, "New CM_CLKSEL2_PLL_MPU="
        LDR     lr,[sp]
        DebugReg lr, "Old CM_CLKSEL1_PLL_MPU="
        DebugReg v5, "Old CM_CLKSEL2_PLL_MPU="
        Pull    "lr"
 ]
        CMP     v4, lr
        CMPEQ   a1, v5
        ANDEQ   lr, v3, #7
        CMPEQ   lr, #7
        BEQ     %FT60 ; No parameters need updating, skip the update
 [ DebugCPUClk
        DebugTX "Going to bypass mode"
 ]
        ; Go to bypass mode
        MOV     lr, #5
        myBFI   v3, lr, 0, 3
        STR     v3, [ip, #CM_CLKEN_PLL_MPU]
        ; Wait for completion
40
        LDR     lr, [ip, #CM_IDLEST_PLL_MPU]
        TST     lr, #1
        BNE     %BT40
 [ DebugCPUClk
        DebugTX "Setting new clock rate"
 ]
        ; Set new frequency
        STR     v4, [ip, #CM_CLKSEL1_PLL_MPU]
        STR     a1, [ip, #CM_CLKSEL2_PLL_MPU]
 [ DebugCPUClk
        DebugTX "Requesting lock"
 ]
        ; Request lock
        ORR     v3, v3, #7
        STR     v3, [ip, #CM_CLKEN_PLL_MPU]
        ; Wait for completion
50
        LDR     lr, [ip, #CM_IDLEST_PLL_MPU]
        TST     lr, #1
        BEQ     %BT50
 [ DebugCPUClk
        DebugTX "Lock complete"
 ]
60
        ; Now check if we need to reduce VDD1
        ; a2-v1 still valid from earlier
        LDRB    a1, [a2]
        LDRB    ip, [v2, #OPPTbl_VDD1]
        CMP     a1, ip
        BEQ     %FT70
 [ DebugCPUClk
        DebugReg ip,"Decreasing VDD1 to "
 ]
        STRB    ip, [a2]
        MOV     a1, #TPSPM_IIC*2
        BL      TPSWrite
        CMP     a1, #0
        BNE     %FT80
70
 [ DebugCPUClk
        DebugTX "CPUClk_Set done"
 ]
        ; All done
        ; However it's still possible we were re-entered; only set CPUClkSpeed if CPUClkNewSpeed != -1
        ADRL    a1, CPUClkWS
        LDR     a2, CPUClkNewSpeed
        CMP     a2, #-1
        STRNE   a2, CPUClkCurSpeed
        MVN     a3, #0
        STR     a3, CPUClkNewSpeed
        MOV     a1, #0
        EXITS   ,c
80
        ; Something bad happened!
 [ DebugCPUClk
        DebugReg a1,"IIC error! "
 ]
        ; Best we can really do is give up
        ADRL    a2, CPUClkWS
        MVN     a1, #0
        STR     a1, [a2, #:INDEX:CPUClkNewSpeed]
        EXITS   ,c


CPUClk_Override
        EntryS
        myCPSIDi
        ; a2 = table ptr
        ; a3 = num entries
        ; a4 = format no.
        CMP     a4, #OPPTbl_Format
        BNE     %FT20
        ; Check we aren't in the middle of setting the speed
        LDR     ip, CPUClkNewSpeed
        CMP     ip, #-1
        MVNNE   a1, #0 ; -1 for try again later
        EXITS   NE,c
        ; Update table
        ASSERT  OPPTbl_Size = 4
        STR     a3, CPUClkOPPTblSize
        ADR     ip, CPUClkOPPTbl
10
        LDR     a4, [a2], #4
        SUBS    a3, a3, #1
        STR     a4, [ip], #4
        BNE     %BT10
20
        MOV     a1, #OPPTbl_Format ; Return expected table format
        EXITS   ,c

        END