#include <cstring>

#define NSAMP 5

#define SIZE_IN  200
#define SIZE_OUT 250

#define FREE1 1
#define RDY1  2
#define FREE2 4
#define RDY2  8
#define nBSY1 (FREE1 | RDY1)
#define nBSY2 (FREE2 | RDY2)

extern cregister volatile unsigned int IFR, ICR, ISR;

/* We may use DMA_1[i] = DMA_0[i+16] and DMA_2[i] = DMA_0[i+1] */
volatile unsigned int *const DMA_0 = (unsigned int *) 0x01840000;
volatile unsigned int *const DMA_1 = (unsigned int *) 0x01840040;
volatile unsigned int *const DMA_2 = (unsigned int *) 0x01840004;
volatile unsigned int *const DMA_3 = (unsigned int *) 0x01840044;

volatile unsigned int *const TCTL = (unsigned int *) 0x01940000; /* Timer 0 */
volatile unsigned int *const SER2 = (unsigned int *) 0x01a40000;
volatile unsigned int *const SER1 = (unsigned int *) 0x01900000;
volatile unsigned int *const SER0 = (unsigned int *) 0x018c0000;
volatile unsigned int *const RRAM = (unsigned int *) 0x01400000;
#define src_ptr_in(n) (RRAM + (((n) & 127) << 8))
#define COUNTERS 0x7fff

/* McBSP2 Pin Control Register values */
#define BUSY_ON  0x3101
#define BUSY_OFF 0x3100

/* Control of the BUSY logic. BUSY will be ON when there are more than
   NEVT_BUSY_ON events waiting in the dual-port RAM. BUSY will be off
   when there are less than NEVT_BUSY_OFF events waiting. */
#ifndef NEVT_BUSY_ON
#define NEVT_BUSY_ON  35
#define NEVT_BUSY_OFF 10
#endif

/* Data sections shared with DMA are in block 1, everything else in block 0 */
#define NBUFOUT 4
#pragma DATA_SECTION(".febdata")
unsigned int data_in1[256];
#pragma DATA_SECTION(".febdata")
unsigned int data_in2[256];
#pragma DATA_SECTION(".febdata")
unsigned int data_out[NBUFOUT][1024];
#define src_ptr_out(n) data_out[(n) & 3]

extern "C"{
  void periph_init(void);
  int  pause(void);
}

#define IOBUFSIZE 7
static int iobuf[IOBUFSIZE];

static int nevt_input, nevt_input_next, nevt_out_rdy, nevt_out_next;
static int nevt_busyWentOn; // 3 Apr 2001 S.Simion

static inline void dumpState()
{
  if (DMA_3[0] & 12) return;
  iobuf[0] = IOBUFSIZE;
  iobuf[1] = RRAM[COUNTERS];
  iobuf[2] = nevt_input;
  iobuf[3] = nevt_input_next;
  iobuf[4] = nevt_out_rdy;
  iobuf[5] = nevt_out_next;
  iobuf[6] = nevt_busyWentOn; // SS 3/4/2001

  nevt_busyWentOn = 0;

  DMA_3[2] = 0x00180008;               /* Secondary control register */
  DMA_3[4] = (unsigned int) &iobuf;    /* Source address */
  DMA_3[8] = IOBUFSIZE;                /* wc = 6 */
  DMA_3[0] = 0x04015011;               /* Trigger the transfer */
}

int main()
{
  /* Number of output words for the next event to be transferred */
  int nw_out = SIZE_OUT;

  /* === Peripherals initialization */
  periph_init();

  /* === Initialization of DMA channel 2 and 3 destination address */
  DMA_2[6] = 0x70000000;
  DMA_3[6] = 0x60000000;
  /* === DMA Global Counter Reload B register, for histograms */
  DMA_0[11] = 256;

  /* --- Debugging only: set timer period */
  TCTL[1] = -1;
  TCTL[0] = 0x2c0;

  /* --- Debugging only */
  std::memset(data_out[0], 0, NBUFOUT*256*4);

#ifdef WAIT
  /* === Wait for INT7 */
  {
    int ier = pause();
    //    SER0[1] = (unsigned int) ier;
    SER0[1] = RRAM[COUNTERS];
  }
#endif

  nevt_input_next = 0; /* Next event to be copied by Input DMA */
  nevt_out_rdy    = 0; /* Number of events processed by DSP so far */
  nevt_out_next   = 0; /* Next event to be copied by Output DMA */

  unsigned short bufStates = (FREE1 | FREE2);

  ISR = 0x800;

  nevt_input = RRAM[COUNTERS] & 0xffff;

  /* Clear the BUSY flag */
  if (nevt_input < NEVT_BUSY_OFF) SER2[9] = BUSY_OFF;

  while(1){

    /* === Go ahead with the next output DMA if needed AND if DMA2 available */
    if (_ext(IFR, 20, 31) && nevt_out_next < nevt_out_rdy){
      ICR = 0x800;
      DMA_2[2] = 0x003c0008;
      DMA_2[4] = (unsigned int) src_ptr_out(nevt_out_next);
      DMA_2[8] = nw_out; /* should be nw_out */
      DMA_2[0] = 0x06018011;
      nevt_out_next++;
    }

    /* === Polling [see note] */
    if (bufStates & (FREE1 | FREE2 | RDY1) &&
	_extu(nevt_input, 16, 16) != _extu(RRAM[COUNTERS], 0, 16)){

      /* Read the counter a second time */
      nevt_input  = _clr(nevt_input, 0, 15);
      nevt_input += _extu(RRAM[COUNTERS], 0, 16);

      /* The input event counter is a 16-bit counter!*/
      if (nevt_input < nevt_input_next)
	nevt_input += 0x10000;

      /* Busy logic: set the busy flag */
      if (nevt_input - nevt_input_next > NEVT_BUSY_ON){
	SER2[9] = BUSY_ON;
	if (nevt_busyWentOn == 0) nevt_busyWentOn = nevt_input; // SS 3/4/2001
      }
    }

    /* Busy logic: clear the BUSY flag */
    if (nevt_input - nevt_input_next == NEVT_BUSY_OFF)
      SER2[9] = BUSY_OFF;


    if (nevt_input > nevt_input_next && bufStates & FREE1){
      DMA_0[4] = (unsigned int) src_ptr_in(nevt_input_next);
      DMA_0[6] = (unsigned int) data_in1;
      DMA_0[8] = SIZE_IN;
      DMA_0[0] = 0x51;
      bufStates ^= FREE1; // FREE -> BUSY
      nevt_input_next++;
    }
    else if (!(bufStates & nBSY1) && !(DMA_0[0] & 12)){
      bufStates |= RDY1;  // BUSY -> RDY
    }


    if (nevt_input > nevt_input_next && bufStates & FREE2){
      DMA_1[4] = (unsigned int) src_ptr_in(nevt_input_next);
      DMA_1[6] = (unsigned int) data_in2;
      DMA_1[8] = SIZE_IN;
      DMA_1[0] = 0x51;
      bufStates ^= FREE2; // FREE -> BUSY
      nevt_input_next++;
    }


    /* Ready input AND there is a free output buffer.
       Note that the last output buffer is never used directly,
       since the last DMA transfer may still be in progress */

    if (bufStates & RDY1 && nevt_out_rdy - nevt_out_next < NBUFOUT-1){
      unsigned int *outp = src_ptr_out(nevt_out_rdy);

#ifdef scope_debug
      /* --- Set FSX pin (scope probe) */
      SER1[9] = 0x3808;
#endif

      /* --- Process event */
      std::memcpy(outp+4, data_in1, SIZE_IN*sizeof(int));

      /* --- Insert own header */
      unsigned int ttc = src_ptr_in(nevt_out_rdy)[0xfe];
      outp[0] = nw_out-2;
      outp[1] = ttc;
      outp[2] = nevt_out_rdy;
      outp[nw_out-1] = 0xe0e;

      /* --- One more events waiting to be serviced by output DMA */
      nevt_out_rdy++;

      /* --- Input buffer becomes free */
      bufStates ^= (FREE1 | RDY1);

#ifdef scope_debug
      /* --- Clear FSX pin (scope probe) */
      SER1[9] = 0x3800;
#endif

      /* goto loop2 (fallthrough, likely to occur at high trigger rate) */
    }
    else{
      /* Upon INT7 send the various counters to the histogram FIFO */
      if (IFR & 0x80){
	ICR = 0x80;
	dumpState();
      }
      /* Testing INT10. If any serial data was received from VME, set new nw_out */
      else if (_ext(IFR, 21, 31)){
	ICR = 0x400;
	nw_out = SER0[0];
      }
      continue; /* goto loop1 */
    }


  loop2:

    /* === Go ahead with the next output DMA if needed AND if DMA2 available */
    if (_ext(IFR, 20, 31) && nevt_out_next < nevt_out_rdy){
      ICR = 0x800;
      DMA_2[2] = 0x003c0008;
      DMA_2[4] = (unsigned int) src_ptr_out(nevt_out_next);
      DMA_2[8] = src_ptr_out(nevt_out_next)[0]+2; /* should be nw_out */
      DMA_2[0] = 0x06018011;
      nevt_out_next++;
    }

    /* === Polling [see note] */
    if (bufStates & (FREE1 | FREE2 | RDY2) &&
	_extu(nevt_input, 16, 16) != _extu(RRAM[COUNTERS], 0, 16)){

      /* Read the counter a second time */
      nevt_input  = _clr(nevt_input, 0, 15);
      nevt_input += _extu(RRAM[COUNTERS], 0, 16);

      /* The input event counter is a 16-bit counter!*/
      if (nevt_input < nevt_input_next)
	nevt_input += 0x10000;

      /* Busy logic: set the busy flag */
      if (nevt_input - nevt_input_next > NEVT_BUSY_ON){
	SER2[9] = BUSY_ON;
	if (nevt_busyWentOn == 0) nevt_busyWentOn = nevt_input; // SS 3/4/2001
      }
    }

    /* Busy logic: clear the BUSY flag */
    if (nevt_input - nevt_input_next == NEVT_BUSY_OFF)
      SER2[9] = BUSY_OFF;


    if (nevt_input > nevt_input_next && bufStates & FREE2){
      DMA_1[4] = (unsigned int) src_ptr_in(nevt_input_next);
      DMA_1[6] = (unsigned int) data_in2;
      DMA_1[8] = SIZE_IN;
      DMA_1[0] = 0x51;
      bufStates ^= FREE2;
      nevt_input_next++;
    }
    else if (!(bufStates & nBSY2) && !(DMA_1[0] & 12)){
      bufStates |= RDY2;  // BUSY -> RDY
    }


    if (nevt_input > nevt_input_next && bufStates & FREE1){
      DMA_0[4] = (unsigned int) src_ptr_in(nevt_input_next);
      DMA_0[6] = (unsigned int) data_in1;
      DMA_0[8] = SIZE_IN;
      DMA_0[0] = 0x51;
      bufStates ^= FREE1;
      nevt_input_next++;
    }


    /* Ready input AND there is a free output buffer.
       Note that the last output buffer is never used directly,
       since the last DMA transfer may still be in progress */

    if (bufStates & RDY2 && nevt_out_rdy - nevt_out_next < NBUFOUT-1){
      unsigned int *outp = src_ptr_out(nevt_out_rdy);

#ifdef scope_debug
      /* --- Set FSX pin (scope probe) */
      SER1[9] = 0x3808;
#endif

      /* --- Process event */
      std::memcpy(outp+4, data_in2, SIZE_IN*sizeof(int));

      /* --- Insert own header */
      unsigned int ttc = src_ptr_in(nevt_out_rdy)[0xfe];
      outp[0] = nw_out-2;
      outp[1] = ttc;
      outp[2] = nevt_out_rdy;
      outp[nw_out-1] = 0xe0e;

      /* --- One more events waiting to be serviced by output DMA */
      nevt_out_rdy++;

      /* --- Input buffer becomes free */
      bufStates ^= (FREE2 | RDY2);

#ifdef scope_debug
      /* --- Clear FSX pin (scope probe) */
      SER1[9] = 0x3800;
#endif

      /* goto loop1 (likely to occur at high trigger rate) */
    }
    else{
      /* Upon INT7 send the various counters to the histogram FIFO */
      if (IFR & 0x80){
	ICR = 0x80;
	dumpState();
      }
      /* Testing INT10. If any serial data was received from VME, set new nw_out */
      else if (_ext(IFR, 21, 31)){
	ICR = 0x400;
	nw_out = SER0[0];
      }
      goto loop2;
    }
  }
}