/*
 * Copyright (C) 2014-2016 FAUmachine Team <info@faumachine.org>.
 * This program is free software. You can redistribute it and/or modify it
 * under the terms of the GNU General Public License, either version 2 of
 * the License, or (at your option) any later version. See COPYING.
 */

/*
 * For most of the comments, infos, ... see:
 * ARM Architecture Reference Manual (ARM DDI 0100I)
 */

#define DEBUG_CONTROL_FLOW	0
#define DEBUG_CONTROL_FLOW_REGS	0

#ifdef INCLUDE

#include <assert.h>
#include <inttypes.h>
#include <stdio.h>

#include "glue.h"
#include "gdb.h"

#endif /* INCLUDE */
#ifdef STATE

struct {
	struct gdb *gdb;
	uint32_t brkstart[4];
	uint32_t brkend[4];

	int wfi;
	int exception_pending;

	uint32_t reg[13];
	uint32_t sp_main;
	uint32_t sp_process;
	uint32_t lr;
	enum { MODE_THREAD, MODE_HANDLER } current_mode;
	int control_spsel;
	int control_npriv;
	int primask_pm;
	int flag_shift_c;
	/*
	 * Program status register
	 * A2.5, A2-11
	 */
	int flag_c;
	int flag_n;
	int flag_v;
	int flag_z;
	int flag_t;
	int flag_align;
	int ipsr;

	uint32_t pc;
	uint32_t pc_next;
	uint32_t pc_next_next;
	uint16_t insn_prev;
	uint32_t insn;
	uint32_t insn_next;
	int flush;
	int stall;
#if (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION
	uint8_t itstate;
#endif

	uint32_t vtor;
	uint8_t prigroup;

	uint8_t sevonpend;
	uint8_t sleepdeep;
	uint8_t sleeponexit;
} NAME;

#endif /* STATE */
#ifdef EXPORT

/*forward*/ static void
NAME_(SCB_st)(struct cpssp *cpssp, uint32_t addr, unsigned int bs, uint32_t val);
/*forward*/ static void
NAME_(SCB_ld)(struct cpssp *cpssp, uint32_t addr, unsigned int bs, uint32_t *valp);
/*forward*/ static void
NAME_(clk)(struct cpssp *cpssp);
/*forward*/ static void
NAME_(irq_set)(struct cpssp *cpssp, unsigned int val);
/*forward*/ static void
NAME_(reset)(struct cpssp *cpssp);
/*forward*/ static void
NAME_(create)(const char *name, struct cpssp *cpssp);
/*forward*/ static void
NAME_(destroy)(struct cpssp *cpssp);

#endif /* EXPORT */
#ifdef BEHAVIOR

#if DEBUG_CONTROL_FLOW_REGS
static void
NAME_(dump)(struct cpssp *cpssp)
{
	fprintf(stderr, "r00=%08lx r01=%08lx r02=%08lx r03=%08lx\n",
		cpssp->NAME.reg[0], cpssp->NAME.reg[1],
		cpssp->NAME.reg[2], cpssp->NAME.reg[3]);
	fprintf(stderr, "r04=%08lx r05=%08lx r06=%08lx r07=%08lx\n",
		cpssp->NAME.reg[4], cpssp->NAME.reg[5],
		cpssp->NAME.reg[6], cpssp->NAME.reg[7]);
	fprintf(stderr, "r08=%08lx r09=%08lx r10=%08lx r11=%08lx\n",
		cpssp->NAME.reg[8], cpssp->NAME.reg[9],
		cpssp->NAME.reg[10], cpssp->NAME.reg[11]);
	fprintf(stderr, "r12=%08lx lr=%08lx\n",
		cpssp->NAME.reg[12], cpssp->NAME.lr);
	fprintf(stderr, "sp_main=%08lx sp_process=%08lx\n",
		cpssp->NAME.sp_main, cpssp->NAME.sp_process);
	fprintf(stderr, "c=%d n=%d v=%d z=%d t=%d align=%d ipsr=0x%02x\n",
		cpssp->NAME.flag_c, cpssp->NAME.flag_n,
		cpssp->NAME.flag_v, cpssp->NAME.flag_z,
		cpssp->NAME.flag_t, cpssp->NAME.flag_align,
		cpssp->NAME.ipsr);
#if 0
	fprintf(stderr, "r00=%e ", *(float *) &cpssp->NAME.reg[0]);
	fprintf(stderr, "r01=%e ", *(float *) &cpssp->NAME.reg[1]);
	fprintf(stderr, "r02=%e ", *(float *) &cpssp->NAME.reg[2]);
	fprintf(stderr, "r03=%e ", *(float *) &cpssp->NAME.reg[3]);
	fprintf(stderr, "\n");
	fprintf(stderr, "r04=%e ", *(float *) &cpssp->NAME.reg[4]);
	fprintf(stderr, "r05=%e ", *(float *) &cpssp->NAME.reg[5]);
	fprintf(stderr, "r06=%e ", *(float *) &cpssp->NAME.reg[6]);
	fprintf(stderr, "r07=%e ", *(float *) &cpssp->NAME.reg[7]);
	fprintf(stderr, "\n");
	fprintf(stderr, "r08=%e ", *(float *) &cpssp->NAME.reg[8]);
	fprintf(stderr, "r09=%e ", *(float *) &cpssp->NAME.reg[9]);
	fprintf(stderr, "r10=%e ", *(float *) &cpssp->NAME.reg[10]);
	fprintf(stderr, "r11=%e ", *(float *) &cpssp->NAME.reg[11]);
	fprintf(stderr, "\n");
#endif
}
#endif

static void
NAME_(store_flags)(struct cpssp *cpssp, int c, int v, uint32_t res)
{
	cpssp->NAME.flag_c = c;
	cpssp->NAME.flag_n = (res >> 31) & 1;
	cpssp->NAME.flag_v = v;
	cpssp->NAME.flag_z = res == 0;
}

#if CONFIG_ARM
static void
NAME_(store_flags64)(struct cpssp *cpssp, int c, int v, uint64_t res)
{
	cpssp->NAME.flag_c = c;
	cpssp->NAME.flag_n = (res >> 63) & 1;
	cpssp->NAME.flag_v = v;
	cpssp->NAME.flag_z = res == 0;
}
#endif /* CONFIG_ARM */

static uint32_t
NAME_(load_sp)(struct cpssp *cpssp)
{
	switch (cpssp->NAME.control_spsel) {
	case 0:
		return cpssp->NAME.sp_main;
	case 1:
		return cpssp->NAME.sp_process;
	default:
		assert(0);
	}
}

static void
NAME_(store_sp)(struct cpssp *cpssp, uint32_t val)
{
	switch (cpssp->NAME.control_spsel) {
	case 0:
		cpssp->NAME.sp_main = val;
		break;
	case 1:
		cpssp->NAME.sp_process = val;
		break;
	default:
		assert(0);
	}
}

static uint32_t
NAME_(load_lr)(struct cpssp *cpssp)
{
	return cpssp->NAME.lr;
}

static void
NAME_(store_lr)(struct cpssp *cpssp, uint32_t val)
{
	cpssp->NAME.lr = val;
}

static uint32_t
NAME_(load_pc)(struct cpssp *cpssp)
{
	return cpssp->NAME.pc + 4;
}

static void
NAME_(store_pc)(struct cpssp *cpssp, uint32_t val)
{
	cpssp->NAME.pc_next_next = val & ~1;
	cpssp->NAME.flush = 1;
}

static uint32_t
NAME_(load_reg)(struct cpssp *cpssp, int nr)
{
	switch (nr) {
	case 13:
		return NAME_(load_sp)(cpssp);
	case 14:
		return NAME_(load_lr)(cpssp);
	case 15:
		return NAME_(load_pc)(cpssp);
	default:
		return cpssp->NAME.reg[nr];
	}
}

static void
NAME_(store_reg)(struct cpssp *cpssp, int nr, uint32_t val)
{
	switch (nr) {
	case 13:
		NAME_(store_sp)(cpssp, val);
		break;
	case 14:
		NAME_(store_lr)(cpssp, val);
		break;
	case 15:
		NAME_(store_pc)(cpssp, val);
		break;
	default:
		cpssp->NAME.reg[nr] = val;
		break;
	}
}

#if CONFIG_ARM
#if 5 <= CONFIG_VERSION && CONFIG_E
static uint64_t
NAME_(load_reg64)(struct cpssp *cpssp, int nr)
{
	uint32_t val0;
	uint32_t val1;

	assert(! (nr & 1));

	val0 = NAME_(load_reg)(cpssp, nr + 0);
	val1 = NAME_(load_reg)(cpssp, nr + 1);

	return ((uint64_t) val1 << 32) | ((uint64_t) val0 << 0);
}

static void
NAME_(store_reg64)(struct cpssp *cpssp, int nr, uint64_t val)
{
	assert(! (nr & 1));

	NAME_(store_reg)(cpssp, nr + 0, ((val >>  0) & 0xffffffff));
	NAME_(store_reg)(cpssp, nr + 1, ((val >> 32) & 0xffffffff));
}
#endif /* 5 <= CONFIG_VERSION && CONFIG_E */
#endif /* CONFIG_ARM */

/*
 * Program status register
 * A2.5, A2-11
 */
static uint32_t
NAME_(load_apsr)(struct cpssp *cpssp)
{
	return (cpssp->NAME.flag_n << 31)
		| (cpssp->NAME.flag_z << 30)
		| (cpssp->NAME.flag_c << 29)
		| (cpssp->NAME.flag_v << 28);
}

static void
NAME_(store_apsr)(struct cpssp *cpssp, uint32_t val)
{
	cpssp->NAME.flag_n = (val >> 31) & 0b1;
	cpssp->NAME.flag_z = (val >> 30) & 0b1;
	cpssp->NAME.flag_c = (val >> 29) & 0b1;
	cpssp->NAME.flag_v = (val >> 28) & 0b1;
}

static uint32_t
NAME_(load_epsr)(struct cpssp *cpssp)
{
	return (cpssp->NAME.flag_t << 24)
		| (cpssp->NAME.flag_align << 9);
}

static void
NAME_(store_epsr)(struct cpssp *cpssp, uint32_t val)
{
	cpssp->NAME.flag_t = (val >> 24) & 0b1;
	cpssp->NAME.flag_align = (val >> 9) & 0b1;
}

static uint32_t
NAME_(load_ipsr)(struct cpssp *cpssp)
{
	return cpssp->NAME.ipsr << 0;
}

static void
NAME_(store_ipsr)(struct cpssp *cpssp, uint32_t val)
{
	cpssp->NAME.ipsr = val & 0x3f;
}

static uint32_t
NAME_(load_xpsr)(struct cpssp *cpssp)
{
	return NAME_(load_apsr)(cpssp)
		| NAME_(load_epsr)(cpssp)
		| NAME_(load_ipsr)(cpssp);
}

static void
NAME_(store_xpsr)(struct cpssp *cpssp, uint32_t val)
{
	NAME_(store_apsr)(cpssp, val);
	NAME_(store_epsr)(cpssp, val);
	NAME_(store_ipsr)(cpssp, val);
}

#if CONFIG_ARM
static uint32_t
NAME_(load_cpsr)(struct cpssp *cpssp)
{
	return NAME_(load_xpsr)(cpssp);
}

static void
NAME_(store_cpsr)(struct cpssp *cpssp, uint32_t val)
{
	NAME_(store_xpsr)(cpssp, val);
}

static uint32_t
NAME_(load_spsr)(struct cpssp *cpssp)
{
	assert(0);
	return 0; /* FIXME */
}

static void
NAME_(store_spsr)(struct cpssp *cpssp, uint32_t val)
{
	assert(0); /* FIXME */
}
#endif /* CONFIG_ARM */

#if CONFIG_M
static void
NAME_(store_spec)(struct cpssp *cpssp, uint32_t sysm, uint32_t val)
{
	switch ((sysm >> 3) & 0b11111) {
	case 0b00000:
		if (((sysm >> 2) & 1) == 0) {
			val &= 0xf8000000;
			NAME_(store_apsr)(cpssp, val);
		}
		break;
	case 0b00001:
		if (cpssp->NAME.current_mode == MODE_HANDLER
		 || ! cpssp->NAME.control_npriv) {
			val &= ~0b11;
			switch ((sysm >> 0) & 0b111) {
			case 0b000:
				cpssp->NAME.sp_main = val;
				break;
			case 0b001:
				cpssp->NAME.sp_process = val;
				break;
			default:
				assert(0); /* FIXME */
			}
		}
		break;
	case 0b00010:
		if (cpssp->NAME.current_mode == MODE_HANDLER
		 || ! cpssp->NAME.control_npriv) {
			switch ((sysm >> 0) & 0b111) {
			case 0b000:
				NAME_(primask_set)(cpssp, (val >> 0) & 1);
				break;
			case 0b001:
				/* BASEPRI */
				/* FIXME */
				break;
			case 0b010:
				/* BASEPRI_MAX */
				assert(0); /* FIXME */
				break;
			case 0b011:
				assert(0); /* FIXME */
				break;
			case 0b100:
				if (cpssp->NAME.current_mode == MODE_THREAD) {
					cpssp->NAME.control_spsel = (val >> 1) & 1;
					cpssp->NAME.control_npriv = (val >> 0) & 1;
				}
				break;
			default:
				assert(0); /* FIXME */
			}
		}
		break;
	default:
		assert(0); /* FIXME */
	}
}

static uint32_t
NAME_(load_spec)(struct cpssp *cpssp, uint32_t sysm)
{
	uint32_t val;

	switch ((sysm >> 3) & 0b11111) {
	case 0b00000:
		val = 0;
		if ((sysm >> 0) & 1) {
			val |= NAME_(load_ipsr(cpssp));
		}
		if ((sysm >> 1) & 1) {
			val |= 0 << 24; /* T-bit reads as zero */
		}
		if (((sysm >> 2) & 1) == 0) {
			val |= NAME_(load_apsr)(cpssp);
		}
		break;
	case 0b00001:
		switch ((sysm >> 0) & 0b111) {
		case 0b000:
			val = cpssp->NAME.sp_main;
			break;
		case 0b001:
			val = cpssp->NAME.sp_process;
			break;
		default:
			assert(0);
		}
		break;
	case 0b00010:
		switch ((sysm >> 0) & 0b111) {
		case 0b000:
			val = NAME_(primask_get)(cpssp);
			break;
		case 0b001:
			/* BASEPRI */
		case 0b010:
			/* BASEPRI_MAX */
			val = 0; /* FIXME */
			break;
		case 0b011:
			assert(0); /* FIXME */
			break;
		case 0b100:
			val = (cpssp->NAME.control_spsel << 1)
				| (cpssp->NAME.control_npriv << 0);
			break;
		default:
			assert(0); /* FIXME */
		}
		break;
	default:
		assert(0); /* FIXME */
	}

	return val;
}
#endif /* CONFIG_M */

#if ! CONFIG_ARM
static void
NAME_(SCB_ld)(struct cpssp *cpssp, uint32_t addr, unsigned int bs, uint32_t *valp)
{
	/*
	 * System Control and ID Registers
	 * B3.2.2
	 * ARMv7-M: B3-652
	 */
	*valp = 0;
	switch (addr & 0xff) {
	case 0x00:
		/*
		 * CPUID Base Register
		 * B3.2.3
		 * ARMv7-M: B3-655
		 */
		*valp = ('F' << 24) /* Implementer 'F'AU */
			| (0x0 << 20) /* Implementation Defined */
			| (0xc0 << 16) /* ARMv6-M */
			| (0x000 << 4) /* Implementation Defined */
			| (0x0 << 0); /* Implementation Defined */
		break;
	case 0x04:
		/*
		 * Interrupt Control State Register (ICSR)
		 * B3-265
		 * ARMv7-M: B3-655
		 */
#if 0 /* FIXME */
		*valp |= cpssp->NAME.NMI_pending << 31;
		/* 30-29: Reserved */
		*valp |= cpssp->NAME.PendSV_pending << 28;
		/* 27: Write-only */
		*valp |= cpssp->NAME.SysTick_pending << 26;
#endif
		/* 25: Write-only */
		/* 24: Reserved */
#if CONFIG_DEBUG
		assert(0); /* FIXME */
#else
		/* 23-22: Reserved */
#endif
		/* 21: Reserved */
		// assert(0); /* 20-12: FIXME */
		/* 11-9: Reserved */
		*valp |= cpssp->NAME.ipsr << 0;
		fprintf(stderr, "WARNING: %s: addr=0x%x\n", __FUNCTION__, addr);
		break;
	case 0x08:
		/*
		 * Vector Table Offset Register (VTOR)
		 * B3-267
		 * ARMv7-M: B3-657
		 */
		*valp = cpssp->NAME.vtor;
		break;
	case 0x0c:
		/*
		 * Application Interrupt and Reset Control Register (AIRCR)
		 * B3-268
		 * ARMv7-M: B3-658
		 */
		*valp = 0;
		*valp |= 0xfa05 << 16;
		*valp |= 0 << 15; /* Little Endian */
		/* Bit 14-11: Reserved */
		*valp |= cpssp->NAME.prigroup << 8;
		/* Bit 7-3: Reserved */
		*valp |= 0 << 2; /* Write-only */
		*valp |= 0 << 1; /* Write-only */
		*valp |= 0 << 0; /* Write-only */
		break;
	case 0x10:
		/* System Control Register (SCR) */
		/* B3-269 */
		/* 31-5: Reserved */
		*valp |= cpssp->NAME.sevonpend << 4;
		/* 3: Reserved */
		*valp |= cpssp->NAME.sleepdeep << 2;
		*valp |= cpssp->NAME.sleeponexit << 1;
		/* 0: Reserved */
		break;
	case 0x14:
		/* Configuration and Control Register (CCR) */
		/* B3-271 */
		/* 31-10: Reserved */
		*valp |= 1 << 9; /* Stack Align */
		*valp |= 0b11111 << 4; /* 8-4: Reserved */
		*valp |= 1 << 3; /* Unaligned Trap */
		/* 2-0: Reserved */
		break;
	case 0x1c:
		/* System Handler Priority Register 2 (SHPR2) */
		/* B3-272 */
		*valp |= NAME_(pri_get)(cpssp, 11) << 24;
		/* 29-0: Reserved */
		break;
	case 0x20:
		/* System Handler Priority Register 3 (SHPR3) */
		/* B3-273 */
		*valp |= NAME_(pri_get)(cpssp, 15) << 24;
		/* 29-24: Reserved */
		*valp |= NAME_(pri_get)(cpssp, 14) << 16;
		/* 21-0: Reserved */
		break;
#if CONFIG_DEBUG
	case 0x24:
		/* System Handler Control and State Register (SHCSR) */
		/* C1-329 */
		assert(0); /* FIXME */
	case 0x30:
		/* Debug Fault Status Register (DFSR) */
		/* C1-330 */
		assert(0); /* FIXME */
#endif /* CONFIG_DEBUG */
	case 0x88:
		/* Coprocessor Access Control Register */
		*valp |= 0; /* FIXME */
		break;
	default:
		fprintf(stderr, "WARNING: %s: addr=0x%08lx\n",
				__FUNCTION__, addr);
		assert(0); /* FIXME */
	}
}

static void
NAME_(SCB_st)(struct cpssp *cpssp, uint32_t addr, unsigned int bs, uint32_t val)
{
	/*
	 * System Control Block
	 * B3.2.2
	 * ARMv7-M: B3-652
	 */
	switch (addr & 0xff) {
	case 0x00:
		/*
		 * CPUID Base Register
		 * B3.2.3
		 * ARMv7-M: B3-655
		 */
		/* Read-only */
		break;
	case 0x04:
		/*
		 * Interrupt Control State Register (ICSR)
		 * B3-265
		 * ARMv7-M: B3-655
		 */
		if ((val >> 31) & 1) {
			/* Set pending NMI. */
			NAME_(NMI_pending_set)(cpssp, 1);
		}
		/* 30-29: Reserved */
		switch ((val >> 27) & 0b11) {
		case 0b00:
			/* Nothing to do... */
			break;
		case 0b01:
			/* Clear pending PendSV. */
			NAME_(PendSV_pending_set)(cpssp, 0);
			break;
		case 0b10:
			/* Set pending PendSV. */
			NAME_(PendSV_pending_set)(cpssp, 1);
			break;
		case 0b11:
		default:
			assert(0); /* FIXME */
		}
		switch ((val >> 25) & 0b11) {
		case 0b00:
			/* Nothing to do... */
			break;
		case 0b01:
			/* Clear pending SysTick. */
			NAME_(SysTick_pending_set)(cpssp, 0);
			break;
		case 0b10:
			/* Set pending SysTick. */
			NAME_(SysTick_pending_set)(cpssp, 1);
			break;
		case 0b11:
		default:
			assert(0); /* FIXME */
		}
		/* 24: Reserved */
#if CONFIG_DEBUG
		assert(0); /* FIXME */
#else
		/* 23-22: Reserved */
#endif
		/* 21: Reserved */
		/* 20-12: Read-only */
		/* 11-9: Reserved */
#if CONFIG_DEBUG
		assert(0); /* FIXME */
#else
		/* 8-0: Reserved */
#endif
		break;
	case 0x08:
		/*
		 * Vector Table Offset Register (VTOR)
		 * B3-267
		 * ARMv7-M: B3-657
		 */
		assert(bs == 0b1111);

		cpssp->NAME.vtor = val & 0xffffff80;
		break;
	case 0x0c:
		/*
		 * Application Interrupt and Reset Control Register (AIRCR)
		 * B3-268
		 * ARMv7-M: B3-658
		 */
		assert(bs == 0b1111);

		assert(((val >> 16) & 0xffff) == 0x05fa); /* FIXME */

		/* Bit 15: Read-only */
		/* Bit 14-11: Reserved */
		cpssp->NAME.prigroup = (val >> 8) & 0x7;
		/* Bit 7-3: Reserved */
		if ((val >> 2) & 1) {
			/* sysresetreq */
			assert(0); /* FIXME */
		}
		if ((val >> 1) & 1) {
			/* vectclractive */
			assert(0); /* FIXME */
		}
		if ((val >> 0) & 1) {
			/* vectreset */
			assert(0); /* FIXME */
		}
		break;
	case 0x10:
		/* System Control Register (SCR) */
		/* B3-269 */
		assert(bs == 0b1111);

		/* 31-5: Reserved */
		cpssp->NAME.sevonpend = (val >> 4) & 1;
		/* 3: Reserved */
		cpssp->NAME.sleepdeep = (val >> 2) & 1;
		cpssp->NAME.sleeponexit = (val >> 1) & 1;
		/* 0: Reserved */
		break;
	case 0x14:
		/* Configuration and Control Register (CCR) */
		/* B3-271 */
		assert(bs == 0b1111);

		/* Read-only */
		break;
	case 0x1c:
		/* System Handler Priority Register 2 (SHPR2) */
		/* B3-272 */
		if ((bs >> 3) & 1) {
			NAME_(pri_set)(cpssp, 11, (val >> 24) & 0xff);
		}
		if ((bs >> 2) & 1) {
			/* 23-16: Reserved */
		}
		if ((bs >> 1) & 1) {
			/* 15-8: Reserved */
		}
		if ((bs >> 0) & 1) {
			/* 7-0: Reserved */
		}
		break;
	case 0x20:
		/* System Handler Priority Register 3 (SHPR3) */
		/* B3-273 */
		if ((bs >> 3) & 1) {
			NAME_(pri_set)(cpssp, 15, (val >> 24) & 0xff);
		}
		if ((bs >> 2) & 1) {
			NAME_(pri_set)(cpssp, 14, (val >> 16) & 0xff);
		}
		if ((bs >> 1) & 1) {
			/* 15-8: Reserved */
		}
		if ((bs >> 0) & 1) {
			/* 7-0: Reserved */
		}
		break;
#if CONFIG_DEBUG
	case 0x24:
		/* System Handler Control and State Register (SHCSR) */
		/* C1-329 */
		assert(bs == 0b1111);

		assert(0); /* FIXME */
	case 0x30:
		/* Debug Fault Status Register (DFSR) */
		/* C1-330 */
		assert(bs == 0b1111);

		assert(0); /* FIXME */
#endif /* CONFIG_DEBUG */
	case 0x88:
		/* Coprocessor Access Control Register */
		/* FIXME */
		break;
	default:
		fprintf(stderr, "WARNING: %s: addr=0x%08lx val=0x%08lx\n",
				__FUNCTION__, addr, val);
		assert(0); /* FIXME */
	}
}
#endif /* ! CONFIG_ARM */

static void
NAME_(ld)(struct cpssp *cpssp, uint32_t addr, int bs, uint32_t *valp)
{
#if ! CONFIG_ARM
	if (0xf8000000 <= addr /* && addr < 0x100000000 */) {
		/* Direct GPIO access. */
		NAME_(gpio_ld)(cpssp, addr, bs, valp);
	} else
#endif
	{
		NAME_(mr)(cpssp, addr, bs, valp);
	}
}

static void
NAME_(st)(struct cpssp *cpssp, uint32_t addr, int bs, uint32_t val)
{
#if ! CONFIG_ARM	/* FIXME */
	if (0xf8000000 <= addr /* && addr < 0x100000000 */) {
		/* Direct GPIO access. */
		NAME_(gpio_st)(cpssp, addr, bs, val);
	} else
#endif
	{
		NAME_(mw)(cpssp, addr, bs, val);
	}
}

#if CONFIG_THUMB
static uint16_t
NAME_(ld16_code)(struct cpssp *cpssp, uint32_t addr)
{
	uint32_t val32;
	uint16_t val;

	switch (addr & 3) {
	case 0:
		NAME_(mx)(cpssp, addr & ~3, 0b0011, &val32);
		val = val32 >> 0;
		break;
	case 1:
		assert(0); /* Unaligned - FIXME */
	case 2:
		NAME_(mx)(cpssp, addr & ~3, 0b1100, &val32);
		val = val32 >> 16;
		break;
	case 3:
		assert(0); /* Unaligned - FIXME */
	default:
		assert(0); /* Cannot happen. */
	}

	return val;
}
#endif /* CONFIG_THUMB */

#if CONFIG_ARM
static uint32_t
NAME_(ld32_code)(struct cpssp *cpssp, uint32_t addr)
{
	uint32_t val;

	NAME_(mx)(cpssp, addr, 0b1111, &val);

	return val;
}
#endif /* CONFIG_ARM */

static uint8_t
NAME_(ld8)(struct cpssp *cpssp, uint32_t addr)
{
	uint32_t val32;
	uint8_t val;

	cpssp->NAME.stall = 1;

	switch (addr & 3) {
	case 0:
		NAME_(ld)(cpssp, addr & ~3, 0b0001, &val32);
		val = val32 >> 0;
		break;
	case 1:
		NAME_(ld)(cpssp, addr & ~3, 0b0010, &val32);
		val = val32 >> 8;
		break;
	case 2:
		NAME_(ld)(cpssp, addr & ~3, 0b0100, &val32);
		val = val32 >> 16;
		break;
	case 3:
		NAME_(ld)(cpssp, addr & ~3, 0b1000, &val32);
		val = val32 >> 24;
		break;
	default:
		assert(0); /* Cannot happen. */
	}

	return val;
}

static void
NAME_(st8)(struct cpssp *cpssp, uint32_t addr, uint8_t val)
{
	uint32_t val32;

	cpssp->NAME.stall = 1;

	switch (addr & 3) {
	case 0:
		val32 = val << 0;
		NAME_(st)(cpssp, addr & ~3, 0b0001, val32);
		break;
	case 1:
		val32 = val << 8;
		NAME_(st)(cpssp, addr & ~3, 0b0010, val32);
		break;
	case 2:
		val32 = val << 16;
		NAME_(st)(cpssp, addr & ~3, 0b0100, val32);
		break;
	case 3:
		val32 = val << 24;
		NAME_(st)(cpssp, addr & ~3, 0b1000, val32);
		break;
	default:
		assert(0); /* Cannot happen. */
	}
}

static uint16_t
NAME_(ld16)(struct cpssp *cpssp, uint32_t addr)
{
	uint32_t val32;
	uint16_t val;

	cpssp->NAME.stall = 1;

	switch (addr & 3) {
	case 0:
		NAME_(ld)(cpssp, addr & ~3, 0b0011, &val32);
		val = val32 >> 0;
		break;
	case 1:
		assert(0); /* Unaligned - FIXME */
	case 2:
		NAME_(ld)(cpssp, addr & ~3, 0b1100, &val32);
		val = val32 >> 16;
		break;
	case 3:
		assert(0); /* Unaligned - FIXME */
	default:
		assert(0); /* Cannot happen. */
	}

	return val;
}

static void
NAME_(st16)(struct cpssp *cpssp, uint32_t addr, uint16_t val)
{
	uint32_t val32;

	cpssp->NAME.stall = 1;

	switch (addr & 3) {
	case 0:
		val32 = val << 0;
		NAME_(st)(cpssp, addr & ~3, 0b0011, val32);
		break;
	case 1:
		assert(0); /* Unaligned - FIXME */
	case 2:
		val32 = val << 16;
		NAME_(st)(cpssp, addr & ~3, 0b1100, val32);
		break;
	case 3:
		assert(0); /* Unaligned - FIXME */
	default:
		assert(0); /* Cannot happen. */
	}
}

static uint32_t
NAME_(ld32)(struct cpssp *cpssp, uint32_t addr)
{
	uint32_t val;

	assert(! (addr & 3));

	cpssp->NAME.stall = 1;

	NAME_(ld)(cpssp, addr, 0b1111, &val);

	return val;
}

static void
NAME_(st32)(struct cpssp *cpssp, uint32_t addr, uint32_t val)
{
	assert(! (addr & 3));

	cpssp->NAME.stall = 1;

	NAME_(st)(cpssp, addr, 0b1111, val);
}

#if CONFIG_ARM
#if (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION
static uint64_t
NAME_(ld64)(struct cpssp *cpssp, uint32_t addr)
{
	uint32_t val0;
	uint32_t val1;

	assert(! (addr & 7));

	cpssp->NAME.stall = 1;

	NAME_(ld)(cpssp, addr + 0, 0b1111, &val0);
	NAME_(ld)(cpssp, addr + 4, 0b1111, &val1);

	return ((uint64_t) val1 << 32) | ((uint64_t) val0 << 0);
}

static void
NAME_(st64)(struct cpssp *cpssp, uint32_t addr, uint64_t val)
{
	assert(! (addr & 7));

	cpssp->NAME.stall = 1;

	NAME_(st)(cpssp, addr + 0, 0b1111, (val >>  0) & 0xffffffff);
	NAME_(st)(cpssp, addr + 4, 0b1111, (val >> 32) & 0xffffffff);
}
#endif /* (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION */
#endif /* CONFIG_ARM */

/*
 * Addressing Mode 2 - Load and Store Word or Unsigned Byte
 * A5.2.1, page A5-19.
 */
static uint32_t
NAME_(addr_mode_2_pre)(
	struct cpssp *cpssp,
	int p,
	int u,
	int w,
	uint8_t rn,
	uint32_t offset
)
{
	uint32_t addr;

	addr = NAME_(load_reg)(cpssp, rn);
	if (rn == 15
	 && cpssp->NAME.flag_t) {
		addr &= ~3;
	}
	if (p) {
		if (u) {
			addr += offset;
		} else {
			addr -= offset;
		}
	}

	return addr;
}

static void
NAME_(addr_mode_2_post)(
	struct cpssp *cpssp,
	int p,
	int u,
	int w,
	uint8_t rn,
	uint32_t addr,
	uint32_t offset
)
{
	if (! p) {
		if (u) {
			addr += offset;
		} else {
			addr -= offset;
		}
	}
	if (! p || w) {
		NAME_(store_reg)(cpssp, rn, addr);
	}
}

static void
NAME_(ldst)(
	struct cpssp *cpssp,
	int p,
	int b,
	int w,
	int l,
	uint8_t rd,
	uint32_t addr
)
{
	uint32_t tmp32;

	assert(! (! p && w)); /* User-mode-access - FIXME */

	if (b) {
		/* Unsigned Byte Access */
		if (l) {
			/* Load */
			tmp32 = (uint32_t) (uint8_t) NAME_(ld8)(cpssp, addr);
			NAME_(store_reg)(cpssp, rd, tmp32);
		} else {
			/* Store */
			tmp32 = NAME_(load_reg)(cpssp, rd);
			NAME_(st8)(cpssp, addr, (uint8_t) tmp32);
		}
	} else {
		/* Word Access */
		if (l) {
			/* Load */
			tmp32 = NAME_(ld32)(cpssp, addr);
			NAME_(store_reg)(cpssp, rd, tmp32);
		} else {
			/* Store */
			tmp32 = NAME_(load_reg)(cpssp, rd);
			NAME_(st32)(cpssp, addr, tmp32);
		}
	}
}

static uint32_t
NAME_(ldusbh)(struct cpssp *cpssp, int s, int h, uint32_t addr)
{
	uint32_t res;

	if (! s) {
		if (! h) {
			res = (uint32_t) (uint8_t) NAME_(ld8)(cpssp, addr);
		} else {
			res = (uint32_t) (uint16_t) NAME_(ld16)(cpssp, addr);
		}
	} else {
		if (! h) {
			res = (int32_t) (int8_t) NAME_(ld8)(cpssp, addr);
		} else {
			res = (int32_t) (int16_t) NAME_(ld16)(cpssp, addr);
		}
	}

	return res;
}

/*
 * Addressing Mode 4 - Load and Store Multiple, A5.4, A5-41
 * Encoding, A5.4.1, A5-42
 */
static uint32_t
NAME_(addr_mode_4_pre)(struct cpssp *cpssp, uint8_t rn)
{
	return NAME_(load_reg)(cpssp, rn);
}

static void
NAME_(addr_mode_4_post)(struct cpssp *cpssp, int w, uint8_t rn, uint32_t addr)
{
	if (w) {
		NAME_(store_reg)(cpssp, rn, addr);
	}
}

static uint32_t
NAME_(ldstm)(
	struct cpssp *cpssp,
	int p,
	int u,
	int s,
	int l,
	uint32_t addr,
	uint16_t reglist
)
{
	uint32_t tmp32;
	int i;

	assert(! s); /* FIXME */

	if (u) {
		for (i = 0; i <= 15; i++) {
			if ((reglist >> i) & 1) {
				if (p) {
					addr += 4;
				}
				if (l) {
					tmp32 = NAME_(ld32)(cpssp, addr);
					if (i == 15) {
						cpssp->NAME.flag_t = tmp32 & 1;
						NAME_(store_pc)(cpssp, tmp32 & ~1);
					} else {
						NAME_(store_reg)(cpssp, i, tmp32);
					}
				} else {
					if (i == 15) {
						tmp32 = NAME_(load_pc)(cpssp);
						tmp32 |= cpssp->NAME.flag_t;
					} else {
						tmp32 = NAME_(load_reg)(cpssp, i);
					}
					NAME_(st32)(cpssp, addr, tmp32);
				}
				if (! p) {
					addr += 4;
				}
			}
		}
	} else {
		for (i = 15; 0 <= i; i--) {
			if ((reglist >> i) & 1) {
				if (p) {
					addr -= 4;
				}
				if (l) {
					tmp32 = NAME_(ld32)(cpssp, addr);
					if (i == 15) {
						cpssp->NAME.flag_t = tmp32 & 1;
						NAME_(store_pc)(cpssp, tmp32 & ~1);
					} else {
						NAME_(store_reg)(cpssp, i, tmp32);
					}
				} else {
					if (i == 15) {
						tmp32 = NAME_(load_pc)(cpssp);
						tmp32 |= cpssp->NAME.flag_t;
					} else {
						tmp32 = NAME_(load_reg)(cpssp, i);
					}
					NAME_(st32)(cpssp, addr, tmp32);
				}
				if (! p) {
					addr -= 4;
				}
			}
		}
	}

	return addr;
}

/* AND, A4.1.4, A4-8 */
static uint32_t
NAME_(ands)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	int *cp,
	int *vp
)
{
	uint32_t res;

	res = op0 & op1;
	*cp = cpssp->NAME.flag_shift_c;
	*vp = cpssp->NAME.flag_v;

	return res;
}

/* EOR, A4.1.18, A4-32 */
static uint32_t
NAME_(eors)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	int *cp,
	int *vp
)
{
	uint32_t res;

	res = op0 ^ op1;
	*cp = cpssp->NAME.flag_shift_c;
	*vp = cpssp->NAME.flag_v;

	return res;
}

/* ORR, A4.1.42, A4-84 */
static uint32_t
NAME_(orrs)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	int *cp,
	int *vp
)
{
	uint32_t res;

	res = op0 | op1;
	*cp = cpssp->NAME.flag_shift_c;
	*vp = cpssp->NAME.flag_v;

	return res;
}

#if (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION
/* ORN, ARMv7-M: A7-333 */
static uint32_t
NAME_(orns)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	int *cp,
	int *vp
)
{
	uint32_t res;

	res = op0 | ~op1;
	*cp = cpssp->NAME.flag_shift_c;
	*vp = cpssp->NAME.flag_v;

	return res;
}
#endif /* (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION */

/* BIC, A4.1.6, A4-12 */
static uint32_t
NAME_(bics)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	int *cp,
	int *vp
)
{
	uint32_t res;

	res = op0 & ~op1;
	*cp = cpssp->NAME.flag_shift_c;
	*vp = cpssp->NAME.flag_v;

	return res;
}

/* ADC, A4.1.2, A4-4 */
static uint32_t
NAME_(adcs)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	int c,
	int *cp,
	int *vp
)
{
	uint64_t unsigned_sum;
	int64_t signed_sum;
	uint32_t result;

	unsigned_sum = (uint64_t) op0 + (uint64_t) op1 + (uint64_t) c;
	signed_sum = (int64_t) (int32_t) op0
			+ (int64_t) (int32_t) op1 + (int64_t) (int32_t) c;
	result = (uint32_t) unsigned_sum;
	*cp = (uint64_t) result != unsigned_sum;
	*vp = (int64_t) (int32_t) result != signed_sum;

	return result;
}

/* SBC, A4.1.65, A4-125 */
static uint32_t
NAME_(sbcs)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	int c,
	int *cp,
	int *vp
)
{
	uint64_t unsigned_sum;
	int64_t signed_sum;
	uint32_t result;

	unsigned_sum = (uint64_t) op0 + ~(uint64_t) op1 + (uint64_t) c;
	signed_sum = (int64_t) (int32_t) op0 + ~(int64_t) (int32_t) op1 + (int64_t) (int32_t) c;
	result = (uint32_t) unsigned_sum;
	*cp = (uint64_t) result == unsigned_sum;
	*vp = (int64_t) (int32_t) result != signed_sum;

	return result;
}

/* MOV, A4.1.35, A4-68 */
static uint32_t
NAME_(movs)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	int *cp,
	int *vp
)
{
	uint32_t res;

	(void) op0;
	res = op1;
	*cp = cpssp->NAME.flag_shift_c;
	*vp = cpssp->NAME.flag_v;

	return res;
}

/* MVN, A4.1.41, A4-82 */
static uint32_t
NAME_(mvns)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	int *cp,
	int *vp
)
{
	uint32_t res;

	(void) op0;
	res = ~op1;
	*cp = cpssp->NAME.flag_shift_c;
	*vp = cpssp->NAME.flag_v;

	return res;
}

/* CLZ, A4.1.13, page A4-25 */
static uint32_t
NAME_(clz)(struct cpssp *cpssp, uint32_t op)
{
	uint32_t res;

	for (res = 0; res < 32; res++) {
		if ((op >> 31) & 1) {
			break;
		}
		op <<= 1;
	}

	return res;
}

#if CONFIG_VERSION <= 7
static uint32_t
NAME_(rbit)(struct cpssp *cpssp, uint32_t op)
{
	uint32_t res;
	unsigned int i;

	res = 0;
	for (i = 0; i < 32; i++) {
		res |= ((op >> i) & 1) << (31 - i);
	}
	return res;
}
#endif /* CONFIG_VERSION <= 7 */

#if CONFIG_VERSION <= 7
/*
 * BFC, ARMv7-M: A7-209
 * BFI, ARMv7-M: A7-210
 */
static uint32_t
NAME_(bfi)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	uint8_t msb,
	uint8_t lsb
)
{
	uint32_t mask;
	uint32_t res;

	mask = ((1 << (msb - lsb + 1)) - 1) << lsb;

	res = (op0 & ~mask) | ((op1 << lsb) & mask);

	return res;
}
#endif /* CONFIG_VERSION <= 7 */

/* UMULL, A4.1.129, A4-251 */
static uint64_t
NAME_(umulls)(
	struct cpssp *cpssp,
	uint32_t op0,
	uint32_t op1,
	int *cp,
	int *vp
)
{
	uint64_t res;

	res = (uint64_t) op0 * (uint64_t) op1;
	*cp = cpssp->NAME.flag_c;
	*vp = cpssp->NAME.flag_v;

	return res;
}

/* MLA, A4.1.34, A4-66 */
static uint32_t
NAME_(mlas)(
	struct cpssp *cpssp,
	uint32_t a,
	uint32_t b,
	uint32_t c,
	int *cp,
	int *vp
)
{
	uint64_t unsigned_sum;
	// int64_t signed_sum;
	uint32_t res;

	unsigned_sum = (uint64_t) a * (uint64_t) b
			+ (uint64_t) c;
	// signed_sum = (int64_t) (int32_t) a * (int64_t) (int32_t) b
	// 		+ (int64_t) (int32_t) c;
	res = (uint32_t) unsigned_sum;
	*cp = cpssp->NAME.flag_c;
	*vp = cpssp->NAME.flag_v;

	return res;
}

static int
NAME_(condition)(struct cpssp *cpssp, uint8_t cond)
{
	int res;

	assert(cond != 0b1111);

	switch ((cond >> 1) & 0x7) {
	case 0x0:
		/* beq/bne */
		res = cpssp->NAME.flag_z;
		break;
	case 0x1:
		/* bcs/bcc */
		res = cpssp->NAME.flag_c;
		break;
	case 0x2:
		/* bmi/bpl */
		res = cpssp->NAME.flag_n;
		break;
	case 0x3:
		/* bvs/bvc */
		res = cpssp->NAME.flag_v;
		break;
	case 0x4:
		/* bhi/bls */
		res = cpssp->NAME.flag_c & ! cpssp->NAME.flag_z;
		break;
	case 0x5:
		/* bge/blt */
		res = ! (cpssp->NAME.flag_n ^ cpssp->NAME.flag_v);
		break;
	case 0x6:
		/* bgt/ble */
		res = ! cpssp->NAME.flag_z
			& ! (cpssp->NAME.flag_n ^ cpssp->NAME.flag_v);
		break;
	case 0x7:
		res = 1;
		break;
	default: assert(0); /* Cannot happen. */
	}
	res ^= cond & 1;

	return res;
}

static int
NAME_(it_true)(struct cpssp *cpssp)
{
#if (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION
	if (cpssp->NAME.itstate == 0x00) {
		return 1;

	} else {
		int res;

		res = NAME_(condition)(cpssp, (cpssp->NAME.itstate >> 4) & 0xf);

		if ((cpssp->NAME.itstate & 0xf) == 0x8) {
			cpssp->NAME.itstate = 0x00;
		} else {
			cpssp->NAME.itstate = (cpssp->NAME.itstate & 0xe0)
					| ((cpssp->NAME.itstate & 0x0f) << 1);
		}

		return res;
	}
#else /* (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION */
	return 1;
#endif /* (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION */
}

static int
NAME_(it_inside)(struct cpssp *cpssp)
{
#if (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION
	return cpssp->NAME.itstate != 0x00;
#else /* (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION */
	return 0;
#endif /* (CONFIG_VERSION == 6 && CONFIG_THUMB2) || 7 <= CONFIG_VERSION */
}

static uint32_t
NAME_(shift)(
	struct cpssp *cpssp,
	unsigned int rm,
	unsigned int op,
	unsigned int shift
)
{
	uint32_t val;

	val = NAME_(load_reg)(cpssp, rm);

	switch (op) {
	case 0x0: /* LSL */
		if (shift == 0) {
			cpssp->NAME.flag_shift_c = cpssp->NAME.flag_c;
		} else {
			cpssp->NAME.flag_shift_c = (val >> (32 - shift)) & 1;
			val <<= shift;
		}
		break;
	case 0x1: /* LSR */
		if (shift == 0) {
			cpssp->NAME.flag_shift_c = 0;
			val = 0x00000000;
		} else {
			cpssp->NAME.flag_shift_c = (val >> (shift - 1)) & 1;
			val >>= shift;
		}
		break;
	case 0x2: /* ASR */
		if (shift == 0) {
			if ((val >> 31) & 1) {
				val = 0xffffffff;
				cpssp->NAME.flag_shift_c = 1;
			} else {
				val = 0x00000000;
				cpssp->NAME.flag_shift_c = 0;
			}
		} else {
			cpssp->NAME.flag_shift_c = (val >> (shift - 1)) & 1;
			val = (uint32_t) ((int32_t) val >> shift);
		}
		break;
	case 0x3:
		if (shift == 0) {
			/* RRX */
			cpssp->NAME.flag_shift_c = (val >> 0) & 1;
			val = (cpssp->NAME.flag_c << 31) | (val >> 1);
		} else {
			/* ROR */
			cpssp->NAME.flag_shift_c = (val >> (shift - 1)) & 1;
			val = (val << (32 - shift)) | (val >> shift);
		}
		break;
	default:
		assert(0);
	}

	return val;
}

#if CONFIG_ARM
#include "arch_arm_pipeline.c"
#else
#include "arch_arm_mpipeline.c"
#endif /* CONFIG_ARM */

static void
NAME_(irq_set)(struct cpssp *cpssp, unsigned int val)
{
	if (DEBUG_CONTROL_FLOW) {
		fprintf(stderr, "%s val=%u\n", __FUNCTION__, val);
	}

	cpssp->NAME.exception_pending = val;
	cpssp->NAME.wfi &= ! cpssp->NAME.exception_pending;
}

static void
NAME_(clk)(struct cpssp *cpssp)
{
	NAME_(insn_step)(cpssp);
}

static void
NAME_(reset)(struct cpssp *cpssp)
{
	int i;

	if (DEBUG_CONTROL_FLOW
	 && loglevel) {
		fprintf(stderr, "Reset\n");
	}

	cpssp->NAME.wfi = 0;
	cpssp->NAME.exception_pending = 0;

	for (i = 0; i < 13; i++) {
		NAME_(store_reg)(cpssp, i, 0);
	}
	NAME_(store_lr)(cpssp, 0);
	cpssp->NAME.sp_main = 0;
	cpssp->NAME.sp_process = 0;

	cpssp->NAME.current_mode = MODE_THREAD;
	cpssp->NAME.control_spsel = 0;
	cpssp->NAME.control_npriv = 0;

	cpssp->NAME.flag_shift_c = 0;
	cpssp->NAME.flag_c = 0;
	cpssp->NAME.flag_n = 0;
	cpssp->NAME.flag_v = 0;
	cpssp->NAME.flag_z = 0;

#if 7 <= CONFIG_VERSION
	cpssp->NAME.itstate = 0x00;
#endif

	cpssp->NAME.vtor = 0x00000000;
	cpssp->NAME.prigroup = 0b000;

	cpssp->NAME.sevonpend = 0;
	cpssp->NAME.sleepdeep = 0;
	cpssp->NAME.sleeponexit = 0;

#if CONFIG_ARM
	/* HACK! */
	/* Read sp. */
	NAME_(store_sp)(cpssp, 0);
	/* Read pc. */
	cpssp->NAME.flag_t = 0;
	NAME_(store_pc)(cpssp, 0xfffd0000); /* Sets flush. */
#else
	/* Read sp. */
	NAME_(store_sp)(cpssp, NAME_(ld32)(cpssp, 0));
	/* Read pc. */
	cpssp->NAME.flag_t = 1;
	NAME_(store_pc)(cpssp, NAME_(ld32)(cpssp, 4) & ~1); /* Sets flush. */
#endif
	cpssp->NAME.stall = 0;
	cpssp->NAME.insn_prev = 0;
}

static int
NAME_(gdb_reg_read)(void *_cpssp, unsigned long n, void *_buf, int len)
{
	struct cpssp *cpssp = _cpssp;
	uint32_t *buf = _buf;

	switch (n) {
	case 0 ... 12:
		assert(sizeof(*buf) <= len);
		*buf = NAME_(load_reg)(cpssp, n);
		return sizeof(*buf);
	case 13:
		assert(sizeof(*buf) <= len);
		*buf = NAME_(load_sp)(cpssp);
		return sizeof(*buf);
	case 14:
		assert(sizeof(*buf) <= len);
		*buf = NAME_(load_lr)(cpssp);
		return sizeof(*buf);
	case 15:
		assert(sizeof(*buf) <= len);
		*buf = cpssp->NAME.pc_next;
		return sizeof(*buf);
	case 16 ... 23:
		assert(3 * sizeof(*buf) <= len);
		*buf++ = 0;
		*buf++ = 0;
		*buf++ = 0;
		return 3 * sizeof(*buf);
	case 24:
		assert(sizeof(*buf) <= len);
		*buf = 0;
		return sizeof(*buf);
	case 25:
		assert(sizeof(*buf) <= len);
		*buf = NAME_(load_xpsr)(cpssp);
		return sizeof(*buf);
	default:
		return -1;
	}
}

static int
NAME_(gdb_mem_read)(void *_cpssp, unsigned long addr, void *_buf, int len)
{
	struct cpssp *cpssp = _cpssp;
	uint8_t *buf = _buf;
	int i;

	for (i = 0; i < len; i++) {
		*buf++ = NAME_(ld8)(cpssp, (uint32_t) addr++);
	}

	return len;
}

static int
NAME_(gdb_mem_write)(void *_cpssp, unsigned long addr, void *_buf, int len)
{
	struct cpssp *cpssp = _cpssp;
	uint8_t *buf = _buf;
	int i;

	for (i = 0; i < len; i++) {
		NAME_(st8)(cpssp, (uint32_t) addr++, *buf++);
	}

	return len;
}

static int
NAME_(gdb_brk_set)(void *_cpssp, int type, unsigned long addr, int len)
{
	struct cpssp *cpssp = _cpssp;
	unsigned int i;

	assert(type == 0); /* FIXME */

	for (i = 0; ; i++) {
		if (i == 4) {
			return -1;
		}
		if (cpssp->NAME.brkstart[i] == 0
		 && cpssp->NAME.brkend[i] == 0) {
			cpssp->NAME.brkstart[i] = addr;
			cpssp->NAME.brkend[i] = addr + len;
			return 0;
		}
	}
}

static int
NAME_(gdb_brk_clr)(void *_cpssp, int type, unsigned long addr, int len)
{
	struct cpssp *cpssp = _cpssp;
	unsigned int i;

	assert(type == 0); /* FIXME */

	for (i = 0; ; i++) {
		if (i == 4) {
			return -1;
		}
		if (cpssp->NAME.brkstart[i] == addr
		 && cpssp->NAME.brkend[i] == addr + len) {
			cpssp->NAME.brkstart[i] = 0;
			cpssp->NAME.brkend[i] = 0;
			return 0;
		}
	}
}

static void
NAME_(create)(const char *name, struct cpssp *cpssp)
{
	static struct gdb_funcs NAME_(funcs) = {
		.reg_read = NAME_(gdb_reg_read),
		.mem_read = NAME_(gdb_mem_read),
		.mem_write = NAME_(gdb_mem_write),
		.brk_set = NAME_(gdb_brk_set),
		.brk_clr = NAME_(gdb_brk_clr),
	};

	cpssp->NAME.gdb = gdb_create(name, cpssp, &NAME_(funcs));

	cpssp->NAME.brkstart[0] = 0; cpssp->NAME.brkend[0] = 0;
	cpssp->NAME.brkstart[1] = 0; cpssp->NAME.brkend[1] = 0;
	cpssp->NAME.brkstart[2] = 0; cpssp->NAME.brkend[2] = 0;
	cpssp->NAME.brkstart[3] = 0; cpssp->NAME.brkend[3] = 0;
}

static void
NAME_(destroy)(struct cpssp *cpssp)
{
	gdb_destroy(cpssp->NAME.gdb);
}

#endif /* BEHAVIOR */

#undef DEBUG_CONTROL_FLOW
#undef DEBUG_CONTROL_FLOW_REGS
