major work, own malloc, making things work

21 files changed, 629 insertions, 117 deletions

diff --git a/include/clock.h b/include/clock.h
index 9d4b17a..77ab7e9 100644
--- a/include/clock.h
+++ b/include/clock.h
@@ -1,3 +1,8 @@
+/**
+ * @file clock.h
+ * Basic clock utilities
+ */
+
 #ifndef CLOCK_H_
 #define CLOCK_H_
 
@@ -5,11 +10,13 @@
 
 /**
  * Sets HCLK (system clock) to 80MHz, the maximum.
+ * @param none
  */
 extern void clock_init(void);
 
 /**
- * Sleeps for given milliseconds.
+ * Sleeps for given amount of milliseconds.
+ * @param ms number of milliseconds to sleep for
  */
 void delay(uint32_t ms);
 
diff --git a/include/display.h b/include/display.h
index fb808c6..6004f19 100644
--- a/include/display.h
+++ b/include/display.h
@@ -9,9 +9,15 @@
 #define COLOR_MAX 31
 
 uint16_t dsp_color(uint8_t r, uint8_t g, uint8_t b);
+
+void dsp_dmode(int mode);
 void dsp_write_cmd(uint8_t data);
 void dsp_write_data(uint8_t data);
+uint8_t dsp_read_data(void);
+
 void dsp_set_addr(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2);
+void dsp_set_addr_read(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2);
+
 void dsp_init(void);
 
 #endif // DISPLAY_H_
diff --git a/include/display_draw.h b/include/display_draw.h
index 1030d45..5ed4224 100644
--- a/include/display_draw.h
+++ b/include/display_draw.h
@@ -1,9 +1,15 @@
 #ifndef DISPLAY_DRAW_H_
 #define DISPLAY_DRAW_H_
 
+#include <stdint.h>
+
+void dsp_cursoron(void);
+
+void dsp_line(int x, int y, int i, int j, uint16_t color);
 void dsp_rect(int x, int y, int w, int h, uint16_t color);
 
 void dsp_cpos(int x, int y);
+void dsp_coff(int x, int y);
 void dsp_puts(const char *s);
 
 #endif // DISPLAY_DRAW_H_
diff --git a/include/heap.h b/include/heap.h
index 9625771..c93eb8e 100644
--- a/include/heap.h
+++ b/include/heap.h
@@ -1,12 +1,12 @@
 #ifndef HEAP_H_
 #define HEAP_H_
 
-//#include <stdint.h>
+#include <stdint.h>
 
-//uint32_t heap_available(void);
+void heap_init(void *buf);
 
-//void *malloc(uint32_t size);
-//void *calloc(uint32_t count, uint32_t size);
-//void free(void *buf);
+void *malloc(uint32_t size);
+void *calloc(uint32_t count, uint32_t size);
+void free(void *buf);
 
 #endif // HEAP_H_
diff --git a/include/initrd.h b/include/initrd.h
index 9c7a9de..de730f2 100644
--- a/include/initrd.h
+++ b/include/initrd.h
@@ -1,23 +1,53 @@
+/**
+ * @file initrd.h
+ * Initrd image support
+ * An archive file (made with ar) can be linked into the final executable to
+ * allow files to be loaded in memory on boot. See mkinitrd.sh or the Makefile
+ * for more info.
+ */
+
 #ifndef INITRD_H_
 #define INITRD_H_
 
 #include <stdint.h>
 
+/**
+ * Structure for the archive's header.
+ */
 typedef struct
 {
-	char signature[8];
+	char signature[8]; /**< The archive's signature. */
 } __attribute__ ((packed)) initrd_header;
 
+/**
+ * Structure for a file entry in the archive.
+ */
 typedef struct 
 {
-	char name[16];
-	uint8_t unused[32];
-	char size[10];
-	char sig[2];
+	char name[16];      /**< The name of the file. */
+	uint8_t unused[32]; /**< Unused information. */
+	char size[10];      /**< The file's size in bytes (as string). */
+	char sig[2];        /**< A signature to start file data. */
 } __attribute__ ((packed)) initrd_file;
 
+/**
+ * Confirms the initrd image is loaded and valid.
+ * @return non-zero if valid image found
+ */
 uint8_t initrd_validate(void);
+
+/**
+ * Gets contents of the given file.
+ * @param name the file's name
+ * @return pointer to file data, null if not found
+ */
 char *initrd_getfile(const char *name);
+
+/**
+ * Gets the size of the given file.
+ * @param name the file's name
+ * @return the file's size, in bytes
+ */
 uint32_t initrd_getfilesize(const char *name);
 
 #endif // INITRD_H_
diff --git a/include/lcd.h b/include/lcd.h
index 441d463..378aac4 100644
--- a/include/lcd.h
+++ b/include/lcd.h
@@ -1,23 +1,63 @@
+/**
+ * @file lcd.h
+ * A basic library for writing a 16x2 text LCD.
+ */
+
 #ifndef LCD_H_
 #define LCD_H_
 
 #include <stdint.h>
 
 /**
- * Direct access
+ * A handler/task to manage asyncronous LCD writes.
+ */
+void lcd_handler(void);
+
+/**
+ * Writes a string asyncronously to the LCD.
+ * The lcd_handler task must be running for the string to actually be printed.
+ * @param s the string to write
+ */
+void lcd_put(const char *s);
+
+//
+// The following functions do not support asyncronous calls.
+//
+
+/**
+ * Initializes the LCD.
  */
 void lcd_init(void);
 
+/**
+ * Writes a string to the LCD.
+ * A cursor position is kept internally. When the end of the screen is reached,
+ * writing resumes at the first position.
+ * @param s the string to write
+ */
 void lcd_puts(const char *s);
+
+/**
+ * Writes a base 10 integer to the screen.
+ * @param i the integer to print
+ */
 void lcd_puti(int i);
+
+/**
+ * Writes a base 16 integer to the screen.
+ * @param h the integer to print
+ */
 void lcd_puth(int h);
+
+/**
+ * Writes a byte in binary to the screen.
+ * @param b the byte to print
+ */
 void lcd_putb(uint8_t b);
-void lcd_clear(void);
 
 /**
- * Buffered/async access
+ * Clears the LCD.
  */
-void lcd_handler(void);
-void lcd_put(const char *s);
+void lcd_clear(void);
 
 #endif // LCD_H_
diff --git a/include/serial.h b/include/serial.h
index 9cb9d24..7192e17 100644
--- a/include/serial.h
+++ b/include/serial.h
@@ -1,10 +1,33 @@
+/**
+ * @file serial.h
+ * Provides basic serial IO (through STM debug stuff)
+ */
+
 #ifndef SERIAL_H_
 #define SERIAL_H_
 
+/**
+ * Initializes the serial device.
+ */
 void serial_init(void);
+
+/**
+ * Puts the given character through serial.
+ * @param c the character to send
+ */
 void serial_put(int c);
 
+/**
+ * Gets a character from serial.
+ * @return the character
+ */
 char serial_get(void);
+
+/**
+ * Gets a string from serial, cut off by a newline.
+ * @param buf the initialized buffer to fill
+ * @param max the max amount of bytes to write to the buffer
+ */
 void serial_gets(char *buf, int max);
 
 #endif // SERIAL_H_
diff --git a/include/task.h b/include/task.h
index c57b2c0..eda2dcd 100644
--- a/include/task.h
+++ b/include/task.h
@@ -1,10 +1,25 @@
+/**
+ * @file task.h
+ * Provides multitasking functionality
+ */
+
 #ifndef TASK_H_
 #define TASK_H_
 
 #include <stdint.h>
 
+/**
+ * Enters multitasking mode. The given function acts as the initial thread.
+ * This task is given a 4kb stack.
+ * @param init the initial thread to run
+ */
 void task_init(void (*init)(void));
 
+/**
+ * Starts a new task.
+ * @param task the code to run
+ * @param stackSize how many bytes of stack to give the thread
+ */
 void task_start(void (*task)(void), uint16_t stackSize);
 
 #endif // TASK_H_
diff --git a/initrd/init b/initrd/init
index 324e519..c7ce93b 100644
--- a/initrd/init
+++ b/initrd/init
@@ -1,23 +1,22 @@
-func div9
-	if ((arg0 % 9) == 0)
-		ret 1
-	end
-	ret 0
-end
+print "Hello."
 
-set a 0
-set b 0
-do
-	set a (a + 1)
-	div9 a > b
-	if (b)
-		print a
-		print ", "
-	end
-	delay 1
-while (a < 100)
+set fg 32767
 
-if (a == 100)
-	print " All good!"
-end
+# draw bg, lines
+rect 50 50 380 220 6375
+line 50 160 430 160 fg
+line 240 50 240 270 fg
 
+set x 50
+#do
+#	delay 10
+#	line x 170 x 150 fg
+#	set x (x + 20)
+#while (x < 430)
+
+#set y 50
+#do
+#	delay 10
+#	line 230 y 250 y fg
+#	set y (y + 20)
+#while (y < 270)
diff --git a/libinterp.a b/libinterp.a
index 2c87928..3921490 100644
--- a/libinterp.a
+++ b/libinterp.a
diff --git a/run.sh b/run.sh
index 88be7fa..a031d27 100755
--- a/run.sh
+++ b/run.sh
@@ -1,7 +1,7 @@
 #!/bin/bash
 
-openocd -f /usr/share/openocd/scripts/board/st_nucleo_l476rg.cfg \
-	-c "init; reset halt; flash write_image erase main.hex; reset run; exit"
+#openocd -f /usr/share/openocd/scripts/board/st_nucleo_l476rg.cfg \
+#	-c "init; reset halt; flash write_image erase main.hex; reset run; exit"
 
-#openocd -f /usr/share/openocd/scripts/board/st_nucleo_l476rg.cfg > /dev/null &
-#gdb-multiarch 
+openocd -f /usr/share/openocd/scripts/board/st_nucleo_l476rg.cfg > /dev/null &
+gdb-multiarch 
diff --git a/src/display.c b/src/display.c
index 8b62e6a..955cf8f 100644
--- a/src/display.c
+++ b/src/display.c
@@ -19,10 +19,24 @@
 #define LCD_D6  GPIO_PORT(B, 10)
 #define LCD_D7  GPIO_PORT(A, 8)
 
-// bbbbbggg gggrrrrr 
+void dsp_dmode(int mode)
+{
+	static int old = 0;
 
-// 00000000
+	if (mode != old) {
+		gpio_mode(LCD_D0, mode);
+		gpio_mode(LCD_D1, mode);
+		gpio_mode(LCD_D2, mode);
+		gpio_mode(LCD_D3, mode);
+		gpio_mode(LCD_D4, mode);
+		gpio_mode(LCD_D5, mode);
+		gpio_mode(LCD_D6, mode);
+		gpio_mode(LCD_D7, mode);
+		old = mode;
+	}
+}
 
+// bbbbbggg gggrrrrr 
 uint16_t dsp_color(uint8_t r, uint8_t g, uint8_t b)
 {
 	r &= 0x1F;
@@ -46,6 +60,22 @@ void dsp_write_data(uint8_t data)
 	gpio_dout(LCD_WR, 1);
 }
 
+uint8_t dsp_read_data(void)
+{
+	uint8_t ret = 0;
+	gpio_dout(LCD_RD, 0);
+	ret |= gpio_din(LCD_D0);
+	ret |= gpio_din(LCD_D1) << 1;
+	ret |= gpio_din(LCD_D2) << 2;
+	ret |= gpio_din(LCD_D3) << 3;
+	ret |= gpio_din(LCD_D4) << 4;
+	ret |= gpio_din(LCD_D5) << 5;
+	ret |= gpio_din(LCD_D6) << 6;
+	ret |= gpio_din(LCD_D7) << 7;
+	gpio_dout(LCD_RD, 1);
+	return ret;
+}
+
 void dsp_write_cmd(uint8_t data)
 {
 	gpio_dout(LCD_RS, 0);
@@ -53,7 +83,7 @@ void dsp_write_cmd(uint8_t data)
 	gpio_dout(LCD_RS, 1);
 }
 
-void dsp_set_addr(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
+void dsp_set_addr_base(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
 {
 	dsp_write_cmd(0x2A);
 	dsp_write_data(x1 >> 8);
@@ -65,9 +95,20 @@ void dsp_set_addr(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
 	dsp_write_data(y1 & 0xFF);
 	dsp_write_data(y2 >> 8);
 	dsp_write_data(y2 & 0xFF);
+}
+
+void dsp_set_addr(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
+{
+	dsp_set_addr_base(x1, y1, x2, y2);
 	dsp_write_cmd(0x2C); // begin writing
 }
 
+void dsp_set_addr_read(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
+{
+	dsp_set_addr_base(x1, y1, x2, y2);
+	dsp_write_cmd(0x2E); // begin reading
+}
+
 void dsp_init(void)
 {
 	gpio_mode(LCD_CS, OUTPUT);
@@ -75,14 +116,7 @@ void dsp_init(void)
 	gpio_mode(LCD_RD, OUTPUT);
 	gpio_mode(LCD_WR, OUTPUT);
 	gpio_mode(LCD_RST, OUTPUT);
-	gpio_mode(LCD_D0, OUTPUT);
-	gpio_mode(LCD_D1, OUTPUT);
-	gpio_mode(LCD_D2, OUTPUT);
-	gpio_mode(LCD_D3, OUTPUT);
-	gpio_mode(LCD_D4, OUTPUT);
-	gpio_mode(LCD_D5, OUTPUT);
-	gpio_mode(LCD_D6, OUTPUT);
-	gpio_mode(LCD_D7, OUTPUT);
+	dsp_dmode(OUTPUT);
 	gpio_speed(LCD_CS, LOW);
 	gpio_speed(LCD_RS, LOW);
 	gpio_speed(LCD_RD, LOW);
diff --git a/src/display_draw.c b/src/display_draw.c
index f52cd8e..0d26e9a 100644
--- a/src/display_draw.c
+++ b/src/display_draw.c
@@ -1,15 +1,54 @@
+#include <display_draw.h>
 #include <display.h>
+#include <task.h>
+#include <clock.h>
+
+volatile uint8_t lock = 0;
+#define LOCK while (lock) { delay(5); } lock = 1
+#define UNLOCK lock = 0
 
 static unsigned int curx = 0;
 static unsigned int cury = 0;
+static unsigned int curxo = 0;
+static unsigned int curyo = 0;
 
 extern const unsigned char inconsolata24[192 * 156 * 2 + 1];
 
+void task_cursor(void)
+{
+	while (1) {
+		int x = curxo + curx * 12;
+		int y = curyo + cury * 26;
+		dsp_rect(x, y + 24, 12, 1, 0xFFFF);
+		delay(300);
+		dsp_rect(x, y + 24, 12, 1, 0);
+		delay(300);
+	}
+}
+
+void dsp_cursoron(void)
+{
+	task_start(task_cursor, 512);
+}
+
 void dsp_putchar(int c)
 {
+	if (c == '\n') {
+		curx = 0;
+		if (++cury == 12) {
+			UNLOCK;
+			dsp_rect(0, 0, LCD_WIDTH, LCD_HEIGHT, 0);
+			LOCK;
+			cury = 0;
+		}
+		return;
+	}
+
 	unsigned int start = ((c - ' ') / 16 * 192 * 26 + (c % 16) * 12) * 2;
 
-	dsp_set_addr(curx * 12, cury * 26, curx * 12 + 11, cury * 26 + 25);
+	unsigned int x = curxo + curx * 12;
+	unsigned int y = curyo + cury * 26;
+	dsp_set_addr(x, y, x + 11, y + 25);
 	// for each row
 	for (unsigned int i = 0; i < 26; i++) {
 		// for each column
@@ -19,16 +58,22 @@ void dsp_putchar(int c)
 
 	if (++curx == 40) {
 		curx = 0;
-		if (++cury == 10)
+		if (++cury == 12) {
+			UNLOCK;
+			dsp_rect(0, 0, LCD_WIDTH, LCD_HEIGHT, 0);
+			LOCK;
 			cury = 0;
+		}
 	}
 }
 
 void dsp_puts(const char *s)
 {
 	unsigned int i = 0;
+	LOCK;
 	while (s[i])
 		dsp_putchar(s[i++]);
+	UNLOCK;
 }
 
 void dsp_cpos(int x, int y)
@@ -37,12 +82,55 @@ void dsp_cpos(int x, int y)
 	cury = y;
 }
 
+void dsp_coff(int x, int y)
+{
+	curxo = x;
+	curyo = y;
+}
+
 void dsp_rect(int x, int y, int w, int h, uint16_t color)
 {
+	LOCK;
 	dsp_set_addr(x, y, x + w - 1, y + h - 1);
 	int countdown = w * h;
 	do {
 		dsp_write_data(color >> 8);
 		dsp_write_data(color & 0xFF);
 	} while (countdown--);
+	UNLOCK;
+}
+
+void dsp_line(int x, int y, int i, int j, uint16_t color)
+{
+	int dx = i - x;
+	int sx = dx >= 0 ? 1 : -1;
+	int dy = j - y;
+	int sy = dy >= 0 ? 1 : -1;
+
+	if (dx < 0)
+		dx *= -1;
+	if (dy < 0)
+		dy *= -1;
+	int err = (dx > dy ? dx : -dy) / 2;
+	int e2;
+
+	LOCK;
+	while (1) {
+		dsp_set_addr(x, y, x, y);
+		dsp_write_data(color >> 8);
+		dsp_write_data(color & 0xFF);
+		if (x == i && y == j)
+			break;
+		e2 = err;
+		if (e2 > -dx) {
+			err -= dy;
+			x += sx;
+		}
+		if (e2 < dy) {
+			err += dx;
+			y += sy;
+		}
+	}
+	UNLOCK;
 }
+
diff --git a/src/heap.c b/src/heap.c
index 8a18d38..2157aad 100644
--- a/src/heap.c
+++ b/src/heap.c
@@ -1,54 +1,56 @@
-#include <heap.h>
-#include <stm32l476xx.h>
+#include "heap.h"
 
-#define HEAP_SIZE (64 * 1024)
+#define HEAP_ALIGN 16
 
-uint8_t heap[HEAP_SIZE];
+typedef struct {
+	uint32_t next;
+	uint32_t size;
+} __attribute__ ((packed)) alloc_t;
 
-void *_sbrk(int inc)
-{
-	static uint8_t *hend;
-	uint8_t *prev;
-
-	if (hend == 0)
-		hend = heap;
-
-	prev = hend;
-	hend += inc;
-
-	return prev;
-}
+static alloc_t root;
+static void *heap_end;
 
-//void heap_init(void)
-//{
+void heap_init(void *buf)
+{
+	heap_end = buf;
+	root.next = 1;
+	root.size = 0;
 	// what to do...
-//}
-
-//uint32_t heap_available(void)
-//{
-//	return HEAP_SIZE - offset;
-//}
+}
 
-/*void *malloc(uint32_t size)
+void *malloc(uint32_t size)
 {
-	void *alloc = &heap[offset];
-	offset += size;
-	return alloc;
+	alloc_t *node = &root;
+	while (node->next & 1 || node->size < size) {
+		if ((node->next & ~(1)) == 0) {
+			node->next |= (uint32_t)(heap_end + HEAP_ALIGN) & ~(HEAP_ALIGN - 1);
+			heap_end += 2 * HEAP_ALIGN + size;
+			node = (void *)(node->next & ~(1));
+			node->next = 0;
+			node->size = size;
+			break;
+		}
+		node = (void *)(node->next & ~(1));
+	}
+
+	node->next |= 1;
+
+	return (void *)((uint32_t)node + sizeof(alloc_t));
 }
 
 void *calloc(uint32_t count, uint32_t size)
 {
-	//uint32_t total = count * size;
-	//void *alloc = hmalloc(total);
-	//for (uint32_t i = 0; i < total; i++)
-	//	((uint8_t *)alloc)[i] = 0;
-	//return alloc;
-
-	// calloc broke
-	return malloc(count * size);
+	uint8_t *buf = malloc(count * size);
+	for (uint8_t i = 0; i < count * size; i++)
+		buf[i] = 0;
+	return buf;
 }
 
-void free(void *ptr)
+void free(void *buf)
 {
-	(void)ptr;
-}*/
+	if (buf == 0)
+		return;
+	alloc_t *alloc = (alloc_t *)((uint32_t)buf - sizeof(alloc_t));
+	alloc->next &= ~(1);
+}
+
diff --git a/src/lcd.c b/src/lcd.c.bak
index a2a8ca7..a2a8ca7 100644
--- a/src/lcd.c
+++ b/src/lcd.c.bak
diff --git a/src/main.c b/src/main.c
index bab8f02..c70d24b 100644
--- a/src/main.c
+++ b/src/main.c
@@ -10,12 +10,11 @@
 #include <serial.h>

 #include <parser.h>

 #include <stack.h>

-

-#include <stdlib.h>

 #include <string.h>

 

+extern char *itoa(int, char *, int);

+

 void kmain(void);

-void task_interpreter(void);

 

 int main(void)

 {

@@ -27,6 +26,8 @@ int main(void)
 

 	//MPU->CTRL |= MPU_CTRL_ENABLE_Msk | MPU_CTRL_PRIVDEFENA_Msk;

 	clock_init();

+	extern uint8_t _ebss;

+	heap_init(&_ebss);

 	gpio_init();

 

 	gpio_mode(GPIOA, 5, OUTPUT);

@@ -45,10 +46,30 @@ int script_puts(interpreter *it)
 {

 	char *s = igetarg_string(it, 0);

 	dsp_puts(s);

+	//dsp_puts("\n");

 	//asm("mov r0, %0; svc 2" :: "r" (s));

 	return 0;

 }

 

+int script_gets(interpreter *it)

+{

+	char *s = malloc(64), c[2] = {0, 0};

+	uint16_t index = 0;

+

+	do {

+		c[0] = serial_get();

+		s[index] = c[0];

+		if (c[0] != '\r')

+			dsp_puts(c);

+	} while (s[index] != '\r' && index++ < 23);

+	s[index] = '\0';

+

+	variable *v = igetarg(it, 0);

+	v->valtype = STRING;

+	v->svalue = s;

+	return 0;

+}

+

 int script_delay(interpreter *it)

 {

 	int ms = igetarg_integer(it, 0);

@@ -56,12 +77,61 @@ int script_delay(interpreter *it)
 	return 0;

 }

 

+int script_rect(interpreter *it)

+{

+	dsp_rect(igetarg_integer(it, 0), igetarg_integer(it, 1),

+		igetarg_integer(it, 2), igetarg_integer(it, 3),

+		igetarg_integer(it, 4));

+	return 0;

+}

+

+int script_line(interpreter *it)

+{

+	dsp_line(igetarg_integer(it, 0), igetarg_integer(it, 1),

+		igetarg_integer(it, 2), igetarg_integer(it, 3),

+		igetarg_integer(it, 4));

+	return 0;

+}

+

+int script_ppos(interpreter *it)

+{

+	dsp_cpos(0, 0);

+	dsp_coff(igetarg_integer(it, 0), igetarg_integer(it, 1));

+	return 0;

+}

+

 void task_interpreter(void)

 {

 	interpreter it;

 	iinit(&it);

 	inew_cfunc(&it, "print", script_puts);

+	inew_cfunc(&it, "gets", script_gets);

 	inew_cfunc(&it, "delay", script_delay);

+	inew_cfunc(&it, "rect", script_rect);

+	inew_cfunc(&it, "ppos", script_ppos);

+	inew_cfunc(&it, "line", script_line);

+

+	/*int ret = 0;

+	char *linebuf = malloc(100), c[2] = {0, 0};

+	while (1) {

+		uint16_t index = 0;

+		if (it.indent > 0)

+			dsp_puts(">");

+		dsp_puts("> ");

+		do {

+			c[0] = serial_get();

+			if (c[0] >= ' ' || c[0] == '\r') {

+				linebuf[index] = c[0];

+				if (c[0] >= ' ')

+					dsp_puts(c);

+			}

+		} while (linebuf[index] != '\r' && index++ < 100);

+		linebuf[index] = '\0';

+		dsp_puts("\n");

+		ret = idoline(&it, linebuf);

+		if (ret < 0)

+			break;

+	}*/

 

 	char *s = initrd_getfile("init");

 	if (s == 0)

@@ -87,8 +157,8 @@ void task_interpreter(void)
 	}

 

 	if (ret < 0) {

-		lcd_puts("Error: ");

-		lcd_puts(itoa(ret, linebuf, 10));

+		dsp_puts("\nError: ");

+		dsp_puts(itoa(ret, linebuf, 10));

 	}

 	free(linebuf);

 	//iend(&it); // nah

@@ -103,20 +173,11 @@ void kmain(void)
 	asm("cpsie i");

 

 	dsp_init();

-

-	dsp_rect(0, 0, LCD_WIDTH, 105, dsp_color(0xFF, 0, 0));

-	dsp_rect(0, 105, LCD_WIDTH, 105, dsp_color(0, 0xFF, 0));

-	dsp_rect(0, 210, LCD_WIDTH, 110, dsp_color(0, 0, 0xFF));

-

-	//dsp_puts("Hello, world! My name is Clyne. I enjoy car rides and long 

-//walks on the beach");

-

-	//task_start(lcd_handler, 128);

-	//delay(200);

+	dsp_rect(0, 0, LCD_WIDTH, LCD_HEIGHT, dsp_color(0, 0, 0));

+	dsp_cursoron();

+	//dsp_puts("Hey.");

 	task_start(task_interpreter, 4096);

 

-	//char *s = initrd_getfile("test.txt");

-

 	while (1) {

 		gpio_dout(GPIOA, 5, 1);

 		delay(500);

diff --git a/src/main.c.bak b/src/main.c.bak
new file mode 100644
index 0000000..95f3210
--- /dev/null
+++ b/src/main.c.bak
@@ -0,0 +1,203 @@
+#include <stm32l476xx.h>

+#include <clock.h>

+#include <heap.h>

+#include <task.h>

+#include <gpio.h>

+#include <lcd.h>

+#include <display.h>

+#include <display_draw.h>

+#include <initrd.h>

+#include <serial.h>

+#include <parser.h>

+#include <stack.h>

+

+#include <stdlib.h>

+#include <string.h>

+

+void kmain(void);

+void task_interpreter(void);

+

+int main(void)

+{

+	asm("cpsid i");

+

+	// prepare flash latency for 40MHz operation

+	FLASH->ACR &= ~(FLASH_ACR_LATENCY);

+	FLASH->ACR |= FLASH_ACR_LATENCY_4WS;

+

+	//MPU->CTRL |= MPU_CTRL_ENABLE_Msk | MPU_CTRL_PRIVDEFENA_Msk;

+	clock_init();

+	gpio_init();

+

+	gpio_mode(GPIOA, 5, OUTPUT);

+

+	serial_init();

+

+	// enable FPU

+	SCB->CPACR |= (0xF << 20);

+

+	task_init(kmain);

+

+	while (1);

+}

+

+int script_puts(interpreter *it)

+{

+	char *s = igetarg_string(it, 0);

+	dsp_puts(s);

+	//dsp_puts("\n");

+	//asm("mov r0, %0; svc 2" :: "r" (s));

+	return 0;

+}

+

+int script_gets(interpreter *it)

+{

+	char *s = malloc(64), c[2] = {0, 0};

+	uint16_t index = 0;

+

+	do {

+		c[0] = serial_get();

+		s[index] = c[0];

+		if (c[0] != '\r')

+			dsp_puts(c);

+	} while (s[index] != '\r' && index++ < 23);

+	s[index] = '\0';

+

+	variable *v = igetarg(it, 0);

+	v->valtype = STRING;

+	v->svalue = s;

+	return 0;

+}

+

+int script_delay(interpreter *it)

+{

+	int ms = igetarg_integer(it, 0);

+	delay(ms);

+	return 0;

+}

+

+int script_rect(interpreter *it)

+{

+	dsp_rect(igetarg_integer(it, 0), igetarg_integer(it, 1),

+		igetarg_integer(it, 2), igetarg_integer(it, 3),

+		igetarg_integer(it, 4));

+	return 0;

+}

+

+int script_line(interpreter *it)

+{

+	dsp_line(igetarg_integer(it, 0), igetarg_integer(it, 1),

+		igetarg_integer(it, 2), igetarg_integer(it, 3),

+		igetarg_integer(it, 4));

+	return 0;

+}

+

+int script_ppos(interpreter *it)

+{

+	dsp_cpos(0, 0);

+	dsp_coff(igetarg_integer(it, 0), igetarg_integer(it, 1));

+	return 0;

+}

+

+void task_interpreter(void)

+{

+	interpreter it;

+	iinit(&it);

+	inew_cfunc(&it, "print", script_puts);

+	inew_cfunc(&it, "gets", script_gets);

+	inew_cfunc(&it, "delay", script_delay);

+	inew_cfunc(&it, "rect", script_rect);

+	inew_cfunc(&it, "ppos", script_ppos);

+	inew_cfunc(&it, "line", script_line);

+

+	/*int ret = 0;

+	char *linebuf = malloc(100), c[2] = {0, 0};

+	while (1) {

+		uint16_t index = 0;

+		if (it.indent > 0)

+			dsp_puts(">");

+		dsp_puts("> ");

+		do {

+			c[0] = serial_get();

+			if (c[0] >= ' ' || c[0] == '\r') {

+				linebuf[index] = c[0];

+				if (c[0] >= ' ')

+					dsp_puts(c);

+			}

+		} while (linebuf[index] != '\r' && index++ < 100);

+		linebuf[index] = '\0';

+		dsp_puts("\n");

+		ret = idoline(&it, linebuf);

+		if (ret < 0)

+			break;

+	}*/

+

+	char *s = initrd_getfile("init");

+	if (s == 0)

+		goto end;

+

+	char *linebuf = (char *)malloc(120);

+	uint32_t i = 0, prev = 0, lc;

+	uint32_t size = initrd_getfilesize("init");

+	int ret;

+	while (i < size) {

+		for (; s[i] != '\n' && s[i] != '\0'; i++);

+		lc = i - prev;

+		if (lc == 0) {

+			prev = ++i;

+			continue;

+		}

+		strncpy(linebuf, s + prev, lc + 1);

+		linebuf[lc] = '\0';

+		ret = idoline(&it, linebuf);

+		if (ret < 0)

+			break;

+		prev = ++i;

+	}

+

+	if (ret < 0) {

+		dsp_puts("\nError: ");

+		dsp_puts(itoa(ret, linebuf, 10));

+	}

+	free(linebuf);

+	//iend(&it); // nah

+

+end:

+	while (1)

+		delay(10);

+}

+

+void kmain(void)

+{

+	asm("cpsie i");

+

+	dsp_init();

+

+	//dsp_rect(0, 0, 40, 40, dsp_color(0x7F, 0, 0x7F));

+

+	//dsp_set_addr_read(0, 0, 39, 39);

+	//dsp_dmode(INPUT);

+	//uint8_t *buf = (uint8_t *)malloc(40 * 40 * 2);

+	//for (int i = 0; i < 180; i++)

+	//	buf[i] = dsp_read_data();

+	//dsp_dmode(OUTPUT);

+	//dsp_set_addr(40, 40, 79, 79);

+	//for (int i = 0; i < 320; i++)

+	//	dsp_write_data(buf[i]);

+	//dsp_rect(80, 80, 40, 40, dsp_color(0x7F, 0x7F, 0));

+

+	dsp_rect(0, 0, LCD_WIDTH, LCD_HEIGHT, dsp_color(0, 0, 0));

+	dsp_cursoron();

+

+	task_start(task_interpreter, 4096);

+

+	//char *s = initrd_getfile("test.txt");

+

+	while (1) {

+		gpio_dout(GPIOA, 5, 1);

+		delay(500);

+		gpio_dout(GPIOA, 5, 0);

+		delay(500);

+	}

+}

+

diff --git a/src/serial.c b/src/serial.c
index 1caa595..de28275 100644
--- a/src/serial.c
+++ b/src/serial.c
@@ -35,8 +35,6 @@ void serial_gets(char *buf, int max)
 	do {
 		buf[index] = serial_get();
 		serial_put(buf[index]);
-	} while (index++ < max && buf[index] != '\r');
-
-	buf[index - 1] = '\0';
-	//return buf;
+	} while (buf[index] != '\r' && index++ < max);
+	buf[index] = '\0';
 }
diff --git a/src/stm32l4xx_it.c b/src/stm32l4xx_it.c
index d007607..ef10406 100644
--- a/src/stm32l4xx_it.c
+++ b/src/stm32l4xx_it.c
@@ -3,7 +3,7 @@
 

 void perror(const char *s)

 {

-	lcd_puts(s);

+	(void)s;//lcd_puts(s);

 }

 

 void NMI_Handler(void) {}

diff --git a/src/svc.c b/src/svc.c
index 507855d..ad6e33f 100644
--- a/src/svc.c
+++ b/src/svc.c
@@ -24,7 +24,7 @@ void SVC_Handler(void) {
 		gpio_dout(GPIOA, 6, 0);
 		break;
 	case 2:
-		lcd_put((char *)stack[0]);
+	//	lcd_put((char *)stack[0]);
 		break;
 	default:
 		break;
diff --git a/src/task.c b/src/task.c
index 71c829c..ee338de 100644
--- a/src/task.c
+++ b/src/task.c
@@ -1,5 +1,5 @@
 #include <task.h>
-#include <stdlib.h>
+#include <heap.h>
 #include <stm32l476xx.h>
 
 typedef struct {