From 491356989599a2b027b47fc29c9a3baf4f3383eb Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Fri, 9 Apr 2021 09:06:22 +0000
Subject: [PATCH] Cross-core signaling now apparently works.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@14156 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 demos/RP/RT-RP2040-PICO/c1_main.c    | 21 +++++++++++++++++++++
 demos/RP/RT-RP2040-PICO/main.c       | 20 ++++++++++++++++----
 os/common/ports/ARMv6-M-RP2/chcore.c | 19 +++++++++++++++----
 os/common/ports/ARMv6-M-RP2/chcore.h | 10 ++++------
 4 files changed, 56 insertions(+), 14 deletions(-)

diff --git a/demos/RP/RT-RP2040-PICO/c1_main.c b/demos/RP/RT-RP2040-PICO/c1_main.c
index 2108e0cdc..af22eb120 100644
--- a/demos/RP/RT-RP2040-PICO/c1_main.c
+++ b/demos/RP/RT-RP2040-PICO/c1_main.c
@@ -65,6 +65,22 @@ static const os_instance_config_t core1_cfg = {
 #endif
 };
 
+
+/*
+ * Green LED blinker thread, times are in milliseconds.
+ */
+static THD_WORKING_AREA(waThreadTimer, 128);
+static THD_FUNCTION(ThreadTimer, arg) {
+  extern semaphore_t blinker_sem;
+
+  (void)arg;
+  chRegSetThreadName("timer");
+  while (true) {
+    chThdSleepMilliseconds(500);
+    chSemSignal(&blinker_sem);
+  }
+}
+
 /**
  * Core 1 entry point.
  */
@@ -90,6 +106,11 @@ void c1_main(void) {
   sioStart(&SIOD1, NULL);
   sioStartOperation(&SIOD1, NULL);
 
+  /*
+   * Creates the timer thread.
+   */
+  chThdCreateStatic(waThreadTimer, sizeof(waThreadTimer), NORMALPRIO, ThreadTimer, NULL);
+
   /*
    * Shell manager initialization.
    */
diff --git a/demos/RP/RT-RP2040-PICO/main.c b/demos/RP/RT-RP2040-PICO/main.c
index 18ccd4fd5..715e2ab41 100644
--- a/demos/RP/RT-RP2040-PICO/main.c
+++ b/demos/RP/RT-RP2040-PICO/main.c
@@ -17,6 +17,8 @@
 #include "ch.h"
 #include "hal.h"
 
+semaphore_t blinker_sem;
+
 /*
  * Green LED blinker thread, times are in milliseconds.
  */
@@ -26,10 +28,8 @@ static THD_FUNCTION(Thread1, arg) {
   (void)arg;
   chRegSetThreadName("blinker");
   while (true) {
-    palClearLine(25U);
-    chThdSleepMilliseconds(500);
-    palSetLine(25U);
-    chThdSleepMilliseconds(500);
+    chSemWait(&blinker_sem);
+    palToggleLine(25U);
   }
 }
 
@@ -43,6 +43,15 @@ static void start_core1(void) {
                              (uint32_t)_crt0_c1_entry};
   unsigned seq;
 
+#if 0
+  /* Resetting core1.*/
+  PSM_SET->FRCE_OFF = PSM_ANY_PROC1;
+  while ((PSM->FRCE_OFF & PSM_ANY_PROC1) == 0U) {
+  }
+  PSM_CLR->FRCE_OFF = PSM_ANY_PROC1;
+#endif
+
+  /* Starting core 1.*/
   seq = 0;
   do {
     uint32_t response;
@@ -77,7 +86,9 @@ int main(void) {
   /*
    * Starting core 1 after performing all OS-related initializations.
    */
+  chSysSuspend();
   start_core1();
+  chSysEnable();
 
   /*
    * Setting up GPIOs.
@@ -87,6 +98,7 @@ int main(void) {
   /*
    * Creates the blinker thread.
    */
+  chSemObjectInit(&blinker_sem, 0);
   chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
 
   /*
diff --git a/os/common/ports/ARMv6-M-RP2/chcore.c b/os/common/ports/ARMv6-M-RP2/chcore.c
index 06f524cc3..13a98ac25 100644
--- a/os/common/ports/ARMv6-M-RP2/chcore.c
+++ b/os/common/ports/ARMv6-M-RP2/chcore.c
@@ -110,10 +110,14 @@ void PendSV_Handler(void) {
  *
  * @isr
  */
-CH_IRQ_HANDLER(RP_SIO_IRQ_PROC0_HANDLER) {
+CH_IRQ_HANDLER(Vector7C) {
 
   CH_IRQ_PROLOGUE();
 
+  /* Error flags cleared and ignored.*/
+  SIO->FIFO_ST = SIO_FIFO_ST_ROE | SIO_FIFO_ST_WOF;
+
+  /* Read FIFO is fully emptied.*/
   while ((SIO->FIFO_ST & SIO_FIFO_ST_VLD) != 0U) {
     uint32_t message = SIO->FIFO_RD;
 #if defined(PORT_HANDLE_FIFO_MESSAGE)
@@ -136,10 +140,14 @@ CH_IRQ_HANDLER(RP_SIO_IRQ_PROC0_HANDLER) {
  *
  * @isr
  */
-CH_IRQ_HANDLER(RP_SIO_IRQ_PROC1_HANDLER) {
+CH_IRQ_HANDLER(Vector80) {
 
   CH_IRQ_PROLOGUE();
 
+  /* Error flags cleared and ignored.*/
+  SIO->FIFO_ST = SIO_FIFO_ST_ROE | SIO_FIFO_ST_WOF;
+
+  /* Read FIFO is fully emptied.*/
   while ((SIO->FIFO_ST & SIO_FIFO_ST_VLD) != 0U) {
     uint32_t message = SIO->FIFO_RD;
 #if defined(PORT_HANDLE_FIFO_MESSAGE)
@@ -180,11 +188,14 @@ void port_init(os_instance_t *oip) {
 
 #if CH_CFG_SMP_MODE != FALSE
   /* FIFO handlers for each core.*/
+  SIO->FIFO_ST = SIO_FIFO_ST_ROE | SIO_FIFO_ST_WOF;
   if (core_id == 0U) {
-    NVIC_SetPriority(15, CORTEX_MAX_KERNEL_PRIORITY);
+    NVIC_SetPriority(15, CORTEX_MINIMUM_PRIORITY);
+    NVIC_EnableIRQ(15);
   }
   else if (core_id == 1U) {
-    NVIC_SetPriority(16, CORTEX_MAX_KERNEL_PRIORITY);
+    NVIC_SetPriority(16, CORTEX_MINIMUM_PRIORITY);
+    NVIC_EnableIRQ(16);
   }
   else {
     chDbgAssert(false, "unexpected core id");
diff --git a/os/common/ports/ARMv6-M-RP2/chcore.h b/os/common/ports/ARMv6-M-RP2/chcore.h
index f17e97f8a..8381b4094 100644
--- a/os/common/ports/ARMv6-M-RP2/chcore.h
+++ b/os/common/ports/ARMv6-M-RP2/chcore.h
@@ -520,13 +520,11 @@ __STATIC_INLINE void port_notify_instance(os_instance_t *oip) {
 
   (void)oip;
 
-  /* Waiting for space into the FIFO.*/
-  while ((SIO->FIFO_ST & SIO_FIFO_ST_RDY) == 0U) {
-    __WFE();
+  /* Sending a reschedule order to the other core if there is space in
+     the FIFO.*/
+  if ((SIO->FIFO_ST & SIO_FIFO_ST_RDY) != 0U) {
+    SIO->FIFO_WR = PORT_FIFO_RESCHEDULE_MESSAGE;
   }
-
-  /* Sending a reschedule order to the other core.*/
-  SIO->FIFO_WR = PORT_FIFO_RESCHEDULE_MESSAGE;
 }
 
 /**