// ================================================================================================= // eModbus: Copyright 2020, 2021 by Michael Harwerth, Bert Melis and the contributors to eModbus // MIT license - see license.md for details // ================================================================================================= #include "CoilData.h" #undef LOCAL_LOG_LEVEL #include "Logging.h" // Constructor: optional size in bits, optional initial value for all bits // Maximum size is 2000 coils (=250 bytes) CoilData::CoilData(uint16_t size, bool initValue) : CDsize(0), CDbyteSize(0), CDbuffer(nullptr) { // Limit the size to 2000 (Modbus rules) if (size > 2000) size = 2000; // Do we have a size? if (size) { // Calculate number of bytes needed CDbyteSize = byteIndex(size - 1) + 1; // Allocate and init buffer CDbuffer = new uint8_t[CDbyteSize]; memset(CDbuffer, initValue ? 0xFF : 0, CDbyteSize); if (initValue) { CDbuffer[CDbyteSize - 1] &= CDfilter[bitIndex(size - 1)]; } CDsize = size; } } // Alternate constructor, taking a "1101..." bit image char array to init CoilData::CoilData(const char *initVector) : CDsize(0), CDbyteSize(0), CDbuffer(nullptr) { // Init with bit image array. setVector(initVector); } // Destructor: take care of cleaning up CoilData::~CoilData() { if (CDbuffer) { delete CDbuffer; } } // Assignment operator CoilData& CoilData::operator=(const CoilData& m) { // Remove old data if (CDbuffer) { delete CDbuffer; } // Are coils in source? if (m.CDsize > 0) { // Yes. Allocate new buffer and copy data CDbuffer = new uint8_t[m.CDbyteSize]; memcpy(CDbuffer, m.CDbuffer, m.CDbyteSize); CDsize = m.CDsize; CDbyteSize = m.CDbyteSize; } else { // No, leave buffer empty CDsize = 0; CDbyteSize = 0; CDbuffer = nullptr; } return *this; } // Copy constructor CoilData::CoilData(const CoilData& m) : CDsize(0), CDbyteSize(0), CDbuffer(nullptr) { // Has the source coils at all? if (m.CDsize > 0) { // Yes. Allocate new buffer and copy data CDbuffer = new uint8_t[m.CDbyteSize]; memcpy(CDbuffer, m.CDbuffer, m.CDbyteSize); CDsize = m.CDsize; CDbyteSize = m.CDbyteSize; } } #ifndef NO_MOVE // Move constructor CoilData::CoilData(CoilData&& m) { // Copy all data CDbuffer = m.CDbuffer; CDsize = m.CDsize; CDbyteSize = m.CDbyteSize; // Then clear source m.CDbuffer = nullptr; m.CDsize = 0; m.CDbyteSize = 0; } // Move assignment CoilData& CoilData::operator=(CoilData&& m) { // Remove buffer, if already allocated if (CDbuffer) { delete CDbuffer; } // Are there coils in the source at all? if (m.CDsize > 0) { // Yes. Copy over all data CDbuffer = m.CDbuffer; CDsize = m.CDsize; CDbyteSize = m.CDbyteSize; // Then clear source m.CDbuffer = nullptr; m.CDsize = 0; m.CDbyteSize = 0; } else { // No, leave object empty. CDbuffer = nullptr; CDsize = 0; CDbyteSize = 0; } return *this; } #endif // Comparison operators bool CoilData::operator==(const CoilData& m) { // Self-compare is always true if (this == &m) return true; // Different sizes are never equal if (CDsize != m.CDsize) return false; // Compare the data if (CDsize > 0 && memcmp(CDbuffer, m.CDbuffer, CDbyteSize)) return false; return true; } // Inequality: invert the result of the equality comparison bool CoilData::operator!=(const CoilData& m) { return !(*this == m); } // Assignment of a bit image char array to re-init CoilData& CoilData::operator=(const char *initVector) { // setVector() may be unsuccessful - then data is deleted! setVector(initVector); return *this; } // If used as vector, return a complete slice CoilData::operator vector const () { // Create new vector to return vector retval; if (CDsize > 0) { // Copy over all buffer content retval.assign(CDbuffer, CDbuffer + CDbyteSize); } // return the copy (or an empty vector) return retval; } // slice: return a CoilData object with coils shifted leftmost // will return empty object if illegal parameters are detected CoilData CoilData::slice(uint16_t start, uint16_t length) { CoilData retval; // Any slice of an empty coilset is an empty coilset ;) if (CDsize == 0) return retval; // If start is beyond the available coils, return empty slice if (start > CDsize) return retval; // length default is all up to the end if (length == 0) length = CDsize - start; // Does the requested slice fit in the buffer? if ((start + length) <= CDsize) { // Yes, it does. Extend return object retval = CoilData(length); // Loop over all requested bits for (uint16_t i = start; i < start + length; ++i) { if (CDbuffer[byteIndex(i)] & (1 << bitIndex(i))) { retval.set(i - start, true); } } } return retval; } // operator[]: return value of a single coil bool CoilData::operator[](uint16_t index) const { if (index < CDsize) { return (CDbuffer[byteIndex(index)] & (1 << bitIndex(index))) ? true : false; } // Wrong parameter -> always return false return false; } // set functions to change coil value(s) // Will return true if done, false if impossible (wrong address or data) // set #1: alter one single coil bool CoilData::set(uint16_t index, bool value) { // Within coils? if (index < CDsize) { // Yes. Determine affected byte and bit therein uint16_t by = byteIndex(index); uint8_t mask = 1 << bitIndex(index); // Stamp out bit CDbuffer[by] &= ~mask; // If required, set it to 1 now if (value) { CDbuffer[by] |= mask; } return true; } // Wrong parameter -> always return false return false; } // set #2: alter a group of coils, overwriting it by the bits from vector newValue bool CoilData::set(uint16_t start, uint16_t length, vector newValue) { // Does the vector contain enough data for the specified size? if (newValue.size() >= (size_t)(byteIndex(length - 1) + 1)) { // Yes, we safely may call set #3 with it return set(start, length, newValue.data()); } return false; } // set #3: alter a group of coils, overwriting it by the bits from uint8_t buffer newValue // **** Watch out! **** // This may be a potential risk if newValue is pointing to an array shorter than required. // Then heap data behind the array may be used to set coils! bool CoilData::set(uint16_t start, uint16_t length, uint8_t *newValue) { // Does the requested slice fit in the buffer? if (length && (start + length) <= CDsize) { // Yes, it does. // Prepare pointers to the source byte and the bit within uint8_t *cp = newValue; uint8_t bitPtr = 0; // Loop over all bits to be set for (uint16_t i = start; i < start + length; i++) { // Get affected byte uint8_t by = byteIndex(i); // Calculate single-bit mask in target byte uint8_t mask = 1 << bitIndex(i); // Stamp out bit CDbuffer[by] &= ~mask; // is source bit set? if (*cp & (1 << bitPtr)) { // Yes. Set it in target as well CDbuffer[by] |= mask; } // Advance source bit ptr bitPtr++; // Overflow? if (bitPtr >= 8) { // Yes. move pointers to first bit in next source byte bitPtr = 0; cp++; } } return true; } return false; } // set #4: alter a group of coils, overwriting it by the coils in another CoilData object // Setting stops when either target storage or source coils are exhausted bool CoilData::set(uint16_t index, const CoilData& c) { // if source object is empty, return false if (c.empty()) return false; // If target is empty, or index is beyond coils, return false if (CDsize == 0 || index >= CDsize) return false; // Take the minimum of remaining coils after index and the length of c uint16_t length = CDsize - index; if (c.coils() < length) length = c.coils(); // Loop over all coils to be copied for (uint16_t i = index; i < index + length; ++i) { set(i, c[i - index]); } return true; } // set #5: alter a group of coils, overwriting it by a bit image array // Setting stops when either target storage or source bits are exhausted bool CoilData::set(uint16_t index, const char *initVector) { // if target is empty or index is beyond coils, return false if (CDsize == 0 || index >= CDsize) return false; // We do a single pass on the bit image array, until it ends or the target is exhausted const char *cp = initVector; // pointer to source array bool skipFlag = false; // Signal next character irrelevant while (*cp && index < CDsize) { switch (*cp) { case '1': // A valid 1 bit case '0': // A valid 0 bit // Shall we ignore it? if (skipFlag) { // Yes. just reset the ignore flag skipFlag = false; } else { // No, we can set it. First stamp out the existing bit CDbuffer[byteIndex(index)] &= ~(1 << bitIndex(index)); // Do we have a 1 bit here? if (*cp == '1') { // Yes. set it in coil storage CDbuffer[byteIndex(index)] |= (1 << bitIndex(index)); } index++; } break; case '_': // Skip next skipFlag = true; break; default: // anything else skipFlag = false; break; } cp++; } return true; } // Comparison against bit image array bool CoilData::operator==(const char *initVector) { const char *cp = initVector; // pointer to source array bool skipFlag = false; // Signal next character irrelevant uint16_t index = 0; // We do a single pass on the bit image array, until it ends or the target is exhausted while (*cp && index < CDsize) { switch (*cp) { case '1': // A valid 1 bit case '0': // A valid 0 bit // Shall we ignore it? if (skipFlag) { // Yes. just reset the ignore flag skipFlag = false; } else { // No, we can compare it uint8_t value = CDbuffer[byteIndex(index)] & (1 << bitIndex(index)); // Do we have a 1 bit here? if (*cp == '1') { // Yes. Is the source different? Then we can stop if (value == 0) return false; } else { // No, it is a 0. Different? if (value) return false; } index++; } break; case '_': // Skip next skipFlag = true; break; default: // anything else skipFlag = false; break; } cp++; } // So far everything was equal, but we may have more bits in the image array! if (*cp) { // There is more. Check for more valid bits while (*cp) { switch (*cp) { case '1': // A valid 1 bit case '0': // A valid 0 bit // Shall we ignore it? if (skipFlag) { // Yes. just reset the ignore flag skipFlag = false; } else { // No, a valid bit that exceeds the target coils count return false; } break; case '_': // Skip next skipFlag = true; break; default: // anything else skipFlag = false; break; } cp++; } } return true; } bool CoilData::operator!=(const char *initVector) { return !(*this == initVector); } // Init all coils by a readable bit image array bool CoilData::setVector(const char *initVector) { uint16_t length = 0; // resulting bit pattern length const char *cp = initVector; // pointer to source array bool skipFlag = false; // Signal next character irrelevant // Do a first pass to count all valid bits in array while (*cp) { switch (*cp) { case '1': // A valid 1 bit case '0': // A valid 0 bit // Shall we ignore it? if (skipFlag) { // Yes. just reset the ignore flag skipFlag = false; } else { // No, we can count it length++; } break; case '_': // Skip next skipFlag = true; break; default: // anything else skipFlag = false; break; } cp++; } // If there are coils already, trash them. if (CDbuffer) { delete CDbuffer; } CDsize = 0; CDbyteSize = 0; // Did we count a manageable number? if (length && length <= 2000) { // Yes. Init the coils CDsize = length; CDbyteSize = byteIndex(length - 1) + 1; // Allocate new coil storage CDbuffer = new uint8_t[CDbyteSize]; memset(CDbuffer, 0, CDbyteSize); // Prepare second loop uint16_t ptr = 0; // bit pointer in coil storage skipFlag = false; cp = initVector; // Do a second pass, converting 1 and 0 into coils (bits) // loop as above, only difference is setting the bits while (*cp) { switch (*cp) { case '1': case '0': if (skipFlag) { skipFlag = false; } else { // Do we have a 1 bit here? if (*cp == '1') { // Yes. set it in coil storage CDbuffer[byteIndex(ptr)] |= (1 << bitIndex(ptr)); } // advance bit pointer in any case 0 or 1 ptr++; } break; case '_': skipFlag = true; break; default: skipFlag = false; break; } cp++; } // We had content, so return true return true; } // No valid bits found, return false return false; } // init: set all coils to 1 or 0 (default) void CoilData::init(bool value) { if (CDsize > 0) { memset(CDbuffer, value ? 0xFF : 0, CDbyteSize); // Stamp out overhang bits CDbuffer[CDbyteSize - 1] &= CDfilter[bitIndex(CDsize - 1)]; } } // Return number of coils set to 1 (or not) // Uses Brian Kernighan's algorithm! uint16_t CoilData::coilsSetON() const { uint16_t count = 0; // Do we have coils at all? if (CDbyteSize) { // Yes. Loop over all bytes summing up the '1' bits for (uint8_t i = 0; i < CDbyteSize; ++i) { uint8_t by = CDbuffer[i]; while (by) { by &= by - 1; // this clears the LSB-most set bit count++; } } } return count; } uint16_t CoilData::coilsSetOFF() const { return CDsize - coilsSetON(); } #if !IS_LINUX // Not for Linux for the Print reference! // Print out a coil storage in readable form to ease debugging void CoilData::print(const char *label, Print& s) { uint8_t bitptr = 0; uint8_t labellen = strlen(label); uint8_t pos = labellen; // Put out the label s.print(label); // Print out all coils as "1" or "0" for (uint16_t i = 0; i < CDsize; ++i) { s.print((CDbuffer[byteIndex(i)] & (1 << bitptr)) ? "1" : "0"); pos++; // Have a blank after every group of 4 if (i % 4 == 3) { // Have a line break if > 80 characters, including the last group of 4 if (pos >= 80) { s.println(""); pos = 0; // Leave a nice empty space below the label while (pos++ < labellen) { s.print(" "); } } else { s.print(" "); pos++; } } bitptr++; bitptr &= 0x07; } s.println(""); } #endif