Diff: Scheduler.cpp

Revision:

0:c68459544a17

Child:

1:8967b575bb46

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Scheduler.cpp	Sun Aug 25 16:48:21 2013 +0900
@@ -0,0 +1,307 @@
+/*
+ * Copyright (C) 2012 audin
+ * This program is licensed under the Apache License, Version 2.0.
+ * Modified 2012/10: For working on Cortex-M0 and M3.
+ *                   Defined static tcb option for the cpu that has quite a less SRAM < 8kbyte.  
+ *
+ * Original file is arduino-1.5\hardware\arduino\sam\libraries\Scheduler
+ */
+
+/*
+ * 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 <stdint.h>
+#include "Scheduler.h"
+
+#define _ARM_CM0_			1		/* 1: Cortex-M0 or M3,  0: Cortex-M3		*/
+#define _USE_MALLOC_		0		/* 1: 8kbyte >= SRAM,   0: 8kbyte < SRAM	*/
+
+
+#if _USE_MALLOC_
+#	include <stdlib.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+uint32_t millis(void);
+
+#define NUM_REGS 10	// r4-r11, sp, lr
+
+typedef struct CoopTask {
+	uint32_t regs[NUM_REGS];
+	void* stackPtr;
+	struct CoopTask* next;
+	struct CoopTask* prev;
+} CoopTask;
+
+#if  !_USE_MALLOC_
+static uint32_t id = 0;
+static CoopTask tcb[ COOP_TASK_NUM_MAX ];
+static uint8_t task_stack[ COOP_TASK_NUM_MAX - 1 ][ MIN_STACK_SIZE ];
+#endif
+
+static CoopTask *cur = 0;
+
+CoopTask* __attribute__((noinline)) coopSchedule(char taskDied) {
+	CoopTask* next = cur->next;
+
+#if _USE_MALLOC_
+	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);
+	}
+#endif
+	cur = next;
+	return next;
+}
+
+static void __attribute__((naked)) __attribute__((noinline)) coopTaskStart(void) {
+#if _ARM_CM0_
+	/* for Cortex-m0 */
+	__asm (
+		"mov   r0, r5;"
+		"blx   r4;"
+		"mov   r0, #1;"
+		"bl    coopSchedule;"
+	/****	"ldmia r0, {r4-r12, lr};"	*/
+		"add r0, r0, #16;"
+		"ldmia r0!, {r4-r7};"		/* get r7->r11, r6->r10, r5->r9, r4->r8		*/
+		"mov r11, r7;"
+		"mov r10, r6;"
+		"mov r9,  r5;"
+		"mov r8 , r4;"
+
+		"ldmia r0!, {r4-r5};"		/* get r5->lr, r4->r12						*/
+		"mov lr,  r5;"
+		"mov r12, r4;"
+
+		"sub r0, r0, #40;"			/* set offset for r4, 40 = 10reg * 4byte	*/
+		"ldmia  r0!, {r4-r7};"		/* get r7,r6,r5,r4							*/
+	/**** end ldmia converted by Cortex-M0 instructions	*/
+		"msr   msp, r12;"			/* use main stack							*/
+		"bx    lr;"
+	);
+#else
+	/* for Cortex-m3 or ARM code cpu */
+	asm (
+		"mov   r0, r5;"						/* r5 = new task func */
+		"blx   r4;"							/* r4 = helper func   */
+		"mov   r0, #1;"						
+		"bl    coopSchedule;"
+		"ldmia r0, {r4-r12, lr};"
+		"mov   sp, r12;"
+		"bx    lr;"
+	);
+#endif
+}
+
+static void __attribute__((naked)) __attribute__((noinline)) coopDoYield(CoopTask* curTask) {
+#if _ARM_CM0_
+	/* for Cortex-m0 */
+	__asm (
+			"mrs   r12, msp;"
+		/**** "stmia r0, {r4-r12, lr};" */
+			"stmia r0!, {r4-r7};"		/* first store r4-r7 data				*/
+
+			"mov r4, r8;"
+			"mov r5, r9;"
+			"mov r6, r10;"
+			"mov r7, r11;"
+			"stmia r0!, {r4-r7};"		/* store r8-r11							*/
+
+			"mov r4, r12;"
+			"mov r5, lr;"
+			"stmia r0!, {r4,r5};"		/* store r12, lr						*/
+		/**** end stmia converted by cortex-m0 instructions	*/
+
+			"mov   r0, #0;"
+			"bl    coopSchedule;"
+
+		/**** "ldmia r0, {r4-r12, lr};"	*/
+			"add r0, r0, #16;"			/* set offset for r8					*/
+			"ldmia r0!, {r4-r7};"		/* get r7->r11, r6->r10, r5->r9, r4->r8 */
+			"mov r11, r7;"
+			"mov r10, r6;"
+			"mov r9,  r5;"
+			"mov r8 , r4;"
+
+			"ldmia r0!, {r4-r5};"		/* get r5->lr, r4->r12					*/
+			"mov lr,  r5;"
+			"mov r12, r4;"
+
+			"sub r0, r0, #40;"			/* set offset for r4, 40 = 10reg * 4byte  */
+			"ldmia  r0!, {r4-r7};"
+		/**** end ldmia converted by Cortex-M0 instructions	*/
+
+			"msr   msp, r12;"
+			"bx    lr;"
+		);
+#else
+	/* for Cortex-m3 or ARM code cpu */
+	__asm (
+			"mov   r12, sp;"
+			"stmia r0, {r4-r12, lr};"
+			"mov   r0, #0;"
+			"bl    coopSchedule;"
+			"ldmia r0, {r4-r12, lr};"
+			"mov   sp, r12;"
+			"bx    lr;"
+		);
+#endif
+
+}
+
+static int coopInit(void) {
+	CoopTask* task;
+#if _USE_MALLOC_
+#	ifdef __cplusplus
+		task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
+#	else
+		task = (CoopTask *)(malloc(sizeof(CoopTask)));
+#	endif
+	if (!task)
+		return 0;
+#else
+	task = &tcb[ id ];
+#endif
+	task->next = task;
+	task->prev = task;
+	task->stackPtr = 0;
+	cur = task;
+
+	return 1;
+}
+
+static int coopSpawn(SchedulerParametricTask taskF, void* taskData, uint32_t stackSz) {
+#if _USE_MALLOC_
+	uint8_t *stack = (uint8_t*)malloc(stackSz);
+	if (!stack)
+		return 0;
+#	ifdef __cplusplus
+		CoopTask *task = reinterpret_cast<CoopTask *>(malloc(sizeof(CoopTask)));
+#	else
+		CoopTask *task = (CoopTask *)(malloc(sizeof(CoopTask)));
+#	endif
+	if (!task) {
+		free(stack);
+		return 0;
+	}
+#else
+	uint8_t *stack = task_stack[ id ];
+	id++;
+	CoopTask *task = &tcb[ id ];
+#endif
+	task->stackPtr = stack;
+	task->regs[0] = (uint32_t) taskF;							/* Helper func: r4	*/
+	task->regs[1] = (uint32_t) taskData;						/* New task :	r5	*/
+#if _USE_MALLOC_
+	task->regs[8] = ((uint32_t)(stack + stackSz)) & ~7;
+#else
+	task->regs[8] = ((uint32_t)(stack + MIN_STACK_SIZE)) & ~7;		/* r12	*/
+#endif
+	task->regs[9] = (uint32_t) & coopTaskStart;					/* lr	*/
+
+	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);
+}
+
+void wait(uint32_t ms) {
+	uint32_t start = millis();
+	while (millis() - start < ms)
+		yield();
+}
+
+#ifdef __cplusplus
+}; // extern "C"
+#endif
+
+void scheduler_init( void ) {
+	coopInit();
+}
+
+static void startLoopHelper(void *taskData) {
+#ifdef __cplusplus
+	SchedulerTask task = reinterpret_cast<SchedulerTask>(taskData);
+#else
+	SchedulerTask task = (SchedulerTask)(taskData);
+#endif
+	while (1){
+		task();
+	}
+}
+
+static void startTaskHelper(void *taskData) {
+#ifdef __cplusplus
+	SchedulerTask task = reinterpret_cast<SchedulerTask>(taskData);
+#else
+	SchedulerTask task = (SchedulerTask)(taskData);
+#endif
+	task();
+}
+
+void scheduler_start( SchedulerTask task ) {
+	coopSpawn( startTaskHelper, (void *)(task), MIN_STACK_SIZE );
+}
+
+void scheduler_startLoop( SchedulerTask task) {
+	coopSpawn( startLoopHelper, (void *)(task), MIN_STACK_SIZE );
+}
+
+#ifdef __cplusplus
+SchedulerClass::SchedulerClass() {
+	coopInit();
+}
+
+void SchedulerClass::startLoop(SchedulerTask task, uint32_t stackSize) {
+	coopSpawn(startLoopHelper, reinterpret_cast<void *>(task), stackSize);
+}
+
+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;
+#endif
+