#include <PR/bbnvram.h>
#include "ultra64.h"
#include  <PR/bcp.h>

#include "nvram.h"

/* fill virage data with data to be written, then 
   call using:  nms_store_and_verify(1, VIRAGE0_CTRL_REG, 16); or
    nms_store_and_verify(1, VIRAGE1_CTRL_REG, 16); or
    nms_store_and_verify(1, VIRAGE2_CTRL_REG, 64);
*/

static int nms_store(int ctrl_reg);

static void
__delay(count) {
    int i, dummy = 0;
    for(i=0; i< count; i++){
	dummy++;
    }
}

static void
nms_tune(int ctrl_reg) {
#define NMS_REG(base,reg)	((base)+((reg)-(VIRAGE0_CTRL_REG&0xffff0000)))
    int base = ctrl_reg & 0xffff0000;
    IO_WRITE(NMS_REG(base, VIRAGE0_NMS_CRSTO_0_REG), 0x8a);
    IO_WRITE(NMS_REG(base, VIRAGE0_NMS_CRSTO_1_REG), 0x13);
    IO_WRITE(NMS_REG(base, VIRAGE0_NMS_CRM_0_REG), 0x80);
    IO_WRITE(NMS_REG(base, VIRAGE0_NMS_CRM_1_REG), 0x92);
    IO_WRITE(NMS_REG(base, VIRAGE0_NMS_CRM_2_REG), 0x18);
    IO_WRITE(NMS_REG(base, VIRAGE0_NMS_CRM_3_REG), 0x05);
#undef NMS_REG
}

void
nms_init(void) {
    IO_WRITE(VIRAGE2_CTRL_REG, 0);	/* take v2 out of bypass */
    nms_tune(VIRAGE0_CTRL_REG);
    nms_tune(VIRAGE1_CTRL_REG);
    nms_tune(VIRAGE2_CTRL_REG);
}


int nms_store_and_verify(int ctrl_reg, const void* data, int size)
{
    int x;
    int i;
    int array_addr = ctrl_reg & 0xffff0000;
    const unsigned int* virage_data = data;
    
    int sysclk = get_sys_clk_ns();

    /*
     * Make sure the NMS is not bypassed
     */
    IO_WRITE(ctrl_reg, 0x0);

    /*
     * This should be set to give 1uSec. It uses sysclock as input. Wait
     * a little afterwards to make sure a few clocks of time-base go through
     */
    IO_WRITE(MI_SEC_VTIMER_REG, 1000/sysclk + 1);

    __delay((4*160000)/sysclk);
    /*
    BCP_STALL((4 * 160000)/sysclk);
    */
    /*
     * Wait for Charge pump to exit PORST
     */
    x = IO_READ(ctrl_reg);
    if (x & VIRAGE_CTRL_CP_PORST) {
        
	__delay(20000/sysclk);
        /*
        BCP_STALL(sysclk_scale *20000/sysclk);
        */
	__delay(20000/sysclk);
        
        x = IO_READ(ctrl_reg);
        if (x & VIRAGE_CTRL_CP_PORST) {
            /* power on reset not reset */
            return FAIL;
        }
    }
    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NMS_READY) == 0) {
        /* not ready */        
        return FAIL;
    }

    for (i=0; i<size; i++) {
        IO_WRITE(array_addr+(i<<2), virage_data[i]); 
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            /* problem storing in array */
            return FAIL;
        }
    }
    

      if (nms_store(ctrl_reg) != PASS) {
        return FAIL;
      }

    /*
     * remove old data: zero out
     */
    for (i=0; i<size; i++) {
        IO_WRITE(array_addr+(i<<2), 0); 
        x = IO_READ(array_addr+(i<<2));
        if (x != 0) {
            /* couldnt zero sram */
            return FAIL;
        }
    }

    if (nms_recall(ctrl_reg) != PASS) {
        /* recall did not pass */
        return FAIL;
    }
    
    
    /*
     * Check all values
     */
    for (i=0; i<size; i++) {
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            /* data not same as that written */
            return FAIL;
        }
    }
/* data written, verified correct */

    return PASS;
}

/*
 * Get the system clock and scale it by some factor to speed up the NoVEA operations
 */
int get_sys_clk_ns(void)
{
    int clk = (IO_READ(PI_GPIO_REG) & PI_ID_SYS_CLOCK_MASK) >> PI_ID_SYS_CLOCK_SHIFT;
    if (clk == PI_ID_SYS_CLOCK_62_5)
	return 1000/62.5;
    else if (clk == PI_ID_SYS_CLOCK_80)
	return 1000/80;
    else
	return 1000/96;
}


int nms_recall(int ctrl_reg)
{
    int x;
    int nms_ctrl_reg = ctrl_reg | 0x2000;
    int sysclk = get_sys_clk_ns();
    
    /*
     * Issue a recall to make sure it got stored
     */
    IO_WRITE(nms_ctrl_reg, NMS_CMD_RECALL << VIRAGE_CTRL_NMS_CMD_SHIFT);

    /*
     * Wait for the RCready time
     */
    __delay((EXTRA_WAIT + Trcready/sysclk)*DELAY_MULT);
/*
    BCP_STALL(EXTRA_WAIT + Trcready/sysclk);

*/
    /*
     * Extra wait since we are going through NMS - Don't have a timing spec here
     */
/*
    BCP_STALL((MORE_EXTRA_WAIT) + Trcready/sysclk);
*/
    __delay((MORE_EXTRA_WAIT + Trcready/sysclk)*DELAY_MULT);

    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NMS_READY) == 0) {
        /* nms not ready */
        return FAIL;
    }

    return PASS;
}


/* this version does not set CRST* registers */

static int nms_store(int ctrl_reg)
{
    u32 x;
    int nms_ctrl_reg = ctrl_reg | 0x2000;

    IO_WRITE(nms_ctrl_reg, NMS_CMD_STORE << VIRAGE_CTRL_NMS_CMD_SHIFT);   
    
    __delay(100);

    x = IO_READ(ctrl_reg);

    if (x & VIRAGE_CTRL_NMS_READY) {
        /* read not low */
        return FAIL;
    }
    
    while(((x = IO_READ(ctrl_reg)) & VIRAGE_CTRL_NMS_READY) == 0){
	__delay(100);
    }
    
    if ((x & VIRAGE_CTRL_NMS_PASS) == 0) {   
        return FAIL;
    }
    return PASS;
}