need to sync with desktop

7 files changed, 695 insertions, 218 deletions

diff --git a/src/heap.c b/src/heap.c
index 18cb8e6..746f0ef 100644
--- a/src/heap.c
+++ b/src/heap.c
@@ -1,15 +1,11 @@
 #include <heap.h>
 #include <stm32l476xx.h>
 
-#define RAM_END 0x20018000
-
 #define HEAP_SIZE (16 * 1024)
 
 static uint32_t offset = 0;
 
-__attribute__ ((section("._user_heap_stack")))
 uint8_t heap[HEAP_SIZE];
-void *end = heap;
 
 void heap_init(void)
 {
@@ -21,14 +17,14 @@ uint32_t heap_available(void)
 	return HEAP_SIZE - offset;
 }
 
-void *hmalloc(uint32_t size)
+void *malloc(uint32_t size)
 {
-	void *alloc = end + offset;
+	void *alloc = &heap[offset];
 	offset += size;
 	return alloc;
 }
 
-void *hcalloc(uint32_t count, uint32_t size)
+void *calloc(uint32_t count, uint32_t size)
 {
 	/*uint32_t total = count * size;
 	void *alloc = hmalloc(total);
@@ -37,5 +33,10 @@ void *hcalloc(uint32_t count, uint32_t size)
 	return alloc;*/
 
 	// calloc broke
-	return hmalloc(count * size);
+	return malloc(count * size);
+}
+
+void free(void *ptr)
+{
+	(void)ptr;
 }
diff --git a/src/initrd.c b/src/initrd.c
index 7b68c40..54bdc4b 100644
--- a/src/initrd.c
+++ b/src/initrd.c
@@ -31,7 +31,7 @@ uint8_t initrd_nametest(char *file, const char *want)
 	return 0;
 }
 
-uint32_t pow10(uint8_t n)
+uint32_t ipow10(uint8_t n)
 {
 	uint32_t i = 1;
 	while (n--)
@@ -46,7 +46,7 @@ uint32_t initrd_getsize(initrd_file *file)
 	while (*--p == ' ');
 
 	for (int8_t i = p - file->size, j = 0; i >= 0; i--, j++)
-		size += (*p-- - '0') * pow10(j);
+		size += (*p-- - '0') * ipow10(j);
 
 	return size;
 }
diff --git a/src/main.c b/src/main.c
index ddf9029..5cce261 100644
--- a/src/main.c
+++ b/src/main.c
@@ -48,57 +48,31 @@ int main(void)
 	while (1);

 }

 

-void script_puts(stack_t *stack)

+int script_puts(interpreter *it)

 {

-	asm("mov r0, %0; svc 2" :: "r" (stack[0]));

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

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

+	return 0;

 }

 

 void task_interpreter(void)

 {

-	interpreter interp;

-	interpreter_init(&interp);

-	interpreter_define_cfunc(&interp, "print", script_puts);

+	interpreter it;

+	iinit(&it);

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

 

-	char buf[32];

-	int index;

-	while (1) {

-		index = 0;

-		do {

-			buf[index] = serial_get();

-		} while (buf[index++] != '\r' && index < 32);

-		buf[index - 1] = '\0';

-	

-		interpreter_doline(&interp, buf);

-	}

-}

-

-void task_suck(void)

-{

-	float i = 1;

-

-	while (1) {

-		i += 0.123;

-		lcd_puti((int)(i * 1000));

-		delay(2000);

-	}

+	while (1);

 }

 

-#include <stdio.h>

 void kmain(void)

 {

 	asm("cpsie i");

 

 	task_start(lcd_handler, 128);

 	delay(200);

-	task_start(task_suck, 1024);

-	//task_start(task_interpreter, 1024);

+	task_start(task_interpreter, 2048);

 

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

-	// svc puts

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

-

-	//extern void lua_start(void);

-	//lua_start();

 

 	while (1) {

 		gpio_dout(GPIOA, 5, 1);

diff --git a/src/parser.c b/src/parser.c
index 851c860..df2dc21 100644
--- a/src/parser.c
+++ b/src/parser.c
@@ -1,228 +1,716 @@
 #include <parser.h>
 
-#include <stdbool.h>
 #include <heap.h>
 
-#define MAX_VAR		8
-#define MAX_STACK	8
+#include <ctype.h>
+#include <string.h>
 
-static const char *interpreter_operators = "=(";
+extern int atoi(char *);
+float strtof(char *s, char **e)
+{ (void)s; (void)e; return 0;}
+#include <stdarg.h>
+extern int ksnprintf(char *, int, const char *, ...);
 
-bool strcmp(const char *a, const char *b)
+#define MAX_VARS  32
+#define MAX_STACK 32
+
+#define INT(v)   (*((int32_t *)&v->value))
+#define FLOAT(v) (*((float *)&v->value))
+
+static char *str_func = "(func)";
+static char *str_undef = "(undefined)";
+
+char *strclone(const char *s)
+{
+	char *clone = (char *)malloc(strlen(s));
+	strcpy(clone, s);
+	return clone;
+}
+
+char *strnclone(const char *s, uint32_t n)
 {
-	int i = 0;
-	for (; a[i] == b[i] && a[i] != '\0'; i++);
-	return a[i] == b[i];
+	char *clone = (char *)malloc(n);
+	strncpy(clone, s, n);
+	return clone;
 }
 
-bool strncmp(const char *a, const char *b, int count)
+void fsetstr(variable *f)
 {
-	int i = 0;
-	for (; a[i] == b[i] && i < count; i++);
-	return i == count;
+	if (f->svalue == 0)
+		f->svalue = (char *)malloc(16);
+	ksnprintf(f->svalue, 16, "%f", FLOAT(f));
 }
 
-uint8_t isalpha(char c)
+void isetstr(variable *i)
 {
-	return (c >= 'A' && c <= 'Z') || (c >= 'a' && c <= 'z');
+	if (i->svalue == 0)
+		i->svalue = (char *)malloc(12);
+	ksnprintf(i->svalue, 12, "%ld", INT(i));
 }
 
-uint8_t isnum(char c)
+uint8_t eol(int c)
 {
-	return (c >= '0' && c <= '9');
+	return c == '\n' || c == '\0';
 }
 
-uint8_t isname(char c)
+uint8_t eot(int c)
 {
-	return isalpha(c) || isnum(c);
+	return eol(c) || c == ' ';
 }
 
-uint8_t isspace(char c)
+uint8_t eoe(int c)
 {
-	return (c == ' ' || c == '\t' || c == '\n');
+	return eol(c) || c == ')';
 }
 
-uint8_t isoper(char c)
+uint32_t findend(const char *s, char o, char c)
 {
-	for (uint8_t i = 0; i < sizeof(interpreter_operators); i++) {
-		if (c == interpreter_operators[i])
-			return 1;
+	uint8_t indent = 0;
+	uint32_t i;
+	for (i = 1; !eol(s[i]); i++) {
+		if (s[i] == o) {
+			indent++;
+		} else if (s[i] == c) {
+			if (indent == 0)
+				break;
+			else
+				indent--;
+		}
 	}
 
-	return 0;
+	return i;
 }
 
-void interpreter_init(interpreter *interp)
+int ifunc_set(interpreter *it);
+int ifunc_jmp(interpreter *it);
+int ifunc_label(interpreter *it);
+int ifunc_end(interpreter *it);
+
+variable *idoexpr(interpreter *interp, const char *line);
+
+variable *itostring(variable *v);
+variable *itoint(variable *v);
+variable *itofloat(variable *v);
+
+void iinit(interpreter *interp)
 {
-	interp->status = READY;
-	interp->vcount = 0;
-	interp->vars = (variable *)hcalloc(MAX_VAR, sizeof(variable));
-	interp->names = (char **)hcalloc(MAX_VAR, sizeof(char *));
-	interp->stack = (stack_t *)hcalloc(MAX_STACK, sizeof(stack_t));
+	interp->vars = (variable *)calloc(MAX_VARS, sizeof(variable));
+	interp->vnames = (char **)calloc(MAX_VARS, sizeof(char *));
+	interp->stack = (stack_t *)calloc(MAX_STACK, sizeof(stack_t));
+	interp->stidx = 0;
+	interp->lines = (char **)calloc(20, sizeof(char *));
+	interp->lnidx = 0;
+	interp->indent = 0;
+
+	inew_cfunc(interp, "set", ifunc_set);
+	inew_cfunc(interp, "jmp", ifunc_jmp);
+	inew_cfunc(interp, "func", ifunc_label);
+	inew_cfunc(interp, "end", ifunc_end);
 }
 
-void interpreter_define_value(interpreter *interp, const char *name, int32_t value)
+void ipush(interpreter *it, void *v)
 {
-	interp->names[interp->vcount] = (char *)name; 
-	interp->vars[interp->vcount].nameidx = interp->vcount;
-	interp->vars[interp->vcount].type = VALUE;
-	interp->vars[interp->vcount].value = (uint32_t)value;
-	interp->vcount++;
+	it->stack[it->stidx++] = v;
 }
 
-void interpreter_define_cfunc(interpreter *interp, const char *name, func_t addr)
+void *ipop(interpreter *it)
 {
-	interp->names[interp->vcount] = (char *)name;
-	interp->vars[interp->vcount].nameidx = interp->vcount;
-	interp->vars[interp->vcount].type = CFUNCTION;
-	interp->vars[interp->vcount].value = (uint32_t)addr;
-	interp->vcount++;
+	return it->stack[--it->stidx];
 }
 
-int32_t interpreter_get_value(interpreter *interp, const char *name)
+variable *interpreter_get_variable(interpreter *interp, const char *name)
 {
-	for (uint16_t i = 0; i < interp->vcount; i++) {
-		if (!strcmp(interp->names[i], name))
-			return (int32_t)interp->vars[i].value;
+	for (uint32_t i = 0; i < MAX_VARS; i++) {
+		if (!interp->vars[i].used) {
+			variable *v = &interp->vars[i];
+			v->used = 1;
+			v->fromc = 0;
+			v->valtype = STRING;
+			v->value = 0;
+			v->svalue = str_undef;
+			interp->vnames[i] = strclone(name);
+			return v;
+		} else if (interp->vnames[i] != 0 && !strcmp(interp->vnames[i], name)) {
+			return &interp->vars[i];
+		}
 	}
-
 	return 0;
 }
 
-/**
- * doline section
- */
-
-bool namencmp(const char *name, const char *s)
+char *interpreter_get_name(interpreter *interp, variable *v)
 {
-	uint16_t i;
-	for (i = 0; name[i] == s[i] && s[i] != '\0'; i++);
-	return (name[i] == '\0' && !isname(s[i]));
+	for (uint32_t i = 0; i < MAX_VARS; i++) {
+		if (v == &interp->vars[i])
+			return interp->vnames[i];
+	}
+	return str_undef;
 }
 
-uint16_t spacecount(const char *s)
+void inew_string(interpreter *interp, const char *name, char *value)
 {
-	uint16_t i;
-	for (i = 0; isspace(s[i]); i++);
-	return i;
+	variable *v = interpreter_get_variable(interp, name);
+	if (v != 0) {
+		v->valtype = STRING;
+		INT(v) = 0;
+		v->svalue = strclone(value);
+	}
 }
 
-char *copystr(const char *s, char end)
+void inew_integer(interpreter *interp, const char *name, int32_t value)
 {
-	uint16_t len = 0;
-	while (s[len++] != end);
-	char *buf = (char *)hmalloc(len);
-	for (uint16_t i = 0; i < len; i++)
-		buf[i] = s[i];
-	return buf;
+	variable *v = interpreter_get_variable(interp, name);
+	if (v != 0) {
+		v->valtype = INTEGER;
+		INT(v) = value;
+		isetstr(v);
+	}
 }
 
-char *copysubstr(const char *s, int end)
+void inew_float(interpreter *interp, const char *name, float value)
 {
-	char *buf = (char *)hmalloc(end);
-	for (uint16_t i = 0; i < end; i++)
-		buf[i] = s[i];
-	return buf;
+	variable *v = interpreter_get_variable(interp, name);
+	if (v != 0) {
+		v->valtype = FLOAT;
+		FLOAT(v) = value;
+		fsetstr(v);
+	}
 }
 
-variable *interpreter_getvar(interpreter *interp, const char *line)
+void inew_cfunc(interpreter *interp, const char *name, func_t func)
 {
-	for (uint16_t i = 0; i < interp->vcount; i++) {
-		if (namencmp(interp->names[i], line))
-			return &interp->vars[i];
+	variable *v = interpreter_get_variable(interp, name);
+	if (v != 0) {
+		v->fromc = 1;
+		v->valtype = FUNC;
+		v->value = (uint32_t)func;
+		v->svalue = str_func;
 	}
+}
 
+variable *make_var(interpreter *interp, const char *line, uint32_t *next)
+{
+	if (line[0] == '\"') { // string literal
+		uint32_t end = 1;
+		while (!eol(line[end])) {
+			if (line[end] == '\"'/* && line[end - 1] != '\\'*/) {
+				if (!eot(line[end + 1]))
+					return 0;
+				// TODO string breakdown
+				variable *v = (variable *)malloc(sizeof(variable));
+				v->used = 0;
+				v->valtype = STRING;
+				v->svalue = strnclone(line + 1, end - 1);
+				*next = end + 1;
+				return v;
+			}
+			end++;
+		}
+		return 0;
+	} else if (line[0] == '(') { // equation literal
+		uint32_t end = findend(line, '(', ')');
+		if (eot(line[end]))
+			return 0;
+		*next = end + 1;
+		return idoexpr(interp, line + 1);
+	} else if (isalpha(line[0])) { // variable/func
+		uint32_t end = 1;
+		for (; isalnum(line[end]); end++);
+		if (!eot(line[end]))
+			return 0;
+		char *name = (char *)malloc(end + 1);
+		strncpy(name, line, end);
+		name[end] = '\0';
+		*next = end;
+		return interpreter_get_variable(interp, name);
+	} else if (isdigit(line[0])) { // number
+		uint32_t end = 1;
+		uint8_t dec = 0;
+		for (; !eot(line[end]); end++) {
+			if (!isdigit(line[end])) {
+				if (line[end] == '.') {
+					if (!dec)
+						dec = 1;
+					else
+						return 0;
+				} else {
+					return 0;
+				}
+			}
+		}
+		variable *v = (variable *)malloc(sizeof(variable));
+		v->used = 0;
+		if (dec) {
+			v->valtype = FLOAT;
+			FLOAT(v) = strtof(line, 0);
+			fsetstr(v);
+		} else {
+			v->valtype = INTEGER;
+			INT(v) = atoi(line);
+			isetstr(v);
+		}
+		*next = end;
+		return v;
+	}
 	return 0;
 }
 
-int interpreter_doline(interpreter *interp, const char *line)
+int idoline(interpreter *interp, const char *line)
 {
-	variable *bits[16];
-	uint16_t offset = 0, boffset = 0;
+	variable *ops[8];
+	uint32_t ooffset, offset, next;
+
+	interp->lines[interp->lnidx] = strclone(line);
+loop:
+	if (line[0] == '#') {
+		goto norun;
+	} else if (interp->indent > 0) {
+		if (!strcmp(line, "end"))
+			interp->indent--;
+		goto norun;
+	}
 
-	// check for var/func set or usage
-	int end;
-getvar:
-	for (end = 0; isname(line[end]); end++);
-	variable *var = interpreter_getvar(interp, line);
+	ooffset = 0;
+	offset = 0;
+
+	// step 1 - convert to tokens
+	while (!eol(line[offset])) {
+		ops[ooffset] = make_var(interp, line + offset, &next);
+		if (ops[ooffset] == 0) {
+			return -4;
+		} else {
+			ooffset++;
+			offset += next;
+		}
+
+		// skip whitespace
+		for (; line[offset] == ' ' && !eol(line[offset]); offset++);
+	}
+
+	// step 2 - execute
+	if (ooffset == 0)
+		return -1;
+
+	if (ops[0]->valtype != FUNC)
+		return -2;
 
-	if (var != 0) {
-		bits[boffset++] = var;
+	if (ops[0]->value == 0)
+		return -3;
+
+	for (uint32_t i = ooffset; --i > 0;)
+		ipush(interp, ops[i]);
+
+	if (ops[0]->fromc) {
+		int ret = ((func_t)ops[0]->value)(interp);
+		if (ret != 0)
+			return ret;
 	} else {
-		// defining new variable
-		interpreter_define_value(interp, copysubstr(line, end), 0);
-		goto getvar; // try again
-	}
-
-	// skip whitespace/name
-	offset += end;
-	offset += spacecount(line + offset); 
-
-	if (boffset == 0 && line[offset] != '=')
-		return -1; // variable not found
-
-	// find operator
-	if (line[offset] == '\0') {
-		// print value
-		return -99;
-	} else if (line[offset] == '=') {
-		// assignment/expression
-		offset++;
-		offset += spacecount(line + offset);
-		bits[0]->value = (uint32_t)copystr(line + offset, '\0');
-	} else if (line[offset] == '(') {
-		// function call
-		offset++;
-		if (bits[0]->type != FUNCTION && bits[0]->type != CFUNCTION)
-			return -2;
-		offset += spacecount(line + offset);
-		
-		// collect arg offsets
-		uint16_t offsets[8];
-		uint8_t ooffset = 0;
-		while (line[offset] != ')' && line[offset] != '\0') {
-			offsets[ooffset] = offset;
-			offset += spacecount(line + offset);
-
-			uint8_t isvn = 1;
-			do {
-				if (line[offset] == ' ' || line[offset] == '\t') {
-					offset += spacecount(line + offset);
-					isvn = 0;
-				}
+		ipush(interp, (void *)(interp->lnidx + 1));
+		interp->lnidx = ops[0]->value;
+	}
 
-				if (line[offset] == ',') {
-					offset++;
-					ooffset++;
-					break;
-				} else if (line[offset] == ')') {
-					ooffset++;
-					break;
-				} else if (isvn == 0) {
-					return -3;
+	interp->stidx -= ooffset - 1;
+
+	if ((int32_t)interp->stidx < 0) {
+		interp->stidx = 0;
+		return -5;
+	}
+
+	for (uint32_t i = 0; i < ooffset; i++) {
+		if (!ops[i]->used)
+			free(ops[i]);
+	}
+
+norun:
+	interp->lnidx++;
+	if (interp->lines[interp->lnidx] != 0) {
+		line = interp->lines[interp->lnidx];
+		goto loop;
+	}
+
+	return 0;
+}
+
+typedef void (*operation_t)(variable *, variable *, variable *);
+
+#define IOPS_COUNT 15
+static char *iops[IOPS_COUNT] = {
+	"+", "-", "*", "/", "&", "|", "^", ">>", "<<",
+	"==", "<", ">", "<=", ">=", "!="
+};
+
+void iop_add(variable *, variable *, variable *);
+void iop_sub(variable *, variable *, variable *);
+void iop_mult(variable *, variable *, variable *);
+void iop_div(variable *, variable *, variable *);
+void iop_and(variable *, variable *, variable *);
+void iop_or(variable *, variable *, variable *);
+void iop_xor(variable *, variable *, variable *);
+void iop_shr(variable *, variable *, variable *);
+void iop_shl(variable *, variable *, variable *);
+void iop_eq(variable *, variable *, variable *);
+void iop_lt(variable *, variable *, variable *);
+void iop_gt(variable *, variable *, variable *);
+void iop_lte(variable *, variable *, variable *);
+void iop_gte(variable *, variable *, variable *);
+void iop_ne(variable *, variable *, variable *);
+
+static operation_t iopfuncs[IOPS_COUNT] = {
+	iop_add, iop_sub, iop_mult, iop_div, iop_and,
+	iop_or, iop_xor, iop_shr, iop_shl,
+	iop_eq, iop_lt, iop_gt, iop_lte, iop_gte, iop_ne
+};
+
+variable *idoexpr(interpreter *interp, const char *line)
+{
+	variable *result = (variable *)malloc(sizeof(variable));
+	char *mline = line;
+	void *ops[16];
+	uint32_t ooffset = 0;
+	uint32_t offset = 0;
+	uint32_t next;
+
+	// step 1 - break apart line
+
+	// skip whitespace
+	for (; line[offset] == ' ' && !eol(line[offset]); offset++);
+	while (!eoe(line[offset])) {
+		if (line[offset] == '(') {
+			uint8_t indent = 0;
+			uint32_t i;
+			for (i = offset + 1; !eol(line[i]); i++) {
+				if (line[i] == '(') {
+					indent++;
+				} else if (line[i] == ')') {
+					if (indent == 0) {
+						break;
+					} else {
+						indent--;
+					}
 				}
-			} while (++offset);
+			}
+			if (eol(line[i]))
+				return 0;
+			ops[ooffset] = idoexpr(interp, line + offset + 1);
+			offset = i + 1;
+		} else {
+			uint32_t end = offset;
+			char cend = 0;
+			if (line[offset] != '\"') {
+				for (; isalnum(line[end]) || line[end] == '.'; end++);
+				cend = line[end];
+				mline[end] = ' ';
+			}
+			ops[ooffset] = make_var(interp, line + offset, &next);
+			if (end != 0)
+				mline[end] = cend;
 		}
+		if (ops[ooffset] == 0)
+			return 0;
+		
+		ooffset++;
+		offset += next;
+
+		// skip whitespace
+		for (; line[offset] == ' ' && !eoe(line[offset]); offset++);
+		if (eoe(line[offset]))
+			break;
+
+		for (uint32_t i = 0; i < IOPS_COUNT; i++) {
+			int len = strlen(iops[i]);
+			if (!strncmp(iops[i], line + offset, len)) {
+				ops[ooffset] = (void *)(i + 1);
+				offset += len;
+				break;
+			}
+		}
+		if (ops[ooffset] == 0)
+			return 0;
+		ooffset++;
 
-		// populate stack
-		for (uint8_t i = 0; i < ooffset; i++) {
-			uint16_t j;
-			for (j = offsets[i]; line[j] != ' ' && line[j] != '\t' &&
-					line[j] != ',' && line[j] != ')'; j++);
-			j -= offsets[i];
+		// skip whitespace
+		for (; line[offset] == ' ' && !eol(line[offset]); offset++);
+	}
 
-			variable *var = interpreter_getvar(interp, line + offsets[i]);
-			if (var != 0)
-				interp->stack[i] = copystr((char *)var->value, '\0');
-			else
-				interp->stack[i] = copysubstr(line + offsets[i], j);
-		}
+	if (ooffset % 2 == 0)
+		return 0;
 
-		((func_t)bits[0]->value)(interp->stack);
-	} else {
-		return -2; // invalid operation
+	// step 2 - do operations
+	result->valtype = ((variable *)ops[0])->valtype;
+	result->value = ((variable *)ops[0])->value;
+	for (uint32_t i = 1; i < ooffset; i += 2) {
+		iopfuncs[(uint32_t)ops[i] - 1](result, result, ops[i + 1]);
+	}
+
+	for (uint32_t i = 0; i < ooffset; i += 2) {
+		if (!((variable *)ops[i])->used)
+			free(ops[i]);
+	}
+
+	if (result->valtype == INTEGER)
+		isetstr(result);
+	else
+		fsetstr(result);
+
+	return result;
+}
+
+variable *igetarg(interpreter *interp, uint32_t index)
+{
+	return interp->stack[interp->stidx - index - 1];
+}
+
+char *igetarg_string(interpreter *interp, uint32_t index)
+{
+	if (index >= interp->stidx)
+		return 0;
+	variable *v = igetarg(interp, index);
+	return v->svalue;
+}
+
+int igetarg_integer(interpreter *interp, uint32_t index)
+{
+	if (index >= interp->stidx)
+		return 0;
+	variable *v = igetarg(interp, index);
+	return INT(itoint(v));
+}
+
+float igetarg_float(interpreter *interp, uint32_t index)
+{
+	if (index >= interp->stidx)
+		return 0;
+	variable *v = igetarg(interp, index);
+	return FLOAT(itofloat(v));
+}
+
+variable *itostring(variable *v)
+{
+	switch (v->valtype) {
+	case INTEGER:
+		v->valtype = STRING;
+		isetstr(v);
+		break;
+	case FLOAT:
+		v->valtype = STRING;
+		fsetstr(v);
+		break;
 	}
+	return v;
+}
+
+variable *itoint(variable *v)
+{
+	switch (v->valtype) {
+	case STRING:
+		v->valtype = INTEGER;
+		INT(v) = atoi(v->svalue);
+		isetstr(v);
+		break;
+	case FLOAT:
+		v->valtype = INTEGER;
+		INT(v) = (int32_t)FLOAT(v);
+		isetstr(v);
+		break;
+	}
+	return v;
+}
 
+variable *itofloat(variable *v)
+{
+	switch (v->valtype) {
+	case STRING:
+		v->valtype = FLOAT;
+		FLOAT(v) = strtof(v->svalue, 0);
+		fsetstr(v);
+		break;
+	case INTEGER:
+		v->valtype = FLOAT;
+		FLOAT(v) = (float)INT(v);
+		fsetstr(v);
+		break;
+	}
+	return v;
+}
+
+/**
+ * Builtin functions
+ */
+
+int ifunc_set(interpreter *it)
+{
+	variable *n = igetarg(it, 0);
+	variable *v = igetarg(it, 1);
+
+	if (n == 0)
+		return -1;
+
+	n->valtype = v->valtype;
+	n->value = v->value;
+	n->svalue = v->svalue;
+	return 0;
+}
+
+int ifunc_label(interpreter *it)
+{
+	variable *n = igetarg(it, 0);
+	
+	if (n == 0)
+		return -1;
+
+	n->valtype = FUNC;
+	n->value = it->lnidx;
+	n->svalue = str_func;
+	it->indent++;
+	return 0;
+}
+
+int ifunc_end(interpreter *it)
+{
+	if (it->stidx > 0) {
+		uint32_t line = (uint32_t)ipop(it);
+		it->lnidx = line - 1;
+	}
+	return 0;
+}
+
+int ifunc_jmp(interpreter *it)
+{
+	int newidx = igetarg_integer(it, 0);
+	ipop(it);
+	ipush(it, (void *)(it->lnidx + 1));
+	ipush(it, 0);
+	it->lnidx = newidx - 1;
 	return 0;
 }
 
+/**
+ * Builtin operations
+ */
+
+void iop_add(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER) {
+		INT(r) = INT(a) + INT(b);
+	} else {
+		itofloat(a);
+		itofloat(b);
+		FLOAT(r) = FLOAT(a) + FLOAT(b);
+	}
+}
+
+void iop_sub(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER) {
+		INT(r) = INT(a) - INT(b);
+	} else {
+		itofloat(a);
+		itofloat(b);
+		FLOAT(r) = FLOAT(a) - FLOAT(b);
+	}
+}
+
+void iop_mult(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER) {
+		INT(r) = INT(a) * INT(b);
+	} else {
+		itofloat(a);
+		itofloat(b);
+		FLOAT(r) = FLOAT(a) * FLOAT(b);
+	}
+}
+
+void iop_div(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER) {
+		INT(r) = INT(a) / INT(b);
+	} else {
+		itofloat(a);
+		itofloat(b);
+		FLOAT(r) = FLOAT(a) / FLOAT(b);
+	}
+}
+
+void iop_and(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER) {
+		INT(r) = INT(a) & INT(b);
+	}
+}
+
+void iop_or(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER) {
+		INT(r) = INT(a) | INT(b);
+	}
+}
+
+void iop_xor(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER) {
+		INT(r) = INT(a) ^ INT(b);
+	}
+}
+
+void iop_shr(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER) {
+		INT(r) = INT(a) >> INT(b);
+	}
+}
+
+void iop_shl(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER) {
+		INT(r) = INT(a) << INT(b);
+	}
+}
+
+void iop_eq(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER)
+		INT(r) = INT(a) == INT(b);
+	else
+		INT(r) = FLOAT(a) == FLOAT(b);
+}
+
+void iop_lt(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER)
+		INT(r) = INT(a) < INT(b);
+	else
+		INT(r) = FLOAT(a) < FLOAT(b);
+}
+
+void iop_gt(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER)
+		INT(r) = INT(a) > INT(b);
+	else
+		INT(r) = FLOAT(a) > FLOAT(b);
+}
+
+void iop_lte(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER)
+		INT(r) = INT(a) <= INT(b);
+	else
+		INT(r) = FLOAT(a) <= FLOAT(b);
+}
+
+void iop_gte(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER)
+		INT(r) = INT(a) >= INT(b);
+	else
+		INT(r) = FLOAT(a) >= FLOAT(b);
+}
+
+void iop_ne(variable *r, variable *a, variable *b)
+{
+	if (a->valtype == INTEGER && b->valtype == INTEGER)
+		INT(r) = INT(a) != INT(b);
+	else
+		INT(r) = FLOAT(a) != FLOAT(b);
+}
+
diff --git a/src/serial.c b/src/serial.c
index db0cfbc..1caa595 100644
--- a/src/serial.c
+++ b/src/serial.c
@@ -1,5 +1,6 @@
 #include <stm32l476xx.h>
 #include <gpio.h>
+#include <clock.h>
 
 void serial_init(void)
 {
@@ -22,7 +23,8 @@ void serial_put(int c)
 
 char serial_get(void)
 {
-	while (!(USART2->ISR & USART_ISR_RXNE));
+	while (!(USART2->ISR & USART_ISR_RXNE))
+		delay(10);
 	return USART2->RDR & 0xFF;
 }
 
@@ -32,6 +34,7 @@ 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';
diff --git a/src/stdlib.c b/src/stdlib.c
index fa2806b..1832033 100644
--- a/src/stdlib.c
+++ b/src/stdlib.c
@@ -1,4 +1,10 @@
+#include <stdarg.h>
 #include <stdint.h>
+#include <string.h>
+
+#include <heap.h>
+
+extern char *itoa(int, char *, int);
 
 void _exit(int code)
 {
@@ -6,28 +12,33 @@ void _exit(int code)
 	for (;;);
 }
 
-int _getpid(int pid)
+int ksnprintf(char *buf, unsigned int count, const char *format, ...)
 {
-	(void)pid;
-	return 0;
-}
+	(void)count;
 
-void _kill(int pid)
-{
-	(void)pid;
-}
+	va_list args;
+	va_start(args, format);
 
-void _sbrk(void)
-{
+	unsigned int i = 0, o = 0;
+	while (o < count && format[i] != '\0') {
+		if (format[i] == '%') {
+			if (format[i + 1] == 'd') {
+				char *s = itoa(va_arg(args, int), malloc(16), 10);
+				strncpy(buf + o, s, count - o);
+				o += strlen(s);
+				free(s);
+			} else if (format[i + 1] == 'f') {
+				strncpy(buf + o, "float", count - o);
+				o += 5;
+			}
+			i++;
+		} else {
+			buf[o++] = format[i];
+		}
 
-}
+		i++;
+	}
 
-char *itoa(int n, int base)
-{
-	static char buf[16];
-	char *p = buf + 15;
-	*p = '\0';
-	do *--p = "0123456789ABCDEF"[n % base];
-	while (n /= base);
-	return p;
+	return o;
 }
+
diff --git a/src/task.c b/src/task.c
index 67ff136..1685bb9 100644
--- a/src/task.c
+++ b/src/task.c
@@ -50,7 +50,7 @@ void task_start(void (*task)(void), uint16_t stackSize)
 
 	for (int i = 0; i < MAX_TASKS; i++) {
 		if (tasks[i].use == 0) {
-			tasks[i].stack = hmalloc(stackSize);
+			tasks[i].stack = malloc(stackSize);
 			tasks[i].sp = tasks[i].stack + stackSize - 16;
 			tasks[i].sp[13] = (uint32_t)task_exit;
 			tasks[i].sp[14] = (uint32_t)task;