Modbus/ModbusPDU.cpp at main · gcardi/Modbus · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
//---------------------------------------------------------------------------
#if defined(__BORLANDC__)
#pragma hdrstop
#endif

#include "ModbusPDU.h"

#define  MODBUS_PDU_FUNCTION_CODE_OFFSET   0
#define  MODBUS_PDU_EXCEPTION_CODE_OFFSET  1
#define  MODBUS_PDU_BYTE_COUNT_OFFSET      1

//---------------------------------------------------------------------------
namespace Modbus {
//---------------------------------------------------------------------------
namespace PDU {
//---------------------------------------------------------------------------

FunctionCode GetFunctionCode( TBytes const Buffer ) noexcept
{
    return FunctionCode( Buffer[MODBUS_PDU_FUNCTION_CODE_OFFSET] );
}
//---------------------------------------------------------------------------

ExceptionCode GetExceptionCode( TBytes const Buffer ) noexcept
{
    return ExceptionCode( Buffer[MODBUS_PDU_EXCEPTION_CODE_OFFSET] );
}
//---------------------------------------------------------------------------

uint8_t GetByteCount( TBytes const Buffer ) noexcept
{
    return Buffer[MODBUS_PDU_BYTE_COUNT_OFFSET];
}
//---------------------------------------------------------------------------

void RaiseExceptionIfReplyIsNotValid( Context const & Context,
                                      TBytes const Buffer,
                                      FunctionCode ExpectedFunctionCode )
{
    if ( Buffer.Length > 1 ) {
        FunctionCode const FnCode = GetFunctionCode( Buffer );
        if ( static_cast<int>( FnCode ) & 0x80 ) {
            RaiseStandardException( Context, GetExceptionCode( Buffer ) );
        }
        else if ( FnCode != ExpectedFunctionCode ) {
            throw EContextException(
                Context,
                Format(
                    _D( "Invalid Function Code: expected 0x%.2X, read 0x%.2X" )
                  , ARRAYOFCONST( (
                        ( static_cast<int>( ExpectedFunctionCode ) & 0xFF ),
                        ( static_cast<int>( FnCode ) & 0xFF )
                    ) )
                )
            );
        }
    }
    else {
        throw EContextException( Context, _D( "reply is too short" ) );
    }
}
//---------------------------------------------------------------------------

int WriteAddressPointCountPair( TBytes & OutBuffer, int StartIdx,
                                RegAddrType StartAddr, RegCountType PointCount ) noexcept
{
    OutBuffer[StartIdx++] = ( StartAddr >> 8 ) & 0xFF;   // Starting Address Hi
    OutBuffer[StartIdx++] = StartAddr & 0xFF;            // Starting Address Lo
    OutBuffer[StartIdx++] = ( PointCount >> 8 ) & 0xFF;  // No. of Points Hi
    OutBuffer[StartIdx++] = PointCount & 0xFF;           // No. of Points Lo
    return StartIdx;
}
//---------------------------------------------------------------------------

int WriteWord( TBytes & OutBuffer, int StartIdx, uint16_t Data ) noexcept
{
    OutBuffer[StartIdx++] = ( Data >> 8 ) & 0xFF;   // Data Hi
    OutBuffer[StartIdx++] = Data & 0xFF;            // Data Lo
    return StartIdx;
}
//---------------------------------------------------------------------------

int GetPayloadLength( Context const & Context, TBytes const Buffer, int BufferOffset )
{
    int const ByteCount = GetByteCount( Buffer );
    if ( ( Buffer.Length - 2 != ByteCount ) ||
         ( ~( Buffer.Length - BufferOffset ) & 1 ) )
    {
        throw EContextException(
            Context,
            Format(
                _D( "Invalid received frame lenght %d" )
              , ARRAYOFCONST( (
                    ByteCount
                ) )
            )
        );
    }
    return ByteCount;
}
//---------------------------------------------------------------------------

void CopyDataWords( Context const & Context, TBytes const Buffer,
                    int BufferOffset, uint16_t* Data )
{
    int DataLength = GetPayloadLength( Context, Buffer, BufferOffset );
    for ( int Idx = BufferOffset + 1 ; DataLength ; DataLength -= 2 ) {
        *Data++ = ( (uint16_t)Buffer[Idx] << 8 ) | ( (uint16_t)Buffer[Idx + 1] & 0xFF );
        Idx += 2;
    }
}

//---------------------------------------------------------------------------
}; // End of namespace PDU
//---------------------------------------------------------------------------
}; // End of namespace Modbus
//---------------------------------------------------------------------------