From d8f4689dba6a11c2449d0e673cff5abcb9a07015 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Fri, 22 Apr 2016 14:03:43 +0200
Subject: bsp/qoriq: Add bsp_restart()

---
 .../lib/libbsp/powerpc/qoriq/startup/bsprestart.c  | 144 +++++++++++++++++++++
 1 file changed, 144 insertions(+)
 create mode 100644 c/src/lib/libbsp/powerpc/qoriq/startup/bsprestart.c

(limited to 'c/src/lib/libbsp/powerpc/qoriq/startup/bsprestart.c')

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 */
-- 
cgit v1.2.3