#include "mb.h"

#include "oled.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

/* Register details */
#define REG_DISCRETE_PBS		0
#define REG_DISCRETE_DIPS		1
#define REG_DISCRETE_PBS_SHIFT	4
#define REG_DISCRETE_PBS_MASK	0x1F
#define REG_DISCRETE_DIPS_SHIFT	9
#define REG_DISCRETE_DIPS_MASK	0x0F

#define REG_COILS_LEDS			0
#define REG_COILS_LEDS_MASK 	0x0F
#define REG_COILS_LEDS_DEFAULT	0x05

#define REG_INPUT_RTC			0
#define REG_INPUT_RV1			1


/* OLED details */
#define OLED_NROWS				2
#define OLED_NCOLS				19
#define OLED_NCHARS				( ( OLED_NROWS ) * ( OLED_NCOLS ) )
#define OLED_GREETING			"Microsemi Corp v1.xSmartFusion Modbus "



/* ------ 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;

	/* OLED holding registers */
	for ( i = 0; i < OLED_NCHARS; i++ )
	{
		usRegHoldingBuf[i] = OLED_GREETING[i];
	}

	/* LEDs coils register */
	ucRegCoilsBuf[REG_COILS_LEDS] = REG_COILS_LEDS_DEFAULT;

}

void synchronizing_modbus_regs(void)
{
	ULONG	gpios;
	UCHAR	oled_row;
	UCHAR 	oled_col;

	/* Flush holding registers (lower 8 bits only) to OLED */
	for ( oled_row = FIRST_LINE;
		  oled_row < ( FIRST_LINE + OLED_NROWS );
		  oled_row++ )
	{
		( void )OLED_set_cursor( oled_row, FIRST_CHARACTER );
		for ( oled_col = FIRST_CHARACTER;
			  oled_col < ( FIRST_CHARACTER + OLED_NCOLS );
			  oled_col++ )
		{
			( void )OLED_write_char(
					usRegHoldingBuf[( oled_row * OLED_NCOLS ) + oled_col]
			);
		}
	}

	/* Flush RTC value (in seconds) to 1st input register  */
	g_usRegInputBuf[REG_INPUT_RTC] = MSS_RTC_get_seconds_count(  );

	/* Flush RV1 3.3V pot voltage in mV to 2nd input register */
	g_usRegInputBuf[REG_INPUT_RV1] =
		ACE_convert_to_mV(
			ACE_get_first_channel(),
			ACE_get_ppe_sample( ACE_get_first_channel() ) );

	/* Flush pushbuttons & DIP switches to discrete input registers */
	gpios = MSS_GPIO_get_inputs(  );
	g_usRegDiscreteBuf[REG_DISCRETE_PBS] =
		( gpios >> REG_DISCRETE_PBS_SHIFT ) & REG_DISCRETE_PBS_MASK;
	g_usRegDiscreteBuf[REG_DISCRETE_DIPS] =
		( gpios >> REG_DISCRETE_DIPS_SHIFT ) & REG_DISCRETE_DIPS_MASK;

	/* 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;


}

/*-----------------------------------------------------------*/
