-
Notifications
You must be signed in to change notification settings - Fork 163
Expand file tree
/
Copy pathModbusServer.cpp
More file actions
209 lines (194 loc) · 6.71 KB
/
Copy pathModbusServer.cpp
File metadata and controls
209 lines (194 loc) · 6.71 KB
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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
// =================================================================================================
// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
// MIT license - see license.md for details
// =================================================================================================
#include <Arduino.h>
#include "ModbusServer.h"
#undef LOCAL_LOG_LEVEL
// #define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
#include "Logging.h"
// registerWorker: register a worker function for a certain serverID/FC combination
// If there is one already, it will be overwritten!
void ModbusServer::registerWorker(uint8_t serverID, uint8_t functionCode, MBSworker worker) {
workerMap[serverID][functionCode] = worker;
LOG_D("Registered worker for %02X/%02X\n", serverID, functionCode);
}
// getWorker: if a worker function is registered, return its address, nullptr otherwise
MBSworker ModbusServer::getWorker(uint8_t serverID, uint8_t functionCode) {
bool serverFound = false;
LOG_D("Need worker for %02X-%02X : ", serverID, functionCode);
// Search the FC map associated with the serverID
auto svmap = workerMap.find(serverID);
// Is there one?
if (svmap != workerMap.end()) {
serverFound = true;
// No explicit serverID entry found, but we may have one for ANY_SERVER
} else {
svmap = workerMap.find(ANY_SERVER);
if (svmap != workerMap.end()) {
serverFound = true;
}
}
// Did we find a serverID?
if (serverFound) {
// Yes. Now look for the function code in the inner map
bool functionCodeFound = false;
auto fcmap = svmap->second.find(functionCode);;
// Found it?
if (fcmap != svmap->second.end()) {
// Yes. Return the function pointer for it.
functionCodeFound = true;
// No, no explicit worker found, but may be there is one for ANY_FUNCTION_CODE?
} else {
fcmap = svmap->second.find(ANY_FUNCTION_CODE);;
// Found it?
if (fcmap != svmap->second.end()) {
// Yes. Return the function pointer for it.
functionCodeFound = true;
}
}
if (functionCodeFound) {
// Yes. Return the function pointer for it.
LOGRAW_D("Worker found for %02X/%02X\n", serverID, functionCode);
return fcmap->second;
}
}
// No matching function pointer found
LOGRAW_D("No matching worker found\n");
return nullptr;
}
// unregisterWorker; remove again all or part of the registered workers for a given server ID
// Returns true if the worker was found and removed
bool ModbusServer::unregisterWorker(uint8_t serverID, uint8_t functionCode) {
uint16_t numEntries = 0; // Number of entries removed
// Is there at least one entry for the serverID?
auto svmap = workerMap.find(serverID);
// Is there one?
if (svmap != workerMap.end()) {
// Yes. we may proceed with it
// Are we to look for a single serverID/FC combination?
if (functionCode) {
// Yes.
numEntries = svmap->second.erase(functionCode);
} else {
// No, the serverID shall be removed with all references
numEntries = workerMap.erase(serverID);
}
}
LOG_D("Removed %d worker entries for %d/%d\n", numEntries, serverID, functionCode);
return (numEntries ? true : false);
}
// isServerFor: if a worker function is registered for the given serverID, return true
// functionCode defaults to ANY_FUNCTION_CODE and will yield true for any function code,
// including ANY_FUNCTION_CODE :D
bool ModbusServer::isServerFor(uint8_t serverID, uint8_t functionCode) {
// Check if there is a non-nullptr function for the given combination
if (getWorker(serverID, functionCode)) {
return true;
}
return false;
}
// isServerFor: short version to look up if the server is known at all
bool ModbusServer::isServerFor(uint8_t serverID) {
// Check if there is a non-nullptr function for the given combination
auto svmap = workerMap.find(serverID);
// Is there one?
if (svmap != workerMap.end()) {
return true;
} else {
svmap = workerMap.find(ANY_SERVER);
if (svmap != workerMap.end()) {
return true;
}
}
return false;
}
// getMessageCount: read number of messages processed
uint32_t ModbusServer::getMessageCount() {
return messageCount;
}
// getErrorCount: read number of errors responded
uint32_t ModbusServer::getErrorCount() {
return errorCount;
}
// resetCounts: set both message and error counts to zero
void ModbusServer::resetCounts() {
{
LOCK_GUARD(cntLock, m);
messageCount = 0;
errorCount = 0;
}
}
// LocalRequest: get response from locally running server.
ModbusMessage ModbusServer::localRequest(ModbusMessage msg) {
ModbusMessage m;
uint8_t serverID = msg.getServerID();
uint8_t functionCode = msg.getFunctionCode();
LOG_D("Local request for %02X/%02X\n", serverID, functionCode);
HEXDUMP_V("Request", msg.data(), msg.size());
messageCount++;
// Try to get a worker for the request
MBSworker worker = getWorker(serverID, functionCode);
// Did we get one?
if (worker != nullptr) {
// Yes. call it and return the response
LOG_D("Call worker\n");
m = worker(msg);
LOG_D("Worker responded\n");
HEXDUMP_V("Worker response", m.data(), m.size());
// Process Response. Is it one of the predefined types?
if (m[0] == 0xFF && (m[1] == 0xF0 || m[1] == 0xF1)) {
// Yes. Check it
switch (m[1]) {
case 0xF0: // NIL
m.clear();
break;
case 0xF1: // ECHO
m.clear();
m.append(msg);
break;
default: // Will not get here, but lint likes it!
break;
}
}
HEXDUMP_V("Response", m.data(), m.size());
if (m.getError() != SUCCESS) {
errorCount++;
}
return m;
} else {
LOG_D("No worker found. Error response.\n");
// No. Is there at least one worker for the serverID?
if (isServerFor(serverID)) {
// Yes. Respond with "illegal function code"
m.setError(serverID, functionCode, ILLEGAL_FUNCTION);
} else {
// No. Respond with "Invalid server ID"
m.setError(serverID, functionCode, INVALID_SERVER);
}
errorCount++;
return m;
}
// We should never get here...
LOG_C("Internal problem: should not get here!\n");
m.setError(serverID, functionCode, UNDEFINED_ERROR);
errorCount++;
return m;
}
// Constructor
ModbusServer::ModbusServer() :
messageCount(0),
errorCount(0) { }
// Destructor
ModbusServer::~ModbusServer() {
}
// listServer: Print out all mapped server/FC combinations
void ModbusServer::listServer() {
for (auto it = workerMap.begin(); it != workerMap.end(); ++it) {
LOG_N("Server %3d: ", it->first);
for (auto it2 = it->second.begin(); it2 != it->second.end(); it2++) {
LOGRAW_N(" %02X", it2->first);
}
LOGRAW_N("\n");
}
}