Scheduler library for Arduino Due to the new format

libraries/Scheduler/src/Scheduler.cpp

@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "Scheduler.h"
+
+extern "C" {
+
+#define NUM_REGS 10	// r4-r11, sp, pc
+
+typedef struct CoopTask {
+	uint32_t regs[NUM_REGS];
+	void* stackPtr;
+	struct CoopTask* next;
+	struct CoopTask* prev;
+} CoopTask;
+
+static CoopTask *cur = 0;
+
+static CoopTask* __attribute__((noinline)) coopSchedule(char taskDied) {
+	CoopTask* next = cur->next;
+
+	if (taskDied) {
+		// Halt if last task died.
+		if (next == cur)
+			while (1)
+				;
+
+		// Delete task
+		if (cur->stackPtr)
+			free(cur->stackPtr);
+		cur->next->prev = cur->prev;
+		cur->prev->next = cur->next;
+		free(cur);
+	}
+	cur = next;
+	return next;
+}
+
+static void __attribute__((naked)) __attribute__((noinline)) coopTaskStart(void) {
+	asm (
+		"mov   r0, r5;"
+		"blx   r4;"
+		"mov   r0, #1;"
+		"bl    coopSchedule;"
+		"ldmia r0, {r4-r12, lr};"
+		"mov   sp, r12;"
+		"bx    lr;"
+	);
+}
+
+static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTask* curTask) {
+	asm (
+		"mov   r12, sp;"
+		"stmia r0, {r4-r12, lr};"
+		"mov   r0, #0;"
+		"bl    coopSchedule;"
+		"ldmia r0, {r4-r12, lr};"
+		"mov   sp, r12;"
+		"bx    lr;"
+	);
+}
+
+static int coopInit(void) {
+	CoopTask* task;
+
+	task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
+	if (!task)
+		return 0;
+	task->next = task;
+	task->prev = task;
+	task->stackPtr = 0;
+	cur = task;
+
+	return 1;
+}
+
+static int coopSpawn(SchedulerParametricTask taskF, void* taskData, uint32_t stackSz) {
+	uint8_t *stack = (uint8_t*)malloc(stackSz);
+	if (!stack)
+		return 0;
+
+	CoopTask *task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
+	if (!task) {
+		free(stack);
+		return 0;
+	}
+	task->stackPtr = stack;
+	task->regs[0] = (uint32_t) taskF;
+	task->regs[1] = (uint32_t) taskData;
+	task->regs[8] = ((uint32_t)(stack + stackSz)) & ~7;
+	task->regs[9] = (uint32_t) & coopTaskStart;
+
+	task->prev = cur;
+	task->next = cur->next;
+	cur->next->prev = task;
+	cur->next = task;
+
+	// These are here so compiler is sure that function is
+	// referenced in both variants (cancels a warning)
+	if (stackSz == 0xFFFFFFFF)
+		coopSchedule(0);
+	if (stackSz == 0xFFFFFFFE)
+		coopSchedule(1);
+
+	return 1;
+}
+
+void yield(void) {
+	coopDoYield(cur);
+}
+
+}; // extern "C"
+
+SchedulerClass::SchedulerClass() {
+	coopInit();
+}
+
+static void startLoopHelper(void *taskData) {
+	SchedulerTask task = reinterpret_cast<SchedulerTask>(taskData);
+	while (true)
+		task();
+}
+
+void SchedulerClass::startLoop(SchedulerTask task, uint32_t stackSize) {
+	coopSpawn(startLoopHelper, reinterpret_cast<void *>(task), stackSize);
+}
+
+static void startTaskHelper(void *taskData) {
+	SchedulerTask task = reinterpret_cast<SchedulerTask>(taskData);
+	task();
+}
+
+void SchedulerClass::start(SchedulerTask task, uint32_t stackSize) {
+	coopSpawn(startTaskHelper, reinterpret_cast<void *>(task), stackSize);
+}
+
+void SchedulerClass::start(SchedulerParametricTask task, void *taskData, uint32_t stackSize) {
+	coopSpawn(task, taskData, stackSize);
+}
+
+SchedulerClass Scheduler;
+