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

#ifdef INCLUDE

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

#endif /* INCLUDE */
#ifdef STATE

struct {
	uint8_t primask;

	uint8_t NMI_pending;
	uint8_t NMI_active;

	uint8_t HardFault_pending;
	uint8_t HardFault_active;

	uint8_t SVCall_pending;
	uint8_t SVCall_active;
	uint8_t pri_11;

	uint8_t PendSV_pending;
	uint8_t PendSV_active;
	uint8_t pri_14;

	uint8_t SysTick_pending;
	uint8_t SysTick_active;
	uint8_t pri_15;

	uint8_t nvic_enabled[CONFIG_INTLINESNUM];
	uint8_t nvic_pending[CONFIG_INTLINESNUM];
	uint8_t nvic_active[CONFIG_INTLINESNUM];
	uint8_t nvic_priority[CONFIG_INTLINESNUM];
} NAME;

#endif /* STATE */
#ifdef EXPORT

/*forward*/ static uint8_t
NAME_(exc_ack)(struct cpssp *cpssp);
/*forward*/ static void
NAME_(exc_eoi)(struct cpssp *cpssp, uint8_t vec);
/*forward*/ static void
NAME_(st)(struct cpssp *cpssp, uint32_t addr, unsigned int bs, uint32_t val);
/*forward*/ static void
NAME_(ld)(struct cpssp *cpssp, uint32_t addr, unsigned int bs, uint32_t *valp);
/*forward*/ static void
NAME_(primask_set)(struct cpssp *cpssp, unsigned int val);
/*forward*/ static unsigned int
NAME_(primask_get)(struct cpssp *cpssp);
/*forward*/ static void
NAME_(pri_set)(struct cpssp *cpssp, unsigned int n, unsigned int val);
/*forward*/ static unsigned int
NAME_(pri_get)(struct cpssp *cpssp, unsigned int n);
/*forward*/ static void
NAME_(NMI_pending_set)(struct cpssp *cpssp, unsigned int val);
/*forward*/ static void
NAME_(PendSV_pending_set)(struct cpssp *cpssp, unsigned int val);
/*forward*/ static void
NAME_(SVCall_pending_set)(struct cpssp *cpssp, unsigned int val);
/*forward*/ static void
NAME_(systick_pending_set)(struct cpssp *cpssp, unsigned int val);
/*forward*/ static void
NAME_(irq_N_set)(struct cpssp *cpssp, int n, unsigned int val);
/*forward*/ static void
NAME_(reset)(struct cpssp *cpssp);
/*forward*/ static void
NAME_(create)(struct cpssp *cpssp);
/*forward*/ static void
NAME_(destroy)(struct cpssp *cpssp);

#endif /* EXPORT */
#ifdef BEHAVIOR

static void
NAME_(irq_update)(struct cpssp *cpssp)
{
	int cpri;
	int bpri;
	int nr;

	if (cpssp->NAME.primask) {
		cpri = 0;
	} else {
		cpri = 256;
	}
	bpri = 256;

	/* NMI */
	if (cpssp->NAME.NMI_active
	 && -2 < cpri) {
		cpri = -2;
	}
	if (cpssp->NAME.NMI_pending
	 && -2 < bpri) {
		bpri = -2;
	}

	/* HardFault */
	if (cpssp->NAME.HardFault_active
	 && -1 < cpri) {
		cpri = -1;
	}
	if (cpssp->NAME.HardFault_pending
	 && -1 < bpri) {
		bpri = -1;
	}

	/* SVCall */
	if (cpssp->NAME.SVCall_active
	 && cpssp->NAME.pri_11 < cpri) {
		cpri = cpssp->NAME.pri_11;
	}
	if (cpssp->NAME.SVCall_pending
	 && cpssp->NAME.pri_11 < bpri) {
		bpri = cpssp->NAME.pri_11;
	}

	/* PendSV */
	if (cpssp->NAME.PendSV_active
	 && cpssp->NAME.pri_14 < cpri) {
		cpri = cpssp->NAME.pri_14;
	}
	if (cpssp->NAME.PendSV_pending
	 && cpssp->NAME.pri_14 < bpri) {
		bpri = cpssp->NAME.pri_14;
	}

	/* SysTick */
	if (cpssp->NAME.SysTick_active
	 && cpssp->NAME.pri_15 < cpri) {
		cpri = cpssp->NAME.pri_15;
	}
	if (cpssp->NAME.SysTick_pending
	 && cpssp->NAME.pri_15 < bpri) {
		bpri = cpssp->NAME.pri_15;
	}

	/* External Interrupts */
	for (nr = 0; nr < CONFIG_INTLINESNUM; nr++) {
		if (cpssp->NAME.nvic_active[nr]
		 && cpssp->NAME.nvic_priority[nr] < cpri) {
			cpri = cpssp->NAME.nvic_priority[nr];
		}
		if (cpssp->NAME.nvic_enabled[nr]
		 && cpssp->NAME.nvic_pending[nr]
		 && cpssp->NAME.nvic_priority[nr] < bpri) {
			bpri = cpssp->NAME.nvic_priority[nr];
		}
	}

	NAME_(irq_set)(cpssp, bpri < cpri);

	if (DEBUG_CONTROL_FLOW) {
		fprintf(stderr, "%s: bpri=%d cpri=%d irq=%d\n", __FUNCTION__,
				bpri, cpri, bpri < cpri);
	}
}

static uint8_t
NAME_(exc_ack)(struct cpssp *cpssp)
{
	int pri;
	int vec;
	int nr;

	pri = 256;
	vec = 0;

	/* NMI */
	if (cpssp->NAME.NMI_pending
	 && -2 < pri) {
		pri = -2;
		vec = 2;
	}

	/* HardFault */
	if (cpssp->NAME.HardFault_pending
	 && -1 < pri) {
		pri = -1;
		vec = 3;
	}

	/* SVCall */
	if (cpssp->NAME.SVCall_pending
	 && cpssp->NAME.pri_11 < pri) {
		pri = cpssp->NAME.pri_11;
		vec = 11;
	}

	/* PendSV */
	if (cpssp->NAME.PendSV_pending
	 && cpssp->NAME.pri_14 < pri) {
		pri = cpssp->NAME.pri_14;
		vec = 14;
	}

	/* SysTick */
	if (cpssp->NAME.SysTick_pending
	 && cpssp->NAME.pri_15 < pri) {
		pri = cpssp->NAME.pri_15;
		vec = 15;
	}

	/* External Interrupts */
	for (nr = 0; nr < CONFIG_INTLINESNUM; nr++) {
		if (cpssp->NAME.nvic_enabled[nr]
		 && cpssp->NAME.nvic_pending[nr]
		 && cpssp->NAME.nvic_priority[nr] < pri) {
			pri = cpssp->NAME.nvic_priority[nr];
			vec = nr + 0x10;
		}
	}

	switch (vec) {
	case 0 ... 1: /* Reset */
		assert(0);
	case 2: /* NMI */
		cpssp->NAME.NMI_pending = 0;
		cpssp->NAME.NMI_active = 1;
		break;
	case 3: /* HardFault */
		cpssp->NAME.HardFault_pending = 0;
		cpssp->NAME.HardFault_active = 1;
		break;
	case 4 ... 10: /* Reserved */
		assert(0);
	case 11: /* SVCall */
		cpssp->NAME.SVCall_pending = 0;
		cpssp->NAME.SVCall_active = 1;
		break;
	case 12 ... 13: /* Reserved */
		assert(0);
	case 14: /* PendSV */
		cpssp->NAME.PendSV_pending = 0;
		cpssp->NAME.PendSV_active = 1;
		break;
	case 15: /* SysTick */
		cpssp->NAME.SysTick_pending = 0;
		cpssp->NAME.SysTick_active = 1;
		break;
	case 0x10 ... 0x10 + CONFIG_INTLINESNUM - 1:
		cpssp->NAME.nvic_pending[vec - 0x10] = 0;
		cpssp->NAME.nvic_active[vec - 0x10] = 1;
		break;
	default:
		assert(0);
	}

	NAME_(irq_update)(cpssp);

	if (DEBUG_CONTROL_FLOW) {
		fprintf(stderr, "%s: vec=0x%02x\n", __FUNCTION__, vec);
	}

	return vec;
}

static void
NAME_(exc_eoi)(struct cpssp *cpssp, uint8_t vec)
{
	if (DEBUG_CONTROL_FLOW) {
		fprintf(stderr, "%s: vec=0x%02x\n", __FUNCTION__, vec);
	}

	switch (vec) {
	case 0x00 ... 0x01: /* Reset */
		assert(0);
	case 0x02: /* NMI */
		cpssp->NAME.NMI_active = 0;
		break;
	case 0x03: /* HardFault */
		cpssp->NAME.HardFault_active = 0;
		break;
	case 0x04 ... 0x0a: /* Reserved */
		assert(0);
	case 0x0b: /* SVCall */
		cpssp->NAME.SVCall_active = 0;
		break;
	case 0x0c ... 0x0d: /* Reserved */
		assert(0);
	case 0x0e: /* PendSV */
		cpssp->NAME.PendSV_active = 0;
		break;
	case 0x0f: /* SysTick */
		cpssp->NAME.SysTick_active = 0;
		break;
	case 0x10 ... 0x10 + CONFIG_INTLINESNUM - 1: /* External Interrupt */
		cpssp->NAME.nvic_active[vec - 0x10] = 0;
		break;
	default:
		assert(0); /* Mustn't happen. */
	}
	NAME_(irq_update)(cpssp);
}

static void
NAME_(st)(struct cpssp *cpssp, uint32_t addr, unsigned int bs, uint32_t val)
{
	int nr;
	int i;

	addr &= 0xfff;

	if (DEBUG_CONTROL_FLOW) {
		fprintf(stderr, "%s: addr=0x%03x val=0x%08x\n",
				__FUNCTION__, addr, val);
	}

	/* Nested Vectored Interrupt Controller Register (0x100-0xcff) */
	/* 3.4.2 */
	switch (addr) {
	case 0x100 ... 0x13c:
		/* Interrupt Set-Enable Register */
		/* B3.4.3 */
		assert(bs == 0b1111); /* UNPREDICTABLE */

		nr = (addr - 0x100) / 4 * 32;
		for (i = 0; i < 32; i++) {
			if (nr + i < CONFIG_INTLINESNUM) {
				cpssp->NAME.nvic_enabled[nr + i] |= (val >> i) & 1;
			}
		}
		NAME_(irq_update)(cpssp);
		break;
	case 0x140 ... 0x17c:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	case 0x180 ... 0x1bc:
		/* Interrupt Clear-Enable Register */
		/* B3.4.4 */
		assert(bs == 0b1111); /* UNPREDICTABLE */

		nr = (addr - 0x180) / 4 * 32;
		for (i = 0; i < 32; i++) {
			if (nr + i < CONFIG_INTLINESNUM) {
				cpssp->NAME.nvic_enabled[nr + i] &= ~((val >> i) & 1);
			}
		}
		NAME_(irq_update)(cpssp);
		break;
	case 0x1c0 ... 0x1fc:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	case 0x200 ... 0x23c:
		/* Interrupt Set-Pending Register */
		/* B3.4.5 */
		assert(bs == 0b1111); /* UNPREDICTABLE */

		nr = (addr - 0x200) / 4 * 32;
		for (i = 0; i < 32; i++) {
			if (nr + i < CONFIG_INTLINESNUM) {
				cpssp->NAME.nvic_pending[nr + i] |= (val >> i) & 1;
			}
		}
		NAME_(irq_update)(cpssp);
		break;
	case 0x240 ... 0x27c:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	case 0x280 ... 0x2bc:
		/* Interrupt Clear-Pending Register */
		/* B3.4.6 */
		assert(bs == 0b1111); /* UNPREDICTABLE */

		nr = (addr - 0x280) / 4 * 32;
		for (i = 0; i < 32; i++) {
			if (nr + i < CONFIG_INTLINESNUM) {
				cpssp->NAME.nvic_pending[nr + i] &= ~((val >> i) & 1);
			}
		}
		NAME_(irq_update)(cpssp);
		break;
	case 0x2c0 ... 0x2fc:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	case 0x300 ... 0x33c:
		/* Interrupt-Active Bit Register */
		/* ARMv7-M: B3-686 */
		assert(0); /* FIXME */
		break;
	case 0x340 ... 0x3fc:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	case 0x400 ... 0x5ec:
		/* Interrupt Priority Registers */
		/* B3.4.7 */
		nr = addr - 0x400;
		if ((bs >> 3) & 1) {
			if (nr + 3 < CONFIG_INTLINESNUM) {
				cpssp->NAME.nvic_priority[nr + 3] = (val >> 24) & 0xff;
			}
		}
		if ((bs >> 2) & 1) {
			if (nr + 2 < CONFIG_INTLINESNUM) {
				cpssp->NAME.nvic_priority[nr + 2] = (val >> 16) & 0xff;
			}
		}
		if ((bs >> 1) & 1) {
			if (nr + 1 < CONFIG_INTLINESNUM) {
				cpssp->NAME.nvic_priority[nr + 1] = (val >> 8) & 0xff;
			}
		}
		if ((bs >> 0) & 1) {
			if (nr + 0 < CONFIG_INTLINESNUM) {
				cpssp->NAME.nvic_priority[nr + 0] = (val >> 0) & 0xff;
			}
		}
		NAME_(irq_update)(cpssp);
		break;
	case 0x5f0 ... 0xcfc:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	default:
		/* Reserved */
		fprintf(stderr, "WARNING: %s: addr=0x%08lx val=0x%08lx\n",
				__FUNCTION__, addr, val);
		assert(0); /* FIXME */
	}
}

static void
NAME_(ld)(struct cpssp *cpssp, uint32_t addr, unsigned int bs, uint32_t *valp)
{
	int nr;
	int i;

	assert(bs == 0b1111); /* UNPREDICTABLE */

	addr &= 0xfff;

	/* Nested Vectored Interrupt Controller Register (0x100-0xcff) */
	/* 3.4.2 */
	*valp = 0;
	switch (addr) {
	case 0x100 ... 0x13c:
		/* Interrupt Set-Enable Register */
		/* B3.4.3 */
		nr = (addr - 0x100) / 4 * 32;
		for (i = 0; i < 32; i++) {
			if (nr + i < CONFIG_INTLINESNUM) {
				*valp |= cpssp->NAME.nvic_enabled[nr + i] << i;
			}
		}
		break;
	case 0x140 ... 0x17c:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	case 0x180 ... 0x1bc:
		/* Interrupt Clear Enable Register */
		/* B3.4.4 */
		nr = (addr - 0x180) / 4 * 32;
		for (i = 0; i < 32; i++) {
			if (nr + i < CONFIG_INTLINESNUM) {
				*valp |= cpssp->NAME.nvic_enabled[nr + i] << i;
			}
		}
		break;
	case 0x1c0 ... 0x1fc:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	case 0x200 ... 0x23c:
		/* Interrupt Set-Pending Register */
		/* B3.4.5 */
		nr = (addr - 0x200) / 4 * 32;
		for (i = 0; i < 32; i++) {
			if (nr + i < CONFIG_INTLINESNUM) {
				*valp |= cpssp->NAME.nvic_pending[nr + i] << i;
			}
		}
		break;
	case 0x240 ... 0x27c:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	case 0x280 ... 0x2bc:
		/* Interrupt Clear-Pending Register */
		/* B3.4.6 */
		nr = (addr - 0x280) / 4 * 32;
		for (i = 0; i < 32; i++) {
			if (nr + i < CONFIG_INTLINESNUM) {
				*valp |= cpssp->NAME.nvic_pending[nr + i] << i;
			}
		}
		break;
	case 0x300 ... 0x3fc:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	case 0x400 ... 0x5ec:
		/* Interrupt Priority Registers */
		/* B3.4.7 */
		nr = addr - 0x400;
		if (nr + 3 < CONFIG_INTLINESNUM) {
			*valp |= cpssp->NAME.nvic_priority[nr + 3] << 24;
		}
		if (nr + 2 < CONFIG_INTLINESNUM) {
			*valp |= cpssp->NAME.nvic_priority[nr + 2] << 16;
		}
		if (nr + 1 < CONFIG_INTLINESNUM) {
			*valp |= cpssp->NAME.nvic_priority[nr + 1] << 8;
		}
		if (nr + 0 < CONFIG_INTLINESNUM) {
			*valp |= cpssp->NAME.nvic_priority[nr + 0] << 0;
		}
		break;
	case 0x5f0 ... 0xcfc:
		/* Reserved */
		assert(0); /* FIXME */
		break;
	default:
		/* Reserved */
		fprintf(stderr, "WARNING: %s: addr=0x%08lx\n",
				__FUNCTION__, addr);
		assert(0); /* FIXME */
	}

	if (DEBUG_CONTROL_FLOW) {
		fprintf(stderr, "%s: addr=0x%03x val=0x%08x\n",
				__FUNCTION__, addr, *valp);
	}
}

static void
NAME_(primask_set)(struct cpssp *cpssp, unsigned int val)
{
	cpssp->NAME.primask = val;
	NAME_(irq_update)(cpssp);
}

static unsigned int
NAME_(primask_get)(struct cpssp *cpssp)
{
	return cpssp->NAME.primask;
}

static void
NAME_(pri_set)(struct cpssp *cpssp, unsigned int n, unsigned int val)
{
	switch (n) {
	case 11:
		cpssp->NAME.pri_11 = val;
		break;
	case 14:
		cpssp->NAME.pri_14 = val;
		break;
	case 15:
		cpssp->NAME.pri_15 = val;
		break;
	default:
		assert(0); /* Mustn't happen. */
	}
}

static unsigned int
NAME_(pri_get)(struct cpssp *cpssp, unsigned int n)
{
	switch (n) {
	case 11:
		return cpssp->NAME.pri_11;
	case 14:
		return cpssp->NAME.pri_14;
	case 15:
		return cpssp->NAME.pri_15;
	default:
		assert(0); /* Mustn't happen. */
	}
}

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

	cpssp->NAME.NMI_pending = val;
	NAME_(irq_update)(cpssp);
}

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

	cpssp->NAME.PendSV_pending = val;
	NAME_(irq_update)(cpssp);
}

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

	cpssp->NAME.SVCall_pending = val;
	NAME_(irq_update)(cpssp);
}

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

	cpssp->NAME.SysTick_pending = 1;
	NAME_(irq_update)(cpssp);
}

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

	if (val) {
		cpssp->NAME.nvic_pending[n] = 1;
		NAME_(irq_update)(cpssp);
	}
}

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

	cpssp->NAME.primask = 0;

	cpssp->NAME.NMI_pending = 0;
	cpssp->NAME.NMI_active = 0;

	cpssp->NAME.HardFault_pending = 0;
	cpssp->NAME.HardFault_active = 0;

	cpssp->NAME.SVCall_pending = 0;
	cpssp->NAME.SVCall_active = 0;
	cpssp->NAME.pri_11 = 0; /* FIXME */

	cpssp->NAME.PendSV_pending = 0;
	cpssp->NAME.PendSV_active = 0;
	cpssp->NAME.pri_14 = 0; /* FIXME */

	cpssp->NAME.SysTick_pending = 0;
	cpssp->NAME.SysTick_active = 0;
	cpssp->NAME.pri_15 = 0; /* FIXME */

	for (nr = 0; nr < CONFIG_INTLINESNUM; nr++) {
		cpssp->NAME.nvic_enabled[nr] = 0;
		cpssp->NAME.nvic_pending[nr] = 0;
		cpssp->NAME.nvic_active[nr] = 0;
		cpssp->NAME.nvic_priority[nr] = 0;
	}
}

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

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

#endif /* BEHAVIOR */

#undef DEBUG_CONTROL_FLOW
