File content as of revision 1:8967b575bb46:

/*
 * Copyright (C) 2012-2013 audin
 * This program is licensed under the Apache License, Version 2.0.
 * 2013/08: Modify for mbed for LPC1114FN28
 * 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 "mbed.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;
}
#ifdef __CC_ARM 
__asm static void coopTaskStart(void) {
	import coopSchedule
#if _ARM_CM0_
	/* for Cortex-m0 */
		mov   r0, r5;
		blx   r4;
		movs   r0, #1;
		bl    coopSchedule;
	/****	ldmia r0, {r4-r12, lr};	*/
		adds 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;

		subs 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 */
		mov   r0, r5;						/* r5 = new task func */
		blx   r4;							/* r4 = helper func   */
		movs   r0, #1;						
		bl    coopSchedule;
		ldmia r0, {r4-r12, lr};
		mov   sp, r12;
		bx    lr;
#endif
}
#else
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
}
#endif

#ifdef __CC_ARM
__asm static void coopDoYield(CoopTask* curTask) {
#if _ARM_CM0_
	/* for Cortex-m0 */
			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	*/

			movs   r0, #0;
			bl    coopSchedule;

		/**** ldmia r0, {r4-r12, lr};	*/
			adds 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;

			subs 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 */
			mov   r12, sp;
			stmia r0, {r4-r12, lr};
			movs   r0, #0;
			bl    coopSchedule;
			ldmia r0, {r4-r12, lr};
			mov   sp, r12;
			bx    lr;
#endif
}

#else
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

}
#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);
}

#ifdef MBED_H
void taskWait(uint32_t ms) {
    uint32_t start = us_ticker_read();
    while ((us_ticker_read() - start) < (uint32_t)(ms*1000)){
		yield();
    }
}
#else
void wait(uint32_t ms) {
	uint32_t start = millis();
	while (millis() - start < ms){
		yield();
	}
}
#endif

#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