From f100a58c823d39fd202a584c2fdae8b7655a98ce Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Tue, 19 Sep 2017 14:35:02 +0200
Subject: bsp/qoriq: Add hypervisor guest SMP support

Update #3085.
---
 c/src/lib/libbsp/powerpc/qoriq/irq/irq.c        | 11 +++++++
 c/src/lib/libbsp/powerpc/qoriq/start/start.S    |  8 +++++
 c/src/lib/libbsp/powerpc/qoriq/startup/bspsmp.c | 43 +++++++++++++++++++++----
 3 files changed, 55 insertions(+), 7 deletions(-)

diff --git a/c/src/lib/libbsp/powerpc/qoriq/irq/irq.c b/c/src/lib/libbsp/powerpc/qoriq/irq/irq.c
index 92d918fd4c..2af3c513eb 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/irq/irq.c
+++ b/c/src/lib/libbsp/powerpc/qoriq/irq/irq.c
@@ -35,6 +35,10 @@
 #include <bsp/utility.h>
 #include <bsp/qoriq.h>
 
+#ifdef RTEMS_SMP
+#include <rtems/score/smpimpl.h>
+#endif
+
 RTEMS_INTERRUPT_LOCK_DEFINE(static, lock, "QorIQ IRQ")
 
 #define SPURIOUS 0xffff
@@ -94,6 +98,13 @@ void bsp_interrupt_dispatch(uintptr_t exception_number)
 		return;
 	}
 
+#ifdef RTEMS_SMP
+	if (exception_number == 36) {
+		_SMP_Inter_processor_interrupt_handler();
+		return;
+	}
+#endif
+
 	ev_int_iack(0, &vector);
 
 	if (vector != SPURIOUS) {
diff --git a/c/src/lib/libbsp/powerpc/qoriq/start/start.S b/c/src/lib/libbsp/powerpc/qoriq/start/start.S
index d5a2ceb474..2d275e3c2b 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/start/start.S
+++ b/c/src/lib/libbsp/powerpc/qoriq/start/start.S
@@ -489,10 +489,18 @@ bsp_exc_vector_base:
 	START_NOP_FOR_LINKER_TOC_POINTER_RESTORE
 #ifdef __PPC_CPU_E6500__
 	/* Processor doorbell interrupt */
+#if defined(QORIQ_IS_HYPERVISOR_GUEST) && defined(RTEMS_SMP)
+	PPC_REG_STORE_UPDATE	r1, -PPC_EXC_INTERRUPT_FRAME_SIZE(r1)
+#else
 	PPC_REG_STORE_UPDATE	r1, -EXC_GENERIC_SIZE(r1)
+#endif
 	PPC_REG_STORE	r3, GPR3_OFFSET(r1)
 	li	r3, 36
+#if defined(QORIQ_IS_HYPERVISOR_GUEST) && defined(RTEMS_SMP)
+	b	ppc_exc_interrupt
+#else
 	b	ppc_exc_fatal_normal
+#endif
 	START_NOP_FOR_LINKER_TOC_POINTER_RESTORE
 	/* Processor doorbell critical interrupt */
 	PPC_REG_STORE_UPDATE	r1, -EXC_GENERIC_SIZE(r1)
diff --git a/c/src/lib/libbsp/powerpc/qoriq/startup/bspsmp.c b/c/src/lib/libbsp/powerpc/qoriq/startup/bspsmp.c
index b99a5b4f65..a2d9fbede5 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/startup/bspsmp.c
+++ b/c/src/lib/libbsp/powerpc/qoriq/startup/bspsmp.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, 2016 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2013, 2017 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -38,12 +38,6 @@ void _start_secondary_processor(void);
 
 #define IPI_INDEX 0
 
-#define TLB_BEGIN (3 * QORIQ_TLB1_ENTRY_COUNT / 4)
-
-#define TLB_END QORIQ_TLB1_ENTRY_COUNT
-
-#define TLB_COUNT (TLB_END - TLB_BEGIN)
-
 #if QORIQ_THREAD_COUNT > 1
 static bool is_started_by_u_boot(uint32_t cpu_index)
 {
@@ -104,10 +98,32 @@ void bsp_start_on_secondary_processor(void)
   _SMP_Start_multitasking_on_secondary_processor();
 }
 
+#ifndef QORIQ_IS_HYPERVISOR_GUEST
 static void bsp_inter_processor_interrupt(void *arg)
 {
   _SMP_Inter_processor_interrupt_handler();
 }
+#endif
+
+static void setup_boot_page(void)
+{
+#ifdef QORIQ_IS_HYPERVISOR_GUEST
+  qoriq_mmu_context mmu_context;
+
+  qoriq_mmu_context_init(&mmu_context);
+  qoriq_mmu_add(
+    &mmu_context,
+    0xfffff000,
+    0xffffffff,
+    0,
+    0,
+    FSL_EIS_MAS3_SR | FSL_EIS_MAS3_SW,
+    0
+  );
+  qoriq_mmu_partition(&mmu_context, 1);
+  qoriq_mmu_write_to_tlb1(&mmu_context, QORIQ_TLB1_ENTRY_COUNT - 1);
+#endif
+}
 
 static uint32_t discover_processors(void)
 {
@@ -139,6 +155,7 @@ uint32_t _CPU_SMP_Initialize(void)
   uint32_t cpu_count = 1;
 
   if (rtems_configuration_get_maximum_processors() > 0) {
+    setup_boot_page();
     cpu_count = discover_processors();
   }
 
@@ -196,6 +213,9 @@ bool _CPU_SMP_Start_processor(uint32_t cpu_index)
 
 void _CPU_SMP_Finalize_initialization(uint32_t cpu_count)
 {
+#ifdef QORIQ_IS_HYPERVISOR_GUEST
+  (void) cpu_count;
+#else
   if (cpu_count > 1) {
     rtems_status_code sc;
 
@@ -210,6 +230,7 @@ void _CPU_SMP_Finalize_initialization(uint32_t cpu_count)
       bsp_fatal(QORIQ_FATAL_SMP_IPI_HANDLER_INSTALL);
     }
   }
+#endif
 }
 
 void _CPU_SMP_Prepare_start_multitasking(void)
@@ -219,5 +240,13 @@ void _CPU_SMP_Prepare_start_multitasking(void)
 
 void _CPU_SMP_Send_interrupt(uint32_t target_processor_index)
 {
+#ifdef QORIQ_IS_HYPERVISOR_GUEST
+  uint32_t msg;
+
+  /* DBELL message type */
+  msg = (0U << (63 - 36)) | target_processor_index;
+  __asm__ volatile ("msgsnd %0" : : "r" (msg));
+#else
   qoriq.pic.ipidr [IPI_INDEX].reg = 1U << target_processor_index;
+#endif
 }
-- 
cgit v1.2.3