/* Copyright 2013 The MathWorks, Inc. */

/* 
 * File: codeinstr_data_stream.c
 */					 

#include "xil_data_stream.h"
#include "xil_interface_lib.h"
#include "xilcomms_wrapper.h"
#include "rx_tx_buffer_sizes.h"

#define USED_WRITE_BUFFER_SIZE (COMMAND_COMPLETE_SIZE + codeInstrWriteDataAvail)
#define TX_BUFFER_SIZE (CODE_INSTR_TX_BUFFER_MEMUNIT_SIZE - COMMAND_COMPLETE_SIZE - BUFFER_HEADER_SIZE)

static IOUnit_T * codeInstrWriteBuffer = NULL;
static IOUnit_T * codeInstrWriteDataPtr;
static MemUnit_T* commandComplete;
static uint16_T codeInstrWriteDataAvail;
static void* pBuffer;

/* defined by xil_data_stream.c */
extern void* pXILCommsRTIOStream;

/* reset the write buffer */
static void resetCodeInstrWriteBuffer(void) {
   codeInstrWriteDataAvail = 0;
   codeInstrWriteDataPtr = &codeInstrWriteBuffer[WRITE_DATA_BUFFER_IDX];   
   /* ready for next command */
   *commandComplete = 0;
}

/* send pending writes */
static XIL_DATA_STREAM_ERROR_CODE sendWriteBuffer(void) {
   XIL_DATA_STREAM_ERROR_CODE errorCode = XIL_DATA_STREAM_SUCCESS;
   if (!xilCommsEnqueue(pXILCommsRTIOStream, pBuffer, CODE_INSTR_APPLICATION_ID, USED_WRITE_BUFFER_SIZE)) { 
      /* throw flush error */
      errorCode = XIL_DATA_FLUSH_ERROR;
      return errorCode;
   }
   /* reset */
   resetCodeInstrWriteBuffer();
   return errorCode;
}


XIL_INTERFACE_LIB_ERROR_CODE codeInstrInit(void) {   
   XIL_INTERFACE_LIB_ERROR_CODE errorCode = XIL_INTERFACE_LIB_SUCCESS;    

   /* Ask the CS to allocate a buffer that the code instrumentation app
    * service will use for transmission
    */
   if (!xilCommsAllocCodeInstrBuffer(pXILCommsRTIOStream, &pBuffer, CODE_INSTR_TX_BUFFER_MEMUNIT_SIZE)) {
       errorCode = XIL_INTERFACE_LIB_ERROR;
       return errorCode;
   }
   codeInstrWriteBuffer = xilCommsGetCodeInstrBufferDataPtr(pBuffer);
   
   /* set commandComplete pointer */
   commandComplete = (MemUnit_T *) &codeInstrWriteBuffer[COMMAND_COMPLETE_IDX];
   
   /* reset */
   resetCodeInstrWriteBuffer();
   
   return errorCode;
}

void codeInstrTerminate(void) {
    /* nothing to do */
}

XIL_DATA_STREAM_ERROR_CODE codeInstrWriteData(const MemUnit_T * src, uint32_T size) {
   XIL_DATA_STREAM_ERROR_CODE errorCode = XIL_DATA_STREAM_SUCCESS;
   const IOUnit_T * srcPtr = (const IOUnit_T *) src;   
   size_t transferAmount;
   uint16_T bufferAvail;
  
   /* block until all data is processed */
   while (size > 0) {      
      /* send if we have a full message worth of data */   
      if (codeInstrWriteDataAvail == TX_BUFFER_SIZE) {
         errorCode = sendWriteBuffer();
         if (errorCode != XIL_DATA_STREAM_SUCCESS) {
            return errorCode;
         }
      }
      bufferAvail = TX_BUFFER_SIZE - codeInstrWriteDataAvail;
      transferAmount = (uint16_T) MIN(bufferAvail, size);
      /* copy data into write buffer */
      memcpy((void *) codeInstrWriteDataPtr, srcPtr, transferAmount);
      size -= (uint32_T) transferAmount;
      codeInstrWriteDataAvail += (uint16_T) transferAmount;
      srcPtr += transferAmount;
      codeInstrWriteDataPtr += transferAmount;
   }
   return errorCode;
}


XIL_DATA_STREAM_ERROR_CODE codeInstrDataFlush(void) {
   XIL_DATA_STREAM_ERROR_CODE errorCode = XIL_DATA_STREAM_SUCCESS;
   /* final part of command */
   *commandComplete = 1;
   /* send the write buffer */
   errorCode = sendWriteBuffer();
   return errorCode;
}