[rtems commit] bsps/arm: Add SMP support
Sebastian Huber
sebh at rtems.org
Fri May 31 13:15:45 UTC 2013
Module: rtems
Branch: master
Commit: db42c079a0479c17add94e5fb5fb89db1dfa6799
Changeset: http://git.rtems.org/rtems/commit/?id=db42c079a0479c17add94e5fb5fb89db1dfa6799
Author: Sebastian Huber <sebastian.huber at embedded-brains.de>
Date: Fri May 31 13:59:47 2013 +0200
bsps/arm: Add SMP support
---
c/src/lib/libbsp/arm/realview-pbx-a9/Makefile.am | 7 ++
c/src/lib/libbsp/arm/realview-pbx-a9/README | 2 +
c/src/lib/libbsp/arm/realview-pbx-a9/configure.ac | 3 +
c/src/lib/libbsp/arm/realview-pbx-a9/include/bsp.h | 6 ++
c/src/lib/libbsp/arm/realview-pbx-a9/include/irq.h | 4 -
.../make/custom/realview_pbx_a9_qemu_smp.cfg | 1 +
c/src/lib/libbsp/arm/realview-pbx-a9/preinstall.am | 4 +
.../arm/realview-pbx-a9/startup/bspstarthooks.c | 27 ++++--
.../{linkcmds => linkcmds.realview_pbx_a9_qemu} | 0
...{linkcmds => linkcmds.realview_pbx_a9_qemu_smp} | 2 +
c/src/lib/libbsp/arm/shared/arm-a9mpcore-smp.c | 71 ++++++++++++++++
c/src/lib/libbsp/arm/shared/arm-gic-irq.c | 61 ++++++--------
.../libbsp/arm/shared/include/arm-a9mpcore-regs.h | 16 ++++
.../libbsp/arm/shared/include/arm-a9mpcore-start.h | 88 ++++++++++++++++++++
.../lib/libbsp/arm/shared/include/arm-cp15-start.h | 6 ++
c/src/lib/libbsp/arm/shared/include/arm-gic-irq.h | 33 +++++++-
c/src/lib/libbsp/arm/shared/start/start.S | 33 +++++++
c/src/lib/libbsp/arm/shared/startup/linkcmds.base | 8 ++
c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am | 7 ++
c/src/lib/libbsp/arm/xilinx-zynq/configure.ac | 3 +
c/src/lib/libbsp/arm/xilinx-zynq/include/bsp.h | 6 ++
c/src/lib/libbsp/arm/xilinx-zynq/include/irq.h | 4 -
.../make/custom/xilinx_zynq_a9_qemu_smp.cfg | 1 +
c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am | 4 +
.../libbsp/arm/xilinx-zynq/startup/bspstarthooks.c | 27 ++++--
.../{linkcmds => linkcmds.xilinx_zynq_a9_qemu} | 0
.../startup/linkcmds.xilinx_zynq_a9_qemu_smp} | 2 +
c/src/lib/libcpu/arm/shared/include/arm-cp15.h | 57 +++++++++++++
28 files changed, 418 insertions(+), 65 deletions(-)
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/Makefile.am b/c/src/lib/libbsp/arm/realview-pbx-a9/Makefile.am
index d8589a3..9410a30 100644
--- a/c/src/lib/libbsp/arm/realview-pbx-a9/Makefile.am
+++ b/c/src/lib/libbsp/arm/realview-pbx-a9/Makefile.am
@@ -35,6 +35,7 @@ include_bsp_HEADERS += ../../shared/tod.h
include_bsp_HEADERS += ../shared/include/start.h
include_bsp_HEADERS += ../shared/include/arm-a9mpcore-irq.h
include_bsp_HEADERS += ../shared/include/arm-a9mpcore-regs.h
+include_bsp_HEADERS += ../shared/include/arm-a9mpcore-start.h
include_bsp_HEADERS += ../shared/include/arm-cp15-start.h
include_bsp_HEADERS += ../shared/include/arm-gic.h
include_bsp_HEADERS += ../shared/include/arm-gic-irq.h
@@ -59,6 +60,8 @@ project_lib_DATA = start.$(OBJEXT)
project_lib_DATA += startup/linkcmds
EXTRA_DIST =
+EXTRA_DIST += startup/linkcmds.realview_pbx_a9_qemu
+EXTRA_DIST += startup/linkcmds.realview_pbx_a9_qemu_smp
###############################################################################
# LibBSP #
@@ -121,6 +124,10 @@ libbsp_a_CPPFLAGS += -I$(srcdir)/../../../libcpu/arm/shared/include
# Start hooks
libbsp_a_SOURCES += startup/bspstarthooks.c
+if HAS_SMP
+libbsp_a_SOURCES += ../shared/arm-a9mpcore-smp.c
+endif
+
###############################################################################
# Special Rules #
###############################################################################
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/README b/c/src/lib/libbsp/arm/realview-pbx-a9/README
index 8fdfdc4..a4e6ac1 100644
--- a/c/src/lib/libbsp/arm/realview-pbx-a9/README
+++ b/c/src/lib/libbsp/arm/realview-pbx-a9/README
@@ -11,3 +11,5 @@ make install
export PATH="$PATH:/opt/qemu/bin"
qemu-system-arm -S -s -no-reboot -net none -nographic -M realview-pbx-a9 -m 256M -kernel ticker.exe
+
+qemu-system-arm -S -s -no-reboot -net none -nographic -smp 2 -icount auto -M realview-pbx-a9 -m 256M -kernel ticker.exe
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/configure.ac b/c/src/lib/libbsp/arm/realview-pbx-a9/configure.ac
index 4052dcc..61d7a2a 100644
--- a/c/src/lib/libbsp/arm/realview-pbx-a9/configure.ac
+++ b/c/src/lib/libbsp/arm/realview-pbx-a9/configure.ac
@@ -33,6 +33,9 @@ RTEMS_BSPOPTS_HELP([CLOCK_DRIVER_USE_FAST_IDLE],
occurs while the IDLE thread is executing. This can significantly reduce
simulation times.])
+RTEMS_CHECK_SMP
+AM_CONDITIONAL(HAS_SMP,[test "$rtems_cv_HAS_SMP" = "yes"])
+
RTEMS_BSP_CLEANUP_OPTIONS(0, 1)
RTEMS_BSP_LINKCMDS
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/include/bsp.h b/c/src/lib/libbsp/arm/realview-pbx-a9/include/bsp.h
index 41b714b..c25c9f6 100644
--- a/c/src/lib/libbsp/arm/realview-pbx-a9/include/bsp.h
+++ b/c/src/lib/libbsp/arm/realview-pbx-a9/include/bsp.h
@@ -31,8 +31,14 @@
extern "C" {
#endif /* __cplusplus */
+#define BSP_ARM_A9MPCORE_SCU_BASE 0x1f000000
+
+#define BSP_ARM_GIC_CPUIF_BASE 0x1f000100
+
#define BSP_ARM_A9MPCORE_PT_BASE 0x1f000600
+#define BSP_ARM_GIC_DIST_BASE 0x1f001000
+
typedef enum {
BSP_ARM_A9MPCORE_FATAL_CLOCK_IRQ_INSTALL,
BSP_ARM_A9MPCORE_FATAL_CLOCK_IRQ_REMOVE
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/include/irq.h b/c/src/lib/libbsp/arm/realview-pbx-a9/include/irq.h
index 2651d6c..b065c36 100644
--- a/c/src/lib/libbsp/arm/realview-pbx-a9/include/irq.h
+++ b/c/src/lib/libbsp/arm/realview-pbx-a9/include/irq.h
@@ -77,10 +77,6 @@ extern "C" {
#define BSP_INTERRUPT_VECTOR_MIN 0
#define BSP_INTERRUPT_VECTOR_MAX 89
-#define BSP_ARM_GIC_CPUIF_BASE 0x1f000100
-
-#define BSP_ARM_GIC_DIST_BASE 0x1f001000
-
#ifdef __cplusplus
}
#endif /* __cplusplus */
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/make/custom/realview_pbx_a9_qemu_smp.cfg b/c/src/lib/libbsp/arm/realview-pbx-a9/make/custom/realview_pbx_a9_qemu_smp.cfg
new file mode 100644
index 0000000..fd51a18
--- /dev/null
+++ b/c/src/lib/libbsp/arm/realview-pbx-a9/make/custom/realview_pbx_a9_qemu_smp.cfg
@@ -0,0 +1 @@
+include $(RTEMS_ROOT)/make/custom/realview_pbx_a9_qemu.cfg
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/preinstall.am b/c/src/lib/libbsp/arm/realview-pbx-a9/preinstall.am
index b153c8c..56e4ac8 100644
--- a/c/src/lib/libbsp/arm/realview-pbx-a9/preinstall.am
+++ b/c/src/lib/libbsp/arm/realview-pbx-a9/preinstall.am
@@ -94,6 +94,10 @@ $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h: ../shared/include/arm-a9mpcore-regs.
$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h
PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h
+$(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h: ../shared/include/arm-a9mpcore-start.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+ $(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h
+
$(PROJECT_INCLUDE)/bsp/arm-cp15-start.h: ../shared/include/arm-cp15-start.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-cp15-start.h
PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-cp15-start.h
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/startup/bspstarthooks.c b/c/src/lib/libbsp/arm/realview-pbx-a9/startup/bspstarthooks.c
index 48e174a..93ebe20 100644
--- a/c/src/lib/libbsp/arm/realview-pbx-a9/startup/bspstarthooks.c
+++ b/c/src/lib/libbsp/arm/realview-pbx-a9/startup/bspstarthooks.c
@@ -15,12 +15,14 @@
#include <bsp.h>
#include <bsp/start.h>
#include <bsp/arm-cp15-start.h>
+#include <bsp/arm-a9mpcore-start.h>
#include <bsp/linker-symbols.h>
-BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
-{
- /* Do nothing */
-}
+#ifdef RTEMS_SMP
+ #define MMU_DATA_READ_WRITE ARMV7_MMU_DATA_READ_WRITE_SHAREABLE
+#else
+ #define MMU_DATA_READ_WRITE ARMV7_MMU_DATA_READ_WRITE_CACHED
+#endif
BSP_START_DATA_SECTION static const arm_cp15_start_section_config
rvpbxa9_mmu_config_table[] = {
@@ -31,7 +33,7 @@ rvpbxa9_mmu_config_table[] = {
}, {
.begin = (uint32_t) bsp_section_fast_data_begin,
.end = (uint32_t) bsp_section_fast_data_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = (uint32_t) bsp_section_start_begin,
.end = (uint32_t) bsp_section_start_end,
@@ -39,7 +41,7 @@ rvpbxa9_mmu_config_table[] = {
}, {
.begin = (uint32_t) bsp_section_vector_begin,
.end = (uint32_t) bsp_section_vector_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = (uint32_t) bsp_section_text_begin,
.end = (uint32_t) bsp_section_text_end,
@@ -51,19 +53,19 @@ rvpbxa9_mmu_config_table[] = {
}, {
.begin = (uint32_t) bsp_section_data_begin,
.end = (uint32_t) bsp_section_data_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = (uint32_t) bsp_section_bss_begin,
.end = (uint32_t) bsp_section_bss_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = (uint32_t) bsp_section_work_begin,
.end = (uint32_t) bsp_section_work_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = (uint32_t) bsp_section_stack_begin,
.end = (uint32_t) bsp_section_stack_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = 0x10000000U,
.end = 0x10020000U,
@@ -91,6 +93,11 @@ BSP_START_TEXT_SECTION static void setup_mmu_and_cache(void)
);
}
+BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
+{
+ arm_a9mpcore_start_hook_0();
+}
+
BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
{
bsp_start_copy_sections();
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds b/c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds.realview_pbx_a9_qemu
similarity index 100%
copy from c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds
copy to c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds.realview_pbx_a9_qemu
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds b/c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds.realview_pbx_a9_qemu_smp
similarity index 92%
copy from c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds
copy to c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds.realview_pbx_a9_qemu_smp
index 0b5e3e0..d76dd2e 100644
--- a/c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds
+++ b/c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds.realview_pbx_a9_qemu_smp
@@ -28,4 +28,6 @@ bsp_vector_table_in_start_section = 1;
bsp_translation_table_base = ORIGIN (RAM_MMU);
+bsp_processor_count = DEFINED (bsp_processor_count) ? bsp_processor_count : 8;
+
INCLUDE linkcmds.armv4
diff --git a/c/src/lib/libbsp/arm/shared/arm-a9mpcore-smp.c b/c/src/lib/libbsp/arm/shared/arm-a9mpcore-smp.c
new file mode 100644
index 0000000..b94bcec
--- /dev/null
+++ b/c/src/lib/libbsp/arm/shared/arm-a9mpcore-smp.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2013 embedded brains GmbH. All rights reserved.
+ *
+ * embedded brains GmbH
+ * Dornierstr. 4
+ * 82178 Puchheim
+ * Germany
+ * <info at embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <assert.h>
+
+#include <rtems/bspsmp.h>
+
+#include <libcpu/arm-cp15.h>
+
+#include <bsp/irq.h>
+
+int bsp_smp_processor_id(void)
+{
+ return (int) arm_cortex_a9_get_multiprocessor_cpu_id();
+}
+
+static void ipi_handler(void *arg)
+{
+ rtems_smp_process_interrupt();
+}
+
+uint32_t bsp_smp_initialize(uint32_t configured_cpu_count)
+{
+ rtems_status_code sc;
+ uint32_t max_cpu_count = arm_gic_irq_processor_count();
+ uint32_t used_cpu_count = configured_cpu_count < max_cpu_count ?
+ configured_cpu_count : max_cpu_count;
+
+ sc = rtems_interrupt_handler_install(
+ ARM_GIC_IRQ_SGI_0,
+ "IPI",
+ RTEMS_INTERRUPT_UNIQUE,
+ ipi_handler,
+ NULL
+ );
+ assert(sc == RTEMS_SUCCESSFUL);
+
+ return used_cpu_count;
+}
+
+void bsp_smp_broadcast_interrupt(void)
+{
+ /*
+ * FIXME: This broadcasts the interrupt also to processors not used by RTEMS.
+ */
+ rtems_status_code sc = arm_gic_irq_generate_software_irq(
+ ARM_GIC_IRQ_SGI_0,
+ ARM_GIC_IRQ_SOFTWARE_IRQ_TO_ALL_EXCEPT_SELF,
+ 0xff
+ );
+}
+
+void bsp_smp_interrupt_cpu(int cpu)
+{
+ rtems_status_code sc = arm_gic_irq_generate_software_irq(
+ ARM_GIC_IRQ_SGI_0,
+ ARM_GIC_IRQ_SOFTWARE_IRQ_TO_ALL_IN_LIST,
+ (uint8_t) (1U << cpu)
+ );
+}
diff --git a/c/src/lib/libbsp/arm/shared/arm-gic-irq.c b/c/src/lib/libbsp/arm/shared/arm-gic-irq.c
index 84d66aa..ed2d389 100644
--- a/c/src/lib/libbsp/arm/shared/arm-gic-irq.c
+++ b/c/src/lib/libbsp/arm/shared/arm-gic-irq.c
@@ -18,14 +18,12 @@
#include <libcpu/arm-cp15.h>
-#include <bsp.h>
#include <bsp/irq.h>
#include <bsp/irq-generic.h>
+#include <bsp/start.h>
#define GIC_CPUIF ((volatile gic_cpuif *) BSP_ARM_GIC_CPUIF_BASE)
-#define GIC_DIST ((volatile gic_dist *) BSP_ARM_GIC_DIST_BASE)
-
#define PRIORITY_DEFAULT 128
void bsp_interrupt_dispatch(void)
@@ -51,7 +49,7 @@ rtems_status_code bsp_interrupt_vector_enable(rtems_vector_number vector)
rtems_status_code sc = RTEMS_SUCCESSFUL;
if (bsp_interrupt_is_valid_vector(vector)) {
- volatile gic_dist *dist = GIC_DIST;
+ volatile gic_dist *dist = ARM_GIC_DIST;
gic_id_enable(dist, vector);
} else {
@@ -66,7 +64,7 @@ rtems_status_code bsp_interrupt_vector_disable(rtems_vector_number vector)
rtems_status_code sc = RTEMS_SUCCESSFUL;
if (bsp_interrupt_is_valid_vector(vector)) {
- volatile gic_dist *dist = GIC_DIST;
+ volatile gic_dist *dist = ARM_GIC_DIST;
gic_id_disable(dist, vector);
} else {
@@ -89,10 +87,15 @@ static inline uint32_t get_id_count(volatile gic_dist *dist)
rtems_status_code bsp_interrupt_facility_initialize(void)
{
volatile gic_cpuif *cpuif = GIC_CPUIF;
- volatile gic_dist *dist = GIC_DIST;
+ volatile gic_dist *dist = ARM_GIC_DIST;
uint32_t id_count = get_id_count(dist);
uint32_t id;
+ arm_cp15_set_exception_handler(
+ ARM_EXCEPTION_IRQ,
+ _ARMV4_Exception_interrupt
+ );
+
for (id = 0; id < id_count; ++id) {
gic_id_set_priority(dist, id, PRIORITY_DEFAULT);
}
@@ -107,14 +110,25 @@ rtems_status_code bsp_interrupt_facility_initialize(void)
dist->icddcr = GIC_DIST_ICDDCR_ENABLE;
- arm_cp15_set_exception_handler(
- ARM_EXCEPTION_IRQ,
- _ARMV4_Exception_interrupt
- );
-
return RTEMS_SUCCESSFUL;
}
+#ifdef RTEMS_SMP
+BSP_START_TEXT_SECTION void arm_gic_irq_initialize_secondary_cpu(void)
+{
+ volatile gic_cpuif *cpuif = GIC_CPUIF;
+ volatile gic_dist *dist = ARM_GIC_DIST;
+
+ while ((dist->icddcr & GIC_DIST_ICDDCR_ENABLE) == 0) {
+ /* Wait */
+ }
+
+ cpuif->iccpmr = GIC_CPUIF_ICCPMR_PRIORITY(0xff);
+ cpuif->iccbpr = GIC_CPUIF_ICCBPR_BINARY_POINT(0x0);
+ cpuif->iccicr = GIC_CPUIF_ICCICR_ENABLE;
+}
+#endif
+
rtems_status_code arm_gic_irq_set_priority(
rtems_vector_number vector,
uint8_t priority
@@ -123,7 +137,7 @@ rtems_status_code arm_gic_irq_set_priority(
rtems_status_code sc = RTEMS_SUCCESSFUL;
if (bsp_interrupt_is_valid_vector(vector)) {
- volatile gic_dist *dist = GIC_DIST;
+ volatile gic_dist *dist = ARM_GIC_DIST;
gic_id_set_priority(dist, vector, priority);
} else {
@@ -141,7 +155,7 @@ rtems_status_code arm_gic_irq_get_priority(
rtems_status_code sc = RTEMS_SUCCESSFUL;
if (bsp_interrupt_is_valid_vector(vector)) {
- volatile gic_dist *dist = GIC_DIST;
+ volatile gic_dist *dist = ARM_GIC_DIST;
*priority = gic_id_get_priority(dist, vector);
} else {
@@ -150,24 +164,3 @@ rtems_status_code arm_gic_irq_get_priority(
return sc;
}
-
-rtems_status_code arm_gic_irq_generate_software_irq(
- rtems_vector_number vector,
- arm_gic_irq_software_irq_target_filter filter,
- uint8_t targets
-)
-{
- rtems_status_code sc = RTEMS_SUCCESSFUL;
-
- if (vector < 16) {
- volatile gic_dist *dist = GIC_DIST;
-
- dist->icdsgir = GIC_DIST_ICDSGIR_TARGET_LIST_FILTER(filter)
- | GIC_DIST_ICDSGIR_CPU_TARGET_LIST(targets)
- | GIC_DIST_ICDSGIR_SGIINTID(vector);
- } else {
- sc = RTEMS_INVALID_ID;
- }
-
- return sc;
-}
diff --git a/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-regs.h b/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-regs.h
index 7d8f637..dc7fd75 100644
--- a/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-regs.h
+++ b/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-regs.h
@@ -19,7 +19,23 @@
typedef struct {
uint32_t ctrl;
+#define A9MPCORE_SCU_CTRL_SCU_EN BSP_BIT32(0)
+#define A9MPCORE_SCU_CTRL_ADDR_FLT_EN BSP_BIT32(1)
+#define A9MPCORE_SCU_CTRL_RAM_PAR_EN BSP_BIT32(2)
+#define A9MPCORE_SCU_CTRL_SCU_SPEC_LINE_FILL_EN BSP_BIT32(3)
+#define A9MPCORE_SCU_CTRL_FORCE_PORT_0_EN BSP_BIT32(4)
+#define A9MPCORE_SCU_CTRL_SCU_STANDBY_EN BSP_BIT32(5)
+#define A9MPCORE_SCU_CTRL_IC_STANDBY_EN BSP_BIT32(6)
uint32_t cfg;
+#define A9MPCORE_SCU_CFG_CPU_COUNT(val) BSP_FLD32(val, 0, 1)
+#define A9MPCORE_SCU_CFG_CPU_COUNT_GET(reg) BSP_FLD32GET(reg, 0, 1)
+#define A9MPCORE_SCU_CFG_CPU_COUNT_SET(reg, val) BSP_FLD32SET(reg, val, 0, 1)
+#define A9MPCORE_SCU_CFG_SMP_MODE(val) BSP_FLD32(val, 4, 7)
+#define A9MPCORE_SCU_CFG_SMP_MODE_GET(reg) BSP_FLD32GET(reg, 4, 7)
+#define A9MPCORE_SCU_CFG_SMP_MODE_SET(reg, val) BSP_FLD32SET(reg, val, 4, 7)
+#define A9MPCORE_SCU_CFG_TAG_RAM_SIZE(val) BSP_FLD32(val, 8, 15)
+#define A9MPCORE_SCU_CFG_TAG_RAM_SIZE_GET(reg) BSP_FLD32GET(reg, 8, 15)
+#define A9MPCORE_SCU_CFG_TAG_RAM_SIZE_SET(reg, val) BSP_FLD32SET(reg, val, 8, 15)
uint32_t pwrst;
uint32_t invss;
uint32_t reserved_10[12];
diff --git a/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-start.h b/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-start.h
new file mode 100644
index 0000000..b42c549
--- /dev/null
+++ b/c/src/lib/libbsp/arm/shared/include/arm-a9mpcore-start.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2013 embedded brains GmbH. All rights reserved.
+ *
+ * embedded brains GmbH
+ * Dornierstr. 4
+ * 82178 Puchheim
+ * Germany
+ * <info at embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#ifndef LIBBSP_ARM_SHARED_ARM_A9MPCORE_START_H
+#define LIBBSP_ARM_SHARED_ARM_A9MPCORE_START_H
+
+#include <rtems/bspsmp.h>
+
+#include <libcpu/arm-cp15.h>
+
+#include <bsp.h>
+#include <bsp/start.h>
+#include <bsp/arm-a9mpcore-regs.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+BSP_START_TEXT_SECTION static inline uint32_t
+arm_cp15_get_auxiliary_control(void);
+
+BSP_START_TEXT_SECTION static inline void
+arm_cp15_set_auxiliary_control(uint32_t val);
+
+BSP_START_TEXT_SECTION static inline arm_a9mpcore_start_hook_0(void)
+{
+#ifdef RTEMS_SMP
+ volatile a9mpcore_scu *scu = (volatile a9mpcore_scu *) BSP_ARM_A9MPCORE_SCU_BASE;
+ uint32_t cpu_id;
+ uint32_t actlr;
+
+ /* Enable Snoop Control Unit (SCU) */
+ scu->ctrl |= A9MPCORE_SCU_CTRL_SCU_EN;
+
+ /* Enable cache coherency support for this processor */
+ actlr = arm_cp15_get_auxiliary_control();
+ actlr |= ARM_CORTEX_A9_ACTL_SMP;
+ arm_cp15_set_auxiliary_control(actlr);
+
+ cpu_id = arm_cortex_a9_get_multiprocessor_cpu_id();
+ if (cpu_id != 0) {
+ if (cpu_id < rtems_configuration_get_maximum_processors()) {
+ uint32_t ctrl;
+
+ arm_gic_irq_initialize_secondary_cpu();
+
+ ctrl = arm_cp15_start_setup_mmu_and_cache(
+ 0,
+ ARM_CP15_CTRL_AFE | ARM_CP15_CTRL_Z
+ );
+
+ arm_cp15_set_domain_access_control(
+ ARM_CP15_DAC_DOMAIN(ARM_MMU_DEFAULT_CLIENT_DOMAIN, ARM_CP15_DAC_CLIENT)
+ );
+
+ /* FIXME: Sharing the translation table between processors is brittle */
+ arm_cp15_set_translation_table_base((uint32_t *) bsp_translation_table_base);
+
+ ctrl |= ARM_CP15_CTRL_I | ARM_CP15_CTRL_C | ARM_CP15_CTRL_M;
+ arm_cp15_set_control(ctrl);
+
+ rtems_smp_secondary_cpu_initialize();
+ } else {
+ /* FIXME: Shutdown processor */
+ while (1) {
+ __asm__ volatile ("wfi");
+ }
+ }
+ }
+#endif
+}
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* LIBBSP_ARM_SHARED_ARM_A9MPCORE_START_H */
diff --git a/c/src/lib/libbsp/arm/shared/include/arm-cp15-start.h b/c/src/lib/libbsp/arm/shared/include/arm-cp15-start.h
index 6fb8226..11efbcd 100644
--- a/c/src/lib/libbsp/arm/shared/include/arm-cp15-start.h
+++ b/c/src/lib/libbsp/arm/shared/include/arm-cp15-start.h
@@ -41,6 +41,12 @@ arm_cp15_cache_invalidate(void);
BSP_START_TEXT_SECTION static inline void
arm_cp15_tlb_invalidate(void);
+BSP_START_TEXT_SECTION static inline uint32_t
+arm_cp15_get_multiprocessor_affinity(void);
+
+BSP_START_TEXT_SECTION static inline uint32_t
+arm_cortex_a9_get_multiprocessor_cpu_id(void);
+
typedef struct {
uint32_t begin;
uint32_t end;
diff --git a/c/src/lib/libbsp/arm/shared/include/arm-gic-irq.h b/c/src/lib/libbsp/arm/shared/include/arm-gic-irq.h
index c45d354..5c152a1 100644
--- a/c/src/lib/libbsp/arm/shared/include/arm-gic-irq.h
+++ b/c/src/lib/libbsp/arm/shared/include/arm-gic-irq.h
@@ -15,7 +15,8 @@
#ifndef LIBBSP_ARM_SHARED_ARM_GIC_IRQ_H
#define LIBBSP_ARM_SHARED_ARM_GIC_IRQ_H
-#include <rtems.h>
+#include <bsp.h>
+#include <bsp/arm-gic.h>
#ifdef __cplusplus
extern "C" {
@@ -37,6 +38,8 @@ extern "C" {
#define ARM_GIC_IRQ_SGI_14 14
#define ARM_GIC_IRQ_SGI_15 15
+#define ARM_GIC_DIST ((volatile gic_dist *) BSP_ARM_GIC_DIST_BASE)
+
rtems_status_code arm_gic_irq_set_priority(
rtems_vector_number vector,
uint8_t priority
@@ -53,11 +56,35 @@ typedef enum {
ARM_GIC_IRQ_SOFTWARE_IRQ_TO_SELF
} arm_gic_irq_software_irq_target_filter;
-rtems_status_code arm_gic_irq_generate_software_irq(
+static inline rtems_status_code arm_gic_irq_generate_software_irq(
rtems_vector_number vector,
arm_gic_irq_software_irq_target_filter filter,
uint8_t targets
-);
+)
+{
+ rtems_status_code sc = RTEMS_SUCCESSFUL;
+
+ if (vector <= ARM_GIC_IRQ_SGI_15) {
+ volatile gic_dist *dist = ARM_GIC_DIST;
+
+ dist->icdsgir = GIC_DIST_ICDSGIR_TARGET_LIST_FILTER(filter)
+ | GIC_DIST_ICDSGIR_CPU_TARGET_LIST(targets)
+ | GIC_DIST_ICDSGIR_SGIINTID(vector);
+ } else {
+ sc = RTEMS_INVALID_ID;
+ }
+
+ return sc;
+}
+
+static inline uint32_t arm_gic_irq_processor_count(void)
+{
+ volatile gic_dist *dist = ARM_GIC_DIST;
+
+ return GIC_DIST_ICDICTR_CPU_NUMBER_GET(dist->icdictr) + 1;
+}
+
+void arm_gic_irq_initialize_secondary_cpu(void);
#ifdef __cplusplus
}
diff --git a/c/src/lib/libbsp/arm/shared/start/start.S b/c/src/lib/libbsp/arm/shared/start/start.S
index e79a5fc..7b59ab4 100644
--- a/c/src/lib/libbsp/arm/shared/start/start.S
+++ b/c/src/lib/libbsp/arm/shared/start/start.S
@@ -31,6 +31,14 @@
.extern boot_card
.extern bsp_start_hook_0
.extern bsp_start_hook_1
+ .extern bsp_stack_irq_end
+ .extern bsp_stack_fiq_end
+ .extern bsp_stack_abt_end
+ .extern bsp_stack_und_end
+ .extern bsp_stack_svc_end
+#ifdef RTEMS_SMP
+ .extern bsp_stack_all_size
+#endif
.extern _ARMV4_Exception_undef_default
.extern _ARMV4_Exception_swi_default
.extern _ARMV4_Exception_data_abort_default
@@ -119,6 +127,16 @@ _start:
* loader.
*/
+#ifdef RTEMS_SMP
+ /* Read MPIDR */
+ mrc p15, 0, r0, c0, c0, 5
+
+ /* Calculate stack offset */
+ and r0, #0xff
+ ldr r1, =bsp_stack_all_size
+ mul r1, r0
+#endif
+
/*
* Set SVC mode, disable interrupts and enable ARM instructions.
*/
@@ -131,26 +149,41 @@ _start:
mov r0, #(ARM_PSR_M_IRQ | ARM_PSR_I | ARM_PSR_F)
msr cpsr, r0
ldr sp, =bsp_stack_irq_end
+#ifdef RTEMS_SMP
+ add sp, r1
+#endif
/* Enter FIQ mode and set up the FIQ stack pointer */
mov r0, #(ARM_PSR_M_FIQ | ARM_PSR_I | ARM_PSR_F)
msr cpsr, r0
ldr sp, =bsp_stack_fiq_end
+#ifdef RTEMS_SMP
+ add sp, r1
+#endif
/* Enter ABT mode and set up the ABT stack pointer */
mov r0, #(ARM_PSR_M_ABT | ARM_PSR_I | ARM_PSR_F)
msr cpsr, r0
ldr sp, =bsp_stack_abt_end
+#ifdef RTEMS_SMP
+ add sp, r1
+#endif
/* Enter UND mode and set up the UND stack pointer */
mov r0, #(ARM_PSR_M_UND | ARM_PSR_I | ARM_PSR_F)
msr cpsr, r0
ldr sp, =bsp_stack_und_end
+#ifdef RTEMS_SMP
+ add sp, r1
+#endif
/* Enter SVC mode and set up the SVC stack pointer */
mov r0, #(ARM_PSR_M_SVC | ARM_PSR_I | ARM_PSR_F)
msr cpsr, r0
ldr sp, =bsp_stack_svc_end
+#ifdef RTEMS_SMP
+ add sp, r1
+#endif
/* Stay in SVC mode */
diff --git a/c/src/lib/libbsp/arm/shared/startup/linkcmds.base b/c/src/lib/libbsp/arm/shared/startup/linkcmds.base
index 27f5acb..d1a73d0 100644
--- a/c/src/lib/libbsp/arm/shared/startup/linkcmds.base
+++ b/c/src/lib/libbsp/arm/shared/startup/linkcmds.base
@@ -56,6 +56,10 @@ bsp_stack_und_size = ALIGN (bsp_stack_und_size, bsp_stack_align);
bsp_stack_main_size = DEFINED (bsp_stack_main_size) ? bsp_stack_main_size : 0;
bsp_stack_main_size = ALIGN (bsp_stack_main_size, bsp_stack_align);
+bsp_stack_all_size = bsp_stack_abt_size + bsp_stack_fiq_size + bsp_stack_irq_size + bsp_stack_svc_size + bsp_stack_und_size + bsp_stack_main_size;
+
+bsp_processor_count = DEFINED (bsp_processor_count) ? bsp_processor_count : 1;
+
MEMORY {
UNEXPECTED_SECTIONS : ORIGIN = 0xffffffff, LENGTH = 0
}
@@ -324,6 +328,10 @@ SECTIONS {
. = . + bsp_stack_main_size;
bsp_stack_main_end = .;
+ bsp_stack_secondary_processors_begin = .;
+ . = . + (bsp_processor_count - 1) * bsp_stack_all_size;
+ bsp_stack_secondary_processors_end = .;
+
*(.bsp_vector)
bsp_section_vector_end = .;
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am b/c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am
index ad42b2f..99ff1fb 100644
--- a/c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/Makefile.am
@@ -35,6 +35,7 @@ include_bsp_HEADERS += ../../shared/tod.h
include_bsp_HEADERS += ../shared/include/start.h
include_bsp_HEADERS += ../shared/include/arm-a9mpcore-irq.h
include_bsp_HEADERS += ../shared/include/arm-a9mpcore-regs.h
+include_bsp_HEADERS += ../shared/include/arm-a9mpcore-start.h
include_bsp_HEADERS += ../shared/include/arm-cp15-start.h
include_bsp_HEADERS += ../shared/include/arm-gic.h
include_bsp_HEADERS += ../shared/include/arm-gic-irq.h
@@ -59,6 +60,8 @@ project_lib_DATA = start.$(OBJEXT)
project_lib_DATA += startup/linkcmds
EXTRA_DIST =
+EXTRA_DIST += startup/linkcmds.xilinx_zynq_a9_qemu
+EXTRA_DIST += startup/linkcmds.xilinx_zynq_a9_qemu_smp
###############################################################################
# LibBSP #
@@ -121,6 +124,10 @@ libbsp_a_CPPFLAGS += -I$(srcdir)/../../../libcpu/arm/shared/include
# Start hooks
libbsp_a_SOURCES += startup/bspstarthooks.c
+if HAS_SMP
+libbsp_a_SOURCES += ../shared/arm-a9mpcore-smp.c
+endif
+
###############################################################################
# Special Rules #
###############################################################################
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/configure.ac b/c/src/lib/libbsp/arm/xilinx-zynq/configure.ac
index fcb77be..c739f69 100644
--- a/c/src/lib/libbsp/arm/xilinx-zynq/configure.ac
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/configure.ac
@@ -33,6 +33,9 @@ RTEMS_BSPOPTS_HELP([CLOCK_DRIVER_USE_FAST_IDLE],
occurs while the IDLE thread is executing. This can significantly reduce
simulation times.])
+RTEMS_CHECK_SMP
+AM_CONDITIONAL(HAS_SMP,[test "$rtems_cv_HAS_SMP" = "yes"])
+
RTEMS_BSP_CLEANUP_OPTIONS(0, 1)
RTEMS_BSP_LINKCMDS
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/include/bsp.h b/c/src/lib/libbsp/arm/xilinx-zynq/include/bsp.h
index a0267d4..617aed3 100644
--- a/c/src/lib/libbsp/arm/xilinx-zynq/include/bsp.h
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/include/bsp.h
@@ -31,8 +31,14 @@
extern "C" {
#endif /* __cplusplus */
+#define BSP_ARM_A9MPCORE_SCU_BASE 0xf8f00000
+
+#define BSP_ARM_GIC_CPUIF_BASE 0xf8f00100
+
#define BSP_ARM_A9MPCORE_PT_BASE 0xf8f00600
+#define BSP_ARM_GIC_DIST_BASE 0xf8f01000
+
typedef enum {
BSP_ARM_A9MPCORE_FATAL_CLOCK_IRQ_INSTALL,
BSP_ARM_A9MPCORE_FATAL_CLOCK_IRQ_REMOVE
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/include/irq.h b/c/src/lib/libbsp/arm/xilinx-zynq/include/irq.h
index 9781768..824dd4e 100644
--- a/c/src/lib/libbsp/arm/xilinx-zynq/include/irq.h
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/include/irq.h
@@ -91,10 +91,6 @@ extern "C" {
#define BSP_INTERRUPT_VECTOR_MIN 0
#define BSP_INTERRUPT_VECTOR_MAX 92
-#define BSP_ARM_GIC_CPUIF_BASE 0xf8f00100
-
-#define BSP_ARM_GIC_DIST_BASE 0xf8f01000
-
#ifdef __cplusplus
}
#endif /* __cplusplus */
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/make/custom/xilinx_zynq_a9_qemu_smp.cfg b/c/src/lib/libbsp/arm/xilinx-zynq/make/custom/xilinx_zynq_a9_qemu_smp.cfg
new file mode 100644
index 0000000..2a0e35a
--- /dev/null
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/make/custom/xilinx_zynq_a9_qemu_smp.cfg
@@ -0,0 +1 @@
+include $(RTEMS_ROOT)/make/custom/xilinx_zynq_a9_qemu.cfg
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am b/c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am
index 40a1e61..5b94e31 100644
--- a/c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/preinstall.am
@@ -94,6 +94,10 @@ $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h: ../shared/include/arm-a9mpcore-regs.
$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h
PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-regs.h
+$(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h: ../shared/include/arm-a9mpcore-start.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+ $(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-a9mpcore-start.h
+
$(PROJECT_INCLUDE)/bsp/arm-cp15-start.h: ../shared/include/arm-cp15-start.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/arm-cp15-start.h
PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/arm-cp15-start.h
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/startup/bspstarthooks.c b/c/src/lib/libbsp/arm/xilinx-zynq/startup/bspstarthooks.c
index f9fceeb..32585fe 100644
--- a/c/src/lib/libbsp/arm/xilinx-zynq/startup/bspstarthooks.c
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/startup/bspstarthooks.c
@@ -15,12 +15,14 @@
#include <bsp.h>
#include <bsp/start.h>
#include <bsp/arm-cp15-start.h>
+#include <bsp/arm-a9mpcore-start.h>
#include <bsp/linker-symbols.h>
-BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
-{
- /* Do nothing */
-}
+#ifdef RTEMS_SMP
+ #define MMU_DATA_READ_WRITE ARMV7_MMU_DATA_READ_WRITE_SHAREABLE
+#else
+ #define MMU_DATA_READ_WRITE ARMV7_MMU_DATA_READ_WRITE_CACHED
+#endif
BSP_START_DATA_SECTION static const arm_cp15_start_section_config
zynq_mmu_config_table[] = {
@@ -31,7 +33,7 @@ zynq_mmu_config_table[] = {
}, {
.begin = (uint32_t) bsp_section_fast_data_begin,
.end = (uint32_t) bsp_section_fast_data_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = (uint32_t) bsp_section_start_begin,
.end = (uint32_t) bsp_section_start_end,
@@ -39,7 +41,7 @@ zynq_mmu_config_table[] = {
}, {
.begin = (uint32_t) bsp_section_vector_begin,
.end = (uint32_t) bsp_section_vector_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = (uint32_t) bsp_section_text_begin,
.end = (uint32_t) bsp_section_text_end,
@@ -51,19 +53,19 @@ zynq_mmu_config_table[] = {
}, {
.begin = (uint32_t) bsp_section_data_begin,
.end = (uint32_t) bsp_section_data_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = (uint32_t) bsp_section_bss_begin,
.end = (uint32_t) bsp_section_bss_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = (uint32_t) bsp_section_work_begin,
.end = (uint32_t) bsp_section_work_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = (uint32_t) bsp_section_stack_begin,
.end = (uint32_t) bsp_section_stack_end,
- .flags = ARMV7_MMU_DATA_READ_WRITE_CACHED
+ .flags = MMU_DATA_READ_WRITE
}, {
.begin = 0xe0000000U,
.end = 0xe0200000U,
@@ -91,6 +93,11 @@ BSP_START_TEXT_SECTION static void setup_mmu_and_cache(void)
);
}
+BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
+{
+ arm_a9mpcore_start_hook_0();
+}
+
BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
{
bsp_start_copy_sections();
diff --git a/c/src/lib/libbsp/arm/xilinx-zynq/startup/linkcmds b/c/src/lib/libbsp/arm/xilinx-zynq/startup/linkcmds.xilinx_zynq_a9_qemu
similarity index 100%
rename from c/src/lib/libbsp/arm/xilinx-zynq/startup/linkcmds
rename to c/src/lib/libbsp/arm/xilinx-zynq/startup/linkcmds.xilinx_zynq_a9_qemu
diff --git a/c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds b/c/src/lib/libbsp/arm/xilinx-zynq/startup/linkcmds.xilinx_zynq_a9_qemu_smp
similarity index 92%
rename from c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds
rename to c/src/lib/libbsp/arm/xilinx-zynq/startup/linkcmds.xilinx_zynq_a9_qemu_smp
index 0b5e3e0..d76dd2e 100644
--- a/c/src/lib/libbsp/arm/realview-pbx-a9/startup/linkcmds
+++ b/c/src/lib/libbsp/arm/xilinx-zynq/startup/linkcmds.xilinx_zynq_a9_qemu_smp
@@ -28,4 +28,6 @@ bsp_vector_table_in_start_section = 1;
bsp_translation_table_base = ORIGIN (RAM_MMU);
+bsp_processor_count = DEFINED (bsp_processor_count) ? bsp_processor_count : 8;
+
INCLUDE linkcmds.armv4
diff --git a/c/src/lib/libcpu/arm/shared/include/arm-cp15.h b/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
index ffb2cc6..6d7cf39 100644
--- a/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
+++ b/c/src/lib/libcpu/arm/shared/include/arm-cp15.h
@@ -842,6 +842,63 @@ static inline void arm_cp15_wait_for_interrupt(void)
);
}
+static inline uint32_t arm_cp15_get_multiprocessor_affinity(void)
+{
+ ARM_SWITCH_REGISTERS;
+ uint32_t mpidr;
+
+ __asm__ volatile (
+ ARM_SWITCH_TO_ARM
+ "mrc p15, 0, %[mpidr], c0, c0, 5\n"
+ ARM_SWITCH_BACK
+ : [mpidr] "=&r" (mpidr) ARM_SWITCH_ADDITIONAL_OUTPUT
+ );
+
+ return mpidr & 0xff;
+}
+
+static inline uint32_t arm_cortex_a9_get_multiprocessor_cpu_id(void)
+{
+ return arm_cp15_get_multiprocessor_affinity() & 0xff;
+}
+
+#define ARM_CORTEX_A9_ACTL_FW (1U << 0)
+#define ARM_CORTEX_A9_ACTL_L2_PREFETCH_HINT_ENABLE (1U << 1)
+#define ARM_CORTEX_A9_ACTL_L1_PREFETCH_ENABLE (1U << 2)
+#define ARM_CORTEX_A9_ACTL_WRITE_FULL_LINE_OF_ZEROS_MODE (1U << 3)
+#define ARM_CORTEX_A9_ACTL_SMP (1U << 6)
+#define ARM_CORTEX_A9_ACTL_EXCL (1U << 7)
+#define ARM_CORTEX_A9_ACTL_ALLOC_IN_ONE_WAY (1U << 8)
+#define ARM_CORTEX_A9_ACTL_PARITY_ON (1U << 9)
+
+static inline uint32_t arm_cp15_get_auxiliary_control(void)
+{
+ ARM_SWITCH_REGISTERS;
+ uint32_t val;
+
+ __asm__ volatile (
+ ARM_SWITCH_TO_ARM
+ "mrc p15, 0, %[val], c1, c0, 1\n"
+ ARM_SWITCH_BACK
+ : [val] "=&r" (val) ARM_SWITCH_ADDITIONAL_OUTPUT
+ );
+
+ return val;
+}
+
+static inline void arm_cp15_set_auxiliary_control(uint32_t val)
+{
+ ARM_SWITCH_REGISTERS;
+
+ __asm__ volatile (
+ ARM_SWITCH_TO_ARM
+ "mcr p15, 0, %[val], c1, c0, 1\n"
+ ARM_SWITCH_BACK
+ : ARM_SWITCH_OUTPUT
+ : [val] "r" (val)
+ );
+}
+
/**
* @brief Sets the @a section_flags for the address range [@a begin, @a end).
*
More information about the vc
mailing list