/* * Copyright 2018-2020 NXP * * SPDX-License-Identifier: BSD-3-Clause * */ #include #include "bl31_data.h" #include "plat_psci.h" #include "platform_def.h" .global _getCoreData .global _setCoreData .global _getCoreState .global _setCoreState .global _init_global_data .global _get_global_data .global _set_global_data .global _initialize_psci .global _init_task_flags .global _set_task1_start .global _set_task1_done /* Function returns the specified data field value from the specified cpu * core data area * in: x0 = core mask lsb * x1 = data field name/offset * out: x0 = data value * uses x0, x1, x2, [x13, x14, x15] */ func _getCoreData /* generate a 0-based core number from the input mask */ clz x2, x0 mov x0, #63 sub x0, x0, x2 /* x0 = core number (0-based) */ /* x1 = field offset */ /* determine if this is bootcore or secondary core */ cbnz x0, 1f /* get base address for bootcore data */ ldr x2, =BC_PSCI_BASE add x2, x2, x1 b 2f 1: /* get base address for secondary core data */ /* x0 = core number (0-based) */ /* x1 = field offset */ /* generate number of regions to offset */ mov x2, #SEC_REGION_SIZE mul x2, x2, x0 /* x1 = field offset */ /* x2 = region offset */ /* generate the total offset to data element */ sub x1, x2, x1 /* x1 = total offset to data element */ /* get the base address */ ldr x2, =SECONDARY_TOP /* apply offset to base addr */ sub x2, x2, x1 2: /* x2 = data element address */ dc ivac, x2 dsb sy isb /* read data */ ldr x0, [x2] ret endfunc _getCoreData /* Function returns the SoC-specific state of the specified cpu * in: x0 = core mask lsb * out: x0 = data value * uses x0, x1, x2, [x13, x14, x15] */ func _getCoreState mov x1, #CORE_STATE_DATA /* generate a 0-based core number from the input mask */ clz x2, x0 mov x0, #63 sub x0, x0, x2 /* x0 = core number (0-based) */ /* x1 = field offset */ /* determine if this is bootcore or secondary core */ cbnz x0, 1f /* get base address for bootcore data */ ldr x2, =BC_PSCI_BASE add x2, x2, x1 b 2f 1: /* get base address for secondary core data */ /* x0 = core number (0-based) */ /* x1 = field offset */ /* generate number of regions to offset */ mov x2, #SEC_REGION_SIZE mul x2, x2, x0 /* x1 = field offset */ /* x2 = region offset */ /* generate the total offset to data element */ sub x1, x2, x1 /* x1 = total offset to data element */ /* get the base address */ ldr x2, =SECONDARY_TOP /* apply offset to base addr */ sub x2, x2, x1 2: /* x2 = data element address */ dc ivac, x2 dsb sy isb /* read data */ ldr x0, [x2] ret endfunc _getCoreState /* Function writes the specified data value into the specified cpu * core data area * in: x0 = core mask lsb * x1 = data field offset * x2 = data value to write/store * out: none * uses x0, x1, x2, x3, [x13, x14, x15] */ func _setCoreData /* x0 = core mask */ /* x1 = field offset */ /* x2 = data value */ clz x3, x0 mov x0, #63 sub x0, x0, x3 /* x0 = core number (0-based) */ /* x1 = field offset */ /* x2 = data value */ /* determine if this is bootcore or secondary core */ cbnz x0, 1f /* get base address for bootcore data */ ldr x3, =BC_PSCI_BASE add x3, x3, x1 b 2f 1: /* get base address for secondary core data */ /* x0 = core number (0-based) */ /* x1 = field offset */ /* x2 = data value */ /* generate number of regions to offset */ mov x3, #SEC_REGION_SIZE mul x3, x3, x0 /* x1 = field offset */ /* x2 = data value */ /* x3 = region offset */ /* generate the total offset to data element */ sub x1, x3, x1 /* x1 = total offset to data element */ /* x2 = data value */ ldr x3, =SECONDARY_TOP /* apply offset to base addr */ sub x3, x3, x1 2: /* x2 = data value */ /* x3 = data element address */ str x2, [x3] dc cvac, x3 dsb sy isb ret endfunc _setCoreData /* Function stores the specified core state * in: x0 = core mask lsb * x1 = data value to write/store * out: none * uses x0, x1, x2, x3, [x13, x14, x15] */ func _setCoreState mov x2, #CORE_STATE_DATA clz x3, x0 mov x0, #63 sub x0, x0, x3 /* x0 = core number (0-based) */ /* x1 = data value */ /* x2 = field offset */ /* determine if this is bootcore or secondary core */ cbnz x0, 1f /* get base address for bootcore data */ ldr x3, =BC_PSCI_BASE add x3, x3, x2 b 2f 1: /* get base address for secondary core data */ /* x0 = core number (0-based) */ /* x1 = data value */ /* x2 = field offset */ /* generate number of regions to offset */ mov x3, #SEC_REGION_SIZE mul x3, x3, x0 /* x1 = data value */ /* x2 = field offset */ /* x3 = region offset */ /* generate the total offset to data element */ sub x2, x3, x2 /* x1 = data value */ /* x2 = total offset to data element */ ldr x3, =SECONDARY_TOP /* apply offset to base addr */ sub x3, x3, x2 2: /* x1 = data value */ /* x3 = data element address */ str x1, [x3] dc civac, x3 dsb sy isb ret endfunc _setCoreState /* Function sets the task1 start * in: w0 = value to set flag to * out: none * uses x0, x1 */ func _set_task1_start ldr x1, =SMC_TASK1_BASE add x1, x1, #TSK_START_OFFSET str w0, [x1] dc cvac, x1 dsb sy isb ret endfunc _set_task1_start /* Function sets the state of the task 1 done flag * in: w0 = value to set flag to * out: none * uses x0, x1 */ func _set_task1_done ldr x1, =SMC_TASK1_BASE add x1, x1, #TSK_DONE_OFFSET str w0, [x1] dc cvac, x1 dsb sy isb ret endfunc _set_task1_done /* Function initializes the smc global data entries * Note: the constant LAST_SMC_GLBL_OFFSET must reference the last entry in the * smc global region * in: none * out: none * uses x0, x1, x2 */ func _init_global_data ldr x1, =SMC_GLBL_BASE /* x1 = SMC_GLBL_BASE */ mov x2, #LAST_SMC_GLBL_OFFSET add x2, x2, x1 1: str xzr, [x1] dc cvac, x1 cmp x2, x1 add x1, x1, #8 b.hi 1b dsb sy isb ret endfunc _init_global_data /* Function gets the value of the specified global data element * in: x0 = offset of data element * out: x0 = requested data element * uses x0, x1 */ func _get_global_data ldr x1, =SMC_GLBL_BASE add x1, x1, x0 dc ivac, x1 isb ldr x0, [x1] ret endfunc _get_global_data /* Function sets the value of the specified global data element * in: x0 = offset of data element * x1 = value to write * out: none * uses x0, x1, x2 */ func _set_global_data ldr x2, =SMC_GLBL_BASE add x0, x0, x2 str x1, [x0] dc cvac, x0 dsb sy isb ret endfunc _set_global_data /* Function initializes the core data areas * only executed by the boot core * in: none * out: none * uses: x0, x1, x2, x3, x4, x5, x6, x7, [x13, x14, x15] */ func _initialize_psci mov x7, x30 /* initialize the bootcore psci data */ ldr x5, =BC_PSCI_BASE mov x6, #CORE_RELEASED str x6, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5], #8 dc cvac, x5 str xzr, [x5] dc cvac, x5 dsb sy isb /* see if we have any secondary cores */ mov x4, #PLATFORM_CORE_COUNT sub x4, x4, #1 cbz x4, 3f /* initialize the secondary core's psci data */ ldr x5, =SECONDARY_TOP /* core mask lsb for core 1 */ mov x3, #2 sub x5, x5, #SEC_REGION_SIZE /* x3 = core1 mask lsb */ /* x4 = number of secondary cores */ /* x5 = core1 psci data base address */ 2: /* set core state in x6 */ mov x0, x3 mov x6, #CORE_IN_RESET bl _soc_ck_disabled cbz x0, 1f mov x6, #CORE_DISABLED 1: add x2, x5, #CORE_STATE_DATA str x6, [x2] dc cvac, x2 add x2, x5, #SPSR_EL3_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #CNTXT_ID_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #START_ADDR_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #LINK_REG_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #GICC_CTLR_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #ABORT_FLAG_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #SCTLR_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #CPUECTLR_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #AUX_01_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #AUX_02_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #AUX_03_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #AUX_04_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #AUX_05_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #SCR_EL3_DATA str xzr, [x2] dc cvac, x2 add x2, x5, #HCR_EL2_DATA str xzr, [x2] dc cvac, x2 dsb sy isb sub x4, x4, #1 cbz x4, 3f /* generate next core mask */ lsl x3, x3, #1 /* decrement base address to next data area */ sub x5, x5, #SEC_REGION_SIZE b 2b 3: mov x30, x7 ret endfunc _initialize_psci /* Function initializes the soc init task flags * in: none * out: none * uses x0, x1, [x13, x14, x15] */ func _init_task_flags /* get the base address of the first task structure */ ldr x0, =SMC_TASK1_BASE /* x0 = task1 base address */ str wzr, [x0, #TSK_START_OFFSET] str wzr, [x0, #TSK_DONE_OFFSET] str wzr, [x0, #TSK_CORE_OFFSET] dc cvac, x0 /* move to task2 structure */ add x0, x0, #SMC_TASK_OFFSET str wzr, [x0, #TSK_START_OFFSET] str wzr, [x0, #TSK_DONE_OFFSET] str wzr, [x0, #TSK_CORE_OFFSET] dc cvac, x0 /* move to task3 structure */ add x0, x0, #SMC_TASK_OFFSET str wzr, [x0, #TSK_START_OFFSET] str wzr, [x0, #TSK_DONE_OFFSET] str wzr, [x0, #TSK_CORE_OFFSET] dc cvac, x0 /* move to task4 structure */ add x0, x0, #SMC_TASK_OFFSET str wzr, [x0, #TSK_START_OFFSET] str wzr, [x0, #TSK_DONE_OFFSET] str wzr, [x0, #TSK_CORE_OFFSET] dc cvac, x0 dsb sy isb ret endfunc _init_task_flags