fernly: wip

Makefile

@@ -1,7 +1,7 @@
 include mkenv.mk
 include magic.mk
 
-CFLAGS = -mtune=arm7tdmi -mcpu=arm7tdmi -mfloat-abi=soft -Wall \
+CFLAGS = -march=armv5te -mfloat-abi=soft -Wall \
 	 -O0 -ggdb -Iinclude
 
 LDFLAGS = --nostdlib -T fernvale.ld
@@ -12,7 +12,8 @@ SRC_C = \
 	vectors.c \
 	serial.c \
 	utils.c \
-	bionic.c
+	bionic.c \
+	vsprintf.c
 
 SRC_S = \
 	start.S

README.md

@@ -40,24 +40,34 @@ Memory Map
 +------------+------------+------------+-------------------------------------+
 | 0x80000000 | 0x80000008 |       0x08 | Config block (chip version, etc.)   |
 +------------+------------+------------+-------------------------------------+
+| 0x82200000 | ?????????? | ?????????? |                                     |
++------------+------------+------------+-------------------------------------+
+| 0x83000000 | ?????????? | ?????????? |                                     |
++------------+------------+------------+-------------------------------------+
 | 0xa0000000 | 0xa0000008 |       0x08 | Config block (mirror?)              |
 +------------+------------+------------+-------------------------------------+
-| 0xa0010000 | ?????????? | ?????????? | ??????????????????????????????????? |
+| 0xa0010000 | ?????????? | ?????????? | (?SPI mode?) ?????????????????????? |
 +------------+------------+------------+-------------------------------------+
 | 0xa0020000 | 0xa0020e10 |     0x0e10 | GPIO control block                  |
 +------------+------------+------------+-------------------------------------+
 | 0xa0030000 | 0xa0030040 |       0x40 | WDT block                           |
 +------------+------------+------------+-------------------------------------+
+| 0xa0030800 | ?????????? | ?????????? | ????????????????????????????        |
++------------+------------+------------+-------------------------------------+
+| 0xa0040000 | ?????????? | ?????????? | ??????????????????????????????????? |
++------------+------------+------------+-------------------------------------+
 | 0xa0050000 | ?????????? | ?????????? | ??????????????????????????????????? |
 +------------+------------+------------+-------------------------------------+
 | 0xa0060000 | ?????????? | ?????????? | ?? Possible IRQs at 0xa0060200 ???? |
 +------------+------------+------------+-------------------------------------+
-| 0xa0070000 | ?????????? | ?????????? | ??????????????????????????????????? |
+| 0xa0070000 | ========== | ========== | == Empty (all zeroes) ============= |
 +------------+------------+------------+-------------------------------------+
 | 0xa0080000 | 0xa008005c |       0x5c | UART1 block                         |
 +------------+------------+------------+-------------------------------------+
 | 0xa0090000 | 0xa009005c |       0x5c | UART2 block                         |
 +------------+------------+------------+-------------------------------------+
+| 0xa00a0000 | ?????????? | ?????????? | ??????????????????????????????????? |
++------------+------------+------------+-------------------------------------+
 | 0xa00b0000 | 0xa00b006c |       0x6c | Bluetooth interface block           |
 +------------+------------+------------+-------------------------------------+
 | 0xa00c0000 | 0xa00c002c |       0x2c | General purpose timer block         |
@@ -126,6 +136,10 @@ Memory Map
 +------------+------------+------------+-------------------------------------+
 | 0xa0540000 | ?????????? | ?????????? | (contains smattering of bits) ????? |
 +------------+------------+------------+-------------------------------------+
+| 0xa0700000 | ?????????? | ?????????? | Power management block              |
+|            |            |            | Write (val & 0xfe0f | 0x140) to     |
+|            |            |            | 0xa0700230 to power off.            |
++------------+------------+------------+-------------------------------------+
 | 0xa0710000 | 0xa0710078 |       0x78 | RTC block                           |
 +------------+------------+------------+-------------------------------------+
 | 0xa0720000 | ?????????? | ?????????? | ??????????????????????????????????? |
@@ -138,7 +152,17 @@ Memory Map
 +------------+------------+------------+-------------------------------------+
 | 0xa0750000 | 0xa075005c |       0x5c | ADCDET block                        |
 +------------+------------+------------+-------------------------------------+
+| 0xa0760000 | ?????????? | ?????????? | ??????????????????????????????????? |
++------------+------------+------------+-------------------------------------+
 | 0xa0790000 | 0xa07900d8 |       0xd8 | ADC block                           |
 +------------+------------+------------+-------------------------------------+
 | 0xa0900000 | 0xa0900240 |       0x33 | USB block                           |
 +------------+------------+------------+-------------------------------------+
+| 0xf00d1000 | 0xf00db000 | ?????????? |                                     |
++------------+------------+------------+-------------------------------------+
+| 0xf0115500 | ?????????? | ?????????? |                                     |
++------------+------------+------------+-------------------------------------+
+| 0xf0133300 | ?????????? | ?????????? |                                     |
++------------+------------+------------+-------------------------------------+
+| 0xf0243c00 | 0xf0244200 | ?????????? |                                     |
++------------+------------+------------+-------------------------------------+

bionic.c

@@ -239,7 +239,8 @@ done:
 #endif
 }
 
-static int _isspace(char c) {
+int _isspace(char c)
+{
 	return (c == ' '
 		|| c == '\f'
 		|| c == '\n'
@@ -248,22 +249,45 @@ static int _isspace(char c) {
 		|| c == '\v');
 }
 
-static int _isdigit(char c) {
+int _isdigit(char c)
+{
 	return (c >= '0' && c <= '9');
 }
 
-static int _isupper(char c) {
+int _isxdigit(char c)
+{
+	return ((c >= '0' && c <= '9') ||
+		(c >= 'a' && c <= 'f') ||
+		(c >= 'A' && c <= 'F'));
+}
+
+int _isupper(char c)
+{
 	return (c >= 'A' && c <= 'Z');
 }
 
-static int _islower(char c) {
+int _islower(char c)
+{
 	return (c >= 'a' && c <= 'z');
 }
 
-static int _isalpha(char c) {
+int _isalpha(char c)
+{
 	return _isupper(c) || _islower(c);
 }
 
+int _isalnum(char c)
+{
+	return _isalpha(c) || _isdigit(c);
+}
+
+int _toupper(char c)
+{
+	if (!_islower(c))
+		return c;
+	return c + ('a' - 'A');
+}
+
 static int get_cuts(int base, unsigned long *cutoff, int *cutlim)
 {
 	switch(base) {
@@ -661,3 +685,16 @@ void _memset(void *dst0, char val, size_t length)
 		*ptr++ = val;
 }
 
+int _strlen(const char *s)
+{
+	int i = 0;
+	while(s[i++]);
+	return i;
+}
+
+int _strnlen(const char *s, int maxlen)
+{
+	int i = 0;
+	while(s[i++] && i < maxlen);
+	return i;
+}

include/bionic.h

@@ -8,4 +8,18 @@ int _strcasecmp(const char *s1, const char *s2);
 void *_memcpy(void *dst0, const void *src0, size_t length);
 void _memset(void *dst0, char val, size_t length);
 unsigned long _strtoul(const void *nptr, void **endptr, int base);
+int _strlen(const char *s);
+int _strnlen(const char *s, int maxlen);
+
+
+/* ctype replacements */
+int _isspace(char c);
+int _isdigit(char c);
+int _isxdigit(char c);
+int _isupper(char c);
+int _islower(char c);
+int _isalpha(char c);
+int _isalnum(char c);
+int _toupper(char c);
+
 #endif /* __BIONIC_H__ */

include/utils.h

@@ -2,4 +2,7 @@
 #define __UTILS_H__
 #include <stdint.h>
 
+uint32_t _udiv64(uint64_t n, uint32_t d);
+int printf(const char *fmt, ...);
+
 #endif /* __UTILS_H__ */

main.c

@@ -3,7 +3,13 @@
 #include "utils.h"
 #include "bionic.h"
 
+//#define AUTOMATED
+
+#if defined(AUTOMATED)
+#define PROMPT "fernly> \n"
+#else
 #define PROMPT "fernly> "
+#endif
 
 static int serial_get_line(uint8_t *bfr, int len)
 {
@@ -11,7 +17,9 @@ static int serial_get_line(uint8_t *bfr, int len)
 
 	while (cur < len) {
 		bfr[cur] = serial_getc();
+#if !defined(AUTOMATED)
 		serial_putc(bfr[cur]);
+#endif
 
 		/* Carriage Return */
 		if (bfr[cur] == '\n') {
@@ -101,28 +109,18 @@ static int memory_test(uint32_t start, uint32_t length)
 	register uint32_t test;
 	uint32_t end = start + length;
 
-	serial_puts("Testing memory from ");
+	printf("Testing memory from %08x to %08x", start, end);
-	serial_puth(start, 8);
-	serial_puts(" to ");
-	serial_puth(end, 8);
-	serial_puts("");
 
 	for (addr = start; addr < end; addr += 4) {
-		if (! (addr & 0xffff)) {
+		if (! (addr & 0xffff))
-			serial_puts("\nAddress ");
+			printf("\nAddress %08x...", addr);
-			serial_puth(addr, 8);
-			serial_puts("...");
-		}
 
 		writel(val, addr);
 		test = readl(addr);
-		if (test != val) {
+		if (test != val)
-			serial_puts("Address ");
+			printf("Address %08x appears to be invalid!  "
-			serial_puth(addr, 8);
+				"Expected 0xdeadbeef, got 0x%08x\n",
-			serial_puts(" appears to be invalid!  Expected 0xdeadbeef, got 0x");
+				addr, test);
-			serial_puth(test, 8);
-			serial_puts("\n");
-		}
 	}
 
 	return 0;
@@ -130,7 +128,15 @@ static int memory_test(uint32_t start, uint32_t length)
 
 static int wdt_kick(void)
 {
+//	writel(0x808, 0xa0030004);
 	writel(0x1971, 0xa0030008);
+//	writel(readl(0xa0030000) | 0x2201, 0xa0030000);
+	return 0;
+}
+
+static int wdt_reboot(void)
+{
+	writel(0x1209, 0xa003001c);
 	return 0;
 }
 
@@ -140,28 +146,18 @@ static int memory_search(uint32_t start, uint32_t length, uint32_t nonce, uint32
 	register uint32_t test;
 	uint32_t end = start + length;
 
-	serial_puts("Looking in memory from ");
+	printf("Looking in memory from %08x to %08x for nonce %08x\n",
-	serial_puth(start, 8);
+			start, end, nonce);
-	serial_puts(" to ");
-	serial_puth(end, 8);
-	serial_puts(" for nonce ");
-	serial_puth(nonce, 8);
-	serial_puts("\n");
 
 	for (addr = start; addr < end; addr += 0x800) {
 	//	if (! (addr & 0xffff)) {
-			serial_puts("\rAddress ");
+			printf("\rAddress %08x...", addr);
-			serial_puth(addr, 8);
-			serial_puts("...");
 			wdt_kick();
 	//	}
 
 		test = readw(addr);
-		if ((test & mask) == nonce) {
+		if ((test & mask) == nonce)
-			serial_puts("    ");
+			printf("    %08x matches nonce\n", addr);
-			serial_puth(addr, 8);
-			serial_puts(" matches nonce\n");
-		}
 	}
 
 	return 0;
@@ -171,102 +167,102 @@ static int list_registers(void)
 {
 	int var;
 
-	serial_puts("Registers:\n");
+	printf("Registers:\n");
 
-	serial_puts("CPSR: ");
+	printf("CPSR: ");
 	asm volatile ("mrs %0, cpsr":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("SPSR: ");
+	printf("SPSR: ");
 	asm volatile ("mrs %0, spsr":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R0:   ");
+	printf("R0:   ");
 	asm volatile ("mov %0, r0":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R1:   ");
+	printf("R1:   ");
 	asm volatile ("mov %0, r1":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R2:   ");
+	printf("R2:   ");
 	asm volatile ("mov %0, r2":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R3:   ");
+	printf("R3:   ");
 	asm volatile ("mov %0, r3":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R4:   ");
+	printf("R4:   ");
 	asm volatile ("mov %0, r4":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R5:   ");
+	printf("R5:   ");
 	asm volatile ("mov %0, r5":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R6:   ");
+	printf("R6:   ");
 	asm volatile ("mov %0, r6":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R7:   ");
+	printf("R7:   ");
 	asm volatile ("mov %0, r7":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R8:   ");
+	printf("R8:   ");
 	asm volatile ("mov %0, r8":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R9:   ");
+	printf("R9:   ");
 	asm volatile ("mov %0, r9":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R10:  ");
+	printf("R10:  ");
 	asm volatile ("mov %0, r10":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("FP:   ");
+	printf("FP:   ");
 	asm volatile ("mov %0, r11":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("IP:   ");
+	printf("IP:   ");
 	asm volatile ("mov %0, r12":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("SP:   ");
+	printf("SP:   ");
 	asm volatile ("mov %0, r13":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R14:  ");
+	printf("R14:  ");
 	asm volatile ("mov %0, r14":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("R15:  ");
+	printf("R15:  ");
 	asm volatile ("mov %0, r15":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
-	serial_puts("LR:   ");
+	printf("LR:   ");
 	asm volatile ("mov %0, lr":"=r" (var));
 	serial_puth(var, 8);
-	serial_puts("\n");
+	printf("\n");
 
 	return 0;
 }
@@ -274,10 +270,7 @@ static int list_registers(void)
 static int read_address(uint8_t *arg)
 {
 	uint32_t addr = _strtoul(arg, 0, 16);
-	serial_puth(addr, 16);
+	printf("%08x: %08x\n", addr, readl(addr));
-	serial_puts(": ");
-	serial_puth(readl(addr), 16);
-	serial_puts("\n");
 	return 0;
 }
 
@@ -287,39 +280,269 @@ static int write_address(uint8_t *arg)
 	uint32_t val;
 	uint32_t addr = _strtoul(arg, (void **)&valpos, 16);
 	val = _strtoul(valpos, 0, 16);
-	serial_puth(addr, 16);
+	printf("%08x: %08x\n", addr, val);
-	serial_puts(": ");
-	serial_puth(val, 16);
-	serial_puts("\n");
 	writel(val, addr);
 	return 0;
 }
 
 static int hex_print(uint8_t *arg)
 {
-	uint32_t *addr = (uint32_t *)_strtoul(arg, 0, 16);
+	uint8_t *valpos;
-	uint32_t data[1024];
+	uint32_t *addr = (uint32_t *)_strtoul(arg, (void **)&valpos, 16);
+	uint32_t count = 0;
 	int i;
+
+	if (valpos)
+		count = _strtoul(valpos, 0, 16);
+	if (!count)
+		count = 256;
+
+	uint32_t data[count / 4];
 	for (i = 0; i < ((sizeof(data) / sizeof(*data))); i++)
 		data[i] = addr[i];
 
-	serial_puth((uint32_t)addr, 8);
+	printf("%08x:\n", (uint32_t)addr);
-	serial_puts(":\n");
 	serial_print_hex(data, sizeof(data));
-	serial_puts("\n");
+	printf("\n");
+	return 0;
+}
+
+#define IRQ_BASE 0xa0060180
+
+#define IRQ_MASK_SET_LOW 0x20
+#define IRQ_MASK_SET_HIGH 0x24
+
+#define IRQ_MASK_CLEAR_LOW 0x40
+#define IRQ_MASK_CLEAR_HIGH 0x44
+
+#define IRQ_SCAN_DEPTH 0x1000
+#define IRQ_SCAN_START 0x020
+static int find_irqs(void)
+{
+	/* There should be a mask register, a mask-clear register, and a
+	   mask-set register.  Look through memory for a register containing
+	   the value 0xffffffff.  Then, try setting it to 0x00000000.  If
+	   that fails, try another address.
+
+	   If that succeeds, mark it as a candidate for the IRQ Mask
+	   Register.
+
+	   Then, begin writing 0xffffffff to other registers, trying to make
+	   the candidate go from 0x00000000 to 0xffffffff.  If no candidate
+	   is found, continue.
+
+	   If you find this, then you have a good candidate for the IRQ Mask
+	   Set register.  Begin looking again for a register that, when
+	   written to, sets the mask register to 0x00000000 again.
+	 */
+
+	int base_num;
+	uint32_t base = 0xa0050000;
+
+	printf("Trying to look for IRQ tables...\n");
+
+	writel(0, 0xa0180820);
+	//for (base = 0xa0180850; base < 0xa0180a00; base += 4) {
+	for (base = 0xa0010410; base < 0xa0030500; base += 4) {
+		printf("Investigating addr 0x%08x...\n", base);
+		writel(1, base);
+	}
+
+#if 0
+	{
+		uint32_t mask_offset;
+		uint32_t set_offset;
+		uint32_t clear_offset;
+
+		//if (!(base & 0x7ffff))
+			printf("Investigating base 0x%08x...\n", base);
+		wdt_kick();
+		/* Look for mask register */
+		for (mask_offset = IRQ_SCAN_START;
+			mask_offset < IRQ_SCAN_DEPTH;
+			mask_offset += 4) {
+
+			/* In our little world, offset registers can be set
+			 * to 0xffffffff */
+
+			/* Try setting the register, make sure it sets */
+			writel(0xffffffff, mask_offset + base);
+			if (readl(mask_offset + base) == 0)
+				continue;
+
+			/* Try clearing the register, make sure it clears */
+			writel(0x00000000, mask_offset + base);
+			if (readl(mask_offset + base) != 0x00000000)
+				continue;
+
+			writel(0x00000000, mask_offset + base);
+			if (readl(mask_offset + base) != 0x00000000)
+				continue;
+
+			writel(0xffffffff, mask_offset + base);
+			if (readl(mask_offset + base) == 0)
+				continue;
+
+			writel(0xffffffff, mask_offset + base);
+			if (readl(mask_offset + base) == 0)
+				continue;
+
+			printf("    Candidate mask register at 0x%08x\n",
+					base + mask_offset);
+
+			/* Candidate mask offset.  Look for clear
+			 * register */
+			wdt_kick();
+			for (clear_offset = (mask_offset - 0x20);
+				clear_offset < (mask_offset + 0x20);
+				clear_offset += 4) {
+				uint32_t old_mask;
+
+				if (clear_offset == mask_offset)
+					continue;
+
+				writel(0xffffffff, mask_offset + base);
+				old_mask = readl(mask_offset + base);
+				writel(0xffffffff, clear_offset + base);
+				if (readl(mask_offset + base) == old_mask)
+					continue;
+
+				printf("    Candidate clear register at 0x%08x\n",
+						base + clear_offset);
+
+				/* Candidate for mask and clear offsets,
+				 * look for set offset
+				 */
+				wdt_kick();
+				for (set_offset = (mask_offset - 0x20);
+					set_offset < (mask_offset + 0x20);
+					set_offset += 4) {
+
+					if (set_offset == mask_offset)
+						continue;
+
+					if (set_offset == clear_offset)
+						continue;
+
+					/* If we write and the mask changes, we
+					 * might have a set register */
+					writel(0x00000000, mask_offset + base);
+					old_mask = readl(mask_offset + base);
+					writel(0xffffffff, set_offset + base);
+					if (readl(mask_offset + base) == old_mask)
+						continue;
+
+
+
+	printf("Candidate for IRQ mask/set/clear:\n");
+	printf("    Base:  0x%08x\n", base);
+	printf("    Mask:  0x%08x\n", mask_offset);
+	printf("    Set:   0x%08x\n", set_offset);
+	printf("    Clear: 0x%08x\n", clear_offset);
+	while(1)
+		wdt_kick();
+
+				}
+			}
+		}
+	}
+#endif
+	return 0;
+}
+
+static int enable_irq(void)
+{
+	int var;
+	asm volatile ("mrs %0, cpsr":"=r" (var));
+	if (!(var & 0x80)) {
+		printf("Interrupts already enabled\n");
+		return -1;
+	}
+
+	printf("Interrupts were disabled.  Re-enabling...\n");
+	var &= ~0x80;
+	var &= ~0x1f;
+	var |= 0x10;
+	asm volatile ("msr cpsr, %0":"=r" (var));
+
+	return 0;
+}
+
+static int enable_fiq(void)
+{
+	int var;
+	asm volatile ("mrs %0, cpsr":"=r" (var));
+	if (!(var & 0x40)) {
+		printf("FIQ already enabled\n");
+		return -1;
+	}
+
+	printf("FIQ was disabled.  Re-enabling...\n");
+	var &= ~0x40;
+	asm volatile ("msr cpsr, %0":"=r" (var));
+
+	return 0;
+}
+
+static int setup_priorities(void)
+{
+	writel(0x43415453, 0xf0244a88);
+	writel(0x444e454b, 0xf0244a8c);
+
+	writel(0x43415453, 0x7000a5d8);
+	writel(0x444e454b, 0x7000a5dc);
+
+	writel(0x43415453, 0xf0244a08);
+	writel(0x444e454b, 0xf0244a0c);
+
+	writel(0x43415453, 0xf0244908);
+	writel(0x444e454b, 0xf024490c);
+
+	writel(0x43415453, 0xf0244988);
+	writel(0x444e454b, 0xf0244988);
+
+	writel(0x43415453, 0xf0244988);
+	writel(0x444e454b, 0xf024498c);
+
+	writel(0x43415453, 0x7000a680);
+	writel(0x444e454b, 0x7000a684);
+	return 0;
+}
+
+static int do_init(void)
+{
+	void *rv_start = (void *)0x10003460;
+	void *rv_end = (void *)0x100034e0;
+	serial_init();
+	setup_priorities();
+
+	/* Kick WDT */
+	writel(0x1971, 0xa0030008);
+
+	printf("\n\nFernly shell\n");
+	printf("Copying vectors");
+	printf("   Src: %p", rv_start);
+	printf(" Src end: %p", rv_end);
+	printf(" Size: %d\n", rv_end - rv_start);
+	printf("(%d bytes from %p to 0)...\n", rv_end - rv_start, rv_start);
+	_memcpy((void *)0, rv_start, rv_end - rv_start);
+	enable_irq();
+	enable_fiq();
+
 	return 0;
 }
 
 int main(int argc, char **argv)
 {
-	serial_init();
+	do_init();
-	serial_puts("\n\nFernly shell\n");
 
 	while (1) {
 		uint8_t line[256];
-		serial_puts(PROMPT);
+		printf(PROMPT);
 		serial_get_line(line, sizeof(line));
-		serial_puts("\n");
+#if !defined(AUTOMATED)
+		printf("\n");
+#endif
 
 		switch(*line) {
 			case ' ':
@@ -343,6 +566,7 @@ int main(int argc, char **argv)
 					0x0000ffff);
 				break;
 
+
 			case 'h':
 				hex_print(line + 1);
 				break;
@@ -351,6 +575,10 @@ int main(int argc, char **argv)
 				list_registers();
 				break;
 
+			case 'i':
+				find_irqs();
+				break;
+
 			case 'r':
 				read_address(line + 1);
 				break;
@@ -360,12 +588,21 @@ int main(int argc, char **argv)
 				break;
 
 			case 'd':
-				serial_puts("Kicking WDT...\n");
+				printf("Kicking WDT...\n");
 				wdt_kick();
 				break;
 
+			case 'b':
+				printf("Rebooting (using WDT)...\n");
+				wdt_reboot();
+				break;
+
+			case 'c':
+				asm volatile ("swi 3");
+				break;
+
 			default:
-				serial_puts("Unknown command\n");
+				printf("Unknown command\n");
 				break;
 		}
 	}

spin.sh

@@ -1,2 +1,2 @@
 #!/bin/sh
-sudo radare2 -c 's 0x3460; wf .//build/firmware.bin; !echo 0 > /sys/class/gpio/gpio17/value; !sleep 0.05; !echo 1 > /sys/class/gpio/gpio17/value' fv://
+make && sudo radare2 -c 's 0x3460; wf .//build/firmware.bin; !echo 0 > /sys/class/gpio/gpio17/value; !sleep 0.05; !echo 1 > /sys/class/gpio/gpio17/value' fv://

start.S

@@ -1,4 +1,6 @@
 .section vectors
+.global rv_start
+rv_start:
 	b	do_reset
 	b	do_undef
 	b	do_swi
@@ -9,21 +11,58 @@
 	b	do_fiq
 
 do_reset:
-	b	reset_handler
+	ldr	r0, =reset_handler
+	mov	pc, r0
+
 do_undef:
-	b	undef_handler
+	ldr	r0, =undef_handler
+	mov	pc, r0
+
 do_swi:
-	b	swi_handler
+	ldr	r0, =swi_handler
+	mov	pc, r0
+
 do_prefetch_abort:
-	b	prefetch_abort_handler
+	ldr	r0, =prefetch_abort_handler
+	mov	pc, r0
+
 do_data_abort:
-	b	data_abort_handler
+	ldr	r0, =data_abort_handler
+	mov	pc, r0
+
 do_reserved:
-	b	reserved_handler
+	ldr	r0, =reserved_handler
+	mov	pc, r0
+
 do_irq:
-	b	irq_handler
+	ldr	r0, =irq_handler
+	mov	pc, r0
+
 do_fiq:
-	b	fiq_handler
+	ldr	r0, =fiq_handler
+	mov	pc, r0
+
+.global rv_end
+rv_end:
 
 .text
 
+
+.global __udiv64
+
+__udiv64:
+	adds	r0,r0,r0
+	adc	r1,r1,r1
+
+	.rept 31
+		cmp	r1,r2
+		subcs	r1,r1,r2
+		adcs	r0,r0,r0
+		adc	r1,r1,r1
+	.endr
+
+	cmp	r1,r2
+	subcs	r1,r1,r2
+	adcs	r0,r0,r0
+
+	bx	lr

utils.c

@@ -61,3 +61,8 @@ int serial_print_hex(void *block, int count)
 	return serial_print_hex_offset(block, count, 0);
 }
 
+extern uint32_t __udiv64(uint32_t a, uint32_t b, uint32_t c);
+uint32_t _udiv64(uint64_t n, uint32_t d)
+{
+	return __udiv64(n >> 32, n, d);
+}

vectors.c

@@ -1,34 +1,46 @@
+#include "serial.h"
+#include "utils.h"
 
 void reset_handler(void) {
 	extern int main(int argc, char **argv);
+	printf("Reset exception\n");
 	main(1, 0);
 	return;
 }
 
 void undef_handler(void) {
+	printf("Undefined instruction exception\n");
 	return;
 }
 
 void swi_handler(void) {
+	printf("SWI exception\n");
 	return;
 }
 
 void prefetch_abort_handler(void) {
+	printf("Prefetch abort exception\n");
 	return;
 }
 
 void data_abort_handler(void) {
+	printf("Data abort exception\n");
 	return;
 }
 
 void reserved_handler(void) {
+	printf("Handled some IRQ that shouldn't exist\n");
 	return;
 }
 
 void irq_handler(void) {
+	while(1)
+		printf("Handled IRQ\n");
 	return;
 }
 
 void fiq_handler(void) {
+	while(1)
+		printf("Handled FIQ\n");
 	return;
 }

vsprintf.c

@@ -0,0 +1,801 @@
+/*
+ *  linux/lib/vsprintf.c
+ *
+ *  Copyright (C) 1991, 1992  Linus Torvalds
+ */
+
+/* vsprintf.c -- Lars Wirzenius & Linus Torvalds. */
+/*
+ * Wirzenius wrote this portably, Torvalds fucked it up :-)
+ *
+ * from hush: simple_itoa() was lifted from boa-0.93.15
+ */
+
+#include <stdarg.h>
+#include <linux/types.h>
+#include <linux/string.h>
+#include <errno.h>
+#include <stdint.h>
+#include "bionic.h"
+#include "utils.h"
+
+# define NUM_TYPE long long
+#define noinline __attribute__((noinline))
+
+/* some reluctance to put this into a new limits.h, so it is here */
+#define INT_MAX		((int)(~0U>>1))
+
+const char hex_asc[] = "0123456789abcdef";
+#define hex_asc_lo(x)   hex_asc[((x) & 0x0f)]
+#define hex_asc_hi(x)   hex_asc[((x) & 0xf0) >> 4]
+
+static inline char *pack_hex_byte(char *buf, uint8_t byte)
+{
+	*buf++ = hex_asc_hi(byte);
+	*buf++ = hex_asc_lo(byte);
+	return buf;
+}
+
+unsigned long simple_strtoul(const char *cp,char **endp,unsigned int base)
+{
+	unsigned long result = 0,value;
+
+	if (*cp == '0') {
+		cp++;
+		if ((*cp == 'x') && _isxdigit(cp[1])) {
+			base = 16;
+			cp++;
+		}
+		if (!base) {
+			base = 8;
+		}
+	}
+	if (!base) {
+		base = 10;
+	}
+	while (_isxdigit(*cp) && (value = _isdigit(*cp) ? *cp-'0' : (_islower(*cp)
+	    ? _toupper(*cp) : *cp)-'A'+10) < base) {
+		result = result*base + value;
+		cp++;
+	}
+	if (endp)
+		*endp = (char *)cp;
+	return result;
+}
+
+int strict_strtoul(const char *cp, unsigned int base, unsigned long *res)
+{
+	char *tail;
+	unsigned long val;
+	size_t len;
+
+	*res = 0;
+	len = _strlen(cp);
+	if (len == 0)
+		return -EINVAL;
+
+	val = simple_strtoul(cp, &tail, base);
+	if (tail == cp)
+		return -EINVAL;
+
+	if ((*tail == '\0') ||
+		((len == (size_t)(tail - cp) + 1) && (*tail == '\n'))) {
+		*res = val;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+long simple_strtol(const char *cp,char **endp,unsigned int base)
+{
+	if(*cp=='-')
+		return -simple_strtoul(cp+1,endp,base);
+	return simple_strtoul(cp,endp,base);
+}
+
+int ustrtoul(const char *cp, char **endp, unsigned int base)
+{
+	unsigned long result = simple_strtoul(cp, endp, base);
+	switch (**endp) {
+	case 'G' :
+		result *= 1024;
+		/* fall through */
+	case 'M':
+		result *= 1024;
+		/* fall through */
+	case 'K':
+	case 'k':
+		result *= 1024;
+		if ((*endp)[1] == 'i') {
+			if ((*endp)[2] == 'B')
+				(*endp) += 3;
+			else
+				(*endp) += 2;
+		}
+	}
+	return result;
+}
+
+unsigned long long simple_strtoull (const char *cp, char **endp, unsigned int base)
+{
+	unsigned long long result = 0, value;
+
+	if (*cp == '0') {
+		cp++;
+		if ((*cp == 'x') && _isxdigit (cp[1])) {
+			base = 16;
+			cp++;
+		}
+		if (!base) {
+			base = 8;
+		}
+	}
+	if (!base) {
+		base = 10;
+	}
+	while (_isxdigit (*cp) && (value = _isdigit (*cp)
+				? *cp - '0'
+				: (_islower (*cp) ? _toupper (*cp) : *cp) - 'A' + 10) < base) {
+		result = result * base + value;
+		cp++;
+	}
+	if (endp)
+		*endp = (char *) cp;
+	return result;
+}
+
+/* we use this so that we can do without the ctype library */
+#define is_digit(c)	((c) >= '0' && (c) <= '9')
+
+static int skip_atoi(const char **s)
+{
+	int i=0;
+
+	while (is_digit(**s))
+		i = i*10 + *((*s)++) - '0';
+	return i;
+}
+
+/* Decimal conversion is by far the most typical, and is used
+ * for /proc and /sys data. This directly impacts e.g. top performance
+ * with many processes running. We optimize it for speed
+ * using code from
+ * http://www.cs.uiowa.edu/~jones/bcd/decimal.html
+ * (with permission from the author, Douglas W. Jones). */
+
+/* Formats correctly any integer in [0,99999].
+ * Outputs from one to five digits depending on input.
+ * On i386 gcc 4.1.2 -O2: ~250 bytes of code. */
+static char* put_dec_trunc(char *buf, unsigned q)
+{
+	unsigned d3, d2, d1, d0;
+	d1 = (q>>4) & 0xf;
+	d2 = (q>>8) & 0xf;
+	d3 = (q>>12);
+
+	d0 = 6*(d3 + d2 + d1) + (q & 0xf);
+	q = (d0 * 0xcd) >> 11;
+	d0 = d0 - 10*q;
+	*buf++ = d0 + '0'; /* least significant digit */
+	d1 = q + 9*d3 + 5*d2 + d1;
+	if (d1 != 0) {
+		q = (d1 * 0xcd) >> 11;
+		d1 = d1 - 10*q;
+		*buf++ = d1 + '0'; /* next digit */
+
+		d2 = q + 2*d2;
+		if ((d2 != 0) || (d3 != 0)) {
+			q = (d2 * 0xd) >> 7;
+			d2 = d2 - 10*q;
+			*buf++ = d2 + '0'; /* next digit */
+
+			d3 = q + 4*d3;
+			if (d3 != 0) {
+				q = (d3 * 0xcd) >> 11;
+				d3 = d3 - 10*q;
+				*buf++ = d3 + '0';  /* next digit */
+				if (q != 0)
+					*buf++ = q + '0';  /* most sign. digit */
+			}
+		}
+	}
+	return buf;
+}
+/* Same with if's removed. Always emits five digits */
+static char* put_dec_full(char *buf, unsigned q)
+{
+	/* BTW, if q is in [0,9999], 8-bit ints will be enough, */
+	/* but anyway, gcc produces better code with full-sized ints */
+	unsigned d3, d2, d1, d0;
+	d1 = (q>>4) & 0xf;
+	d2 = (q>>8) & 0xf;
+	d3 = (q>>12);
+
+	/*
+	 * Possible ways to approx. divide by 10
+	 * gcc -O2 replaces multiply with shifts and adds
+	 * (x * 0xcd) >> 11: 11001101 - shorter code than * 0x67 (on i386)
+	 * (x * 0x67) >> 10:  1100111
+	 * (x * 0x34) >> 9:    110100 - same
+	 * (x * 0x1a) >> 8:     11010 - same
+	 * (x * 0x0d) >> 7:      1101 - same, shortest code (on i386)
+	 */
+
+	d0 = 6*(d3 + d2 + d1) + (q & 0xf);
+	q = (d0 * 0xcd) >> 11;
+	d0 = d0 - 10*q;
+	*buf++ = d0 + '0';
+	d1 = q + 9*d3 + 5*d2 + d1;
+		q = (d1 * 0xcd) >> 11;
+		d1 = d1 - 10*q;
+		*buf++ = d1 + '0';
+
+		d2 = q + 2*d2;
+			q = (d2 * 0xd) >> 7;
+			d2 = d2 - 10*q;
+			*buf++ = d2 + '0';
+
+			d3 = q + 4*d3;
+				q = (d3 * 0xcd) >> 11; /* - shorter code */
+				/* q = (d3 * 0x67) >> 10; - would also work */
+				d3 = d3 - 10*q;
+				*buf++ = d3 + '0';
+					*buf++ = q + '0';
+	return buf;
+}
+/* No inlining helps gcc to use registers better */
+static noinline char* put_dec(char *buf, unsigned NUM_TYPE num)
+{
+	while (1) {
+		unsigned rem;
+		if (num < 100000)
+			return put_dec_trunc(buf, num);
+		rem = _udiv64(num, 100000);
+		buf = put_dec_full(buf, rem);
+	}
+}
+
+#define ZEROPAD	1		/* pad with zero */
+#define SIGN	2		/* unsigned/signed long */
+#define PLUS	4		/* show plus */
+#define SPACE	8		/* space if plus */
+#define LEFT	16		/* left justified */
+#define SMALL	32		/* Must be 32 == 0x20 */
+#define SPECIAL	64		/* 0x */
+
+#ifdef CONFIG_SYS_VSNPRINTF
+/*
+ * Macro to add a new character to our output string, but only if it will
+ * fit. The macro moves to the next character position in the output string.
+ */
+#define ADDCH(str, ch) do { \
+	if ((str) < end) \
+		*(str) = (ch); \
+	++str; \
+	} while (0)
+#else
+#define ADDCH(str, ch)	(*(str)++ = (ch))
+#endif
+
+static char *number(char *buf, char *end, unsigned NUM_TYPE num,
+		int base, int size, int precision, int type)
+{
+	/* we are called with base 8, 10 or 16, only, thus don't need "G..."  */
+	static const char digits[16] = "0123456789ABCDEF"; /* "GHIJKLMNOPQRSTUVWXYZ"; */
+
+	char tmp[66];
+	char sign;
+	char locase;
+	int need_pfx = ((type & SPECIAL) && base != 10);
+	int i;
+
+	/* locase = 0 or 0x20. ORing digits or letters with 'locase'
+	 * produces same digits or (maybe lowercased) letters */
+	locase = (type & SMALL);
+	if (type & LEFT)
+		type &= ~ZEROPAD;
+	sign = 0;
+	if (type & SIGN) {
+		if ((signed NUM_TYPE) num < 0) {
+			sign = '-';
+			num = - (signed NUM_TYPE) num;
+			size--;
+		} else if (type & PLUS) {
+			sign = '+';
+			size--;
+		} else if (type & SPACE) {
+			sign = ' ';
+			size--;
+		}
+	}
+	if (need_pfx) {
+		size--;
+		if (base == 16)
+			size--;
+	}
+
+	/* generate full string in tmp[], in reverse order */
+	i = 0;
+	if (num == 0)
+		tmp[i++] = '0';
+	/* Generic code, for any base:
+	else do {
+		tmp[i++] = (digits[do_div(num,base)] | locase);
+	} while (num != 0);
+	*/
+	else if (base != 10) { /* 8 or 16 */
+		int mask = base - 1;
+		int shift = 3;
+		if (base == 16) shift = 4;
+		do {
+			tmp[i++] = (digits[((unsigned char)num) & mask] | locase);
+			num >>= shift;
+		} while (num);
+	} else { /* base 10 */
+		i = put_dec(tmp, num) - tmp;
+	}
+
+	/* printing 100 using %2d gives "100", not "00" */
+	if (i > precision)
+		precision = i;
+	/* leading space padding */
+	size -= precision;
+	if (!(type & (ZEROPAD + LEFT))) {
+		while (--size >= 0)
+			ADDCH(buf, ' ');
+	}
+	/* sign */
+	if (sign)
+		ADDCH(buf, sign);
+	/* "0x" / "0" prefix */
+	if (need_pfx) {
+		ADDCH(buf, '0');
+		if (base == 16)
+			ADDCH(buf, 'X' | locase);
+	}
+	/* zero or space padding */
+	if (!(type & LEFT)) {
+		char c = (type & ZEROPAD) ? '0' : ' ';
+
+		while (--size >= 0)
+			ADDCH(buf, c);
+	}
+	/* hmm even more zero padding? */
+	while (i <= --precision)
+		ADDCH(buf, '0');
+	/* actual digits of result */
+	while (--i >= 0)
+		ADDCH(buf, tmp[i]);
+	/* trailing space padding */
+	while (--size >= 0)
+		ADDCH(buf, ' ');
+	return buf;
+}
+
+static char *string(char *buf, char *end, char *s, int field_width,
+		int precision, int flags)
+{
+	int len, i;
+
+	if (s == 0)
+		s = "<NULL>";
+
+	len = _strnlen(s, precision);
+
+	if (!(flags & LEFT))
+		while (len < field_width--)
+			ADDCH(buf, ' ');
+	for (i = 0; i < len; ++i)
+		ADDCH(buf, *s++);
+	while (len < field_width--)
+		ADDCH(buf, ' ');
+	return buf;
+}
+
+#ifdef CONFIG_CMD_NET
+static char *mac_address_string(char *buf, char *end, uint8_t *addr, int field_width,
+				int precision, int flags)
+{
+	char mac_addr[6 * 3]; /* (6 * 2 hex digits), 5 colons and trailing zero */
+	char *p = mac_addr;
+	int i;
+
+	for (i = 0; i < 6; i++) {
+		p = pack_hex_byte(p, addr[i]);
+		if (!(flags & SPECIAL) && i != 5)
+			*p++ = ':';
+	}
+	*p = '\0';
+
+	return string(buf, end, mac_addr, field_width, precision,
+		      flags & ~SPECIAL);
+}
+
+static char *ip6_addr_string(char *buf, char *end, uint8_t *addr, int field_width,
+			 int precision, int flags)
+{
+	char ip6_addr[8 * 5]; /* (8 * 4 hex digits), 7 colons and trailing zero */
+	char *p = ip6_addr;
+	int i;
+
+	for (i = 0; i < 8; i++) {
+		p = pack_hex_byte(p, addr[2 * i]);
+		p = pack_hex_byte(p, addr[2 * i + 1]);
+		if (!(flags & SPECIAL) && i != 7)
+			*p++ = ':';
+	}
+	*p = '\0';
+
+	return string(buf, end, ip6_addr, field_width, precision,
+		      flags & ~SPECIAL);
+}
+
+static char *ip4_addr_string(char *buf, char *end, uint8_t *addr, int field_width,
+			 int precision, int flags)
+{
+	char ip4_addr[4 * 4]; /* (4 * 3 decimal digits), 3 dots and trailing zero */
+	char temp[3];	/* hold each IP quad in reverse order */
+	char *p = ip4_addr;
+	int i, digits;
+
+	for (i = 0; i < 4; i++) {
+		digits = put_dec_trunc(temp, addr[i]) - temp;
+		/* reverse the digits in the quad */
+		while (digits--)
+			*p++ = temp[digits];
+		if (i != 3)
+			*p++ = '.';
+	}
+	*p = '\0';
+
+	return string(buf, end, ip4_addr, field_width, precision,
+		      flags & ~SPECIAL);
+}
+#endif
+
+/*
+ * Show a '%p' thing.  A kernel extension is that the '%p' is followed
+ * by an extra set of alphanumeric characters that are extended format
+ * specifiers.
+ *
+ * Right now we handle:
+ *
+ * - 'M' For a 6-byte MAC address, it prints the address in the
+ *       usual colon-separated hex notation
+ * - 'I' [46] for IPv4/IPv6 addresses printed in the usual way (dot-separated
+ *       decimal for v4 and colon separated network-order 16 bit hex for v6)
+ * - 'i' [46] for 'raw' IPv4/IPv6 addresses, IPv6 omits the colons, IPv4 is
+ *       currently the same
+ *
+ * Note: The difference between 'S' and 'F' is that on ia64 and ppc64
+ * function pointers are really function descriptors, which contain a
+ * pointer to the real address.
+ */
+static char *pointer(const char *fmt, char *buf, char *end, void *ptr,
+		int field_width, int precision, int flags)
+{
+	if (!ptr)
+		return string(buf, end, "(null)", field_width, precision,
+			      flags);
+
+#ifdef CONFIG_CMD_NET
+	switch (*fmt) {
+	case 'm':
+		flags |= SPECIAL;
+		/* Fallthrough */
+	case 'M':
+		return mac_address_string(buf, end, ptr, field_width,
+					  precision, flags);
+	case 'i':
+		flags |= SPECIAL;
+		/* Fallthrough */
+	case 'I':
+		if (fmt[1] == '6')
+			return ip6_addr_string(buf, end, ptr, field_width,
+					       precision, flags);
+		if (fmt[1] == '4')
+			return ip4_addr_string(buf, end, ptr, field_width,
+					       precision, flags);
+		flags &= ~SPECIAL;
+		break;
+	}
+#endif
+	flags |= SMALL;
+	if (field_width == -1) {
+		field_width = 2*sizeof(void *);
+		flags |= ZEROPAD;
+	}
+	return number(buf, end, (unsigned long)ptr, 16, field_width,
+		      precision, flags);
+}
+
+static int vsnprintf_internal(char *buf, size_t size, const char *fmt,
+			      va_list args)
+{
+	unsigned NUM_TYPE num;
+	int base;
+	char *str;
+
+	int flags;		/* flags to number() */
+
+	int field_width;	/* width of output field */
+	int precision;		/* min. # of digits for integers; max
+				   number of chars for from string */
+	int qualifier;		/* 'h', 'l', or 'L' for integer fields */
+				/* 'z' support added 23/7/1999 S.H.    */
+				/* 'z' changed to 'Z' --davidm 1/25/99 */
+				/* 't' added for ptrdiff_t */
+	char *end = buf + size;
+
+#ifdef CONFIG_SYS_VSNPRINTF
+	/* Make sure end is always >= buf - do we want this in U-Boot? */
+	if (end < buf) {
+		end = ((void *)-1);
+		size = end - buf;
+	}
+#endif
+	str = buf;
+
+	for (; *fmt ; ++fmt) {
+		if (*fmt != '%') {
+			ADDCH(str, *fmt);
+			continue;
+		}
+
+		/* process flags */
+		flags = 0;
+		repeat:
+			++fmt;		/* this also skips first '%' */
+			switch (*fmt) {
+				case '-': flags |= LEFT; goto repeat;
+				case '+': flags |= PLUS; goto repeat;
+				case ' ': flags |= SPACE; goto repeat;
+				case '#': flags |= SPECIAL; goto repeat;
+				case '0': flags |= ZEROPAD; goto repeat;
+			}
+
+		/* get field width */
+		field_width = -1;
+		if (is_digit(*fmt))
+			field_width = skip_atoi(&fmt);
+		else if (*fmt == '*') {
+			++fmt;
+			/* it's the next argument */
+			field_width = va_arg(args, int);
+			if (field_width < 0) {
+				field_width = -field_width;
+				flags |= LEFT;
+			}
+		}
+
+		/* get the precision */
+		precision = -1;
+		if (*fmt == '.') {
+			++fmt;
+			if (is_digit(*fmt))
+				precision = skip_atoi(&fmt);
+			else if (*fmt == '*') {
+				++fmt;
+				/* it's the next argument */
+				precision = va_arg(args, int);
+			}
+			if (precision < 0)
+				precision = 0;
+		}
+
+		/* get the conversion qualifier */
+		qualifier = -1;
+		if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L' ||
+		    *fmt == 'Z' || *fmt == 'z' || *fmt == 't') {
+			qualifier = *fmt;
+			++fmt;
+			if (qualifier == 'l' && *fmt == 'l') {
+				qualifier = 'L';
+				++fmt;
+			}
+		}
+
+		/* default base */
+		base = 10;
+
+		switch (*fmt) {
+		case 'c':
+			if (!(flags & LEFT)) {
+				while (--field_width > 0)
+					ADDCH(str, ' ');
+			}
+			ADDCH(str, (unsigned char) va_arg(args, int));
+			while (--field_width > 0)
+				ADDCH(str, ' ');
+			continue;
+
+		case 's':
+			str = string(str, end, va_arg(args, char *),
+				     field_width, precision, flags);
+			continue;
+
+		case 'p':
+			str = pointer(fmt+1, str, end,
+					va_arg(args, void *),
+					field_width, precision, flags);
+			/* Skip all alphanumeric pointer suffixes */
+			while (_isalnum(fmt[1]))
+				fmt++;
+			continue;
+
+		case 'n':
+			if (qualifier == 'l') {
+				long * ip = va_arg(args, long *);
+				*ip = (str - buf);
+			} else {
+				int * ip = va_arg(args, int *);
+				*ip = (str - buf);
+			}
+			continue;
+
+		case '%':
+			ADDCH(str, '%');
+			continue;
+
+		/* integer number formats - set up the flags and "break" */
+		case 'o':
+			base = 8;
+			break;
+
+		case 'x':
+			flags |= SMALL;
+		case 'X':
+			base = 16;
+			break;
+
+		case 'd':
+		case 'i':
+			flags |= SIGN;
+		case 'u':
+			break;
+
+		default:
+			ADDCH(str, '%');
+			if (*fmt)
+				ADDCH(str, *fmt);
+			else
+				--fmt;
+			continue;
+		}
+		if (qualifier == 'L')  /* "quad" for 64 bit variables */
+			num = va_arg(args, unsigned long long);
+		else if (qualifier == 'l') {
+			num = va_arg(args, unsigned long);
+			if (flags & SIGN)
+				num = (signed long) num;
+		} else if (qualifier == 'Z' || qualifier == 'z') {
+			num = va_arg(args, size_t);
+		} else if (qualifier == 't') {
+			num = va_arg(args, ptrdiff_t);
+		} else if (qualifier == 'h') {
+			num = (unsigned short) va_arg(args, int);
+			if (flags & SIGN)
+				num = (signed short) num;
+		} else {
+			num = va_arg(args, unsigned int);
+			if (flags & SIGN)
+				num = (signed int) num;
+		}
+		str = number(str, end, num, base, field_width, precision,
+			     flags);
+	}
+
+#ifdef CONFIG_SYS_VSNPRINTF
+	if (size > 0) {
+		ADDCH(str, '\0');
+		if (str > end)
+			end[-1] = '\0';
+	}
+#else
+	*str = '\0';
+#endif
+	/* the trailing null byte doesn't count towards the total */
+	return str-buf;
+}
+
+#ifdef CONFIG_SYS_VSNPRINTF
+int vsnprintf(char *buf, size_t size, const char *fmt,
+			      va_list args)
+{
+	return vsnprintf_internal(buf, size, fmt, args);
+}
+
+int vscnprintf(char *buf, size_t size, const char *fmt, va_list args)
+{
+	int i;
+
+	i = vsnprintf(buf, size, fmt, args);
+
+	if (likely(i < size))
+		return i;
+	if (size != 0)
+		return size - 1;
+	return 0;
+}
+
+int snprintf(char *buf, size_t size, const char *fmt, ...)
+{
+	va_list args;
+	int i;
+
+	va_start(args, fmt);
+	i = vsnprintf(buf, size, fmt, args);
+	va_end(args);
+
+	return i;
+}
+
+int scnprintf(char *buf, size_t size, const char *fmt, ...)
+{
+	va_list args;
+	int i;
+
+	va_start(args, fmt);
+	i = vscnprintf(buf, size, fmt, args);
+	va_end(args);
+
+	return i;
+}
+#endif /* CONFIG_SYS_VSNPRINT */
+
+/**
+ * Format a string and place it in a buffer (va_list version)
+ *
+ * @param buf	The buffer to place the result into
+ * @param fmt	The format string to use
+ * @param args	Arguments for the format string
+ *
+ * The function returns the number of characters written
+ * into @buf. Use vsnprintf() or vscnprintf() in order to avoid
+ * buffer overflows.
+ *
+ * If you're not already dealing with a va_list consider using sprintf().
+ */
+int vsprintf(char *buf, const char *fmt, va_list args)
+{
+	return vsnprintf_internal(buf, INT_MAX, fmt, args);
+}
+
+int sprintf(char * buf, const char *fmt, ...)
+{
+	va_list args;
+	int i;
+
+	va_start(args, fmt);
+	i=vsprintf(buf,fmt,args);
+	va_end(args);
+	return i;
+}
+
+int printf(const char *fmt, ...)
+{
+	va_list args;
+	int i;
+	char printbuffer[256];
+
+	va_start(args, fmt);
+	i = vsnprintf_internal(printbuffer, sizeof(printbuffer), fmt, args);
+	va_end(args);
+	serial_puts(printbuffer);
+	return i;
+}
+
+char *simple_itoa(uint32_t i)
+{
+	/* 21 digits plus null terminator, good for 64-bit or smaller ints */
+	static char local[22];
+	char *p = &local[21];
+
+	*p-- = '\0';
+	do {
+		*p-- = '0' + i % 10;
+		i /= 10;
+	} while (i > 0);
+	return p + 1;
+}