/**********************************************
 *
 * nvram_test.c
 *
 * Collection of Virage NVRAM tests for IOSIM
 *
 **********************************************/
#include <stdio.h>
#include <PR/bcp.h>
#include <stdlib.h>
#include <sys/times.h>
#include <PR/ultratypes.h>
#include "PR/bcp.h"
#include "PR/bbnvram.h"
#include "simipc.h"
#include "iomap.h"
#include "bcptest.h"
#include "bcp_util.h"
#include "simipc.h"
#include "iotest.h"
#include "nvram_test.h"
#include "nvram.h"


#define V2_DEFAULT_VALUE 0x0

/*
 * Temporary storage which should match the contents for the most
 * recent store
 */
static int virage_data[64];


/*
 * Not sure how to do self-checking here. Can only put a monitor in and check
 * the register gets written internally
 */
int cp_vpp_test(int ctrl_reg)
{
    /*
     * Cycle through all VPP range and VPP select on the charge pump
     *
     * Range is 0
     */
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 0);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 1<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 2<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 3<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 4<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 5<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 6<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 7<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 8<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 9<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 10<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 11<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 12<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 13<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 14<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | 15<<VIRAGE_CTRL_CP_VPPSEL_SHIFT);

    /*
     * Range is 1
     */
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (1<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (2<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (3<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (4<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (5<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (6<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (7<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (8<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (9<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (10<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (11<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (12<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (13<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (14<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_VRANGE | 
             (15<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));

    return PASS;
}

int cp_reset_unlock_test(int sysclk_scale, int ctrl_reg)
{
  int x;
    int cp_ctrl_reg = ctrl_reg | 0x1000;
    int sysclk = get_sys_clk_ns(sysclk_scale);

    fprintf(LogFp, "CP control reg 0x%x\n", cp_ctrl_reg);
    fprintf(LogFp, "sysclk scale = %d\n", sysclk_scale);

    /*
     * Wait for Charge pump to exit PORST
     */
    if (x & VIRAGE_CTRL_CP_PORST) {
        BCP_STALL(sysclk_scale *20000/sysclk);
        if (x & VIRAGE_CTRL_CP_PORST) {
            fprintf(LogFp, "Timeout waiting for CP PORST\n");
            return FAIL;
        }
    }

    /*
     * Now reset the pump
     */
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_RESET);
    BCP_STALL(1 + Trh/sysclk);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS);

    /*
     * Wait some clocks before clocking
     */
    BCP_STALL(1 + Trrc/sysclk);

    /*
     * Write the unlock sequence
     */
    IO_WRITE(cp_ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_DATA);
    IO_WRITE(cp_ctrl_reg, VIRAGE_CTRL_NMS_BYPASS);
    IO_WRITE(cp_ctrl_reg, VIRAGE_CTRL_NMS_BYPASS);
    IO_WRITE(cp_ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_DATA);
    IO_WRITE(cp_ctrl_reg, VIRAGE_CTRL_NMS_BYPASS);
    IO_WRITE(cp_ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_DATA);
    IO_WRITE(cp_ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_CP_DATA);
    IO_WRITE(cp_ctrl_reg, VIRAGE_CTRL_NMS_BYPASS);

    /*
     * Now wait for unlock - should only be 9 nS at worst
     */
    BCP_STALL(2);
    x = IO_READ(ctrl_reg);
    print_ctrl(LogFp, x);

    if ((x & VIRAGE_CTRL_CP_UNLOCK) == 0) {
        fprintf(LogFp, "Pump unlock failed\n");
        return FAIL;
    }

    return PASS;
}

/*
 * Do a single compare operation
 */
int nv_compare_test(int sysclk_scale, int ctrl_reg, int mrcl, int tecc, int bias)
{
    int x;
    int sysclk = get_sys_clk_ns(sysclk_scale);

    fprintf(LogFp, "NV Compare 0x%x\n", ctrl_reg);

    /*
     * Bus config - MRCL, TECC and BIAS
     */
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | (mrcl<<VIRAGE_CTRL_NV_MRCL_SHIFT) | 
             (tecc<<VIRAGE_CTRL_NV_TECC_SHIFT) | (bias<<VIRAGE_CTRL_NV_BIAS_SHIFT));
    /*
     * Wait for stable bus
     */
    BCP_STALL(Tscomp/sysclk);
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_NV_COMP |
             (mrcl<<VIRAGE_CTRL_NV_MRCL_SHIFT) | 
             (tecc<<VIRAGE_CTRL_NV_TECC_SHIFT) |
             (bias<<VIRAGE_CTRL_NV_BIAS_SHIFT));

    /*
     * Wait for RCREADY
     */
    BCP_STALL(EXTRA_WAIT + Trcready/sysclk);
    

    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NV_RCREADY) == 0) {
        fprintf(LogFp, "NV Compare: Timeout failure\n");
        return FAIL;
    }

    if ((x & VIRAGE_CTRL_NV_MATCH) == 0) {
        fprintf(LogFp, "NV Compare: Match failure\n");
        return FAIL;
    }

    return PASS;

}

int nv_margin_compare_test(int sysclk_scale, int ctrl_reg)
{
    int x;
    int n_failure = 0;

    fprintf(LogFp, "NV Margin compare test 0x%x\n", ctrl_reg);

    /*
     * Try the 5 margin compare tests
     */
    
    if (nv_compare_test(sysclk_scale, ctrl_reg, 0, 0, 1) != PASS) {
        fprintf(LogFp, "Compare 1 Failed\n");
        n_failure++;
    }
    x = IO_READ(ctrl_reg);
    print_ctrl(LogFp, x);

    if (nv_compare_test(sysclk_scale, ctrl_reg, 2, 2, 1) != PASS) {
        fprintf(LogFp, "Compare 2 Failed\n");
        n_failure++;
    }
    x = IO_READ(ctrl_reg);
    print_ctrl(LogFp, x);


    if (nv_compare_test(sysclk_scale, ctrl_reg, 2, 1, 1) != PASS) {
        fprintf(LogFp, "Compare 3 Failed\n");
        n_failure++;
    }
    x = IO_READ(ctrl_reg);
    print_ctrl(LogFp, x);

    if (nv_compare_test(sysclk_scale, ctrl_reg, 1, 2, 1) != PASS) {
        fprintf(LogFp, "Compare 4 Failed\n");
        n_failure++;
    }
    x = IO_READ(ctrl_reg);
    print_ctrl(LogFp, x);

    if (nv_compare_test(sysclk_scale, ctrl_reg, 1, 1, 1) != PASS) {
        fprintf(LogFp, "Compare 4 Failed\n");
        n_failure++;
    }
    x = IO_READ(ctrl_reg);
    print_ctrl(LogFp, x);


    if (n_failure > 0) {
        return FAIL;
    }
    return PASS;

}

/*
 * Just one store pulse is used here - which should be enough in the
 * simulation model.
 *
 * pulse_width is in nSec
 */
int nv_single_store(int sysclk_scale, int ctrl_reg, int pulse_width)
{
    int cp_ctrl_reg = ctrl_reg | 0x1000;
    int sysclk = get_sys_clk_ns(sysclk_scale);

    /*
     * Set up VPPSel
     */
    IO_WRITE(cp_ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | (9<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));

    /*
     * Set up store
     */
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | (9<<VIRAGE_CTRL_CP_VPPSEL_SHIFT) |
             VIRAGE_CTRL_NV_STORE);

    /*
     * Wait after store to enable pump
     */
    BCP_STALL(Trsp/sysclk);

    /*
     * Enable charge pump
     */
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | (9<<VIRAGE_CTRL_CP_VPPSEL_SHIFT) |
             VIRAGE_CTRL_NV_STORE |
             VIRAGE_CTRL_CP_PE);

    /*
     * Wait for time it takes for VPP to be high
     */
    BCP_STALL(Tpvh/sysclk);

    /*
     * Then wait for the required store pulse width, which could be from 2 - 250 mS
     */
    BCP_STALL(pulse_width/sysclk);

    /*
     * Then take the pump enable away
     */
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | (9<<VIRAGE_CTRL_CP_VPPSEL_SHIFT) |
             VIRAGE_CTRL_NV_STORE);

    /*
     * Then wait some time .... and take store away
     */
    BCP_STALL(Trps/sysclk);

    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | (9<<VIRAGE_CTRL_CP_VPPSEL_SHIFT));

    /*
     * Now wait the recovery time
     */
    BCP_STALL(Trstore/sysclk);

    return PASS;
}

int nv_recall_test(int sysclk_scale, int ctrl_reg)
{
    int x;
    int sysclk = get_sys_clk_ns(sysclk_scale);

    fprintf(LogFp, "NV Recall 0x%x\n", ctrl_reg);

    /*
     * Issue this to stabilize the values for BIAS before issuing recall
     */
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS);
    /*
     * Wait for stable bus
     */
    BCP_STALL(Tscomp/sysclk);

    /*
     * Issue the recall
     */
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS | VIRAGE_CTRL_NV_RECALL);

    /*
     * Wait for RCREADY
     */
    BCP_STALL(EXTRA_WAIT + Trcready/sysclk);

    x = IO_READ(ctrl_reg);

    /*
     * Turn recall off
     */
    IO_WRITE(ctrl_reg, VIRAGE_CTRL_NMS_BYPASS);

    /*
     * Wait for the recovery time
     */
    BCP_STALL(Trrecall/sysclk);

    if ((x & VIRAGE_CTRL_NV_RCREADY) == 0) {
        fprintf(LogFp, "Recall timeout failure\n");
        return FAIL;
    }

    return PASS;
    
}

/*
 * Note, size is in words
 */
int nv_store_recall_test(int sysclk_scale, int ctrl_reg, int size)
{
    int i;
    int x;
    int array_addr = ctrl_reg & 0xffff0000;
    int seed = times(NULL);
    srand(seed);
   
    fprintf(LogFp, "NV Store Recall Test 0x%x\n", ctrl_reg); 

    /*
     * Reset and unlock the charge pump
     */
    if (cp_reset_unlock_test(sysclk_scale, ctrl_reg)) {
        fprintf(LogFp, "CP unlock failed\n");
        return FAIL;
    }

    /*
     * Fill the SRAM with random stuff
     */
    for (i=0; i<size; i++) {
        virage_data[i] = rand();
        IO_WRITE(array_addr+(i<<2), virage_data[i]); 
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            fprintf(LogFp, "SRAM write/read failure\n");
            return FAIL;
        }
    }

    /*
     * Store - just do one and use 10mS
     */
    if (nv_single_store(sysclk_scale, ctrl_reg, 10000000) != PASS) {
        fprintf(LogFp, "Store failure\n");
        return FAIL;
    }
    
    /*
     * Zero the array
     */
    for (i=0; i<size; i++) {
        IO_WRITE(array_addr+(i<<2), 0); 
    }

    if (nv_recall_test(sysclk_scale, ctrl_reg) != PASS) {
        fprintf(LogFp, "Recall failure\n");
        return FAIL;
    }

    for (i=0; i<size; i++) {
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            fprintf(LogFp, "Recall read back failure at %d expected 0x%x got 0x%x\n", i, virage_data[i], x);
            return FAIL;
        }
    }
    return PASS;

}

/*
 * V2 boot up test
 */
int nv_v2_reset_test(int *expected)
{
    int ctrl_reg = VIRAGE2_CTRL_REG;
    int array_addr = ctrl_reg & 0xffff0000;
    int x;
    int i;

    /*
     * Check that the SRAM contains contents from the array (ie recall
     * succeeded
     */
    fprintf(LogFp, "Virage 2 reset state test\n");

    for (i=0; i<64; i++) {
        x = IO_READ(array_addr + (i<<2));
        if (x != expected[i]) {
            fprintf(LogFp, "Compare failure %d expected: 0x%x got: 0x%x\n", i, expected[i], x);
            return FAIL;
        }
    }

    return PASS;

}

int nms_store_test(int sysclk_scale, int ctrl_reg, int size, int test_num)
{
    int x;
    int i, picked_start, picked_end, picked_end_actual;
    int expected[20];
    int index;
    int array_addr = ctrl_reg & 0xffff0000;
    int seed = times(NULL);
    int sysclk = get_sys_clk_ns(sysclk_scale);

    srand(seed);

    fprintf(LogFp, "NMS store test 0x%x\n", ctrl_reg);

    /*
     * 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);
    fprintf(stderr," wrote to vtimer reg %d\n", 1000/sysclk + 1);
    BCP_STALL((4 * 160000)/sysclk);

    /*
     * Wait for Charge pump to exit PORST
     */
    x = IO_READ(ctrl_reg);
    if (x & VIRAGE_CTRL_CP_PORST) {
        BCP_STALL(sysclk_scale *20000/sysclk);
        x = IO_READ(ctrl_reg);
        if (x & VIRAGE_CTRL_CP_PORST) {
            fprintf(LogFp, "Timeout waiting for CP PORST\n");
            return FAIL;
        }
    }

    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NMS_READY) == 0) {
        fprintf(LogFp, "NMS not ready\n");
        return FAIL;
    }

    /*
     * Fill the SRAM with random stuff
     */
    for (i=0; i<size; i++) {
        virage_data[i] = rand();
        IO_WRITE(array_addr+(i<<2), virage_data[i]); 
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            fprintf(LogFp, "SRAM write/read failure\n");
            return FAIL;
        }
    }

    switch(test_num){
    case 0:
      /* 
       * store and recall with default values 
       */
      if (nms_store_default(LogFp, ctrl_reg) != PASS) {
        return FAIL;
      }
      break;
      
    case 1:
      /*
       * store and recall with typical values
       */
      
      if (nms_store(LogFp, ctrl_reg, NMS_STORE_PW_10MS, NMS_VPPLEVEL_7P4V, NMS_VPPMAX_8P0V,
                  NMS_VPPDELTA_400MV, NMS_NMAX_2, 7) != PASS) {
        return FAIL;
      }
      break;
      
    case 2:
      /* 
       * use random values for start, stop vpp values
       */
      picked_start = rand() & 0x0000000f;
      if(picked_start < 8){
	/* pick any of 4 available choices */
	picked_end = (rand() & 0x00000003);
      }
      else if(picked_start < 10){
	/* pick any of 3 available choices: 1, 2, 3 */
	picked_end = ((rand() & 0x00000003) % 3) + 1;
      }
      else if(picked_start < 12){
	/* pick any of 2 choices : 2, 3*/
	picked_end = (rand() & 0x00000001) + 2;
      }
      else{ /* >= 12*/
	/* pick last one*/
	picked_end = 3;
      }
      /* init expected results */
      for(i=0; i< 20; i++){
	expected[i] =0;
      }
      if(picked_end == 0){
	picked_end_actual = 9;
      }
      if(picked_end == 1){
	picked_end_actual = 11;
      }
      if(picked_end == 2){
	picked_end_actual = 13;
      }
      if(picked_end == 3){
	picked_end_actual = 15;
      }

      /* compute expected results */
      index =0;
      for(i=picked_start; i<= picked_end_actual; i++){
	expected[index] = i;
	index++;
      }
      fprintf(LogFp, "picked start = %d\n", picked_start);
      fprintf(LogFp, "picked_end = %d\n", picked_end);
      for(i=0; i< 20; i++){
        fprintf(LogFp, "expected[%d] = %d ", i, expected[i]);
      }
      /* run store here */
      
      if (nms_store_checkvpp(LogFp, ctrl_reg, NMS_STORE_PW_10MS, picked_start, picked_end, NMS_VPPDELTA_400MV, NMS_NMAX_2, 7, expected, 20) != PASS) {
        return FAIL;
      }
	
    }
      

    /*
     * Blow away the values
     */
    for (i=0; i<size; i++) {
        IO_WRITE(array_addr+(i<<2), 0); 
        x = IO_READ(array_addr+(i<<2));
        if (x != 0) {
            fprintf(LogFp, "SRAM clear failure\n");
            return FAIL;
        }
    }

    if (nms_recall(sysclk_scale, LogFp, ctrl_reg) != PASS) {
        fprintf(LogFp, "Recall failed\n");
        return FAIL;
    }

    /*
     * Check all values
     */
    for (i=0; i<size; i++) {
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            fprintf(LogFp, "SRAM read failure\n");
            return FAIL;
        }
    }
    return PASS;
}


int nms_store_test_interrupt(int sysclk_scale, int ctrl_reg, int size)
{
    int x;
    int i;
    int array_addr = ctrl_reg & 0xffff0000;
    int seed = times(NULL);
    int sysclk = get_sys_clk_ns(sysclk_scale);

    srand(seed);

    fprintf(LogFp, "NMS store test 0x%x\n", ctrl_reg);

    /*
     * 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);
    fprintf(stderr," wrote to vtimer reg %d\n", 1000/sysclk + 1);
    BCP_STALL((4 * 160000)/sysclk);

    /*
     * Wait for Charge pump to exit PORST
     */
    x = IO_READ(ctrl_reg);
    if (x & VIRAGE_CTRL_CP_PORST) {
        BCP_STALL(sysclk_scale *20000/sysclk);
        x = IO_READ(ctrl_reg);
        if (x & VIRAGE_CTRL_CP_PORST) {
            fprintf(LogFp, "Timeout waiting for CP PORST\n");
            return FAIL;
        }
    }

    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NMS_READY) == 0) {
        fprintf(LogFp, "NMS not ready\n");
        return FAIL;
    }

    /*
     * Fill the SRAM with random stuff
     */
    for (i=0; i<size; i++) {
        virage_data[i] = rand();
        IO_WRITE(array_addr+(i<<2), virage_data[i]); 
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            fprintf(LogFp, "SRAM write/read failure\n");
            return FAIL;
        }
    }

    
    /*
     * store and recall with typical values: interrupt
     */
      
    if (nms_store_interrupt(LogFp, ctrl_reg, NMS_STORE_PW_10MS, NMS_VPPLEVEL_7P4V, NMS_VPPMAX_8P0V,NMS_VPPDELTA_400MV, NMS_NMAX_2, 7) != FAIL) {
      return FAIL;
    }
     
    /* do a pin reset and enter secure mode */
    bd_pin_reset();
    BCP_STALL(10);
    secure_mode_entry_app();
    BCP_STALL(2000);

    /*
     * Make sure the NMS is not bypassed
     */
    IO_WRITE(ctrl_reg, 0x0);
    x = IO_READ(ctrl_reg);
    print_ctrl(LogFp, x);
    fflush(NULL);

    /*
     * 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);
    fprintf(stderr," wrote to vtimer reg %d\n", 1000/sysclk + 1);
    BCP_STALL((3* 160000)/sysclk);

    /* Prepare to store again */

    /*
     * Wait for Charge pump to exit PORST
     */
    x = IO_READ(ctrl_reg);
    if (x & VIRAGE_CTRL_CP_PORST) {
        BCP_STALL(sysclk_scale *20000/sysclk);
        x = IO_READ(ctrl_reg);
        if (x & VIRAGE_CTRL_CP_PORST) {
            fprintf(LogFp, "Timeout waiting for CP PORST\n");
            return FAIL;
        }
    }

    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NMS_READY) == 0) {
        fprintf(LogFp, "NMS not ready\n");
        return FAIL;
    }

    /*
     * Fill the SRAM with random stuff
     */
    for (i=0; i<size; i++) {
        virage_data[i] = rand();
        IO_WRITE(array_addr+(i<<2), virage_data[i]); 
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            fprintf(LogFp, "SRAM write/read failure\n");
            return FAIL;
        }
    }

    if (nms_store(LogFp, ctrl_reg, NMS_STORE_PW_10MS, NMS_VPPLEVEL_7P4V, NMS_VPPMAX_8P0V,NMS_VPPDELTA_400MV, NMS_NMAX_2, 7) != PASS) {
      return FAIL;
    }

    /*
     * Blow away the values
     */
    for (i=0; i<size; i++) {
        IO_WRITE(array_addr+(i<<2), 0); 
        x = IO_READ(array_addr+(i<<2));
        if (x != 0) {
            fprintf(LogFp, "SRAM clear failure\n");
            return FAIL;
        }
    }

    if (nms_recall(sysclk_scale, LogFp, ctrl_reg) != PASS) {
        fprintf(LogFp, "Recall failed\n");
        return FAIL;
    }

    /*
     * Check all values
     */
    for (i=0; i<size; i++) {
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            fprintf(LogFp, "SRAM read failure\n");
            return FAIL;
        }
    }
    return PASS;
}

int nms_keep_mode_test(int sysclk_scale, int ctrl_reg, int size)
{
    int nms_ctrl_reg = ctrl_reg | 0x2000;
    int array_addr = ctrl_reg & 0xffff0000;
    int x;
    int i;
    int n_failures = 0;
    int retry = 1000;

    fprintf(LogFp, "NMS Keep mode test 0x%x\n", ctrl_reg);

    /*
     * Set up for fast timing - not the real value
     */
    IO_WRITE(MI_SEC_VTIMER_REG, 1000/get_sys_clk_ns(sysclk_scale) + 1);

    if (nms_ready(LogFp, ctrl_reg) != PASS) {
        fprintf(LogFp, "NMS not ready\n");
        return FAIL;
    }

    /*
     * Do a recall and read the array
     */
    if (nms_recall(sysclk_scale, LogFp, ctrl_reg) != PASS) {
        fprintf(LogFp, "Couldn't do a recall\n");
        return FAIL;
    }
    for (i=0; i<size; i++) {
        x = IO_READ(array_addr+(i<<2));
        virage_data[i] = x;
    }

    /*
     * Put the array into keep mode 
     * XXX Need a time-out, don't have a spec
     */
    IO_WRITE(nms_ctrl_reg, (NMS_CMD_KEEP << VIRAGE_CTRL_NMS_CMD_SHIFT));
    BCP_STALL(10);
    i = retry;
    do {
        BCP_STALL(20);
        x = IO_READ(ctrl_reg);
        if (i-- <= 0) {
            fprintf(LogFp, "Time out waiting for NMS_READY\n");
            break;
        }
    } while ((x & VIRAGE_CTRL_NMS_READY) == 0);

    if ((x & VIRAGE_CTRL_NMS_KEEP) == 0) {
        fprintf(LogFp, "Not in Keep mode, ctrl reg 0x%x\n", x);
        print_ctrl(LogFp, x);
        n_failures++;
    }

    /*
     * Now check I can read the array
     */
    for (i=0; i<size; i++) {
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            fprintf(LogFp, "SRAM read failure at 0x%x expected 0x%x got 0x%x\n", array_addr + (i<<2), virage_data[i],
                    x);
            n_failures++;
        }
    }

    /*
     * Put back into IDLE
     * XXX Need a timeout - don't have a spec
     */
    IO_WRITE(nms_ctrl_reg, (NMS_CMD_IDLE << VIRAGE_CTRL_NMS_CMD_SHIFT));
    BCP_STALL(10);
    i = retry;
    do {
        BCP_STALL(10);
        x = IO_READ(ctrl_reg);
        if (i-- <= 0) {
            fprintf(LogFp, "Timeout waiting for NMS_READY\n");
            break;
        }
    } while ((x & VIRAGE_CTRL_NMS_READY) == 0);

    if ((x & VIRAGE_CTRL_NMS_KEEP) != 0) {
        fprintf(LogFp, "Keep didn't turn off\n");
        print_ctrl(LogFp, x);
        n_failures++;
    }


    /*
     * Now check again I can read the array
     */
    for (i=0; i<size; i++) {
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            fprintf(LogFp, "SRAM read failure at %d expected 0x%x got 0x%x\n", i, virage_data[i],
                    x);
            n_failures++;
        }
    }

    if (n_failures > 0) {
        return FAIL;
    } else {
        return PASS;
    }

}


int nms_compare_test(int sysclk_scale, int ctrl_reg)
{
    int nms_ctrl_reg = ctrl_reg | 0x2000;
    int x;
    int sysclk = get_sys_clk_ns(sysclk_scale);

    fprintf(LogFp, "NMS Compare test 0x%x\n", ctrl_reg);

    /*
     * Set up for fast timing - not the real value
     */
    IO_WRITE(MI_SEC_VTIMER_REG, 1000/get_sys_clk_ns(sysclk_scale) + 1);
    fprintf(LogFp, "Programming to vtimer reg = %d\n", 1000/get_sys_clk_ns(sysclk_scale) + 1);
    
    
    if (nms_recall(sysclk_scale, LogFp, ctrl_reg) != PASS) {
        fprintf(LogFp, "Recall failed\n");
        return FAIL;
    }

    /*
     * Check for NMS ready
     */
    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NMS_READY) == 0) {
        fprintf(LogFp, "NMS not ready\n");
        return FAIL;
    }

    /*
     * Execute compare
     */
    IO_WRITE(nms_ctrl_reg, (NMS_CMD_COMPARE << VIRAGE_CTRL_NMS_CMD_SHIFT));

    /*
     * Wait for RC ready
     */
    BCP_STALL(EXTRA_WAIT + Trcready/sysclk);

    /*
     * Wait some extra time since we are going through NMS
     */
    BCP_STALL(4* MORE_EXTRA_WAIT);

    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NMS_READY) == 0) {
        fprintf(LogFp, "Problem! ready not low\n");
        print_ctrl(LogFp, x);
        return FAIL;
    }

    if ((x & VIRAGE_CTRL_NMS_PASS) == 0) {
        fprintf(LogFp, "Compare didn't pass\n");
        print_ctrl(LogFp, x);
        return FAIL;
    }

    return PASS;

}

int nms_compare_test_negative(int sysclk_scale, int ctrl_reg, int size)
{
    int nms_ctrl_reg = ctrl_reg | 0x2000;
    int x, i;
    int array_addr = ctrl_reg & 0xffff0000;
    int sysclk = get_sys_clk_ns(sysclk_scale);

    fprintf(LogFp, "NMS Compare test 0x%x\n", ctrl_reg);

    /*
     * Set up for fast timing - not the real value
     */
    IO_WRITE(MI_SEC_VTIMER_REG, 1000/get_sys_clk_ns(sysclk_scale) + 1);
    fprintf(LogFp, "Programming to vtimer reg = %d\n", 1000/get_sys_clk_ns(sysclk_scale) + 1);
    
    
    if (nms_recall(sysclk_scale, LogFp, ctrl_reg) != PASS) {
        fprintf(LogFp, "Recall failed\n");
        return FAIL;
    }

    /*
     * Check for NMS ready
     */
    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NMS_READY) == 0) {
        fprintf(LogFp, "NMS not ready\n");
        return FAIL;
    }

    
    /*
     * Fill the SRAM with random stuff: blow away recalled data
     */
    for (i=0; i<size; i++) {
        virage_data[i] = rand();
        IO_WRITE(array_addr+(i<<2), virage_data[i]); 
        x = IO_READ(array_addr+(i<<2));
        if (x != virage_data[i]) {
            fprintf(LogFp, "SRAM write/read failure\n");
            return FAIL;
        }
    }

    /*
     * Execute compare
     */
    IO_WRITE(nms_ctrl_reg, (NMS_CMD_COMPARE << VIRAGE_CTRL_NMS_CMD_SHIFT));

    /*
     * Wait for RC ready
     */
    BCP_STALL(EXTRA_WAIT + Trcready/sysclk);

    /*
     * Wait some extra time since we are going through NMS
     */
    BCP_STALL(4* MORE_EXTRA_WAIT);

    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NMS_READY) == 0) {
        fprintf(LogFp, "Problem! ready not low\n");
        print_ctrl(LogFp, x);
        return FAIL;
    }

    if ((x & VIRAGE_CTRL_NMS_PASS) != 0) {
        fprintf(LogFp, "Negative Compare test didn't pass\n");
        print_ctrl(LogFp, x);
        return FAIL;
    }

    return PASS;

}

int nms_recall_auto_margin_test(int sysclk_scale, int ctrl_reg, int size)
{
    int x, i;
    int index, correct, previous;
    unsigned char crm0, crm1, crm2, crm3;
    unsigned char tecc_input[8];
    unsigned char bias_input[8];
    unsigned char mrcl_input[8];
    unsigned char mrcl_array[8];
    unsigned char tecc_array[8];
    unsigned char bias_array[8];
    unsigned char newmrcl, newtecc, newbias;
    int nms_ctrl_reg;
    int retry = 1000;
    
    /*
     * Make sure the NMS is not bypassed
     */
    IO_WRITE(ctrl_reg, 0x0);

    nms_ctrl_reg = ctrl_reg | 0x2000;

    fprintf(LogFp, "NMS Recall Auto Margin test 0x%x\n", ctrl_reg);
    /*
     * Set up for fast timing - not the real value
     */
    IO_WRITE(MI_SEC_VTIMER_REG, 1000/get_sys_clk_ns(sysclk_scale) + 1);
    fprintf(LogFp, "Programming to vtimer reg = %d\n", 1000/get_sys_clk_ns(sysclk_scale) + 1);

    /* do an NMS recall just for testing */
        
    
    if (nms_recall(sysclk_scale, LogFp, ctrl_reg) != PASS) {
        fprintf(LogFp, "Recall failed\n");
        return FAIL;
    }
    
    x = IO_READ(ctrl_reg);
    if ((x & VIRAGE_CTRL_NMS_READY) == 0) {
        fprintf(LogFp, "NMS not ready\n");
        return FAIL;
    }
    fprintf(LogFp, "Finished recall\n");
    /*
     * Set up the configuration registers for the full store
     * set up crm0-4
     */
    /* This setting is for MRCL, TECC= 1,1...2,2...3,3...0,0...1,1 */
    /*
    mrcl_input[0] = 1;
    mrcl_input[1] = 2;
    mrcl_input[2] = 3;
    mrcl_input[3] = 0;
    mrcl_input[4] = 1;

    tecc_input[0] = 1;
    tecc_input[1] = 2;
    tecc_input[2] = 3;
    tecc_input[3] = 0;
    tecc_input[4] = 1;

    bias_input[0] = 1;
    bias_input[1] = 2;
    bias_input[2] = 3;
    bias_input[3] = 0;
    bias_input[4] = 1;
    */

    /* This is for random settings */
    
    for(i=0; i<5; i++){
      mrcl_input[i] = rand() & 0x3;
      tecc_input[i] = rand() & 0x3;
      bias_input[i] = rand() & 0x3;
    }
    
    crm0 = ((tecc_input[1] << 6)|(bias_input[0] << 4)|(mrcl_input[0] <<2)|(tecc_input[0]));
    crm1 = ((mrcl_input[2] << 6)|(tecc_input[2] << 4)|(bias_input[1] <<2)|(mrcl_input[1]));
    crm2 = ((bias_input[3] << 6)|(mrcl_input[3] << 4)|(tecc_input[3] <<2)|(bias_input[2]));
    crm3 = ((bias_input[4] << 4)|(mrcl_input[4] <<2)|(tecc_input[4]));
    
    
    if (ctrl_reg == VIRAGE0_CTRL_REG){
      IO_WRITE(VIRAGE0_NMS_CRM_0_REG, crm0);
      IO_WRITE(VIRAGE0_NMS_CRM_1_REG, crm1);
      IO_WRITE(VIRAGE0_NMS_CRM_2_REG, crm2);
      IO_WRITE(VIRAGE0_NMS_CRM_3_REG, crm3);
    }
    else if (ctrl_reg == VIRAGE1_CTRL_REG){

      IO_WRITE(VIRAGE1_NMS_CRM_0_REG, crm0);
      IO_WRITE(VIRAGE1_NMS_CRM_1_REG, crm1);
      IO_WRITE(VIRAGE1_NMS_CRM_2_REG, crm2);
      IO_WRITE(VIRAGE1_NMS_CRM_3_REG, crm3);
    }
    else if (ctrl_reg == VIRAGE2_CTRL_REG){
      IO_WRITE(VIRAGE2_NMS_CRM_0_REG, crm0);
      IO_WRITE(VIRAGE2_NMS_CRM_1_REG, crm1);
      IO_WRITE(VIRAGE2_NMS_CRM_2_REG, crm2);
      IO_WRITE(VIRAGE2_NMS_CRM_3_REG, crm3);
    }

    /*
     * Execute recall auto margin command
     */
    IO_WRITE(nms_ctrl_reg, (NMS_CMD_RECALL_AUTO << VIRAGE_CTRL_NMS_CMD_SHIFT));

    /*
     * Wait for ready - XXX timeout needed
     */
    index =0;
    previous = 0;
    do {
        BCP_STALL(10);
        x = IO_READ(ctrl_reg);
	print_ctrl(LogFp, x);
	if(x & VIRAGE_CTRL_NV_COMP){
	  get_mrcl_tecc_bias(x, &newmrcl, &newtecc, &newbias);
	  if(index ==0){
	    /* update on first index */
	    mrcl_array[index] = newmrcl;
	    tecc_array[index] = newtecc;
	    bias_array[index] = newbias;
	    index++;
	  }
	  /*else if ((mrcl_array[index-1] != newmrcl)
		   || (tecc_array[index-1] != newtecc)
		   || (bias_array[index-1] != newbias)){
	  */
	  else if (previous == 0){
	    /* update if compare changed from zero to one */
	    mrcl_array[index] = newmrcl;
	    tecc_array[index] = newtecc;
	    bias_array[index] = newbias;
	    index++;
	  }
	  previous = 1;
	}
	else{
	  previous = 0;
	}
	
        if (retry-- <= 0) {
            fprintf(LogFp, "Timeout waiting for NMS_READY\n");
            return FAIL;
            break;
        }
    } while ((x & VIRAGE_CTRL_NMS_READY) == 0);
    correct = 1;
    for(i = 0; i < 5; i++){
      fprintf(LogFp, "input = %02x %02x %02x\n", mrcl_input[i], tecc_input[i], bias_input[i]);
      fprintf(LogFp, "in register = %02x %02x %02x\n", mrcl_array[i], tecc_array[i], bias_array[i]);
      if((mrcl_input[i] != mrcl_array[i]) 
	 ||(tecc_input[i] != tecc_array[i]) 
	 ||(bias_input[i] != bias_array[i])){
	correct = 0;
      }
    }
    if (((x & VIRAGE_CTRL_NMS_PASS) == 0)||(correct == 0)) {
        fprintf(LogFp, "Recall auto margin didn't pass\n");
        return FAIL;
    }

    return PASS;

}