| /* |
| * Copyright 2018-2020 NXP |
| * |
| * SPDX-License-Identifier: BSD-3-Clause |
| * |
| */ |
| |
| #include <asm_macros.S> |
| |
| #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 |