Added own mini scheduler

2 files changed, 234 insertions, 0 deletions

diff --git a/sem4/embedded/emb_m4/jrnl/jrnl.ino b/sem4/embedded/emb_m4/jrnl/jrnl.ino
new file mode 100644
index 0000000..ffd76c9
--- /dev/null
+++ b/sem4/embedded/emb_m4/jrnl/jrnl.ino
@@ -0,0 +1,153 @@
+#include "regs.h"
+
+struct tsk {
+	uint8_t stack_h;
+	uint8_t stack_l;
+
+	struct tsk *next;
+};
+
+struct tsk *cur = NULL;
+struct tsk *tasks = NULL;
+
+// Setup a timer running every 10 ms
+void timersetup()
+{
+	noInterrupts();
+
+	TCNT1 = 0;
+
+	// Enable CTC and set prescaler = 1024.
+	// This will give a freq of 15625
+	TCCR1B = (1 << WGM12) | (1 << CS12) | (1 << CS10);
+
+	// Clear on match
+	//TCCR1A = (1 << COM1A1);
+	TCCR1A = 0;
+
+	// Setup compare. Must be around 156 to create 10ms
+	OCR1A = 156;
+
+	// Enable interrupt on compare
+	TIMSK1 |= (1 << OCF1A);
+
+	interrupts();
+}
+
+inline struct tsk *get_next() {
+	if (cur->next) {
+		return cur->next;
+	}
+
+	return tasks;
+}
+
+void create_tsk(struct tsk *t, void (*f)(void), uint8_t *stack, size_t len) {
+	// Move to the end of stack
+	uint8_t *s = stack + len - 1;
+	
+	// Push load address
+	*(s--) = (uint16_t)f & 0xFF;
+	*(s--) = ((uint16_t)f >> 8) & 0xFF;
+
+	// Push all the stuff. Reg + SREG
+	for (int i = 0; i < 32 + 1; i++) {
+		*(s--) = 0;
+	}
+
+	// Save the stack pointer
+	t->stack_l = (uint16_t)s & 0xFF;
+	t->stack_h = ((uint16_t)s >> 8) & 0xFF;
+
+	// Put it on the list
+	t->next = tasks;
+	tasks = t;
+}
+
+void run() {
+	cur = tasks;
+
+	// Put it back
+	SPL = cur->stack_l;
+	SPH = cur->stack_h;
+
+	POPREGS();
+	__asm__("RETI");
+}
+
+void func(void) {
+	for(;;) {
+		Serial.println(1);
+	}
+}
+void func3(void) {
+	for(;;) {
+		Serial.println(3);
+	}
+}
+void func4(void) {
+	for(;;) {
+		Serial.println(4);
+	}
+}
+
+void func2(void) {
+	bool state = false;
+	for(;;) {
+		digitalWrite(13, state);
+		state = !state;
+		delay(1000);
+	}
+}
+
+ISR(TIMER1_COMPA_vect, ISR_NAKED)
+{
+	PUSHREGS();
+
+	if (!cur) {
+		goto exit;
+	}
+
+	// Save stack space 
+	cur->stack_l = SPL;
+	cur->stack_h = SPH;
+
+	// Switch
+	cur = get_next();	
+
+	// Put it back
+	SPL = cur->stack_l;
+	SPH = cur->stack_h;
+
+exit:
+
+	POPREGS();
+	__asm__("RETI");
+}
+
+void setup()
+{
+	pinMode(13, OUTPUT);
+	Serial.begin(115200);
+
+	Serial.println("Starting stuff");
+
+	struct tsk task1, task2, task4, task3;
+	uint8_t stack[100], stack2[100], stack3[100], stack4[100];
+
+	timersetup();
+
+	create_tsk(&task1, func, stack, 100);
+	create_tsk(&task2, func2, stack2, 100);
+	create_tsk(&task3, func3, stack3, 100);
+	create_tsk(&task4, func4, stack4, 100);
+
+	run();
+
+}
+
+void loop()
+{
+}
+
+
diff --git a/sem4/embedded/emb_m4/jrnl/regs.h b/sem4/embedded/emb_m4/jrnl/regs.h
new file mode 100644
index 0000000..3f6ae03
--- /dev/null
+++ b/sem4/embedded/emb_m4/jrnl/regs.h
@@ -0,0 +1,81 @@
+#ifndef REGS_H
+#define REGS_H
+
+// Taken from Jens
+#define PUSHREGS() __asm__ volatile	( \
+		"push r1 \n" \
+		"push r0 \n" \
+		"in r0, __SREG__ \n" \
+		"cli \n" \
+		"push r0 \n" \
+		"cli \n" \
+		"push r2 \n" \
+		"push r3 \n" \
+		"push r4 \n" \
+		"push r5 \n" \
+		"push r6 \n" \
+		"push r7 \n" \
+		"push r8 \n" \
+		"push r9 \n" \
+		"push r10 \n" \
+		"push r11 \n" \
+		"push r12 \n" \
+		"push r13 \n" \
+		"push r14 \n" \
+		"push r15 \n" \
+		"push r16 \n" \
+		"push r17 \n" \
+		"push r18 \n" \
+		"push r19 \n" \
+		"push r20 \n" \
+		"push r21 \n" \
+		"push r22 \n" \
+		"push r23 \n" \
+		"push r24 \n" \
+		"push r25 \n" \
+		"push r26 \n" \
+		"push r27 \n" \
+		"push r28 \n" \
+		"push r29 \n" \
+		"push r30 \n" \
+		"push r31 \n" \
+		) 
+
+#define POPREGS() __asm__ volatile	( \
+		"pop r31 \n" \
+		"pop r30 \n" \
+		"pop r29 \n" \
+		"pop r28 \n" \
+		"pop r27 \n" \
+		"pop r26 \n" \
+		"pop r25 \n" \
+		"pop r24 \n" \
+		"pop r23 \n" \
+		"pop r22 \n" \
+		"pop r21 \n" \
+		"pop r20 \n" \
+		"pop r19 \n" \
+		"pop r18 \n" \
+		"pop r17 \n" \
+		"pop r16 \n" \
+		"pop r15 \n" \
+		"pop r14 \n" \
+		"pop r13 \n" \
+		"pop r12 \n" \
+		"pop r11 \n" \
+		"pop r10 \n" \
+		"pop r9 \n" \
+		"pop r8 \n" \
+		"pop r7 \n" \
+		"pop r6 \n" \
+		"pop r5 \n" \
+		"pop r4 \n" \
+		"pop r3 \n" \
+		"pop r2 \n" \
+		"pop r0 \n" \
+		"out __SREG__, r0 \n" \
+		"pop r0 \n" \
+		"pop r1 \n" \
+		) 
+
+#endif