--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/leo-obj/tool/thumbdis.c	Sun Apr 06 22:14:39 2014 +0000
@@ -0,0 +1,436 @@
+/*
+ * Thumb state disassembly
+ */
+
+#include <sys/types.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <strings.h>
+#include "intstruct.h"
+#include "globals.h"
+
+extern unsigned get_u16(), get_u32();
+extern struct internal_reloc *find_word32_reloc();
+
+extern char *regnames[16], *condition_decode[16], *shift_types[4];
+
+static void
+format_1_2(word)
+	unsigned word;
+{
+	unsigned op, imm;
+
+	op = (word >> 11) & 3;
+	if (op != 3) {
+		/* format 1 */
+		imm = (word >> 6) & 0x1F;
+		if (op != 0 && imm == 0)
+			imm = 32;
+		printf("%s\t%s, %s, #%u\n", shift_types[op], regnames[word&7],
+			regnames[(word>>3)&7], imm);
+		return;
+	}
+	/* format 2 */
+	printf("%s\t%s, %s, ", word&0x200 ? "sub" : "add", regnames[word&7],
+		regnames[(word>>3)&7]);
+	if (word & 0x400)
+		printf("#%u\n", (word >> 6) & 7);
+	else
+		printf("%s\n", regnames[(word >> 6) & 7]);
+}
+
+static void
+format_3(word)
+	unsigned word;
+{
+	static char *opctab[4] = {"mov", "cmp", "add", "sub"};
+	unsigned imm;
+
+	imm = word & 0xFF;
+	printf("%s\t%s, #%u", opctab[(word>>11)&3], regnames[(word>>8)&7], imm);
+	if (imm > 9)
+		printf("\t; 0x%x", imm);
+	putchar('\n');
+}
+
+static void
+format_4(word)
+	unsigned word;
+{
+	static char *opc[16] = {"and", "eor", "lsl", "lsr",
+				"asr", "adc", "sbc", "ror",
+				"tst", "neg", "cmp", "cmn",
+				"orr", "mul", "bic", "mvn"};
+
+	printf("%s\t%s, %s\n", opc[(word>>6)&0xF], regnames[word&7],
+		regnames[(word>>3)&7]);
+}
+
+static void
+format_5_bx(word)
+	unsigned word;
+{
+	if (word & 0x80)
+		printf("<invalid: blx instead of bx>\n");
+	else
+		printf("bx\t%s\n", regnames[(word>>3)&0xF]);
+}
+
+static void
+format_5_hiops(word)
+	unsigned word;
+{
+	static char *opctab[3] = {"add", "cmp", "mov"};
+	int reg1, reg2, op;
+
+	if (word & 0xC0) {
+		reg1 = word & 7;
+		if (word & 0x80)
+			reg1 += 8;
+		reg2 = (word >> 3) & 0xF;
+		op = (word >> 8) & 3;
+		if (op == 2 && reg1 == reg2 && reg1 != 15)
+			printf("nop\t\t\t(mov %s, %s)\n",
+				regnames[reg1], regnames[reg2]);
+		else
+			printf("%s\t%s, %s\n", opctab[op],
+				regnames[reg1], regnames[reg2]);
+	} else
+		printf("<invalid: hi-reg format with both low regs>\n");
+}
+
+static void
+format_5(word)
+	unsigned word;
+{
+	if ((word & 0x300) == 0x300)
+		format_5_bx(word);
+	else
+		format_5_hiops(word);
+}
+
+static void
+format_6(sec, off, word)
+	struct internal_scnhdr *sec;
+	unsigned off, word;
+{
+	unsigned loff, litoff, datum;
+	struct internal_reloc *rel;
+
+	loff = (word & 0xFF) << 2;
+	off &= ~3;
+	off += 4;
+	litoff = off + loff;
+	if (litoff+4 <= sec->size) {
+		rel = find_word32_reloc(sec, litoff);
+		datum = get_u32(filemap + sec->data_offset + litoff);
+		printf("ldr\t%s, =", regnames[(word>>8)&7]);
+		if (rel)
+			disasm_reloc_target(sec, rel, datum);
+		else
+			printf("0x%x", datum);
+		printf("\t; via 0x%x\n", litoff);
+	} else
+		printf("ldr\t%s, [pc, #%u]\t(0x%x)\n", regnames[(word>>8)&7],
+			loff, litoff);
+}
+
+static void
+format_7(word)
+	unsigned word;
+{
+	printf("%s%s\t%s, [%s, %s]\n", word&0x800 ? "ldr" : "str",
+		word&0x400 ? "b" : "", regnames[word&7],
+		regnames[(word>>3)&7], regnames[(word>>6)&7]);
+}
+
+static void
+format_8(word)
+	unsigned word;
+{
+	static char *opc[4] = {"strh", "ldrsb", "ldrh", "ldrsh"};
+
+	printf("%s\t%s, [%s, %s]\n", opc[(word>>10)&3], regnames[word&7],
+		regnames[(word>>3)&7], regnames[(word>>6)&7]);
+}
+
+static void
+format_9(word)
+	unsigned word;
+{
+	unsigned loff;
+
+	loff = (word >> 6) & 0x1F;
+	if (!(word & 0x1000))
+		loff <<= 2;
+	printf("%s%s\t%s, [%s, #%u]", word&0x800 ? "ldr" : "str",
+		word&0x1000 ? "b" : "", regnames[word&7],
+		regnames[(word>>3)&7], loff);
+	if (loff >= 10)
+		printf("\t; 0x%x", loff);
+	putchar('\n');
+}
+
+static void
+format_10(word)
+	unsigned word;
+{
+	unsigned loff;
+
+	loff = (word >> 6) & 0x1F;
+		loff <<= 1;
+	printf("%sh\t%s, [%s, #%u]", word&0x800 ? "ldr" : "str",
+		regnames[word&7], regnames[(word>>3)&7], loff);
+	if (loff >= 10)
+		printf("\t; 0x%x", loff);
+	putchar('\n');
+}
+
+static void
+format_11(word)
+	unsigned word;
+{
+	unsigned loff;
+
+	loff = (word & 0xFF) << 2;
+	printf("%s\t%s, [sp, #%u]", word&0x800 ? "ldr" : "str",
+		regnames[(word>>8)&7], loff);
+	if (loff >= 10)
+		printf("\t; 0x%x", loff);
+	putchar('\n');
+}
+
+static void
+format_12(off, word)
+	unsigned off, word;
+{
+	unsigned loff;
+
+	loff = (word & 0xFF) << 2;
+	printf("add\t%s, ", regnames[(word>>8)&7]);
+	if (word & 0x800) {
+		printf("sp, #%u", loff);
+		if (loff >= 10)
+			printf("\t; 0x%x", loff);
+		putchar('\n');
+	} else {
+		off &= ~3;
+		printf("pc, #%u\t; 0x%x\n", loff, off + 4 + loff);
+	}
+}
+
+static void
+format_13(word)
+	unsigned word;
+{
+	unsigned loff;
+
+	if ((word & 0xFF00) != 0xB000) {
+		printf("<invalid format 13>\n");
+		return;
+	}
+	loff = (word & 0x7F) << 2;
+	printf("%s\tsp, #%u", word&0x80 ? "sub" : "add", loff);
+	if (loff >= 10)
+		printf("\t; 0x%x", loff);
+	putchar('\n');
+}
+
+static void
+format_14(word)
+	unsigned word;
+{
+	int r, flag;
+
+	if ((word & 0xF600) != 0xB400) {
+		printf("<invalid format 14>\n");
+		return;
+	}
+	printf("%s\t{", word&0x800 ? "pop" : "push");
+	flag = 0;
+	for (r = 0; r < 9; r++)
+		if (word & (1 << r)) {
+			if (flag)
+				fputs(", ", stdout);
+			if (r == 8)
+				fputs(word&0x800 ? "pc" : "lr", stdout);
+			else
+				fputs(regnames[r], stdout);
+			flag = 1;
+		}
+	putchar('}');
+	putchar('\n');
+}
+
+static void
+format_15(word)
+	unsigned word;
+{
+	int r, flag;
+
+	printf("%sia\t%s!, {", word&0x800 ? "ldm" : "stm",
+		regnames[(word>>8)&7]);
+	flag = 0;
+	for (r = 0; r < 8; r++)
+		if (word & (1 << r)) {
+			if (flag)
+				fputs(", ", stdout);
+			fputs(regnames[r], stdout);
+			flag = 1;
+		}
+	putchar('}');
+	putchar('\n');
+}
+
+static void
+format_16_17(off, word)
+	unsigned off, word;
+{
+	unsigned cond;
+	unsigned dest;
+
+	cond = (word >> 8) & 0xF;
+	switch (cond) {
+	case 0xE:
+		printf("<invalid: bal>\n");
+		return;
+	case 0xF:
+		printf("swi\t0x%x\n", word & 0xFF);
+		return;
+	}
+	dest = (word & 0xFF) << 1;
+	if (dest & 0x00000100)
+		dest |= 0xFFFFFE00;
+	dest += off + 4;
+	printf("b%s\t0x%x\n", condition_decode[cond], dest);
+}
+
+static void
+format_18(off, word)
+	unsigned off, word;
+{
+	unsigned dest;
+
+	if (word & 0x800) {
+		printf("<invalid format 18>\n");
+		return;
+	}
+	dest = (word & 0x7FF) << 1;
+	if (dest & 0x00000800)
+		dest |= 0xFFFFF000;
+	dest += off + 4;
+	printf("b\t0x%x\n", dest);
+}
+
+void
+thumb_disasm_line(sec, off)
+	struct internal_scnhdr *sec;
+	unsigned off;
+{
+	unsigned word;
+
+	word = get_u16(filemap + sec->data_offset + off);
+	printf("%04x\t\t", word);
+	switch (word >> 12) {
+	case 0:
+	case 1:
+		format_1_2(word);
+		return;
+	case 2:
+	case 3:
+		format_3(word);
+		return;
+	case 4:
+		if (word & 0x800)
+			format_6(sec, off, word);
+		else if (word & 0x400)
+			format_5(word);
+		else
+			format_4(word);
+		return;
+	case 5:
+		if (word & 0x200)
+			format_8(word);
+		else
+			format_7(word);
+		return;
+	case 6:
+	case 7:
+		format_9(word);
+		return;
+	case 8:
+		format_10(word);
+		return;
+	case 9:
+		format_11(word);
+		return;
+	case 0xA:
+		format_12(off, word);
+		return;
+	case 0xB:
+		if (word & 0x400)
+			format_14(word);
+		else
+			format_13(word);
+		return;
+	case 0xC:
+		format_15(word);
+		return;
+	case 0xD:
+		format_16_17(off, word);
+		return;
+	case 0xE:
+		format_18(off, word);
+		return;
+	case 0xF:
+		printf("<half-bl>\n");
+		return;
+	}
+}
+
+thumb_check_bl(sec, off)
+	struct internal_scnhdr *sec;
+	unsigned off;
+{
+	unsigned ins1, ins2;
+	unsigned dest;
+
+	ins1 = get_u16(filemap + sec->data_offset + off);
+	if ((ins1 & 0xF800) != 0xF000)
+		return(0);
+	ins2 = get_u16(filemap + sec->data_offset + off + 2);
+	if ((ins2 & 0xF800) != 0xF800)
+		return(0);
+	/* match */
+	dest = ((ins1 & 0x7FF) << 12) | ((ins2 & 0x7FF) << 1);
+	if (dest & 0x00400000)
+		dest |= 0xFF800000;
+	dest += off + 4;
+	printf("%04x %04x\tbl\t0x%x\n", ins1, ins2, dest);
+	return(1);
+}
+
+void
+thumb_bl_reloc(sec, rel)
+	struct internal_scnhdr *sec;
+	struct internal_reloc *rel;
+{
+	unsigned ins1, ins2;
+	unsigned dest;
+
+	ins1 = get_u16(filemap + sec->data_offset + rel->location);
+	ins2 = get_u16(filemap + sec->data_offset + rel->location + 2);
+	printf("%04x %04x R\t", ins1, ins2);
+	if ((ins1 & 0xF800) != 0xF000 || (ins2 & 0xF800) != 0xF800) {
+		printf("<invalid Thumb_BL reloc: opcode not BL>\n");
+		return;
+	}
+	dest = ((ins1 & 0x7FF) << 12) | ((ins2 & 0x7FF) << 1);
+	if (dest & 0x00400000)
+		dest |= 0xFF800000;
+	dest += rel->location + 4;
+	fputs("bl\t", stdout);
+	disasm_reloc_target(sec, rel, dest);
+	putchar('\n');
+}