/**************************************************************************/
/* This File implements the call back functions and define the example */
/* data model required to be used with the freeModbus TCP stack */
/**************************************************************************/

/**************************************************************************/
/* FreeModbus includes */
/**************************************************************************/
#include "mb.h"
#include "mbutils.h"
#include "mss_gpio.h"

/* Application example data for Modbus */
/* Single Bit Read and Write */
#define REG_COILS_START         0x1
#define REG_COILS_SIZE          32

/* Single Bit Read Only */
#define REG_DESC_COILS_START    0x1
#define REG_DESC_COILS_SIZE     32

/* 16 bit Read Only */
#define REG_INPUT_START         0x1
#define REG_INPUT_NREGS         1

/* 16 bit Read and Write */
#define REG_HOLDING_START       0x1
#define REG_HOLDING_NREGS       40

#define REG_COILS_LEDS			0
#define REG_COILS_LEDS_MASK 	0x0F

/* ------ Static variables ---------------------------------*/
static USHORT   usRegInputStart = REG_INPUT_START;
static USHORT   * usRegInputBuf = (USHORT *)0x40021000;
static UCHAR    usRegHoldingStart = REG_HOLDING_START;
static USHORT   usRegHoldingBuf[REG_HOLDING_NREGS];
static UCHAR    ucRegCoilsBuf[REG_COILS_SIZE/8];
static UCHAR    ucRegDescCoilBuf[REG_DESC_COILS_SIZE/8];

/*
 * Initialize Modbus slave registers
 */

void init_modbus_regs( void )
{
    UCHAR i;
    for(i=0;i<REG_HOLDING_NREGS; i++)
    {
        usRegHoldingBuf[i] = 0;
    }
    for(i=0;i<(REG_COILS_SIZE/8); i++)
    {
        ucRegCoilsBuf[i] = 0;
    }
    for(i=0;i<(REG_DESC_COILS_SIZE/8); i++)
    {
        ucRegDescCoilBuf[i] = 0;
    }

}

void update_modbus_regs(void)
{
    static int i = 0;
    usRegHoldingBuf[0] = i++;
    ucRegCoilsBuf[0]   = i++;
    ucRegDescCoilBuf[0]= i++;

	/* Flush coils register to LEDs */
	MSS_GPIO_set_outputs(ucRegCoilsBuf[REG_COILS_LEDS] & REG_COILS_LEDS_MASK	);

}

eMBErrorCode
eMBRegInputCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs )
{
    eMBErrorCode    eStatus = MB_ENOERR;
    int             iRegIndex;
    USHORT          regVal;

    if( ( usAddress >= REG_INPUT_START )
        && ( usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS ) )


    {
        iRegIndex = ( int )( usAddress - usRegInputStart );
        while( usNRegs > 0 )
        {

            regVal = (USHORT)usRegInputBuf[iRegIndex];
            *pucRegBuffer++ = ( UCHAR ) ( regVal >> 8 );
            *pucRegBuffer++ = ( UCHAR ) ( regVal & 0xFF );
            iRegIndex++;
            usNRegs--;

        }
    }
    else
    {
        eStatus = MB_ENOREG;
    }
    return eStatus;
}

eMBErrorCode
eMBRegHoldingCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs, eMBRegisterMode eMode )
{
    eMBErrorCode    eStatus = MB_ENOERR;
    int             iRegIndex;
    USHORT          regVal;
    if( ( usAddress >= REG_HOLDING_START ) &&
        ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
    {
        iRegIndex = ( int )( usAddress - usRegHoldingStart );
        switch ( eMode )
        {
            /* Pass current register values to the protocol stack. */
        case MB_REG_READ:
            while( usNRegs > 0 )
            {

                regVal = usRegHoldingBuf[iRegIndex];
                *pucRegBuffer++ = ( UCHAR ) ( regVal >> 8 );
                *pucRegBuffer++ = ( UCHAR ) ( regVal & 0xFF );

                iRegIndex++;
                usNRegs--;
            }
            break;

            /* Update current register values with new values from the
             * protocol stack. */
        case MB_REG_WRITE:
            while( usNRegs > 0 )
            {
                usRegHoldingBuf[iRegIndex] = *pucRegBuffer++ << 8;
                usRegHoldingBuf[iRegIndex] |= *pucRegBuffer++;
                iRegIndex++;
                usNRegs--;
            }
        }
    }
    else
    {
        eStatus = MB_ENOREG;
    }
    return eStatus;
}

eMBErrorCode
eMBRegCoilsCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNCoils, eMBRegisterMode eMode )
{
    eMBErrorCode    eStatus = MB_ENOERR;
    short           iNCoils = ( short )usNCoils;
    unsigned short  usBitOffset;

    /* Check if we have registers mapped at this block. */
    if( ( usAddress >= REG_COILS_START ) &&
        ( usAddress + usNCoils <= REG_COILS_START + REG_COILS_SIZE ) )
    {
        usBitOffset = ( unsigned short )( usAddress - REG_COILS_START );
        switch ( eMode )
        {
                /* Read current values and pass to protocol stack. */
            case MB_REG_READ:
                while( iNCoils > 0 )
                {
                    *pucRegBuffer++ =
                        xMBUtilGetBits( ucRegCoilsBuf, usBitOffset,
                                        ( unsigned char )( iNCoils >
                                                           8 ? 8 :
                                                           iNCoils ) );
                    iNCoils -= 8;
                    usBitOffset += 8;
                }
                break;

                /* Update current register values. */
            case MB_REG_WRITE:
                while( iNCoils > 0 )
                {
                    xMBUtilSetBits( ucRegCoilsBuf, usBitOffset,
                                    ( unsigned char )( iNCoils > 8 ? 8 : iNCoils ),
                                    *pucRegBuffer++ );
                    iNCoils -= 8;
                }
                break;
        }

    }
    else
    {
        eStatus = MB_ENOREG;
    }
    return eStatus;

}

eMBErrorCode
eMBRegDiscreteCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNDiscrete )
{
    eMBErrorCode    eStatus = MB_ENOERR;
    unsigned short  usBitOffset;
    unsigned char   noOfBits;

    /* Check if we have registers mapped at this block. */
    if( ( usAddress >= REG_DESC_COILS_START ) &&
        ( usAddress + usNDiscrete <= REG_DESC_COILS_START + REG_DESC_COILS_SIZE ) )
    {
        usBitOffset = ( unsigned short )( usAddress - REG_DESC_COILS_START );
        while( usNDiscrete > 0 )
        {
            noOfBits = ( unsigned char )( usNDiscrete > 8 ? 8 : usNDiscrete );
            *pucRegBuffer++ =
                xMBUtilGetBits( ucRegDescCoilBuf, usBitOffset,
                                 noOfBits);
            usNDiscrete -= noOfBits;
            usBitOffset += noOfBits;
        }
    }
    else
    {
        eStatus = MB_ENOREG;
    }
    return eStatus;


}

