From 8438c8790c0ad11545371a07f0b0ce4c65c62101 Mon Sep 17 00:00:00 2001
From: Damien George <damien@micropython.org>
Date: Tue, 2 Jan 2024 14:45:44 +1100
Subject: [PATCH] rp2/mutex_extra: Implement additional mutex functions.

These allow entering/exiting a mutex and also disabling/restoring
interrupts, in an atomic way.

Signed-off-by: Damien George <damien@micropython.org>
---
 LICENSE                  |  2 ++
 ports/rp2/CMakeLists.txt |  1 +
 ports/rp2/mutex_extra.c  | 30 ++++++++++++++++++++++++++++++
 ports/rp2/mutex_extra.h  | 34 ++++++++++++++++++++++++++++++++++
 4 files changed, 67 insertions(+)
 create mode 100644 ports/rp2/mutex_extra.c
 create mode 100644 ports/rp2/mutex_extra.h

diff --git a/LICENSE b/LICENSE
index 9bd457b42a..b85f31808a 100644
--- a/LICENSE
+++ b/LICENSE
@@ -69,6 +69,8 @@ used during the build process and is not part of the compiled source code.
             /FreeRTOS (GPL-2.0 with FreeRTOS exception)
         /esp32
             /ppp_set_auth.* (Apache-2.0)
+        /rp2
+            /mutex_extra.c (BSD-3-clause)
         /stm32
             /usbd*.c (MCD-ST Liberty SW License Agreement V2)
             /stm32_it.* (MIT + BSD-3-clause)
diff --git a/ports/rp2/CMakeLists.txt b/ports/rp2/CMakeLists.txt
index 559ae23f52..281b0c3bc2 100644
--- a/ports/rp2/CMakeLists.txt
+++ b/ports/rp2/CMakeLists.txt
@@ -129,6 +129,7 @@ set(MICROPY_SOURCE_PORT
     mphalport.c
     mpnetworkport.c
     mpthreadport.c
+    mutex_extra.c
     pendsv.c
     rp2_flash.c
     rp2_pio.c
diff --git a/ports/rp2/mutex_extra.c b/ports/rp2/mutex_extra.c
new file mode 100644
index 0000000000..6df57e64ce
--- /dev/null
+++ b/ports/rp2/mutex_extra.c
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "mutex_extra.h"
+
+// These functions are taken from lib/pico-sdk/src/common/pico_sync/mutex.c and modified
+// so that they atomically obtain the mutex and disable interrupts.
+
+uint32_t __time_critical_func(mutex_enter_blocking_and_disable_interrupts)(mutex_t * mtx) {
+    lock_owner_id_t caller = lock_get_caller_owner_id();
+    do {
+        uint32_t save = spin_lock_blocking(mtx->core.spin_lock);
+        if (!lock_is_owner_id_valid(mtx->owner)) {
+            mtx->owner = caller;
+            spin_unlock_unsafe(mtx->core.spin_lock);
+            return save;
+        }
+        lock_internal_spin_unlock_with_wait(&mtx->core, save);
+    } while (true);
+}
+
+void __time_critical_func(mutex_exit_and_restore_interrupts)(mutex_t * mtx, uint32_t save) {
+    spin_lock_unsafe_blocking(mtx->core.spin_lock);
+    assert(lock_is_owner_id_valid(mtx->owner));
+    mtx->owner = LOCK_INVALID_OWNER_ID;
+    lock_internal_spin_unlock_with_notify(&mtx->core, save);
+}
diff --git a/ports/rp2/mutex_extra.h b/ports/rp2/mutex_extra.h
new file mode 100644
index 0000000000..be7cb96dcc
--- /dev/null
+++ b/ports/rp2/mutex_extra.h
@@ -0,0 +1,34 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2023 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MICROPY_INCLUDED_RP2_MUTEX_EXTRA_H
+#define MICROPY_INCLUDED_RP2_MUTEX_EXTRA_H
+
+#include "pico/mutex.h"
+
+uint32_t mutex_enter_blocking_and_disable_interrupts(mutex_t *mtx);
+void mutex_exit_and_restore_interrupts(mutex_t *mtx, uint32_t save);
+
+#endif // MICROPY_INCLUDED_RP2_MUTEX_EXTRA_H