/*
MPC5554 Interrupt Controller Simulator

11.2005 MAR

NOTE: This simulator does not support hardware mode. The only effect of setting bit HVEN in MCR is 
      the reading of IACKR does not negate interrupt request.
      
      See implementation description under pdf\simulator_api_lib.pdf
*/

#include "simul.h"

#define MCR_OFFSET   0x00
#define CPR_OFFSET   0x08
#define IACKR_OFFSET 0x10
#define EOIR_OFFSET  0x18
#define SSCIR_OFFSET 0x20
#define PSR_OFFSET   0x40
#define NUM_OF_INTS  308
#define LIFO_TOP     14

#define PORT_BASE	0				/* defines first intterrupt line input port pin */
#define ADDR_BASE	0xfff48000

typedef struct
{
	simulWord32		mcr, cpr, iackr;
	unsigned char	sscir[8], psr[NUM_OF_INTS];
}
Regs;

typedef struct
{
	int		cnt;
	char	irq[NUM_OF_INTS];
}
Ints;

typedef struct
{
	char		data[LIFO_TOP];
	char		top, items;
}
Lifo;

typedef struct
{
	int			intno[4];
	char		top;
}
Queue;

typedef struct
{
	simulWord32	startaddress;
	int			bustype;
	int			portbase;
	simulWord32	intvec;
	int			intreq;
	simulTime	inttime;
	Regs		regs;
	Ints		ints;
	Lifo		lifo;
	Queue		queue;
	void		* irtimer;
	void		* lifotimer;
}
intCtrl;

/* function array for int in's */

static int IntPort_Change(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private, int group);

static int SIMULAPI IntPortGp0(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
	{return IntPort_Change(processor, cbs, private, 0);}
static int SIMULAPI IntPortGp1(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
	{return IntPort_Change(processor, cbs, private, 1);}
static int SIMULAPI IntPortGp2(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
	{return IntPort_Change(processor, cbs, private, 2);}
static int SIMULAPI IntPortGp3(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
	{return IntPort_Change(processor, cbs, private, 3);}
static int SIMULAPI IntPortGp4(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private) 
	{return IntPort_Change(processor, cbs, private, 4);}
static int SIMULAPI IntPortGp5(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
	{return IntPort_Change(processor, cbs, private, 5);}
static int SIMULAPI IntPortGp6(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private) 
	{return IntPort_Change(processor, cbs, private, 6);}
static int SIMULAPI IntPortGp7(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
	{return IntPort_Change(processor, cbs, private, 7);}
static int SIMULAPI IntPortGp8(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private) 
	{return IntPort_Change(processor, cbs, private, 8);}
static int SIMULAPI IntPortGp9(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
	{return IntPort_Change(processor, cbs, private, 9);}


void * IntPort_Tab[10] = {IntPortGp0, IntPortGp1, IntPortGp2, IntPortGp3, IntPortGp4, 
                          IntPortGp5, IntPortGp6, IntPortGp7, IntPortGp8, IntPortGp9};

/* --- interrupt processing --- */

static void CheckQueue(simulProcessor processor, intCtrl *intc)
{	
	int	i, d, ptr;
	int vector = -1;
	simulTime time;

	SIMUL_GetClock(processor, 0, &time);

	if (intc->inttime < time)
	{
		d = 0;
		for (i = 0; i < 4; i ++)			/* clears queue */
			intc->queue.intno[i] = -1;
	}
	else
	{
		d = (int) (3 - (intc->inttime - time));
	}

	ptr = (intc->queue.top + d) & 0x3;
	for (i = NUM_OF_INTS - 1; i >= 0; i--)
	{
		if ((intc->ints.irq[i] & 0x1)&&(intc->regs.psr[i] >= ((vector != -1) ? intc->regs.psr[vector] : -1)))
			vector = i;
	}
	intc->queue.intno[(intc->queue.top + d + ((vector > 7) ? 0 : 1)) & 0x3] = vector;

	if ((intc->inttime < time)&&(intc->ints.cnt))
	{
		intc->inttime = time + 3;

		if (intc->queue.intno[intc->queue.top] == -1)
		{
			intc->queue.top = (intc->queue.top + 1) & 0x3;
			intc->inttime++;
		}
		SIMUL_StartTimer(processor, intc->irtimer, SIMUL_TIMER_ABS | SIMUL_TIMER_CLOCKS, &intc->inttime);
	}
}

static void IntSet(simulProcessor processor, intCtrl * intc, unsigned int intno)
{
	if (~intc->ints.irq[intno] & 0x1)
	{
		intc->ints.irq[intno] |= 0x1;
		intc->ints.cnt++;
	}

	CheckQueue(processor, intc);
}

static void IntClr(intCtrl * intc, unsigned int intno)
{
	if (intc->ints.irq[intno] & 0x1)
	{
		intc->ints.irq[intno] &= ~0x1;
		intc->ints.cnt--;
	}
}

/* --- bus read/write --- */

int bus2longreg(simulBusCallbackStruct * bus, simulWord32 * reg)
{
	switch (bus->width)
	{
	case 8:
		*reg = (*reg & ~(0xff << ((~bus->address & 0x3) << 3))) | (bus->data << ((~bus->address & 0x3) << 3));
		break;
	case 16:
		if (bus->address & 0x1)
			return SIMUL_MEMORY_FAIL;
		*reg = (*reg & ~(0xffff << ((~bus->address & 0x2) << 3))) | (bus->data << ((~bus->address & 0x2) << 3));
		break;
	case 32:
		if (bus->address & 0x3)
			return SIMUL_MEMORY_FAIL;
		*reg = bus->data;
		break;
	default:
		return SIMUL_MEMORY_FAIL;
	}
	return SIMUL_MEMORY_OK;
}

int longreg2bus(simulBusCallbackStruct * bus, simulWord32 * reg)
{
	switch (bus->width)
	{
	case 8:
		bus->data = (*reg >> ((~bus->address & 0x3) << 3)) & 0xff;
		break;
	case 16:
		if (bus->address & 0x1)
			return SIMUL_MEMORY_FAIL;
		bus->data = (*reg >> ((~bus->address & 0x2) << 3)) & 0xffff;
		break;
	case 32:
		if (bus->address & 0x3)
			return SIMUL_MEMORY_FAIL;
		bus->data = *reg;
		break;
	default:
		return SIMUL_MEMORY_FAIL;
	}
	return SIMUL_MEMORY_OK;
}

/* --- Regs I/O --- */

/* MCR register */

static int SIMULAPI MCR_Write(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	cbs->x.bus.clocks = 1;
	return bus2longreg(&cbs->x.bus, &intc->regs.mcr);
}

static int SIMULAPI MCR_Read(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	simulWord32 reg = intc->regs.mcr & 0x21;
	cbs->x.bus.clocks = 1;
	return longreg2bus(&cbs->x.bus, &reg);
}

static void MCR_Init(simulProcessor processor, intCtrl * intc)
{
    simulWord from	= intc->startaddress + MCR_OFFSET;
	simulWord to	= intc->startaddress + MCR_OFFSET + 3;

    intc->regs.mcr = 0;
    SIMUL_RegisterBusWriteCallback(processor, MCR_Write, (simulPtr) intc, intc->bustype, &from, &to);
    SIMUL_RegisterBusReadCallback(processor, MCR_Read, (simulPtr) intc, intc->bustype, &from, &to);
}

/* CPR register */

static int SIMULAPI CPR_Write(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	int i;

	cbs->x.bus.clocks = 1;
	i = bus2longreg(&cbs->x.bus, &intc->regs.cpr);

	if (intc->ints.cnt)
		CheckQueue(processor, intc);

	return i;
}

static int SIMULAPI CPR_Read(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	simulWord32 reg = intc->regs.cpr & 0xf;
	cbs->x.bus.clocks = 1;
	return longreg2bus(&cbs->x.bus, &reg);
}

static void CPR_Init(simulProcessor processor, intCtrl * intc)
{
    simulWord from	= intc->startaddress + CPR_OFFSET;
	simulWord to	= intc->startaddress + CPR_OFFSET + 3;

    intc->regs.cpr = 0xf;
    SIMUL_RegisterBusWriteCallback(processor, CPR_Write, (simulPtr) intc, intc->bustype, &from, &to);
    SIMUL_RegisterBusReadCallback(processor, CPR_Read, (simulPtr) intc, intc->bustype, &from, &to);
}

/* IACKR register */

static int SIMULAPI IACKR_Write(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	simulWord32 reg;
	cbs->x.bus.clocks = 1;
	if (bus2longreg(&cbs->x.bus, &reg) == SIMUL_MEMORY_FAIL)
		return SIMUL_MEMORY_FAIL;
	intc->regs.iackr = reg;
	return SIMUL_MEMORY_OK;
}

static int SIMULAPI IACKR_Read(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	simulWord32 reg;
	simulWord data = 0;

	if (intc->regs.mcr & 0x20)
		reg = (intc->regs.iackr & ~0x7f8) | ((intc->intvec & 0xff) << 3);
	else
		reg = (intc->regs.iackr & ~0x7fc) | ((intc->intvec & 0xff) << 2);

	cbs->x.bus.clocks = 1;
	if ((cbs->x.bus.cycletype) && !(intc->regs.mcr & 0x1))
	{
		if (intc->intreq)
		{
			SIMUL_SetPort(processor, SIMUL_PORT_INTERRUPT, 1, &data);
			intc->intreq &= ~0x1;
		}
	}

	/* push lifo */

	if (~intc->regs.cpr & 0xf)
	{
		intc->lifo.top = (intc->lifo.top < (LIFO_TOP - 1)) ? (intc->lifo.top + 1) : 0;
		intc->lifo.data[intc->lifo.top] = intc->regs.cpr & 0xf;
		if (intc->lifo.items < LIFO_TOP)
			intc->lifo.items++;
	}

	intc->regs.cpr = (intc->intvec < NUM_OF_INTS) ? (intc->regs.psr[intc->intvec]) : 0x0;

	return longreg2bus(&cbs->x.bus, &reg);
}

static void IACKR_Init(simulProcessor processor, intCtrl * intc)
{
    simulWord from	= intc->startaddress + IACKR_OFFSET;
	simulWord to	= intc->startaddress + IACKR_OFFSET + 3;

    intc->regs.iackr = 0x0;
    SIMUL_RegisterBusWriteCallback(processor, IACKR_Write, (simulPtr) intc, intc->bustype, &from, &to);
    SIMUL_RegisterBusReadCallback(processor, IACKR_Read, (simulPtr) intc, intc->bustype, &from, &to);
}

/* EOIR register */

static int SIMULAPI EOIR_Write(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	simulWord32 reg;
	simulTime time = 1;

	SIMUL_StartTimer(processor, intc->lifotimer, SIMUL_TIMER_REL | SIMUL_TIMER_CLOCKS, &time);

	return bus2longreg(&cbs->x.bus, &reg);
}

static int SIMULAPI EOIR_Read(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	simulWord32 reg = 0x0;
	cbs->x.bus.clocks = 1;
	return longreg2bus(&cbs->x.bus, &reg);
}

static void EOIR_Init(simulProcessor processor, intCtrl * intc)
{
    simulWord from	= intc->startaddress + EOIR_OFFSET;
	simulWord to	= intc->startaddress + EOIR_OFFSET + 3;

    SIMUL_RegisterBusWriteCallback(processor, EOIR_Write, (simulPtr) intc, intc->bustype, &from, &to);
    SIMUL_RegisterBusReadCallback(processor, EOIR_Read, (simulPtr) intc, intc->bustype, &from, &to);
}

/* SSCIR registers */

static int SIMULAPI SSCIR_Write(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
	intCtrl *intc = (intCtrl*) private;
	simulWord32 adr, i;

	cbs->x.bus.clocks = 1;

	if ((cbs->x.bus.width != 8)&&(cbs->x.bus.width != 16)&&(cbs->x.bus.width != 32))
		return SIMUL_MEMORY_FAIL;

	if (cbs->x.bus.address & ~(~0x0 << (cbs->x.bus.width >> 4)))
		return SIMUL_MEMORY_FAIL;

	adr = cbs->x.bus.address & 0x7;
	for (i = cbs->x.bus.width; i > 0; i -= 8)
	{
		if (cbs->x.bus.data & (0x2 << (i - 8)))
		{
			intc->regs.sscir[adr] |= 0x1;
			IntSet(processor, intc, adr);
		}
		else if (cbs->x.bus.data & (0x1 << (i - 8)))
		{
			intc->regs.sscir[adr] &= ~0x1;
			IntClr(intc, adr);
		}
		adr++;
	}

	return SIMUL_MEMORY_OK;
}

static int SIMULAPI SSCIR_Read(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	simulWord32 i, adr;

	cbs->x.bus.clocks = 1;
	cbs->x.bus.data = 0x0;

	if ((cbs->x.bus.width != 8)&&(cbs->x.bus.width != 16)&&(cbs->x.bus.width != 32))
		return SIMUL_MEMORY_FAIL;

	if (cbs->x.bus.address & ~(~0x0 << (cbs->x.bus.width >> 4)))
		return SIMUL_MEMORY_FAIL;

	adr = cbs->x.bus.address & 0x7;
	for (i = cbs->x.bus.width; i > 0; i -= 8)
		if (intc->regs.sscir[adr++] & 0x1)
			cbs->x.bus.data |= (0x1 << (i - 8));

	return SIMUL_MEMORY_OK;
}

static void SSCIR_Init(simulProcessor processor, intCtrl * intc)
{
    simulWord from	= intc->startaddress + SSCIR_OFFSET;
	simulWord to	= intc->startaddress + SSCIR_OFFSET + 7;

    SIMUL_RegisterBusWriteCallback(processor, SSCIR_Write, (simulPtr) intc, intc->bustype, &from, &to);
    SIMUL_RegisterBusReadCallback(processor, SSCIR_Read, (simulPtr) intc, intc->bustype, &from, &to);
}

/* PSR registers */

static int SIMULAPI PSR_Write(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	simulWord32 adr, i;

	cbs->x.bus.clocks = 1;

	if ((cbs->x.bus.width != 8)&&(cbs->x.bus.width != 16)&&(cbs->x.bus.width != 32))
		return SIMUL_MEMORY_FAIL;

	if (cbs->x.bus.address & ~(~0x0 << (cbs->x.bus.width >> 4)))
		return SIMUL_MEMORY_FAIL;

	adr = cbs->x.bus.address - intc->startaddress - PSR_OFFSET;
	for (i = cbs->x.bus.width; i > 0; i -= 8)
		intc->regs.psr[adr++] = (cbs->x.bus.data >> (i - 8)) & 0xf;

	return SIMUL_MEMORY_OK;
}

static int SIMULAPI PSR_Read(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
    intCtrl *intc = (intCtrl*) private;
	simulWord32 i, adr;

	cbs->x.bus.clocks = 1;
	cbs->x.bus.data = 0x0;

	if ((cbs->x.bus.width != 8)&&(cbs->x.bus.width != 16)&&(cbs->x.bus.width != 32))
		return SIMUL_MEMORY_FAIL;

	if (cbs->x.bus.address & ~(~0x0 << (cbs->x.bus.width >> 4)))
		return SIMUL_MEMORY_FAIL;

	adr = cbs->x.bus.address - intc->startaddress - PSR_OFFSET;
	for (i = cbs->x.bus.width; i > 0; i -= 8)
		cbs->x.bus.data |= (intc->regs.psr[adr++] & 0xf) << (i - 8);

	return SIMUL_MEMORY_OK;
}

static void PSR_Init(simulProcessor processor, intCtrl * intc)
{
    simulWord from	= intc->startaddress + PSR_OFFSET;
	simulWord to	= intc->startaddress + PSR_OFFSET + NUM_OF_INTS - 1;

    SIMUL_RegisterBusWriteCallback(processor, PSR_Write, (simulPtr) intc, intc->bustype, &from, &to);
    SIMUL_RegisterBusReadCallback(processor, PSR_Read, (simulPtr) intc, intc->bustype, &from, &to);
}

/* interrupt ports */

static int IntPort_Change(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private, int group)
{
    intCtrl *intc = (intCtrl*) private;
	int i;
	simulWord portbit = ~cbs->x.port.olddata & cbs->x.port.newdata;

	for (i = 0; i < 32; i++)
	{
		if (portbit & (1 << i))
			IntSet(processor, intc, (i | (group << 5)) + 8);
	}

	portbit = cbs->x.port.olddata & ~cbs->x.port.newdata;
	for (i = 0; i < 32; i++)
	{
		if (portbit & (1 << i))
			IntClr(intc, (i | (group << 5)) + 8);
	}

    return SIMUL_PORT_OK;
}

static void IntPort_Init(simulProcessor processor, intCtrl * intc)
{
	int i;
	for (i = 0; i < 9; i++)
		SIMUL_RegisterPortChangeCallback(processor, IntPort_Tab[i], (simulPtr) intc, (intc->portbase + (i << 5)), 32);

	SIMUL_RegisterPortChangeCallback(processor, IntPort_Tab[9], (simulPtr) intc, (intc->portbase + (i << 5)), 12);
}

/* --- internal timers --- */

static int SIMULAPI IntReq_Timer(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
	intCtrl *intc = (intCtrl*) private;
	simulTime time;
	simulWord data = 1;
	int vector;

	vector = intc->queue.intno[intc->queue.top];
	if ((intc->regs.psr[vector] > (signed long) (intc->regs.cpr & 0xf))&&(intc->ints.irq[vector] & 0x1))
	{	
		intc->intvec = intc->queue.intno[intc->queue.top];

		if (!intc->intreq)
		{
			SIMUL_SetPort(processor, SIMUL_PORT_INTERRUPT, 1, &data);
			intc->intreq |= 0x1;
		}
	}

	intc->queue.intno[intc->queue.top] = -1;

	for (time = 1; time <= 4; time++)
	{
		intc->queue.top = (intc->queue.top + 1) & 0x3;
		if (intc->queue.intno[intc->queue.top] == -1)
			continue;

		intc->inttime = time;
		SIMUL_StartTimer(processor, intc->irtimer, SIMUL_TIMER_REL | SIMUL_TIMER_CLOCKS, &intc->inttime);
		break;
	}

	return SIMUL_TIMER_OK;
}

static int SIMULAPI LifoPop_Timer(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
	intCtrl *intc = (intCtrl*) private;

	if (intc->lifo.items)
	{
		intc->regs.cpr = intc->lifo.data[intc->lifo.top];
		intc->lifo.top = (intc->lifo.top) ? (intc->lifo.top - 1) : (LIFO_TOP - 1);
		if (intc->lifo.items)
			intc->lifo.items--;
	}
	else
		intc->regs.cpr = 0x0;

	CheckQueue(processor, intc);
	return SIMUL_TIMER_OK;
}

/* --- Init --- */

static int SIMULAPI INTC_Reset(simulProcessor processor, simulCallbackStruct * cbs, simulPtr private)
{
	intCtrl *intc = (intCtrl*) private;

	intc->intvec = 0;
	intc->intreq = 0;
	intc->inttime = 0;

	memset(&intc->regs, 0x00, sizeof(intc->regs));
	intc->regs.cpr = 0xf;

	memset(&intc->ints, 0x00, sizeof(intc->ints));

	memset(&intc->lifo, 0x00, sizeof(intc->lifo));
	intc->lifo.top = 13;

	memset(&intc->queue.intno, 0xff, sizeof(intc->queue.intno));
	intc->queue.top = 0;

	SIMUL_StopAllTimer(processor);
    return SIMUL_RESET_OK;
}

int SIMULAPI SIMUL_Init(simulProcessor processor, simulCallbackStruct * cbs)
{
	intCtrl * intc;
	int i;

	strcpy(cbs->x.init.modelname, __DATE__ "  MPC5554 Interrupt Ctrl");
    intc = (intCtrl*) SIMUL_Alloc(processor, sizeof(intCtrl));

	intc->bustype = 0;
	intc->startaddress = ADDR_BASE;
	intc->portbase = PORT_BASE;

	for (i = 1; i <= cbs->x.init.argc - 1; i++)
	{
		if (i == 1)
		{
			intc->portbase = cbs->x.init.argpport[1];
			continue;
		}
		else if (i == 2)
		{
			intc->bustype = cbs->x.init.argpbustype[2];
		    intc->startaddress = cbs->x.init.argpaddress[2];
			continue;
		}

		SIMUL_Warning(processor, "usage parameters: [<port offset> [<base address>]]");
		return SIMUL_INIT_FAIL;
	}
	
	/* inits */

	MCR_Init(processor, intc);
	CPR_Init(processor, intc);
	IACKR_Init(processor, intc);
	EOIR_Init(processor, intc);
	SSCIR_Init(processor, intc);
	PSR_Init(processor, intc);
	IntPort_Init(processor, intc);

	intc->irtimer = SIMUL_RegisterTimerCallback(processor, IntReq_Timer, (simulPtr) intc);
	intc->lifotimer = SIMUL_RegisterTimerCallback(processor, LifoPop_Timer, (simulPtr) intc);
	SIMUL_RegisterResetCallback(processor, INTC_Reset, (simulPtr) intc);

	//INTC_Reset(processor, cbs, intc);

	return SIMUL_INIT_OK;
}