From 662dc8602bf0ab99332748d178b0e950189df9e7 Mon Sep 17 00:00:00 2001
From: Jim Mussared <jim.mussared@gmail.com>
Date: Thu, 30 Jun 2022 11:42:20 +1000
Subject: [PATCH] rp2: Make atomic sections suspend the other core (if active).

When a flash write/erase is in progress, we need to ensure that the
other core cannot be using XIP.

This also implements MICROPY_BEGIN_ATOMIC_SECTION as a full mutex, which
is necessary as it's used to syncronise access to things like the scheduler
queue.

Signed-off-by: Jim Mussared <jim.mussared@gmail.com>
---
 ports/rp2/mpconfigport.h |  9 ++++++---
 ports/rp2/mpthreadport.c | 40 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 46 insertions(+), 3 deletions(-)

diff --git a/ports/rp2/mpconfigport.h b/ports/rp2/mpconfigport.h
index ef1e60082..d022c7a28 100644
--- a/ports/rp2/mpconfigport.h
+++ b/ports/rp2/mpconfigport.h
@@ -30,6 +30,7 @@
 #include "hardware/spi.h"
 #include "hardware/sync.h"
 #include "pico/binary_info.h"
+#include "pico/multicore.h"
 #include "mpconfigboard.h"
 #if MICROPY_HW_USB_MSC
 #include "hardware/flash.h"
@@ -225,9 +226,11 @@ extern const struct _mod_network_nic_type_t mod_network_nic_type_wiznet5k;
 
 // Miscellaneous settings
 
-// TODO need to look and see if these could/should be spinlock/mutex
-#define MICROPY_BEGIN_ATOMIC_SECTION()     save_and_disable_interrupts()
-#define MICROPY_END_ATOMIC_SECTION(state)  restore_interrupts(state)
+// Entering a critical section.
+extern uint32_t mp_thread_begin_atomic_section(void);
+extern void mp_thread_end_atomic_section(uint32_t);
+#define MICROPY_BEGIN_ATOMIC_SECTION()     mp_thread_begin_atomic_section()
+#define MICROPY_END_ATOMIC_SECTION(state)  mp_thread_end_atomic_section(state)
 
 // Prevent the "lwIP task" from running when unsafe to do so.
 #define MICROPY_PY_LWIP_ENTER   lwip_lock_acquire();
diff --git a/ports/rp2/mpthreadport.c b/ports/rp2/mpthreadport.c
index 187f74bb5..252f7b01f 100644
--- a/ports/rp2/mpthreadport.c
+++ b/ports/rp2/mpthreadport.c
@@ -36,16 +36,52 @@ extern uint8_t __StackTop, __StackBottom;
 
 void *core_state[2];
 
+// This will be non-NULL while Python code is execting.
 STATIC void *(*core1_entry)(void *) = NULL;
+
 STATIC void *core1_arg = NULL;
 STATIC uint32_t *core1_stack = NULL;
 STATIC size_t core1_stack_num_words = 0;
 
+// Thread mutex.
+STATIC mp_thread_mutex_t atomic_mutex;
+
+uint32_t mp_thread_begin_atomic_section(void) {
+    if (core1_entry) {
+        // When both cores are executing, we also need to provide
+        // full mutual exclusion.
+        mp_thread_mutex_lock(&atomic_mutex, 1);
+        // In case this atomic section is for flash access, then
+        // suspend the other core.
+        multicore_lockout_start_blocking();
+    }
+
+    return save_and_disable_interrupts();
+}
+
+void mp_thread_end_atomic_section(uint32_t state) {
+    restore_interrupts(state);
+
+    if (core1_entry) {
+        multicore_lockout_end_blocking();
+        mp_thread_mutex_unlock(&atomic_mutex);
+    }
+}
+
+// Initialise threading support.
 void mp_thread_init(void) {
+    assert(get_core_num() == 0);
+
+    mp_thread_mutex_init(&atomic_mutex);
+
+    // Allow MICROPY_BEGIN_ATOMIC_SECTION to be invoked from core1.
+    multicore_lockout_victim_init();
+
     mp_thread_set_state(&mp_state_ctx.thread);
     core1_entry = NULL;
 }
 
+// Shutdown threading support -- stops the second thread.
 void mp_thread_deinit(void) {
     assert(get_core_num() == 0);
     // Must ensure that core1 is not currently holding the GC lock, otherwise
@@ -69,9 +105,13 @@ void mp_thread_gc_others(void) {
 }
 
 STATIC void core1_entry_wrapper(void) {
+    // Allow MICROPY_BEGIN_ATOMIC_SECTION to be invoked from core0.
+    multicore_lockout_victim_init();
+
     if (core1_entry) {
         core1_entry(core1_arg);
     }
+
     core1_entry = NULL;
     // returning from here will loop the core forever (WFI)
 }