/*
 * Copyright (C) 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.
 */

#define DEBUG_CONTROL_FLOW	0

/*
 * Instruction Set Summary
 *
 * 0000 0000 0000 0000	nop (85)

 * 0000 0001 dddd rrrr	movw (80)

 * 0000 0010 dddd rrrr	muls (82)
 * 0000 0011 0ddd 1rrr	fmul (59)
 * 0000 0011 0ddd 0rrr	mulsu (83)
 * 0000 0011 1ddd 0rrr	fmuls (60)
 * 0000 0011 1ddd 1rrr	fmulsu (61)

 * 0000 01rd dddd rrrr	cpc (50)
 * 0000 10rd dddd rrrr	sbc (97)
 * 0000 11rd dddd rrrr	add (6) (lsl)
 * 0001 00rd dddd rrrr	cpse (52)
 * 0001 01rd dddd rrrr	cp (49)
 * 0001 10rd dddd rrrr	sub (123)
 * 0001 11rd dddd rrrr	adc (5) (rol)
 * 0010 00rd dddd rrrr	and (8) (tst)
 * 0010 01rd dddd rrrr	eor (58) (clr)
 * 0010 10rd dddd rrrr	or (86)
 * 0010 11rd dddd rrrr	mov (79)

 * 1001 0110 KKdd KKKK	adiw (7)
 * 1001 0111 KKdd KKKK	sbiw (102)

 * 0011 KKKK dddd KKKK	cpi (51)
 * 0100 KKKK dddd KKKK	sbci (98)
 * 0101 KKKK dddd KKKK	subi (134)
 * 0110 KKKK dddd KKKK	ori (87) (sbr)
 * 0111 KKKK dddd KKKK	andi (9) (cbr)
 * 1110 KKKK dddd KKKK	ldi Rd,K (73) (ser)

 * 1001 000d dddd 0000 kkkk kkkk kkkk kkkk	lds Rd,k (74)
 * 1001 001d dddd 0000 kkkk kkkk kkkk kkkk	sts k,Rd (121)

 * 1001 010k kkkk 110k kkkk kkkk kkkk kkkk	jmp k (66)
 * 1001 010k kkkk 111k kkkk kkkk kkkk kkkk	call k (36)

 * 1001 000d dddd 0001	ld Rd,Z+ (72)
 * 1001 000d dddd 0010	ld Rd,-Z (72)
 * 1001 000d dddd 0100	lpm Rd,Z (76)
 * 1001 000d dddd 0101	lpm Rd,Z+ (76)
 * 1001 000d dddd 0110	elpm Rd,RAMPZ:Z (57)
 * 1001 000d dddd 0111	elpm Rd,RAMPZ:Z+ (57)
 * 1001 000d dddd 1001	ld Rd,Y+ (71)
 * 1001 000d dddd 1010	ld Rd,-Y (71)
 * 1001 000d dddd 1100	ld X (70)
 * 1001 000d dddd 1101	ld X+ (70)
 * 1001 000d dddd 1110	ld -X (70)

 * 1001 001r rrrr 0001	st Z+ (120)
 * 1001 001r rrrr 0010	st -Z (120)
 * 1001 001r rrrr 1001	st Y+ (119)
 * 1001 001r rrrr 1010	st -Y (119)
 * 1001 001r rrrr 1100	st X (118)
 * 1001 001r rrrr 1101	st X+ (118)
 * 1001 001r rrrr 1110	st -X (118)

 * 1001 001r rrrr 0100	xch (128)
 * 1001 001r rrrr 0101	las (68)
 * 1001 001r rrrr 0110	lac (67)
 * 1001 001r rrrr 0111	lat (69)

 * 1001 000d dddd 1111	pop (89)
 * 1001 001d dddd 1111	push (90)

 * 1001 010d dddd 0000	com (48)
 * 1001 010d dddd 0001	neg (84)
 * 1001 010d dddd 0010	swap (125)
 * 1001 010d dddd 0011	inc (65)
 * 1001 010d dddd 0101	asr (10
 * 1001 010d dddd 0110	lsr (78)
 * 1001 010d dddd 0111	ror (96)
 * 1001 010d dddd 1010	dec (53)

 * 1001 0101 1010 1000  wdr (127)

 * 1001 0100 0bbb 1000	bset (sec, seh, sei, sen, ses, set, sev, sez)
 * 1001 0100 1bbb 1000	bclr (11) (clc, clh, cli, cln, cls, clt, clv, clz)

 * 1001 1000 AAAA Abbb	cbi (37)
 * 1001 1001 AAAA Abbb	sbic (100)
 * 1001 1010 AAAA Abbb	sbi (99)
 * 1001 1011 AAAA Abbb	sbis (101)

 * 1001 11rd dddd rrrr	mul (81)

 * 1011 0AAd dddd AAAA	in (64)
 * 1011 1AAr rrrr AAAA	out (88)

 * 10q0 qq0d dddd 0qqq	ldd Rd,Z+q (72)
 * 10q0 qq0d dddd 1qqq	ldd Rd,Y+q (71)
 * 10q0 qq1r rrrr 0qqq	std Z+q (120)
 * 10q0 qq1r rrrr 1qqq	std Y+q (119)

 * 1100 kkkk kkkk kkkk	rjmp (94)
 * 1101 kkkk kkkk kkkk	rcall (91)

 * 1001 0100 0000 1001	ijmp (63)
 * 1001 0100 0001 1001	eijmp (56)
 * 1001 0100 KKKK 1011	des (54)

 * 1001 0101 0000 1001	icall (62)
 * 1001 0101 0000 1000	ret (92)
 * 1001 0101 0001 1000	reti (93)
 * 1001 0101 0001 1001	eicall (55)
 * 1001 0101 1000 1000	sleep (115)
 * 1001 0101 1001 1000	break (17)
 * 1001 0101 1010 1000	wdr (127)
 * 1001 0101 1100 1000	lpm R0,Z (76)
 * 1001 0101 1101 1000	elpm R0,RAMPZ:Z (57)
 * 1001 0101 1110 1000	spm Z+ (116)
 * 1001 0101 1110 1000	spm (117)
 * 1001 0101 1111 1000	spm Z+ (117)

 * 1010 0kkk dddd kkkk	lds Rd,k (75)
 * 1010 1kkk dddd kkkk	sts k,Rd (122)

 * 1111 100d dddd 0bbb	bld (12)
 * 1111 101d dddd 0bbb	bst (35)
 * 1111 110r rrrr 0bbb	sbrc (104)
 * 1111 111r rrrr 0bbb	sbrs (105)

 * 1111 00kk kkkk kbbb	brbs (14) (brcs, breq, brhs, ...)
 * 1111 01kk kkkk kbbb	brbc (13) (brcc, brge, brhc, ...)
 */

#ifdef INCLUDE
#endif /* INCLUDE */
#ifdef STATE

/* Page 11 */
#define NAME_REG_X	0x1a
#define NAME_REG_Y	0x1c
#define NAME_REG_Z	0x1e

struct {
	uint8_t skip;
	uint8_t hlt;
	uint16_t pc;
	uint16_t sp;

	uint8_t reg[32];

	/* SREG */
	uint8_t idelay;
	uint8_t i;
	uint8_t t;
	uint8_t h;
	uint8_t s;
	uint8_t v;
	uint8_t n;
	uint8_t z;
	uint8_t c;

	uint64_t irr;
} NAME;

#endif /* STATE */
#ifdef BEHAVIOR

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

	fprintf(stderr, "pc=%04x sp=%03x skip=%d hlt=%d\n",
			cpssp->NAME.pc * 2, cpssp->NAME.sp,
			cpssp->NAME.skip, cpssp->NAME.hlt);

	for (i = 0; i < 32; i++) {
		if (i % 16 != 0) {
			fprintf(stderr, " ");
		}
		fprintf(stderr, "%02x", cpssp->NAME.reg[i]);
		if (i % 16 == 15) {
			fprintf(stderr, "\n");
		}
	}

	fprintf(stderr, "i=%d", cpssp->NAME.i);
	fprintf(stderr, " ");
	fprintf(stderr, "t=%d", cpssp->NAME.t);
	fprintf(stderr, " ");
	fprintf(stderr, "h=%d", cpssp->NAME.h);
	fprintf(stderr, " ");
	fprintf(stderr, "s=%d", cpssp->NAME.s);
	fprintf(stderr, " ");
	fprintf(stderr, "v=%d", cpssp->NAME.v);
	fprintf(stderr, " ");
	fprintf(stderr, "n=%d", cpssp->NAME.n);
	fprintf(stderr, " ");
	fprintf(stderr, "z=%d", cpssp->NAME.z);
	fprintf(stderr, " ");
	fprintf(stderr, "c=%d", cpssp->NAME.c);
	fprintf(stderr, "\n");
}

static void
NAME_(reg8_set_res)(struct cpssp *cpssp, uint16_t r, uint16_t res)
{
	cpssp->NAME.reg[r] = res;
}

static uint16_t
NAME_(reg8_get_op1)(struct cpssp *cpssp, uint16_t r)
{
	return cpssp->NAME.reg[r];
}

static uint16_t
NAME_(reg8_get_op2)(struct cpssp *cpssp, unsigned int r)
{
	return cpssp->NAME.reg[r];
}

static void
NAME_(reg16_set_addr)(struct cpssp *cpssp, unsigned int r, uint16_t addr)
{
	assert(/* 0 <= r && */ r < 31);

	cpssp->NAME.reg[r + 1] = (addr >> 8) & 0xff;
	cpssp->NAME.reg[r + 0] = (addr >> 0) & 0xff;
}

static void
NAME_(reg16_set_res)(struct cpssp *cpssp, unsigned int r, uint16_t res)
{
	assert(/* 0 <= r && */ r < 31);

	cpssp->NAME.reg[r + 1] = (res >> 8) & 0xff;
	cpssp->NAME.reg[r + 0] = (res >> 0) & 0xff;
}

static uint16_t
NAME_(reg16_get_addr)(struct cpssp *cpssp, unsigned int r)
{
	uint16_t addr;

	assert(/* 0 <= r && */ r < 31);

	addr = (cpssp->NAME.reg[r + 1] << 8) | cpssp->NAME.reg[r + 0];

	return addr;
}

static uint16_t
NAME_(reg16_get_op1)(struct cpssp *cpssp, unsigned int r)
{
	return (cpssp->NAME.reg[r + 1] << 8) | cpssp->NAME.reg[r + 0];
}

static void
NAME_(sreg_set_res)(struct cpssp *cpssp, uint16_t res)
{
	cpssp->NAME.i = (res >> 7) & 1;
	cpssp->NAME.t = (res >> 6) & 1;
	cpssp->NAME.h = (res >> 5) & 1;
	cpssp->NAME.s = (res >> 4) & 1;
	cpssp->NAME.v = (res >> 3) & 1;
	cpssp->NAME.n = (res >> 2) & 1;
	cpssp->NAME.z = (res >> 1) & 1;
	cpssp->NAME.c = (res >> 0) & 1;
}

static uint8_t
NAME_(sreg_get_op1)(struct cpssp *cpssp)
{
	return (cpssp->NAME.i << 7)
		| (cpssp->NAME.t << 6)
		| (cpssp->NAME.h << 5)
		| (cpssp->NAME.s << 4)
		| (cpssp->NAME.v << 3)
		| (cpssp->NAME.n << 2)
		| (cpssp->NAME.z << 1)
		| (cpssp->NAME.c << 0);
}

static void
NAME_(sph_set_res)(struct cpssp *cpssp, uint16_t res)
{
	cpssp->NAME.sp = (cpssp->NAME.sp & ~0xff00) | (res << 8);
}

static uint8_t
NAME_(sph_get_op1)(struct cpssp *cpssp)
{
	return (cpssp->NAME.sp >> 8) & 0xff;
}

static void
NAME_(spl_set_res)(struct cpssp *cpssp, uint16_t res)
{
	cpssp->NAME.sp = (cpssp->NAME.sp & ~0x00ff) | (res << 0);
}

static uint8_t
NAME_(spl_get_op1)(struct cpssp *cpssp)
{
	return (cpssp->NAME.sp >> 0) & 0xff;
}

static uint16_t
NAME_(lpm_op1)(struct cpssp *cpssp, uint16_t addr)
{
	uint16_t tmp;

	NAME_(fetch)(cpssp, addr >> 1, &tmp);

	return tmp >> ((addr & 1) * 8);
}

/*
 * See table at page 327.
 */
static void
NAME_(out_res)(struct cpssp *cpssp, uint16_t addr, uint16_t res)
{
	switch (addr) {
	case 0x5f: /* SREG */
		NAME_(sreg_set_res)(cpssp, res);
		break;
	case 0x5e: /* SPH */
		NAME_(sph_set_res)(cpssp, res);
		break;
	case 0x5d: /* SPL */
		NAME_(spl_set_res)(cpssp, res);
		break;
	default:
		NAME_(io_st)(cpssp, addr, res);
		break;
	}
}

/*
 * See table at page 327.
 */
static uint16_t
NAME_(in_op1)(struct cpssp *cpssp, uint16_t addr)
{
	uint8_t op1;

	switch (addr) {
	case 0x5f:
		op1 = NAME_(sreg_get_op1)(cpssp);
		break;
	case 0x5e:
		op1 = NAME_(sph_get_op1)(cpssp);
		break;
	case 0x5d:
		op1 = NAME_(spl_get_op1)(cpssp);
		break;
	default:
		NAME_(io_ld)(cpssp, addr, &op1);
		break;
	}

	return (uint16_t) op1;
}

static void
NAME_(st_res)(struct cpssp *cpssp, uint16_t addr, uint16_t res)
{
	if (addr < 0x20) {
		NAME_(reg8_set_res)(cpssp, addr, res);
	} else if (addr < 0x60) {
		NAME_(out_res)(cpssp, addr, res);
	} else {
		NAME_(ram_st)(cpssp, addr, res);
	}
}

static uint16_t
NAME_(ld_op1)(struct cpssp *cpssp, uint16_t addr)
{
	uint16_t op1;

	if (addr < 0x20) {
		op1 = NAME_(reg8_get_op1)(cpssp, addr);
	} else if (addr < 0x60) {
		op1 = NAME_(in_op1)(cpssp, addr);
	} else {
		NAME_(ram_ld)(cpssp, addr, &op1);
	}

	return op1;
}

static void
NAME_(push_res)(struct cpssp *cpssp, uint16_t res)
{
	uint16_t addr;

	addr = cpssp->NAME.sp;
	cpssp->NAME.sp = (cpssp->NAME.sp - 1) & 0xfff;

	NAME_(st_res)(cpssp, addr, res);
}

static uint16_t
NAME_(pop_op1)(struct cpssp *cpssp)
{
	uint16_t addr;
	uint16_t op1;

	cpssp->NAME.sp = (cpssp->NAME.sp + 1) & 0x0fff;
	addr = cpssp->NAME.sp;

	op1 = NAME_(ld_op1)(cpssp, addr);

	return op1;
}

static void
NAME_(push_pc)(struct cpssp *cpssp)
{
	uint16_t res;

	res = (cpssp->NAME.pc >> 8) & 0xff;
	NAME_(push_res)(cpssp, res);

	res = (cpssp->NAME.pc >> 0) & 0xff;
	NAME_(push_res)(cpssp, res);
}

static void
NAME_(pop_pc)(struct cpssp *cpssp)
{
	uint16_t op1;

	op1 = NAME_(pop_op1)(cpssp);
	cpssp->NAME.pc &= ~0x00ff;
	cpssp->NAME.pc |= op1 << 0;

	op1 = NAME_(pop_op1)(cpssp);
	cpssp->NAME.pc &= ~0xff00;
	cpssp->NAME.pc |= op1 << 8;
}

static uint16_t
NAME_(com)(struct cpssp *cpssp, uint16_t op1)
{
	uint16_t res;

	res = 0xff - op1;

	cpssp->NAME.c = 1;
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = 0;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;

	return res;
}

static uint16_t
NAME_(neg)(struct cpssp *cpssp, uint16_t op1)
{
	uint16_t res;

	res = 0 - op1;

	cpssp->NAME.c = res != 0x00;
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = res == 0x80;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	/* cpssp->NAME.h = ... FIXME */

	return res;
}

static uint16_t
NAME_(swap)(struct cpssp *cpssp, uint16_t op1)
{
	uint16_t res;

	res = (op1 << 4) | (op1 >> 4);

	/* cpssp->NAME.c unchanged */
	/* cpssp->NAME.z unchanged */
	/* cpssp->NAME.n unchanged */
	/* cpssp->NAME.v unchanged */
	/* cpssp->NAME.s unchanged */
	/* cpssp->NAME.h unchanged */

	return res;
}

static uint16_t
NAME_(inc)(struct cpssp *cpssp, uint16_t op1)
{
	uint16_t res;

	res = op1 + 1;

	/* cpssp->NAME.c unchanged */
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = res == 0x80;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	/* cpssp->NAME.h unchanged */

	return res;
}

static uint16_t
NAME_(asr)(struct cpssp *cpssp, uint16_t op1)
{
	uint16_t res;

	res = (int8_t) op1 >> 1;

	cpssp->NAME.c = op1 & 1;
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = cpssp->NAME.n ^ cpssp->NAME.c;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;

	return res;
}

static uint16_t
NAME_(lsr)(struct cpssp *cpssp, uint16_t op1)
{
	uint16_t res;

	res = op1 >> 1;

	cpssp->NAME.c = op1 & 1;
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = 0;
	cpssp->NAME.v = cpssp->NAME.n ^ cpssp->NAME.c;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;

	return res;
}

static uint16_t
NAME_(ror)(struct cpssp *cpssp, uint16_t op1)
{
	uint16_t res;

	res = (cpssp->NAME.c << 7) | (op1 >> 1);

	cpssp->NAME.c = op1 & 1;
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = 0;
	cpssp->NAME.v = cpssp->NAME.n ^ cpssp->NAME.c;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	/* cpssp->NAME.h unchanged */

	return res;
}

static uint16_t
NAME_(dec)(struct cpssp *cpssp, uint16_t op1)
{
	uint16_t res;

	res = op1 - 1;

	/* cpssp->NAME.c unchanged */
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = res == 0x7f;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	/* cpssp->NAME.h unchanged */

	return res;
}

static uint16_t
NAME_(add)(struct cpssp *cpssp, uint16_t op1, uint16_t op2)
{
	uint16_t unsigned_res;
	int16_t signed_res;
	uint16_t res;

	unsigned_res = (uint16_t) (uint8_t) op1 + (uint16_t) (uint8_t) op2;
	signed_res = (int16_t) (int8_t) op1 + (int16_t) (int8_t) op2;

	res = op1 + op2;

	cpssp->NAME.c = unsigned_res != (uint16_t) (uint8_t) res;
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = signed_res != (int16_t) (int8_t) res;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	cpssp->NAME.h = ((res ^ op1 ^ op2) >> 4) & 1;

	return res;
}

static uint16_t
NAME_(addw)(struct cpssp *cpssp, uint16_t op1, uint16_t op2)
{
	uint32_t unsigned_res;
	int32_t signed_res;
	uint16_t res;

	unsigned_res = (uint32_t) (uint16_t) op1 + (uint32_t) (uint16_t) op2;
	signed_res = (int32_t) (int16_t) op1 + (int32_t) (int16_t) op2;

	res = op1 + op2;

	cpssp->NAME.c = unsigned_res != (uint32_t) (uint16_t) res;
	cpssp->NAME.z = res == 0x0000;
	cpssp->NAME.n = (res >> 15) & 1;
	cpssp->NAME.v = signed_res != (int32_t) (int16_t) res;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	/* cpssp->NAME.h unchanged */

	return res;
}

static uint16_t
NAME_(adc)(struct cpssp *cpssp, uint16_t op1, uint16_t op2)
{
	uint16_t unsigned_res;
	int16_t signed_res;
	uint16_t res;

	unsigned_res = (uint16_t) (uint8_t) op1
			+ (uint16_t) (uint8_t) op2
			+ (uint16_t) cpssp->NAME.c;
	signed_res = (int16_t) (int8_t) op1
			+ (int16_t) (int8_t) op2
			+ (int16_t) cpssp->NAME.c;

	res = op1 + op2 + cpssp->NAME.c;

	cpssp->NAME.c = unsigned_res != (uint16_t) (uint8_t) res;
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = signed_res != (int16_t) (int8_t) res;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	cpssp->NAME.h = ((res ^ op1 ^ op2) >> 4) & 1; /* Correct? FIXME */

	return res;
}

static uint16_t
NAME_(and)(struct cpssp *cpssp, uint16_t op1, uint16_t op2)
{
	uint16_t res;

	res = op1 & op2;

	/* cpssp->NAME.c unchanged */
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = 0;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	/* cpssp->NAME.h unchanged */

	return res;
}

static uint16_t
NAME_(eor)(struct cpssp *cpssp, uint16_t op1, uint16_t op2)
{
	uint16_t res;

	res = op1 ^ op2;

	/* cpssp->NAME.c unchanged */
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = 0;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	/* cpssp->NAME.h unchanged */

	return res;
}

static uint16_t
NAME_(or)(struct cpssp *cpssp, uint16_t op1, uint16_t op2)
{
	uint16_t res;

	res = op1 | op2;

	/* cpssp->NAME.c unchanged */
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = 0;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	/* cpssp->NAME.h unchanged */

	return res;
}

static uint16_t
NAME_(mul)(struct cpssp *cpssp, uint16_t op1, uint16_t op2)
{
	uint16_t res;

	res = (uint16_t) op1 * (uint16_t) op2;

	cpssp->NAME.c = (res >> 15) & 1;
	cpssp->NAME.z = res == 0x0000;
	/* cpssp->NAME.n unchanged */
	/* cpssp->NAME.v unchanged */
	/* cpssp->NAME.s unchanged */
	/* cpssp->NAME.h unchanged */

	return res;
}

static uint16_t
NAME_(sub)(struct cpssp *cpssp, uint16_t op1, uint16_t op2)
{
	uint16_t unsigned_res;
	int16_t signed_res;
	uint16_t res;

	unsigned_res = (uint16_t) (uint8_t) op1
			- (uint16_t) (uint8_t) op2;
	signed_res = (int16_t) (int8_t) op1
			- (int16_t) (int8_t) op2;

	res = op1 - op2;

	cpssp->NAME.c = unsigned_res != (uint16_t) (uint8_t) res;
	cpssp->NAME.z = res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = signed_res != (int16_t) (int8_t) res;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	cpssp->NAME.h = ((res ^ op1 ^ op2) >> 4) & 1;

	return res;
}

static uint16_t
NAME_(subw)(struct cpssp *cpssp, uint16_t op1, uint16_t op2)
{
	uint32_t unsigned_res;
	int32_t signed_res;
	uint16_t res;

	unsigned_res = (uint32_t) (uint16_t) op1 - (uint32_t) (uint16_t) op2;
	signed_res = (int32_t) (int16_t) op1 - (int32_t) (int16_t) op2;

	res = op1 - op2;

	cpssp->NAME.c = unsigned_res != (uint32_t) (uint16_t) res;
	cpssp->NAME.z = res == 0x0000;
	cpssp->NAME.n = (res >> 15) & 1;
	cpssp->NAME.v = signed_res != (int32_t) (int16_t) res;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	/* cpssp->NAME.h unchanged */

	return res;
}

static uint16_t
NAME_(sbc)(struct cpssp *cpssp, uint16_t op1, uint16_t op2)
{
	uint16_t unsigned_res;
	int16_t signed_res;
	uint16_t res;

	unsigned_res = (uint16_t) (uint8_t) op1
			- (uint16_t) (uint8_t) op2
			- (uint16_t) cpssp->NAME.c;
	signed_res = (int16_t) (int8_t) op1
			- (int16_t) (int8_t) op2
			- (int16_t) cpssp->NAME.c;

	res = op1 - op2 - cpssp->NAME.c;

	cpssp->NAME.c = unsigned_res != (uint16_t) (uint8_t) res;
	cpssp->NAME.z &= res == 0x00;
	cpssp->NAME.n = (res >> 7) & 1;
	cpssp->NAME.v = signed_res != (int16_t) (int8_t) res;
	cpssp->NAME.s = cpssp->NAME.n ^ cpssp->NAME.v;
	cpssp->NAME.h = ((res ^ op1 ^ op2) >> 4) & 1;

	return res;
}

static void
NAME_(interrupt)(struct cpssp *cpssp, unsigned int irq)
{
	cpssp->NAME.hlt = 0;

	cpssp->NAME.i = 0;

	NAME_(push_pc)(cpssp);

	cpssp->NAME.pc = irq << 1;
}

static void
NAME_(exec)(struct cpssp *cpssp)
{
	uint16_t addr;
	uint16_t op1;
	uint16_t op2;
	uint16_t res;
	uint16_t inst;
	uint16_t inst2;
	uint8_t b;
	uint8_t d;
	uint8_t q;
	uint8_t r;
	uint8_t A;
	uint8_t K;
	uint32_t k;

	if (DEBUG_CONTROL_FLOW) {
		fprintf(stderr, "\n");
		fprintf(stderr, "%s at 0x%04x i=%d t=%d h=%d s=%d v=%d n=%d z=%d c=%d\n",
				cpssp->NAME.skip ? "Skipping" : "Executing",
				cpssp->NAME.pc << 1,
				cpssp->NAME.i, cpssp->NAME.t,
				cpssp->NAME.h, cpssp->NAME.s,
				cpssp->NAME.v, cpssp->NAME.n,
				cpssp->NAME.z, cpssp->NAME.c);
		for (d = 0; d < 32; d++) {
			if (d == 0 || d == 16) {
				fprintf(stderr, "\t");
			} else if (d == 8 || d == 24) {
				fprintf(stderr, "  ");
			} else {
				fprintf(stderr, " ");
			}
			fprintf(stderr, "%02x", cpssp->NAME.reg[d]);
			if (d == 15 || d == 31) {
				fprintf(stderr, "\n");
			}
		}
	}

	NAME_(fetch)(cpssp, cpssp->NAME.pc, &inst);
	cpssp->NAME.pc++;
	if ((inst & 0xfe0e) == 0x940c		/* jmp (66) */
	 || (inst & 0xfe0e) == 0x940e		/* call (36) */
	 || (inst & 0xfe0f) == 0x9000		/* lds (74) */
	 || (inst & 0xfe0f) == 0x9200) {	/* sts (121) */
		NAME_(fetch)(cpssp, cpssp->NAME.pc, &inst2);
		cpssp->NAME.pc++;
	} else {
		inst2 = 0x0000; /* Not used. */
	}

	cpssp->NAME.idelay = 0;

	if (! cpssp->NAME.skip) goto label_else;

	cpssp->NAME.skip = 0;
	goto label_endif;

label_else:;
	if ((inst & 0xff8f) == 0x9408) {		/* bset (34) */
							/* sec (106) */
							/* seh (107) */
							/* sei (108) */
							/* sen (109) */
							/* ses (111) */
							/* set (112) */
							/* sev (113) */
							/* sez (114) */
		b = (inst >> 4) & 0x7;

		switch (b) {
		case 0:
			cpssp->NAME.c = 1;
			break;
		case 1:
			cpssp->NAME.z = 1;
			break;
		case 2:
			cpssp->NAME.n = 1;
			break;
		case 3:
			cpssp->NAME.v = 1;
			break;
		case 4:
			cpssp->NAME.s = 1;
			break;
		case 5:
			cpssp->NAME.h = 1;
			break;
		case 6:
			cpssp->NAME.t = 1;
			break;
		case 7:
			cpssp->NAME.i = 1;
			cpssp->NAME.idelay = 1;
			break;
		default:
			assert(0); /* Cannot happen. */
		}

	} else if ((inst & 0xff8f) == 0x9488) {		/* bclr (11) */
		b = (inst >> 4) & 0x7;

		switch (b) {
		case 0:
			cpssp->NAME.c = 0;
			break;
		case 1:
			cpssp->NAME.z = 0;
			break;
		case 2:
			cpssp->NAME.n = 0;
			break;
		case 3:
			cpssp->NAME.v = 0;
			break;
		case 4:
			cpssp->NAME.s = 0;
			break;
		case 5:
			cpssp->NAME.h = 0;
			break;
		case 6:
			cpssp->NAME.t = 0;
			break;
		case 7:
			cpssp->NAME.i = 0;
			break;
		default:
			assert(0); /* Cannot happen. */
		}

	} else if ((inst & 0xffff) == 0x9508		/* ret (92) */
		|| (inst & 0xffff) == 0x9518) {		/* reti (93) */
		NAME_(pop_pc)(cpssp);

		if ((inst >> 4) & 1) {
			cpssp->NAME.i = 1;
		}

	} else if ((inst & 0xffff) == 0x9409) {		/* ijmp (63) */
		addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Z);
		cpssp->NAME.pc = addr;

	} else if ((inst & 0xffff) == 0x9419) {		/* eijmp (56) */
		assert(0); /* FIXME */

	} else if ((inst & 0xffff) == 0x9509) {		/* icall (62) */
		NAME_(push_pc)(cpssp);

		addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Z);
		cpssp->NAME.pc = addr;

	} else if ((inst & 0xffff) == 0x9519) {		/* eicall (55) */
		assert(0); /* FIXME */

	} else if ((inst & 0xffff) == 0x9598) {		/* break (17) */
		assert(0); /* FIXME */

	} else if ((inst & 0xffff) == 0x9588) {		/* sleep (115) */
		cpssp->NAME.hlt = 1;

	} else if ((inst & 0xfe0e) == 0x940c		/* jmp (66) */
		|| (inst & 0xfe0e) == 0x940e) {		/* call (36) */
		k = ((inst >> 4) & 0x1f) << 17;
		k |= ((inst >> 0) & 0x1) << 16;
		k |= inst2;

		switch (inst & 0xfe0e) {
		case 0x940c:
			/* jmp */
			break;
		case 0x940e:
			/* call */
			NAME_(push_pc)(cpssp);
			break;
		default:
			assert(0);
		}

		cpssp->NAME.pc = k;

	} else if ((inst & 0xf000) == 0xc000		/* rjmp (94) */
		|| (inst & 0xf000) == 0xd000) {		/* rcall (91) */
		k = inst & 0xfff;
		k = (int32_t) k << (32-12);
		k = (int32_t) k >> (32-12);

		switch (inst & 0xf000) {
		case 0xc000:
			/* rjmp */
			break;
		case 0xd000:
			/* rcall */
			NAME_(push_pc)(cpssp);
			break;
		default:
			assert(0);
		}

		cpssp->NAME.pc = cpssp->NAME.pc + k;

	} else if ((inst & 0xff00) == 0x0100) {		/* movw (80) */
		d = ((inst >> 4) & 0xf) << 1;
		r = (inst & 0xf) << 1;

		op1 = NAME_(reg16_get_op1)(cpssp, r);
		res = op1;
		NAME_(reg16_set_res)(cpssp, d, res);

	} else if ((inst & 0xff00) == 0x9600		/* adiw (7) */
		|| (inst & 0xff00) == 0x9700) {		/* sbiw (102) */
		d = (inst >> 4) & 0x3;
		d = 24 + d * 2;
		K = ((inst >> 6) & 0x3) << 4;
		K |= inst & 0xf;

		op1 = NAME_(reg16_get_op1)(cpssp, d);
		op2 = K;
		switch (inst & 0xff00) {
		case 0x9600:
			res = NAME_(addw)(cpssp, op1, op2);
			break;
		case 0x9700:
			res = NAME_(subw)(cpssp, op1, op2);
			break;
		default:
			assert(0);
		}
		NAME_(reg16_set_res)(cpssp, d, res);

	} else if ((inst & 0xff00) == 0x9800		/* cbi (37) */
		|| (inst & 0xff00) == 0x9a00) {		/* sbi (99) */
		A = (inst >> 3) & 0x1f;
		b = inst & 0x7;

		addr = A + 0x20;
		op1 = NAME_(in_op1)(cpssp, addr);
		switch (inst & 0xff00) {
		case 0x9800:
			/* cbi */
			res = op1 & ~(1 << b);
			break;
		case 0x9a00:
			/* sbi */
			res = op1 | (1 << b);
			break;
		default:
			assert(0);
		}
		NAME_(out_res)(cpssp, addr, res);

	} else if ((inst & 0xff00) == 0x9900		/* sbic (100) */
		|| (inst & 0xff00) == 0x9b00) {		/* sbis (101) */
		A = (inst >> 3) & 0x1f;
		b = (inst >> 0) & 0x7;

		addr = A + 0x20;
		op1 = NAME_(in_op1)(cpssp, addr);
		res = (op1 >> b) & 1;
		cpssp->NAME.skip = res ^ ((inst >> 9) & 1) ^ 1;

	} else if ((inst & 0xfe08) == 0xfc00		/* sbrc (104) */
		|| (inst & 0xfe08) == 0xfe00) {		/* sbrs (105) */
		r = (inst >> 4) & 0x1f;
		b = (inst >> 0) & 0x7;

		op1 = NAME_(reg8_get_op1)(cpssp, r);
		res = (op1 >> b) & 1;
		cpssp->NAME.skip = res ^ ((inst >> 9) & 1) ^ 1;

	} else if ((inst & 0xfe08) == 0xf800		/* bld (12) */
		|| (inst & 0xfe08) == 0xfa00) {		/* bst (35) */
		r = (inst >> 4) & 0x1f;
		b = (inst >> 0) & 0x7;

		op1 = NAME_(reg8_get_op1)(cpssp, r);
		switch (inst & 0xfe08) {
		case 0xf800:
			/* bld */
			res = (op1 & ~(1 << b)) | (cpssp->NAME.t << b);
			break;
		case 0xfa00:
			/* bst */
			cpssp->NAME.t = (op1 >> b) & 1;
			res = op1;
			break;
		default:
			assert(0); /* Cannot happen. */
		}
		NAME_(reg8_set_res)(cpssp, r, res);

	} else if ((inst & 0xfc00) == 0x0000	/* nop */
		|| (inst & 0xfc00) == 0x0400	/* cpc (50) */
		|| (inst & 0xfc00) == 0x0800	/* sbc (97) */
		|| (inst & 0xfc00) == 0x0c00	/* add (6) */
		|| (inst & 0xfc00) == 0x1000	/* cpse (52) */
		|| (inst & 0xfc00) == 0x1400	/* cp (49) */
		|| (inst & 0xfc00) == 0x1800	/* sub (123) */
		|| (inst & 0xfc00) == 0x1c00	/* adc (5) (rol) */
		|| (inst & 0xfc00) == 0x2000	/* and (8) */
		|| (inst & 0xfc00) == 0x2400	/* eor (58) */
		|| (inst & 0xfc00) == 0x2800	/* or (86) */
		|| (inst & 0xfc00) == 0x2c00) {	/* mov (79) */
		d = (inst >> 4) & 0x1f;
		r = ((inst >> 9) & 0x1) << 4;
		r |= (inst >> 0) & 0xf;

		op1 = NAME_(reg8_get_op1)(cpssp, d);
		op2 = NAME_(reg8_get_op2)(cpssp, r);

		switch (inst & 0xfc00) {
		case 0x0000: /* nop */
			res = 0;
			break;
		case 0x0400: /* cpc */
			res = NAME_(sbc)(cpssp, op1, op2);
			break;
		case 0x0800: /* sbc */
			res = NAME_(sbc)(cpssp, op1, op2);
			break;
		case 0x0c00: /* add */
			res = NAME_(add)(cpssp, op1, op2);
			break;
		case 0x1000: /* cpse */
			cpssp->NAME.skip = op1 == op2;
			break;
		case 0x1400: /* cp */
			res = NAME_(sub)(cpssp, op1, op2);
			break;
		case 0x1800: /* sub */
			res = NAME_(sub)(cpssp, op1, op2);
			break;
		case 0x1c00: /* adc */
			res = NAME_(adc)(cpssp, op1, op2);
			break;
		case 0x2000: /* and */
			res = NAME_(and)(cpssp, op1, op2);
			break;
		case 0x2400: /* eor */
			res = NAME_(eor)(cpssp, op1, op2);
			break;
		case 0x2800: /* or */
			res = NAME_(or)(cpssp, op1, op2);
			break;
		case 0x2c00: /* mov */
			res = op2;
			break;
		default:
			assert(0);
		}

		if ((inst & 0xfc00) != 0x0000
		 && (inst & 0xfc00) != 0x0400
		 && (inst & 0xfc00) != 0x1000
		 && (inst & 0xfc00) != 0x1400) {
			NAME_(reg8_set_res)(cpssp, d, res);
		}

	} else if ((inst & 0xfc00) == 0x9c00) {		/* mul (81) */
		d = (inst >> 4) & 0x1f;
		r = ((inst >> 9) & 0x1) << 4;
		r |= (inst >> 0) & 0xf;

		op1 = NAME_(reg8_get_op1)(cpssp, d);
		op2 = NAME_(reg8_get_op2)(cpssp, r);

		res = NAME_(mul)(cpssp, op1, op2);

		NAME_(reg16_set_res)(cpssp, 0, res);

	} else if ((inst & 0xfc00) == 0xf000		/* brbs (14) */
		|| (inst & 0xfc00) == 0xf400) {		/* brbc (13) */
		k = (inst >> 3) & 0x7f;
		k = (int16_t) k << (16 - 7);
		k = (int16_t) k >> (16 - 7);
		b = (inst >> 0) & 0x7;

		switch (b) {
		case 0:
			res = cpssp->NAME.c;
			break;
		case 1:
			res = cpssp->NAME.z;
			break;
		case 2:
			res = cpssp->NAME.n;
			break;
		case 3:
			res = cpssp->NAME.v;
			break;
		case 4:
			res = cpssp->NAME.s;
			break;
		case 5:
			res = cpssp->NAME.h;
			break;
		case 6:
			res = cpssp->NAME.t;
			break;
		case 7:
			res = cpssp->NAME.i;
			break;
		default:
			assert(0); /* Cannot happen. */
		}
		res ^= (inst >> 10) & 1;

		if (! res) goto label;

		cpssp->NAME.pc += k;

	label:	;

	} else if ((inst & 0xff0f) == 0x940b) {		/* des (54) */
		assert(0); /* FIXME */

	} else if ((inst & 0xfe0f) == 0x9000		/* lds (74) */
		|| (inst & 0xfe0f) == 0x9200) {		/* sts (121) */
		d = (inst >> 4) & 0x1f;
		k = inst2;

		addr = k;
		if ((inst & 0xfe0f) == 0x9000) {
			/* lds */
			op1 = NAME_(ld_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
		} else {
			/* sts */
			op1 = NAME_(reg8_get_op1)(cpssp, d);
			res = op1;
			NAME_(st_res)(cpssp, addr, res);
		}

	} else if ((inst & 0xd000) == 0x8000) {
		/* 0x8000: ldd Z+q (72) */
		/* 0x8008: ldd Y+q (71) */
		/* 0x8200: std Z+q (120) */
		/* 0x8208: std Y+q (119) */
		d = (inst >> 4) & 0x1f;
		q = ((inst >> 13) & 0x1) << 5;
		q |= ((inst >> 10) & 0x3) << 3;
		q |= (inst >> 0) & 0x7;

		assert(/* 0 <= d && */ d < 32);
		assert(/* 0 <= q && */ q < 64);

		if ((inst >> 3) & 1) {
			/* Y */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Y);
		} else {
			/* Z */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Z);
		}
		addr += q;

		if ((inst >> 9) & 1) {
			/* std */
			op1 = NAME_(reg8_get_op1)(cpssp, d);
			res = op1;
			NAME_(st_res)(cpssp, addr, res);
		} else {
			/* ldd */
			op1 = NAME_(ld_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
		}

	} else if ((inst & 0xfe0f) == 0x9400		/* com (48) */
		|| (inst & 0xfe0f) == 0x9401		/* neg (84) */
		|| (inst & 0xfe0f) == 0x9402		/* swap (125) */
		|| (inst & 0xfe0f) == 0x9403		/* inc (65) */
		|| (inst & 0xfe0f) == 0x9405		/* asr (10) */
		|| (inst & 0xfe0f) == 0x9406		/* lsr (78) */
		|| (inst & 0xfe0f) == 0x9407		/* ror (96) */
		|| (inst & 0xfe0f) == 0x940a) {		/* dec (53) */
		d = (inst >> 4) & 0x1f;

		op1 = NAME_(reg8_get_op1)(cpssp, d);

		switch (inst & 0xfe0f) {
		case 0x9400:
			res = NAME_(com)(cpssp, op1);
			break;
		case 0x9401:
			res = NAME_(neg)(cpssp, op1);
			break;
		case 0x9402:
			res = NAME_(swap)(cpssp, op1);
			break;
		case 0x9403:
			res = NAME_(inc)(cpssp, op1);
			break;
		case 0x9405:
			res = NAME_(asr)(cpssp, op1);
			break;
		case 0x9406:
			res = NAME_(lsr)(cpssp, op1);
			break;
		case 0x9407:
			res = NAME_(ror)(cpssp, op1);
			break;
		case 0x940a:
			res = NAME_(dec)(cpssp, op1);
			break;
		default:
			assert(0); /* Cannot happen. */
		}

		NAME_(reg8_set_res)(cpssp, d, res);

	} else if ((inst & 0xffff) == 0x95a8) {		/* wdr (127) */
		/* Ignore... */

	} else if ((inst & 0xfe0f) == 0x900f		/* pop (89) */
		|| (inst & 0xfe0f) == 0x920f) {		/* push (90) */
		d = (inst >> 4) & 0x1f;

		if ((inst >> 9) & 1) {
			/* push */
			op1 = NAME_(reg8_get_op1)(cpssp, d);
			res = op1;
			NAME_(push_res)(cpssp, res);
		} else {
			/* pop */
			op1 = NAME_(pop_op1)(cpssp);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
		}

	} else if ((inst & 0xfe0f) == 0x9001		/* ld Z+ (72) */
		|| (inst & 0xfe0f) == 0x9002		/* ld -Z (72) */
		|| (inst & 0xfe0f) == 0x9004		/* lpm Z (76) */
		|| (inst & 0xfe0f) == 0x9005		/* lpm Z+ (76) */
		|| (inst & 0xfe0f) == 0x9009		/* ld Y+ (71) */
		|| (inst & 0xfe0f) == 0x900a		/* ld -Y (71) */
		|| (inst & 0xfe0f) == 0x900c		/* ld X (70) */
		|| (inst & 0xfe0f) == 0x900d		/* ld X+ (70) */
		|| (inst & 0xfe0f) == 0x9201		/* st Z+ (120) */
		|| (inst & 0xfe0f) == 0x9202		/* st -Z (120) */
		|| (inst & 0xfe0f) == 0x9209		/* st Y+ (119) */
		|| (inst & 0xfe0f) == 0x920a		/* st -Y (119) */
		|| (inst & 0xfe0f) == 0x920c		/* st X (118) */
		|| (inst & 0xfe0f) == 0x920d		/* st X+ (118) */
		|| (inst & 0xfe0f) == 0x920e) {		/* st -X (118) */
		d = (inst >> 4) & 0x1f;

		switch (inst & 0xfe0f) {
		case 0x9001:
			/* ld Z+ */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Z);
			op1 = NAME_(ld_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
			addr += 1;
			NAME_(reg16_set_addr)(cpssp, NAME_REG_Z, addr);
			break;
		case 0x9002:
			/* ld -Z */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Z);
			addr -= 1;
			op1 = NAME_(ld_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
			NAME_(reg16_set_addr)(cpssp, NAME_REG_Z, addr);
			break;
		case 0x9004:
			/* lpm Z */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Z);
			op1 = NAME_(lpm_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
			break;
		case 0x9005:
			/* lpm Z+ */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Z);
			op1 = NAME_(lpm_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
			addr += 1;
			NAME_(reg16_set_addr)(cpssp, NAME_REG_Z, addr);
			break;
		case 0x9009:
			/* ld Y+ */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Y);
			op1 = NAME_(ld_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
			addr += 1;
			NAME_(reg16_set_addr)(cpssp, NAME_REG_Y, addr);
			break;
		case 0x900a:
			/* ld -Y */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Y);
			addr -= 1;
			op1 = NAME_(ld_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
			NAME_(reg16_set_addr)(cpssp, NAME_REG_Y, addr);
			break;
		case 0x900c:
			/* ld X */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_X);
			op1 = NAME_(ld_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
			break;
		case 0x900d:
			/* ld X+ */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_X);
			op1 = NAME_(ld_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, d, res);
			addr += 1;
			NAME_(reg16_set_addr)(cpssp, NAME_REG_X, addr);
			break;
		case 0x9201:
			/* st Z+ */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Z);
			op1 = NAME_(reg8_get_op1)(cpssp, d);
			res = op1;
			NAME_(st_res)(cpssp, addr, res);
			addr += 1;
			NAME_(reg16_set_addr)(cpssp, NAME_REG_Z, addr);
			break;
		case 0x9202:
			/* st -Z */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Z);
			addr -= 1;
			op1 = NAME_(reg8_get_op1)(cpssp, d);
			res = op1;
			NAME_(st_res)(cpssp, addr, res);
			NAME_(reg16_set_addr)(cpssp, NAME_REG_Z, addr);
			break;
		case 0x9209:
			/* st Y+ */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Y);
			op1 = NAME_(reg8_get_op1)(cpssp, d);
			res = op1;
			NAME_(st_res)(cpssp, addr, res);
			addr += 1;
			NAME_(reg16_set_addr)(cpssp, NAME_REG_Y, addr);
			break;
		case 0x920a:
			/* st -Y */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_Y);
			addr -= 1;
			op1 = NAME_(reg8_get_op1)(cpssp, d);
			res = op1;
			NAME_(st_res)(cpssp, addr, res);
			NAME_(reg16_set_addr)(cpssp, NAME_REG_Y, addr);
			break;
		case 0x920c:
			/* st X */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_X);
			op1 = NAME_(reg8_get_op1)(cpssp, d);
			res = op1;
			NAME_(st_res)(cpssp, addr, res);
			break;
		case 0x920d:
			/* st X+ */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_X);
			op1 = NAME_(reg8_get_op1)(cpssp, d);
			res = op1;
			NAME_(st_res)(cpssp, addr, res);
			addr += 1;
			NAME_(reg16_set_addr)(cpssp, NAME_REG_X, addr);
			break;
		case 0x920e:
			/* st -X */
			addr = NAME_(reg16_get_addr)(cpssp, NAME_REG_X);
			addr -= 1;
			op1 = NAME_(reg8_get_op1)(cpssp, d);
			res = op1;
			NAME_(st_res)(cpssp, addr, res);
			NAME_(reg16_set_addr)(cpssp, NAME_REG_X, addr);
			break;
		default:
			assert(0);
		}

	} else if ((inst & 0xf800) == 0xb000		/* in (64) */
		|| (inst & 0xf800) == 0xb800) {		/* out (88) */
		r = (inst >> 4) & 0x1f;
		A = ((inst >> 9) & 0x3) << 4;
		A |= inst & 0xf;

		addr = A + 0x20;
		if ((inst >> 11) & 1) {
			/* out */
			op1 = NAME_(reg8_get_op1)(cpssp, r);
			res = op1;
			NAME_(out_res)(cpssp, addr, res);
		} else {
			/* in */
			op1 = NAME_(in_op1)(cpssp, addr);
			res = op1;
			NAME_(reg8_set_res)(cpssp, r, res);
		}

	} else if ((inst & 0xf000) == 0x3000		/* cpi (51) */
		|| (inst & 0xf000) == 0x4000		/* sbci (98) */
		|| (inst & 0xf000) == 0x5000		/* subi (124) */
		|| (inst & 0xf000) == 0x6000		/* ori (87) */
		|| (inst & 0xf000) == 0x7000		/* andi (9) */
		|| (inst & 0xf000) == 0xe000) {		/* ldi (73) */
		d = (inst >> 4) & 0x0f;
		d |= 0x10;
		K = ((inst >> 8) & 0xf) << 4;
		K |= (inst >> 0) & 0xf;

		op1 = NAME_(reg8_get_op1)(cpssp, d);
		op2 = K;

		switch (inst & 0xf000) {
		case 0x3000:
			/* cpi */
			res = NAME_(sub)(cpssp, op1, op2);
			break;
		case 0x4000:
			/* sbci */
			res = NAME_(sbc)(cpssp, op1, op2);
			break;
		case 0x5000:
			/* subi */
			res = NAME_(sub)(cpssp, op1, op2);
			break;
		case 0x6000:
			/* ori */
			res = NAME_(or)(cpssp, op1, op2);
			break;
		case 0x7000:
			/* andi */
			res = NAME_(and)(cpssp, op1, op2);
			break;
		case 0xe000:
			/* ldi */
			res = op2;
			break;
		default:
			assert(0);
		}

		if ((inst & 0xf000) != 0x3000) {
			NAME_(reg8_set_res)(cpssp, d, res);
		}

	} else {
		/* Unknown instruction. */
		fprintf(stderr, "Unknown instruction %04x\n", inst);
		assert(0); /* FIXME */
	}
label_endif:;
}

static void
NAME_(step)(struct cpssp *cpssp)
{
	if (unlikely(cpssp->NAME.irr
	  && cpssp->NAME.i
	  && ! cpssp->NAME.idelay
	  && ! cpssp->NAME.skip)) {
		/* Interrupt pending. */
		unsigned int irq;

		for (irq = 1; ; irq++) {
			assert(irq < 8 * sizeof(cpssp->NAME.irr));
			if ((cpssp->NAME.irr >> irq) & 1) {
				break;
			}
		}
		NAME_(irq_ack)(cpssp, irq);

		if (DEBUG_CONTROL_FLOW) {
			fprintf(stderr, "Irq %u at 0x%04x\n", irq, cpssp->NAME.pc << 1);
		}

		NAME_(interrupt)(cpssp, irq);

	} else if (unlikely(cpssp->NAME.hlt)) {
		/* Waiting... */

	} else {
		/* Normal instruction execution. */
		if (DEBUG_CONTROL_FLOW) {
			NAME_(dump)(cpssp);
		}

		NAME_(exec)(cpssp);
	}
}

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

	assert(val < 8 * sizeof(cpssp->NAME.irr));

	cpssp->NAME.irr &= ~((uint64_t) 1 << n);
	cpssp->NAME.irr |= (uint64_t) val << n;
}

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

	cpssp->NAME.skip = 0;
	cpssp->NAME.hlt = 0;
	cpssp->NAME.pc = 0x0000;
	cpssp->NAME.sp = 0x000;
	for (i = 0; i < 32; i++) {
		cpssp->NAME.reg[i] = 0x00;
	}
	cpssp->NAME.irr = 0;
}

static void
NAME_(create)(struct cpssp *cpssp)
{
}

static void
NAME_(destroy)(struct cpssp *cpssp)
{
}

#endif /* BEHAVIOR */

#undef DEBUG_CONTROL_FLOW
