diff --git a/Makefile b/Makefile
index 48175ff..87dd3a0 100644
--- a/Makefile
+++ b/Makefile
@@ -1,16 +1,44 @@
+CROSS = arm-none-eabi-
+CC = gcc
+AS = as
+AR = ar
+OBJCOPY = objcopy
+
 MCUFLAGS = -mthumb -mcpu=cortex-m4
-CFLAGS = -Iinclude $(MCUFLAGS)
-
-all:
-	arm-none-eabi-as $(MCUFLAGS) startup_stm32l476xx.s -c -o out/startup_stm32l476xx.o
-	arm-none-eabi-gcc $(CFLAGS) system_stm32l4xx.c -c -o out/system_stm32l4xx.o
-	arm-none-eabi-gcc $(CFLAGS) stm32l4xx_it.c -c -o out/stm32l4xx_it.o
-	arm-none-eabi-gcc $(CFLAGS) clock.c -c -o out/clock.o
-	arm-none-eabi-gcc $(CFLAGS) heap.c -c -o out/heap.o
-	arm-none-eabi-gcc $(CFLAGS) task.c -c -o out/task.o
-	arm-none-eabi-gcc $(CFLAGS) main.c -c -o out/main.o
-	arm-none-eabi-gcc $(CFLAGS) -T link.ld out/*.o -o out/main.elf
-	arm-none-eabi-objcopy -O ihex out/main.elf main.hex
+AFLAGS = $(MCUFLAGS) 
+CFLAGS = $(MCUFLAGS) -Iinclude -ffreestanding -Wall -Werror -Wextra
+OFLAGS = -O ihex
+
+CFILES = $(wildcard src/*.c)
+AFILES = $(wildcard src/*.s) 
+
+OUTDIR = out
+OFILES = $(patsubst src/%.c, $(OUTDIR)/%.o, $(CFILES)) \
+	 $(patsubst src/%.s, $(OUTDIR)/%.asm.o, $(AFILES))
+
+LIBDIR = -Llib
+LIBS = -llua
+
+HEX = main.hex
+
+all: $(HEX)
+
+$(HEX): $(OFILES)
+	@echo "  LINK   " $(HEX)
+	@$(CROSS)$(OBJCOPY) -B arm -I binary -O elf32-littlearm initrd.img out/initrd.img.o
+	@$(CROSS)$(CC) $(CFLAGS) $(LIBDIR) $(LIBS) -T link.ld out/*.o -o out/main.elf
+	@$(CROSS)$(OBJCOPY) $(OFLAGS) out/main.elf $(HEX)
+
+$(OUTDIR)/%.o: src/%.c
+	@echo "  CC     " $<
+	@$(CROSS)$(CC) $(CFLAGS) -c $< -o $@
+
+$(OUTDIR)/%.asm.o: src/%.s
+	@echo "  AS     " $<
+	@$(CROSS)$(AS) $(AFLAGS) -c $< -o $@
 
 clean:
-	rm -rf out/*
+	@echo "  CLEAN"
+	@rm -rf out/*
+
+
diff --git a/README.md b/README.md
index 56c56fd..f2e4a04 100644
--- a/README.md
+++ b/README.md
@@ -8,4 +8,6 @@ Required packages:
 * openocd
 * make
   
-Use run.sh to upload the final output to the processor.
+Use ```run.sh``` to upload the final output to the processor.  
+  
+To make an initrd, add files to ```initrd/``` and then run ```./mkinitrd.sh```.
diff --git a/include/gpio.h b/include/gpio.h
new file mode 100644
index 0000000..25b32fe
--- /dev/null
+++ b/include/gpio.h
@@ -0,0 +1,45 @@
+#ifndef GPIO_H_
+#define GPIO_H_
+
+#include <stm32l476xx.h>
+
+#define GPIO_PORT(p, b) GPIO##p, b
+
+enum GPIO_MODE
+{
+	INPUT = 0,
+	OUTPUT,
+	ALTERNATE,
+	ANALOG
+};
+
+enum GPIO_TYPE
+{
+	PUSHPULL = 0,
+	OPENDRAIN
+};
+
+enum GPIO_SPEED
+{
+	LOW = 0,
+	MEDIUM,
+	HIGH,
+	VERYHIGH
+};
+
+enum GPIO_PUPD
+{
+	NOPUPD,
+	PULLUP,
+	PULLDOWN
+};
+
+void gpio_init(void);
+
+void gpio_pupd(GPIO_TypeDef *port, uint8_t pin, uint8_t pupd);
+void gpio_type(GPIO_TypeDef *port, uint8_t pin, uint8_t type);
+void gpio_mode(GPIO_TypeDef *port, uint8_t pin, uint8_t mode);
+void gpio_dout(GPIO_TypeDef *port, uint8_t pin, uint8_t val);
+uint8_t gpio_din(GPIO_TypeDef *port, uint8_t pin);
+
+#endif // GPIO_H_
diff --git a/include/initrd.h b/include/initrd.h
new file mode 100644
index 0000000..9c7a9de
--- /dev/null
+++ b/include/initrd.h
@@ -0,0 +1,23 @@
+#ifndef INITRD_H_
+#define INITRD_H_
+
+#include <stdint.h>
+
+typedef struct
+{
+	char signature[8];
+} __attribute__ ((packed)) initrd_header;
+
+typedef struct 
+{
+	char name[16];
+	uint8_t unused[32];
+	char size[10];
+	char sig[2];
+} __attribute__ ((packed)) initrd_file;
+
+uint8_t initrd_validate(void);
+char *initrd_getfile(const char *name);
+uint32_t initrd_getfilesize(const char *name);
+
+#endif // INITRD_H_
diff --git a/include/lcd.h b/include/lcd.h
new file mode 100644
index 0000000..0962e08
--- /dev/null
+++ b/include/lcd.h
@@ -0,0 +1,14 @@
+#ifndef LCD_H_
+#define LCD_H_
+
+#include <stdint.h>
+
+void lcd_init(void);
+
+void lcd_puts(const char *s);
+void lcd_puti(int i);
+void lcd_puth(int h);
+void lcd_putb(uint8_t b);
+void lcd_clear(void);
+
+#endif // LCD_H_
diff --git a/initrd/test.txt b/initrd/test.txt
new file mode 100644
index 0000000..af5626b
--- /dev/null
+++ b/initrd/test.txt
@@ -0,0 +1 @@
+Hello, world!
diff --git a/initrd/two.bin b/initrd/two.bin
new file mode 100644
index 0000000..b3f1928
--- /dev/null
+++ b/initrd/two.bin
@@ -0,0 +1,3 @@
+print "yay"
+
+yesyesye
diff --git a/lib/liblua.a b/lib/liblua.a
new file mode 100644
index 0000000..d5a4c6b
Binary files /dev/null and b/lib/liblua.a differ
diff --git a/link.ld b/link.ld
index 8877898..13e5cca 100644
--- a/link.ld
+++ b/link.ld
@@ -35,9 +35,6 @@ ENTRY(Reset_Handler)
 /* Highest address of the user mode stack */
 _estack = 0x20018000;    /* end of RAM */
 
-_Min_Stack_Size = 0x0400; /* 1k min. stack */
-_Min_Heap_Size = 0x0400; /* 1k min. heap */
-
 /* Specify the memory areas */
 MEMORY
 {
@@ -82,14 +79,14 @@ SECTIONS
     . = ALIGN(8);
   } >FLASH
 
-  .ARM.extab   : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
+  .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
   .ARM : {
     __exidx_start = .;
     *(.ARM.exidx*)
     __exidx_end = .;
   } >FLASH
 
-  .preinit_array     :
+  .preinit_array :
   {
     PROVIDE_HIDDEN (__preinit_array_start = .);
     KEEP (*(.preinit_array*))
@@ -146,10 +143,7 @@ SECTIONS
   ._user_heap_stack :
   {
     . = ALIGN(4);
-    PROVIDE ( end = . );
-    PROVIDE ( _end = . );
-    . = . + _Min_Heap_Size;
-    . = . + _Min_Stack_Size;
+    *(._user_heap_stack)
     . = ALIGN(4);
   } >RAM
 
@@ -166,4 +160,3 @@ SECTIONS
   .ARM.attributes 0 : { *(.ARM.attributes) }
 }
 
-_total_ram = _estack - _end;
diff --git a/main.c b/main.c
deleted file mode 100644
index 9a95465..0000000
--- a/main.c
+++ /dev/null
@@ -1,75 +0,0 @@
-#include <stm32l476xx.h>
-#include <clock.h>
-#include <heap.h>
-#include <task.h>
-
-/**
- * Accomplishments:
- *   - GPIO in/out
- *   - got to 80MHz clock
- *   - basic heap
- *   - multitask, can exit
- */
-
-void pulse(uint8_t byte);
-void kmain(void);
-
-int main(void)
-{
-	asm("cpsid i");
-
-	// prepare flash latency for 40MHz operation
-	FLASH->ACR &= ~(FLASH_ACR_LATENCY);
-	FLASH->ACR |= FLASH_ACR_LATENCY_4WS;
-
-	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;	// A clk enable
-	GPIOA->MODER &= ~(GPIO_MODER_MODE5 | GPIO_MODER_MODE6);	// A5 -> output, A0 input
-	GPIOA->MODER |= GPIO_MODER_MODE5_0 | GPIO_MODER_MODE6_0;
-	GPIOA->PUPDR &= ~(GPIO_PUPDR_PUPD5 | GPIO_PUPDR_PUPD6);
-	GPIOA->PUPDR |= GPIO_PUPDR_PUPD5_0 | GPIO_PUPDR_PUPD6_0; // pulldown for button (1)
-	//if (GPIOA->IDR & 0x01)
-
-	clock_init();
-	task_init(kmain);
-
-	while (1);
-}
-
-void task(void);
-void kmain(void)
-{
-	asm("cpsie i");
-
-	task_start(task, 1024);
-	while (1) {
-		GPIOA->BSRR |= 1 << 6;
-		delay(500);
-		GPIOA->BRR |= 1 << 6;
-		delay(500);
-	}
-}
-
-void task(void)
-{
-	while (1) {
-		GPIOA->BSRR |= 1 << 5;
-		delay(200);
-		GPIOA->BRR |= 1 << 5;
-		delay(200);
-	}
-}
-
-
-void _exit(int code)
-{ for (;;); }
-
-void pulse(uint8_t byte)
-{
-	int8_t i = 7;
-	do {
-		GPIOA->BSRR |= 1 << 5;
-		delay((byte & (1 << i)) ? 400 : 100);
-		GPIOA->BRR |= 1 << 5;
-		delay((byte & (1 << i)) ? 100 : 400);
-	} while (--i >= 0);
-}
diff --git a/mkinitrd.sh b/mkinitrd.sh
new file mode 100755
index 0000000..eb47fd8
--- /dev/null
+++ b/mkinitrd.sh
@@ -0,0 +1,4 @@
+#!/bin/bash
+echo "Making initrd.img..."
+rm -f initrd.img
+arm-none-eabi-ar r initrd.img initrd/*
diff --git a/clock.c b/src/clock.c
similarity index 100%
rename from clock.c
rename to src/clock.c
diff --git a/src/gpio.c b/src/gpio.c
new file mode 100644
index 0000000..c2617ee
--- /dev/null
+++ b/src/gpio.c
@@ -0,0 +1,44 @@
+#include <gpio.h>
+
+void gpio_init(void)
+{
+	// enable clocks
+	RCC->AHB2ENR |= 0xFF;
+}
+
+void gpio_pupd(GPIO_TypeDef *port, uint8_t pin, uint8_t pupd)
+{
+	port->PUPDR &= ~(0x03 << (2 * pin));
+	port->PUPDR |= pupd << (2 * pin);
+}
+
+void gpio_speed(GPIO_TypeDef *port, uint8_t pin, uint8_t speed)
+{
+	port->OSPEEDR &= ~(0x03 << (2 * pin));
+	port->OSPEEDR |= speed << (2 * pin);
+}
+
+void gpio_type(GPIO_TypeDef *port, uint8_t pin, uint8_t type)
+{
+	port->OTYPER &= ~(1 << pin);
+	port->OTYPER |= type << pin;
+}
+
+void gpio_mode(GPIO_TypeDef *port, uint8_t pin, uint8_t mode)
+{
+	port->MODER &= ~(0x03 << (2 * pin));
+	port->MODER |= mode << (2 * pin);
+}
+
+void gpio_dout(GPIO_TypeDef *port, uint8_t pin, uint8_t val)
+{
+	if (val)
+		port->BSRR |= (1 << pin);
+	else
+		port->BRR |= (1 << pin);
+}
+
+uint8_t gpio_din(GPIO_TypeDef *port, uint8_t pin)
+{
+	return port->IDR & (1 << pin);
+}
diff --git a/heap.c b/src/heap.c
similarity index 76%
rename from heap.c
rename to src/heap.c
index 0433a1f..1eba901 100644
--- a/heap.c
+++ b/src/heap.c
@@ -3,12 +3,12 @@
 
 #define RAM_END 0x20018000
 
-//extern void *end;
-//extern uint32_t _total_ram;
+#define HEAP_SIZE (16 * 1024)
+
 static uint32_t offset = 0;
 
-__attribute__ ((section(".data")))
-uint8_t heap[8192];
+__attribute__ ((section("._user_heap_stack")))
+uint8_t heap[HEAP_SIZE];
 void *end = heap;
 
 void heap_init(void)
@@ -18,7 +18,7 @@ void heap_init(void)
 
 uint32_t heap_available(void)
 {
-	return 0;//	return _total_ram - offset;
+	return HEAP_SIZE - offset;
 }
 
 void *hmalloc(uint32_t size)
diff --git a/src/initrd.c b/src/initrd.c
new file mode 100644
index 0000000..7b68c40
--- /dev/null
+++ b/src/initrd.c
@@ -0,0 +1,90 @@
+#include <initrd.h>
+
+extern uint8_t _binary_initrd_img_start[];
+extern uint8_t _binary_initrd_img_size[];
+
+static const void *initrd_start = (void *)_binary_initrd_img_start;
+static const uint32_t initrd_size = (uint32_t)_binary_initrd_img_size;
+
+static const char *initrd_sig = "!<arch>\n";
+
+uint8_t initrd_validate(void)
+{
+	initrd_header *header = (initrd_header *)initrd_start;
+	for (uint8_t i = 0; i < 8; i++) {
+		if (header->signature[i] != initrd_sig[i])
+			return 0;
+	}
+
+	return 1;
+}
+
+uint8_t initrd_nametest(char *file, const char *want)
+{
+	for (uint8_t i = 0; i < 16; i++) {
+		if (want[i] == '\0')
+			return (file[i] == '/');
+		else if (want[i] != file[i])
+			return 0;
+	}
+
+	return 0;
+}
+
+uint32_t pow10(uint8_t n)
+{
+	uint32_t i = 1;
+	while (n--)
+		i *= 10;
+	return i;
+}
+
+uint32_t initrd_getsize(initrd_file *file)
+{
+	uint32_t size = 0;
+	char *p = file->size + 10;
+	while (*--p == ' ');
+
+	for (int8_t i = p - file->size, j = 0; i >= 0; i--, j++)
+		size += (*p-- - '0') * pow10(j);
+
+	return size;
+}
+
+initrd_file *initrd_getfileptr(const char *name)
+{
+	initrd_file *file = (initrd_file *)(initrd_start + sizeof(initrd_header));
+	uint32_t offset = sizeof(initrd_header);
+
+	while (offset < initrd_size) {
+		if (initrd_nametest(file->name, name))
+			return file;
+		uint32_t size = initrd_getsize(file) + sizeof(initrd_file);
+		offset += size;
+		file += size;
+	}
+
+	return 0;
+}
+
+char *initrd_getfile(const char *name)
+{
+	initrd_file *file = initrd_getfileptr(name);
+	if (file == 0)
+		return 0;
+
+
+	char *ptr = (char *)((void *)file + sizeof(initrd_file));
+	ptr[initrd_getsize(file) - 1] = 0;
+	return ptr;
+}
+
+uint32_t initrd_getfilesize(const char *name)
+{
+	initrd_file *file = initrd_getfileptr(name);
+	if (file == 0)
+		return 0;
+
+	return initrd_getsize(file);
+}
+
diff --git a/src/lcd.c b/src/lcd.c
new file mode 100644
index 0000000..ca1c1a6
--- /dev/null
+++ b/src/lcd.c
@@ -0,0 +1,117 @@
+#include <lcd.h>
+#include <clock.h>
+#include <gpio.h>
+
+#define LCD_D0 GPIO_PORT(A, 0)
+#define LCD_D1 GPIO_PORT(A, 1)
+#define LCD_D2 GPIO_PORT(A, 4)
+#define LCD_D3 GPIO_PORT(B, 0)
+#define LCD_D4 GPIO_PORT(C, 1)
+#define LCD_D5 GPIO_PORT(C, 0)
+#define LCD_D6 GPIO_PORT(C, 2)
+#define LCD_D7 GPIO_PORT(C, 3)
+#define LCD_E  GPIO_PORT(C, 12)
+#define LCD_RS GPIO_PORT(C, 10)
+
+#define lcd_data() gpio_dout(LCD_RS, 1)
+
+void lcd_pulse(void)
+{
+	gpio_dout(LCD_E, 1);
+	delay(1);
+	gpio_dout(LCD_E, 0);
+}
+
+void lcd_byte(uint8_t byte)
+{
+	gpio_dout(LCD_D0, byte & 0x01);
+	gpio_dout(LCD_D1, byte & 0x02);
+	gpio_dout(LCD_D2, byte & 0x04);
+	gpio_dout(LCD_D3, byte & 0x08);
+	gpio_dout(LCD_D4, byte & 0x10);
+	gpio_dout(LCD_D5, byte & 0x20);
+	gpio_dout(LCD_D6, byte & 0x40);
+	gpio_dout(LCD_D7, byte & 0x80);
+}
+
+void lcd_cmd(uint8_t cmd)
+{
+	gpio_dout(LCD_RS, 0);
+	lcd_byte(cmd);
+	lcd_pulse();
+}
+
+void lcd_putchar(int c)
+{
+	lcd_data();
+	lcd_byte((uint8_t)c);
+	lcd_pulse();
+}
+
+static int lcd_index = 0;
+void lcd_puts(const char *s)
+{
+	lcd_cmd(0x06);
+	while (*s) {
+		lcd_putchar(*s++);
+		if (++lcd_index == 0x10) {
+			lcd_cmd(0x80 | 0x40);
+		} else if (lcd_index == 0x20) {
+			lcd_cmd(0x80);
+			lcd_index = 0;
+		}
+	}
+}
+
+extern char *itoa(int n, int base);
+void lcd_puti(int i)
+{
+	lcd_puts(itoa(i, 10));
+}
+
+void lcd_puth(int h)
+{
+	lcd_puts(itoa(h, 16));
+}
+
+void lcd_putb(uint8_t b)
+{
+	lcd_puts(itoa(b, 2));
+}
+
+void lcd_clear(void)
+{
+	lcd_cmd(0x01);
+	delay(2);
+	lcd_index = 0;
+}
+
+void lcd_init(void)
+{
+	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);
+	gpio_mode(LCD_E,  OUTPUT);
+	gpio_mode(LCD_RS, OUTPUT);
+	gpio_dout(LCD_D0, 0);
+	gpio_dout(LCD_D1, 0);
+	gpio_dout(LCD_D2, 0);
+	gpio_dout(LCD_D3, 0);
+	gpio_dout(LCD_D4, 0);
+	gpio_dout(LCD_D5, 0);
+	gpio_dout(LCD_D6, 0);
+	gpio_dout(LCD_D7, 0);
+	gpio_dout(LCD_E,  0);
+	gpio_dout(LCD_RS, 0);
+
+	lcd_cmd(0x38);
+	lcd_cmd(0x10);
+	lcd_cmd(0x0D);
+	delay(5);
+	lcd_clear();
+}
diff --git a/src/main.c b/src/main.c
new file mode 100644
index 0000000..e71e712
--- /dev/null
+++ b/src/main.c
@@ -0,0 +1,56 @@
+#include <stm32l476xx.h>
+#include <clock.h>
+#include <heap.h>
+#include <task.h>
+#include <gpio.h>
+#include <lcd.h>
+#include <initrd.h>
+
+/**
+ * Accomplishments:
+ *   - GPIO in/out
+ *   - got to 80MHz clock
+ *   - basic heap
+ *   - multitask, can exit
+ *   - gpio lib
+ *   - lcd support
+ *   - initrd support
+ *   - lua?
+ */
+
+void pulse(uint8_t byte);
+void kmain(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);
+
+	task_init(kmain);
+
+	while (1);
+}
+
+void task(void);
+void kmain(void)
+{
+	asm("cpsie i");
+
+	lcd_init();
+
+	char *s = initrd_getfile("test.txt");
+	lcd_puts(s);
+
+	while (1)
+		delay(100);
+}
+
diff --git a/startup_stm32l476xx.s b/src/startup_stm32l476xx.s
similarity index 100%
rename from startup_stm32l476xx.s
rename to src/startup_stm32l476xx.s
diff --git a/src/stdlib.c b/src/stdlib.c
new file mode 100644
index 0000000..e7bf622
--- /dev/null
+++ b/src/stdlib.c
@@ -0,0 +1,17 @@
+#include <stdint.h>
+
+void _exit(int code)
+{
+	(void)code;
+	for (;;);
+}
+
+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;
+}
diff --git a/stm32l4xx_it.c b/src/stm32l4xx_it.c
similarity index 87%
rename from stm32l4xx_it.c
rename to src/stm32l4xx_it.c
index dc6dc16..2aac023 100644
--- a/stm32l4xx_it.c
+++ b/src/stm32l4xx_it.c
@@ -26,7 +26,9 @@ void UsageFault_Handler(void)
 	while (1);
 }
 
-void SVC_Handler(void) {}
+void SVC_Handler(void) {
+
+}
 
 void DebugMon_Handler(void) {}
 
diff --git a/system_stm32l4xx.c b/src/system_stm32l4xx.c
similarity index 100%
rename from system_stm32l4xx.c
rename to src/system_stm32l4xx.c
diff --git a/task.c b/src/task.c
similarity index 89%
rename from task.c
rename to src/task.c
index b77d4f5..a34988a 100644
--- a/task.c
+++ b/src/task.c
@@ -12,7 +12,9 @@ typedef struct {
 #define MAX_TASKS	4
 
 static task_t tasks[MAX_TASKS];
-static int next_idx = 0;
+static volatile int next_idx = 0;
+
+static uint8_t task_enable = 0;
 
 void task_init(void (*init)(void))
 {
@@ -23,15 +25,13 @@ void task_init(void (*init)(void))
 	asm("\
 		msr psp, %0; \
 		mrs r0, control; \
-		orr r0, r0, #2; \
+		orr r0, r0, #3; \
 		msr control, r0; \
 		isb; \
 	" :: "r" (tasks[0].sp));
 
+	task_enable = 1;
 	init();
-
-	// force switch to tasking, call pendsv
-	//SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
 }
 
 extern void _exit(int);
@@ -42,7 +42,6 @@ void task_exit(void)
 	tasks[next_idx].use = 0;
 	asm("cpsie i");
 	while (1); // bye
-	//_exit(0);
 }
 
 void task_start(void (*task)(void), uint16_t stackSize)
@@ -69,8 +68,10 @@ void task_start(void (*task)(void), uint16_t stackSize)
 __attribute__ ((naked))
 void PendSV_Handler(void) 
 {
+	if (task_enable == 0)
+		asm("bx lr");
+
 	// save state
-	//stmdb r0!, {r4-r11};
 	asm("\
 		cpsid i; \
 		isb; \
@@ -89,7 +90,6 @@ void PendSV_Handler(void)
 	} while (tasks[next_idx].use == 0);
 
 	// restore
-	//ldmia r0!, {r4-r11};
 	asm("\
 		mov r0, %0; \
 		ldmia r0!, {r4-r11}; \