
/* Application example for using the DDR2SPA with EDAC together with
 * the memory scrubber under the Gaisler RTEMS environment.
 * 
 * Author: Magnus Hjorth, Aeroflex Gaisler
 * Contact: support@gaisler.com
 */

#include <stdio.h>
#include <stdlib.h>
#include <rtems.h>
#include <amba.h>
#include "memscrub_rtems.h"
#include "failover.h"

/* -------------- Application setup ----------   */
#ifndef MEMSTART
#define MEMSTART (0x40000000)
#endif
#ifndef MEMSIZE
#define MEMSIZE  (0x1000000)
#endif
#ifndef OPERMODE
#define OPERMODE 0
#endif
#ifndef FOOPTIONS
#define FOOPTIONS 0
#endif

/* ---------------- RTEMS Setup ---------------  */

/* configuration information */
#define CONFIGURE_INIT
#include <bsp.h> /* for device driver prototypes */

rtems_task Init( rtems_task_argument argument);	/* forward declaration needed */

/* configuration information */
#define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
#define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
#define CONFIGURE_MAXIMUM_TASKS             4
#define CONFIGURE_MAXIMUM_MESSAGE_QUEUES    MEMSCRUB_MAXIMUM_MESSAGE_QUEUES
#define CONFIGURE_MESSAGE_BUFFER_MEMORY     MEMSCRUB_MESSAGE_BUFFER_MEMORY
#define CONFIGURE_RTEMS_INIT_TASKS_TABLE
#define CONFIGURE_EXTRA_TASK_STACKS         (3 * RTEMS_MINIMUM_STACK_SIZE)

#include <rtems/confdefs.h>

#define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
#define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
#define CONFIGURE_DRIVER_CUSTOM1
#define DRIVER_CUSTOM1_REG { memscrubr_register }

#include <drvmgr/drvmgr_confdefs.h>

#if defined(MEMFAIL_ML510) || defined(MEMFAIL_NGFP)
#define MEMFAIL
struct failover_data fodata = {
  .edacregs = (void *)0xFFE00020,
  .options = FOOPTIONS
};
#endif

static struct drvmgr_key memscrub_res[] = {
  { "memstart",    KEY_TYPE_INT, { .i = (MEMSTART) } }, 
  { "memsize",     KEY_TYPE_INT, { .i = (MEMSIZE) } },
  { "autostart",   KEY_TYPE_INT, { .i = 1 } },
  { "opermode",    KEY_TYPE_INT, { .i = (OPERMODE) } },
  { "scrubdelay",  KEY_TYPE_INT, { .i = 10 } },
  { "regendelay",  KEY_TYPE_INT, { .i = 5 } },
#ifdef MEMFAIL
  { "errfunc",     KEY_TYPE_POINTER, { .ptr = (void *)failover_errfunc } },
  { "errfuncdata", KEY_TYPE_POINTER, { .ptr = (void *)&fodata } },
#endif
  KEY_EMPTY
};

struct drvmgr_bus_res grlib_drv_resources = {
  .next = NULL,
  .resource = {
    { DRIVER_AMBAPP_GAISLER_MEMSCRUB_ID, 0, memscrub_res },
    RES_EMPTY,
  }
};


/* ----------------- Application code ---------------- */

struct ddr2info {
  volatile unsigned long *regs;
  unsigned long start,size;
};

static struct ddr2info di; /* Setup by init task */


/* /////////
 * Report task. Reads the scrubber message queue and prints a text
 * message whenever something has happened. */

static rtems_task report_task_entry(rtems_task_argument ignored)
{
  int i;
#ifdef MEMFAIL
  int os,ns,bp;
#endif
  struct memscrubr_message m;
  while (1) {
    i = memscrubr_get_message(0,1,&m);
    switch (m.msgtype) {
    case 1: 
      printf("[R] Scrubber iteration done, errcount=%d\n",m.d.done.cecount);
      break;
    case 2:
      printf("[R] %s detected, addr=%08x, %s, mst=%d, size=%d\n",
	     (m.d.err.errtype==2)?"CE":"UE",
	     (unsigned int)m.d.err.addr, m.d.err.hwrite?"wr":"rd",
	     m.d.err.master, (1<<m.d.err.hsize));
      break;
    case 3:
      printf("[R] Scrubber switched to regeneration mode\n");
      break;
#ifdef MEMFAIL
    case 4:
      failover_parse_message(m.d.custom, &os, &ns, &bp);
      printf("[R] Failover state switch %d -> %d. Bad part mask=%x\n",
	     os,ns,bp);
      break;
#endif
    default:
      printf("[R] memscrubr_get_message returned %d, msgtype %d\n",i,m.msgtype);
      break;
    }
  }
}



#ifndef NOFITASK

/* //////// 
 * Fault injection task.  Injects single errors using the DDR2 FT
 * diagnostic registers. The errors are injected in 10 second intervals, the
 * first time 1 error is injected, then 2 errors are injected, then 3,
 * ..., up to 20, it then starts over at 1
 */


static int injected_total = 0;

static void flip_bit(volatile unsigned long *v, int bit)
{
  (*v) ^= (1<<bit);
}

#ifdef MEMFAIL_ML510

/* For ML510, implement memory failure and healing by pushing the delay
 * line setting 32 steps. Only works with 32+16-bit config currently. */

static void memfail_fail(int part)
{
  int i;
  volatile unsigned long *p;
  unsigned long l;
  if (part < 4) {
    p = &(di.regs[2]);
    l = *p;
    l = (l & 0x1FFF0000) | (0x101 << part);
  } else {
    p = &(di.regs[3]);
    l = *p;
    l = (l & 0x00E027FF) | (0x01100000 << part);
  }
  for (i=0; i<32; i++)
    *p = l;
}

static void memfail_heal(int part)
{
  memfail_fail(part);
}

#endif

#ifdef MEMFAIL_NGFP
/* For NGFP we simulate memory failure by changing the read-enable delay
 *  and phsel setting to maximum value. */
/* This only is implemented for DDR only, both widths are supported. */

/* Note: This way of injecting faults is not fully realistic. Because of
 * implementation details of the NGFP DDR2 PHY, this reconfiguration may
 * only produce errors in read-after-write cases and not when only reading.
 * To get predictable failover behavior, we need to setup the scrubber
 * driver to always probe the memory for errors after each scrub run
 */

static int ngfp_oldval[6];

static void ngfp_part2bl(int part, int *mp, int *mb)
{
  int w,b;
  w = (di.regs[1] >> 12) & 7;
  b = (w>2)?(part*2):part;
  *mp = b >> 2;
  *mb = b & 3;
}

static void ngfp_setregmux(int muxpos)
{
  unsigned long l;
  l = di.regs[6];
  if (((l >> 8) & 3) != muxpos) {
    l &= ~(3<<8);
    l |= (muxpos << 8);
    di.regs[6] = l;
  }
}

static int ngfp_setrddelphsel(int muxpos, int byte, int newval)
{
  unsigned long l;
  int oldr,oldp,newr,newp;
  ngfp_setregmux(muxpos);
  l = di.regs[7];
  oldr = (l >> (8+5*byte)) & 31;
  oldp = (l >> (2*byte)) & 3;
  newr = newval >> 2;
  newp = newval & 3;
  l &= ~(31 << (8+5*byte));
  l |= newr << (8+5*byte);
  l &= ~(3 << (2*byte));
  l |= newp << (2*byte);
  di.regs[7] = l;
  return (oldr << 2) | oldp;
}

static void memfail_fail(int part)
{
  int mp,mb;
  ngfp_part2bl(part,&mp,&mb);
  ngfp_oldval[part] = ngfp_setrddelphsel(mp,mb,(16 << 2)|1);
}

static void memfail_heal(int part)
{
  int mp,mb;
  ngfp_part2bl(part,&mp,&mb);
  ngfp_setrddelphsel(mp,mb,ngfp_oldval[part]);
}

#endif

static rtems_task fault_task_entry(rtems_task_argument arg)
{
  int i,j,y;
  rtems_interval t,d;
  struct ddr2info *di = (struct ddr2info *)arg;
  unsigned long x;
  int totals[2];
#ifdef MEMFAIL
  int failstate=0,faillanes[2]={0,0};
#endif
  t = rtems_clock_get_ticks_since_boot();
  while (1) {
    for (i=1; i<=20; i++) {
      
      /* Wake up next multiple of 10 s */
      t += rtems_clock_get_ticks_per_second() * 10;
      d = t - rtems_clock_get_ticks_since_boot();
      rtems_task_wake_after(d);

      /* Inject errors */
      memscrubr_get_totals(0,totals);
      printf("[F] Error counts: total_inj=%d, total_ce=%d\n"
	     "[F] Injecting %d single-bit errors. \n",injected_total,totals[0],i);

      

      for (j=0; j<i; j++) {
	/* Select random address to put in diag address register */
	x = (rand() % MEMSIZE) + MEMSTART;
	di->regs[9] = x;
	/* Select one of the 32 bits in either the diag data register (0-31) 
	 * or the diag CB register (32-63) */
	y = rand() % 64;
	if (y > 31)
	  flip_bit(di->regs+10, y-32);
	else
	  flip_bit(di->regs+11, y);
      }
      injected_total += i;
    }
#ifdef MEMFAIL
  do {
    /* Wake up next multiple of 10 s */
    t += rtems_clock_get_ticks_per_second() * 10;
    d = t - rtems_clock_get_ticks_since_boot();
    rtems_task_wake_after(d);
    /* Select fail or heal */
    i = 0;
    if (failstate == 0) {
      i = 1;
      /* j = rand() % 6; */
      j = faillanes[0]-1;
      if (j<0) j=5;
    } else if (failstate == 1) {
      i = 0;
      j = faillanes[0];
    } else {
      i = rand() & 1;
      if (i == 0) j=faillanes[0];
      else {
	j = rand() % 5;
	if (j >= faillanes[0]) j++;
      }
    }
    printf("[F] %s memory part %d\n",i?"Failing":"Healing",j);
    if (i) {
      memfail_fail(j);
      faillanes[failstate] = j;
      failstate++;
    } else {
      memfail_heal(j);
      failstate--;
    }
  } while (failstate==2);  
#endif

  }

}

#endif

/* /////////
 * Init task. */


static int ddr2_scan_func(struct ambapp_dev *dev, int index, void *arg)
{
  struct ddr2info *info = (struct ddr2info *)arg;
  struct ambapp_ahb_info *ai = (struct ambapp_ahb_info *)(&(dev->devinfo));
  int i,memfound=0,regfound=0;

  for (i=0; i<4; i++) {
    if (ai->type[i] == AMBA_TYPE_MEM && !memfound) {
      info->start = ai->start[i];
      info->size = ( ~ (ai->mask[i]) )+1;
      memfound++;
    } else if (ai->type[i] == AMBA_TYPE_AHBIO && !regfound) {
      info->regs = (volatile unsigned long *)ai->start[i];
      regfound++;
    }
  }
  
  if (!memfound || !regfound) return 0;
  
  return 1;  
}

rtems_task Init(
  rtems_task_argument ignored
)
{
  int i;
  rtems_id report_task,fault_task;

  printf("-- Scrubber RTEMS test application --\n");  
  printf("Config: Mem range: %08x-%08x\n", (unsigned int)MEMSTART, 
	 (unsigned int)(MEMSTART+MEMSIZE-1));
  /* Check for scrubber */
  if (memscrubr_count() < 1) {
    printf("Error: No memscrub devices found!\n");
    exit(1);
  }
  memscrubr_print_status(0);
  /* Find DDR2 controller */
  i = ambapp_for_each(&ambapp_plb,OPTIONS_ALL|OPTIONS_ALL_DEVS,
		      VENDOR_GAISLER,GAISLER_DDR2SP,ddr2_scan_func,
		      (void *)&di);
  if (i != 1) {
    printf("DDR2 controller not found.\n");
    exit(1);
  } else if ((di.regs[1] & (1<<16)) == 0) {
    printf("DDR2 controller found but does not have EDAC!\n");
    exit(1);
  } else 
    printf("FT DDR2 controller found, Memory bar %d MB @ %08x, regs @ %08x\n\n",
	   (int)(di.size >> 20), (unsigned int)di.start, (unsigned int)di.regs);
  
  /* Enable EDAC */
  di.regs[8] = 1;

  /* Launch application tasks */
  i = rtems_task_create(rtems_build_name('r','e','p','t'), 200,
			RTEMS_MINIMUM_STACK_SIZE, 0, 0, &report_task);
  if (i != 0) rtems_fatal_error_occurred(i);
#ifndef NOFITASK
  i = rtems_task_create(rtems_build_name('f','i','n','j'), 150,
			RTEMS_MINIMUM_STACK_SIZE, 0, 0, &fault_task);
  if (i != 0) rtems_fatal_error_occurred(i);
#endif
  i = rtems_task_start(report_task, report_task_entry, 0);
  if (i != 0) rtems_fatal_error_occurred(i);  
#ifndef NOFITASK
  i = rtems_task_start(fault_task, fault_task_entry, 
		       (rtems_task_argument)(&di));
  if (i != 0) rtems_fatal_error_occurred(i);
#endif
  
  /* Remove init task */
  rtems_task_delete(RTEMS_SELF);
  rtems_fatal_error_occurred(0);
}
