diff --git a/Modbus.cpp b/Modbus.cpp
index 0e674ff..d1b624d 100644
--- a/Modbus.cpp
+++ b/Modbus.cpp
@@ -11,12 +11,12 @@ std::vector<TRegister> _regs;
 std::vector<TCallback> _callbacks;
 #endif
 
-uint16_t Modbus::mbCallback(TRegister* reg, uint16_t val, TCallback::CallbackType t)
-{
+uint16_t Modbus::mbCallback(TRegister *reg, uint16_t val, TCallback::CallbackType t) {
     uint16_t newVal = val;
-    std::vector<TCallback>::iterator it = _callbacks.begin();
+    auto it = _callbacks.begin();
     do {
-        it = std::find_if(it, _callbacks.end(), [reg, t](TCallback& cb) { return cb.address == reg->address && cb.type == t; });
+        it = std::find_if(it, _callbacks.end(),
+                          [reg, t](TCallback &cb) { return cb.address == reg->address && cb.type == t; });
         if (it != _callbacks.end()) {
             newVal = it->cb(reg, newVal);
             it++;
@@ -25,31 +25,28 @@ uint16_t Modbus::mbCallback(TRegister* reg, uint16_t val, TCallback::CallbackTyp
     return newVal;
 }
 
-TRegister* Modbus::searchRegister(TAddress address)
-{
-    std::vector<TRegister>::iterator it = std::find_if(_regs.begin(), _regs.end(), [address](TRegister& addr) { return addr.address == address; });
+TRegister *Modbus::searchRegister(TAddress address) {
+    auto it = std::find_if(_regs.begin(), _regs.end(), [address](TRegister &addr) { return addr.address == address; });
     if (it != _regs.end())
         return &*it;
     return nullptr;
 }
 
-bool Modbus::addReg(TAddress address, uint16_t value, uint16_t numregs)
-{
+bool Modbus::addReg(TAddress address, uint16_t value, uint16_t numregs) {
 #ifdef MB_MAX_REGS
     if (_regs.size() + numregs > MB_MAX_REGS)
         return false;
 #endif
     for (uint16_t i = 0; i < numregs; i++) {
         if (!searchRegister(address + i))
-            _regs.push_back({ address + i, value });
+            _regs.push_back({address + i, value});
     }
     //std::sort(_regs.begin(), _regs.end());
     return true;
 }
 
-bool Modbus::Reg(TAddress address, uint16_t value)
-{
-    TRegister* reg;
+bool Modbus::Reg(TAddress address, uint16_t value) {
+    TRegister *reg;
     reg = searchRegister(address); //search for the register address
     if (reg) { //if found then assign the register value to the new value.
         if (cbEnabled) {
@@ -62,9 +59,8 @@ bool Modbus::Reg(TAddress address, uint16_t value)
         return false;
 }
 
-uint16_t Modbus::Reg(TAddress address)
-{
-    TRegister* reg;
+uint16_t Modbus::Reg(TAddress address) {
+    TRegister *reg;
     reg = searchRegister(address);
     if (reg)
         if (cbEnabled) {
@@ -76,9 +72,8 @@ uint16_t Modbus::Reg(TAddress address)
         return 0;
 }
 
-bool Modbus::removeReg(TAddress address, uint16_t numregs)
-{
-    TRegister* reg;
+bool Modbus::removeReg(TAddress address, uint16_t numregs) {
+    TRegister *reg;
     bool atLeastOne = false;
     for (uint16_t i = 0; i < numregs; i++) {
         reg = searchRegister(address + i);
@@ -92,115 +87,114 @@ bool Modbus::removeReg(TAddress address, uint16_t numregs)
     return atLeastOne;
 }
 
-void Modbus::slavePDU(uint8_t* frame)
-{
-    FunctionCode fcode = (FunctionCode)frame[0];
-    uint16_t field1 = (uint16_t)frame[1] << 8 | (uint16_t)frame[2];
-    uint16_t field2 = (uint16_t)frame[3] << 8 | (uint16_t)frame[4];
+void Modbus::slavePDU(uint8_t *frame) {
+    auto fcode = (FunctionCode) frame[0];
+    uint16_t field1 = (uint16_t) frame[1] << 8 | (uint16_t) frame[2];
+    uint16_t field2 = (uint16_t) frame[3] << 8 | (uint16_t) frame[4];
     uint16_t bytecount_calc;
     uint16_t k;
     switch (fcode) {
-    case FC_WRITE_REG:
-        //field1 = reg, field2 = value
-        if (!Hreg(field1, field2)) { //Check Address and execute (reg exists?)
-            exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
-            break;
-        }
-        if (Hreg(field1) != field2) { //Check for failure
-            exceptionResponse(fcode, EX_SLAVE_FAILURE);
-            break;
-        }
-        _reply = REPLY_ECHO;
-        break;
-
-    case FC_READ_REGS:
-        //field1 = startreg, field2 = numregs, header len = 3
-        readWords(HREG(field1), field2, fcode);
-        break;
-
-    case FC_WRITE_REGS:
-        //field1 = startreg, field2 = numregs, frame[5] = data lenght, header len = 6
-        if (field2 < 0x0001 || field2 > 0x007B || frame[5] != 2 * field2) { //Check value
-            exceptionResponse(fcode, EX_ILLEGAL_VALUE);
-            break;
-        }
-        for (k = 0; k < field2; k++) { //Check Address (startreg...startreg + numregs)
-            if (!searchRegister(HREG(field1) + k)) {
+        case FC_WRITE_REG:
+            //field1 = reg, field2 = value
+            if (!Hreg(field1, field2)) { //Check Address and execute (reg exists?)
                 exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
                 break;
             }
-        }
-        if (k >= field2) {
-            setMultipleWords((uint16_t*)(frame + 6), HREG(field1), field2);
-            successResponce(HREG(field1), field2, fcode);
-            _reply = REPLY_NORMAL;
-        }
-        break;
-
-    case FC_READ_COILS:
-        //field1 = startreg, field2 = numregs
-        readBits(COIL(field1), field2, fcode);
-        break;
-
-    case FC_READ_INPUT_STAT:
-        //field1 = startreg, field2 = numregs
-        readBits(ISTS(field1), field2, fcode);
-        break;
-
-    case FC_READ_INPUT_REGS:
-        //field1 = startreg, field2 = numregs
-        readWords(IREG(field1), field2, fcode);
-        break;
-
-    case FC_WRITE_COIL:
-        //field1 = reg, field2 = status, header len = 3
-        if (field2 != 0xFF00 && field2 != 0x0000) { //Check value (status)
-            exceptionResponse(fcode, EX_ILLEGAL_VALUE);
+            if (Hreg(field1) != field2) { //Check for failure
+                exceptionResponse(fcode, EX_SLAVE_FAILURE);
+                break;
+            }
+            _reply = REPLY_ECHO;
             break;
-        }
-        if (!Coil(field1, COIL_BOOL(field2))) { //Check Address and execute (reg exists?)
-            exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
-            break;
-        }
-        if (Coil(field1) != COIL_BOOL(field2)) { //Check for failure
-            exceptionResponse(fcode, EX_SLAVE_FAILURE);
-            break;
-        }
-        _reply = REPLY_ECHO;
-        break;
 
-    case FC_WRITE_COILS:
-        //field1 = startreg, field2 = numregs, frame[5] = bytecount, header len = 6
-        bytecount_calc = field2 / 8;
-        if (field2 % 8)
-            bytecount_calc++;
-        if (field2 < 0x0001 || field2 > 0x07B0 || frame[5] != bytecount_calc) { //Check registers range and data size maches
-            exceptionResponse(fcode, EX_ILLEGAL_VALUE);
+        case FC_READ_REGS:
+            //field1 = startreg, field2 = numregs, header len = 3
+            readWords(HREG(field1), field2, fcode);
             break;
-        }
-        for (k = 0; k < field2; k++) { //Check Address (startreg...startreg + numregs)
-            if (!searchRegister(COIL(field1) + k)) {
+
+        case FC_WRITE_REGS:
+            //field1 = startreg, field2 = numregs, frame[5] = data lenght, header len = 6
+            if (field2 < 0x0001 || field2 > 0x007B || frame[5] != 2 * field2) { //Check value
+                exceptionResponse(fcode, EX_ILLEGAL_VALUE);
+                break;
+            }
+            for (k = 0; k < field2; k++) { //Check Address (startreg...startreg + numregs)
+                if (!searchRegister(HREG(field1) + k)) {
+                    exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
+                    break;
+                }
+            }
+            if (k >= field2) {
+                setMultipleWords((uint16_t *) (frame + 6), HREG(field1), field2);
+                successResponce(HREG(field1), field2, fcode);
+                _reply = REPLY_NORMAL;
+            }
+            break;
+
+        case FC_READ_COILS:
+            //field1 = startreg, field2 = numregs
+            readBits(COIL(field1), field2, fcode);
+            break;
+
+        case FC_READ_INPUT_STAT:
+            //field1 = startreg, field2 = numregs
+            readBits(ISTS(field1), field2, fcode);
+            break;
+
+        case FC_READ_INPUT_REGS:
+            //field1 = startreg, field2 = numregs
+            readWords(IREG(field1), field2, fcode);
+            break;
+
+        case FC_WRITE_COIL:
+            //field1 = reg, field2 = status, header len = 3
+            if (field2 != 0xFF00 && field2 != 0x0000) { //Check value (status)
+                exceptionResponse(fcode, EX_ILLEGAL_VALUE);
+                break;
+            }
+            if (!Coil(field1, COIL_BOOL(field2))) { //Check Address and execute (reg exists?)
                 exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
                 break;
             }
-        }
-        if (k >= field2) {
-            setMultipleBits(frame + 6, COIL(field1), field2);
-            successResponce(COIL(field1), field2, fcode);
-            _reply = REPLY_NORMAL;
-        }
-        break;
+            if (Coil(field1) != COIL_BOOL(field2)) { //Check for failure
+                exceptionResponse(fcode, EX_SLAVE_FAILURE);
+                break;
+            }
+            _reply = REPLY_ECHO;
+            break;
 
-    default:
-        exceptionResponse(fcode, EX_ILLEGAL_FUNCTION);
+        case FC_WRITE_COILS:
+            //field1 = startreg, field2 = numregs, frame[5] = bytecount, header len = 6
+            bytecount_calc = field2 / 8;
+            if (field2 % 8)
+                bytecount_calc++;
+            if (field2 < 0x0001 || field2 > 0x07B0 ||
+                frame[5] != bytecount_calc) { //Check registers range and data size maches
+                exceptionResponse(fcode, EX_ILLEGAL_VALUE);
+                break;
+            }
+            for (k = 0; k < field2; k++) { //Check Address (startreg...startreg + numregs)
+                if (!searchRegister(COIL(field1) + k)) {
+                    exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
+                    break;
+                }
+            }
+            if (k >= field2) {
+                setMultipleBits(frame + 6, COIL(field1), field2);
+                successResponce(COIL(field1), field2, fcode);
+                _reply = REPLY_NORMAL;
+            }
+            break;
+
+        default:
+            exceptionResponse(fcode, EX_ILLEGAL_FUNCTION);
     }
 }
 
-void Modbus::successResponce(TAddress startreg, uint16_t numoutputs, FunctionCode fn)
-{
+void Modbus::successResponce(TAddress startreg, uint16_t numoutputs, FunctionCode fn) {
     free(_frame);
     _len = 5;
-    _frame = (uint8_t*)malloc(_len);
+    _frame = (uint8_t *) malloc(_len);
     _frame[0] = fn;
     _frame[1] = startreg.address >> 8;
     _frame[2] = startreg.address & 0x00FF;
@@ -208,18 +202,16 @@ void Modbus::successResponce(TAddress startreg, uint16_t numoutputs, FunctionCod
     _frame[4] = numoutputs & 0x00FF;
 }
 
-void Modbus::exceptionResponse(FunctionCode fn, ResultCode excode)
-{
+void Modbus::exceptionResponse(FunctionCode fn, ResultCode excode) {
     free(_frame);
     _len = 2;
-    _frame = (uint8_t*)malloc(_len);
+    _frame = (uint8_t *) malloc(_len);
     _frame[0] = fn + 0x80;
     _frame[1] = excode;
     _reply = REPLY_NORMAL;
 }
 
-void Modbus::getMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numregs)
-{
+void Modbus::getMultipleBits(uint8_t *frame, TAddress startreg, uint16_t numregs) {
     uint8_t bitn = 0;
     uint16_t i = 0;
     while (numregs--) {
@@ -236,15 +228,13 @@ void Modbus::getMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numregs
     }
 }
 
-void Modbus::getMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numregs)
-{
-    for (uint8_t i = 0; i < numregs; i++) {
+void Modbus::getMultipleWords(uint16_t *frame, TAddress startreg, uint16_t numregs) {
+    for (uint16_t i = 0; i < numregs; i++) {
         frame[i] = __bswap_16(Reg(startreg + i));
     }
 }
 
-void Modbus::readBits(TAddress startreg, uint16_t numregs, FunctionCode fn)
-{
+void Modbus::readBits(TAddress startreg, uint16_t numregs, FunctionCode fn) {
     if (numregs < 0x0001 || numregs > 0x07D0) { //Check value (numregs)
         exceptionResponse(fn, EX_ILLEGAL_VALUE);
         return;
@@ -264,7 +254,7 @@ void Modbus::readBits(TAddress startreg, uint16_t numregs, FunctionCode fn)
     _len = 2 + numregs / 8;
     if (numregs % 8)
         _len++; //Add 1 to the message length for the partial byte.
-    _frame = (uint8_t*)malloc(_len);
+    _frame = (uint8_t *) malloc(_len);
     if (!_frame) {
         exceptionResponse(fn, EX_SLAVE_FAILURE);
         return;
@@ -276,8 +266,7 @@ void Modbus::readBits(TAddress startreg, uint16_t numregs, FunctionCode fn)
     _reply = REPLY_NORMAL;
 }
 
-void Modbus::readWords(TAddress startreg, uint16_t numregs, FunctionCode fn)
-{
+void Modbus::readWords(TAddress startreg, uint16_t numregs, FunctionCode fn) {
     //Check value (numregs)
     if (numregs < 0x0001 || numregs > 0x007D) {
         exceptionResponse(fn, EX_ILLEGAL_VALUE);
@@ -289,19 +278,18 @@ void Modbus::readWords(TAddress startreg, uint16_t numregs, FunctionCode fn)
     }
     free(_frame);
     _len = 2 + numregs * 2; //calculate the query reply message length. 2 bytes per register + 2 bytes for header
-    _frame = (uint8_t*)malloc(_len);
+    _frame = (uint8_t *) malloc(_len);
     if (!_frame) {
         exceptionResponse(fn, EX_SLAVE_FAILURE);
         return;
     }
     _frame[0] = fn;
     _frame[1] = _len - 2; //byte count
-    getMultipleWords((uint16_t*)(_frame + 2), startreg, numregs);
+    getMultipleWords((uint16_t *) (_frame + 2), startreg, numregs);
     _reply = REPLY_NORMAL;
 }
 
-void Modbus::setMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numoutputs)
-{
+void Modbus::setMultipleBits(const uint8_t *frame, TAddress startreg, uint16_t numoutputs) {
     uint8_t bitn = 0;
     uint16_t i = 0;
     while (numoutputs--) {
@@ -315,16 +303,14 @@ void Modbus::setMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numoutp
     }
 }
 
-void Modbus::setMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numregs)
-{
-    for (uint8_t i = 0; i < numregs; i++) {
+void Modbus::setMultipleWords(const uint16_t *frame, TAddress startreg, uint16_t numregs) {
+    for (uint16_t i = 0; i < numregs; i++) {
         Reg(startreg + i, __bswap_16(frame[i]));
     }
 }
 
-bool Modbus::onGet(TAddress address, cbModbus cb, uint16_t numregs)
-{
-    TRegister* reg;
+bool Modbus::onGet(TAddress address, cbModbus cb, uint16_t numregs) {
+    TRegister *reg;
     bool atLeastOne = false;
     if (!cb) {
         return removeOnGet(address);
@@ -332,25 +318,7 @@ bool Modbus::onGet(TAddress address, cbModbus cb, uint16_t numregs)
     while (numregs > 0) {
         reg = searchRegister(address);
         if (reg) {
-            _callbacks.push_back({ TCallback::ON_GET, address, cb });
-            atLeastOne = true;
-        }
-        address++;
-        numregs--;
-    }
-    return atLeastOne;
-}
-bool Modbus::onSet(TAddress address, cbModbus cb, uint16_t numregs)
-{
-    TRegister* reg;
-    bool atLeastOne = false;
-    if (!cb) {
-        return removeOnGet(address);
-    }
-    while (numregs > 0) {
-        reg = searchRegister(address);
-        if (reg) {
-            _callbacks.push_back({ TCallback::ON_SET, address, cb });
+            _callbacks.push_back({TCallback::ON_GET, address, cb});
             atLeastOne = true;
         }
         address++;
@@ -359,28 +327,48 @@ bool Modbus::onSet(TAddress address, cbModbus cb, uint16_t numregs)
     return atLeastOne;
 }
 
-bool Modbus::removeOnSet(TAddress address, cbModbus cb, uint16_t numregs)
-{
-    while (numregs--) {
-        _callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), [address, cb](TCallback entry) { return entry.type == TCallback::ON_SET && entry.address == address && (!cb || entry.cb == cb); }), _callbacks.end());
-        address++;
+bool Modbus::onSet(TAddress address, cbModbus cb, uint16_t numregs) {
+    TRegister *reg;
+    bool atLeastOne = false;
+    if (!cb) {
+        return removeOnGet(address);
     }
-    return false;
+    while (numregs > 0) {
+        reg = searchRegister(address);
+        if (reg) {
+            _callbacks.push_back({TCallback::ON_SET, address, cb});
+            atLeastOne = true;
+        }
+        address++;
+        numregs--;
+    }
+    return atLeastOne;
 }
-bool Modbus::removeOnGet(TAddress address, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::removeOnSet(TAddress address, cbModbus cb, uint16_t numregs) {
     while (numregs--) {
-        _callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), [address, cb](TCallback entry) { return entry.type == TCallback::ON_GET && entry.address == address && (!cb || entry.cb == cb); }), _callbacks.end());
+        _callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), [address, cb](TCallback entry) {
+            return entry.type == TCallback::ON_SET && entry.address == address && (!cb || entry.cb == cb);
+        }), _callbacks.end());
         address++;
     }
     return false;
 }
 
-bool Modbus::readSlave(uint16_t address, uint16_t numregs, FunctionCode fn)
-{
+bool Modbus::removeOnGet(TAddress address, cbModbus cb, uint16_t numregs) {
+    while (numregs--) {
+        _callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), [address, cb](TCallback entry) {
+            return entry.type == TCallback::ON_GET && entry.address == address && (!cb || entry.cb == cb);
+        }), _callbacks.end());
+        address++;
+    }
+    return false;
+}
+
+bool Modbus::readSlave(uint16_t address, uint16_t numregs, FunctionCode fn) {
     free(_frame);
     _len = 5;
-    _frame = (uint8_t*)malloc(_len);
+    _frame = (uint8_t *) malloc(_len);
     _frame[0] = fn;
     _frame[1] = address >> 8;
     _frame[2] = address & 0x00FF;
@@ -389,13 +377,12 @@ bool Modbus::readSlave(uint16_t address, uint16_t numregs, FunctionCode fn)
     return true;
 }
 
-bool Modbus::writeSlaveBits(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, bool* data)
-{
+bool Modbus::writeSlaveBits(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, bool *data) {
     free(_frame);
     _len = 6 + numregs / 8;
     if (numregs % 8)
         _len++; //Add 1 to the message length for the partial byte.
-    _frame = (uint8_t*)malloc(_len);
+    _frame = (uint8_t *) malloc(_len);
     if (_frame) {
         _frame[0] = fn;
         _frame[1] = to >> 8;
@@ -416,11 +403,10 @@ bool Modbus::writeSlaveBits(TAddress startreg, uint16_t to, uint16_t numregs, Fu
     return false;
 }
 
-bool Modbus::writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, uint16_t* data)
-{
+bool Modbus::writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, const uint16_t *data) {
     free(_frame);
     _len = 6 + 2 * numregs;
-    _frame = (uint8_t*)malloc(_len);
+    _frame = (uint8_t *) malloc(_len);
     if (_frame) {
         _frame[0] = fn;
         _frame[1] = to >> 8;
@@ -429,12 +415,12 @@ bool Modbus::writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, F
         _frame[4] = numregs & 0x00FF;
         _frame[5] = _len - 6;
         if (data) {
-            uint16_t* frame = (uint16_t*)(_frame + 6);
-            for (uint8_t i = 0; i < numregs; i++) {
+            auto *frame = (uint16_t *) (_frame + 6);
+            for (uint16_t i = 0; i < numregs; i++) {
                 frame[i] = __bswap_16(data[i]);
             }
         } else {
-            getMultipleWords((uint16_t*)(_frame + 6), startreg, numregs);
+            getMultipleWords((uint16_t *) (_frame + 6), startreg, numregs);
         }
         return true;
     }
@@ -442,8 +428,7 @@ bool Modbus::writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, F
     return false;
 }
 
-void Modbus::boolToBits(uint8_t* dst, bool* src, uint16_t numregs)
-{
+void Modbus::boolToBits(uint8_t *dst, const bool *src, uint16_t numregs) {
     uint8_t bitn = 0;
     uint16_t i = 0;
     uint16_t j = 0;
@@ -461,8 +446,7 @@ void Modbus::boolToBits(uint8_t* dst, bool* src, uint16_t numregs)
     }
 }
 
-void Modbus::bitsToBool(bool* dst, uint8_t* src, uint16_t numregs)
-{
+void Modbus::bitsToBool(bool *dst, const uint8_t *src, uint16_t numregs) {
     uint8_t bitn = 0;
     uint16_t i = 0;
     uint16_t j = 0;
@@ -477,202 +461,197 @@ void Modbus::bitsToBool(bool* dst, uint8_t* src, uint16_t numregs)
     }
 }
 
-void Modbus::masterPDU(uint8_t* frame, uint8_t* sourceFrame, TAddress startreg, uint16_t* output)
-{
+void Modbus::masterPDU(uint8_t *frame, const uint8_t *sourceFrame, TAddress startreg, uint16_t *output) {
     uint8_t fcode = frame[0];
     _reply = EX_SUCCESS;
     if ((fcode & 0x80) != 0) {
         _reply = frame[1];
         return;
     }
-    uint16_t field2 = (uint16_t)sourceFrame[3] << 8 | (uint16_t)sourceFrame[4];
+    uint16_t field2 = (uint16_t) sourceFrame[3] << 8 | (uint16_t) sourceFrame[4];
     uint8_t bytecount_calc;
     switch (fcode) {
-    case FC_READ_REGS:
-    case FC_READ_INPUT_REGS:
-        //field2 = numregs, frame[1] = data lenght, header len = 2
-        if (frame[1] != 2 * field2) { //Check if data size matches
-            _reply = EX_DATA_MISMACH;
-            break;
-        }
-        if (output) {
-            frame += 2;
-            while (field2) {
-                *((uint16_t*)output) = __bswap_16(*((uint16_t*)frame));
-                frame += 2;
-                output += 2;
-                field2--;
+        case FC_READ_REGS:
+        case FC_READ_INPUT_REGS:
+            //field2 = numregs, frame[1] = data lenght, header len = 2
+            if (frame[1] != 2 * field2) { //Check if data size matches
+                _reply = EX_DATA_MISMACH;
+                break;
+            }
+            if (output) {
+                frame += 2;
+                while (field2) {
+                    *((uint16_t *) output) = __bswap_16(*((uint16_t *) frame));
+                    frame += 2;
+                    output += 2;
+                    field2--;
+                }
+            } else {
+                setMultipleWords((uint16_t *) (frame + 2), startreg, field2);
             }
-        } else {
-            setMultipleWords((uint16_t*)(frame + 2), startreg, field2);
-        }
-        break;
-    case FC_READ_COILS:
-    case FC_READ_INPUT_STAT:
-        //field2 = numregs, frame[1] = data length, header len = 2
-        bytecount_calc = field2 / 8;
-        if (field2 % 8)
-            bytecount_calc++;
-        if (frame[1] != bytecount_calc) { // check if data size matches
-            _reply = EX_DATA_MISMACH;
             break;
-        }
-        if (output) {
-            bitsToBool((bool*)output, frame + 2, field2);
-        } else {
-            setMultipleBits(frame + 2, startreg, field2);
-        }
-        break;
-    case FC_WRITE_REG:
-    case FC_WRITE_REGS:
-    case FC_WRITE_COIL:
-    case FC_WRITE_COILS:
-        break;
-    default:
-        _reply = EX_GENERAL_FAILURE;
+        case FC_READ_COILS:
+        case FC_READ_INPUT_STAT:
+            //field2 = numregs, frame[1] = data length, header len = 2
+            bytecount_calc = field2 / 8;
+            if (field2 % 8)
+                bytecount_calc++;
+            if (frame[1] != bytecount_calc) { // check if data size matches
+                _reply = EX_DATA_MISMACH;
+                break;
+            }
+            if (output) {
+                bitsToBool((bool *) output, frame + 2, field2);
+            } else {
+                setMultipleBits(frame + 2, startreg, field2);
+            }
+            break;
+        case FC_WRITE_REG:
+        case FC_WRITE_REGS:
+        case FC_WRITE_COIL:
+        case FC_WRITE_COILS:
+            break;
+        default:
+            _reply = EX_GENERAL_FAILURE;
     }
 }
 
-void Modbus::cbEnable(bool state)
-{
+void Modbus::cbEnable(bool state) {
     cbEnabled = state;
 }
-void Modbus::cbDisable()
-{
+
+void Modbus::cbDisable() {
     cbEnable(false);
 }
 
-bool Modbus::addHreg(uint16_t offset, uint16_t value, uint16_t numregs)
-{
+bool Modbus::addHreg(uint16_t offset, uint16_t value, uint16_t numregs) {
     return addReg(HREG(offset), value, numregs);
 }
-bool Modbus::Hreg(uint16_t offset, uint16_t value)
-{
+
+bool Modbus::Hreg(uint16_t offset, uint16_t value) {
     return Reg(HREG(offset), value);
 }
-uint16_t Modbus::Hreg(uint16_t offset)
-{
+
+uint16_t Modbus::Hreg(uint16_t offset) {
     return Reg(HREG(offset));
 }
-uint16_t Modbus::removeHreg(uint16_t offset, uint16_t numregs)
-{
+
+uint16_t Modbus::removeHreg(uint16_t offset, uint16_t numregs) {
     return removeReg(HREG(offset), numregs);
 }
-bool Modbus::addCoil(uint16_t offset, bool value, uint16_t numregs)
-{
+
+bool Modbus::addCoil(uint16_t offset, bool value, uint16_t numregs) {
     return addReg(COIL(offset), COIL_VAL(value), numregs);
 }
-bool Modbus::addIsts(uint16_t offset, bool value, uint16_t numregs)
-{
+
+bool Modbus::addIsts(uint16_t offset, bool value, uint16_t numregs) {
     return addReg(ISTS(offset), ISTS_VAL(value), numregs);
 }
-bool Modbus::addIreg(uint16_t offset, uint16_t value, uint16_t numregs)
-{
+
+bool Modbus::addIreg(uint16_t offset, uint16_t value, uint16_t numregs) {
     return addReg(IREG(offset), value, numregs);
 }
-bool Modbus::Coil(uint16_t offset, bool value)
-{
+
+bool Modbus::Coil(uint16_t offset, bool value) {
     return Reg(COIL(offset), COIL_VAL(value));
 }
-bool Modbus::Ists(uint16_t offset, bool value)
-{
+
+bool Modbus::Ists(uint16_t offset, bool value) {
     return Reg(ISTS(offset), ISTS_VAL(value));
 }
-bool Modbus::Ireg(uint16_t offset, uint16_t value)
-{
+
+bool Modbus::Ireg(uint16_t offset, uint16_t value) {
     return Reg(IREG(offset), value);
 }
-bool Modbus::Coil(uint16_t offset)
-{
+
+bool Modbus::Coil(uint16_t offset) {
     return COIL_BOOL(Reg(COIL(offset)));
 }
-bool Modbus::Ists(uint16_t offset)
-{
+
+bool Modbus::Ists(uint16_t offset) {
     return ISTS_BOOL(Reg(ISTS(offset)));
 }
-uint16_t Modbus::Ireg(uint16_t offset)
-{
+
+uint16_t Modbus::Ireg(uint16_t offset) {
     return Reg(IREG(offset));
 }
-bool Modbus::removeCoil(uint16_t offset, uint16_t numregs)
-{
+
+bool Modbus::removeCoil(uint16_t offset, uint16_t numregs) {
     return removeReg(COIL(offset), numregs);
 }
-bool Modbus::removeIsts(uint16_t offset, uint16_t numregs)
-{
+
+bool Modbus::removeIsts(uint16_t offset, uint16_t numregs) {
     return removeReg(ISTS(offset), numregs);
 }
-bool Modbus::removeIreg(uint16_t offset, uint16_t numregs)
-{
+
+bool Modbus::removeIreg(uint16_t offset, uint16_t numregs) {
     return removeReg(IREG(offset), numregs);
 }
-bool Modbus::onGetCoil(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::onGetCoil(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return onGet(COIL(offset), cb, numregs);
 }
-bool Modbus::onSetCoil(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::onSetCoil(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return onSet(COIL(offset), cb, numregs);
 }
-bool Modbus::onGetHreg(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::onGetHreg(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return onGet(HREG(offset), cb, numregs);
 }
-bool Modbus::onSetHreg(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::onSetHreg(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return onSet(HREG(offset), cb, numregs);
 }
-bool Modbus::onGetIsts(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::onGetIsts(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return onGet(ISTS(offset), cb, numregs);
 }
-bool Modbus::onSetIsts(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::onSetIsts(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return onSet(ISTS(offset), cb, numregs);
 }
-bool Modbus::onGetIreg(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::onGetIreg(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return onGet(IREG(offset), cb, numregs);
 }
-bool Modbus::onSetIreg(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::onSetIreg(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return onSet(IREG(offset), cb, numregs);
 }
 
-bool Modbus::removeOnGetCoil(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+bool Modbus::removeOnGetCoil(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return removeOnGet(COIL(offset), cb, numregs);
 }
-bool Modbus::removeOnSetCoil(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::removeOnSetCoil(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return removeOnSet(COIL(offset), cb, numregs);
 }
-bool Modbus::removeOnGetHreg(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::removeOnGetHreg(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return removeOnGet(HREG(offset), cb, numregs);
 }
-bool Modbus::removeOnSetHreg(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::removeOnSetHreg(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return removeOnSet(HREG(offset), cb, numregs);
 }
-bool Modbus::removeOnGetIsts(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::removeOnGetIsts(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return removeOnGet(ISTS(offset), cb, numregs);
 }
-bool Modbus::removeOnSetIsts(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::removeOnSetIsts(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return removeOnSet(ISTS(offset), cb, numregs);
 }
-bool Modbus::removeOnGetIreg(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::removeOnGetIreg(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return removeOnGet(IREG(offset), cb, numregs);
 }
-bool Modbus::removeOnSetIreg(uint16_t offset, cbModbus cb, uint16_t numregs)
-{
+
+bool Modbus::removeOnSetIreg(uint16_t offset, cbModbus cb, uint16_t numregs) {
     return removeOnSet(IREG(offset), cb, numregs);
 }
 
-Modbus::~Modbus()
-{
+Modbus::~Modbus() {
     free(_frame);
 }
diff --git a/Modbus.h b/Modbus.h
index 31862e6..72727a2 100644
--- a/Modbus.h
+++ b/Modbus.h
@@ -8,6 +8,7 @@
 #include "mbed.h"
 #include <algorithm>
 #include <vector>
+
 #ifdef ARDUINO_ARCH_ESP32
 #include <byteswap.h>
 #endif
@@ -39,66 +40,70 @@
 
 struct TRegister;
 
-typedef uint16_t (*cbModbus)(TRegister* reg, uint16_t val); // Callback function Type
+typedef uint16_t (*cbModbus)(TRegister *reg, uint16_t val); // Callback function Type
 
 struct TAddress {
-    enum RegType { COIL,
+    enum RegType {
+        COIL,
         ISTS,
         IREG,
-        HREG };
+        HREG
+    };
     RegType type;
     uint16_t address;
-    bool operator==(const TAddress& obj) const
-    { // TAddress == TAddress
+
+    bool operator==(const TAddress &obj) const { // TAddress == TAddress
         return type == obj.type && address == obj.address;
     }
-    bool operator!=(const TAddress& obj) const
-    { // TAddress != TAddress
+
+    bool operator!=(const TAddress &obj) const { // TAddress != TAddress
         return type != obj.type || address != obj.address;
     }
-    TAddress& operator++()
-    { // ++TAddress
+
+    TAddress &operator++() { // ++TAddress
         address++;
         return *this;
     }
-    TAddress operator++(int)
-    { // TAddress++
+
+    const TAddress operator++(int) { // TAddress++
         TAddress result(*this);
         ++(*this);
         return result;
     }
-    TAddress& operator+=(const int& inc)
-    { // TAddress += integer
+
+    TAddress &operator+=(const int &inc) { // TAddress += integer
         address += inc;
         return *this;
     }
-    const TAddress operator+(const int& inc) const
-    { // TAddress + integer
+
+    TAddress operator+(const int &inc) const { // TAddress + integer
         TAddress result(*this);
         result.address += inc;
         return result;
     }
-    bool isCoil()
-    {
+
+    bool isCoil() const {
         return type == COIL;
     }
-    bool isIsts()
-    {
+
+    bool isIsts() const {
         return type == ISTS;
     }
-    bool isIreg()
-    {
+
+    bool isIreg() const {
         return type == IREG;
     }
-    bool isHreg()
-    {
+
+    bool isHreg() const {
         return type == HREG;
     }
 };
 
 struct TCallback {
-    enum CallbackType { ON_SET,
-        ON_GET };
+    enum CallbackType {
+        ON_SET,
+        ON_GET
+    };
     CallbackType type;
     TAddress address;
     cbModbus cb;
@@ -107,8 +112,8 @@ struct TCallback {
 struct TRegister {
     TAddress address;
     uint16_t value;
-    bool operator==(const TRegister& obj) const
-    {
+
+    bool operator==(const TRegister &obj) const {
         return address == obj.address;
     }
 };
@@ -151,23 +156,41 @@ public:
         EX_CONNECTION_LOST = 0xE5, // Custom. Connection with device lost
         EX_CANCEL = 0xE6 // Custom. Transaction/request canceled
     };
+
     ~Modbus();
+
     bool addHreg(uint16_t offset, uint16_t value = 0, uint16_t numregs = 1);
+
     bool Hreg(uint16_t offset, uint16_t value);
+
     uint16_t Hreg(uint16_t offset);
+
     uint16_t removeHreg(uint16_t offset, uint16_t numregs = 1);
+
     bool addCoil(uint16_t offset, bool value = false, uint16_t numregs = 1);
+
     bool addIsts(uint16_t offset, bool value = false, uint16_t numregs = 1);
+
     bool addIreg(uint16_t offset, uint16_t value = 0, uint16_t numregs = 1);
+
     bool Coil(uint16_t offset, bool value);
+
     bool Ists(uint16_t offset, bool value);
+
     bool Ireg(uint16_t offset, uint16_t value);
+
     bool Coil(uint16_t offset);
+
     bool Ists(uint16_t offset);
+
     uint16_t Ireg(uint16_t offset);
+
     bool removeCoil(uint16_t offset, uint16_t numregs = 1);
+
     bool removeIsts(uint16_t offset, uint16_t numregs = 1);
+
     bool removeIreg(uint16_t offset, uint16_t numregs = 1);
+
     /*
         bool Hreg(uint16_t offset, uint16_t* value);
         bool Coil(uint16_t offset, bool* value);
@@ -175,38 +198,57 @@ public:
         bool Ireg(uint16_t offset, uint16_t* value);
         */
     void cbEnable(bool state = true);
+
     void cbDisable();
 
     bool onGetCoil(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool onSetCoil(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool onGetHreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool onSetHreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool onGetIsts(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool onSetIsts(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool onGetIreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool onSetIreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
 
     bool removeOnGetCoil(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool removeOnSetCoil(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool removeOnGetHreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool removeOnSetHreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool removeOnGetIsts(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool removeOnSetIsts(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool removeOnGetIreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool removeOnSetIreg(uint16_t offset, cbModbus cb = nullptr, uint16_t numregs = 1);
 
 private:
     void readBits(TAddress startreg, uint16_t numregs, FunctionCode fn);
+
     void readWords(TAddress startreg, uint16_t numregs, FunctionCode fn);
 
-    void setMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numoutputs);
-    void setMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numoutputs);
+    void setMultipleBits(const uint8_t *frame, TAddress startreg, uint16_t numoutputs);
 
-    void getMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numregs);
-    void getMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numregs);
+    void setMultipleWords(const uint16_t *frame, TAddress startreg, uint16_t numoutputs);
 
-    void bitsToBool(bool* dst, uint8_t* src, uint16_t numregs);
-    void boolToBits(uint8_t* dst, bool* src, uint16_t numregs);
+    void getMultipleBits(uint8_t *frame, TAddress startreg, uint16_t numregs);
+
+    void getMultipleWords(uint16_t *frame, TAddress startreg, uint16_t numregs);
+
+    static void bitsToBool(bool *dst, const uint8_t *src, uint16_t numregs);
+
+    static void boolToBits(uint8_t *dst, const bool *src, uint16_t numregs);
 
 protected:
     //Reply Types
@@ -221,24 +263,31 @@ protected:
     std::vector<TRegister> _regs;
     std::vector<TCallback> _callbacks;
 #endif
-    uint8_t* _frame = nullptr;
+    uint8_t *_frame = nullptr;
     uint16_t _len = 0;
     uint8_t _reply = 0;
     bool cbEnabled = true;
-    uint16_t mbCallback(TRegister* reg, uint16_t val, TCallback::CallbackType t);
-    TRegister* searchRegister(TAddress addr);
+
+    uint16_t mbCallback(TRegister *reg, uint16_t val, TCallback::CallbackType t);
+
+    TRegister *searchRegister(TAddress addr);
+
     void exceptionResponse(FunctionCode fn, ResultCode excode); // Fills _frame with response
     void successResponce(TAddress startreg, uint16_t numoutputs, FunctionCode fn); // Fills frame with response
-    void slavePDU(uint8_t* frame); //For Slave
-    void masterPDU(uint8_t* frame, uint8_t* sourceFrame, TAddress startreg, uint16_t* output = nullptr); //For Master
+    void slavePDU(uint8_t *frame); //For Slave
+    void
+    masterPDU(uint8_t *frame, const uint8_t *sourceFrame, TAddress startreg, uint16_t *output = nullptr); //For Master
     // frame - data received form slave
     // sourceFrame - data have sent fo slave
     // startreg - local register to start put data to
     // output - if not null put data to the buffer insted local registers. output assumed to by array of uint16_t or boolean
 
     bool readSlave(uint16_t address, uint16_t numregs, FunctionCode fn);
-    bool writeSlaveBits(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, bool* data = nullptr);
-    bool writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, uint16_t* data = nullptr);
+
+    bool writeSlaveBits(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, bool *data = nullptr);
+
+    bool
+    writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, const uint16_t *data = nullptr);
     // startreg - local register to get data from
     // to - slave register to write data to
     // numregs - number of registers
@@ -246,16 +295,23 @@ protected:
     // data - if null use local registers. Otherwise use data from array to erite to slave
 
     bool addReg(TAddress address, uint16_t value = 0, uint16_t numregs = 1);
+
     bool Reg(TAddress address, uint16_t value);
+
     uint16_t Reg(TAddress address);
+
     bool removeReg(TAddress address, uint16_t numregs = 1);
 
     bool onGet(TAddress address, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool onSet(TAddress address, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool removeOnSet(TAddress address, cbModbus cb = nullptr, uint16_t numregs = 1);
+
     bool removeOnGet(TAddress address, cbModbus cb = nullptr, uint16_t numregs = 1);
 
     virtual uint32_t eventSource() { return 0; }
 };
 
-typedef bool (*cbTransaction)(Modbus::ResultCode event, uint16_t transactionId, void* data); // Callback skeleton for requests
+typedef bool (*cbTransaction)(Modbus::ResultCode event, uint16_t transactionId,
+                              void *data); // Callback skeleton for requests
diff --git a/ModbusRTU.cpp b/ModbusRTU.cpp
index 1dff110..2d08559 100644
--- a/ModbusRTU.cpp
+++ b/ModbusRTU.cpp
@@ -11,28 +11,44 @@
 
 // Table of CRC values
 static const uint16_t _auchCRC[] = {
-    0x0000, 0xC1C0, 0x81C1, 0x4001, 0x01C3, 0xC003, 0x8002, 0x41C2, 0x01C6, 0xC006, 0x8007, 0x41C7, 0x0005, 0xC1C5, 0x81C4,
-    0x4004, 0x01CC, 0xC00C, 0x800D, 0x41CD, 0x000F, 0xC1CF, 0x81CE, 0x400E, 0x000A, 0xC1CA, 0x81CB, 0x400B, 0x01C9, 0xC009,
-    0x8008, 0x41C8, 0x01D8, 0xC018, 0x8019, 0x41D9, 0x001B, 0xC1DB, 0x81DA, 0x401A, 0x001E, 0xC1DE, 0x81DF, 0x401F, 0x01DD,
-    0xC01D, 0x801C, 0x41DC, 0x0014, 0xC1D4, 0x81D5, 0x4015, 0x01D7, 0xC017, 0x8016, 0x41D6, 0x01D2, 0xC012, 0x8013, 0x41D3,
-    0x0011, 0xC1D1, 0x81D0, 0x4010, 0x01F0, 0xC030, 0x8031, 0x41F1, 0x0033, 0xC1F3, 0x81F2, 0x4032, 0x0036, 0xC1F6, 0x81F7,
-    0x4037, 0x01F5, 0xC035, 0x8034, 0x41F4, 0x003C, 0xC1FC, 0x81FD, 0x403D, 0x01FF, 0xC03F, 0x803E, 0x41FE, 0x01FA, 0xC03A,
-    0x803B, 0x41FB, 0x0039, 0xC1F9, 0x81F8, 0x4038, 0x0028, 0xC1E8, 0x81E9, 0x4029, 0x01EB, 0xC02B, 0x802A, 0x41EA, 0x01EE,
-    0xC02E, 0x802F, 0x41EF, 0x002D, 0xC1ED, 0x81EC, 0x402C, 0x01E4, 0xC024, 0x8025, 0x41E5, 0x0027, 0xC1E7, 0x81E6, 0x4026,
-    0x0022, 0xC1E2, 0x81E3, 0x4023, 0x01E1, 0xC021, 0x8020, 0x41E0, 0x01A0, 0xC060, 0x8061, 0x41A1, 0x0063, 0xC1A3, 0x81A2,
-    0x4062, 0x0066, 0xC1A6, 0x81A7, 0x4067, 0x01A5, 0xC065, 0x8064, 0x41A4, 0x006C, 0xC1AC, 0x81AD, 0x406D, 0x01AF, 0xC06F,
-    0x806E, 0x41AE, 0x01AA, 0xC06A, 0x806B, 0x41AB, 0x0069, 0xC1A9, 0x81A8, 0x4068, 0x0078, 0xC1B8, 0x81B9, 0x4079, 0x01BB,
-    0xC07B, 0x807A, 0x41BA, 0x01BE, 0xC07E, 0x807F, 0x41BF, 0x007D, 0xC1BD, 0x81BC, 0x407C, 0x01B4, 0xC074, 0x8075, 0x41B5,
-    0x0077, 0xC1B7, 0x81B6, 0x4076, 0x0072, 0xC1B2, 0x81B3, 0x4073, 0x01B1, 0xC071, 0x8070, 0x41B0, 0x0050, 0xC190, 0x8191,
-    0x4051, 0x0193, 0xC053, 0x8052, 0x4192, 0x0196, 0xC056, 0x8057, 0x4197, 0x0055, 0xC195, 0x8194, 0x4054, 0x019C, 0xC05C,
-    0x805D, 0x419D, 0x005F, 0xC19F, 0x819E, 0x405E, 0x005A, 0xC19A, 0x819B, 0x405B, 0x0199, 0xC059, 0x8058, 0x4198, 0x0188,
-    0xC048, 0x8049, 0x4189, 0x004B, 0xC18B, 0x818A, 0x404A, 0x004E, 0xC18E, 0x818F, 0x404F, 0x018D, 0xC04D, 0x804C, 0x418C,
-    0x0044, 0xC184, 0x8185, 0x4045, 0x0187, 0xC047, 0x8046, 0x4186, 0x0182, 0xC042, 0x8043, 0x4183, 0x0041, 0xC181, 0x8180,
-    0x4040, 0x0000
+        0x0000, 0xC1C0, 0x81C1, 0x4001, 0x01C3, 0xC003, 0x8002, 0x41C2, 0x01C6, 0xC006, 0x8007, 0x41C7, 0x0005, 0xC1C5,
+        0x81C4,
+        0x4004, 0x01CC, 0xC00C, 0x800D, 0x41CD, 0x000F, 0xC1CF, 0x81CE, 0x400E, 0x000A, 0xC1CA, 0x81CB, 0x400B, 0x01C9,
+        0xC009,
+        0x8008, 0x41C8, 0x01D8, 0xC018, 0x8019, 0x41D9, 0x001B, 0xC1DB, 0x81DA, 0x401A, 0x001E, 0xC1DE, 0x81DF, 0x401F,
+        0x01DD,
+        0xC01D, 0x801C, 0x41DC, 0x0014, 0xC1D4, 0x81D5, 0x4015, 0x01D7, 0xC017, 0x8016, 0x41D6, 0x01D2, 0xC012, 0x8013,
+        0x41D3,
+        0x0011, 0xC1D1, 0x81D0, 0x4010, 0x01F0, 0xC030, 0x8031, 0x41F1, 0x0033, 0xC1F3, 0x81F2, 0x4032, 0x0036, 0xC1F6,
+        0x81F7,
+        0x4037, 0x01F5, 0xC035, 0x8034, 0x41F4, 0x003C, 0xC1FC, 0x81FD, 0x403D, 0x01FF, 0xC03F, 0x803E, 0x41FE, 0x01FA,
+        0xC03A,
+        0x803B, 0x41FB, 0x0039, 0xC1F9, 0x81F8, 0x4038, 0x0028, 0xC1E8, 0x81E9, 0x4029, 0x01EB, 0xC02B, 0x802A, 0x41EA,
+        0x01EE,
+        0xC02E, 0x802F, 0x41EF, 0x002D, 0xC1ED, 0x81EC, 0x402C, 0x01E4, 0xC024, 0x8025, 0x41E5, 0x0027, 0xC1E7, 0x81E6,
+        0x4026,
+        0x0022, 0xC1E2, 0x81E3, 0x4023, 0x01E1, 0xC021, 0x8020, 0x41E0, 0x01A0, 0xC060, 0x8061, 0x41A1, 0x0063, 0xC1A3,
+        0x81A2,
+        0x4062, 0x0066, 0xC1A6, 0x81A7, 0x4067, 0x01A5, 0xC065, 0x8064, 0x41A4, 0x006C, 0xC1AC, 0x81AD, 0x406D, 0x01AF,
+        0xC06F,
+        0x806E, 0x41AE, 0x01AA, 0xC06A, 0x806B, 0x41AB, 0x0069, 0xC1A9, 0x81A8, 0x4068, 0x0078, 0xC1B8, 0x81B9, 0x4079,
+        0x01BB,
+        0xC07B, 0x807A, 0x41BA, 0x01BE, 0xC07E, 0x807F, 0x41BF, 0x007D, 0xC1BD, 0x81BC, 0x407C, 0x01B4, 0xC074, 0x8075,
+        0x41B5,
+        0x0077, 0xC1B7, 0x81B6, 0x4076, 0x0072, 0xC1B2, 0x81B3, 0x4073, 0x01B1, 0xC071, 0x8070, 0x41B0, 0x0050, 0xC190,
+        0x8191,
+        0x4051, 0x0193, 0xC053, 0x8052, 0x4192, 0x0196, 0xC056, 0x8057, 0x4197, 0x0055, 0xC195, 0x8194, 0x4054, 0x019C,
+        0xC05C,
+        0x805D, 0x419D, 0x005F, 0xC19F, 0x819E, 0x405E, 0x005A, 0xC19A, 0x819B, 0x405B, 0x0199, 0xC059, 0x8058, 0x4198,
+        0x0188,
+        0xC048, 0x8049, 0x4189, 0x004B, 0xC18B, 0x818A, 0x404A, 0x004E, 0xC18E, 0x818F, 0x404F, 0x018D, 0xC04D, 0x804C,
+        0x418C,
+        0x0044, 0xC184, 0x8185, 0x4045, 0x0187, 0xC047, 0x8046, 0x4186, 0x0182, 0xC042, 0x8043, 0x4183, 0x0041, 0xC181,
+        0x8180,
+        0x4040, 0x0000
 };
 
-uint16_t ModbusRTU::crc16(uint8_t address, uint8_t* frame, uint8_t pduLen)
-{
+uint16_t ModbusRTU::crc16(uint8_t address, uint8_t *frame, uint8_t pduLen) {
     uint8_t i = 0xFF ^ address;
     uint16_t val = pgm_read_word(_auchCRC + i);
     uint8_t CRCHi = 0xFF ^ highByte(val); // Hi
@@ -46,8 +62,7 @@ uint16_t ModbusRTU::crc16(uint8_t address, uint8_t* frame, uint8_t pduLen)
     return (CRCHi << 8) | CRCLo;
 }
 
-void ModbusRTU::setBaudrate(uint32_t baud)
-{
+void ModbusRTU::setBaudrate(uint32_t baud) {
     if (baud > 19200) {
         _t = 50ms;
     } else {
@@ -56,16 +71,14 @@ void ModbusRTU::setBaudrate(uint32_t baud)
     }
 }
 
-bool ModbusRTU::begin(UnbufferedSerial* port)
-{
+bool ModbusRTU::begin(UnbufferedSerial *port) {
     _port = port;
     _t = 50ms;
     _port->attach(callback(this, &ModbusRTU::SerialRXHandler), SerialBase::RxIrq);
     return true;
 }
 
-bool ModbusRTU::begin(UnbufferedSerial* port, PinName txPin)
-{
+bool ModbusRTU::begin(UnbufferedSerial *port, PinName txPin) {
     uint32_t baud = 0;
 #if defined(ESP32) || defined(ESP8266)
     // baudRate() only available with ESP32+ESP8266
@@ -85,17 +98,15 @@ bool ModbusRTU::begin(UnbufferedSerial* port, PinName txPin)
     return true;
 }
 
-void ModbusRTU::SerialRXHandler()
-{
+void ModbusRTU::SerialRXHandler() {
     char c;
     if (_port->readable()) {
         _port->read(&c, 1);
-        RX_RingBuff.push((uint8_t)(c & (uint8_t)0xFFU));
+        RX_RingBuff.push((uint8_t) (c & (uint8_t) 0xFFU));
     }
 }
 
-void ModbusRTU::SerialTXHandler()
-{
+void ModbusRTU::SerialTXHandler() {
     //Здесь проверка на TC а не на RX!!!!
     //Изменено в serial_api.c
     if (_port->writeable()) {
@@ -104,17 +115,15 @@ void ModbusRTU::SerialTXHandler()
     }
 }
 
-uint8_t ModbusRTU::Read(void)
-{
+uint8_t ModbusRTU::Read() {
     uint8_t cur = 0xff;
     if (!RX_RingBuff.empty()) {
-        RX_RingBuff.pop((char&)cur);
+        RX_RingBuff.pop((char &) cur);
     }
     return cur;
 }
 
-uint8_t ModbusRTU::isDataAvailable(void)
-{
+uint8_t ModbusRTU::isDataAvailable() {
     if (RX_RingBuff.full()) {
         RX_RingBuff.reset();
     }
@@ -133,8 +142,7 @@ bool ModbusRTU::begin(SoftwareSerial* port, int16_t txPin)
 }
 #endif
 
-bool ModbusRTU::rawSend(uint8_t slaveId, uint8_t* frame, uint8_t len)
-{
+bool ModbusRTU::rawSend(uint8_t slaveId, uint8_t *frame, uint8_t len) {
     uint16_t newCrc = crc16(slaveId, frame, len);
     char crc[2];
     crc[0] = newCrc >> 8;
@@ -153,8 +161,7 @@ bool ModbusRTU::rawSend(uint8_t slaveId, uint8_t* frame, uint8_t len)
     return true;
 }
 
-bool ModbusRTU::send(uint8_t slaveId, TAddress startreg, cbTransaction cb, void* data, bool waitResponse)
-{
+bool ModbusRTU::send(uint8_t slaveId, TAddress startreg, cbTransaction cb, void *data, bool waitResponse) {
     if (_slaveId)
         return false; // Break if waiting for previous request result
     rawSend(slaveId, _frame, _len);
@@ -171,8 +178,7 @@ bool ModbusRTU::send(uint8_t slaveId, TAddress startreg, cbTransaction cb, void*
     return true;
 }
 
-void ModbusRTU::task()
-{
+void ModbusRTU::task() {
 
     if (RX_RingBuff.size() > _len) {
         _len = RX_RingBuff.size();
@@ -201,13 +207,13 @@ void ModbusRTU::task()
     }
 
     free(_frame); //Just in case
-    _frame = (uint8_t*)malloc(_len);
+    _frame = (uint8_t *) malloc(_len);
     if (!_frame) { // Fail to allocate buffer
         RX_RingBuff.reset(); // Skip packet if can't allocate buffer
         _len = 0;
         return;
     }
-    for (uint8_t i = 0; i < _len; i++) {
+    for (uint16_t i = 0; i < _len; i++) {
         _frame[i] = Read(); // read data + crc
 #if defined(MODBUSRTU_DEBUG)
         Serial.printf("%02X ", _frame[i]);
@@ -229,9 +235,9 @@ void ModbusRTU::task()
         _reply = EX_SUCCESS;
         if ((_frame[0] & 0x7F) == _sentFrame[0]) { // Check if function code the same as requested
             // Procass incoming frame as master
-            masterPDU(_frame, _sentFrame, _sentReg, (uint16_t*)_data);
+            masterPDU(_frame, _sentFrame, _sentReg, (uint16_t *) _data);
             if (_cb) {
-                _cb((ResultCode)_reply, 0, nullptr);
+                _cb((ResultCode) _reply, 0, nullptr);
             }
             free(_sentFrame);
             _sentFrame = nullptr;
@@ -253,8 +259,7 @@ void ModbusRTU::task()
     _len = 0;
 }
 
-bool ModbusRTU::cleanup()
-{
+bool ModbusRTU::cleanup() {
     // Remove timeouted request and forced event
     if (_slaveId && (Kernel::Clock::now() - _timestamp > MODBUSRTU_TIMEOUT)) {
         if (_cb)
@@ -269,68 +274,59 @@ bool ModbusRTU::cleanup()
     return false;
 }
 
-uint16_t ModbusRTU::writeHreg(uint8_t slaveId, uint16_t offset, uint16_t value, cbTransaction cb)
-{
+uint16_t ModbusRTU::writeHreg(uint8_t slaveId, uint16_t offset, uint16_t value, cbTransaction cb) {
     readSlave(offset, value, FC_WRITE_REG);
     return send(slaveId, HREG(offset), cb, nullptr, cb);
 }
 
-uint16_t ModbusRTU::writeCoil(uint8_t slaveId, uint16_t offset, bool value, cbTransaction cb)
-{
+uint16_t ModbusRTU::writeCoil(uint8_t slaveId, uint16_t offset, bool value, cbTransaction cb) {
     readSlave(offset, COIL_VAL(value), FC_WRITE_COIL);
     return send(slaveId, COIL(offset), cb, nullptr, cb);
 }
 
-uint16_t ModbusRTU::readCoil(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::readCoil(uint8_t slaveId, uint16_t offset, bool *value, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs << 4)
         return false;
     readSlave(offset, numregs, FC_READ_COILS);
     return send(slaveId, COIL(offset), cb, value);
 }
 
-uint16_t ModbusRTU::writeCoil(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::writeCoil(uint8_t slaveId, uint16_t offset, bool *value, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > 0x07D0)
         return false;
     writeSlaveBits(COIL(offset), offset, numregs, FC_WRITE_COILS, value);
     return send(slaveId, COIL(offset), cb, nullptr, cb);
 }
 
-uint16_t ModbusRTU::writeHreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::writeHreg(uint8_t slaveId, uint16_t offset, uint16_t *value, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > 0x007D)
         return false;
     writeSlaveWords(HREG(offset), offset, numregs, FC_WRITE_REGS, value);
     return send(slaveId, HREG(offset), cb, nullptr, cb);
 }
 
-uint16_t ModbusRTU::readHreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::readHreg(uint8_t slaveId, uint16_t offset, uint16_t *value, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs)
         return false;
     readSlave(offset, numregs, FC_READ_REGS);
     return send(slaveId, HREG(offset), cb, value);
 }
 
-uint16_t ModbusRTU::readIsts(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::readIsts(uint8_t slaveId, uint16_t offset, bool *value, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs << 4)
         return false;
     readSlave(offset, numregs, FC_READ_INPUT_STAT);
     return send(slaveId, ISTS(offset), cb, value);
 }
 
-uint16_t ModbusRTU::readIreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::readIreg(uint8_t slaveId, uint16_t offset, uint16_t *value, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs)
         return false;
     readSlave(offset, numregs, FC_READ_INPUT_REGS);
     return send(slaveId, IREG(offset), cb, value);
 }
 
-uint16_t ModbusRTU::pushCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::pushCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > 0x07D0)
         return false;
     if (!searchRegister(COIL(from)))
@@ -343,8 +339,7 @@ uint16_t ModbusRTU::pushCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16
     return send(slaveId, COIL(from), cb);
 }
 
-uint16_t ModbusRTU::pullCoil(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::pullCoil(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs << 4)
         return false;
 #ifdef MODBUSRTU_ADD_REG
@@ -354,8 +349,7 @@ uint16_t ModbusRTU::pullCoil(uint8_t slaveId, uint16_t from, uint16_t to, uint16
     return send(slaveId, COIL(to), cb);
 }
 
-uint16_t ModbusRTU::pullIsts(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::pullIsts(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs << 4)
         return false;
 #ifdef MODBUSRTU_ADD_REG
@@ -365,8 +359,7 @@ uint16_t ModbusRTU::pullIsts(uint8_t slaveId, uint16_t from, uint16_t to, uint16
     return send(slaveId, ISTS(to), cb);
 }
 
-uint16_t ModbusRTU::pushHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::pushHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > 0x007D)
         return false;
     if (!searchRegister(HREG(from)))
@@ -379,8 +372,7 @@ uint16_t ModbusRTU::pushHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16
     return send(slaveId, HREG(from), cb);
 }
 
-uint16_t ModbusRTU::pullHreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::pullHreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs)
         return false;
 #ifdef MODBUSRTU_ADD_REG
@@ -390,8 +382,7 @@ uint16_t ModbusRTU::pullHreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16
     return send(slaveId, HREG(to), cb);
 }
 
-uint16_t ModbusRTU::pullIreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::pullIreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs)
         return false;
 #ifdef MODBUSRTU_ADD_REG
@@ -401,8 +392,7 @@ uint16_t ModbusRTU::pullIreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16
     return send(slaveId, IREG(to), cb);
 }
 
-uint16_t ModbusRTU::pushIregToHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::pushIregToHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > 0x007D)
         return false;
     if (!searchRegister(IREG(from)))
@@ -415,8 +405,7 @@ uint16_t ModbusRTU::pushIregToHreg(uint8_t slaveId, uint16_t to, uint16_t from,
     return send(slaveId, IREG(from), cb);
 }
 
-uint16_t ModbusRTU::pushIstsToCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::pushIstsToCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs << 4)
         return false;
     if (!searchRegister(ISTS(from)))
@@ -429,8 +418,7 @@ uint16_t ModbusRTU::pushIstsToCoil(uint8_t slaveId, uint16_t to, uint16_t from,
     return send(slaveId, ISTS(from), cb);
 }
 
-uint16_t ModbusRTU::pullHregToIreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::pullHregToIreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs)
         return false;
 #ifdef MODBUSRTU_ADD_REG
@@ -440,8 +428,7 @@ uint16_t ModbusRTU::pullHregToIreg(uint8_t slaveId, uint16_t from, uint16_t to,
     return send(slaveId, IREG(to), cb);
 }
 
-uint16_t ModbusRTU::pullCoilToIsts(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb)
-{
+uint16_t ModbusRTU::pullCoilToIsts(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs, cbTransaction cb) {
     if (numregs < 0x0001 || numregs > maxRegs << 4)
         return false;
 #ifdef MODBUSRTU_ADD_REG
diff --git a/ModbusRTU.h b/ModbusRTU.h
index 55f35ab..1fb8111 100644
--- a/ModbusRTU.h
+++ b/ModbusRTU.h
@@ -5,6 +5,7 @@
 	This code is licensed under the BSD New License. See LICENSE.txt for more info.
 */
 #pragma once
+
 #include "mbed.h"
 #include <Modbus.h>
 
@@ -24,12 +25,17 @@
 class ModbusRTU : public Modbus {
 protected:
     CircularBuffer<char, 200> RX_RingBuff;
-    UnbufferedSerial* _port;
-    DigitalOut* _txPin;
+    UnbufferedSerial *_port;
+    DigitalOut *_txPin;
+
     void SerialRXHandler();
+
     void SerialTXHandler();
-    uint8_t isDataAvailable(void);
-    uint8_t Read(void);
+
+    uint8_t isDataAvailable();
+
+    uint8_t Read();
+
     //    unsigned int
     Kernel::Clock::duration MODBUSRTU_TIMEOUT = 1s;
     Kernel::Clock::duration _t; // inter-frame delay in mS
@@ -38,53 +44,87 @@ protected:
     uint8_t _slaveId;
     Kernel::Clock::time_point _timestamp;
     cbTransaction _cb = nullptr;
-    void* _data = nullptr;
-    uint8_t* _sentFrame = nullptr;
+    void *_data = nullptr;
+    uint8_t *_sentFrame = nullptr;
     TAddress _sentReg = COIL(0);
     uint16_t maxRegs = 0x007D;
 #ifdef ESP32
     portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
 #endif
-    bool send(uint8_t slaveId, TAddress startreg, cbTransaction cb, void* data = nullptr, bool waitResponse = true);
+
+    bool send(uint8_t slaveId, TAddress startreg, cbTransaction cb, void *data = nullptr, bool waitResponse = true);
+
     // Prepare and send ModbusRTU frame. _frame buffer and _len should be filled with Modbus data
     // slaveId - slave id
     // startreg - first local register to save returned data to (miningless for write to slave operations)
     // cb - transaction callback function
     // data - if not null use buffer to save returned data instead of local registers
-    bool rawSend(uint8_t slaveId, uint8_t* frame, uint8_t len);
+    bool rawSend(uint8_t slaveId, uint8_t *frame, uint8_t len);
+
     bool cleanup(); // Free clients if not connected and remove timedout transactions and transaction with forced events
-    uint16_t crc16(uint8_t address, uint8_t* frame, uint8_t pdulen);
+    static uint16_t crc16(uint8_t address, uint8_t *frame, uint8_t pdulen);
 
 public:
     void setBaudrate(uint32_t baud = -1);
+
 #if defined(ESP8266)
     bool begin(BufferedSerial* port, int16_t txPin = -1);
 #endif
-    bool begin(UnbufferedSerial* port, PinName txPin = NC);
-    bool begin(UnbufferedSerial* port);
+
+    bool begin(UnbufferedSerial *port, PinName txPin = NC);
+
+    bool begin(UnbufferedSerial *port);
+
     void task();
+
     void master() { isMaster = true; };
+
     void slave(uint8_t slaveId) { _slaveId = slaveId; };
-    uint8_t slave() { return _slaveId; }
+
+    uint8_t slave() const { return _slaveId; }
+
     uint32_t eventSource() override { return _slaveId; }
+
     uint16_t writeHreg(uint8_t slaveId, uint16_t offset, uint16_t value, cbTransaction cb = nullptr);
+
     uint16_t writeCoil(uint8_t slaveId, uint16_t offset, bool value, cbTransaction cb = nullptr);
-    uint16_t readCoil(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
-    uint16_t writeCoil(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
-    uint16_t writeHreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
-    uint16_t readIsts(uint8_t slaveId, uint16_t offset, bool* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
-    uint16_t readHreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
-    uint16_t readIreg(uint8_t slaveId, uint16_t offset, uint16_t* value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
+    uint16_t readCoil(uint8_t slaveId, uint16_t offset, bool *value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
+    uint16_t writeCoil(uint8_t slaveId, uint16_t offset, bool *value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
+    uint16_t
+    writeHreg(uint8_t slaveId, uint16_t offset, uint16_t *value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
+    uint16_t readIsts(uint8_t slaveId, uint16_t offset, bool *value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
+    uint16_t
+    readHreg(uint8_t slaveId, uint16_t offset, uint16_t *value, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
+    uint16_t
+    readIreg(uint8_t slaveId, uint16_t offset, uint16_t *value, uint16_t numregs = 1, cbTransaction cb = nullptr);
 
     uint16_t pushCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
     uint16_t pullCoil(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
     uint16_t pullIsts(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
     uint16_t pushHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
     uint16_t pullHreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
     uint16_t pullIreg(uint8_t slaveId, uint16_t from, uint16_t to, uint16_t numregs = 1, cbTransaction cb = nullptr);
 
-    uint16_t pullHregToIreg(uint8_t slaveId, uint16_t offset, uint16_t startreg, uint16_t numregs = 1, cbTransaction cb = nullptr);
-    uint16_t pullCoilToIsts(uint8_t slaveId, uint16_t offset, uint16_t startreg, uint16_t numregs = 1, cbTransaction cb = nullptr);
-    uint16_t pushIstsToCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs = 1, cbTransaction cb = nullptr);
-    uint16_t pushIregToHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs = 1, cbTransaction cb = nullptr);
+    uint16_t pullHregToIreg(uint8_t slaveId, uint16_t offset, uint16_t startreg, uint16_t numregs = 1,
+                            cbTransaction cb = nullptr);
+
+    uint16_t pullCoilToIsts(uint8_t slaveId, uint16_t offset, uint16_t startreg, uint16_t numregs = 1,
+                            cbTransaction cb = nullptr);
+
+    uint16_t
+    pushIstsToCoil(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs = 1, cbTransaction cb = nullptr);
+
+    uint16_t
+    pushIregToHreg(uint8_t slaveId, uint16_t to, uint16_t from, uint16_t numregs = 1, cbTransaction cb = nullptr);
 };