#include <melf.h>
#include <stdint.h>
#include <unistd.h>
#include <merror.h>
#include <mips32r6.h>
#include "sim.h"

/* sign extension */
#define SE(n) ((uint32_t)(int16_t)(n))
/* sign extension 64bit */
#define SE64(n) ((uint64_t)(int32_t)(n))
/* signed sign extension 64bit */
#define SSE64(n) ((int64_t)(int32_t)(n))
/* shifted sign extention */
#define SSE(n, s) (SE(n) << (s))
/* zero extension */
#define ZE(n) ((uint32_t)(uint16_t)(n))
/* get vaddr from offset and base */
#define VADDR(sim, ins) \
	((sim->reg[ins.rs] /* base */) + \
	 (SE(ins.offset) /* offset */))
/* gets the low 32 bits of a 64 bit value */
#define LO(n) (((1ULL << 32) - 1) & (n))
/* gets the hi 32 bits of a 64 bit value */
#define HI(n) ((n) >> 32)

/* convert to a pointer */
#define PTR(ptr, type) ((type *)(uintptr_t)(ptr))

static void sim_delay_slot(struct simulator *sim)
{
	if (sim->args->jdelay == false)
		return;

	uint32_t ins = * (uint32_t *) (uintptr_t) sim->pc;
	union mips32_instruction data = { .raw = B32(ins) };
	sim->pc += 4;

	switch (data.op) {
	case MIPS32R6_OP_REGIMM:
	case MIPS32R6_OP_J:
	case MIPS32R6_OP_JAL:
	case MIPS32R6_OP_JALX:
	case MIPS32R6_OP_BEQ:
	case MIPS32R6_OP_BEQL:
	case MIPS32R6_OP_BNE:
	case MIPS32R6_OP_BNEL:
	case MIPS32R6_OP_BGTZ:
	case MIPS32R6_OP_BGTZL:
	case MIPS32R6_OP_BLEZ:
	case MIPS32R6_OP_BLEZL:
		sim_dump(sim, "attempted to execute jump instruction in delay"
			"slot (0b%05b)", data.op);
	default:
	}

	sim_ins(sim, ins);
}

static void sim_ins_special_sop30(struct simulator *sim,
				  union mips32_instruction ins)
{
	switch (ins.shamt) {
	case MIPS32R6_SOP30_MUL:
		sim->reg[ins.rd] = (SSE64(sim->reg[ins.rs]) *
				SSE64(sim->reg[ins.rt])) >> 0;
		break;

	case MIPS32R6_SOP30_MUH:
		sim->reg[ins.rd] = (SSE64(sim->reg[ins.rs]) *
				SSE64(sim->reg[ins.rt])) >> 32;
		break;

	default:
		sim_dump(sim, "unknown sop30 funct (0b%06b)", ins.shamt);
	}
}

static void sim_ins_special_sop31(struct simulator *sim,
				  union mips32_instruction ins)
{
	switch (ins.shamt) {
	case MIPS32R6_SOP31_MULU:
		sim->reg[ins.rd] = (SE64(sim->reg[ins.rs]) *
				SE64(sim->reg[ins.rt])) >> 0;
		break;

	case MIPS32R6_SOP31_MUHU:
		sim->reg[ins.rd] = (SE64(sim->reg[ins.rs]) *
				SE64(sim->reg[ins.rt])) >> 32;
		break;

	default:
		sim_dump(sim, "unknown sop31 funct (0b%06b)", ins.shamt);
	}
}

static void sim_ins_special_sop32(struct simulator *sim,
				  union mips32_instruction ins)
{
	switch (ins.shamt) {
	case MIPS32R6_SOP32_DIV:
		sim->reg[ins.rd] = (signed) sim->reg[ins.rs] /
				(signed) sim->reg[ins.rt];
		break;

	case MIPS32R6_SOP32_MOD:
		sim->reg[ins.rd] = (signed) sim->reg[ins.rs] %
				(signed) sim->reg[ins.rt];
		break;

	default:
		sim_dump(sim, "unknown sop32 funct (0b%06b)", ins.shamt);
	}
}

static void sim_ins_special_sop33(struct simulator *sim,
				  union mips32_instruction ins)
{
	switch (ins.shamt) {
	case MIPS32R6_SOP33_DIVU:
		sim->reg[ins.rd] = sim->reg[ins.rs] / sim->reg[ins.rt];
		break;

	case MIPS32R6_SOP33_MODU:
		sim->reg[ins.rd] = sim->reg[ins.rs] % sim->reg[ins.rt];
		break;

	default:
		sim_dump(sim, "unknown sop33 funct (0b%06b)", ins.shamt);
	}
}

static void sim_ins_special(struct simulator *sim,
			    union mips32_instruction ins)
{
	switch (ins.funct) {
	case MIPS32R6_FUNCT_ADD:
		// TODO: trap on overflow
		sim->reg[ins.rd] = sim->reg[ins.rs] + sim->reg[ins.rt];
		break;

	case MIPS32R6_FUNCT_ADDU:
		sim->reg[ins.rd] = sim->reg[ins.rs] + sim->reg[ins.rt];
		break;

	case MIPS32R6_FUNCT_AND:
		sim->reg[ins.rd] = sim->reg[ins.rs] & sim->reg[ins.rt];
		break;

	case MIPS32R6_FUNCT_SOP30:
		sim_ins_special_sop30(sim, ins);
		break;

	case MIPS32R6_FUNCT_SOP31:
		sim_ins_special_sop31(sim, ins);
		break;

	case MIPS32R6_FUNCT_SOP32:
		sim_ins_special_sop32(sim, ins);
		break;

	case MIPS32R6_FUNCT_SOP33:
		sim_ins_special_sop33(sim, ins);
		break;

	case MIPS32R6_FUNCT_JALR:
		sim->reg[ins.rd] = sim->pc + 4;
		/* fall through */
	case MIPS32R6_FUNCT_JR:
		sim_delay_slot(sim);
		sim->pc = sim->reg[ins.rs];
		break;

	case MIPS32R6_FUNCT_MFHI:
		sim->reg[ins.rd] = sim->hi;
		break;

	case MIPS32R6_FUNCT_MFLO:
		sim->reg[ins.rd] = sim->low;
		break;

	case MIPS32R6_FUNCT_MTHI:
		sim->hi = sim->reg[ins.rd];
		break;

	case MIPS32R6_FUNCT_MTLO:
		sim->low = sim->reg[ins.rd];
		break;

	case MIPS32R6_FUNCT_SLL:
		sim->reg[ins.rd] = sim->reg[ins.rt] << ins.shamt;
		break;

	case MIPS32R6_FUNCT_SLLV:
		sim->reg[ins.rd] = sim->reg[ins.rt] << sim->reg[ins.rs];
		break;

	case MIPS32R6_FUNCT_SLT:
		sim->reg[ins.rd] = (signed) sim->reg[ins.rs] <
				(signed) sim->reg[ins.rt] ? 1 : 0;
		break;

	case MIPS32R6_FUNCT_SLTU:
		sim->reg[ins.rd] = sim->reg[ins.rs] < sim->reg[ins.rt] ? 1 : 0;
		break;

	case MIPS32R6_FUNCT_SRA:
		sim->reg[ins.rd] = (signed) sim->reg[ins.rt] >> ins.shamt;
		break;

	case MIPS32R6_FUNCT_SRAV:
		sim->reg[ins.rd] = (signed) sim->reg[ins.rt] >>
				sim->reg[ins.rs];
		break;

	case MIPS32R6_FUNCT_SRL:
		sim->reg[ins.rd] = sim->reg[ins.rt] >> ins.shamt;
		break;

	case MIPS32R6_FUNCT_SRLV:
		sim->reg[ins.rd] = sim->reg[ins.rt] >> sim->reg[ins.rs];
		break;

	case MIPS32R6_FUNCT_SUB:
		// TODO: trap on overflow
		sim->reg[ins.rd] = sim->reg[ins.rs] - sim->reg[ins.rt];
		break;

	case MIPS32R6_FUNCT_SUBU:
		sim->reg[ins.rd] = sim->reg[ins.rs] - sim->reg[ins.rt];
		break;

	case MIPS32R6_FUNCT_SYSCALL:
		sim->reg[MIPS32_REG_V0] = syscall(
				sim->reg[MIPS32_REG_V0],
				sim->reg[MIPS32_REG_A0],
				sim->reg[MIPS32_REG_A1],
				sim->reg[MIPS32_REG_A2],
				sim->reg[MIPS32_REG_A3]
			);
		break;

	case MIPS32R6_FUNCT_OR:
		sim->reg[ins.rd] = sim->reg[ins.rs] | sim->reg[ins.rt];
		break;

	case MIPS32R6_FUNCT_NOR:
		sim->reg[ins.rd] = !(sim->reg[ins.rs] | sim->reg[ins.rt]);
		break;

	case MIPS32R6_FUNCT_XOR:
		sim->reg[ins.rd] = sim->reg[ins.rs] ^ sim->reg[ins.rt];
		break;

	default:
		sim_dump(sim, "unknown funct (0b%05b)", ins.funct);
	}
}

static void sim_ins_regimm(struct simulator *sim,
			   union mips32_instruction ins)
{
	uint32_t pc = sim->pc;

	switch (ins.bfunct) {
	case MIPS32R6_FUNCT_BGEZAL:
	case MIPS32R6_FUNCT_BGEZALL:
		sim->reg[MIPS32_REG_RA] = sim->pc + 4;
		/* fall through */
	case MIPS32R6_FUNCT_BGEZ:
	case MIPS32R6_FUNCT_BGEZL:
		sim_delay_slot(sim);
		if ((signed) sim->reg[ins.rs] >= 0)
			sim->pc = pc + SSE(ins.offset, 2);
		break;

	case MIPS32R6_FUNCT_BLTZAL:
	case MIPS32R6_FUNCT_BLTZALL:
		sim->reg[MIPS32_REG_RA] = sim->pc + 4;
		/* fall through */
	case MIPS32R6_FUNCT_BLTZ:
	case MIPS32R6_FUNCT_BLTZL:
		sim_delay_slot(sim);
		if ((signed) sim->reg[ins.rs] < 0)
			sim->pc = pc + SSE(ins.offset, 2);
		break;

	default:
		sim_dump(sim, "unknown branch funct (0b%06b)", ins.bfunct);
	}
}

void sim_ins(struct simulator *sim, uint32_t raw)
{
	// get ins parts
	union mips32_instruction ins = {
		.raw = B32(raw)
	};
	uint32_t pc = sim->pc;

	// reset zero reg
	sim->reg[MIPS32_REG_ZERO] = 0;

	switch (ins.op) {
	case MIPS32R6_OP_SPECIAL:
		sim_ins_special(sim, ins);
		break;

	case MIPS32R6_OP_REGIMM:
		sim_ins_regimm(sim, ins);
		break;

	case MIPS32R6_OP_ADDI:
		sim->reg[ins.rt] = (int32_t)sim->reg[ins.rs] +
				SE(ins.immd);
		break;

	case MIPS32R6_OP_ADDIU:
		sim->reg[ins.rt] = sim->reg[ins.rs] + SE(ins.immd);
		break;

	case MIPS32R6_OP_ANDI:
		sim->reg[ins.rt] = sim->reg[ins.rs] & ZE(ins.immd);
		break;

	case MIPS32R6_OP_BALC:
		sim->reg[MIPS32_REG_RA] = sim->pc;
		/* fall through */
	case MIPS32R6_OP_BC:
		sim->pc += SSE(ins.offs26, 2);
		break;

	case MIPS32R6_OP_BEQ:
	case MIPS32R6_OP_BEQL:
		sim_delay_slot(sim);
		if (sim->reg[ins.rs] == sim->reg[ins.rt])
			sim->pc = pc + SSE(ins.offset, 2);
		break;

	case MIPS32R6_OP_BGTZ:
	case MIPS32R6_OP_BGTZL:
		sim_delay_slot(sim);
		if ((signed) sim->reg[ins.rs] <= 0)
			sim->pc = pc + SSE(ins.offset, 2);
		break;

	case MIPS32R6_OP_BLEZ:
	case MIPS32R6_OP_BLEZL:
		sim_delay_slot(sim);
		if ((signed) sim->reg[ins.rs] <= 0)
			sim->pc = pc + SSE(ins.offset, 2);
		break;

	case MIPS32R6_OP_BNE:
	case MIPS32R6_OP_BNEL:
		sim_delay_slot(sim);
		if (sim->reg[ins.rs] != sim->reg[ins.rt])
			sim->pc = pc + SSE(ins.offset, 2);
		break;

	case MIPS32R6_OP_JAL:
		sim->reg[MIPS32_REG_RA] = sim->pc + 4;
		/* fall through */
	case MIPS32R6_OP_J:
		sim_delay_slot(sim);
		sim->pc &= 0xF0000000;
		sim->pc |= ins.target << 2;
		break;

	case MIPS32R6_OP_LB:
		sim->reg[ins.rt] = *PTR(VADDR(sim, ins), int8_t);
		break;

	case MIPS32R6_OP_LBU:
		sim->reg[ins.rt] = *PTR(VADDR(sim, ins), uint8_t);
		break;

	case MIPS32R6_OP_LH:
		sim->reg[ins.rt] = *PTR(VADDR(sim, ins), int16_t);
		break;

	case MIPS32R6_OP_LHU:
		sim->reg[ins.rt] = *PTR(VADDR(sim, ins), uint16_t);
		break;

	case MIPS32R6_OP_LUI:
		sim->reg[ins.rt] = ins.immd << 16;
		break;

	case MIPS32R6_OP_LW:
		sim->reg[ins.rt] = *PTR(VADDR(sim, ins), uint32_t);
		break;

	case MIPS32R6_OP_SB:
		*PTR(VADDR(sim, ins), uint8_t) = sim->reg[ins.rt];
		break;

	case MIPS32R6_OP_SH:
		*PTR(VADDR(sim, ins), uint16_t) = sim->reg[ins.rt];
		break;

	case MIPS32R6_OP_SW:
		*PTR(VADDR(sim, ins), uint32_t) = sim->reg[ins.rt];
		break;

	case MIPS32R6_OP_SLTI:
		sim->reg[ins.rt] = (signed) sim->reg[ins.rs] <
				(signed) SE(ins.immd) ? 1 : 0;
		break;

	case MIPS32R6_OP_SLTIU:
		sim->reg[ins.rt] = sim->reg[ins.rs] < SE(ins.immd) ? 1 : 0;
		break;

	case MIPS32R6_OP_ORI:
		sim->reg[ins.rt] = sim->reg[ins.rs] | ins.immd;
		break;

	case MIPS32R6_OP_XORI:
		sim->reg[ins.rt] = sim->reg[ins.rs] ^ ins.immd;
		break;

	default:
		sim_dump(sim, "unknown op code (0b%05b)", ins.op);
	}
}