--- trunk/src/cpus/cpu_arm.c	2007/10/08 16:18:51	14
+++ trunk/src/cpus/cpu_arm.c	2007/10/08 16:20:10	26
@@ -1,5 +1,5 @@
 /*
- *  Copyright (C) 2005  Anders Gavare.  All rights reserved.
+ *  Copyright (C) 2005-2006  Anders Gavare.  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
  *  modification, are permitted provided that the following conditions are met:
@@ -25,16 +25,14 @@
  *  SUCH DAMAGE.
  *
  *
- *  $Id: cpu_arm.c,v 1.28 2005/10/07 10:26:03 debug Exp $
+ *  $Id: cpu_arm.c,v 1.60 2006/06/24 21:47:23 debug Exp $
  *
  *  ARM CPU emulation.
  *
+ *
  *  A good source of quick info on ARM instruction encoding:
  *
  *	http://www.pinknoise.demon.co.uk/ARMinstrs/ARMinstrs.html
- *
- *  (Most "xxxx0101..." and similar strings in this file are from that URL,
- *  or from the ARM manual.)
  */
 
 #include <stdio.h>
@@ -47,6 +45,7 @@
 #include "machine.h"
 #include "memory.h"
 #include "misc.h"
+#include "of.h"
 #include "symbol.h"
 
 #define DYNTRANS_32
@@ -62,8 +61,11 @@
 static int arm_dpi_uses_d[16] = { 1,1,1,1,1,1,1,1,0,0,0,0,1,1,1,1 };
 static int arm_dpi_uses_n[16] = { 1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0 };
 
-/*  Forward reference:  */
+static int arm_exception_to_mode[N_ARM_EXCEPTIONS] = ARM_EXCEPTION_TO_MODE;
+
+/*  For quick_pc_to_pointers():  */
 void arm_pc_to_pointers(struct cpu *cpu);
+#include "quick_pc_to_pointers.h"
 
 
 /*
@@ -92,25 +94,26 @@
 
 	cpu->memory_rw = arm_memory_rw;
 	cpu->update_translation_table = arm_update_translation_table;
-	cpu->invalidate_translation_caches_paddr =
-	    arm_invalidate_translation_caches_paddr;
+	cpu->invalidate_translation_caches =
+	    arm_invalidate_translation_caches;
 	cpu->invalidate_code_translation = arm_invalidate_code_translation;
-	cpu->translate_address = arm_translate_address;
+	cpu->translate_v2p = arm_translate_v2p;
 
-	cpu->cd.arm.cpu_type    = cpu_type_defs[found];
-	cpu->name               = cpu->cd.arm.cpu_type.name;
-	cpu->is_32bit           = 1;
+	cpu->cd.arm.cpu_type = cpu_type_defs[found];
+	cpu->name            = cpu->cd.arm.cpu_type.name;
+	cpu->is_32bit        = 1;
 
 	cpu->cd.arm.cpsr = ARM_FLAG_I | ARM_FLAG_F;
 	cpu->cd.arm.control = ARM_CONTROL_PROG32 | ARM_CONTROL_DATA32
 	    | ARM_CONTROL_CACHE | ARM_CONTROL_ICACHE | ARM_CONTROL_ALIGN;
+	/*  TODO: default auxctrl contents  */
 
 	if (cpu->machine->prom_emulation) {
 		cpu->cd.arm.cpsr |= ARM_MODE_SVC32;
 		cpu->cd.arm.control |= ARM_CONTROL_S;
 	} else {
-		cpu->cd.arm.cpsr |= ARM_MODE_USR32;
-		cpu->cd.arm.control |= ARM_CONTROL_S | ARM_CONTROL_R;
+		cpu->cd.arm.cpsr |= ARM_MODE_SVC32;
+		cpu->cd.arm.control |= ARM_CONTROL_R;
 	}
 
 	/*  Only show name and caches etc for CPU nr 0:  */
@@ -128,9 +131,26 @@
 		}
 	}
 
+	/*  TODO: Some of these values (iway and dway) aren't used yet:  */
+	cpu->cd.arm.cachetype =
+	      (5 << ARM_CACHETYPE_CLASS_SHIFT)
+	    | (1 << ARM_CACHETYPE_HARVARD_SHIFT)
+	    | ((cpu->cd.arm.cpu_type.dcache_shift - 9) <<
+		ARM_CACHETYPE_DSIZE_SHIFT)
+	    | (5 << ARM_CACHETYPE_DASSOC_SHIFT)		/*  32-way  */
+	    | (2 << ARM_CACHETYPE_DLINE_SHIFT)		/*  8 words/line  */
+	    | ((cpu->cd.arm.cpu_type.icache_shift - 9) <<
+		ARM_CACHETYPE_ISIZE_SHIFT)
+	    | (5 << ARM_CACHETYPE_IASSOC_SHIFT)		/*  32-way  */
+	    | (2 << ARM_CACHETYPE_ILINE_SHIFT);		/*  8 words/line  */
+
 	/*  Coprocessor 15 = the system control coprocessor.  */
 	cpu->cd.arm.coproc[15] = arm_coproc_15;
 
+	/*  Coprocessor 14 for XScale:  */
+	if (cpu->cd.arm.cpu_type.flags & ARM_XSCALE)
+		cpu->cd.arm.coproc[14] = arm_coproc_xscale_14;
+
 	/*
 	 *  NOTE/TODO: Ugly hack for OpenFirmware emulation:
 	 */
@@ -140,6 +160,8 @@
 		store_32bit_word(cpu, cpu->cd.arm.of_emul_addr, 0xef8c64be);
 	}
 
+	cpu->cd.arm.flags = cpu->cd.arm.cpsr >> 28;
+
 	return 1;
 }
 
@@ -163,6 +185,7 @@
 	}
 
 	cpu->cd.arm.control |= ARM_CONTROL_MMU;
+	cpu->translate_v2p = arm_translate_v2p_mmu;
 	cpu->cd.arm.dacr |= 0x00000003;
 	cpu->cd.arm.ttb = ttb_addr;
 
@@ -175,9 +198,63 @@
 			uint32_t addr = cpu->cd.arm.ttb +
 			    (((j << 28) + (i << 20)) >> 18);
 			uint32_t d = (1048576*i) | 0xc02;
+
+			if (cpu->byte_order == EMUL_LITTLE_ENDIAN) {
+				descr[0] = d;       descr[1] = d >> 8;
+				descr[2] = d >> 16; descr[3] = d >> 24;
+			} else {
+				descr[3] = d;       descr[2] = d >> 8;
+				descr[1] = d >> 16; descr[0] = d >> 24;
+			}
+			cpu->memory_rw(cpu, cpu->mem, addr, &descr[0],
+			    sizeof(descr), MEM_WRITE, PHYSICAL | NO_EXCEPTIONS);
+		}
+}
+
+
 /*
-d = (1048576 * (i + (j==12? 10 : j)*256)) | 2;
-*/
+ *  arm_translation_table_set_l1():
+ */
+void arm_translation_table_set_l1(struct cpu *cpu, uint32_t vaddr,
+	uint32_t paddr)
+{
+	unsigned int i, j, vhigh = vaddr >> 28, phigh = paddr >> 28;
+
+	for (i=0; i<256; i++)
+		for (j=vhigh; j<=vhigh; j++) {
+			unsigned char descr[4];
+			uint32_t addr = cpu->cd.arm.ttb +
+			    (((j << 28) + (i << 20)) >> 18);
+			uint32_t d = ((phigh << 28) + 1048576*i) | 0xc02;
+
+			if (cpu->byte_order == EMUL_LITTLE_ENDIAN) {
+				descr[0] = d;       descr[1] = d >> 8;
+				descr[2] = d >> 16; descr[3] = d >> 24;
+			} else {
+				descr[3] = d;       descr[2] = d >> 8;
+				descr[1] = d >> 16; descr[0] = d >> 24;
+			}
+			cpu->memory_rw(cpu, cpu->mem, addr, &descr[0],
+			    sizeof(descr), MEM_WRITE, PHYSICAL | NO_EXCEPTIONS);
+		}
+}
+
+
+/*
+ *  arm_translation_table_set_l1_b():
+ */
+void arm_translation_table_set_l1_b(struct cpu *cpu, uint32_t vaddr,
+	uint32_t paddr)
+{
+	unsigned int i, j, vhigh = vaddr >> 24, phigh = paddr >> 24;
+
+	for (i=0; i<16; i++)
+		for (j=vhigh; j<=vhigh; j++) {
+			unsigned char descr[4];
+			uint32_t addr = cpu->cd.arm.ttb +
+			    (((j << 24) + (i << 20)) >> 18);
+			uint32_t d = ((phigh << 24) + 1048576*i) | 0xc02;
+
 			if (cpu->byte_order == EMUL_LITTLE_ENDIAN) {
 				descr[0] = d;       descr[1] = d >> 8;
 				descr[2] = d >> 16; descr[3] = d >> 24;
@@ -244,8 +321,11 @@
 				m->cpus[cpunr]->cd.arm.r[i] = *valuep;
 				if (i == ARM_PC)
 					m->cpus[cpunr]->pc = *valuep;
-			} else
+			} else {
 				*valuep = m->cpus[cpunr]->cd.arm.r[i];
+				if (i == ARM_PC)
+					*valuep = m->cpus[cpunr]->pc;
+			}
 			*match_register = 1;
 		}
 	}
@@ -267,9 +347,12 @@
 	int mode = cpu->cd.arm.cpsr & ARM_FLAG_MODE;
 	int i, x = cpu->cpu_id;
 
+	cpu->cd.arm.cpsr &= 0x0fffffff;
+	cpu->cd.arm.cpsr |= (cpu->cd.arm.flags << 28);
+
 	if (gprs) {
 		symbol = get_symbol_name(&cpu->machine->symbol_context,
-		    cpu->cd.arm.r[ARM_PC], &offset);
+		    cpu->pc, &offset);
 		debug("cpu%i:  cpsr = ", x);
 		debug("%s%s%s%s%s%s",
 		    (cpu->cd.arm.cpsr & ARM_FLAG_N)? "N" : "n",
@@ -279,10 +362,9 @@
 		    (cpu->cd.arm.cpsr & ARM_FLAG_I)? "I" : "i",
 		    (cpu->cd.arm.cpsr & ARM_FLAG_F)? "F" : "f");
 		if (mode < ARM_MODE_USR32)
-			debug("   pc =  0x%07x",
-			    (int)(cpu->cd.arm.r[ARM_PC] & 0x03ffffff));
+			debug("   pc =  0x%07x", (int)(cpu->pc & 0x03ffffff));
 		else
-			debug("   pc = 0x%08x", (int)cpu->cd.arm.r[ARM_PC]);
+			debug("   pc = 0x%08x", (int)cpu->pc);
 
 		debug("  <%s>\n", symbol != NULL? symbol : " no symbol ");
 
@@ -319,42 +401,42 @@
 		}
 
 		if (m != ARM_MODE_USR32 && m != ARM_MODE_SYS32) {
-			debug("cpu%i:  usr r8..r14 =", x);
+			debug("cpu%i:  usr r8-14:", x);
 			for (i=0; i<7; i++)
 				debug(" %08x", cpu->cd.arm.default_r8_r14[i]);
 			debug("\n");
 		}
 
 		if (m != ARM_MODE_FIQ32) {
-			debug("cpu%i:  fiq r8..r14 =", x);
+			debug("cpu%i:  fiq r8-14:", x);
 			for (i=0; i<7; i++)
 				debug(" %08x", cpu->cd.arm.fiq_r8_r14[i]);
 			debug("\n");
 		}
 
 		if (m != ARM_MODE_IRQ32) {
-			debug("cpu%i:  irq r13..r14 =", x);
+			debug("cpu%i:  irq r13-14:", x);
 			for (i=0; i<2; i++)
 				debug(" %08x", cpu->cd.arm.irq_r13_r14[i]);
 			debug("\n");
 		}
 
 		if (m != ARM_MODE_SVC32) {
-			debug("cpu%i:  svc r13..r14 =", x);
+			debug("cpu%i:  svc r13-14:", x);
 			for (i=0; i<2; i++)
 				debug(" %08x", cpu->cd.arm.svc_r13_r14[i]);
 			debug("\n");
 		}
 
 		if (m != ARM_MODE_ABT32) {
-			debug("cpu%i:  abt r13..r14 =", x);
+			debug("cpu%i:  abt r13-14:", x);
 			for (i=0; i<2; i++)
 				debug(" %08x", cpu->cd.arm.abt_r13_r14[i]);
 			debug("\n");
 		}
 
 		if (m != ARM_MODE_UND32) {
-			debug("cpu%i:  und r13..r14 =", x);
+			debug("cpu%i:  und r13-14:", x);
 			for (i=0; i<2; i++)
 				debug(" %08x", cpu->cd.arm.und_r13_r14[i]);
 			debug("\n");
@@ -391,6 +473,20 @@
 		    cpu->cd.arm.control &
 		    ARM_CONTROL_V? "yes (0xffff0000)" : "no");
 
+		/*  TODO: auxctrl on which CPU types?  */
+		if (cpu->cd.arm.cpu_type.flags & ARM_XSCALE) {
+			debug("cpu%i:  auxctrl = 0x%08x\n", x,
+			    cpu->cd.arm.auxctrl);
+			debug("cpu%i:      minidata cache attr = 0x%x\n", x,
+			    (cpu->cd.arm.auxctrl & ARM_AUXCTRL_MD)
+			    >> ARM_AUXCTRL_MD_SHIFT);
+			debug("cpu%i:      page table memory attr: %i\n", x,
+			    (cpu->cd.arm.auxctrl & ARM_AUXCTRL_P)? 1 : 0);
+			debug("cpu%i:      write buffer coalescing: %s\n", x,
+			    (cpu->cd.arm.auxctrl & ARM_AUXCTRL_K)?
+			    "disabled" : "enabled");
+		}
+
 		debug("cpu%i:  ttb = 0x%08x  dacr = 0x%08x\n", x,
 		    cpu->cd.arm.ttb, cpu->cd.arm.dacr);
 		debug("cpu%i:  fsr = 0x%08x  far = 0x%08x\n", x,
@@ -400,32 +496,6 @@
 
 
 /*
- *  arm_cpu_show_full_statistics():
- *
- *  Show detailed statistics on opcode usage on each cpu.
- */
-void arm_cpu_show_full_statistics(struct machine *m)
-{
-	fatal("arm_cpu_show_full_statistics(): TODO\n");
-}
-
-
-/*
- *  arm_cpu_tlbdump():
- *
- *  Called from the debugger to dump the TLB in a readable format.
- *  x is the cpu number to dump, or -1 to dump all CPUs.
- *
- *  If rawflag is nonzero, then the TLB contents isn't formated nicely,
- *  just dumped.
- */
-void arm_cpu_tlbdump(struct machine *m, int x, int rawflag)
-{
-	fatal("arm_cpu_tlbdump(): TODO\n");
-}
-
-
-/*
  *  arm_save_register_bank():
  */
 void arm_save_register_bank(struct cpu *cpu)
@@ -442,22 +512,26 @@
 		    &cpu->cd.arm.r[8], sizeof(uint32_t) * 7);
 		break;
 	case ARM_MODE_IRQ32:
+		memcpy(cpu->cd.arm.default_r8_r14,
+		    &cpu->cd.arm.r[8], sizeof(uint32_t) * 5);
 		cpu->cd.arm.irq_r13_r14[0] = cpu->cd.arm.r[13];
 		cpu->cd.arm.irq_r13_r14[1] = cpu->cd.arm.r[14];
 		break;
 	case ARM_MODE_SVC32:
-if ((cpu->cd.arm.r[13] & 0xffff0000) == 0xffff0000) {
-	fatal("NEJ! pc=0x%08x\n", (int)cpu->pc);
-	exit(1);
-}
+		memcpy(cpu->cd.arm.default_r8_r14,
+		    &cpu->cd.arm.r[8], sizeof(uint32_t) * 5);
 		cpu->cd.arm.svc_r13_r14[0] = cpu->cd.arm.r[13];
 		cpu->cd.arm.svc_r13_r14[1] = cpu->cd.arm.r[14];
 		break;
 	case ARM_MODE_ABT32:
+		memcpy(cpu->cd.arm.default_r8_r14,
+		    &cpu->cd.arm.r[8], sizeof(uint32_t) * 5);
 		cpu->cd.arm.abt_r13_r14[0] = cpu->cd.arm.r[13];
 		cpu->cd.arm.abt_r13_r14[1] = cpu->cd.arm.r[14];
 		break;
 	case ARM_MODE_UND32:
+		memcpy(cpu->cd.arm.default_r8_r14,
+		    &cpu->cd.arm.r[8], sizeof(uint32_t) * 5);
 		cpu->cd.arm.und_r13_r14[0] = cpu->cd.arm.r[13];
 		cpu->cd.arm.und_r13_r14[1] = cpu->cd.arm.r[14];
 		break;
@@ -485,18 +559,26 @@
 		    sizeof(uint32_t) * 7);
 		break;
 	case ARM_MODE_IRQ32:
+		memcpy(&cpu->cd.arm.r[8],
+		    cpu->cd.arm.default_r8_r14, sizeof(uint32_t) * 5);
 		cpu->cd.arm.r[13] = cpu->cd.arm.irq_r13_r14[0];
 		cpu->cd.arm.r[14] = cpu->cd.arm.irq_r13_r14[1];
 		break;
 	case ARM_MODE_SVC32:
+		memcpy(&cpu->cd.arm.r[8],
+		    cpu->cd.arm.default_r8_r14, sizeof(uint32_t) * 5);
 		cpu->cd.arm.r[13] = cpu->cd.arm.svc_r13_r14[0];
 		cpu->cd.arm.r[14] = cpu->cd.arm.svc_r13_r14[1];
 		break;
 	case ARM_MODE_ABT32:
+		memcpy(&cpu->cd.arm.r[8],
+		    cpu->cd.arm.default_r8_r14, sizeof(uint32_t) * 5);
 		cpu->cd.arm.r[13] = cpu->cd.arm.abt_r13_r14[0];
 		cpu->cd.arm.r[14] = cpu->cd.arm.abt_r13_r14[1];
 		break;
 	case ARM_MODE_UND32:
+		memcpy(&cpu->cd.arm.r[8],
+		    cpu->cd.arm.default_r8_r14, sizeof(uint32_t) * 5);
 		cpu->cd.arm.r[13] = cpu->cd.arm.und_r13_r14[0];
 		cpu->cd.arm.r[14] = cpu->cd.arm.und_r13_r14[1];
 		break;
@@ -512,7 +594,6 @@
  */
 void arm_exception(struct cpu *cpu, int exception_nr)
 {
-	int arm_exception_to_mode[N_ARM_EXCEPTIONS] = ARM_EXCEPTION_TO_MODE;
 	int oldmode, newmode;
 	uint32_t retaddr;
 
@@ -523,44 +604,52 @@
 
 	retaddr = cpu->pc;
 
-	debug("[ arm_exception(): ");
+	if (!quiet_mode) {
+		debug("[ arm_exception(): ");
+		switch (exception_nr) {
+		case ARM_EXCEPTION_RESET:
+			fatal("RESET: TODO");
+			break;
+		case ARM_EXCEPTION_UND:
+			debug("UNDEFINED");
+			break;
+		case ARM_EXCEPTION_SWI:
+			debug("SWI");
+			break;
+		case ARM_EXCEPTION_PREF_ABT:
+			debug("PREFETCH ABORT");
+			break;
+		case ARM_EXCEPTION_IRQ:
+			debug("IRQ");
+			break;
+		case ARM_EXCEPTION_FIQ:
+			debug("FIQ");
+			break;
+		case ARM_EXCEPTION_DATA_ABT:
+			debug("DATA ABORT, far=0x%08x fsr=0x%02x",
+			    cpu->cd.arm.far, cpu->cd.arm.fsr);
+			break;
+		}
+		debug(" ]\n");
+	}
 
 	switch (exception_nr) {
 	case ARM_EXCEPTION_RESET:
 		cpu->running = 0;
-		fatal("RESET: TODO");
+		fatal("ARM RESET: TODO");
 		exit(1);
-	case ARM_EXCEPTION_UND:
-		debug("UNDEFINED");
-		retaddr += 4;
-		break;
-	case ARM_EXCEPTION_SWI:
-		debug("SWI");
-		retaddr += 4;
-		break;
-	case ARM_EXCEPTION_PREF_ABT:
-		debug("PREFETCH ABORT");
-		retaddr += 4;
-		break;
-	case ARM_EXCEPTION_IRQ:
-		debug("IRQ");
-		retaddr += 4;
-		break;
-	case ARM_EXCEPTION_FIQ:
-		debug("FIQ");
-		retaddr += 4;
-		break;
 	case ARM_EXCEPTION_DATA_ABT:
-		debug("DATA ABORT, far=0x%08x fsr=0x%02x",
-		    cpu->cd.arm.far, cpu->cd.arm.fsr);
-		retaddr += 8;
+		retaddr += 4;
 		break;
 	}
 
-	debug(" ]\n");
+	retaddr += 4;
 
 	arm_save_register_bank(cpu);
 
+	cpu->cd.arm.cpsr &= 0x0fffffff;
+	cpu->cd.arm.cpsr |= (cpu->cd.arm.flags << 28);
+
 	switch (arm_exception_to_mode[exception_nr]) {
 	case ARM_MODE_SVC32:
 		cpu->cd.arm.spsr_svc = cpu->cd.arm.cpsr; break;
@@ -587,10 +676,18 @@
 	cpu->cd.arm.cpsr &= ~ARM_FLAG_MODE;
 	cpu->cd.arm.cpsr |= arm_exception_to_mode[exception_nr];
 
+	/*
+	 *  Usually, an exception should change modes (so that saved status
+	 *  bits don't get lost). However, Linux on ARM seems to use floating
+	 *  point instructions in the kernel (!), and it emulates those using
+	 *  its own fp emulation code. This leads to a situation where we
+	 *  sometimes change from SVC32 to SVC32.
+	 */
 	newmode = cpu->cd.arm.cpsr & ARM_FLAG_MODE;
-	if (oldmode == newmode) {
-		fatal("Exception caused no mode change? TODO\n");
-		exit(1);
+	if (oldmode == newmode && oldmode != ARM_MODE_SVC32) {
+		fatal("[ WARNING! Exception caused no mode change? "
+		    "mode 0x%02x (pc=0x%x) ]\n", newmode, (int)cpu->pc);
+		/*  exit(1);  */
 	}
 
 	cpu->cd.arm.cpsr |= ARM_FLAG_I;
@@ -601,12 +698,104 @@
 	/*  Load the new register bank, if we switched:  */
 	arm_load_register_bank(cpu);
 
-	/*  Set the return address and new PC:  */
+	/*
+	 *  Set the return address and new PC.
+	 *
+	 *  NOTE: r[ARM_PC] is also set; see cpu_arm_instr_loadstore.c for
+	 *  details. (If an exception occurs during a load into the pc
+	 *  register, the code in that file assumes that the r[ARM_PC]
+	 *  was changed to the address of the exception handler.)
+	 */
 	cpu->cd.arm.r[ARM_LR] = retaddr;
-
 	cpu->pc = cpu->cd.arm.r[ARM_PC] = exception_nr * 4 +
 	    ((cpu->cd.arm.control & ARM_CONTROL_V)? 0xffff0000 : 0);
-	arm_pc_to_pointers(cpu);
+	quick_pc_to_pointers(cpu);
+}
+
+
+/*
+ *  arm_cpu_tlbdump():
+ *
+ *  Called from the debugger to dump the TLB in a readable format.
+ *  x is the cpu number to dump, or -1 to dump all CPUs.
+ *
+ *  If rawflag is nonzero, then the TLB contents isn't formated nicely,
+ *  just dumped.
+ */
+void arm_cpu_tlbdump(struct machine *m, int x, int rawflag)
+{
+}
+
+
+static void add_response_word(struct cpu *cpu, char *r, uint32_t value,
+	size_t maxlen)
+{
+	if (cpu->byte_order == EMUL_LITTLE_ENDIAN) {
+		value = ((value & 0xff) << 24) +
+			((value & 0xff00) << 8) +
+			((value & 0xff0000) >> 8) +
+			((value & 0xff000000) >> 24);
+	}
+	snprintf(r + strlen(r), maxlen - strlen(r), "%08"PRIx32, value);
+}
+
+
+/*
+ *  arm_cpu_gdb_stub():
+ *
+ *  Execute a "remote GDB" command. Returns a newly allocated response string
+ *  on success, NULL on failure.
+ */
+char *arm_cpu_gdb_stub(struct cpu *cpu, char *cmd)
+{
+	if (strcmp(cmd, "g") == 0) {
+		/*  15 gprs, pc, 8 fprs, fps, cpsr.  */
+		int i;
+		char *r;
+		size_t len = 1 + 18 * sizeof(uint32_t);
+		r = malloc(len);
+		if (r == NULL) {
+			fprintf(stderr, "out of memory\n");
+			exit(1);
+		}
+		r[0] = '\0';
+		for (i=0; i<15; i++)
+			add_response_word(cpu, r, cpu->cd.arm.r[i], len);
+		add_response_word(cpu, r, cpu->pc, len);
+		/*  TODO: fprs:  */
+		for (i=0; i<8; i++)
+			add_response_word(cpu, r, 0, len);
+		/*  TODO: fps  */
+		add_response_word(cpu, r, 0, len);
+		add_response_word(cpu, r, cpu->cd.arm.cpsr, len);
+		return r;
+	}
+
+	if (cmd[0] == 'p') {
+		int regnr = strtol(cmd + 1, NULL, 16);
+		size_t len = 2 * sizeof(uint32_t) + 1;
+		char *r = malloc(len);
+		r[0] = '\0';
+		if (regnr == ARM_PC) {
+			add_response_word(cpu, r, cpu->pc, len);
+		} else if (regnr >= 0 && regnr < ARM_PC) {
+			add_response_word(cpu, r, cpu->cd.arm.r[regnr], len);
+		} else if (regnr >= 0x10 && regnr <= 0x17) {
+			/*  TODO: fprs  */
+			add_response_word(cpu, r, 0, len);
+			add_response_word(cpu, r, 0, len);
+			add_response_word(cpu, r, 0, len);
+		} else if (regnr == 0x18) {
+			/*  TODO: fps  */
+			add_response_word(cpu, r, 0, len);
+		} else if (regnr == 0x19) {
+			add_response_word(cpu, r, cpu->cd.arm.cpsr, len);
+		}
+		return r;
+	}
+
+	fatal("arm_cpu_gdb_stub(): TODO\n");
+	return NULL;
 }
 
 
@@ -620,13 +809,14 @@
  */
 int arm_cpu_interrupt(struct cpu *cpu, uint64_t irq_nr)
 {
-	/*  fatal("arm_cpu_interrupt(): 0x%llx\n", (int)irq_nr);  */
+	/*  fatal("arm_cpu_interrupt(): 0x%x\n", (int)irq_nr);  */
 	if (irq_nr <= 64) {
 		if (cpu->machine->md_interrupt != NULL)
 			cpu->machine->md_interrupt(cpu->machine,
 			    cpu, irq_nr, 1);
 		else
-			fatal("arm_cpu_interrupt(): md_interrupt == NULL\n");
+			fatal("arm_cpu_interrupt(): irq_nr=%i md_interrupt =="
+			    " NULL\n", (int)irq_nr);
 	} else {
 		/*  Assert ARM IRQs:  */
 		cpu->cd.arm.irq_asserted = 1;
@@ -667,7 +857,7 @@
  *  cpu->pc for relative addresses.
  */                     
 int arm_cpu_disassemble_instr(struct cpu *cpu, unsigned char *ib,
-        int running, uint64_t dumpaddr, int bintrans)
+        int running, uint64_t dumpaddr)
 {
 	uint32_t iw, tmp;
 	int main_opcode, secondary_opcode, s_bit, r16, r12, r8;
@@ -740,10 +930,22 @@
 			int a_bit = (iw >> 21) & 1;
 			debug("%s%sl%s%s\t", u_bit? "s" : "u",
 			    a_bit? "mla" : "mul", condition, s_bit? "s" : "");
-			debug("%s,", arm_regname[r12]);
-			debug("%s,", arm_regname[r16]);
-			debug("%s,", arm_regname[iw & 15]);
-			debug("%s\n", arm_regname[r8]);
+			debug("%s,%s,", arm_regname[r12], arm_regname[r16]);
+			debug("%s,%s\n", arm_regname[iw&15], arm_regname[r8]);
+			break;
+		}
+
+		/*
+		 *  xxxx0001 0000nnnn dddd0000 0101mmmm  qadd Rd,Rm,Rn
+		 *  xxxx0001 0010nnnn dddd0000 0101mmmm  qsub Rd,Rm,Rn
+		 *  xxxx0001 0100nnnn dddd0000 0101mmmm  qdadd Rd,Rm,Rn
+		 *  xxxx0001 0110nnnn dddd0000 0101mmmm  qdsub Rd,Rm,Rn
+		 */
+		if ((iw & 0x0f900ff0) == 0x01000050) {
+			debug("q%s%s%s\t", iw & 0x400000? "d" : "",
+			    iw & 0x200000? "sub" : "add", condition);
+			debug("%s,%s,%s\n", arm_regname[r12],
+			    arm_regname[iw&15], arm_regname[r16]);
 			break;
 		}
 
@@ -801,9 +1003,68 @@
 		}
 
 		/*
+		 *  xxxx0001 01101111 dddd1111 0001mmmm    CLZ Rd,Rm
+		 */
+		if ((iw & 0x0fff0ff0) == 0x016f0f10) {
+			debug("clz%s\t", condition);
+			debug("%s,%s\n", arm_regname[r12], arm_regname[iw&15]);
+			break;
+		}
+
+		/*
+		 *  xxxx0001 0000dddd nnnnssss 1yx0mmmm  SMLAxy Rd,Rm,Rs,Rn
+		 *  xxxx0001 0100dddd DDDDssss 1yx0mmmm  SMLALxy RdL,RdH,Rm,Rs
+		 *  xxxx0001 0010dddd nnnnssss 1y00mmmm  SMLAWy Rd,Rm,Rs,Rn
+		 *  xxxx0001 0110dddd 0000ssss 1yx0mmmm  SMULxy Rd,Rm,Rs
+		 *  xxxx0001 0010dddd 0000ssss 1y10mmmm  SMULWy Rd,Rm,Rs
+		 */
+		if ((iw & 0x0ff00090) == 0x01000080) {
+			debug("smla%s%s%s\t",
+			    iw & 0x20? "t" : "b", iw & 0x40? "t" : "b",
+			    condition);
+			debug("%s,%s,%s,%s\n", arm_regname[r16],
+			    arm_regname[iw&15], arm_regname[r8],
+			    arm_regname[r12]);
+			break;
+		}
+		if ((iw & 0x0ff00090) == 0x01400080) {
+			debug("smlal%s%s%s\t",
+			    iw & 0x20? "t" : "b", iw & 0x40? "t" : "b",
+			    condition);
+			debug("%s,%s,%s,%s\n", arm_regname[r12],
+			    arm_regname[r16], arm_regname[iw&15],
+			    arm_regname[r8]);
+			break;
+		}
+		if ((iw & 0x0ff000b0) == 0x01200080) {
+			debug("smlaw%s%s\t", iw & 0x40? "t" : "b",
+			    condition);
+			debug("%s,%s,%s,%s\n", arm_regname[r16],
+			    arm_regname[iw&15], arm_regname[r8],
+			    arm_regname[r12]);
+			break;
+		}
+		if ((iw & 0x0ff0f090) == 0x01600080) {
+			debug("smul%s%s%s\t",
+			    iw & 0x20? "t" : "b", iw & 0x40? "t" : "b",
+			    condition);
+			debug("%s,%s,%s\n", arm_regname[r16],
+			    arm_regname[iw&15], arm_regname[r8]);
+			break;
+		}
+		if ((iw & 0x0ff0f0b0) == 0x012000a0) {
+			debug("smulw%s%s\t", iw & 0x40? "t" : "b",
+			    condition);
+			debug("%s,%s,%s\n", arm_regname[r16],
+			    arm_regname[iw&15], arm_regname[r8]);
+			break;
+		}
+
+		/*
 		 *  xxxx000P U1WLnnnn ddddHHHH 1SH1LLLL load/store rd,imm(rn)
 		 */
 		if ((iw & 0x0e000090) == 0x00000090) {
+			char *op = "st";
 			int imm = ((iw >> 4) & 0xf0) | (iw & 0xf);
 			int regform = !(iw & 0x00400000);
 			p_bit = main_opcode & 1;
@@ -816,8 +1077,11 @@
 				break;
 			}
 			/*  Semi-generic case:  */
-			debug("%sr%s", iw & 0x00100000? "ld" : "st",
-			    condition);
+			if (iw & 0x00100000)
+				op = "ld";
+			if (!l_bit && (iw & 0xd0) == 0xd0)
+				op = iw & 0x20? "st" : "ld";
+			debug("%sr%s", op, condition);
 			if (!l_bit && (iw & 0xd0) == 0xd0) {
 				debug("d");		/*  Double-register  */
 			} else {
@@ -1017,7 +1281,7 @@
 				} else {
 					tmpw[0] = addr = cpu->cd.arm.r[r12];
 					if (r12 == ARM_PC)
-						addr += 8;
+						addr = cpu->pc + 8;
 				}
 				debug(": ");
 				if (b_bit)
@@ -1081,6 +1345,29 @@
 		break;
 	case 0xc:				/*  Coprocessor  */
 	case 0xd:				/*  LDC/STC  */
+		/*
+		 *  xxxx1100 0100nnnn ddddcccc oooommmm    MCRR c,op,Rd,Rn,CRm
+		 *  xxxx1100 0101nnnn ddddcccc oooommmm    MRRC c,op,Rd,Rn,CRm
+		 */
+		if ((iw & 0x0fe00fff) == 0x0c400000) {
+			debug("%s%s\t", iw & 0x100000? "mra" : "mar",
+			    condition);
+			if (iw & 0x100000)
+				debug("%s,%s,acc0\n",
+				    arm_regname[r12], arm_regname[r16]);
+			else
+				debug("acc0,%s,%s\n",
+				    arm_regname[r12], arm_regname[r16]);
+			break;
+		}
+		if ((iw & 0x0fe00000) == 0x0c400000) {
+			debug("%s%s\t", iw & 0x100000? "mrrc" : "mcrr",
+			    condition);
+			debug("%i,%i,%s,%s,cr%i\n", r8, (iw >> 4) & 15,
+			    arm_regname[r12], arm_regname[r16], iw & 15);
+			break;
+		}
+
 		/*  xxxx110P UNWLnnnn DDDDpppp oooooooo LDC/STC  */
 		debug("TODO: coprocessor LDC/STC\n");
 		break;
@@ -1089,6 +1376,22 @@
 		 *  xxxx1110 oooonnnn ddddpppp qqq0mmmm		CDP
 		 *  xxxx1110 oooLNNNN ddddpppp qqq1MMMM		MRC/MCR
 		 */
+		if ((iw & 0x0ff00ff0) == 0x0e200010) {
+			/*  Special case: mia* DSP instructions  */
+			switch ((iw >> 16) & 0xf) {
+			case  0: debug("mia"); break;
+			case  8: debug("miaph"); break;
+			case 12: debug("miaBB"); break;
+			case 13: debug("miaTB"); break;
+			case 14: debug("miaBT"); break;
+			case 15: debug("miaTT"); break;
+			default: debug("UNKNOWN mia vector instruction?");
+			}
+			debug("%s\t", condition);
+			debug("acc%i,%s,%s\n", ((iw >> 5) & 7),
+			    arm_regname[iw & 15], arm_regname[r12]);
+			break;
+		}
 		if (iw & 0x10) {
 			debug("%s%s\t",
 			    (iw & 0x00100000)? "mrc" : "mcr", condition);
@@ -1145,9 +1448,10 @@
 		cpu->cd.arm.coproc[cp_num](cpu, opcode1, opcode2, l_bit,
 		    crn, crm, rd);
 	else {
-		fatal("arm_mcr_mrc: pc=0x%08x, iword=0x%08x: "
-		    "cp_num=%i\n", (int)cpu->pc, iword, cp_num);
-		exit(1);
+		fatal("[ arm_mcr_mrc: pc=0x%08x, iword=0x%08x: "
+		    "cp_num=%i ]\n", (int)cpu->pc, iword, cp_num);
+		arm_exception(cpu, ARM_EXCEPTION_UND);
+		/*  exit(1);  */
 	}
 }
 
@@ -1162,8 +1466,9 @@
  */
 void arm_cdp(struct cpu *cpu, uint32_t iword)
 {
-	fatal("arm_cdp: pc=0x%08x, iword=0x%08x\n", (int)cpu->pc, iword);
-	exit(1);
+	fatal("[ arm_cdp: pc=0x%08x, iword=0x%08x ]\n", (int)cpu->pc, iword);
+	arm_exception(cpu, ARM_EXCEPTION_UND);
+	/*  exit(1);  */
 }