modbus-potato/ModbusUtil.cpp at master · lorcap/modbus-potato · 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
118
119
120
121
122
123
124
125
#include "ModbusUtil.h"
namespace ModbusPotato
{

bool
pdu_short (uint8_t station_address,
           const uint8_t* buffer,
           size_t buffer_len)
{
    const size_t len = pdu_len(station_address, buffer, buffer_len);

    return len <= 0 || buffer_len < len;
}

size_t
pdu_len (uint8_t station_address,
         const uint8_t* buffer,
         size_t buffer_len)
{
    return (station_address != 0)
         ? pdu_len_req(buffer, buffer_len)
         : pdu_len_rsp(buffer, buffer_len);
}

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

size_t
pdu_len_req (const uint8_t* buffer,
             size_t buffer_len)
{
    if (buffer_len == 0)
        return -1;

    const auto func = static_cast<function_code::function_code>(buffer[0]);

    size_t pos = -1;
    switch (func)
    {
    case function_code::read_coil_status             : pos = -1; break;
    case function_code::read_discrete_input_status   : pos = -1; break;
    case function_code::read_holding_registers       : pos = -1; break;
    case function_code::read_input_registers         : pos = -1; break;
    case function_code::write_single_coil            : pos = -1; break;
    case function_code::write_single_register        : pos = -1; break;
    case function_code::write_multiple_coils         : pos =  5; break;
    case function_code::write_multiple_registers     : pos =  5; break;
    case function_code::read_write_multiple_registers: pos =  9; break;
    }

    const size_t byte_count = (buffer_len > pos) ? buffer[pos] : 0;

    return pdu_len_req(func, byte_count);
}

size_t
pdu_len_rsp (const uint8_t* buffer,
             size_t buffer_len)
{
    if (buffer_len == 0)
        return -1;

    const auto func = static_cast<function_code::function_code>(buffer[0]);

    size_t pos = -1;
    switch (func)
    {
    case function_code::read_coil_status             : pos =  1; break;
    case function_code::read_discrete_input_status   : pos =  1; break;
    case function_code::read_holding_registers       : pos =  1; break;
    case function_code::read_input_registers         : pos =  1; break;
    case function_code::write_single_coil            : pos = -1; break;
    case function_code::write_single_register        : pos = -1; break;
    case function_code::write_multiple_coils         : pos = -1; break;
    case function_code::write_multiple_registers     : pos = -1; break;
    case function_code::read_write_multiple_registers: pos =  1; break;
    }

    const size_t byte_count = (buffer_len > pos) ? buffer[pos] : 0;

    return pdu_len_rsp(func, byte_count);
}

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

size_t
pdu_len_req (function_code::function_code func,
             size_t bc) // byte count
{
    switch (func)
    {
    case function_code::read_coil_status             :         return pdu_len_req_read_coil_status             (  ); break;
    case function_code::read_discrete_input_status   :         return pdu_len_req_read_discrete_input_status   (  ); break;
    case function_code::read_holding_registers       :         return pdu_len_req_read_holding_registers       (  ); break;
    case function_code::read_input_registers         :         return pdu_len_req_read_input_registers         (  ); break;
    case function_code::write_single_coil            :         return pdu_len_req_write_single_coil            (  ); break;
    case function_code::write_single_register        :         return pdu_len_req_write_single_register        (  ); break;
    case function_code::write_multiple_coils         : if (bc) return pdu_len_req_write_multiple_coils         (bc); break;
    case function_code::write_multiple_registers     : if (bc) return pdu_len_req_write_multiple_registers     (bc); break;
    case function_code::read_write_multiple_registers: if (bc) return pdu_len_req_read_write_multiple_registers(bc); break;
    }

    return -1;
}

size_t
pdu_len_rsp (function_code::function_code func,
             size_t bc) // byte count
{
    switch (func)
    {
    case function_code::read_coil_status             : if (bc) return pdu_len_rsp_read_coil_status             (bc); break;
    case function_code::read_discrete_input_status   : if (bc) return pdu_len_rsp_read_discrete_input_status   (bc); break;
    case function_code::read_holding_registers       : if (bc) return pdu_len_rsp_read_holding_registers       (bc); break;
    case function_code::read_input_registers         : if (bc) return pdu_len_rsp_read_input_registers         (bc); break;
    case function_code::write_single_coil            :         return pdu_len_rsp_write_single_coil            (  ); break;
    case function_code::write_single_register        :         return pdu_len_rsp_write_single_register        (  ); break;
    case function_code::write_multiple_coils         :         return pdu_len_rsp_write_multiple_coils         (  ); break;
    case function_code::write_multiple_registers     :         return pdu_len_rsp_write_multiple_registers     (  ); break;
    case function_code::read_write_multiple_registers: if (bc) return pdu_len_rsp_read_write_multiple_registers(bc); break;
    }

    return -1;
}

}