;/*****************************************************************************
; * Copyright (C) 2022-2024, Xiaohua Semiconductor Co., Ltd. All rights reserved.
; *
; * This software component is licensed by XHSC under BSD 3-Clause license
; * (the "License"); You may not use this file except in compliance with the
; * License. You may obtain a copy of the License at:
; *                    opensource.org/licenses/BSD-3-Clause
; *
; */
;/****************************************************************************/
;/*  Test for IAR                                                            */
;/*  Date             Author          Notes                                  */
;/*  2022-09-14       CDT             First version                          */
;/*  2022-12-20       CDT             Load data to R0 before USAT            */
;/*  2023-05-31       CDT             Typo: Veriry -> Verify                 */
;/****************************************************************************/

            SECTION constdata:CONST(2)
data0xAAAAAAAA       DCD     0xAAAAAAAA
data0x55555555       DCD     0x55555555
data0x80000000       DCD     0x80000000
data0xAAAAAAA8       DCD     0xAAAAAAA8
data0x55555554       DCD     0x55555554
data0x00000000       DCD     0x00000000
data0x00000001       DCD     0x00000001
data0x50000000       DCD     0x50000000
data0xA8000000       DCD     0xA8000000
data0x00000050       DCD     0x00000050
data0x000000A0       DCD     0x000000A0
data0xFFFFFFFF       DCD     0xFFFFFFFF

            ; Exported function
            EXPORT STL_CpuTestStartup

;*******************************************************************************
; Function Name  : STL_CpuTestStartup
; Description    : Test CPU at start-up
; Input          : None.
; Output         : Perform routine when detect failure at set of self test cases
; Return         : STL_OK (=0):test pass; STL_ERR (=1):test fail;
; WARNING        : all registers destroyed when exiting this function (including
;                  preserved registers R4 to R11) and excluding stack point R13)
;*******************************************************************************/
            THUMB
            SECTION .text:CODE(2)
STL_CpuTestStartup:
            PUSH {R4-R7}                         ; Save registers

_test_cpu_reg0_reg8
            MOVS R0, #0x00
            UXTB R0, R0
            ADDS R0, #0                          ; Set Z(ero) Flag
            BNE _test_cpu_reg0_reg13_fail        ; Fails if Z clear
            BMI _test_cpu_reg0_reg13_fail        ; Fails if N is set
            SUBS R0, #1                          ; Set N(egative) Flag
            BPL _test_cpu_reg0_reg13_fail        ; Fails if N clear
            ADDS R0, #2                          ; Set C(arry) Flag and do not set Z
            BCC _test_cpu_reg0_reg13_fail        ; Fails if C clear
            BEQ _test_cpu_reg0_reg13_fail        ; Fails if Z is set
            BMI _test_cpu_reg0_reg13_fail        ; Fails if N is set

            LDR R0, =data0x80000000              ; Prepares Overflow test
            LDR R0, [R0]
            ADDS R0, R0, R0                      ; Set V(overflow) Flag
            BVC _test_cpu_reg0_reg13_fail        ; Fails if V clear

            MOV R0, #0
            MSR APSR, R0
            LDR R0, =data0xFFFFFFFF              ; Prepares Saturation test
            LDR R0, [R0]
            USAT R1, #10, R0                     ; Set Q(saturation) flag
            MRS R0, APSR                         ; Get APSR status register
            CMP R0, #0x08000000                  ; Verify Q=1
            BNE _test_cpu_reg0_reg13_fail        ; Fails if Q is set

            ; Register R1
            LDR R0, =data0xAAAAAAAA
            LDR R1, [R0]
            LDR R0, [R0]
            CMP R0, R1
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R1, [R0]
            LDR R0, [R0]
            CMP R0, R1
            BNE _test_cpu_reg0_reg13_fail
            MOVS R1, #0x1

            ; Register R2
            LDR R0, =data0xAAAAAAAA
            LDR R2, [R0]
            LDR R0, [R0]
            CMP R0, R2
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R2, [R0]
            LDR R0, [R0]
            CMP R0, R2
            BNE _test_cpu_reg0_reg13_fail
            MOVS R2, #0x2

            ; Register R3
            LDR R0, =data0xAAAAAAAA
            LDR R3, [R0]
            LDR R0, [R0]
            CMP R0, R3
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R3, [R0]
            LDR R0, [R0]
            CMP R0, R3
            BNE _test_cpu_reg0_reg13_fail
            MOVS R3, #0x3

            ; Register R4
            LDR R0, =data0xAAAAAAAA
            LDR R4, [R0]
            LDR R0, [R0]
            CMP R0, R4
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R4, [R0]
            LDR R0, [R0]
            CMP R0, R4
            BNE _test_cpu_reg0_reg13_fail
            MOVS R4, #0x4

            ; Register R5
            LDR R0, =data0xAAAAAAAA
            LDR R5, [R0]
            LDR R0, [R0]
            CMP R0, R5
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R5, [R0]
            LDR R0, [R0]
            CMP R0, R5
            BNE _test_cpu_reg0_reg13_fail
            MOVS R5, #0x5

            ; Register R6
            LDR R0, =data0xAAAAAAAA
            LDR R6, [R0]
            LDR R0, [R0]
            CMP R0, R6
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R6, [R0]
            LDR R0, [R0]
            CMP R0, R6
            BNE _test_cpu_reg0_reg13_fail
            MOVS R6, #0x6

            ; Register R7
            LDR R0, =data0xAAAAAAAA
            LDR R7, [R0]
            LDR R0, [R0]
            CMP R0, R7
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R7, [R0]
            LDR R0, [R0]
            CMP R0, R7
            BNE _test_cpu_reg0_reg13_fail
            MOVS R7, #0x7

            ; Register R8
            LDR R0, =data0xAAAAAAAA
            LDR R0, [R0]
            MOV R8, R0
            CMP R0, R8
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R0, [R0]
            MOV R8, R0
            CMP R0, R8
            BNE _test_cpu_reg0_reg13_fail
            MOVS R0, #0x08
            MOV R8, R0

            BAL _test_cpu_continue

_test_cpu_reg0_reg13_fail
            ; test fail, R0 will hold value 1
            MOVS R0, #0x1                        ; STL_ERR
            B _test_exit

_test_cpu_continue
            ; Register R9
            LDR R0, =data0xAAAAAAAA
            LDR R0, [R0]
            MOV R9, R0
            CMP R0, R9
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R0, [R0]
            MOV R9, R0
            CMP R0, R9
            BNE _test_cpu_reg0_reg13_fail
            MOVS R0, #0x09
            MOV R9, R0

            ; Register R10
            LDR R0, =data0xAAAAAAAA
            LDR R0, [R0]
            MOV R10, R0
            CMP R0, R10
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R0, [R0]
            MOV R10, R0
            CMP R0, R10
            BNE _test_cpu_reg0_reg13_fail
            MOVS R0, #0x0A
            MOV R10, R0

            ; Register R11
            LDR R0, =data0xAAAAAAAA
            LDR R0, [R0]
            MOV R11, R0
            CMP R0, R11
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R0, [R0]
            MOV R11, R0
            CMP R0, R11
            BNE _test_cpu_reg0_reg13_fail
            MOVS R0, #0x0B
            MOV R11, R0

            ; Register R12
            LDR R0, =data0xAAAAAAAA
            LDR R0, [R0]
            MOV R12, R0
            CMP R0, R12
            BNE _test_cpu_reg0_reg13_fail
            LDR R0, =data0x55555555
            LDR R0, [R0]
            MOV R12, R0
            CMP R0, R12
            BNE _test_cpu_reg0_reg13_fail
            MOVS R0, #0x0C
            MOV R12, R0
            LDR R0, =_test_cpu_continue

            ; pattern verification (R0 is not tested)
            CMP R1, #0x01
            BNE _test_cpu_reg0_reg13_fail
            CMP R2, #0x02
            BNE _test_cpu_reg0_reg13_fail
            CMP R3, #0x03
            BNE _test_cpu_reg0_reg13_fail
            CMP R4, #0x04
            BNE _test_cpu_reg0_reg13_fail
            CMP R5, #0x05
            BNE _test_cpu_reg0_reg13_fail
            CMP R6, #0x06
            BNE _test_cpu_reg0_reg13_fail
            CMP R7, #0x07
            BNE _test_cpu_reg0_reg13_fail
            MOVS R0, #0x08
            CMP R0, R8
            BNE _test_cpu_reg0_reg13_fail
            MOVS R0, #0x09
            CMP R0, R9
            BNE _test_cpu_reg0_reg13_fail
            MOVS R0, #0x0A
            CMP R0, R10
            BNE _test_cpu_reg0_reg13_fail
            MOVS R0, #0x0B
            CMP R0, R11
            BNE _test_cpu_reg0_reg13_fail
            MOVS R0, #0x0C
            CMP R0, R12
            BNE _test_cpu_reg0_reg13_fail

            ; Process Stack pointer (banked Register R13)
            MRS R0, PSP                          ; Save process stack value
            LDR R1, =data0xAAAAAAA8              ; Test is different (PSP is word aligned, 2 LSB cleared)
            LDR R1, [R1]
            MSR PSP, R1                          ; load process stack value
            MRS R2, PSP                          ; Get back process stack value
            CMP R2, R1                           ; Verify value
            BNE _test_cpu_reg0_reg13_fail
            LDR R1, =data0x55555554              ; Test is different (PSP is word aligned, 2 LSB cleared)
            LDR R1, [R1]
            MSR PSP, R1                          ; load process stack value
            MRS R2, PSP                          ; Get back process stack value
            CMP R2, R1                           ; Verify value
            BNE _test_cpu_reg0_reg13_fail
            MSR PSP, R0                          ; Restore process stack value

            ; Stack pointer (Register R13)
            MRS R0, MSP                          ; Save stack pointer value
            LDR R1, =data0xAAAAAAA8              ; Test is different (SP is word aligned, 2 LSB cleared)
            LDR R1, [R1]
            MSR MSP, R1                          ; load SP value
            MRS R2, MSP                          ; Get back SP value
            CMP R2, R1                           ; Verify value
            BNE _test_cpu_reg0_reg13_fail
            LDR R1, =data0x55555554
            LDR R1, [R1]                         ; load SP value
            MSR MSP, R1                          ; Get back SP value
            MRS R2, MSP                          ; Verify value
            CMP R2, R1
            BNE _test_cpu_reg0_reg13_fail
            MSR MSP, R0                          ; Restore stack pointer value

_test_cpu_r14_sfr
            ; Link register R14
            MOV R1, LR
            LDR R0, =data0x55555555
            LDR R0, [R0]
            MOV R14, R0
            CMP R0, R14
            BNE _test_cpu_r14_sfr_fail

            LDR R0, =data0xAAAAAAAA
            LDR R0, [R0]
            MOV R14, R0
            CMP R0, R14
            BNE _test_cpu_r14_sfr_fail

            LDR R0, =data0x55555555
            LDR R0, [R0]
            MOV R14, R0
            CMP R0, R14
            BNE _test_cpu_r14_sfr_fail

            LDR R0, =data0xAAAAAAAA
            LDR R0, [R0]
            MOV R14, R0
            CMP R0, R14
            BNE _test_cpu_r14_sfr_fail
            MOV LR, R1

            ; APSR
            MRS R0, APSR
            LDR R1, =data0x50000000
            LDR R1,[R1]
            MSR APSR,R1
            MRS R2, APSR
            CMP R1, R2
            BNE _test_cpu_r14_sfr_fail

            LDR R1, =data0xA8000000
            LDR R1,[R1]
            MSR APSR,R1
            MRS R2, APSR
            CMP R1, R2
            BNE _test_cpu_r14_sfr_fail
            MSR APSR,R0

            ; PRIMASK register
            MRS R0, PRIMASK
            LDR R1, =data0x00000000
            LDR R1, [R1]
            MSR PRIMASK, R1
            MRS R2, PRIMASK
            CMP R1, R2
            BNE _test_cpu_r14_sfr_fail

            LDR R1, =data0x00000001
            LDR R1, [R1]
            MSR PRIMASK, R1
            MRS R2, PRIMASK
            CMP R1, R2
            BNE _test_cpu_r14_sfr_fail
            MSR PRIMASK, R0

            ; FAULTMASK register
            MRS R0, FAULTMASK
            LDR R1, =data0x00000000
            LDR R1, [R1]
            MSR FAULTMASK, R1
            MRS R2, FAULTMASK
            CMP R1, R2
            BNE _test_cpu_r14_sfr_fail

            LDR R1, =data0x00000001
            LDR R1, [R1]
            MSR FAULTMASK, R1
            MRS R2, FAULTMASK
            CMP R1, R2
            BNE _test_cpu_r14_sfr_fail
            MSR FAULTMASK, R0

            ; BASEPRI register
            MRS R0, BASEPRI
            LDR R1, =data0x000000A0
            LDR R1, [R1]
            MSR BASEPRI, R1
            MRS R2, BASEPRI
            CMP R1, R2
            BNE _test_cpu_r14_sfr_fail

            LDR R1, =data0x00000050
            LDR R1, [R1]
            MSR BASEPRI, R1
            MRS R2, BASEPRI
            CMP R1, R2
            BNE _test_cpu_r14_sfr_fail
            MSR BASEPRI, R0
            B _test_cpu_pass

_test_cpu_r14_sfr_fail
            ; test fail, R0 will hold value 1
            MOVS R0, #0x1                        ; STL_ERR
            B _test_exit

_test_cpu_pass
            ; test pass, R0 will hold value 0
            MOVS R0, #0x0                        ; STL_OK
            B _test_exit

_test_exit
            POP {R4-R7}                          ; Restore registers
            BX LR

            END
