/* Copyright 2006-2013 The MathWorks, Inc. */

/* 
 * File: xil_interface_lib.c
 *
 * SIL/PIL support library
 *
 */

#include "xil_interface_lib.h"
#include "xil_interface_lib_private.h"
#include "xil_data_stream.h"
#include "xil_interface.h"
#include "xilcomms_wrapper.h"

#ifdef LDRA_ENABLED
#include "code_coverage_utils.h"
#endif /* LDRA_ENABLED */

/* Internal state variable holding information about the
 * XILIOData currently being processed. 
 *
 * Note: this variable is shared by UDATA processing code
 * and YDATA processing code and used for all IO.
 *
 */
static XILIOData * xilIODataPtr;
static int isTerminateCommand = 0;
static int processMsgSuccess = 1;

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

/* static functions */
static void getNextXILIOData(void) {
   /* increment xilIOData ptr if we have not reached the end */
   if (xilIODataPtr->memUnitLength!=0) {
      xilIODataPtr++;
   }
}

static XIL_PROCESSDATA_ERROR_CODE processUData(uint32_T fcnid,
        XIL_COMMAND_TYPE_ENUM command,
        uint32_T commandIdx,
        const IOUnit_T * src,
        uint16_T * size,
        int * processingUDataComplete)  {
    
    /* this function receives some arbitrary data size that needs to be 
     * into the appropriate memory addresses. Since it is not guaranteed 
     * that we will copy all the required data in one go, we need to keep 
     * track of which address (and size) in xilIODataPtr we last copied 
     * to so we can continue copying the remaining data the next time we 
     * receive them.
     */
    static MemUnit_T * uDataAddress = NULL;
    static uint32_T uDataLength = 0;
    uint16_T sizeToCopy = 0;
    XIL_PROCESSDATA_ERROR_CODE processDataErrorCode = XIL_PROCESSDATA_SUCCESS;    
    *processingUDataComplete = 0;
        
	/* initialize uDataAddress and uDataLength if necessary*/
    if (uDataAddress == NULL) {
		/* initialise xilIODataPtr before beginning to process data */
		if (xilGetUIOData(fcnid, command, commandIdx, &xilIODataPtr)) {
			/* no udata processing to be done - we're complete */
			*processingUDataComplete = 1;
			processDataErrorCode = XIL_PROCESSDATA_SUCCESS;
			return processDataErrorCode;
		}

        uDataAddress = xilIODataPtr->address;
        uDataLength = xilIODataPtr->memUnitLength;
    }
    
    while(!(*processingUDataComplete) && *size>0) {
        sizeToCopy = (uint16_T) MIN(uDataLength, *size);
        memcpy(uDataAddress, src, sizeToCopy);
        *size-=sizeToCopy;
        src+=sizeToCopy;
        uDataLength-=(uint32_T)sizeToCopy;
        uDataAddress+=sizeToCopy;
        if (uDataLength != 0)  {
            /* not done copying all the data to this xilIOData */            
        } else {
            /* done copying for this xilIOData, get next xilIOData */
            getNextXILIOData();
            
            if (xilIODataPtr->memUnitLength == 0) {
                /* done processing all xilIOData */                
                *processingUDataComplete = 1;
                uDataAddress = NULL;
            } else {
                /* reset our pointer to next xilIOData */
                uDataAddress = xilIODataPtr->address;
                uDataLength = xilIODataPtr->memUnitLength;
            }
        }
    }
    return processDataErrorCode;
}

static XIL_PROCESSDATA_ERROR_CODE processYData(uint32_T fcnid,
        XIL_COMMAND_TYPE_ENUM command,
        uint32_T commandIdx)  {
    
    XIL_PROCESSDATA_ERROR_CODE processDataErrorCode = XIL_PROCESSDATA_SUCCESS;
    int moreXILYData = 1;
    
    /* initialise before beginning to process data */
    MemUnit_T responseId = XIL_RESPONSE_OUTPUT_DATA;
    if (xilGetYIOData(fcnid, command, commandIdx, &xilIODataPtr)) {
        /* no ydata processing to be done - we're complete */
        processDataErrorCode = XIL_PROCESSDATA_SUCCESS;
        return processDataErrorCode;
    }
    /* send response id */
    if (xilWriteData(&responseId, sizeof(responseId)) != XIL_DATA_STREAM_SUCCESS) {
        processDataErrorCode = XIL_PROCESSDATA_DATA_STREAM_ERROR;
        return processDataErrorCode;
    }
    while(moreXILYData) {
        if (xilWriteData(xilIODataPtr->address, xilIODataPtr->memUnitLength) != XIL_DATA_STREAM_SUCCESS) {
            processDataErrorCode = XIL_PROCESSDATA_DATA_STREAM_ERROR;
            return processDataErrorCode;
        }
        /* get next xilIOData */
        getNextXILIOData();
        if (xilIODataPtr->memUnitLength == 0) {
            moreXILYData = 0;
        }
    }
    return processDataErrorCode;
}

static XIL_INTERFACE_LIB_ERROR_CODE finalizeCommandResponse(XIL_RESPONSE_ERROR_ID errorId) {

#ifdef CODE_INSTRUMENTATION_ENABLED
    /* flush the output stream
     * before beginning next command */
    if (codeInstrDataFlush() != XIL_DATA_STREAM_SUCCESS) {
        return XIL_INTERFACE_LIB_ERROR;
    }
#endif

   /* only send the error id if necessary */
   if (errorId != XIL_RESPONSE_ERROR_SUCCESS) {
      /* send response id code */
      MemUnit_T memUnitData = XIL_RESPONSE_ERROR;
      if (xilWriteData(&memUnitData, sizeof(memUnitData)) != XIL_DATA_STREAM_SUCCESS) {
        return XIL_INTERFACE_LIB_ERROR;      
      }
      /* send error id */
	  memUnitData = (MemUnit_T) errorId;
      if (xilWriteData(&memUnitData, sizeof(memUnitData)) != XIL_DATA_STREAM_SUCCESS) {
         return XIL_INTERFACE_LIB_ERROR;      
      }	  
   }
   /* flush the output stream 
    * before beginning next command */
   if (xilDataFlush() != XIL_DATA_STREAM_SUCCESS) {
      return XIL_INTERFACE_LIB_ERROR;      
   }   
   return XIL_INTERFACE_LIB_SUCCESS;
}

/* Given current state and command, compute the next state for handleReceive */
static XIL_HANDLE_RECV_STATE computeNextState(XIL_HANDLE_RECV_STATE currentState, XIL_COMMAND_TYPE_ENUM command) {
    
    XIL_HANDLE_RECV_STATE nextState;
    
    switch(currentState) {
        case XIL_READ_COMMAND_CODE_AND_FCNID:
            switch(command) {
                case XIL_INIT_COMMAND:
                    nextState = XIL_CALL_XILINTERFACE;
                    break;
                case XIL_CONST_OUTPUT_COMMAND:
                    nextState = XIL_PROCESS_OUTPUTS;
                    break;
                case XIL_INITIALIZE_COMMAND:
                case XIL_INITIALIZE_CONDITIONS_COMMAND:
                case XIL_TERMINATE_COMMAND:
                case XIL_PROCESS_PARAMS_COMMAND:
                    nextState = XIL_PROCESS_INPUTS;
                    break;
                case XIL_STEP_COMMAND:
                case XIL_ENABLE_COMMAND:
                case XIL_DISABLE_COMMAND:
                    nextState = XIL_READ_XILTID;
                    break;
                default:
                    nextState = XIL_INVALID_STATE;
                    break;
            }
            break;
        case XIL_READ_XILTID:
            switch(command) {
                case XIL_STEP_COMMAND:
                case XIL_ENABLE_COMMAND:
                case XIL_DISABLE_COMMAND:
                    nextState = XIL_PROCESS_INPUTS;
                    break;
                default:
                    nextState = XIL_INVALID_STATE;
                    break;
            }
            break;
        case XIL_PROCESS_INPUTS:
            switch(command) {
                case XIL_INITIALIZE_COMMAND:
                case XIL_INITIALIZE_CONDITIONS_COMMAND:
                case XIL_STEP_COMMAND:
                case XIL_ENABLE_COMMAND:
                case XIL_DISABLE_COMMAND:
                case XIL_TERMINATE_COMMAND:
                case XIL_PROCESS_PARAMS_COMMAND:
                    nextState = XIL_CALL_XILINTERFACE;
                    break;
                default:
                    nextState = XIL_INVALID_STATE;
                    break;
            }
            break;
        case XIL_CALL_XILINTERFACE:
            switch(command) {
                case XIL_INIT_COMMAND:
                case XIL_PROCESS_PARAMS_COMMAND:
                    nextState = XIL_COMPLETE;
                    break;
                case XIL_INITIALIZE_COMMAND:
                case XIL_INITIALIZE_CONDITIONS_COMMAND:
                case XIL_CONST_OUTPUT_COMMAND:
                case XIL_STEP_COMMAND:
                case XIL_ENABLE_COMMAND:
                case XIL_DISABLE_COMMAND:
                case XIL_TERMINATE_COMMAND:
                    nextState = XIL_PROCESS_OUTPUTS;
                    break;
                default:
                    nextState = XIL_INVALID_STATE;
                    break;
            }
            break;
        case XIL_PROCESS_OUTPUTS:
            /* done */
            nextState = XIL_COMPLETE;
            break;
        default:
            nextState = XIL_INVALID_STATE;
            break;
    }
    return nextState;
}

#define XIL_COMMAND_TYPE_SIZE sizeof(MemUnit_T)
#define XIL_COMMAND_CODE_IDX 0
#define XIL_COMMAND_FCNID_IDX (XIL_COMMAND_CODE_IDX + XIL_COMMAND_TYPE_SIZE)
#define XIL_COMMAND_FCNID_SIZE sizeof(uint32_T)
#define XIL_COMMAND_XILTID_IDX (XIL_COMMAND_FCNID_IDX + XIL_COMMAND_FCNID_SIZE)
#define XIL_COMMAND_XILTID_SIZE sizeof(uint32_T)
#define XIL_COMMAND_ERROR_STATUS_SIZE sizeof(MemUnit_T)

#define XIL_RUN_EXIT_ERROR processMsgSuccess = 0;      \
                           return
#define XIL_RUN_EXIT return

#define XIL_EXIT_ERROR_IF_INVALID_STATE if (state == XIL_INVALID_STATE) {      \
                                         XIL_RUN_EXIT_ERROR;                   \
                                      }                                                                                           

void xilProcessMsg(const MemUnit_T * const src, uint16_T size) {
    MemUnit_T commandCode;    	
    static uint32_T fcnid;
    static uint32_T xilTID = 0;
    static uint32_T commandIdx = 0;	
    int processingUDataComplete = 0;    
    static XIL_COMMAND_TYPE_ENUM command;            
    
    /* keep track of what we expect to receive next from the CS */
    static XIL_HANDLE_RECV_STATE state = XIL_READ_COMMAND_CODE_AND_FCNID;    

	const IOUnit_T* srcPtr = (const IOUnit_T*) &src[XIL_COMMAND_CODE_IDX];

    while (size > 0) {
        if (state==XIL_READ_COMMAND_CODE_AND_FCNID) {
            /* read commandCode */            
            memcpy(&commandCode, srcPtr, XIL_COMMAND_TYPE_SIZE);
            srcPtr+=XIL_COMMAND_TYPE_SIZE;
            size-=XIL_COMMAND_TYPE_SIZE;
            
            /* cast from the MemUnit commandCode to the enumeration commandCode */
            command = (XIL_COMMAND_TYPE_ENUM) commandCode;
            
            /* read fcnid */
			memcpy(&fcnid, srcPtr, XIL_COMMAND_FCNID_SIZE);            
            srcPtr+=XIL_COMMAND_FCNID_SIZE;
            size-=XIL_COMMAND_FCNID_SIZE;
            
            /* compute next state */
            state = computeNextState(state, command);
            XIL_EXIT_ERROR_IF_INVALID_STATE;
        }
        
        
        if (state==XIL_READ_XILTID) {
            /* read xilTID */
            memcpy(&xilTID, srcPtr, XIL_COMMAND_XILTID_SIZE);
            srcPtr+=XIL_COMMAND_XILTID_SIZE;
            size-=XIL_COMMAND_XILTID_SIZE;
            
            /* update commandIdx */
            commandIdx = xilTID;
            /* compute next state */
            state = computeNextState(state, command);
            XIL_EXIT_ERROR_IF_INVALID_STATE;
        }
        
        if (state == XIL_PROCESS_INPUTS) {
            /* process inputs if required by command */
            XIL_PROCESSDATA_ERROR_CODE processDataError;
            /* process UData */
            processDataError = processUData(fcnid, command, commandIdx, srcPtr, &size, &processingUDataComplete);
            if (processDataError == XIL_PROCESSDATA_DATA_STREAM_ERROR) {
                XIL_RUN_EXIT_ERROR;
            } else if (processDataError != XIL_PROCESSDATA_SUCCESS) {
                if (finalizeCommandResponse(XIL_RESPONSE_ERROR_PROCESS_UDATA) != XIL_INTERFACE_LIB_SUCCESS) {
                    XIL_RUN_EXIT_ERROR;
                }
                else {
                    XIL_RUN_EXIT;
                }
            }
            
            /* compute next state if we have finished receiving all inport
             * data */
            if (processingUDataComplete) {
                state = computeNextState(state, command);
                XIL_EXIT_ERROR_IF_INVALID_STATE
            }
        }
        
        if (state == XIL_CALL_XILINTERFACE) {
            /* call xilInterface function */
            switch(command) {
                case XIL_INIT_COMMAND:
                    if (xilGetDataTypeInfo() != XIL_INTERFACE_SUCCESS) {
                        XIL_RUN_EXIT_ERROR;
                    }
                    break;
                case XIL_CONST_OUTPUT_COMMAND:
                    /* no function */
                    break;
                case XIL_PROCESS_PARAMS_COMMAND:
                    if (xilProcessParams(fcnid) != XIL_INTERFACE_SUCCESS) {
                        if (finalizeCommandResponse(XIL_RESPONSE_ERROR_PROCESS_PARAMS) != XIL_INTERFACE_LIB_SUCCESS) {
                            XIL_RUN_EXIT_ERROR;
                        }
                        else {
                            XIL_RUN_EXIT;
                        }
                    }
                    break;
                case XIL_INITIALIZE_COMMAND:
                    if (xilInitialize(fcnid) != XIL_INTERFACE_SUCCESS) {
                        if (finalizeCommandResponse(XIL_RESPONSE_ERROR_INITIALIZE) != XIL_INTERFACE_LIB_SUCCESS) {
                            XIL_RUN_EXIT_ERROR;
                        }
                        else {
                            XIL_RUN_EXIT;
                        }
                    }
                    break;
                case XIL_INITIALIZE_CONDITIONS_COMMAND:
                    if (xilInitializeConditions(fcnid) != XIL_INTERFACE_SUCCESS) {
                        if (finalizeCommandResponse(XIL_RESPONSE_ERROR_INITIALIZE_CONDITIONS) != XIL_INTERFACE_LIB_SUCCESS) {
                            XIL_RUN_EXIT_ERROR;
                        }
                        else {
                            XIL_RUN_EXIT;
                        }
                    }
                    break;
                case XIL_STEP_COMMAND:
                    /* call output */
                    if (xilOutput(fcnid, xilTID) != XIL_INTERFACE_SUCCESS) {
                        if (finalizeCommandResponse(XIL_RESPONSE_ERROR_STEP) != XIL_INTERFACE_LIB_SUCCESS) {
                            XIL_RUN_EXIT_ERROR;
                        }
                        else {
                            XIL_RUN_EXIT;
                        }
                    }
                    /* call update */
                    if (xilUpdate(fcnid, xilTID) != XIL_INTERFACE_SUCCESS) {
                        if (finalizeCommandResponse(XIL_RESPONSE_ERROR_UPDATE) != XIL_INTERFACE_LIB_SUCCESS) {
                            XIL_RUN_EXIT_ERROR;
                        }
                        else {
                            XIL_RUN_EXIT;
                        }
                    }
                    break;
                case XIL_TERMINATE_COMMAND:
                    if (xilTerminate(fcnid) != XIL_INTERFACE_SUCCESS) {
                        if (finalizeCommandResponse(XIL_RESPONSE_ERROR_TERMINATE) != XIL_INTERFACE_LIB_SUCCESS) {
                            XIL_RUN_EXIT_ERROR;
                        }
                        else {
                            XIL_RUN_EXIT;
                        }
                    }
                    break;
                case XIL_ENABLE_COMMAND:
                    if (xilEnable(fcnid, xilTID) != XIL_INTERFACE_SUCCESS) {
                        if (finalizeCommandResponse(XIL_RESPONSE_ERROR_ENABLE) != XIL_INTERFACE_LIB_SUCCESS) {
                            XIL_RUN_EXIT_ERROR;
                        }
                        else {
                            XIL_RUN_EXIT;
                        }
                    }
                    break;
                case XIL_DISABLE_COMMAND:
                    if (xilDisable(fcnid, xilTID) != XIL_INTERFACE_SUCCESS) {
                        if (finalizeCommandResponse(XIL_RESPONSE_ERROR_DISABLE) != XIL_INTERFACE_LIB_SUCCESS) {
                            XIL_RUN_EXIT_ERROR;
                        }
                        else {
                            XIL_RUN_EXIT;
                        }
                    }
                    break;
                default:
                    XIL_RUN_EXIT_ERROR;
            }
            /* compute next state */
            state = computeNextState(state, command);
            XIL_EXIT_ERROR_IF_INVALID_STATE;
        }
        if (state == XIL_PROCESS_OUTPUTS) {
            XIL_PROCESSDATA_ERROR_CODE processDataError;
            /* process YData */
            processDataError = processYData(fcnid, command, commandIdx);
            if (processDataError == XIL_PROCESSDATA_DATA_STREAM_ERROR) {
                XIL_RUN_EXIT_ERROR;
            } else if (processDataError != XIL_PROCESSDATA_SUCCESS) {
                if (finalizeCommandResponse(XIL_RESPONSE_ERROR_PROCESS_YDATA) != XIL_INTERFACE_LIB_SUCCESS) {
                    XIL_RUN_EXIT_ERROR;
                }
                else {
                    XIL_RUN_EXIT;
                }
            }
            /* compute next state */
            state = computeNextState(state, command);
            XIL_EXIT_ERROR_IF_INVALID_STATE;
            break;
        }
    }
        

#ifdef LDRA_ENABLED
    /* upload code coverage execution history from target*/
    if (command == XIL_TERMINATE_COMMAND) {
        callTargetUploadFcns();
    }
#endif /* LDRA_ENABLED */
                
    if (state == XIL_COMPLETE) {
        /* finalize the response */
        if (finalizeCommandResponse(XIL_RESPONSE_ERROR_SUCCESS) != XIL_INTERFACE_LIB_SUCCESS) {
            XIL_RUN_EXIT_ERROR;
        }
        
        /* reset state */
        state = XIL_READ_COMMAND_CODE_AND_FCNID;
    }

    /* Terminate this process when XIL simulation is complete */
    if (command == XIL_TERMINATE_COMMAND) {
        isTerminateCommand = 1;        
    }
    XIL_RUN_EXIT;
    
}

XIL_INTERFACE_LIB_ERROR_CODE xilRun(void) {
    XIL_INTERFACE_LIB_ERROR_CODE errorCode = XIL_INTERFACE_LIB_SUCCESS;
    int success;    
    
    /* read one msg and pass to owning application */
    success = xilCommsRun(pXILCommsRTIOStream);
    success = success && processMsgSuccess;
    
    if (success) {        
        if (isTerminateCommand) {
            errorCode = XIL_INTERFACE_LIB_TERMINATE;
        } else {
            errorCode = XIL_INTERFACE_LIB_SUCCESS;
        }
        
    } else {
        errorCode = XIL_INTERFACE_LIB_ERROR;
    }
    return errorCode;
}