[fiber] Adapt for multicore processing

Author: andryblack

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

@@ -9,15 +9,22 @@
  * file, You can obtain one at http://mozilla.org/MPL/2.0/.
  */
 // ----------------------------------------------------------------------------
+%% if multicore
+%% set CoreDecl = ", size_t core=0"
+%% set CoreArg = ", size_t core"
+%% set CoreForward = ", core"
+%% else
+%% set CoreDecl = ""
+%% set CoreArg = ""
+%% set CoreForward = ""
+%% endif
 
 #pragma once
 #include "context.h"
 #include "stack.hpp"
 #include <memory>
-
 namespace modm
 {
-
 namespace fiber
 {
 
@@ -50,11 +57,11 @@ class Fiber
 
 public:
 	template<size_t Size>
-	Fiber(fiber::Stack<Size>& stack, void(*fn)());
+	Fiber(fiber::Stack<Size>& stack, void(*fn)(){{CoreDecl}});
 
 	template<size_t Size, class T>
 	requires requires { &std::decay_t<T>::operator(); }
-	Fiber(fiber::Stack<Size>& stack, T&& closure);
+	Fiber(fiber::Stack<Size>& stack, T&& closure{{CoreDecl}});
 
 protected:
 	inline void
@@ -75,21 +82,20 @@ class Fiber
 
 namespace modm
 {
-
 template<size_t Size>
-Fiber::Fiber(fiber::Stack<Size>& stack, void(*fn)())
+Fiber::Fiber(fiber::Stack<Size>& stack, void(*fn)(){{CoreArg}})
 {
 	ctx = modm_context_init((uintptr_t) stack.memory,
 							(uintptr_t) stack.memory + stack.size,
 							(uintptr_t) fn,
 							(uintptr_t) fiber::Scheduler::deregisterFiber);
 	// register this fiber to be scheduled
-	fiber::Scheduler::registerFiber(this);
+	fiber::Scheduler::registerFiber(this{{CoreForward}});
 }
 
 template<size_t Size, class T>
 requires requires { &std::decay_t<T>::operator(); }
-Fiber::Fiber(fiber::Stack<Size>& stack, T&& closure)
+Fiber::Fiber(fiber::Stack<Size>& stack, T&& closure{{CoreArg}})
 {
 	// Find a suitable aligned area at the top of stack to allocate the closure
 	uintptr_t ptr = uintptr_t(stack.memory) + stack.size;
@@ -106,14 +112,13 @@ Fiber::Fiber(fiber::Stack<Size>& stack, T&& closure)
 	ctx = modm_context_init((uintptr_t) stack.memory, ptr, function,
 							(uintptr_t) fiber::Scheduler::deregisterFiber);
 	// register this fiber to be scheduled
-	fiber::Scheduler::registerFiber(this);
+	fiber::Scheduler::registerFiber(this{{CoreForward}});
 }
 
 void
 Fiber::jump(Fiber& other)
 {
-	fiber::Scheduler::current = &other;
-	modm_context_jump(&(ctx.sp), other.ctx.sp);
+	fiber::Scheduler::jump(*this,other);
 }
 
 }	// namespace modm

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,13 @@ 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
+    env.template("fiber.hpp.in")
+    env.template("scheduler.hpp.in")
 
     env.template("stack.hpp.in")
 

src/modm/processing/fiber/scheduler.hpp

@@ -0,0 +1,179 @@
+/*
+ * Copyright (c) 2020, Erik Henriksson
+ * 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/.
+ */
+// ----------------------------------------------------------------------------
+
+#pragma once
+
+#include "fiber.hpp"
+%% if multicore
+#include <modm/platform/core/multicore.hpp>
+%% endif
+
+namespace modm::fiber
+{
+
+void yield();
+
+class Scheduler
+{
+	friend class ::modm::Fiber;
+	friend class Waitable;
+	friend void yield();
+	Scheduler(const Scheduler&) = delete;
+	Scheduler() = delete;
+
+protected:
+	struct Data
+	{
+		/// Last fiber in the ready queue.
+		Fiber* last = nullptr;
+		/// Current running fiber
+		Fiber* current = nullptr;
+
+		inline void registerFiber(Fiber* fiber)
+		{
+			if (last == nullptr)
+			{
+				fiber->next = fiber;
+				last = fiber;
+				return;
+			}
+			runLast(fiber);
+		}
+		inline void runLast(Fiber* fiber)
+		{
+			fiber->next = last->next;
+			last->next = fiber;
+			last = fiber;
+		}
+		inline Fiber* removeCurrent()
+		{
+			if (current == last) last = nullptr;
+			else last->next = current->next;
+			current->next = nullptr;
+			return current;
+		}
+		inline bool empty() const
+		{
+			return last == nullptr;
+		}
+		inline void jump(Fiber& other)
+		{
+			auto from = current;
+			current = &other;
+			modm_context_jump(&(from->ctx.sp), other.ctx.sp);
+		}
+	};
+%% if multicore
+	%% for i in range(num_cores)
+	modm_core{{i}}_bss static inline Data data{{i}} = {nullptr, nullptr};
+	%% endfor
+	static constexpr Data* lookup[] = {
+	%% for i in range(num_cores)
+		&data{{i}},
+	%% endfor
+	};
+	static inline Data& getData()
+	{ return *lookup[::modm::platform::multicore::Core::cpuId()]; }
+%% else
+	static inline Data data = {nullptr,nullptr};
+	static inline Data& getData() { return data; }
+%% endif
+	static inline void jump(Fiber& from, Fiber& to)
+	{
+		auto& d = getData();
+		d.current = &to;
+		modm_context_jump(&(from.ctx.sp), to.ctx.sp);
+	}
+public:
+	// Should be called by the main() function.
+	static inline bool
+	run()
+	{
+		auto& d = getData();
+		if (d.last == nullptr) return false;
+		d.current = d.last->next;
+		modm_context_start(d.current->ctx.sp);
+		return true;
+	}
+
+	static inline bool
+	empty()
+	{
+		return getData().empty();
+	}
+
+	static inline Fiber*
+	removeCurrent()
+	{
+		return getData().removeCurrent();
+	}
+
+	static inline void
+	runNext(Fiber* fiber)
+	{
+		auto& d = getData();
+		fiber->next = d.current->next;
+		d.current->next = fiber;
+	}
+
+	static inline void
+	runLast(Fiber* fiber)
+	{
+		getData().runLast(fiber);
+	}
+
+protected:
+%% if multicore
+	static inline void
+	registerFiber(Fiber* fiber, size_t core)
+	{
+		::modm::platform::multicore::SystemSpinLockGuard g;
+		if (core != ::modm::platform::multicore::Core::cpuId())
+		{
+			lookup[core]->registerFiber(fiber);
+			return;
+		}
+%% else
+	static inline void
+	registerFiber(Fiber* fiber)
+	{
+%% endif
+		getData().registerFiber(fiber);
+	}
+
+	static inline void
+	deregisterFiber()
+	{
+		auto& d = getData();
+		Fiber* next = d.current->next;
+		d.removeCurrent();
+		if (d.empty())
+		{
+			d.current = nullptr;
+			modm_context_end();
+		}
+		d.jump(*next);
+	}
+};
+
+inline void
+yield()
+{
+	auto& d = Scheduler::getData();
+	if (d.current == nullptr) return;
+	Fiber* next = d.current->next;
+	if (next == d.current) return;
+	d.last = d.current;
+	d.jump(*next);
+}
+
+}  // namespace modm::fiber