From 7a019d06e7a99cb154bb6751ffc9db1b3c3abf45 Mon Sep 17 00:00:00 2001
From: Stefan `Sec` Zehl <sec@42.org>
Date: Sun, 24 Jul 2011 15:44:35 +0200
Subject: [PATCH] Add simple queue to move jobs from systick to main/idle.

---
 firmware/applications/default.c      |  2 ++
 firmware/applications/tester/timer.c | 53 ++++++++++++++++++++++++++++
 firmware/applications/tester/util.c  | 14 ++++++++
 firmware/basic/Makefile              |  1 +
 firmware/basic/basic.h               |  5 +++
 firmware/basic/idle.c                | 52 +++++++++++++++++++++++++++
 firmware/basic/idle.h                | 22 ++++++++++++
 7 files changed, 149 insertions(+)
 create mode 100644 firmware/applications/tester/timer.c
 create mode 100644 firmware/basic/idle.c
 create mode 100644 firmware/basic/idle.h

diff --git a/firmware/applications/default.c b/firmware/applications/default.c
index 85d1a07..c361cb0 100644
--- a/firmware/applications/default.c
+++ b/firmware/applications/default.c
@@ -115,6 +115,7 @@ void main_default(void) {
 void tick_default(void) {
     static int ctr;
     ctr++;
+    incTimer();
     if(ctr>100){
         VoltageCheck();
         LightCheck();
@@ -144,3 +145,4 @@ void tick_default(void) {
 
     return;
 };
+
diff --git a/firmware/applications/tester/timer.c b/firmware/applications/tester/timer.c
new file mode 100644
index 0000000..4ba526d
--- /dev/null
+++ b/firmware/applications/tester/timer.c
@@ -0,0 +1,53 @@
+#include <sysinit.h>
+
+#include "basic/basic.h"
+
+#include "lcd/lcd.h"
+#include "lcd/print.h"
+#include "lcd/allfonts.h"
+
+#include "filesystem/ff.h"
+#include "filesystem/select.h"
+#include "funk/nrf24l01p.h"
+#include "usb/usbmsc.h"
+
+#include <string.h>
+
+/**************************************************************************/
+
+void s_ticks(void) {
+    int dx=0;
+    int dy=8;
+    lcdClear();
+    dx=DoString(0,dy,"Ticks:");
+    dx=DoString(0,dy+16,"Qdepth:");
+    while ((getInputRaw())==BTN_NONE){
+        DoInt(0,dy+8,_timectr);
+        DoInt(dx,dy+16,(the_queue.qend-the_queue.qstart+MAXQENTRIES)%MAXQENTRIES);
+        lcdDisplay();
+        __asm volatile ("WFI");
+    };
+    dy+=16;
+    dx=DoString(0,dy,"Done.");
+};
+
+void b_one(void){
+    gpioSetValue (RB_LED2, 0); 
+    delayms(100);
+    gpioSetValue (RB_LED2, 1); 
+    delayms(100);
+    gpioSetValue (RB_LED2, 0); 
+};
+
+void do_qone(void) {
+    work_queue();
+};
+
+void do_q(void) {
+    idle_queue(500);
+};
+
+void push_qone(void) {
+    push_queue(&b_one);
+};
+
diff --git a/firmware/applications/tester/util.c b/firmware/applications/tester/util.c
index 101f874..02240b0 100644
--- a/firmware/applications/tester/util.c
+++ b/firmware/applications/tester/util.c
@@ -15,6 +15,20 @@
 
 /**************************************************************************/
 
+void show_ticks(void) {
+    int dx=0;
+    int dy=8;
+    lcdClear();
+    dx=DoString(0,dy,"Ticks:");
+    while ((getInputRaw())==BTN_NONE){
+        DoInt(0,dy+8,_timectr);
+        lcdDisplay();
+    };
+    dy+=16;
+    dx=DoString(0,dy,"Done.");
+};
+
+
 void adc_light(void) {
     int dx=0;
     int dy=8;
diff --git a/firmware/basic/Makefile b/firmware/basic/Makefile
index ae1d63a..cc372be 100644
--- a/firmware/basic/Makefile
+++ b/firmware/basic/Makefile
@@ -17,6 +17,7 @@ OBJS += xxtea.o
 OBJS += ecc.o
 OBJS += byteorder.o
 OBJS += random.o
+OBJS += idle.o
 
 LIBNAME=basic
 
diff --git a/firmware/basic/basic.h b/firmware/basic/basic.h
index 55d10fc..eff70fb 100644
--- a/firmware/basic/basic.h
+++ b/firmware/basic/basic.h
@@ -176,4 +176,9 @@ struct MENU {
 
 
 void handleMenu(const struct MENU *the_menu);
+
+// idle.c
+
+#include "basic/idle.h"
+
 #endif
diff --git a/firmware/basic/idle.c b/firmware/basic/idle.c
new file mode 100644
index 0000000..e1c75fd
--- /dev/null
+++ b/firmware/basic/idle.c
@@ -0,0 +1,52 @@
+#include <sysinit.h>
+
+#include "basic/basic.h"
+#include "lcd/print.h"
+
+QUEUE the_queue;
+volatile uint32_t _timectr=0;
+
+/**************************************************************************/
+
+void work_queue(void){
+    void (*elem)(void);
+	int start;
+
+	if (the_queue.qstart == the_queue.qend){
+		__asm volatile ("WFI");
+		return;
+	};
+
+	start=the_queue.qstart;
+	start=(start+1)%MAXQENTRIES;
+	elem=the_queue.queue[start].callback;
+	the_queue.qstart=start;
+
+	elem();
+};
+
+void idle_queue(uint32_t ms){
+	int end=_timectr+ms/10;
+	do {
+		if (the_queue.qstart == the_queue.qend){
+			__asm volatile ("WFI");
+		}else{
+			work_queue();
+		};
+	} while (end >_timectr);
+};
+
+int push_queue(void (*new)(void)){
+	int end;
+
+	end=the_queue.qend;
+	end=(end+1)%MAXQENTRIES;
+
+	if(end == the_queue.qstart) // Queue full
+		return -1;
+
+	the_queue.queue[end].callback=new;
+	the_queue.qend=end;
+
+	return 0;
+};
diff --git a/firmware/basic/idle.h b/firmware/basic/idle.h
new file mode 100644
index 0000000..10b4681
--- /dev/null
+++ b/firmware/basic/idle.h
@@ -0,0 +1,22 @@
+#define MAXQENTRIES 8
+
+typedef struct {
+    void (*callback)(void);
+} QENTRY;
+
+typedef struct {
+    int qstart;
+    int qend;
+    QENTRY queue[MAXQENTRIES];
+} QUEUE;
+
+extern QUEUE the_queue;
+extern volatile uint32_t _timectr;
+
+void work_queue(void);
+void idle_queue(uint32_t delayms);
+int push_queue(void (*new)(void));
+int magic(void *new);
+#define incTimer(void) do{_timectr++;}while(0);
+
+