bsp/qoriq: Add bsp_restart()

5 files changed, 237 insertions, 2 deletions

diff --git a/c/src/lib/libbsp/powerpc/qoriq/Makefile.am b/c/src/lib/libbsp/powerpc/qoriq/Makefile.am
index 2fd34e9412..25def4fe12 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/Makefile.am
+++ b/c/src/lib/libbsp/powerpc/qoriq/Makefile.am
@@ -75,9 +75,11 @@ libbsp_a_SOURCES += \
 	startup/mmu.c \
 	startup/mmu-tlb1.S \
 	startup/mmu-config.c \
+	startup/restart.S \
 	startup/bsppredriverhook.c \
 	startup/bspstart.c \
-	startup/bspreset.c
+	startup/bspreset.c \
+	startup/bsprestart.c
 
 # Clock
 libbsp_a_SOURCES += clock/clock-config.c \
diff --git a/c/src/lib/libbsp/powerpc/qoriq/include/bsp.h b/c/src/lib/libbsp/powerpc/qoriq/include/bsp.h
index 089c8ca496..d1fd907d90 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/include/bsp.h
+++ b/c/src/lib/libbsp/powerpc/qoriq/include/bsp.h
@@ -68,6 +68,8 @@ int qoriq_if_intercom_attach_detach(
   );
 #endif
 
+void bsp_restart(void *addr) RTEMS_NO_RETURN;
+
 void *bsp_idle_thread( uintptr_t ignored );
 #define BSP_IDLE_TASK_BODY bsp_idle_thread
 
@@ -98,6 +100,10 @@ qoriq_start_spin_table_addr[QORIQ_CPU_COUNT / QORIQ_THREAD_COUNT];
 
 void qoriq_start_thread(void);
 
+void qoriq_restart_secondary_processor(
+  const qoriq_start_spin_table *spin_table
+) RTEMS_NO_RETURN;
+
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */
diff --git a/c/src/lib/libbsp/powerpc/qoriq/startup/bsprestart.c b/c/src/lib/libbsp/powerpc/qoriq/startup/bsprestart.c
new file mode 100644
index 0000000000..7df5135b55
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/qoriq/startup/bsprestart.c
@@ -0,0 +1,144 @@
+/**
+ * @file
+ *
+ * @ingroup QorIQ
+ *
+ * @brief BSP restart.
+ */
+
+/*
+ * Copyright (c) 2016 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@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.org/license/LICENSE.
+ */
+
+#include <bsp.h>
+#include <bsp/irq.h>
+#include <bsp/fatal.h>
+#include <bsp/fdt.h>
+#include <bsp/linker-symbols.h>
+#include <bsp/qoriq.h>
+
+#include <libcpu/powerpc-utility.h>
+
+static RTEMS_NO_RETURN void do_restart(void *addr)
+{
+  void (*restart)(uintptr_t);
+  uintptr_t fdt;
+
+  restart = addr;
+
+  fdt = (uintptr_t) bsp_fdt_get();
+#ifdef BSP_FDT_BLOB_READ_ONLY
+  fdt -= (uintptr_t) bsp_section_rodata_begin;
+  fdt += (uintptr_t) bsp_section_rodata_load_begin;
+#endif
+
+  (*restart)(fdt);
+  bsp_fatal(QORIQ_FATAL_RESTART_FAILED);
+}
+
+#ifdef RTEMS_SMP
+
+#include <rtems/score/smpimpl.h>
+#include <rtems/score/smpbarrier.h>
+
+#define RESTART_IPI_INDEX 1
+
+static SMP_barrier_Control restart_barrier = SMP_BARRIER_CONTROL_INITIALIZER;
+
+static void restart_interrupt(void *arg)
+{
+  uint32_t cpu_self_index;
+  uint32_t thread_index;
+  rtems_interrupt_level level;
+  SMP_barrier_State bs;
+
+  rtems_interrupt_local_disable(level);
+  (void) level;
+
+  _SMP_barrier_State_initialize(&bs);
+  _SMP_barrier_Wait(&restart_barrier, &bs, _SMP_Processor_count);
+
+  cpu_self_index = rtems_get_current_processor();
+  thread_index = cpu_self_index % QORIQ_THREAD_COUNT;
+
+  if (cpu_self_index == 0) {
+    do_restart(arg);
+  } else if (thread_index == 0) {
+    uint32_t real_processor_index;
+    const qoriq_start_spin_table *spin_table;
+
+    real_processor_index = cpu_self_index / QORIQ_THREAD_COUNT;
+    spin_table = qoriq_start_spin_table_addr[real_processor_index];
+
+    qoriq_restart_secondary_processor(spin_table);
+  } else {
+    uint32_t pir_reset_value;
+
+    /* Restore reset PIR value */
+    pir_reset_value = (cpu_self_index & ~0x1U) << 2;
+    PPC_SET_SPECIAL_PURPOSE_REGISTER(BOOKE_PIR, pir_reset_value);
+
+    /* Thread Enable Clear (TENC) */
+    PPC_SET_SPECIAL_PURPOSE_REGISTER(FSL_EIS_TENC, 1U << thread_index);
+
+    RTEMS_UNREACHABLE();
+  }
+}
+
+static void raise_restart_interrupt(void)
+{
+  qoriq.pic.ipidr[RESTART_IPI_INDEX].reg = _SMP_Online_processors[0];
+  ppc_synchronize_data();
+  ppc_synchronize_instructions();
+}
+
+void bsp_restart(void *addr)
+{
+  rtems_status_code sc;
+  size_t i;
+
+  for (i = 0; i < RTEMS_ARRAY_SIZE(qoriq_start_spin_table_addr); ++i) {
+    qoriq_start_spin_table *spin_table;
+
+    spin_table = qoriq_start_spin_table_addr[i];
+    memset(spin_table, 0, sizeof(*spin_table));
+    rtems_cache_flush_multiple_data_lines(spin_table, sizeof(*spin_table));
+  }
+
+  sc = rtems_interrupt_handler_install(
+    QORIQ_IRQ_IPI_0 + RESTART_IPI_INDEX,
+    "Restart",
+    RTEMS_INTERRUPT_UNIQUE,
+    restart_interrupt,
+    addr
+  );
+  if (sc != RTEMS_SUCCESSFUL) {
+    bsp_fatal(QORIQ_FATAL_RESTART_INSTALL_INTERRUPT);
+  }
+
+  raise_restart_interrupt();
+  bsp_fatal(QORIQ_FATAL_RESTART_INTERRUPT_FAILED);
+}
+
+#else /* RTEMS_SMP */
+
+void bsp_restart(void *addr)
+{
+  rtems_interrupt_level level;
+
+  rtems_interrupt_local_disable(level);
+  (void) level;
+  do_restart(addr);
+}
+
+#endif /* !RTEMS_SMP */
diff --git a/c/src/lib/libbsp/powerpc/qoriq/startup/restart.S b/c/src/lib/libbsp/powerpc/qoriq/startup/restart.S
new file mode 100644
index 0000000000..7dd9eb198d
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/qoriq/startup/restart.S
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2016 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@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.org/license/LICENSE.
+ */
+
+#include <libcpu/powerpc-utility.h>
+
+#define FIRST_TLB 0
+
+#define SCRATCH_TLB QORIQ_TLB1_ENTRY_COUNT - 1
+
+	.global qoriq_restart_secondary_processor
+
+	.section ".bsp_start_text", "ax"
+
+qoriq_restart_secondary_processor:
+
+	mr	r14, r3
+
+	/* Invalidate all TS1 MMU entries */
+	li	r3, 1
+	bl	qoriq_tlb1_invalidate_all_by_ts
+
+	/* Add TS1 entry for the first 4GiB of RAM */
+	li	r3, SCRATCH_TLB
+	li	r4, FSL_EIS_MAS1_TS
+	li	r5, FSL_EIS_MAS2_I
+	li	r6, FSL_EIS_MAS3_SR | FSL_EIS_MAS3_SW | FSL_EIS_MAS3_SX
+	li	r7, 0
+	li	r8, 0
+	li	r9, 11
+	bl	qoriq_tlb1_write
+
+	bl	qoriq_l1cache_invalidate
+
+	/* Set MSR and use TS1 for address translation */
+	LWI	r0, QORIQ_INITIAL_MSR | MSR_IS | MSR_DS
+	mtmsr	r0
+	isync
+
+	/* Invalidate all TS0 MMU entries */
+	li	r3, 0
+	bl	qoriq_tlb1_invalidate_all_by_ts
+
+	/* Add TS0 entry for the first 4GiB of RAM */
+	li	r3, FIRST_TLB
+	li	r4, 0
+	li	r5, FSL_EIS_MAS2_I
+	li	r6, FSL_EIS_MAS3_SR | FSL_EIS_MAS3_SW | FSL_EIS_MAS3_SX
+	li	r7, 0
+	li	r8, 0
+	li	r9, 11
+	bl	qoriq_tlb1_write
+
+	/* Use TS0 for address translation */
+	LWI	r0, QORIQ_INITIAL_MSR
+	mtmsr	r0
+	isync
+
+	bl	qoriq_l1cache_invalidate
+
+	/* Wait for restart request */
+	li	r0, 0
+.Lrestartagain:
+	lwz	r4, 4(r14)
+	cmpw	r0, r4
+	beq	.Lrestartagain
+	isync
+	mtctr	r4
+	lwz	r3, 12(r14)
+	bctr
diff --git a/c/src/lib/libbsp/shared/include/fatal.h b/c/src/lib/libbsp/shared/include/fatal.h
index 25dda3933a..783d2b1754 100644
--- a/c/src/lib/libbsp/shared/include/fatal.h
+++ b/c/src/lib/libbsp/shared/include/fatal.h
@@ -118,7 +118,10 @@ typedef enum {
   QORIQ_FATAL_SMP_IPI_HANDLER_INSTALL = BSP_FATAL_CODE_BLOCK(10),
   QORIQ_FATAL_FDT_NO_BUS_FREQUENCY,
   QORIQ_FATAL_FDT_NO_CLOCK_FREQUENCY,
-  QORIQ_FATAL_FDT_NO_TIMEBASE_FREQUENCY
+  QORIQ_FATAL_FDT_NO_TIMEBASE_FREQUENCY,
+  QORIQ_FATAL_RESTART_FAILED,
+  QORIQ_FATAL_RESTART_INSTALL_INTERRUPT,
+  QORIQ_FATAL_RESTART_INTERRUPT_FAILED
 } bsp_fatal_code;
 
 RTEMS_NO_RETURN static inline void