/*
    Nios-sim - one simple NIOSII simulator only for personal interest and fun.
    Copyright (C) 2010  chysun2000@gmail.com

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/

#include <stdio.h>
#include "public.h"
#include "niosii.h"
#include "instruction.h"


static uint32_t ldbu(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	int32_t addr = 0;
	
	addr = cpu->gp_regs[instr->a] + (int16_t)(instr->imm16);
	cpu->gp_regs[instr->b] = get_byte(addr) & 0x000000FF;
	
	return PC_INC_NORMAL;
}

static uint32_t addi(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;

	cpu->gp_regs[instr->b] = cpu->gp_regs[instr->a] + (int16_t)(instr->imm16);
	return PC_INC_NORMAL;
}

static uint32_t stb(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	uint32_t addr = 0;

	addr = cpu->gp_regs[instr->a] + (int16_t)(instr->imm16);
	store_byte(addr, (cpu->gp_regs[instr->b] & 0xFF));
	return PC_INC_NORMAL;
}

static uint32_t br(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	int16_t imm16 = (int16_t)instr->imm16;
	
	cpu->pc = cpu->pc + 4 + (int16_t)(imm16 & 0xFFFC);
	return PC_INC_BY_INSTR;
}

static uint32_t ldb(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	uint32_t addr = cpu->gp_regs[instr->a] + (int16_t)(instr->imm16);

	cpu->gp_regs[instr->b] = (int)((char)(get_byte(addr)));
	return PC_INC_NORMAL;
}

static uint32_t cmpgei(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	
	if ((int32_t)(cpu->gp_regs[instr->a]) >= (int32_t)((int16_t)(instr->imm16))){
		cpu->gp_regs[instr->b] = 1;
	}
	else{
		cpu->gp_regs[instr->b] = 0;
	}
	return PC_INC_NORMAL;
}

static uint32_t cmplti(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	
	if ((int32_t)(cpu->gp_regs[instr->a]) < (int32_t)((int16_t)(instr->imm16))){
		cpu->gp_regs[instr->b] = 1;
	}
	else{
		cpu->gp_regs[instr->b] = 0;
	}
	
	return PC_INC_NORMAL;
}

static uint32_t ldhu(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	int16_t offset = (int16_t) instr->imm16;
	int16_t value = 0;
	
	value = get_half_word((int16_t)offset + cpu->gp_regs[instr->a]);
	cpu->gp_regs[instr->b] = ((uint32_t)value) & 0xFFFF;
	return PC_INC_NORMAL;
}

static uint32_t andi(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	uint32_t temp = instr->imm16;
	
	temp = temp & 0xFFFF;
	cpu->gp_regs[instr->b] = cpu->gp_regs[instr->a] & temp;
	return PC_INC_NORMAL;
}

static uint32_t sth(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	int16_t offset = (int16_t)instr->imm16;
	uint32_t addr = 0;
	
	addr = cpu->gp_regs[instr->a] + (int16_t)offset;
	store_half_word(addr, (uint16_t)(cpu->gp_regs[instr->b] & 0xFFFF));
	return PC_INC_NORMAL;
}

static uint32_t bge(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;

	int32_t a = (int32_t)(cpu->gp_regs[instr->a]);
	int32_t b = (int32_t)(cpu->gp_regs[instr->b]);

	if (a >= b){
		cpu->pc = cpu->pc + 4 + (int16_t)((int16_t)instr->imm16 & 0xFFFC);
	}
	else{
		cpu->pc = cpu->pc + 4;
	}

	return PC_INC_BY_INSTR;
}

static uint32_t ldh(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	int16_t value = 0;
	
	value = (int16_t)get_half_word(cpu->gp_regs[instr->a] + (int16_t)(instr->imm16));
	cpu->gp_regs[instr->b] = (int32_t)value;
	return PC_INC_NORMAL;
}

static uint32_t initda(struct NIOS_CPU * cpu, uint32_t code)
{
#if 0
	def_i_type_code;
#endif
	return PC_INC_NORMAL;
}

static uint32_t ori(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	int32_t temp = (int16_t)instr->imm16;
	
	temp = temp & 0xFFFF;
	cpu->gp_regs[instr->b] = cpu->gp_regs[instr->a] | temp;
	return PC_INC_NORMAL;
}

static uint32_t stw(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;

	int16_t offset = (int16_t)(instr->imm16);

	store_word(cpu->gp_regs[instr->a] + (int16_t)offset, cpu->gp_regs[instr->b]);
	return PC_INC_NORMAL;
}

static uint32_t blt(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	int32_t a = (int32_t)(cpu->gp_regs[instr->a]);
	int32_t b = (int32_t)(cpu->gp_regs[instr->b]);
	int16_t imm16 = (int16_t)(instr->imm16);

	if (a < b){
		cpu->pc = cpu->pc + 4 + (int16_t)(imm16 & 0xFFFC);
	}
	else{
		cpu->pc = cpu->pc + 4;
	}
	return PC_INC_BY_INSTR;
}

static uint32_t ldw(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;	
	int16_t offset = (int16_t)(instr->imm16);
	cpu->gp_regs[instr->b] = get_word(cpu->gp_regs[instr->a] + (int16_t)offset);
	return PC_INC_NORMAL;
}

static uint32_t cmpnei(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;	
	int16_t value = (int16_t)(instr->imm16);

	if (cpu->gp_regs[instr->a] != (int16_t)value){
		cpu->gp_regs[instr->b] = 1;
	}
	else{
		cpu->gp_regs[instr->b] = 0;
	}
	return PC_INC_NORMAL;
}

static uint32_t flushda(struct NIOS_CPU * cpu, uint32_t code)
{
#if 0
	def_i_type_code;
#endif	
	return PC_INC_NORMAL;
}

static uint32_t xori(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	uint32_t value = 0;
	
	value = (uint16_t)instr->imm16;
	value &= 0xFFFF;
	cpu->gp_regs[instr->b] = cpu->gp_regs[instr->a] ^ value;
	return PC_INC_NORMAL;
}

static uint32_t bne(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	
	if (cpu->gp_regs[instr->a] != cpu->gp_regs[instr->b]){
		cpu->pc = cpu->pc + 4 + (int16_t)((int16_t)instr->imm16 & 0xFFFC);
	}
	else {
		cpu->pc = cpu->pc + 4;
	}
	
	return PC_INC_BY_INSTR;
}

static uint32_t cmpeqi(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	
	if(cpu->gp_regs[instr->a] == (int32_t)(instr->imm16)){
		cpu->gp_regs[instr->b] = 1;
	}
	else{
		cpu->gp_regs[instr->b] = 0;
	}
	return PC_INC_NORMAL;
}

static uint32_t ldbuio(struct NIOS_CPU * cpu, uint32_t code)
{
	return ldbu(cpu, code);
}

static uint32_t muli(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	unsigned long long data = 0;
	uint32_t a = cpu->gp_regs[instr->a];
	int32_t b = (int32_t)((int16_t)instr->imm16);
	
	data = (a * b);
	cpu->gp_regs[instr->b] = (uint32_t)(data & 0xFFFFFFFF);
	return PC_INC_NORMAL;
}

static uint32_t stbio(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	uint32_t addr = cpu->gp_regs[instr->a] + (int16_t)(instr->imm16);
	
	store_byte(addr, (uint8_t)(cpu->gp_regs[instr->b] & 0xFF));
	return PC_INC_NORMAL;
}

static uint32_t beq(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	
	int16_t temp = (int16_t)instr->imm16;

	if (cpu->gp_regs[instr->a] == cpu->gp_regs[instr->b]){
		cpu->pc = cpu->pc + 4 + (int16_t)(temp & 0xFFFC);
	}
	else{
		cpu->pc = cpu->pc + 4;
	}
	return PC_INC_BY_INSTR;
}

static uint32_t ldbio(struct NIOS_CPU * cpu, uint32_t code)
{
	return ldb(cpu, code);
}

static uint32_t cmpgeui(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	uint32_t value = 0;
	
	value = (uint32_t)(((uint16_t)(instr->imm16)) & 0xFFFF);
	if ((uint32_t)(cpu->gp_regs[instr->a]) >= value){
		cpu->gp_regs[instr->b] = 1;
	}
	else{
		cpu->gp_regs[instr->b] = 0;
	}
	return PC_INC_NORMAL;
}

static uint32_t ldhuio(struct NIOS_CPU * cpu, uint32_t code)
{
	return ldhu(cpu, code);
}

static uint32_t andhi(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	int32_t value = 0;
	int32_t temp = (int32_t)(instr->imm16);

	value = temp << 16;
	value = value & 0xFFFF0000;
	cpu->gp_regs[instr->b] = cpu->gp_regs[instr->a] & value;
	return PC_INC_NORMAL;
}

static uint32_t sthio(struct NIOS_CPU * cpu, uint32_t code)
{
	return sth(cpu, code);
}

static uint32_t bgeu(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	
	if (cpu->gp_regs[instr->a] >= cpu->gp_regs[instr->b]){
		cpu->pc = cpu->pc + 4 + (int16_t)((int16_t)(instr->imm16) & 0xFFFC);
	}
	else{
		cpu->pc = cpu->pc + 4;
	}
	return PC_INC_BY_INSTR;
}

static uint32_t ldhio(struct NIOS_CPU * cpu, uint32_t code)
{
	return ldh(cpu, code);
}

static uint32_t cmpltui(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	
	if (cpu->gp_regs[instr->a] < ((uint32_t)((uint16_t)instr->imm16) & 0xFFFF)){
		cpu->gp_regs[instr->b] = 1;
	}
	else{
		cpu->gp_regs[instr->b] = 0;
	}
	return PC_INC_NORMAL;
}

static uint32_t custom(struct NIOS_CPU * cpu, uint32_t code)
{
	return custom_instr(cpu, code);
}

static uint32_t initd(struct NIOS_CPU * cpu, uint32_t code)
{
	return PC_INC_NORMAL;
}

static uint32_t orhi(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	
	cpu->gp_regs[instr->b] = cpu->gp_regs[instr->a] | (((uint32_t)((uint16_t)instr->imm16) << 16) & 0xFFFF0000);
	return PC_INC_NORMAL;
}

static uint32_t stwio(struct NIOS_CPU * cpu, uint32_t code)
{
	return stw(cpu, code);
}

static uint32_t bltu(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;

	if (cpu->gp_regs[instr->a] < cpu->gp_regs[instr->b]){
		cpu->pc = cpu->pc + 4 + (int16_t)((int16_t)instr->imm16 & 0xFFFC);
	}
	else{
		cpu->pc = cpu->pc + 4;
	}
	return PC_INC_BY_INSTR;
}

static uint32_t ldwio(struct NIOS_CPU * cpu, uint32_t code)
{
	return ldw(cpu, code);
}

static uint32_t rdprs(struct NIOS_CPU * cpu, uint32_t code)
{
	return PC_INC_NORMAL;
}

static uint32_t flushd(struct NIOS_CPU * cpu, uint32_t code)
{
	return PC_INC_NORMAL;
}

static uint32_t xorhi(struct NIOS_CPU * cpu, uint32_t code)
{
	def_i_type_code;
	uint32_t value = (uint16_t)(instr->imm16);
	
	value = value << 16;
	value = value & 0xFFFF0000;
	cpu->gp_regs[instr->b] = cpu->gp_regs[instr->a] ^ value;
	return PC_INC_NORMAL;
}

/*----------------------------------------------------------------------------*/
#define I_TYPE_HANDLER_COUNT (0x40)
struct i_type_handler i_type_handlers[I_TYPE_HANDLER_COUNT] = {
	[LDBU] = handler_item(ldbu),
	[ADDI] = handler_item(addi),
	[STB] = handler_item(stb),
	[BR] = handler_item(br),
	[LDB] = handler_item(ldb),
	[CMPGEI] = handler_item(cmpgei),
	[LDHU] = handler_item(ldhu),
	[ANDI] = handler_item(andi),
	[STH] = handler_item(sth),
	[BGE] = handler_item(bge),
	[LDH] = handler_item(ldh),
	[CMPLTI] = handler_item(cmplti),
	[INITDA] = handler_item(initda),
	[ORI] = handler_item(ori),
	[STW] = handler_item(stw),
	[BLT] = handler_item(blt),
	[LDW] = handler_item(ldw),
	[CMPNEI] = handler_item(cmpnei),
	[FLUSHDA] = handler_item(flushda),
	[XORI] = handler_item(xori),
	[BNE] = handler_item(bne),
	[CMPEQI] = handler_item(cmpeqi),
	[LDBUIO] = handler_item(ldbuio),
	[MULI] = handler_item(muli),
	[STBIO] = handler_item(stbio),
	[BEQ] = handler_item(beq),
	[LDBIO] = handler_item(ldbio),
	[CMPGEUI] = handler_item(cmpgeui),
	[LDHUIO] = handler_item(ldhuio),
	[ANDHI] = handler_item(andhi),
	[STHIO] = handler_item(sthio),
	[BGEU] = handler_item(bgeu),
	[LDHIO] = handler_item(ldhio),
	[CMPLTUI] = handler_item(cmpltui),
	[CUSTOM] = handler_item(custom),
	[INITD] = handler_item(initd),
	[ORHI] = handler_item(orhi),
	[STWIO] = handler_item(stwio),
	[BLTU] = handler_item(bltu),
	[LDWIO] = handler_item(ldwio),
	[RDPRS] = handler_item(rdprs),
	[FLUSHD] = handler_item(flushd),
	[XORHI] = handler_item(xorhi)
};

/*----------------------------------------------------------------------------*/