/*
 * This file is subject to the terms and conditions of the GNU General Public
 * License.  See the file "COPYING" in the main directory of this archive
 * for more details.
 *
 * Copyright (C) 1996, 97, 2000, 2001 by Ralf Baechle
 * Copyright (C) 2001 MIPS Technologies, Inc.
 */
#include <linux/kernel.h>
#include <linux/sched.h>
#include <linux/signal.h>
#include <asm/branch.h>
#include <asm/cpu.h>
#include <asm/cpu-features.h>
#include <asm/fpu.h>
#include <asm/inst_mips16.h>
#include <asm/ptrace.h>
#include <asm/uaccess.h>

/*
 * Compute the return address and do emulate branch simulation, if required.
 */
int __compute_return_epc_mips16(struct pt_regs *regs)
{
	union mips16_instruction insn, next_insn;
    short offset, rx;
	long epc;
    unsigned int test = 0;

	/*--- unsigned int bit, fcr31, dspcontrol; ---*/

	epc = regs->cp0_epc;

    if(read_mips16_insn(&insn, epc&~0x1 ))
        goto sigsegv;
    if(read_mips16_insn(&next_insn, (epc&~0x1) + MIPS16_INSN_SIZE(insn.meta.opcode)))
        goto sigsegv;

	regs->regs[0] = 0;

    switch (insn.meta.opcode) {
#ifdef __BIG_ENDIAN

        case mips16_op_jal:
            regs->regs[31] = epc + 4 + MIPS16_INSN_SIZE(next_insn.meta.opcode);
            epc = (epc+4) & ~((1 << 28) - 1);
            epc |= ( insn.j_format.target0 | 
                    (insn.j_format.target1 << 16) | 
                    (insn.j_format.target2 << 21)) << 2;

            printk(KERN_ERR "[%s] JAL%s 0x%x  =>  GPR=0x%lx, EPC=0x%lx [word=0x%x]\n", __FUNCTION__,
                   insn.j_format.x_mips16 ? "X" : "",
                   insn.j_format.target0 | (insn.j_format.target1 << 16) | (insn.j_format.target2 << 21),
                   regs->regs[31], epc | (0x1 ^ insn.j_format.x_mips16), insn.word);
            regs->cp0_epc = epc | (0x1 ^ insn.j_format.x_mips16);
            break;
        case mips16_op_rr: {
            unsigned int delay_slot_size = 0;
            if(insn.rr_format.offset != mips16_rr_jr)
                goto sigbus;
            switch (insn.rr_format.ry) {
                case mips16_jr_jalr: 
                    delay_slot_size = MIPS16_INSN_SIZE(next_insn.meta.opcode);
                case mips16_jr_jalrc:
                    regs->regs[31] = epc + 2 + delay_slot_size;
                case mips16_jr_jr_rx:
                case mips16_jr_jrc_rx:
                    regs->cp0_epc = regs->regs[insn.rr_format.rx];
                    printk(KERN_ERR "[%s] JR(%d) r%d  =>  JR 0x%lx GPR=0x%lx delay_slot_size=%d [word=0x%x]\n", __FUNCTION__,
                           insn.rr_format.ry, insn.rr_format.rx, regs->regs[insn.rr_format.rx], regs->regs[31], delay_slot_size, insn.word);
                    break;
                case mips16_jr_jr_ra:
                case mips16_jr_jrc_ra:
                    regs->cp0_epc = regs->regs[31];
                    printk(KERN_ERR "[%s] JR(%d) ra  =>  JR 0x%lx [word=0x%x]\n", __FUNCTION__, insn.rr_format.ry, regs->regs[31], insn.word);
                    break;

                default: goto sigbus;
            }
            }

        case mips16_op_beqz: 
            test = 1;
        case mips16_op_bnez:
            get_mips16_rx(&insn, &rx, &offset, 0);
            if(regs->regs[insn.rr_format.rx] ^ test)
                regs->cp0_epc += (offset << 1) + MIPS16_INSN_SIZE(insn.meta.opcode);
            break;

        case mips16_op_b:
            get_mips16_offset(&insn, &offset);
            regs->cp0_epc += (offset << 1) + MIPS16_INSN_SIZE(insn.meta.opcode);
            break;

        case mips16_op_i8:
            switch (insn.rr_format.rx) {
                case mips16_i8_bteqz:
                    test = 1;
                case mips16_i8_btnez:
                    get_mips16_rx(&insn, &rx, &offset, 0);
                    if(regs->regs[24] ^ test)
                        regs->cp0_epc += (offset << 1) + MIPS16_INSN_SIZE(insn.meta.opcode);
                    break;
                default: goto sigbus;
            }
#endif /*--- #ifdef __BIG_ENDIAN ---*/

        default: goto sigbus;
    }
#if 0
	switch (insn.i_format.opcode) {
	/*
	 * jr and jalr are in r_format format.
	 */
	case spec_op:
		switch (insn.r_format.func) {
		case jalr_op:
			regs->regs[insn.r_format.rd] = epc + 8;
			/* Fall through */
		case jr_op:
			regs->cp0_epc = regs->regs[insn.r_format.rs];
			break;
		}
		break;

	/*
	 * This group contains:
	 * bltz_op, bgez_op, bltzl_op, bgezl_op,
	 * bltzal_op, bgezal_op, bltzall_op, bgezall_op.
	 */
	case bcond_op:
		switch (insn.i_format.rt) {
	 	case bltz_op:
		case bltzl_op:
			if ((long)regs->regs[insn.i_format.rs] < 0)
				epc = epc + 4 + (insn.i_format.simmediate << 2);
			else
				epc += 8;
			regs->cp0_epc = epc;
			break;

		case bgez_op:
		case bgezl_op:
			if ((long)regs->regs[insn.i_format.rs] >= 0)
				epc = epc + 4 + (insn.i_format.simmediate << 2);
			else
				epc += 8;
			regs->cp0_epc = epc;
			break;

		case bltzal_op:
		case bltzall_op:
			regs->regs[31] = epc + 8;
			if ((long)regs->regs[insn.i_format.rs] < 0)
				epc = epc + 4 + (insn.i_format.simmediate << 2);
			else
				epc += 8;
			regs->cp0_epc = epc;
			break;

		case bgezal_op:
		case bgezall_op:
			regs->regs[31] = epc + 8;
			if ((long)regs->regs[insn.i_format.rs] >= 0)
				epc = epc + 4 + (insn.i_format.simmediate << 2);
			else
				epc += 8;
			regs->cp0_epc = epc;
			break;
		case bposge32_op:
			if (!cpu_has_dsp)
				goto sigill;

			dspcontrol = rddsp(0x01);

			if (dspcontrol >= 32) {
				epc = epc + 4 + (insn.i_format.simmediate << 2);
			} else
				epc += 8;
			regs->cp0_epc = epc;
			break;
		}
		break;

	/*
	 * These are unconditional and in j_format.
	 */
	case jal_op:
		regs->regs[31] = regs->cp0_epc + 8;
	case j_op:
		epc += 4;
		epc >>= 28;
		epc <<= 28;
		epc |= (insn.j_format.target << 2);
		regs->cp0_epc = epc;
		break;

	/*
	 * These are conditional and in i_format.
	 */
	case beq_op:
	case beql_op:
		if (regs->regs[insn.i_format.rs] ==
		    regs->regs[insn.i_format.rt])
			epc = epc + 4 + (insn.i_format.simmediate << 2);
		else
			epc += 8;
		regs->cp0_epc = epc;
		break;

	case bne_op:
	case bnel_op:+ 
		if (regs->regs[insn.i_format.rs] !=
		    regs->regs[insn.i_format.rt])
			epc = epc + 4 + (insn.i_format.simmediate << 2);
		else
			epc += 8;
		regs->cp0_epc = epc;
		break;

	case blez_op: /* not really i_format */
	case blezl_op:
		/* rt field assumed to be zero */
		if ((long)regs->regs[insn.i_format.rs] <= 0)
			epc = epc + 4 + (insn.i_format.simmediate << 2);
		else
			epc += 8;
		regs->cp0_epc = epc;
		break;

	case bgtz_op:
	case bgtzl_op:
		/* rt field assumed to be zero */
		if ((long)regs->regs[insn.i_format.rs] > 0)
			epc = epc + 4 + (insn.i_format.simmediate << 2);
		else
			epc += 8;
		regs->cp0_epc = epc;
		break;

	/*
	 * And now the FPA/cp1 branch instructions.
	 */
	case cop1_op:
		preempt_disable();
		if (is_fpu_owner())
			asm volatile("cfc1\t%0,$31" : "=r" (fcr31));
		else
			fcr31 = current->thread.fpu.fcr31;
		preempt_enable();

		bit = (insn.i_format.rt >> 2);
		bit += (bit != 0);
		bit += 23;
		switch (insn.i_format.rt & 3) {
		case 0:	/* bc1f */
		case 2:	/* bc1fl */
			if (~fcr31 & (1 << bit))
				epc = epc + 4 + (insn.i_format.simmediate << 2);
			else
				epc += 8;
			regs->cp0_epc = epc;
			break;

		case 1:	/* bc1t */
		case 3:	/* bc1tl */
			if (fcr31 & (1 << bit))
				epc = epc + 4 + (insn.i_format.simmediate << 2);
			else
				epc += 8;
			regs->cp0_epc = epc;
			break;
		}
		break;
	}
#endif

	return 0;

sigbus:
    printk(KERN_ERR "[%s] ERROR - no unaligned handling for instruction 0x%x (opcode=%d%s) at address 0x%x\n",
            __FUNCTION__,  insn.word, insn.meta.opcode, insn.meta.is_extended ? ", extended" : "", (unsigned int)epc);
	force_sig(SIGBUS, current);
	return -EFAULT;
sigsegv:
	printk("[%s] %s: ERROR on reading instruction - sending SIGSEGV.\n", __FUNCTION__, current->comm);
	force_sig(SIGSEGV, current);
	return -EFAULT;
}