[rp2040] run fibers on each core

Author: andryblack

examples/rp_pico/fiber/main.cpp

@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2022, Andrey Kunitsyn
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+
+#include <modm/board.hpp>
+#include <modm/debug/logger.hpp>
+#include <modm/processing.hpp>
+#include <modm/platform/core/multicore.hpp>
+#include <mutex>
+
+using namespace Board;
+using namespace std::chrono_literals;
+
+// Create an IODeviceWrapper around the Uart Peripheral we want to use
+modm::IODeviceWrapper< Uart0, modm::IOBuffer::BlockIfFull > loggerDevice;
+
+// Set all four logger streams to use the UART
+modm::log::Logger modm::log::debug(loggerDevice);
+modm::log::Logger modm::log::info(loggerDevice);
+modm::log::Logger modm::log::warning(loggerDevice);
+modm::log::Logger modm::log::error(loggerDevice);
+
+constexpr uint32_t cycles = 1'000'000;
+
+static modm::platform::multicore::Mutex log_mutex;
+
+struct CoreData {
+	uint32_t total_counter = 0;
+	uint32_t f1counter = 0;
+	uint32_t f2counter = 0;
+};
+
+
+void
+fiber_function1(CoreData& d)
+{
+	while (++d.f1counter < cycles) { modm::fiber::yield(); d.total_counter++; }
+}
+
+void
+fiber_function2(CoreData& d)
+{
+	while (++d.f2counter < cycles) { modm::fiber::yield(); d.total_counter++; }
+}
+
+// put cores to mostly equalent environment
+modm_section(".scratch_y_data") static CoreData d0;
+modm_section(".scratch_x_data") static CoreData d1;
+
+
+modm_section(".scratch_y_noinit") modm::fiber::Stack<384> stack01;
+modm_section(".scratch_y_noinit") modm::fiber::Stack<384> stack02;
+modm_section(".scratch_x_noinit") modm::fiber::Stack<384> stack11;
+modm_section(".scratch_x_noinit") modm::fiber::Stack<384> stack12;
+
+modm_section(".scratch_y_data")  modm::Fiber<0> fiber01(stack01, [](){ fiber_function1(d0); });
+modm_section(".scratch_y_data")  modm::Fiber<0> fiber02(stack02, [](){ fiber_function2(d0); });
+modm_section(".scratch_x_data")  modm::Fiber<1> fiber11(stack11, [](){ fiber_function1(d1); });
+modm_section(".scratch_x_data")  modm::Fiber<1> fiber12(stack12, [](){ fiber_function2(d1); });
+
+template <typename TimeDiff>
+static void print_result(const CoreData& d,TimeDiff diff) {
+	std::lock_guard<modm::platform::multicore::Mutex> g(log_mutex);
+	MODM_LOG_INFO << "Benchmark for core" << multicore::Core::getNum() << " done!" << modm::endl;
+	MODM_LOG_INFO << "Executed " << d.total_counter << " yields in " << diff << modm::endl;
+	MODM_LOG_INFO << ((d.total_counter * 1'000'000ull) / std::chrono::microseconds(diff).count());
+	MODM_LOG_INFO << " yields per second, ";
+	MODM_LOG_INFO << (std::chrono::nanoseconds(diff).count() / d.total_counter);
+	MODM_LOG_INFO << "ns per yield" << modm::endl;
+	MODM_LOG_INFO.flush();
+}
+
+void core1_main() {
+	const modm::PreciseTimestamp start = modm::PreciseClock::now();
+	modm::fiber::Scheduler<1>::run();
+	const auto diff = (modm::PreciseClock::now() - start);
+
+	print_result(d1,diff);
+	while (true) {}
+}
+
+int
+main()
+{
+	Board::initialize();
+
+	// initialize Uart0 for MODM_LOG_*
+	Uart0::connect<GpioOutput0::Tx>();
+	Uart0::initialize<Board::SystemClock, 115200_Bd>();
+
+	MODM_LOG_INFO << "Starting fiber modm::yield benchmark..." << modm::endl;
+	MODM_LOG_INFO.flush();
+
+	modm::platform::multicore::Core1::run(core1_main);
+
+	const modm::PreciseTimestamp start = modm::PreciseClock::now();
+	modm::fiber::Scheduler<0>::run();
+	const auto diff = (modm::PreciseClock::now() - start);
+
+	print_result(d0,diff);
+	while (true) {}
+
+	return 0;
+}

examples/rp_pico/fiber/project.xml

@@ -0,0 +1,14 @@
+<library>
+  <extends>modm:rp-pico</extends>
+  <options>
+    <option name="modm:build:build.path">../../../build/rp_pico/fiber</option>
+    <option name="modm:__fibers">yes</option>
+  </options>
+  <modules>
+    <module>modm:debug</module>
+    <module>modm:platform:uart:0</module>
+    <module>modm:build:scons</module>
+    <module>modm:processing:timer</module>
+    <module>modm:processing:fiber</module>
+  </modules>
+</library>

src/modm/board/rp_pico/board.xml

@@ -8,6 +8,8 @@
   <options>
     <option name="modm:target">rp2040</option>
     <option name="modm:platform:core:boot2">w25q080</option>
+    <option name="modm:platform:multicore:core_storage.0">modm_section(".scratch_y_data")</option>
+    <option name="modm:platform:multicore:core_storage.1">modm_section(".scratch_x_data")</option>
   </options>
   <modules>
     <module>modm:board:rp-pico</module>

src/modm/platform/core/rp/flash.ld.in

@@ -58,14 +58,45 @@ ASSERT(__boot2_end__ - __boot2_start__ == 256,
 
 %% macro section_mem_bank(memory)
     /* Sections in {{memory}} */
-    .{{memory | lower}} :
+    .{{memory | lower}}.data :
     {
-        __{{memory | lower}}_start__ = .;
-         *(.{{memory | lower}}.*)
+        __{{memory | lower}}_data_load = LOADADDR(.{{memory | lower}}.data);
+        __{{memory | lower}}_data_start = .;
+         *(.{{memory | lower}}_data)
+         *(.{{memory | lower}}_data.*)
         . = ALIGN(4);
-        __{{memory | lower}}_end__ = .;
+        __{{memory | lower}}_data_end = .;
     } >{{memory}} AT > FLASH
-    __{{memory | lower}}_source__ = LOADADDR(.{{memory | lower}});
+    %% do table_copy.append( memory.lower() + '_data' )
+
+    .{{memory | lower}}.text :
+    {
+        __{{memory | lower}}_text_load = LOADADDR(.{{memory | lower}}.text);
+        __{{memory | lower}}_text_start = .;
+         *(.{{memory | lower}}_text)
+         *(.{{memory | lower}}_text.*)
+        . = ALIGN(4);
+        __{{memory | lower}}_text_end = .;
+    } >{{memory}} AT > FLASH
+    %% do table_copy.append( memory.lower() + '_text' )
+
+    .{{memory | lower}}.bss (NOLOAD) :
+    {
+        __{{memory | lower}}_bss_start = . ;
+        *(.{{memory | lower}}_bss)
+        *(.{{memory | lower}}_bss.*)
+        . = ALIGN(4);
+        __{{memory | lower}}_bss_end = . ;
+    } >{{memory}}
+    %% do table_zero.append(memory.lower() + "_bss")
+    .{{memory | lower}}.noinit (NOLOAD) :
+    {
+        __{{memory | lower}}_noinit_start = . ;
+        *(.{{memory | lower}}_noinit)
+        *(.{{memory | lower}}_noinit.*)
+        . = ALIGN(4);
+        __{{memory | lower}}_noinit_end = . ;
+    } >{{memory}}
 %% endmacro
 
 {{ section_mem_bank("SCRATCH_X") }}

src/modm/platform/core/rp/multicore.lb

@@ -19,6 +19,12 @@ def init(module):
 def prepare(module, options):
     if options[":target"].identifier.platform != "rp":
         return False
+    for i in range(int(options[":target"].identifier.cores)):
+        module.add_option(
+            StringOption(
+                name="core_storage.{}".format(i),
+                description="Storage for Core{}".format(i),
+                default=''))
     return True
 
 

src/modm/processing/fiber/fiber.hpp renamed to src/modm/processing/fiber/fiber.hpp.in

@@ -15,6 +15,14 @@
 #include "stack.hpp"
 #include <memory>
 
+%% if multicore
+%% set Instance = "<Core>"
+%% set TemplateInstance = "template <size_t Core>"
+%% else
+%% set Instance = ""
+%% set TemplateInstance = ""
+%% endif
+
 namespace modm
 {
 
@@ -23,6 +31,11 @@ namespace fiber
 
 void yield();
 
+%% if multicore
+template <size_t>
+void yieldCore();
+template <size_t>
+%% endif
 class Scheduler;
 class Waitable;
 template<class Data_t>
@@ -40,13 +53,20 @@ class Channel;
  * @author Niklas Hauser
  * @ingroup	modm_processing_fiber
  */
+%% if multicore
+template <size_t Core = 0>
+%% endif
 class Fiber
 {
-	friend class fiber::Scheduler;
+	friend class fiber::Scheduler{{Instance}};
 	template<class>
 	friend class fiber::Channel;
 	friend class fiber::Waitable;
+%% if multicore
+	friend void fiber::yieldCore<Core>();
+%% else
 	friend void fiber::yield();
+%% endif
 
 public:
 	template<size_t Size>
@@ -75,21 +95,22 @@ class Fiber
 
 namespace modm
 {
-
+{{TemplateInstance}}
 template<size_t Size>
-Fiber::Fiber(fiber::Stack<Size>& stack, void(*fn)())
+Fiber{{Instance}}::Fiber(fiber::Stack<Size>& stack, void(*fn)())
 {
 	ctx = modm_context_init((uintptr_t) stack.memory,
 							(uintptr_t) stack.memory + stack.size,
 							(uintptr_t) fn,
-							(uintptr_t) fiber::Scheduler::deregisterFiber);
+							(uintptr_t) fiber::Scheduler{{Instance}}::deregisterFiber);
 	// register this fiber to be scheduled
-	fiber::Scheduler::registerFiber(this);
+	fiber::Scheduler{{Instance}}::registerFiber(this);
 }
 
+{{TemplateInstance}}
 template<size_t Size, class T>
 requires requires { &std::decay_t<T>::operator(); }
-Fiber::Fiber(fiber::Stack<Size>& stack, T&& closure)
+Fiber{{Instance}}::Fiber(fiber::Stack<Size>& stack, T&& closure)
 {
 	// Find a suitable aligned area at the top of stack to allocate the closure
 	uintptr_t ptr = uintptr_t(stack.memory) + stack.size;
@@ -104,15 +125,16 @@ Fiber::Fiber(fiber::Stack<Size>& stack, T&& closure)
 	auto function = (uintptr_t) +[](std::decay_t<T>* closure) { (*closure)(); };
 	// format the stack below the allocated closure
 	ctx = modm_context_init((uintptr_t) stack.memory, ptr, function,
-							(uintptr_t) fiber::Scheduler::deregisterFiber);
+							(uintptr_t) fiber::Scheduler{{Instance}}::deregisterFiber);
 	// register this fiber to be scheduled
-	fiber::Scheduler::registerFiber(this);
+	fiber::Scheduler{{Instance}}::registerFiber(this);
 }
 
+{{TemplateInstance}}
 void
-Fiber::jump(Fiber& other)
+Fiber{{Instance}}::jump(Fiber& other)
 {
-	fiber::Scheduler::current = &other;
+	fiber::Scheduler{{Instance}}::current = &other;
 	modm_context_jump(&(ctx.sp), other.ctx.sp);
 }
 

src/modm/processing/fiber/module.lb

@@ -29,9 +29,6 @@ def build(env):
     env.outbasepath = "modm/src/modm/processing/fiber"
     env.copy("../fiber.hpp")
 
-    env.copy("fiber.hpp")
-    env.copy("scheduler.hpp")
-
     env.copy("context.h")
 
     core = env[":target"].get_driver("core")["type"]
@@ -41,7 +38,17 @@ def build(env):
         "with_fpu": env.get(":platform:cortex-m:float-abi", "soft") != "soft",
         "with_windows": env[":target"].identifier.family == "windows",
         "target": env[":target"].identifier,
+        "multicore": env.has_module(":platform:multicore"),
     }
+    if env.has_module(":platform:multicore"):
+        cores = int(env[":target"].identifier.cores)
+        env.substitutions["num_cores"] = cores
+        core_storage = []
+        for i in range(cores):
+            core_storage.append(env.get(":platform:multicore:core_storage.{}".format(i),""))
+        env.substitutions["core_storage"] = core_storage
+    env.template("fiber.hpp.in")
+    env.template("scheduler.hpp.in")
 
     env.template("stack.hpp.in")