/**************************************************************************
*
* Copyright (c) 2019-2020 Diality Inc. - All Rights Reserved.
*
* THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN
* WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER.
*
* @file CommBuffers.c
*
* @author (last) Sean Nash
* @date (last) 14-Oct-2020
*
* @author (original) Dara Navaei
* @date (original) 05-Nov-2019
*
***************************************************************************/

#include <HDCommon.h>
#include <string.h>     // For memcpy()

#include "CommBuffers.h"
#include "SystemComm.h"
#include "SystemCommMessages.h"
#include "Timers.h"

/**
 *  @addtogroup CommBuffers
 *  @{
 */

// ********** private definitions **********

#define COMM_BUFFER_LENGTH              512             ///< Max bytes in each comm buffer (each side of double buffer is this size)
#define DOUBLE_BUFFERS                  2               ///< Need 2 buffers for double buffering
#define BUFFER_OVERFLOW_PERSISTENCE_MS  5000            ///< How many ms buffer overflows must persist before fault

// ********** private data **********

static volatile U32 commBufferByteCount[ NUM_OF_COMM_BUFFERS ][ DOUBLE_BUFFERS ];      ///< For each buffer, how many bytes does it contain?  (also index to next available)
static volatile U32 activeDoubleBuffer[ NUM_OF_COMM_BUFFERS ];                         ///< For each buffer, which double buffer is being fed right now?
static U08 commBuffers[ NUM_OF_COMM_BUFFERS ][ DOUBLE_BUFFERS ][ COMM_BUFFER_LENGTH ]; ///< Each is double buffered to avoid thread contention
static U32 firstBufferOverflowTimeStamp = 0;                                           ///< Time stamp of a prior overflow event - allows for an overflow persistence check

// ********** private function prototypes **********

static U32 switchDoubleBuffer( COMM_BUFFER_T buffer );
static void getDataFromInactiveBuffer( COMM_BUFFER_T buffer, U08 *data, U32 len );

/*********************************************************************//**
 * @brief
 * The initCommBuffers function initializes the CommBuffers module.
 * @details Inputs: none
 * @details Outputs: CommBuffers module initialized.
 * @return none
 *************************************************************************/
void initCommBuffers( void )
{
    S32 b;

    // Reset and zero out all buffers
    for ( b = 0; b < NUM_OF_COMM_BUFFERS; b++ )
    {
        clearBuffer( (COMM_BUFFER_T)b );
    }
}

/*********************************************************************//**
 * @brief
 * The clearBuffer function clears (empties) a given buffer.
 * Caller should ensure buffer won't be used while this function is clearing
 * the buffer.
 * @details Inputs: none
 * @details Outputs: given buffer is cleared.
 * @param buffer the buffer to clear
 * @return none
 *************************************************************************/
void clearBuffer( COMM_BUFFER_T buffer )
{
    if ( buffer < NUM_OF_COMM_BUFFERS )
    {
        S32 d;

        // Thread protection for queue operations
        _disable_IRQ();

        activeDoubleBuffer[ buffer ] = 0;
        for ( d = 0; d < DOUBLE_BUFFERS; d++ )
        {
            commBufferByteCount[ buffer ][ d ] = 0;
            memset( &commBuffers[ buffer ][ d ][ 0 ], 0, COMM_BUFFER_LENGTH );
        }

        // Release thread protection
        _enable_IRQ();
    }
}

/*********************************************************************//**
 * @brief
 * The addToCommBuffer function adds data of specified length to a specified
 * communication buffer.  S/W fault if buffer too full to add data.
 * This function will always add to the active double buffer.
 * This function should only be called from the background, general, or
 * priority tasks (BG or IRQ) for thread safety.
 * @details Inputs: commBufferByteCount[], activeDoubleBuffer[]
 * @details Outputs: commBuffers[], commBufferByteCount[]
 * @param buffer which comm buffer to add data to
 * @param data pointer to byte array containing data to add
 * @param len length of data (in bytes)
 * @return TRUE if data added to buffer successfully, FALSE if not
 *************************************************************************/
BOOL addToCommBuffer( COMM_BUFFER_T buffer, U08* data, U32 len )
{
    BOOL result = FALSE;

    // Verify given buffer
    if ( buffer < NUM_OF_COMM_BUFFERS )
    {
        BOOL bufferFull = FALSE;
        U32 activeBuffer;
        U32 currentActiveBufCount;  // Where to start adding new data to buffer (after existing data)

        if ( ( FALSE == isHDOnlyCANNode() ) || ( FALSE == isCANBoxForXmit( (CAN_MESSAGE_BOX_T)buffer ) ) )
        {
            // Thread protection for queue operations
            _disable_IRQ();

            activeBuffer = activeDoubleBuffer[ buffer ];
            currentActiveBufCount = commBufferByteCount[ buffer ][ activeBuffer ];

            // Check to make sure buffer is not too full to service this add
            if ( len <= ( COMM_BUFFER_LENGTH - currentActiveBufCount ) )
            {
                U08 *buffPtr;  // Buffer destination for added data

                // Set destination pointer to end of active buffer data
                buffPtr = &commBuffers[ buffer ][ activeBuffer ][ currentActiveBufCount ];
                // Copy source data to destination buffer
                memcpy( buffPtr, data, len );
                // Adjust buffer count per this data add (also reserves space to add data before releasing thread protection)
                commBufferByteCount[ buffer ][ activeBuffer ] += len;
                // Data successfully added to buffer
                result = TRUE;
            }
            else // Buffer too full to add this much data
            {
                bufferFull = TRUE;
            }
            // Release thread protection
            _enable_IRQ();
        }

        // If buffer was full, check persistence - trigger s/w fault if persists
        if ( TRUE == bufferFull )
        {
#ifdef DEBUG_ENABLED
            char debugStr[ 100 ];
            sprintf( debugStr, "Buf OF:#%3d,%3d, %02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X\n", buffer, len, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7] );
            sendDebugData( (U08*)debugStr, strlen(debugStr) );
            sendDebugDataToUI( (U08*)debugStr );
#endif
            clearBuffer( buffer );
            // Not first overflow?
            if ( firstBufferOverflowTimeStamp != 0 )
            {
                // If buffer overflows persists, fault
                if ( calcTimeSince( firstBufferOverflowTimeStamp ) > BUFFER_OVERFLOW_PERSISTENCE_MS )
                {
                    SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_COMM_BUFFERS_ADD_TOO_MUCH_DATA, (U32)buffer )
                }
            }
            else // First overflow - set time stamp for persistence check
            {
                firstBufferOverflowTimeStamp = getMSTimerCount();
            }
        }
        else
        {   // If good for persistence time period, reset persistence check
            if ( ( firstBufferOverflowTimeStamp != 0 ) && ( calcTimeSince( firstBufferOverflowTimeStamp ) > BUFFER_OVERFLOW_PERSISTENCE_MS ) )
            {
                firstBufferOverflowTimeStamp = 0;
            }
        }
    }
    else // Invalid buffer given
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_COMM_BUFFERS_ADD_INVALID_BUFFER, buffer )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getFromCommBuffer function fills a given byte array with a given
 * number of bytes from a given buffer and returns the number of bytes
 * retrieved from the buffer. This function will draw from the inactive
 * double buffer first and, if needed, switch double buffers to draw the
 * rest of the requested data.
 * Only one function in one thread should be calling this function for a given
 * buffer.
 * @details Inputs: commBuffers[], commBufferByteCount[], activeDoubleBuffer[]
 * @details Outputs: commBuffers[], commBufferByteCount[], activeDoubleBuffer[],
 * and the given data array is populated with data from the buffer.
 * @param buffer which comm buffer to retrieve data from
 * @param data pointer to byte array to stuff data into
 * @param len number of bytes to retrieve into given data array.
 * @return the number of bytes retrieved.
 *************************************************************************/
U32 getFromCommBuffer( COMM_BUFFER_T buffer, U08* data, U32 len )
{
    U32 result = 0;

    // Verify given buffer
    if ( buffer < NUM_OF_COMM_BUFFERS )
    {
        // Thread protection for queue operations
        _disable_IRQ();
        // Verify requested # of bytes to get are in the buffer
        if ( ( len <= ( COMM_BUFFER_LENGTH * DOUBLE_BUFFERS ) ) && ( len <= numberOfBytesInCommBuffer( buffer ) ) )
        {
            U32 activeBuffer = activeDoubleBuffer[ buffer ];
            U32 inactiveBuffer = ( activeBuffer == 0 ? 1 : 0 );
            U32 bytesInInactiveBuffer = commBufferByteCount[ buffer ][ inactiveBuffer ];
            U32 sizeOfFirstConsumption = MIN( len, bytesInInactiveBuffer );

            // See what we can get from inactive buffer
            getDataFromInactiveBuffer( buffer, data, sizeOfFirstConsumption );  // Will switch double buffers if we empty inactive buffer
            // Will return # of bytes consumed
            result = sizeOfFirstConsumption;
            // Do we need more from active buffer?
            if ( len > bytesInInactiveBuffer )
            {
                U32 remNumOfBytes = len - sizeOfFirstConsumption;
                U08 *remPtr = data + sizeOfFirstConsumption;

                getDataFromInactiveBuffer( buffer, remPtr, remNumOfBytes );
                // Will return # of bytes consumed
                result += remNumOfBytes;
            }
        }
        // Release thread protection
        _enable_IRQ();
    }
    else // Invalid buffer given
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_COMM_BUFFERS_GET_INVALID_BUFFER, buffer )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The peekFromCommBuffer function fills a given byte array with a given
 * number of bytes from a given buffer.  This function does NOT consume
 * the bytes - it only peeks at them. A call to numberOfBytesInCommBuffer()
 * should be made before calling this function to determine how many bytes
 * are currently in the buffer.  Do not call this function with a "len"
 * longer than what is currently in the buffer.
 * @details Inputs: commBuffers[], commBufferByteCount[], activeDoubleBuffer[]
 * @details Outputs: given array populated with requested number of bytes from the buffer.
 * @param buffer which comm buffer to retrieve data from
 * @param data pointer to byte array to stuff data into
 * @param len number of bytes to retrieve into given data array.
 * @return the number of bytes retrieved.
 *************************************************************************/
U32 peekFromCommBuffer( COMM_BUFFER_T buffer, U08 *data, U32 len )
{
    U32  numOfBytesPeeked = 0;

    // Verify given buffer
    if ( buffer < NUM_OF_COMM_BUFFERS )
    {
        // Thread protection for queue operations
        _disable_IRQ();
        // Verify requested # of bytes to peek are in the buffer
        if ( ( len <= ( COMM_BUFFER_LENGTH * DOUBLE_BUFFERS ) ) && ( len <= numberOfBytesInCommBuffer( buffer ) ) )
        {
            U32 activeBuffer = activeDoubleBuffer[ buffer ];
            U32 inactiveBuffer = ( activeBuffer == 0 ? 1 : 0 );
            U32 bytesInInactiveBuffer = commBufferByteCount[ buffer ][ inactiveBuffer ];

            if ( len <= bytesInInactiveBuffer )
            {
                memcpy( data, &commBuffers[ buffer ][ inactiveBuffer ][ 0 ], len );
                numOfBytesPeeked = len;
            }
            else // Will need to get the rest from active buffer
            {
                U32 remNumOfBytes = len - bytesInInactiveBuffer;
                U08 *remPtr = data + bytesInInactiveBuffer;

                memcpy( data, &commBuffers[ buffer ][ inactiveBuffer ][ 0 ], bytesInInactiveBuffer );
                memcpy( remPtr, &commBuffers[ buffer ][ activeBuffer ][ 0 ], remNumOfBytes );
                numOfBytesPeeked = bytesInInactiveBuffer + remNumOfBytes;
            }
        }
        // Release thread protection
        _enable_IRQ();
    }
    else // Invalid buffer given
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_COMM_BUFFERS_PEEK_INVALID_BUFFER, buffer )
    }

    return numOfBytesPeeked;
}

/*********************************************************************//**
 * @brief
 * The numberOfBytesInCommBuffer function determines how many bytes
 * are currently contained in a given comm buffer.  Both double buffers
 * are considered for this.
 * @details Inputs: activeDoubleBuffer[], commBufferByteCount[]
 * @details Outputs: none
 * @param buffer which comm buffer to get byte count for
 * @return the number of bytes in the given comm buffer.
 *************************************************************************/
U32 numberOfBytesInCommBuffer( COMM_BUFFER_T buffer )
{
    U32 result = 0;

    // Verify given buffer
    if ( buffer < NUM_OF_COMM_BUFFERS )
    {
        U32 activeBuffer = activeDoubleBuffer[ buffer ];
        U32 inactiveBuffer = GET_TOGGLE( activeBuffer, 0, 1 );

        result = commBufferByteCount[ buffer ][ inactiveBuffer ] + commBufferByteCount[ buffer ][ activeBuffer ];
    }
    else // Invalid buffer
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_COMM_BUFFERS_COUNT_INVALID_BUFFER, buffer )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The switchDoubleBuffer function switches the active and inactive buffers
 * for the given buffer.
 * This function should only be called when the current inactive buffer has
 * been emptied.  Any unconsumed data in inactive buffer will be lost.
 * @details Inputs: activeDoubleBuffer[]
 * @details Outputs: activeDoubleBuffer[], commBufferByteCount[]
 * @param buffer which comm buffer to switch double buffers on
 * @return the new active buffer for the given buffer.
 *************************************************************************/
static U32 switchDoubleBuffer( COMM_BUFFER_T buffer )
{
    U32 activeBuffer = activeDoubleBuffer[ buffer ];
    U32 inactiveBuffer = ( activeBuffer == 0 ? 1 : 0 );

    // Ensure inactive buffer is reset before making active
    commBufferByteCount[ buffer ][ inactiveBuffer ] = 0;
    // Switch buffers
    activeDoubleBuffer[ buffer ] = inactiveBuffer;

    // Return the new active buffer (was just inactive)
    return inactiveBuffer;
}

/*********************************************************************//**
 * @brief
 * The getDataFromInactiveBuffer function retrieves a given number of bytes
 * from the inactive buffer of a given buffer.  This function should only be
 * called by getFromCommBuffer().  Params will be pre-validated there.
 * @details Inputs: commBuffers[], activeDoubleBuffer[], commBufferByteCount[]
 * @details Outputs: commBuffers[], activeDoubleBuffer[], commBufferByteCount[]
 * @param buffer which comm buffer get data from
 * @param data pointer to byte array to populate with data
 * @param len number of bytes to get from comm buffer
 * @return none
 *************************************************************************/
static void getDataFromInactiveBuffer( COMM_BUFFER_T buffer, U08 *data, U32 len )
{
    U32 activeBuffer = activeDoubleBuffer[ buffer ];
    U32 inactiveBuffer = ( activeBuffer == 0 ? 1 : 0 );
    U32 bytesInInactiveBuffer = commBufferByteCount[ buffer ][ inactiveBuffer ];

    // Get the requested data from inactive buffer
    memcpy( data, &commBuffers[ buffer ][ inactiveBuffer ][ 0 ], len );

    if ( len < bytesInInactiveBuffer )
    {
        U08 *endPtr = (&commBuffers[ buffer ][ inactiveBuffer ][ 0 ] + len);

        // Move un-consumed data in inactive buffer to start of inactive buffer
        memcpy( &commBuffers[ buffer ][ inactiveBuffer ][ 0 ], endPtr, ( bytesInInactiveBuffer - len ) );
        // Reduce byte count for inactive buffer by # of bytes consumed
        commBufferByteCount[ buffer ][ inactiveBuffer ] -= len;
    }
    else
    {
        // Inactive buffer has been emptied so switch double buffers
        switchDoubleBuffer( buffer );  // Switch will zero count off inactive buffer
    }
}

/**@}*/