#include <cstring>

#define SIZE_IN  400 // Typically 384 32-bit words
#define NBUF_IN  64  // Must be 2^N
#define SIZE_OUT 500
#define NBUF_OUT 4   // Must be 2^N

extern cregister volatile unsigned int IFR, ICR, ISR;

volatile unsigned int *const DMA_0 = (unsigned int *) 0x01840000;
volatile unsigned int *const TCTL0 = (unsigned int *) 0x01940000; // Timer 0
volatile unsigned int *const TCTL1 = (unsigned int *) 0x01980000; // Timer 1
volatile unsigned int *const SER2  = (unsigned int *) 0x01a40000;
volatile unsigned int *const SER1  = (unsigned int *) 0x01900000;
volatile unsigned int *const SER0  = (unsigned int *) 0x018c0000;

/* McBSP0 Pin Control Register values */
#define BUSY_ON  0x2800
#define BUSY_OFF 0x2820

/* 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. */
#define NEVT_BUSY_ON  20
#define NEVT_BUSY_OFF 10

/* Data sections shared with DMA are in block 1, everything else in block 0 */
#pragma DATA_SECTION(".febdata")
unsigned int data_in[NBUF_IN][SIZE_IN];
#define src_ptr_in(n) data_in[(n) & NBUF_IN-1]

#pragma DATA_SECTION(".febdata")
unsigned int data_out[NBUF_OUT][SIZE_OUT];
#define src_ptr_out(n) data_out[(n) & NBUF_OUT-1]

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

int main()
{
  int nevt_input, nevt_out_rdy, nevt_out_next;

  /* Number of output words for the next event to be transferred */
  int nw_out;

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


  /* --- Debugging only */
  std::memset(data_in[0],  0, NBUF_IN*SIZE_IN*sizeof(int));
  std::memset(data_out[0], 5, NBUF_OUT*SIZE_OUT*sizeof(int));


  /* === Initialization of DMA channel 1 for input events */
  DMA_0[15] = (unsigned int) data_in;  // GBLADDRB is the input event buffer
  DMA_0[11] = NBUF_IN << 16 | SIZE_IN; // GBLCNTB
  DMA_0[20] = 0x01400000;              // SRC1 is EMIF CE1 in FIFO mode
  DMA_0[22] = (unsigned int) data_in;  // DST1 is the input event buffer
  DMA_0[24] = NBUF_IN << 16 | SIZE_IN; // XFRCNT1
  DMA_0[18] = 0x40000;                 // SECCTL1 DMAC=FRAME COND
  DMA_0[16] = 0x44015043;              // PRICTL1

  /* === Initialization of DMA channel 2 and 3 destination address */
  DMA_0[7]  = 0x50000000;   // DST2 is now XCE1
  DMA_0[23] = 0x40000000;   // DST3 is now XCE0


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

  /* === Set FEB event counter period and start the counter */
  TCTL1[1] = (unsigned int)-1;
  TCTL1[0] = 0xc0;

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

  ISR = 0x800;

  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_0[3] = 0x003c0008;
      DMA_0[5] = (unsigned int) src_ptr_out(nevt_out_next);
      DMA_0[9] = nw_out; /* should be nw_out */
      DMA_0[1] = 0x06018011;
      nevt_out_next++;
    }

    /* === Polling on the value of Timer/Counter 1 */
    nevt_input = TCTL1[2];

    /* Busy logic: set the busy flag */
    if (nevt_input - nevt_out_rdy > NEVT_BUSY_ON)
      SER0[9] = BUSY_ON;

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


    /* 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 (nevt_input - nevt_out_rdy > 0 &&
	nevt_out_rdy - nevt_out_next < NBUF_OUT-1){
      unsigned int *out = src_ptr_out(nevt_out_rdy);
      unsigned int *inp = src_ptr_in(nevt_input-1);

#ifdef scope_debug
      /* --- Set FSX pin (scope probe) */
      SER0[9] |= 8;
#endif

      /* --- Process event */
      nw_out = SIZE_IN;
      std::memcpy(out+4, inp, nw_out*sizeof(int));
      out[nw_out-1] = 0xe0e;
      out[0] = nw_out-2;

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

#ifdef scope_debug
      /* --- Clear FSX pin (scope probe) */
      SER0[9] ^= 8;
#endif
    }
  }
}