/* Memory failover routine for NGMP / FTDDR2SPA
 *
 * Author: Magnus Hjorth, Aeroflex Gaisler
 * Contact: support@gaisler.com
 */


#include "failover.h"
#include "memscrub.h"
#include "memscrub_rtems.h"

struct edacregs {
  volatile unsigned long ftcfg,ftdaddr,ftdcb,ftddata,ftbound;
};

static int memprobe_main(struct edacregs *er, int addr, int ftcfg)
{
  unsigned long l,x,b,w,code;
  int badparts=0;
  int i;

  addr &= ~7;
  w = (ftcfg >> 16) & 7;
  code = ftcfg & 2;
  x=0x3569AC35; /* Test pattern */
  for (i=0; i<3; i++) {
    if (i == 0)
      er->ftdaddr = addr;
    else if (i == 1)
      er->ftdaddr = addr+4;    
    if (i < 2) {
      er->ftddata = x;
      l = er->ftddata ^ x;
      er->ftddata = ~x;
      l |= (er->ftddata ^ (~x));
    } else {
      er->ftdcb = x;
      l = er->ftdcb ^ x;
      er->ftdcb = ~x;
      l |= (er->ftdcb ^ (~x));
    }
    b = 0;
    switch (w) {
    case 1:  /* 16 bit */
      if (i < 2) {
	if ((l & 0xF000F000) != 0) b |= 8;
	if ((l & 0x0F000F00) != 0) b |= 4;
	if ((l & 0x00F000F0) != 0) b |= 2;
	if ((l & 0x000F000F) != 0) b |= 1;
      } else if (code == 0) {
	if ((l & 0xF00FF00F) != 0) b |= 0x20;
	if ((l & 0x0FF00FF0) != 0) b |= 0x10;
      } else {
	if ((l & 0xFFFF0000) != 0) b |= 0x20;
	if ((l & 0x0000FFFF) != 0) b |= 0x10;
      }
    case 2:  /* 32 bit */
      if (i < 2) {
	if ((l & 0xFF000000) != 0) b |= 8;
	if ((l & 0x00FF0000) != 0) b |= 4;
	if ((l & 0x0000FF00) != 0) b |= 2;
	if ((l & 0x000000FF) != 0) b |= 1;	
      } else if (code==0) {
	if ((l & 0xF0F00F0F) != 0) b |= 0x20;
	if ((l & 0x0F0FF0F0) != 0) b |= 0x10;
      } else {
	if ((l & 0xFFFF0000) != 0) b |= 0x20;
	if ((l & 0x0000FFFF) != 0) b |= 0x10;
      }
      break;
    default: /* 64 bit */
      if (i < 2) {
	if ((l & 0xFFFF0000) != 0) b |= 2;
	if ((l & 0x0000FFFF) != 0) b |= 1;
	if (i == 0) b <<= 2;
      } else if (code==0) {
	if ((l & 0xF0F0F0F0) != 0) b |= 0x20;
	if ((l & 0x0F0F0F0F) != 0) b |= 0x10;
      } else {
	if ((l & 0xFFFF0000) != 0) b |= 0x20;
	if ((l & 0x0000FFFF) != 0) b |= 0x10;
      } 
      break;
    }
    badparts |= b;
  }
  er->ftdaddr = addr;
  er->ftddata = 0;
  er->ftdaddr = addr+4;
  er->ftddata = 0;
  er->ftdcb = 0;
  return badparts;
}

static int memprobe(struct failover_data *d, struct edacregs *er, int muxed, 
		    int *bp, int ftcfg)
{
  int j,m,a,x;
  unsigned long long testbuf2;
  /* Backup and restore diagnostic address, in case we interrupted 
   * during fault injection */
  a = er->ftdaddr;
  /* Try probing on two addrs (one on data/heap, one on stack) and check 
     for common errors. */
  j = memprobe_main(er, (int)&(d->testbuf[1]), ftcfg);
  if (j != 0)
    j &= memprobe_main(er, (int)&testbuf2, ftcfg);
  er->ftdaddr = a;
  *bp = j;

  /* Ignore already muxed out lane */
  if (muxed != 0) {
    x = (ftcfg >> 5) & 7;
    if (x==0) x=6;
    j &= ~(1<<(x-1));
  }

  /* Check if both addrs had permanent errors in same byte lane(s) */  
  if (j != 0) {
    /* Check EDAC state */
    /* Check which mux setting should work. */    
    if      ((j & 0x01) != 0) m=1;
    else if ((j & 0x02) != 0) m=2;
    else if ((j & 0x04) != 0) m=3;
    else if ((j & 0x08) != 0) m=4;
    else if ((j & 0x10) != 0) m=5;
    else                      m=6;
    return m;
  }
  return 0;
}

int failover_errfunc(int event, unsigned long *addr, 
		     unsigned long *msgout,
		     void *ptr, struct memscrubr_meminfo *info)
{
  struct failover_data *d = (struct failover_data *)ptr;
  struct edacregs *er = (struct edacregs *)d->edacregs;
  int r=0,bp,i,st;
  unsigned long l;

  st = d->state;
  if (st != 0) r=ERRFUNC_CALLDONE;

  switch (st) {


    /* State 0: No error, code A or B */
  case 0: 
    if ((event & MEMSCRUB_CE_RUNTOT) != 0) {
      l = er->ftcfg;
      i = memprobe(d,er,0,&bp,l);
      if (i != 0) {
	/* Check if we're running with code A or B */
	if ((l & 2) == 0) {
	  /* Code A - initiate failover */
	  er->ftbound = *addr = info->memstart;
	  l = (1 << 3) | (1 << 2) | 1;
	  if (i < 6) l |= i<<5;
	  er->ftcfg = l;
	  st = 1;
	  r = ERRFUNC_REGEN | ERRFUNC_SENDMSG | ERRFUNC_CALLDONE;	  
	} else {
	  /* Code B - if error is in other part than 6, 
	   * mask errors and run slow regen loop. */
	  er->ftcfg = (1 << 4) | (1 << 1) | 1;
	  st = 8;
	  r = ERRFUNC_REGENSLOW | ERRFUNC_SENDMSG | ERRFUNC_CALLDONE;
	}
      }
    }
    break;


    /* State 1: Switching from code A -> B with mux */
  case 1: 
  case 4:
  case 7:
    if ((event & MEMSCRUB_UE) != 0) {
      /* Uncorrectable error during memory failover procedure */
      /* If this was caused by the scrubber itself, the scrubber will 
       * skip past the bad location and the boundary will remain at this
       * point. When this happens, we bump the boundary 64 bits and 
       * restart the scrubbing from this point. */
      l = er->ftbound;
      if ( (*addr & ~info->blockmask) == (l & ~info->blockmask) ) {
	l += 8;
	er->ftbound = l;
	r = ERRFUNC_REGEN | ERRFUNC_CALLDONE;
	break;
      }
    }
    if ((event & MEMSCRUB_DONE) != 0) {
      l = er->ftbound;
      if (l >= info->memstart+info->memsize) {
	if (st == 1) {
	  l = er->ftcfg;
	  er->ftcfg = (l & 0xE0) | 3;
	  st = 2;
	} else {
	  er->ftcfg = 1;
	  st = 0;
	} 
	r = ERRFUNC_SENDMSG | ERRFUNC_CALLDONE;	
      } else {
	*addr = l;
	r = ERRFUNC_REGEN | ERRFUNC_CALLDONE;
      }
    }
    break;


    /* State 2: Scrubbing after code switch */
  case 2:
    if ((event & (MEMSCRUB_DONE | MEMSCRUB_CE_RUNTOT)) != 0) {
      l = er->ftcfg;
      i = memprobe(d,er,1,&bp,l);
      if (bp == 0) {
	/* Memory has healed */
	/* First step is to regenerate memory shadowed by the mux. */
	*addr = info->memstart;
	r = ERRFUNC_REGEN | ERRFUNC_SENDMSG | ERRFUNC_CALLDONE;
	st = 3;	
      } else if (i != 0) {
	/* Another memory part has failed. */
	/* Disable CERR and regenerate slowly */
	*addr = info->memstart;
	r = ERRFUNC_REGENSLOW | ERRFUNC_SENDMSG | ERRFUNC_CALLDONE;
	st = 5;
      }
    }
    break;
    

    /* State 3: Healing part 1: Regenerate shadow memory, code B, muxed */
  case 3:
  case 6:
    if ((event & MEMSCRUB_DONE) != 0) {
      /* Check that memory is still healed */
      l = er->ftcfg;
      i = memprobe(d,er,0,&bp,l);
      if (i == 0 || (st==6 && bp==(1<<(i-1))) ) {
	/* Continue switching back from code B to code A */
	er->ftbound = *addr = info->memstart;
	l = (1 << 3) | (1 << 2) | (1 << 1) | 1;
	if (st==6) l |= (1 <<4);
	er->ftcfg = l;
	st ++;
	r = ERRFUNC_REGEN | ERRFUNC_SENDMSG | ERRFUNC_CALLDONE;
      } else {
	/* Abort switching procedure */
	st --;
	r = ERRFUNC_SENDMSG | ERRFUNC_CALLDONE;
      }
    }
    break;


    /* State 4: Healing part 2: Switch from code B to A, no mux */
    /* Handled together with state 1 above */


    /* State 5: Two permanent errors, 
     *    running slow-regen in code B with mux and CE masked */    
  case 5:
    if ((event & MEMSCRUB_DONE) != 0) {      
      l = er->ftcfg;
      i = memprobe(d,er,1,&bp,l);
      *addr = info->memstart;
      if (i == 0) {
	/* Second permanent error has healed, disable CE mask */
	er->ftcfg = l & 0xEF;
	st = 2;
	r = ERRFUNC_REGEN | ERRFUNC_SENDMSG | ERRFUNC_CALLDONE;
      } else if (bp == (1 << (i-1))) {
	/* Second error remains, but first error has healed! */
	/* We switch back to code A, then again to code B with a 
	 * different mux setting */
	/* First, make sure the healed memory is reinitialized */
	st = 6;
	r = ERRFUNC_REGEN | ERRFUNC_SENDMSG | ERRFUNC_CALLDONE;
      } else {
	r = ERRFUNC_REGENSLOW | ERRFUNC_CALLDONE;
      }
    }
    break;

    /* State 6: Mux switching part 1: Regenerate shadowed memory */
    /* Handled with state 3 above */
    
    /* State 7: Mux switching part 2: Switch from code B -> A */
    /* Handled with state 1 above */
    
    /* State 8: Started in code B, got permanent error. regenerating slowly with CE masked */
    case 8:
      if ((event & MEMSCRUB_DONE) != 0) {
	l = er->ftcfg;
	i = memprobe(d,er,0,&bp,l);
	if (i == 0) {
	  /* Disable CE mask */
	  er->ftcfg = 3;
	  st = 0;
	  r = ERRFUNC_SENDMSG;
	} else {
	  *addr = info->memstart;
	  r = ERRFUNC_REGENSLOW | ERRFUNC_CALLDONE;
	}
      }
      break;

  }

  if ((r & ERRFUNC_SENDMSG) != 0)
    *msgout = (bp << 16) | (d->state << 4) | (st);

  d->state = st;
  return r;
}

void failover_parse_message(unsigned long msg, int *oldstate, int *newstate, int *badparts)
{
  *oldstate = (msg >> 4) & 15;
  *newstate = msg & 15;
  *badparts = msg >> 16;
}
