AndyWhittaker
/
OpenPLC_v3
mirror of https://github.com/AndyWhittaker/OpenPLC_v3.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

PR-777 Address code review feedback

This commit is contained in:
Garret Fick 2019-11-19 10:48:32 -05:00
parent 465d9bcefc
commit be51b45358
8 changed files with 389 additions and 444 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
runtime/core/glue.cpp
View File

@ -13,11 +13,14 @@
// limitations under the License.
#include <cstdlib>
#include <algorithm>
#include <string>
#include "glue.h"
#include "ladder.h"
using namespace std;
/// @brief Locates a partiular glue variable from the list of all glue
/// variables.
/// @param dir The direction of the variable.
@ -28,9 +31,9 @@
/// @return The variable, or nullptr if there is no such variable.
const GlueVariable* GlueVariablesBinding::find(IecLocationDirection dir,
IecLocationSize size,
std::uint16_t msi,
std::uint8_t lsi) const {
for (std::uint16_t i = 0; i < this->size; ++i) {
uint16_t msi,
uint8_t lsi) const {
for (uint16_t i = 0; i < this->size; ++i) {
const GlueVariable& cur_var = glue_variables[i];
if (cur_var.dir == dir && cur_var.size == size && cur_var.msi == msi && cur_var.lsi == lsi) {
return &glue_variables[i];
@ -40,7 +43,7 @@ const GlueVariable* GlueVariablesBinding::find(IecLocationDirection dir,
return nullptr;
}
const GlueVariable* GlueVariablesBinding::find(const std::string& location) const {
const GlueVariable* GlueVariablesBinding::find(const string& location) const {
if (location.length() < 4 || location[0] != '%') {
return nullptr;
}
@ -85,7 +88,7 @@ const GlueVariable* GlueVariablesBinding::find(const std::string& location) cons
long msi = strtol(location.c_str() + 3, &end_msi, 10);
// Do we have more characters left in the string to read for lsi?
std::size_t start_lsi = end_msi + 1 - location.c_str();
size_t start_lsi = end_msi + 1 - location.c_str();
if (start_lsi >= location.length()) {
find(direction, size, msi, 0);
}
@ -95,3 +98,17 @@ const GlueVariable* GlueVariablesBinding::find(const std::string& location) cons
return find(direction, size, msi, lsi);
}
int32_t GlueVariablesBinding::find_max_msi(IecGlueValueType type,
IecLocationDirection dir) const {
int32_t max_index(-1);
const GlueVariable* glue_variables = this->glue_variables;
for (size_t index = 0; index < this->size; ++index) {
if (type == glue_variables[index].type && dir == glue_variables[index].dir) {
max_index = max(max_index, static_cast<int32_t>(glue_variables[index].msi));
}
}
return max_index;
}

9
runtime/core/glue.h
View File

@ -192,6 +192,15 @@ class GlueVariablesBinding {
/// @return the variable or null if there is no variable that matches all
/// criteria in the specification.
const GlueVariable* find(const std::string& location) const;
/// @brief Find the maximum most significant index for glued variables
/// that match the specified type and direction.
/// @param type the type to match on.
/// @param dir the direction to match on.
/// @return The maximum MSI or less than 0 if there are none with the
/// specified type.
std::int32_t find_max_msi(IecGlueValueType type,
IecLocationDirection dir) const;
};
#endif // CORE_GLUE_H

361
runtime/core/modbusslave/indexed_strategy.cpp
View File

@ -25,8 +25,6 @@
* @{
*/
#define MAX_REGISTERS 8192
#define MIN_16B_RANGE 1024
#define MAX_16B_RANGE 2047
#define MIN_32B_RANGE 2048
@ -36,38 +34,52 @@
using namespace std;
/// How many booleans do we have in a group for the glue variables.
/// We have 8!
const uint8_t BOOL_PER_GROUP(8);
inline size_t bool_group_size(const GlueBoolGroup* group) {
size_t size(0);
for (uint8_t index = 0; index < BOOL_PER_GROUP; ++index) {
if (group->values[index]) {
size += 1;
/// Simple indexed masks to get or set bit values in the Modbus packed
/// structure where we use every bit in the byte.
const uint8_t BOOL_BIT_MASK[8] = {
0x01,
0x02,
0x04,
0x08,
0x10,
0x20,
0x40,
0x80,
};
inline void initialize_mapped_from_group(uint16_t msi,
const GlueBoolGroup* group,
vector<MappedBool>& buffer) {
auto start_index = msi * BOOL_PER_GROUP;
for (size_t bool_index = 0; bool_index < BOOL_PER_GROUP; ++bool_index) {
auto mapped_index = start_index + bool_index;
if (group->values[bool_index]) {
buffer[mapped_index].value = group->values[bool_index];
buffer[mapped_index].cached_value = *group->values[bool_index];
}
}
return size;
}
int32_t find_bounds(const GlueVariablesBinding& bindings, IecGlueValueType type,
IecLocationDirection dir) {
int32_t max_index(-1);
IndexedStrategy::IndexedStrategy(const GlueVariablesBinding& bindings) :
glue_mutex(bindings.buffer_lock)
{
lock_guard<mutex> guard(*this->glue_mutex);
// This constructor is pretty long - what we are doing here is
// setting up structures that map between caches and the bound
// glue. These structures give fast (index -based) read/write of
// values. The caches ensure that we are unlikely to have to wait
// for a lock.
const GlueVariable* glue_variables = bindings.glue_variables;
for (size_t index = 0; index < bindings.size; ++index) {
if (type == glue_variables[index].type && dir == glue_variables[index].dir) {
max_index = max(max_index, static_cast<int32_t>(glue_variables[index].msi));
}
}
// Allocate a big enough read and write buffers. For the "int" types,
// we know the size in advance. For the boolean types, we don't, so
// figure out how big of a buffer we need for boolean types.
const int32_t max_coil_index = bindings.find_max_msi(IECVT_BOOL, IECLDT_OUT);
const int32_t max_di_index = bindings.find_max_msi(IECVT_BOOL, IECLDT_IN);
return max_index;
}
IndexedStrategy::IndexedStrategy(const GlueVariablesBinding& bindings) {
const int32_t max_coil_index = find_bounds(bindings, IECVT_BOOL, IECLDT_OUT);
const int32_t max_di_index = find_bounds(bindings, IECVT_BOOL, IECLDT_IN);
// Allocate a big enough read and write buffers
if (max_coil_index >= 0) {
coil_read_buffer.resize((max_coil_index + 1) * BOOL_PER_GROUP);
coil_write_buffer.resize((max_coil_index + 1) * BOOL_PER_GROUP);
@ -80,7 +92,6 @@ IndexedStrategy::IndexedStrategy(const GlueVariablesBinding& bindings) {
// Now go through the items and assign the pointers and initial values
// Note that if we have persistent storage, then the values have already
// been initialized.
const GlueVariable* glue_variables = bindings.glue_variables;
for (size_t index = 0; index < bindings.size; ++index) {
IecGlueValueType type = glue_variables[index].type;
@ -93,71 +104,77 @@ IndexedStrategy::IndexedStrategy(const GlueVariablesBinding& bindings) {
// multiplied by the number of booleans in that group
// If this index is out of range, then skip it.
if (dir == IECLDT_OUT) {
auto coil_start_index = msi * BOOL_PER_GROUP;
for (size_t bool_index = 0; bool_index < BOOL_PER_GROUP; ++bool_index) {
auto coil_index = coil_start_index + bool_index;
if (group->values[bool_index]) {
coil_read_buffer[coil_index].value = group->values[bool_index];
coil_read_buffer[coil_index].cached_value = *group->values[bool_index];
}
}
initialize_mapped_from_group(msi, group, coil_read_buffer);
} else if (dir == IECLDT_IN) {
auto di_start_index = msi * BOOL_PER_GROUP;
for (size_t bool_index = 0; bool_index < BOOL_PER_GROUP; ++bool_index) {
auto coil_index = di_start_index + bool_index;
if (group->values[bool_index]) {
di_read_buffer[coil_index].value = group->values[bool_index];
di_read_buffer[coil_index].cached_value = *group->values[bool_index];
}
}
initialize_mapped_from_group(msi, group, di_read_buffer);
}
}
else if (type == IECVT_INT) {
if (dir == IECLDT_OUT) {
int_register_read_buffer[msi].value = reinterpret_cast<IEC_INT*>(glue_variables[index].value);
int_register_read_buffer[msi].cached_value = *reinterpret_cast<IEC_INT*>(glue_variables[index].value);
int_register_read_buffer[msi].init(reinterpret_cast<IEC_INT*>(glue_variables[index].value));
} else if (dir == IECLDT_MEM && msi >= MIN_16B_RANGE && msi < MAX_16B_RANGE) {
intm_register_read_buffer[msi - MIN_16B_RANGE].value = reinterpret_cast<IEC_INT*>(glue_variables[index].value);
intm_register_read_buffer[msi - MIN_16B_RANGE].cached_value = *reinterpret_cast<IEC_INT*>(glue_variables[index].value);
intm_register_read_buffer[msi - MIN_16B_RANGE].init(reinterpret_cast<IEC_INT*>(glue_variables[index].value));
} else if (dir == IECLDT_IN) {
int_input_read_buffer[msi].value = reinterpret_cast<IEC_INT*>(glue_variables[index].value);
int_input_read_buffer[msi].cached_value = *reinterpret_cast<IEC_INT*>(glue_variables[index].value);
int_input_read_buffer[msi].init(reinterpret_cast<IEC_INT*>(glue_variables[index].value));
}
}
else if (type == IECVT_DINT && dir == IECLDT_MEM && msi <= MIN_32B_RANGE && msi < MAX_32B_RANGE) {
dintm_register_read_buffer[msi - MIN_32B_RANGE].value = reinterpret_cast<IEC_DINT*>(glue_variables[index].value);
dintm_register_read_buffer[msi - MIN_32B_RANGE].cached_value = *reinterpret_cast<IEC_DINT*>(glue_variables[index].value);
dintm_register_read_buffer[msi - MIN_32B_RANGE].init(reinterpret_cast<IEC_DINT*>(glue_variables[index].value));
}
else if (type == IECVT_LINT && dir == IECLDT_MEM && msi <= MIN_64B_RANGE && msi < MAX_64B_RANGE) {
lintm_register_read_buffer[msi - MIN_64B_RANGE].value = reinterpret_cast<IEC_LINT*>(glue_variables[index].value);
lintm_register_read_buffer[msi - MIN_64B_RANGE].cached_value = *reinterpret_cast<IEC_LINT*>(glue_variables[index].value);
lintm_register_read_buffer[msi - MIN_64B_RANGE].init(reinterpret_cast<IEC_LINT*>(glue_variables[index].value));
}
}
}
/// Exchange values between the read and write buffers. This would normally
/// be called periodically so that we transfer values between the cache
/// that we maintain here and the glue variables that the runtime uses.
/// @param write_buffer The write buffer we are exchanging with that contains
/// writes that have not yet be flushed to the glue.
/// @param read_buffer The read buffer that contains the glue variable
/// and a local cache of the value.
template <typename T>
void exchange(array<PendingValue<T>, NUM_REGISTER_VALUES>& write_buffer,
array<MappedValue<T>, NUM_REGISTER_VALUES>& read_buffer) {
for (size_t index = 0; index < write_buffer.size(); ++index) {
// Skip any index that is not mapped to a located variable.
if (!read_buffer[index].value) {
continue;
}
// If there was a write that hasn't be written, then transfer the
// value to the read buffer.
if (write_buffer[index].has_pending) {
*read_buffer[index].value = write_buffer[index].value;
}
// Finally, update our cached value so that we can read the value
// without waiting.
read_buffer[index].cached_value = *read_buffer[index].value;
}
}
void IndexedStrategy::Exchange() {
lock_guard<mutex> guard(*this->glue_mutex);
// Since we already figured out the mapping in an efficient structure
// the process of exchange is simply going through the items. We first
// handle populating writes into the structure.
// Update the read caches for coils and discrete inputs.
// Only the coils can be set, so we only only to check for pending
// writes to those.
for (size_t index = 0; index < coil_write_buffer.size(); ++index) {
if (coil_write_buffer[index].has_pending) {
*coil_read_buffer[index].value = coil_write_buffer[index].value;
}
coil_read_buffer[index].update_cache();
}
// Update the boolean values.
for (size_t index = 0; index < di_read_buffer.size(); ++index) {
di_read_buffer[index].update_cache();
}
exchange(int_register_write_buffer, int_register_read_buffer);
@ -165,228 +182,141 @@ void IndexedStrategy::Exchange() {
exchange(dintm_register_write_buffer, dintm_register_read_buffer);
exchange(lintm_register_write_buffer, lintm_register_read_buffer);
// Update the read caches
for (size_t index = 0; index < coil_read_buffer.size(); ++index) {
if (coil_read_buffer[index].value) {
coil_read_buffer[index].cached_value = *coil_read_buffer[index].value;
}
}
for (size_t index = 0; index < di_read_buffer.size(); ++index) {
if (di_read_buffer[index].value) {
di_read_buffer[index].cached_value = *di_read_buffer[index].value;
}
for (size_t index = 0; index < int_input_read_buffer.size(); ++index) {
int_input_read_buffer[index].update_cache();
}
}
modbus_errno IndexedStrategy::WriteCoil(uint16_t coil_index, bool value) {
lock_guard<mutex> guard(buffer_mutex);
if (coil_index < coil_write_buffer.size() && coil_read_buffer[coil_index].value) {
coil_write_buffer[coil_index].has_pending = true;
coil_write_buffer[coil_index].value = value;
if (coil_index < coil_write_buffer.size()
&& coil_read_buffer[coil_index].value) {
coil_write_buffer[coil_index].set(value);
return 0;
}
return -1;
}
modbus_errno IndexedStrategy::WriteMultipleCoils(uint16_t coil_start_index, uint16_t num_coils, uint8_t* values) {
lock_guard<mutex> guard(buffer_mutex);
const uint8_t bit_values[8] = {
0x01,
0x02,
0x04,
0x08,
0x10,
0x20,
0x40,
0x80,
};
modbus_errno IndexedStrategy::WriteMultipleCoils(uint16_t coil_start_index,
uint16_t num_coils,
uint8_t* values) {
if (coil_start_index + num_coils >= coil_write_buffer.size()) {
return -1;
}
lock_guard<mutex> guard(buffer_mutex);
for (uint16_t index = 0; index < num_coils; ++index) {
// Get the value from the packed structure
bool value = values[index / 8] & bit_values[index % 8];
coil_write_buffer[coil_start_index + index].has_pending = true;
coil_write_buffer[coil_start_index + index].value = value;
bool value = values[index / 8] & BOOL_BIT_MASK[index % 8];
coil_write_buffer[coil_start_index + index].set(value);
}
return 0;
}
modbus_errno IndexedStrategy::ReadCoils(uint16_t coil_start_index, uint16_t num_values, uint8_t* values) {
auto max_index = coil_start_index + num_values - 1;
if (max_index >= coil_read_buffer.size()) {
modbus_errno IndexedStrategy::ReadCoils(uint16_t coil_start_index,
uint16_t num_values,
uint8_t* values) {
return this->ReadBools(coil_read_buffer, coil_start_index, num_values, values);
}
modbus_errno IndexedStrategy::ReadDiscreteInputs(uint16_t di_start_index,
uint16_t num_values,
uint8_t* values) {
return this->ReadBools(di_read_buffer, di_start_index, num_values, values);
}
modbus_errno IndexedStrategy::ReadBools(const vector<MappedBool>& buffer,
uint16_t start_index,
uint16_t num_values,
uint8_t* values) {
auto max_index = start_index + num_values - 1;
if (max_index >= buffer.size()) {
return -1;
}
const uint8_t bit_values[8] = {
0x01,
0x02,
0x04,
0x08,
0x10,
0x20,
0x40,
0x80,
};
// Start by filling with 0 - we know the buffer is bigger than this can be
memset(values, 0, num_values / 8 + 1);
lock_guard<mutex> guard(buffer_mutex);
for (uint16_t index = 0; index < num_values; ++index) {
values[index / 8] |= coil_read_buffer[coil_start_index + index].cached_value ? bit_values[index % 8] : 0;
values[index / 8] |= buffer[start_index + index].cached_value ? BOOL_BIT_MASK[index % 8] : 0;
}
return 0;
}
modbus_errno IndexedStrategy::ReadDiscreteInputs(uint16_t di_start_index, uint16_t num_values, uint8_t* values) {
auto max_index = di_start_index + num_values - 1;
if (max_index >= di_read_buffer.size()) {
return -1;
}
const uint8_t bit_values[8] = {
0x01,
0x02,
0x04,
0x08,
0x10,
0x20,
0x40,
0x80,
};
// Start by filling with 0 - we know the buffer is bigger than this can be
memset(values, 0, num_values / 8 + 1);
lock_guard<mutex> guard(buffer_mutex);
for (uint16_t index = 0; index < num_values; ++index) {
values[index / 8] |= di_read_buffer[di_start_index + index].cached_value ? bit_values[index % 8] : 0;
}
return 0;
}
modbus_errno IndexedStrategy::WriteHoldingRegister(uint16_t hr_start_index, uint8_t* value) {
lock_guard<mutex> guard(buffer_mutex);
if (hr_start_index < MIN_16B_RANGE) {
int_register_write_buffer[hr_start_index].has_pending = true;
int_register_write_buffer[hr_start_index].value = mb_to_word(value[0], value[1]);
} else if (hr_start_index < MAX_16B_RANGE) {
hr_start_index -= MIN_16B_RANGE;
intm_register_write_buffer[hr_start_index].has_pending = true;
intm_register_write_buffer[hr_start_index].value = mb_to_word(value[0], value[1]);
} else if (hr_start_index < MAX_32B_RANGE) {
hr_start_index -= MIN_32B_RANGE;
uint32_t temp_value = (uint32_t) mb_to_word(value[0], value[1]);
PendingValue<IEC_DINT>& dst = dintm_register_write_buffer[hr_start_index / 2];
dst.has_pending = true;
if (hr_start_index % 2 == 0) {
// First word
dst.value = dst.value & 0x0000ffff;
dst.value = dst.value | temp_value;
} else {
// Second word
dst.value = dst.value & 0xffff0000;
dst.value = dst.value | temp_value;
}
} else if (hr_start_index < MAX_64B_RANGE) {
hr_start_index -= MIN_64B_RANGE;
uint64_t temp_value = (uint64_t) mb_to_word(value[0], value[1]);
PendingValue<IEC_LINT>& dst = lintm_register_write_buffer[hr_start_index / 4];
dst.has_pending = true;
if (hr_start_index % 4 == 0) {
// First word
dst.value = dst.value & 0x0000ffffffffffff;
dst.value = dst.value | (temp_value << 48);
}
else if (hr_start_index % 4 == 1) {
// Second word
dst.value = dst.value & 0xffff0000ffffffff;
dst.value = dst.value | (temp_value << 32);
}
else if (hr_start_index % 4 == 2) {
// Third word
dst.value = dst.value & 0xffffffff0000ffff;
dst.value = dst.value | (temp_value << 16);
}
else if (hr_start_index % 4 == 3) {
// Fourth word
dst.value = dst.value & 0xffffffffffff0000;
dst.value = dst.value | temp_value;
}
} else {
return -1;
}
return 0;
}
modbus_errno IndexedStrategy::WriteHoldingRegisters(uint16_t hr_start_index, uint16_t num_registers, uint8_t* value) {
modbus_errno IndexedStrategy::WriteHoldingRegisters(uint16_t hr_start_index,
uint16_t num_registers,
uint8_t* value) {
// Each holding register is 2 bytes. Depending on the destination value
// we are either updating the entire value or part of the value.
// Here we go through each index and update the appropriate part of the value.
lock_guard<mutex> guard(buffer_mutex);
for (uint16_t index = 0; index < num_registers; ++index) {
uint16_t hr_index = hr_start_index + index;
uint16_t word = mb_to_word(value[0], value[1]);
if (hr_index < MIN_16B_RANGE) {
int_register_write_buffer[hr_index].has_pending = true;
int_register_write_buffer[hr_index].value = mb_to_word(value[0], value[1]);
int_register_write_buffer[hr_index].set(word);
} else if (hr_index < MAX_16B_RANGE) {
hr_index -= MIN_16B_RANGE;
intm_register_write_buffer[hr_index].has_pending = true;
intm_register_write_buffer[hr_index].value = mb_to_word(value[0], value[1]);
intm_register_write_buffer[hr_index].set(word);
} else if (hr_index < MAX_32B_RANGE) {
hr_index -= MIN_32B_RANGE;
uint32_t temp_value = (uint32_t) mb_to_word(value[0], value[1]);
// The word we got is part of a larger 32-bit value, and we will
// bit shift to write the appropriate part. Resize to 32-bits
// so we can shift appropriately.
uint32_t partial_value = (uint32_t) word;
PendingValue<IEC_DINT>& dst = dintm_register_write_buffer[hr_index / 2];
dst.has_pending = true;
if (hr_index % 2 == 0) {
// First word
dst.value = dst.value & 0x0000ffff;
dst.value = dst.value | temp_value;
dst.value = dst.value | partial_value;
} else {
// Second word
dst.value = dst.value & 0xffff0000;
dst.value = dst.value | temp_value;
dst.value = dst.value | partial_value;
}
} else if (hr_index < MAX_64B_RANGE) {
hr_index -= MIN_64B_RANGE;
uint64_t temp_value = (uint64_t) mb_to_word(value[0], value[1]);
// Same as with a 32-bit value, here we are updating part of a
// 64-bit value, so resize so we can bit-shift appropriately.
uint64_t partial_value = (uint64_t) word;
PendingValue<IEC_LINT>& dst = lintm_register_write_buffer[hr_index / 4];
dst.has_pending = true;
if (hr_index % 4 == 0) {
// First word
dst.value = dst.value & 0x0000ffffffffffff;
dst.value = dst.value | (temp_value << 48);
}
else if (hr_index % 4 == 1) {
// Second word
dst.value = dst.value & 0xffff0000ffffffff;
dst.value = dst.value | (temp_value << 32);
}
else if (hr_index % 4 == 2) {
// Third word
dst.value = dst.value & 0xffffffff0000ffff;
dst.value = dst.value | (temp_value << 16);
}
else if (hr_index % 4 == 3) {
// Fourth word
dst.value = dst.value & 0xffffffffffff0000;
dst.value = dst.value | temp_value;
auto word_index = hr_index % 4;
switch (word_index) {
case 0:
// First word
dst.value = dst.value & 0x0000ffffffffffff;
dst.value = dst.value | (partial_value << 48);
break;
case 1:
// Second word
dst.value = dst.value & 0xffff0000ffffffff;
dst.value = dst.value | (partial_value << 32);
break;
case 2:
// Third word
dst.value = dst.value & 0xffffffff0000ffff;
dst.value = dst.value | (partial_value << 16);
break;
case 3:
// Fourth word
dst.value = dst.value & 0xffffffffffff0000;
dst.value = dst.value | partial_value;
break;
}
} else {
return -1;
}
// Move our buffer pointer so that we will handle the next register
value += 2;
}
return 0;
@ -438,18 +368,19 @@ modbus_errno IndexedStrategy::ReadInputRegisters(uint16_t hr_start_index, uint16
for (uint16_t index = 0; index < num_registers; ++index) {
uint16_t hr_index = hr_start_index + index;
uint16_t val;
if (hr_index < MIN_16B_RANGE) {
val = int_input_read_buffer[hr_index].cached_value;
} else {
if (hr_index >= MIN_16B_RANGE) {
return -1;
}
uint16_t val = int_input_read_buffer[hr_index].cached_value;
value[0] = mb_high_byte(val);
value[1] = mb_low_byte(val);
// Move to the next 16-bit position
value += 2;
}
return 0;
}

47
runtime/core/modbusslave/indexed_strategy.h
View File

@ -34,6 +34,12 @@ struct MappedBool {
MappedBool() : cached_value(0), value(nullptr) {}
IEC_BOOL cached_value;
IEC_BOOL *value;
inline void update_cache() {
if (this->value) {
this->cached_value = *this->value;
}
}
};
/// Defines a write that has been submitted via Modbus
@ -42,6 +48,12 @@ struct PendingBool {
PendingBool() : has_pending(false), value(0) {}
bool has_pending;
IEC_BOOL value;
/// Set the value and mark it as updated.
inline void set(IEC_BOOL val) {
this->has_pending = true;
this->value = val;
}
};
/// Defines the mapping between a located value
@ -51,6 +63,19 @@ struct MappedValue {
MappedValue() : cached_value(0), value(nullptr) {}
T cached_value;
T* value;
/// Initialize the glue link and the cached value.
/// @param val The glue variable to initialize from.
inline void init(T* val) {
this->value = val;
this->cached_value = *val;
}
inline void update_cache() {
if (this->value) {
this->cached_value = *this->value;
}
}
};
/// Defines a write that has been submitted via Modbus
@ -60,6 +85,12 @@ struct PendingValue {
PendingValue() : has_pending(false), value(0) {}
bool has_pending;
T value;
/// Set the value and mark it as updated.
inline void set(T val) {
this->has_pending = true;
this->value = val;
}
};
typedef std::uint8_t modbus_errno;
@ -107,13 +138,6 @@ class IndexedStrategy {
modbus_errno ReadDiscreteInputs(std::uint16_t coil_start_index,
std::uint16_t num_coils,
std::uint8_t* values);
/// Write the values from a single holding resister into the cache.
/// @param hr_start_index The index of the first holding register that
/// was written.
/// @param value An array of at least 2 bytes to read from.
modbus_errno WriteHoldingRegister(std::uint16_t hr_start_index,
std::uint8_t* values);
/// Write the values from holding resisters into the cache.
/// @param hr_start_index Index of the first holding register that
@ -141,6 +165,14 @@ class IndexedStrategy {
modbus_errno ReadInputRegisters(std::uint16_t hr_start_index,
std::uint16_t num_registers,
std::uint8_t* value);
private:
/// Read the boolean values from the mapped structure into the values.
modbus_errno ReadBools(const std::vector<MappedBool>& buffer,
std::uint16_t coil_start_index,
std::uint16_t num_values,
std::uint8_t* values);
private:
std::vector<MappedBool> coil_read_buffer;
std::vector<PendingBool> coil_write_buffer;
@ -162,6 +194,7 @@ class IndexedStrategy {
// Protects access to the cached values in this class.
std::mutex buffer_mutex;
std::mutex* glue_mutex;
};
/** @}*/

5
runtime/core/modbusslave/mb_util.h
View File

@ -1,4 +1,5 @@
// Copyright 2015 Thiago Alves
// Copyright 2019 Smarter Grid Solutions
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@ -22,8 +23,8 @@
*/
inline std::int16_t mb_to_word(std::uint8_t byte1, std::uint8_t byte2) {
std::int16_t returnValue = (std::int16_t)(byte1 << 8) | (std::int16_t)byte2;
return returnValue;
std::int16_t returnValue = (std::int16_t)(byte1 << 8) | (std::int16_t)byte2;
return returnValue;
}
inline std::uint8_t mb_low_byte(std::uint16_t w) {

376
runtime/core/modbusslave/slave.cpp
View File

@ -58,12 +58,6 @@
#define ERR_SLAVE_DEVICE_FAILURE 4
#define ERR_SLAVE_DEVICE_BUSY 6
#define bit_read(value, bit) (((value) >> (bit)) & 0x01)
#define bit_set(value, bit) ((value) |= (1UL << (bit)))
#define bit_clear(value, bit) ((value) &= ~(1UL << (bit)))
#define bit_write(value, bit, bitvalue) (bitvalue ? bit_set(value, bit) : bit_clear(value, bit))
using namespace std;
/// \brief Response to a Modbus Error
@ -71,46 +65,42 @@ using namespace std;
/// \param mb_error
int modbus_error(unsigned char *buffer, int mb_error)
{
buffer[4] = 0;
buffer[5] = 3;
buffer[7] = buffer[7] | 0x80; //set the highest bit
buffer[8] = mb_error;
return 9;
buffer[4] = 0;
buffer[5] = 3;
buffer[7] = buffer[7] | 0x80; //set the highest bit
buffer[8] = mb_error;
return 9;
}
inline int read_sizes(unsigned char* buffer, int buffer_size, int16_t& start,
int16_t& num_items) {
// This request must have at least 12 bytes. If it doesn't, it's a corrupted message
if (buffer_size < 12)
{
return modbus_error(buffer, ERR_ILLEGAL_DATA_VALUE);
}
// This request must have at least 12 bytes. If it doesn't, it's a corrupted message
if (buffer_size < 12)
{
return modbus_error(buffer, ERR_ILLEGAL_DATA_VALUE);
}
start = mb_to_word(buffer[8], buffer[9]);
num_items = mb_to_word(buffer[10], buffer[11]);
start = mb_to_word(buffer[8], buffer[9]);
num_items = mb_to_word(buffer[10], buffer[11]);
return 0;
return 0;
}
inline int read_sized_bytes(unsigned char* buffer, int buffer_size, int16_t& start,
int16_t& num_coils, int16_t& num_bytes) {
int ret = read_sizes(buffer, buffer_size, start, num_coils);
int ret = read_sizes(buffer, buffer_size, start, num_coils);
// Calculate the size of the message in bytes - they are packed into
// 8 coils per byte.
num_bytes = num_coils / 8;
// "Round up" since the above is integer division and truncates any
// extra items beyond 8.
if(num_bytes * 8 < num_coils) {
num_bytes++;
// Calculate the size of the message in bytes - they are packed into
// 8 coils per byte. Round up to make sure we cross the byte boundary
// if that many were requested.
num_bytes = ((num_coils + 7) / 8);
if (num_bytes > 255) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_ADDRESS);
}
if (num_bytes > 255) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_ADDRESS);
}
return 0;
return 0;
}
/// @brief Implementation of Modbus/TCP Read Coils
@ -118,20 +108,20 @@ inline int read_sized_bytes(unsigned char* buffer, int buffer_size, int16_t& sta
/// @param buffer_size
int read_coils(unsigned char *buffer, int buffer_size, IndexedStrategy* strategy)
{
int16_t start;
int16_t coil_data_length;
int16_t byte_data_length;
int ret = read_sized_bytes(buffer, buffer_size, start, coil_data_length, byte_data_length);
if (ret != 0) {
return ret;
}
int16_t start;
int16_t coil_data_length;
int16_t byte_data_length;
int ret = read_sized_bytes(buffer, buffer_size, start, coil_data_length, byte_data_length);
if (ret != 0) {
return ret;
}
// Preparing response
buffer[4] = mb_high_byte(byte_data_length + 3);
// Preparing response
buffer[4] = mb_high_byte(byte_data_length + 3);
// Number of bytes after this one
buffer[5] = mb_low_byte(byte_data_length + 3);
buffer[5] = mb_low_byte(byte_data_length + 3);
// Number of bytes of data
buffer[8] = byte_data_length;
buffer[8] = byte_data_length;
modbus_errno err = strategy->ReadCoils(start, coil_data_length, reinterpret_cast<uint8_t*>(buffer + 9));
if (err) {
@ -146,18 +136,18 @@ int read_coils(unsigned char *buffer, int buffer_size, IndexedStrategy* strategy
/// @param buffer_size
int read_discrete_inputs(unsigned char *buffer, int buffer_size, IndexedStrategy* strategy)
{
int16_t start;
int16_t input_data_length;
int16_t byte_data_length;
int ret = read_sized_bytes(buffer, buffer_size, start, input_data_length, byte_data_length);
if (ret != 0) {
return ret;
}
int16_t start;
int16_t input_data_length;
int16_t byte_data_length;
int ret = read_sized_bytes(buffer, buffer_size, start, input_data_length, byte_data_length);
if (ret != 0) {
return ret;
}
//Preparing response
buffer[4] = mb_high_byte(byte_data_length + 3);
buffer[5] = mb_low_byte(byte_data_length + 3); //Number of bytes after this one
buffer[8] = byte_data_length; //Number of bytes of data
//Preparing response
buffer[4] = mb_high_byte(byte_data_length + 3);
buffer[5] = mb_low_byte(byte_data_length + 3); //Number of bytes after this one
buffer[8] = byte_data_length; //Number of bytes of data
modbus_errno err = strategy->ReadDiscreteInputs(start, input_data_length, reinterpret_cast<uint8_t*>(buffer + 9));
if (err) {
@ -172,21 +162,21 @@ int read_discrete_inputs(unsigned char *buffer, int buffer_size, IndexedStrategy
/// @param bufferSize
int read_holding_registers(unsigned char *buffer, int buffer_size, IndexedStrategy* strategy)
{
int16_t start;
int16_t num_registers;
int ret = read_sizes(buffer, buffer_size, start, num_registers);
if (ret != 0) {
return ret;
}
int16_t start;
int16_t num_registers;
int ret = read_sizes(buffer, buffer_size, start, num_registers);
if (ret != 0) {
return ret;
}
int16_t byte_data_length = num_registers * 2;
int16_t byte_data_length = num_registers * 2;
//preparing response
buffer[4] = mb_high_byte(byte_data_length + 3);
buffer[5] = mb_low_byte(byte_data_length + 3); //Number of bytes after this one
buffer[8] = byte_data_length; //Number of bytes of data
//preparing response
buffer[4] = mb_high_byte(byte_data_length + 3);
buffer[5] = mb_low_byte(byte_data_length + 3); //Number of bytes after this one
buffer[8] = byte_data_length; //Number of bytes of data
modbus_errno err = strategy->ReadHoldingRegisters(start, num_registers, reinterpret_cast<uint8_t*>(buffer + 9));
modbus_errno err = strategy->ReadHoldingRegisters(start, num_registers, reinterpret_cast<uint8_t*>(buffer + 9));
if (err) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_ADDRESS);
}
@ -199,21 +189,21 @@ int read_holding_registers(unsigned char *buffer, int buffer_size, IndexedStrate
/// @param bufferSize
int read_input_registers(unsigned char *buffer, int buffer_size, IndexedStrategy* strategy)
{
int16_t start;
int16_t num_registers;
int ret = read_sizes(buffer, buffer_size, start, num_registers);
if (ret != 0) {
return ret;
}
int16_t start;
int16_t num_registers;
int ret = read_sizes(buffer, buffer_size, start, num_registers);
if (ret != 0) {
return ret;
}
int16_t byte_data_length = num_registers * 2;
int16_t byte_data_length = num_registers * 2;
//preparing response
buffer[4] = mb_high_byte(byte_data_length + 3);
buffer[5] = mb_low_byte(byte_data_length + 3); //Number of bytes after this one
buffer[8] = byte_data_length; //Number of bytes of data
//preparing response
buffer[4] = mb_high_byte(byte_data_length + 3);
buffer[5] = mb_low_byte(byte_data_length + 3); //Number of bytes after this one
buffer[8] = byte_data_length; //Number of bytes of data
modbus_errno err = strategy->ReadInputRegisters(start, num_registers, reinterpret_cast<uint8_t*>(buffer + 9));
modbus_errno err = strategy->ReadInputRegisters(start, num_registers, reinterpret_cast<uint8_t*>(buffer + 9));
if (err) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_ADDRESS);
}
@ -223,84 +213,84 @@ int read_input_registers(unsigned char *buffer, int buffer_size, IndexedStrategy
int write_coil(unsigned char* buffer, int buffer_size, IndexedStrategy* strategy)
{
int16_t start = mb_to_word(buffer[8], buffer[9]);
bool value = mb_to_word(buffer[10], buffer[11]) != 0;
int16_t start = mb_to_word(buffer[8], buffer[9]);
bool value = mb_to_word(buffer[10], buffer[11]) != 0;
modbus_errno err = strategy->WriteCoil(start, value);
if (err) {
modbus_errno err = strategy->WriteCoil(start, value);
if (err) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_ADDRESS);
}
buffer[4] = 0;
//Number of bytes after this one.
buffer[5] = 6;
return 12;
buffer[4] = 0;
//Number of bytes after this one.
buffer[5] = 6;
return 12;
}
int write_holding_register(unsigned char* buffer, int buffer_size, IndexedStrategy* strategy)
{
int16_t start = mb_to_word(buffer[8], buffer[9]);
int16_t start = mb_to_word(buffer[8], buffer[9]);
modbus_errno err = strategy->WriteHoldingRegister(start, buffer + 9);
if (err) {
modbus_errno err = strategy->WriteHoldingRegisters(start, 1, buffer + 9);
if (err) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_ADDRESS);
}
buffer[4] = 0;
//Number of bytes after this one.
buffer[5] = 6;
return 12;
buffer[4] = 0;
//Number of bytes after this one.
buffer[5] = 6;
return 12;
}
int write_multiple_coils(unsigned char* buffer, int buffer_size, IndexedStrategy* strategy)
{
int16_t start;
int16_t input_data_length;
int16_t byte_data_length;
int ret = read_sized_bytes(buffer, buffer_size, start, input_data_length, byte_data_length);
if (ret != 0) {
return ret;
}
int16_t start;
int16_t input_data_length;
int16_t byte_data_length;
int ret = read_sized_bytes(buffer, buffer_size, start, input_data_length, byte_data_length);
if (ret != 0) {
return ret;
}
// Check that we have enough bytes
if (buffer_size < (byte_data_length + 13) || buffer[12] != byte_data_length) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_VALUE);
}
// Check that we have enough bytes
if (buffer_size < (byte_data_length + 13) || buffer[12] != byte_data_length) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_VALUE);
}
modbus_errno err = strategy->WriteMultipleCoils(start, input_data_length, buffer + 13);
if (err) {
modbus_errno err = strategy->WriteMultipleCoils(start, input_data_length, buffer + 13);
if (err) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_ADDRESS);
}
//preparing response
buffer[4] = 0;
buffer[5] = 6; //Number of bytes after this one.
return 12;
//preparing response
buffer[4] = 0;
buffer[5] = 6; //Number of bytes after this one.
return 12;
}
int write_multiple_registers(unsigned char* buffer, int buffer_size, IndexedStrategy* strategy)
{
int16_t start;
int16_t num_registers;
int ret = read_sizes(buffer, buffer_size, start, num_registers);
if (ret != 0) {
return ret;
}
int16_t start;
int16_t num_registers;
int ret = read_sizes(buffer, buffer_size, start, num_registers);
if (ret != 0) {
return ret;
}
// Check that we have enough bytes
if (buffer_size < (num_registers * 2 + 13) || buffer[12] != num_registers * 2) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_VALUE);
}
// Check that we have enough bytes
if (buffer_size < (num_registers * 2 + 13) || buffer[12] != num_registers * 2) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_VALUE);
}
modbus_errno err = strategy->WriteHoldingRegisters(start, num_registers, buffer + 13);
if (err) {
modbus_errno err = strategy->WriteHoldingRegisters(start, num_registers, buffer + 13);
if (err) {
return modbus_error(buffer, ERR_ILLEGAL_DATA_ADDRESS);
}
//preparing response
buffer[4] = 0;
buffer[5] = 6; //Number of bytes after this one.
return 12;
//preparing response
buffer[4] = 0;
buffer[5] = 6; //Number of bytes after this one.
return 12;
}
/// Parse and process the client request and write back the response for it.
@ -311,93 +301,57 @@ int write_multiple_registers(unsigned char* buffer, int buffer_size, IndexedStra
int modbus_process_message(unsigned char *buffer, int buffer_size, void* user_data)
{
auto strategy = reinterpret_cast<IndexedStrategy*>(user_data);
int message_length = 0;
//check if the message is long enough
if (buffer_size < 8)
{
message_length = modbus_error(buffer, ERR_ILLEGAL_FUNCTION);
}
//check if the message is long enough
if (buffer_size < 8)
{
return modbus_error(buffer, ERR_ILLEGAL_FUNCTION);
}
//****************** Read Coils **********************
else if(buffer[7] == MB_FC_READ_COILS)
{
message_length = read_coils(buffer, buffer_size, strategy);
}
//*************** Read Discrete Inputs ***************
else if(buffer[7] == MB_FC_READ_INPUTS)
{
message_length = read_discrete_inputs(buffer, buffer_size, strategy);
}
//****************** Read Holding Registers ******************
else if(buffer[7] == MB_FC_READ_HOLDING_REGISTERS)
{
message_length = read_holding_registers(buffer, buffer_size, strategy);
}
//****************** Read Input Registers ******************
else if(buffer[7] == MB_FC_READ_INPUT_REGISTERS)
{
message_length = read_input_registers(buffer, buffer_size, strategy);
}
//****************** Write Coil **********************
else if(buffer[7] == MB_FC_WRITE_COIL)
{
message_length = write_coil(buffer, buffer_size, strategy);
}
//****************** Write Register ******************
else if(buffer[7] == MB_FC_WRITE_REGISTER)
{
message_length = write_holding_register(buffer, buffer_size, strategy);
}
//****************** Write Multiple Coils **********************
else if(buffer[7] == MB_FC_WRITE_MULTIPLE_COILS)
{
message_length = write_multiple_coils(buffer, buffer_size, strategy);
}
//****************** Write Multiple Registers ******************
else if(buffer[7] == MB_FC_WRITE_MULTIPLE_REGISTERS)
{
message_length = write_multiple_registers(buffer, buffer_size, strategy);
}
//****************** Function Code Error ******************
else
{
message_length = modbus_error(buffer, ERR_ILLEGAL_FUNCTION);
}
return message_length;
switch (buffer[7]) {
case MB_FC_READ_COILS:
return read_coils(buffer, buffer_size, strategy);
case MB_FC_READ_INPUTS:
return read_discrete_inputs(buffer, buffer_size, strategy);
case MB_FC_READ_HOLDING_REGISTERS:
return read_holding_registers(buffer, buffer_size, strategy);
case MB_FC_READ_INPUT_REGISTERS:
return read_input_registers(buffer, buffer_size, strategy);
case MB_FC_WRITE_COIL:
return write_coil(buffer, buffer_size, strategy);
case MB_FC_WRITE_REGISTER:
return write_holding_register(buffer, buffer_size, strategy);
case MB_FC_WRITE_MULTIPLE_COILS:
return write_multiple_coils(buffer, buffer_size, strategy);
case MB_FC_WRITE_MULTIPLE_REGISTERS:
return write_multiple_registers(buffer, buffer_size, strategy);
default:
return modbus_error(buffer, ERR_ILLEGAL_FUNCTION);
}
}
/// Arguments that are passed to the thread to exchange modbus data with the
/// runtime.
struct ModbusExchangeArgs {
IndexedStrategy* strategy;
volatile bool* run;
std::chrono::milliseconds interval;
IndexedStrategy* strategy;
volatile bool* run;
std::chrono::milliseconds interval;
};
/// The main function for the thread that is responsible for exchanging
/// modbus data with the located variables.
void* modbus_exchange_data(void* args) {
auto exchange_args = reinterpret_cast<ModbusExchangeArgs*>(args);
auto exchange_args = reinterpret_cast<ModbusExchangeArgs*>(args);
while (*exchange_args->run) {
spdlog::trace("Exchanging modbus master data");
exchange_args->strategy->Exchange();
this_thread::sleep_for(exchange_args->interval);
}
while (*exchange_args->run) {
spdlog::trace("Exchanging modbus master data");
exchange_args->strategy->Exchange();
this_thread::sleep_for(exchange_args->interval);
}
delete exchange_args;
delete exchange_args;
return nullptr;
return nullptr;
}
/// Container for reading in configuration from the config.ini
@ -470,24 +424,24 @@ int8_t modbus_slave_run(std::unique_ptr<std::istream, std::function<void(std::is
IndexedStrategy strategy(bindings);
pthread_t exchange_data_thread;
auto args = new ModbusExchangeArgs {
.strategy=&strategy,
.run=&run,
.interval=std::chrono::milliseconds(100)
};
pthread_t exchange_data_thread;
auto args = new ModbusExchangeArgs {
.strategy=&strategy,
.run=&run,
.interval=std::chrono::milliseconds(100)
};
int ret = pthread_create(&exchange_data_thread, NULL, modbus_exchange_data, args);
if (ret == 0) {
pthread_detach(exchange_data_thread);
} else {
delete args;
}
if (ret == 0) {
pthread_detach(exchange_data_thread);
} else {
delete args;
}
spdlog::info("Starting modbus slave on port {}", config.port);
spdlog::info("Starting modbus slave on port {}", config.port);
startServer(config.port, run, &modbus_process_message, &strategy);
pthread_join(exchange_data_thread, nullptr);
pthread_join(exchange_data_thread, nullptr);
return 0;
}

4
runtime/core/server.cpp
View File

@ -260,8 +260,8 @@ void startServer(uint16_t port, volatile bool& run_server, process_message_fn pr
.user_data=user_data
};
spdlog::trace("Server: Client accepted! Creating thread for the new client ID: {}...", client_fd);
int ret = pthread_create(&thread, NULL, handleConnections, args);
if (ret == 0) {
int success = pthread_create(&thread, NULL, handleConnections, args);
if (success == 0) {
pthread_detach(thread);
} else {
delete args;

4
runtime/test/modbusslave/test_indexed_strategy.cpp
View File

@ -98,7 +98,7 @@ SCENARIO("indexed_strategy", "")
{
int_val = 0;
uint8_t buffer[2] = { 0, 1 };
REQUIRE(strategy.WriteHoldingRegister(0, buffer) == 0);
REQUIRE(strategy.WriteHoldingRegisters(0, 1, buffer) == 0);
strategy.Exchange();
REQUIRE(int_val == 1);
}
@ -139,7 +139,7 @@ SCENARIO("indexed_strategy", "")
{
int_val = 0;
uint8_t buffer[2] = { 0, 1 };
REQUIRE(strategy.WriteHoldingRegister(1024, buffer) == 0);
REQUIRE(strategy.WriteHoldingRegisters(1024, 1, buffer) == 0);
strategy.Exchange();
REQUIRE(int_val == 1);
}