forked from eModbus/eModbus
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathModbusServer.cpp
More file actions
169 lines (156 loc) · 5.61 KB
/
ModbusServer.cpp
File metadata and controls
169 lines (156 loc) · 5.61 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
// =================================================================================================
// 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) {
// Search the FC map associated with the serverID
auto svmap = workerMap.find(serverID);
// Is there one?
if (svmap != workerMap.end()) {
// Yes. Now look for the function code in the inner map
auto fcmap = svmap->second.find(functionCode);;
// Found it?
if (fcmap != svmap->second.end()) {
// Yes. Return the function pointer for it.
LOG_D("Worker found for %02X/%02X\n", serverID, functionCode);
return fcmap->second;
// 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.
LOG_D("Worker found for %02X/ANY\n", serverID);
return fcmap->second;
}
}
}
// No matching function pointer found
LOG_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 any worker function is registered for the given serverID, return true
bool ModbusServer::isServerFor(uint8_t serverID) {
// Search the FC map for the serverID
auto svmap = workerMap.find(serverID);
// Is it there? Then return true
if (svmap != workerMap.end()) return true;
// No, serverID was not found. Return false
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());
// 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());
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);
}
return m;
}
// We should never get here...
LOG_C("Internal problem: should not get here!\n");
m.setError(serverID, functionCode, UNDEFINED_ERROR);
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");
}
}