nxp: add data handler used by bl31
bl31-data file written in assembly helps to manage data at bl31.
Signed-off-by: rocket <rod.dorris@nxp.com>
Signed-off-by: Pankaj Gupta <pankaj.gupta@nxp.com>
Change-Id: Ic3ace03364648cc1174bb05b5b334b9ccdaaa4ed
diff --git a/plat/nxp/common/aarch64/bl31_data.S b/plat/nxp/common/aarch64/bl31_data.S
new file mode 100644
index 0000000..cc91540
--- /dev/null
+++ b/plat/nxp/common/aarch64/bl31_data.S
@@ -0,0 +1,558 @@
+/*
+ * 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