/**************************************************************************
 *                                                                        *
 *               Copyright (C) 1994, Silicon Graphics, Inc.               *
 *                                                                        *
 *  These coded instructions, statements, and computer programs  contain  *
 *  unpublished  proprietary  information of Silicon Graphics, Inc., and  *
 *  are protected by Federal copyright  law.  They  may not be disclosed  *
 *  to  third  parties  or copied or duplicated in any form, in whole or  *
 *  in part, without the prior written consent of Silicon Graphics, Inc.  *
 *                                                                        *
 *************************************************************************/

/*
 * File:	vadd.c
 * Creator:	hsa@sgi.com
 * Create Date:	Tue Mar  8 10:59:15 PST 1994
 *
 * This file holds the add instructions for the VU.
 *
 */

#include <stdio.h>
#include "rsp.h"
#include "i128.h"
#include "rspctl.h"
#include "opcode.h"
#include "vu.h"
#include "trace_print.h"

#define ACC_LOW(indx,val)       ((rsp_ACC[indx] & ~0xffffLL) | (val & 0xffff))

rsp_vuPipe_t	vu_AddPipe[rsp_VUPIPEDEPTH+1];

/* 16-bit vector add */
static void
vadd(rsp_vuPipe_t *mp)
{
    int		element, i, carry;
    i16		si, ti;
    i32		di;

    for (i=0; i<8; i++) {

	if (rsp_VCO & (0x01 << i))
	    carry = 1;
	else
	    carry = 0;

	element = ((i & cmask_tab[(mp->format & 0x0e) >> 1]) + 
		   (mp->format & emask_tab[(mp->format & 0x0e) >> 1]));

	si = (i16) Get128By16(&(rsp_VUR[mp->rs]), i);
	ti = (i16) Get128By16(&(rsp_VUR[mp->rt]), element);
	di = si + ti + carry;

	rsp_ACC[i] = ACC_LOW(i,di); /* HW compatibility */

	if (di > 0x00007fff)
	    di = 0x00007fff;
	if (di < -32768)
	    di = -32768;
	Set128By16(&(mp->result), (u16) di, i);
	rsp_VCO = UnFlag(rsp_VCO, (0x0101 << i));
    }
}

/* 16-bit vector sub */
static void
vsub(rsp_vuPipe_t *mp)
{
    int		element, i, borrow;
    i16		si, ti;
    i32		di;

    for (i=0; i<8; i++) {

	if (rsp_VCO & (0x01 << i))
	    borrow = 1;
	else
	    borrow = 0;

	element = ((i & cmask_tab[(mp->format & 0x0e) >> 1]) + 
		   (mp->format & emask_tab[(mp->format & 0x0e) >> 1]));

	si = (i16) Get128By16(&(rsp_VUR[mp->rs]), i);
	ti = (i16) Get128By16(&(rsp_VUR[mp->rt]), element);

	di = si;
	di = di - ti - borrow;

	rsp_ACC[i] = ACC_LOW(i,di); /* HW compatibility */

	if (di > 0x00007fff)
	    di = 0x00007fff;
	if (di < -32768)
	    di = -32768;
	Set128By16(&(mp->result), (u16) di, i);
	rsp_VCO = UnFlag(rsp_VCO, (0x0101 << i));
    }
}

/* 16-bit vector sub (allow scaler - vector) */
static void
vsut(rsp_vuPipe_t *mp)
{
    int		element, i, borrow;
    i16		si, ti;
    i32		di;

    for (i=0; i<8; i++) {

	if (rsp_VCO & (0x01 << i))
	    borrow = 1;
	else
	    borrow = 0;

	element = ((i & cmask_tab[(mp->format & 0x0e) >> 1]) + 
		   (mp->format & emask_tab[(mp->format & 0x0e) >> 1]));

	si = (i16) Get128By16(&(rsp_VUR[mp->rs]), i);
	ti = (i16) Get128By16(&(rsp_VUR[mp->rt]), element);

	di = ti;
	di = di - borrow - si;

        rsp_ACC[i] = ACC_LOW(i,di); /* HW compatibility */

	if (di > 0x00007fff)
	    di = 0x00007fff;
	if (di < -32768)
	    di = -32768;
	Set128By16(&(mp->result), (u16) di, i);
	rsp_VCO = UnFlag(rsp_VCO, (0x01 << i));

    }
}

/* 16-bit vector abs */
static void
vabs(rsp_vuPipe_t *mp)
{
    int		element, i;
    i16		si, ti;
    i32		di;

    for (i=0; i<8; i++) {

	element = ((i & cmask_tab[(mp->format & 0x0e) >> 1]) + 
		   (mp->format & emask_tab[(mp->format & 0x0e) >> 1]));

	si = (i16) Get128By16(&(rsp_VUR[mp->rs]), i);
	ti = (i16) Get128By16(&(rsp_VUR[mp->rt]), element);

	if (si < 0)
	    di = -ti;
	else if (si == 0)
	    di = 0;
	else
	    di = ti;

        rsp_ACC[i] = ACC_LOW(i,di); /* HW compatibility */

	if (di > 0x00007fff)
	    di = 0x00007fff;
	if (di < -32768)
	    di = -32768;
	Set128By16(&(mp->result), (u16) di, i);
    }
}

/* 16-bit vector add, no clamp */
static void
vaddc(rsp_vuPipe_t *mp)
{
    int		element, i;
    u16		si, ti;
    u32 	di;

    rsp_VCO = 0x0; /*clear VCO*/
    for (i=0; i<8; i++) {

	element = ((i & cmask_tab[(mp->format & 0x0e) >> 1]) + 
		   (mp->format & emask_tab[(mp->format & 0x0e) >> 1]));

	si = (u16) Get128By16(&(rsp_VUR[mp->rs]), i);
	ti = (u16) Get128By16(&(rsp_VUR[mp->rt]), element);

	di = si + ti;

        rsp_ACC[i] = ACC_LOW(i,di); /* HW compatibility */

	rsp_VCO |= 0x0000<<i;       /* set not-equal bits*/
	rsp_VCO |= (di > 65535)<<i; /* set carry out bits*/

	Set128By16(&(mp->result), (u16) di, i);
    }
}

/* 16-bit vector sub, no clamp */
static void
vsubc(rsp_vuPipe_t *mp)
{
    int		element, i;
    i32		di;
    u16		si,ti;

    rsp_VCO = 0x0; /*clear VCO*/

    for (i=0; i<8; i++) {

	element = ((i & cmask_tab[(mp->format & 0x0e) >> 1]) + 
		   (mp->format & emask_tab[(mp->format & 0x0e) >> 1]));

	si = (u16) Get128By16(&(rsp_VUR[mp->rs]), i);
	ti = (u16) Get128By16(&(rsp_VUR[mp->rt]), element);

	di = si - ti;

        rsp_ACC[i] = ACC_LOW(i,di); /* HW compatibility */

	     if (di<0) rsp_VCO |= (0x0101<<i);  /*(vs<vt)  set !Equal,  BorrowIn */
	else if (di>0) rsp_VCO |= (0x0100<<i);  /*(vs>vt)  set !Equal, !BorrowIn */
	else           rsp_VCO |= (0x0000<<i);  /*(vs==vt) set  Equal,  BorrowIn */

	Set128By16(&(mp->result), (u16) di, i);
    }
}

/* 8-bit vector add */
static void
vaddb(rsp_vuPipe_t *mp)
{
    int		element, i, round;
    i32		di, si, ti;
    i32		di2;

    element = mp->format & 0x07;

    if( element == 0 )
	round = 0;
    else
	round = 1 << (element - 1);

#ifdef DEBUG_ADDB
    printf("VADDB[%d] :\n\t", element );
    Print128( stdout, rsp_VUR[mp->rs] );
    printf("  +\n\t");
    Print128( stdout, rsp_VUR[mp->rt] );
    printf("  =\n\t");
#endif

    /* vector of 8-bit elements */
    for (i=0; i<8; i++) {
	si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i);
	ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i);
	di = si + ti + round;

	si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i+1);
	ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i+1);
	di2 = si + ti + round;

	rsp_ACC[i] = ((u32)(di<<16) + (u32)di2);

	di >>= element;
	if (di > 255)
	    di = 255;
	if (di < 0)
	    di = 0;
	Set128By8(&(mp->result), (u8) di, 2*i);

	di2 >>= element;
	if (di2 > 255)
	    di2 = 255;
	if (di2 < 0)
	    di2 = 0;
	Set128By8(&(mp->result), (u8) di2, 2*i+1);
    }

#ifdef DEBUG_ADDB
    Print128( stdout, mp->result );
    printf("\n");
#endif
}

/* 8-bit vector sub */
static void
vsubb(rsp_vuPipe_t *mp)
{
    int		i, element, round;
    i32		di, si, ti;
    i32		di2;

    element = mp->format & 0x07;

    if( element == 0 )
	round = 0;
    else
	round = 1 << (element - 1);

#ifdef DEBUG_ADDB
    printf("VSUBB[%d] :\n\t", element );
    Print128( stdout, rsp_VUR[mp->rs] );
    printf("  -\n\t");
    Print128( stdout, rsp_VUR[mp->rt] );
    printf("  =\n\t");
#endif

    /* vector of 8-bit elements */
    for (i=0; i<8; i++) {
	si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i);
	ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i);
	di = si - ti + round;

	si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i+1);
	ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i+1);
	di2 = si - ti + round;

	rsp_ACC[i] = ((u32)(di&0xffff)<<16) + (u32)(di2&0xffff);

	di >>= element;
	if (di > 255)
	    di = 255;
	if (di < 0)
	    di = 0;
	Set128By8(&(mp->result), (u8) di, 2*i);

	di2 >>= element;
	if (di2 > 255)
	    di2 = 255;
	if (di2 < 0)
	    di2 = 0;
	Set128By8(&(mp->result), (u8) di2, 2*i+1);
    }

#ifdef DEBUG_ADDB
    Print128( stdout, mp->result );
    printf("\n");
#endif
}

/* 8-bit vector avg */
static void
vaccb(rsp_vuPipe_t *mp)
{
    int		i, element;
    i32		di, si, ti, ri;
    i32		di2;

    element = mp->format & 0x07;

#ifdef DEBUG_ADDB
    printf("VACCB[%d] :\n\t", element );
    Print128( stdout, rsp_VUR[mp->rs] );
    printf("  +\n\t");
    Print128( stdout, rsp_VUR[mp->rt] );
    printf("  + ACC =\n\t");
#endif

    /* vector of 8-bit elements */
    for (i=0; i<8; i++) {


	ri = rsp_ACC[i]>>16;
	ri <<= 16;
	ri >>= 16;
	si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i);
	ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i);
	di = si + ti + ri;


	ri = rsp_ACC[i] & 0xffff;
	ri <<= 16;
	ri >>= 16;
	si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i+1);
	ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i+1);
	di2 = si + ti + ri;

	rsp_ACC[i] = ((u32) (di&0xffff)<<16) + (u32) (di2&0xffff);

	di >>= element;
	if (di > 255)
	    di = 255;
	if (di < 0)
	    di = 0;
	Set128By8(&(mp->result), (u8) di, 2*i);

	di2 >>= element;
	if (di2 > 255)
	    di2 = 255;
	if (di2 < 0)
	    di2 = 0;
	Set128By8(&(mp->result), (u8) di2, 2*i+1);
    }

#ifdef DEBUG_ADDB
    Print128( stdout, mp->result );
    printf("\n");
#endif
}


/* 8-bit vector avg */
static void
vsucb(rsp_vuPipe_t *mp)
{
    int		i, element;
    i32		di, si, ti, ri;
    i32		di2;

    element = mp->format & 0x07;

#ifdef DEBUG_ADDB
    printf("VSUCB[%d] :\n\t", element );
    Print128( stdout, rsp_VUR[mp->rs] );
    printf("  -\n\t");
    Print128( stdout, rsp_VUR[mp->rt] );
    printf("  + ACC =\n\t");
#endif

    /* vector of 8-bit elements */
    for (i=0; i<8; i++) {

	ri = rsp_ACC[i]>>16;
	ri <<= 16;
	ri >>= 16;
	si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i);
	ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i);
	di = si - ti + ri;

	ri = rsp_ACC[i] & 0xffff;
	ri <<= 16;
	ri >>= 16;
	si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i+1);
	ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i+1);
	di2 = si - ti + ri;

	rsp_ACC[i] = ((u32) (di&0xffff)<<16) +(u32)(di2&0xffff);

	di >>= element;
	if (di > 255)
	    di = 255;
	if (di < 0)
	    di = 0;
	Set128By8(&(mp->result), (u8) di, 2*i);

	di2 >>= element;
	if (di2 > 255)
	    di2 = 255;
	if (di2 < 0)
	    di2 = 0;
	Set128By8(&(mp->result), (u8) di2, 2*i+1);
    }

#ifdef DEBUG_ADDB
    Print128( stdout, mp->result );
    printf("\n");
#endif
}

static void
vsaw(rsp_vuPipe_t *mp)
{
    int		i, element;
    u16		ri;

    element = mp->format & 0x07;
    for (i=0; i<8; i++) {
	
	if (element==0) ri = (u16) (rsp_ACC[i]>>32) & 0xffff;
	if (element==1) ri = (u16) (rsp_ACC[i]>>16) & 0xffff;
	if (element==2) ri = (u16) (rsp_ACC[i]>>0) & 0xffff;

	Set128By16(&(mp->result), ri, i);
	}
}


/* 8-bit vector sad */
static void
vsad(rsp_vuPipe_t *mp)
{
    int         i,j;
    i32         si, ti, diffL, diffH;

    for (i=0; i<8; i++) {

        si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i);
        ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i);
        diffL = si - ti;
        if (diffL < 0) diffL =  -diffL;

        si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i+1);
        ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i+1);
        diffH = si - ti;
        if (diffH < 0) diffH =  -diffH;

	rsp_ACC[i] =  diffL + diffH;

        si = (i16) Get128By16(&(rsp_VUR[mp->rs]), i);
	Set128By16(&(mp->result), (u16) si, i);
	}

}

/* 8-bit vector sac */
static void
vsac(rsp_vuPipe_t *mp)
{
    int         i,j;
    i32         si, ti, diffL, diffH, tacc;

    for (i=0; i<8; i++) {

        si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i);
        ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i);
        diffL = si - ti;
        if (diffL < 0) diffL = -diffL;

        si = (i32) Get128By8(&(rsp_VUR[mp->rs]), 2*i+1);
        ti = (i32) Get128By8(&(rsp_VUR[mp->rt]), 2*i+1);
        diffH = si - ti;
        if (diffH < 0) diffH = -diffH;


        tacc =  rsp_ACC[i]&0xffff;
	rsp_ACC[i] = diffL + diffH + tacc;

        si = (i16) Get128By16(&(rsp_VUR[mp->rs]), i);
	Set128By16(&(mp->result), (u16) si, i);
	}

}

/* 8-bit vector sum */
static void
vsum(rsp_vuPipe_t *mp)
{
    int         i;
    i32         di;

    di = 0;

    for (i=0; i<8; i++)  di = di + (i32) rsp_ACC[i]&0xffff;

    Set128By16(&(mp->result), (u16) di, 0);

}



/*
 * decode and execute a Add instruction
 */
static void
rsp_VUAddExec(rsp_vuPipe_t *mp)
{
    int		opKey;

    /* decode instruction */
    opKey = ExtractBits(mp->inst, 5, 0);
    mp->format = ExtractBits(mp->inst, 24, 21);
    mp->rt = ExtractBits(mp->inst, 20, 16);
    mp->rs = ExtractBits(mp->inst, 15, 11);
    mp->rd = ExtractBits(mp->inst, 10,  6);
    rsp_VURegLock(mp->rd, mp->pc);

    /* check for stall. If okay, execute and decrement delay field */
    if (rsp_VURegIsLocked(mp->rt, mp->pc) || rsp_VURegIsLocked(mp->rs, mp->pc)) {
	/* can't do anything right now... */
	mp->stalled = TRUE;
	rsp_VUStalled = TRUE;
	rsp_Verbose(stderr,"VU add stalled... (%08x)\n",mp->pc);
    } else {
	/* execute */
	switch (opKey) {
	  case rsp_VADD:
	    strcpy(mp->opString, "vadd");
	    vadd(mp);
	    break;

	  case rsp_VSUB:
	    strcpy(mp->opString, "vsub");
	    vsub(mp);
	    break;

	  case rsp_VSUT:
	    strcpy(mp->opString, "vsut");
	    vsut(mp);
	    break;

	  case rsp_VABS:
	    strcpy(mp->opString, "vabs");
	    vabs(mp);
	    break;

	  case rsp_VADDC:
	    strcpy(mp->opString, "vaddc");
	    vaddc(mp);
	    break;

	  case rsp_VSUBC:
	    strcpy(mp->opString, "vsubc");
	    vsubc(mp);
	    break;

	  case rsp_VADDB:
	    strcpy(mp->opString, "vaddb");
	    vaddb(mp);
	    break;

	  case rsp_VSUBB:
	    strcpy(mp->opString, "vsubb");
	    vsubb(mp);
	    break;

	  case rsp_VACCB:
	    strcpy(mp->opString, "vaccb");
	    vaccb(mp);
	    break;

	  case rsp_VSUCB:
	    strcpy(mp->opString, "vsucb");
	    vsucb(mp);
	    break;

	  case rsp_VSAD:
	    strcpy(mp->opString, "vsad");
	    vsad(mp);
	    break;

	  case rsp_VSAC:
	    strcpy(mp->opString, "vsac");
	    vsac(mp);
	    break;

	  case rsp_VSUM:
	    strcpy(mp->opString, "vsum");
	    vsum(mp);
	    break;

	  case rsp_VSAW:
	    strcpy(mp->opString, "vsaw");
	    vsaw(mp);
	    break;

	}

	mp->stalled = FALSE;
	mp->delay--;
    }
}


/* PUBLIC FUNCTIONS */

/*
 * check Add pipeline for stalls
 * returns TRUE is stalled.
 */
boolean
rsp_VUAddCheckStall(void)
{
    int		i;

    for (i=0; i<rsp_VUPIPEDEPTH; i++) {
	if (vu_AddPipe[i].delay > 0 && vu_AddPipe[i].stalled)
	    return TRUE;
    }

    return FALSE;
}

/*
 * install a VU instruction into the pipeline
 */
void
rsp_VUAddInstall(u32 inst, u32 pc)
{
    int		i;

    for (i=0; i<rsp_VUPIPEDEPTH; i++) {
	if (vu_AddPipe[i].delay == 0 || VUZeroPipe) {
	    vu_AddPipe[i].inst = inst;
	    vu_AddPipe[i].pc = pc;
	    vu_AddPipe[i].delay = (VUZeroPipe) ? 1 : rsp_VUPIPEDEPTH;
	    vu_AddPipe[i].stalled = FALSE;
	    strcpy(vu_AddPipe[i].opString, "?");
	    if (VUZeroPipe) rsp_VUAddPipeStep();
	    break;
	}
    }
}

/*
 * this function is called once per clock, advances the add
 * pipeline one step.
 */
boolean
rsp_VUAddPipeStep(void)
{
    int		i;

    /* advance all the things in the pipe */
    for (i=0; i<rsp_VUPIPEDEPTH; i++) {
	if (vu_AddPipe[i].delay > 0) {
	 
	    if (vu_AddPipe[i].delay == rsp_VUPIPEDEPTH-1 || VUZeroPipe) {
		/* do decode and exec, if possible */
		rsp_VUAddExec(&(vu_AddPipe[i]));
	    } else {
		/*
		 * fakes pipelining by waiting to write-back answer
		 */
               if (rsp_VUStalled==FALSE ||
                         vu_AddPipe[i].delay<rsp_VUPIPE_STAGE_EX)
		vu_AddPipe[i].delay--;
	    }

	    if (vu_AddPipe[i].delay == 0) {
		/* do write-back */
		rsp_Verbose(stderr,"VU ADD (%s) did write-back. (v%d)\n",
			    vu_AddPipe[i].opString, vu_AddPipe[i].rd);
		Set128(&(rsp_VUR[vu_AddPipe[i].rd]), &(vu_AddPipe[i].result));

    traceVUbyVU(vu_AddPipe[i].rd,0xff,&(vu_AddPipe[i].result), vu_AddPipe[i].pc);

		/* mark registers not in use */
		rsp_VURegUnLock(vu_AddPipe[i].rd, vu_AddPipe[i].pc);
	    }
	}
    }
}