diff --git a/src/knx/bau_systemB.cpp b/src/knx/bau_systemB.cpp
index fc1fdf81..95270c07 100644
--- a/src/knx/bau_systemB.cpp
+++ b/src/knx/bau_systemB.cpp
@@ -1,7 +1,7 @@
 #include "bau_systemB.h"
 #include "bits.h"
-#include <string.h>
 #include <stdio.h>
+#include <string.h>
 
 enum NmReadSerialNumberType
 {
@@ -14,9 +14,10 @@ enum NmReadSerialNumberType
 static constexpr auto kFunctionPropertyResultBufferMaxSize = 0xFF;
 static constexpr auto kRestartProcessTime = 3;
 
-BauSystemB::BauSystemB(Platform& platform): _memory(platform, _deviceObj),
-     _appProgram(_memory),
-    _platform(platform)
+BauSystemB::BauSystemB(Platform &platform)
+    : _memory(platform, _deviceObj),
+      _appProgram(_memory),
+      _platform(platform)
 {
     _memory.addSaveRestore(&_appProgram);
 }
@@ -31,71 +32,63 @@ void BauSystemB::writeMemory()
     _memory.writeMemory();
 }
 
-Platform& BauSystemB::platform()
+Platform &BauSystemB::platform()
 {
     return _platform;
 }
 
-ApplicationProgramObject& BauSystemB::parameters()
+ApplicationProgramObject &BauSystemB::parameters()
 {
     return _appProgram;
 }
 
-DeviceObject& BauSystemB::deviceObject()
+DeviceObject &BauSystemB::deviceObject()
 {
     return _deviceObj;
 }
 
 uint8_t BauSystemB::checkmasterResetValidity(EraseCode eraseCode, uint8_t channel)
 {
-    static constexpr uint8_t successCode = 0x00; // Where does this come from? It is the code for "success".
+    static constexpr uint8_t successCode = 0x00;      // Where does this come from? It is the code for "success".
     static constexpr uint8_t invalidEraseCode = 0x02; // Where does this come from? It is the error code for "unspported erase code".
 
     switch (eraseCode)
     {
-        case EraseCode::ConfirmedRestart:
-        {
+        case EraseCode::ConfirmedRestart: {
             println("Confirmed restart requested.");
             return successCode;
         }
-        case EraseCode::ResetAP:
-        {
+        case EraseCode::ResetAP: {
             // TODO: increase download counter except for confirmed restart (PID_DOWNLOAD_COUNTER)
             println("ResetAP requested. Not implemented yet.");
             return successCode;
         }
-        case EraseCode::ResetIA:
-        {
+        case EraseCode::ResetIA: {
             // TODO: increase download counter except for confirmed restart (PID_DOWNLOAD_COUNTER)
             println("ResetIA requested. Not implemented yet.");
             return successCode;
         }
-        case EraseCode::ResetLinks:
-        {
+        case EraseCode::ResetLinks: {
             // TODO: increase download counter except for confirmed restart (PID_DOWNLOAD_COUNTER)
             println("ResetLinks requested. Not implemented yet.");
             return successCode;
         }
-        case EraseCode::ResetParam:
-        {
+        case EraseCode::ResetParam: {
             // TODO: increase download counter except for confirmed restart (PID_DOWNLOAD_COUNTER)
             println("ResetParam requested. Not implemented yet.");
             return successCode;
         }
-        case EraseCode::FactoryReset:
-        {
+        case EraseCode::FactoryReset: {
             // TODO: increase download counter except for confirmed restart (PID_DOWNLOAD_COUNTER)
             println("Factory reset requested. type: with IA");
             return successCode;
         }
-        case EraseCode::FactoryResetWithoutIA:
-        {
+        case EraseCode::FactoryResetWithoutIA: {
             // TODO: increase download counter except for confirmed restart (PID_DOWNLOAD_COUNTER)
             println("Factory reset requested. type: without IA");
             return successCode;
         }
-        default:
-        {
+        default: {
             print("Unhandled erase code: ");
             println(eraseCode, HEX);
             return invalidEraseCode;
@@ -107,14 +100,14 @@ void BauSystemB::deviceDescriptorReadIndication(Priority priority, HopCountType
 {
     if (descriptorType != 0)
         descriptorType = 0x3f;
-    
+
     uint8_t data[2];
     pushWord(_deviceObj.maskVersion(), data);
     applicationLayer().deviceDescriptorReadResponse(AckRequested, priority, hopType, asap, secCtrl, descriptorType, data);
 }
 
 void BauSystemB::memoryWriteIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t number,
-    uint16_t memoryAddress, uint8_t * data)
+                                       uint16_t memoryAddress, uint8_t *data)
 {
     _memory.writeMemory(memoryAddress, number, data);
     if (_deviceObj.verifyMode())
@@ -122,19 +115,19 @@ void BauSystemB::memoryWriteIndication(Priority priority, HopCountType hopType,
 }
 
 void BauSystemB::memoryReadIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t number,
-    uint16_t memoryAddress, uint8_t * data)
+                                      uint16_t memoryAddress, uint8_t *data)
 {
     applicationLayer().memoryReadResponse(AckRequested, priority, hopType, asap, secCtrl, number, memoryAddress, data);
 }
 
 void BauSystemB::memoryReadIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t number,
-    uint16_t memoryAddress)
+                                      uint16_t memoryAddress)
 {
     applicationLayer().memoryReadResponse(AckRequested, priority, hopType, asap, secCtrl, number, memoryAddress,
-        _memory.toAbsolute(memoryAddress));
+                                          _memory.toAbsolute(memoryAddress));
 }
 
-void BauSystemB::memoryExtWriteIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t number, uint32_t memoryAddress, uint8_t * data)
+void BauSystemB::memoryExtWriteIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t number, uint32_t memoryAddress, uint8_t *data)
 {
     _memory.writeMemory(memoryAddress, number, data);
 
@@ -165,6 +158,8 @@ void BauSystemB::restartRequestIndication(Priority priority, HopCountType hopTyp
         uint8_t errorCode = checkmasterResetValidity(eraseCode, channel);
         // We send the restart response now before actually applying the reset values
         // Processing time is kRestartProcessTime (example 3 seconds) that we require for the applying the master reset with restart
+        if (_beforeRestart != 0)
+            _beforeRestart();
         applicationLayer().restartResponse(AckRequested, priority, hopType, secCtrl, errorCode, (errorCode == 0) ? kRestartProcessTime : 0);
         doMasterReset(eraseCode, channel);
     }
@@ -188,10 +183,10 @@ void BauSystemB::authorizeIndication(Priority priority, HopCountType hopType, ui
 void BauSystemB::userMemoryReadIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t number, uint32_t memoryAddress)
 {
     applicationLayer().userMemoryReadResponse(AckRequested, priority, hopType, asap, secCtrl, number, memoryAddress,
-        _memory.toAbsolute(memoryAddress));
+                                              _memory.toAbsolute(memoryAddress));
 }
 
-void BauSystemB::userMemoryWriteIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t number, uint32_t memoryAddress, uint8_t* data)
+void BauSystemB::userMemoryWriteIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t number, uint32_t memoryAddress, uint8_t *data)
 {
     _memory.writeMemory(memoryAddress, number, data);
 
@@ -200,37 +195,37 @@ void BauSystemB::userMemoryWriteIndication(Priority priority, HopCountType hopTy
 }
 
 void BauSystemB::propertyDescriptionReadIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t objectIndex,
-    uint8_t propertyId, uint8_t propertyIndex)
+                                                   uint8_t propertyId, uint8_t propertyIndex)
 {
     uint8_t pid = propertyId;
     bool writeEnable = false;
     uint8_t type = 0;
     uint16_t numberOfElements = 0;
     uint8_t access = 0;
-    InterfaceObject* obj = getInterfaceObject(objectIndex);
+    InterfaceObject *obj = getInterfaceObject(objectIndex);
     if (obj)
         obj->readPropertyDescription(pid, propertyIndex, writeEnable, type, numberOfElements, access);
 
     applicationLayer().propertyDescriptionReadResponse(AckRequested, priority, hopType, asap, secCtrl, objectIndex, pid, propertyIndex,
-        writeEnable, type, numberOfElements, access);
+                                                       writeEnable, type, numberOfElements, access);
 }
 
 void BauSystemB::propertyValueWriteIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t objectIndex,
-    uint8_t propertyId, uint8_t numberOfElements, uint16_t startIndex, uint8_t* data, uint8_t length)
+                                              uint8_t propertyId, uint8_t numberOfElements, uint16_t startIndex, uint8_t *data, uint8_t length)
 {
-    InterfaceObject* obj = getInterfaceObject(objectIndex);
-    if(obj)
+    InterfaceObject *obj = getInterfaceObject(objectIndex);
+    if (obj)
         obj->writeProperty((PropertyID)propertyId, startIndex, data, numberOfElements);
     propertyValueReadIndication(priority, hopType, asap, secCtrl, objectIndex, propertyId, numberOfElements, startIndex);
 }
 
 void BauSystemB::propertyValueExtWriteIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, ObjectType objectType, uint8_t objectInstance,
-    uint8_t propertyId, uint8_t numberOfElements, uint16_t startIndex, uint8_t* data, uint8_t length, bool confirmed)
+                                                 uint8_t propertyId, uint8_t numberOfElements, uint16_t startIndex, uint8_t *data, uint8_t length, bool confirmed)
 {
     uint8_t returnCode = ReturnCodes::Success;
 
-    InterfaceObject* obj = getInterfaceObject(objectType, objectInstance);
-    if(obj)
+    InterfaceObject *obj = getInterfaceObject(objectType, objectInstance);
+    if (obj)
         obj->writeProperty((PropertyID)propertyId, startIndex, data, numberOfElements);
     else
         returnCode = ReturnCodes::AddressVoid;
@@ -242,11 +237,11 @@ void BauSystemB::propertyValueExtWriteIndication(Priority priority, HopCountType
 }
 
 void BauSystemB::propertyValueReadIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t objectIndex,
-    uint8_t propertyId, uint8_t numberOfElements, uint16_t startIndex)
+                                             uint8_t propertyId, uint8_t numberOfElements, uint16_t startIndex)
 {
     uint8_t size = 0;
     uint8_t elementCount = numberOfElements;
-    InterfaceObject* obj = getInterfaceObject(objectIndex);
+    InterfaceObject *obj = getInterfaceObject(objectIndex);
     if (obj)
     {
         uint8_t elementSize = obj->propertySize((PropertyID)propertyId);
@@ -259,22 +254,22 @@ void BauSystemB::propertyValueReadIndication(Priority priority, HopCountType hop
         elementCount = 0;
 
     uint8_t data[size];
-    if(obj)
+    if (obj)
         obj->readProperty((PropertyID)propertyId, startIndex, elementCount, data);
-    
+
     if (elementCount == 0)
         size = 0;
-    
+
     applicationLayer().propertyValueReadResponse(AckRequested, priority, hopType, asap, secCtrl, objectIndex, propertyId, elementCount,
-                                        startIndex, data, size);
+                                                 startIndex, data, size);
 }
 
 void BauSystemB::propertyValueExtReadIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, ObjectType objectType, uint8_t objectInstance,
-    uint8_t propertyId, uint8_t numberOfElements, uint16_t startIndex)
+                                                uint8_t propertyId, uint8_t numberOfElements, uint16_t startIndex)
 {
     uint8_t size = 0;
     uint8_t elementCount = numberOfElements;
-    InterfaceObject* obj = getInterfaceObject(objectType, objectInstance);
+    InterfaceObject *obj = getInterfaceObject(objectType, objectInstance);
     if (obj)
     {
         uint8_t elementSize = obj->propertySize((PropertyID)propertyId);
@@ -287,26 +282,26 @@ void BauSystemB::propertyValueExtReadIndication(Priority priority, HopCountType
         elementCount = 0;
 
     uint8_t data[size];
-    if(obj)
+    if (obj)
         obj->readProperty((PropertyID)propertyId, startIndex, elementCount, data);
 
     if (elementCount == 0)
         size = 0;
 
     applicationLayer().propertyValueExtReadResponse(AckRequested, priority, hopType, asap, secCtrl, objectType, objectInstance, propertyId, elementCount,
-                                           startIndex, data, size);
+                                                    startIndex, data, size);
 }
 
 void BauSystemB::functionPropertyCommandIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t objectIndex,
-                                                   uint8_t propertyId, uint8_t* data, uint8_t length)
+                                                   uint8_t propertyId, uint8_t *data, uint8_t length)
 {
     uint8_t resultData[kFunctionPropertyResultBufferMaxSize];
     uint8_t resultLength = sizeof(resultData); // tell the callee the maximum size of the buffer
 
     bool handled = false;
 
-    InterfaceObject* obj = getInterfaceObject(objectIndex);
-    if(obj)
+    InterfaceObject *obj = getInterfaceObject(objectIndex);
+    if (obj)
     {
         if (obj->property((PropertyID)propertyId)->Type() == PDT_FUNCTION)
         {
@@ -315,31 +310,33 @@ void BauSystemB::functionPropertyCommandIndication(Priority priority, HopCountTy
         }
         else
         {
-            if(_functionProperty != 0)
-                if(_functionProperty(objectIndex, propertyId, length, data, resultData, resultLength))
+            if (_functionProperty != 0)
+                if (_functionProperty(objectIndex, propertyId, length, data, resultData, resultLength))
                     handled = true;
         }
-    } else {
-        if(_functionProperty != 0)
-            if(_functionProperty(objectIndex, propertyId, length, data, resultData, resultLength))
+    }
+    else
+    {
+        if (_functionProperty != 0)
+            if (_functionProperty(objectIndex, propertyId, length, data, resultData, resultLength))
                 handled = true;
     }
 
-    //only return a value it was handled by a property or function
-    if(handled)
+    // only return a value it was handled by a property or function
+    if (handled)
         applicationLayer().functionPropertyStateResponse(AckRequested, priority, hopType, asap, secCtrl, objectIndex, propertyId, resultData, resultLength);
 }
 
 void BauSystemB::functionPropertyStateIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, uint8_t objectIndex,
-                                                 uint8_t propertyId, uint8_t* data, uint8_t length)
+                                                 uint8_t propertyId, uint8_t *data, uint8_t length)
 {
     uint8_t resultData[kFunctionPropertyResultBufferMaxSize];
     uint8_t resultLength = sizeof(resultData); // tell the callee the maximum size of the buffer
 
     bool handled = true;
 
-    InterfaceObject* obj = getInterfaceObject(objectIndex);
-    if(obj)
+    InterfaceObject *obj = getInterfaceObject(objectIndex);
+    if (obj)
     {
         if (obj->property((PropertyID)propertyId)->Type() == PDT_FUNCTION)
         {
@@ -348,29 +345,31 @@ void BauSystemB::functionPropertyStateIndication(Priority priority, HopCountType
         }
         else
         {
-            if(_functionPropertyState != 0)
-                if(_functionPropertyState(objectIndex, propertyId, length, data, resultData, resultLength))
+            if (_functionPropertyState != 0)
+                if (_functionPropertyState(objectIndex, propertyId, length, data, resultData, resultLength))
                     handled = true;
         }
-    } else {
-        if(_functionPropertyState != 0)
-            if(_functionPropertyState(objectIndex, propertyId, length, data, resultData, resultLength))
+    }
+    else
+    {
+        if (_functionPropertyState != 0)
+            if (_functionPropertyState(objectIndex, propertyId, length, data, resultData, resultLength))
                 handled = true;
     }
 
-    //only return a value it was handled by a property or function
-    if(handled)
+    // only return a value it was handled by a property or function
+    if (handled)
         applicationLayer().functionPropertyStateResponse(AckRequested, priority, hopType, asap, secCtrl, objectIndex, propertyId, resultData, resultLength);
 }
 
 void BauSystemB::functionPropertyExtCommandIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, ObjectType objectType, uint8_t objectInstance,
-                                                      uint8_t propertyId, uint8_t* data, uint8_t length)
+                                                      uint8_t propertyId, uint8_t *data, uint8_t length)
 {
     uint8_t resultData[kFunctionPropertyResultBufferMaxSize];
     uint8_t resultLength = 1; // we always have to include the return code at least
 
-    InterfaceObject* obj = getInterfaceObject(objectType, objectInstance);
-    if(obj)
+    InterfaceObject *obj = getInterfaceObject(objectType, objectInstance);
+    if (obj)
     {
         PropertyDataType propType = obj->property((PropertyID)propertyId)->Type();
 
@@ -420,13 +419,13 @@ void BauSystemB::functionPropertyExtCommandIndication(Priority priority, HopCoun
 }
 
 void BauSystemB::functionPropertyExtStateIndication(Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl &secCtrl, ObjectType objectType, uint8_t objectInstance,
-                                                    uint8_t propertyId, uint8_t* data, uint8_t length)
+                                                    uint8_t propertyId, uint8_t *data, uint8_t length)
 {
     uint8_t resultData[kFunctionPropertyResultBufferMaxSize];
     uint8_t resultLength = sizeof(resultData); // tell the callee the maximum size of the buffer
 
-    InterfaceObject* obj = getInterfaceObject(objectType, objectInstance);
-    if(obj)
+    InterfaceObject *obj = getInterfaceObject(objectType, objectInstance);
+    if (obj)
     {
         PropertyDataType propType = obj->property((PropertyID)propertyId)->Type();
 
@@ -479,7 +478,7 @@ void BauSystemB::individualAddressWriteIndication(HopCountType hopType, const Se
 }
 
 void BauSystemB::individualAddressSerialNumberWriteIndication(Priority priority, HopCountType hopType, const SecurityControl &secCtrl, uint16_t newIndividualAddress,
-                                                          uint8_t* knxSerialNumber)
+                                                              uint8_t *knxSerialNumber)
 {
     // If the received serial number matches our serial number
     // then store the received new individual address in the device object
@@ -487,7 +486,7 @@ void BauSystemB::individualAddressSerialNumberWriteIndication(Priority priority,
         _deviceObj.individualAddress(newIndividualAddress);
 }
 
-void BauSystemB::individualAddressSerialNumberReadIndication(Priority priority, HopCountType hopType, const SecurityControl &secCtrl, uint8_t* knxSerialNumber)
+void BauSystemB::individualAddressSerialNumberReadIndication(Priority priority, HopCountType hopType, const SecurityControl &secCtrl, uint8_t *knxSerialNumber)
 {
     // If the received serial number matches our serial number
     // then send a response with the serial number. The domain address is set to 0 for closed media.
@@ -499,7 +498,7 @@ void BauSystemB::individualAddressSerialNumberReadIndication(Priority priority,
     }
 }
 
-void BauSystemB::addSaveRestore(SaveRestore* obj)
+void BauSystemB::addSaveRestore(SaveRestore *obj)
 {
     _memory.addSaveRestore(obj);
 }
@@ -561,14 +560,14 @@ void BauSystemB::nextRestartState()
 }
 
 void BauSystemB::systemNetworkParameterReadIndication(Priority priority, HopCountType hopType, const SecurityControl &secCtrl, uint16_t objectType,
-                                                      uint16_t propertyId, uint8_t* testInfo, uint16_t testInfoLength)
+                                                      uint16_t propertyId, uint8_t *testInfo, uint16_t testInfoLength)
 {
     uint8_t operand;
 
     popByte(operand, testInfo + 1); // First byte (+ 0) contains only 4 reserved bits (0)
 
     // See KNX spec. 3.5.2 p.33 (Management Procedures: Procedures with A_SystemNetworkParameter_Read)
-    switch((NmReadSerialNumberType)operand)
+    switch ((NmReadSerialNumberType)operand)
     {
         case NM_Read_SerialNumber_By_ProgrammingMode: // NM_Read_SerialNumber_By_ProgrammingMode
             // Only send a reply if programming mode is on
@@ -576,23 +575,23 @@ void BauSystemB::systemNetworkParameterReadIndication(Priority priority, HopCoun
             {
                 // Send reply. testResult data is KNX serial number
                 applicationLayer().systemNetworkParameterReadResponse(priority, hopType, secCtrl, objectType, propertyId,
-                                                             testInfo, testInfoLength, (uint8_t*)_deviceObj.propertyData(PID_SERIAL_NUMBER), 6);
+                                                                      testInfo, testInfoLength, (uint8_t *)_deviceObj.propertyData(PID_SERIAL_NUMBER), 6);
             }
-        break;
+            break;
 
         case NM_Read_SerialNumber_By_ExFactoryState: // NM_Read_SerialNumber_By_ExFactoryState
-        break;
+            break;
 
         case NM_Read_SerialNumber_By_PowerReset: // NM_Read_SerialNumber_By_PowerReset
-        break;
+            break;
 
         case NM_Read_SerialNumber_By_ManufacturerSpecific: // Manufacturer specific use of A_SystemNetworkParameter_Read
-        break;
+            break;
     }
 }
 
 void BauSystemB::systemNetworkParameterReadLocalConfirm(Priority priority, HopCountType hopType, const SecurityControl &secCtrl, uint16_t objectType,
-                                                         uint16_t propertyId, uint8_t* testInfo, uint16_t testInfoLength, bool status)
+                                                        uint16_t propertyId, uint8_t *testInfo, uint16_t testInfoLength, bool status)
 {
 }
 
@@ -603,7 +602,7 @@ void BauSystemB::propertyValueRead(ObjectType objectType, uint8_t objectInstance
     uint32_t size = 0;
     uint8_t elementCount = numberOfElements;
 
-    InterfaceObject* obj = getInterfaceObject(objectType, objectInstance);
+    InterfaceObject *obj = getInterfaceObject(objectType, objectInstance);
 
     if (obj)
     {
@@ -612,7 +611,7 @@ void BauSystemB::propertyValueRead(ObjectType objectType, uint8_t objectInstance
             size = elementSize * numberOfElements;
         else
             size = sizeof(uint16_t); // size of property array entry 0 which contains the current number of elements
-        *data = new uint8_t [size];
+        *data = new uint8_t[size];
         obj->readProperty((PropertyID)propertyId, startIndex, elementCount, *data);
     }
     else
@@ -627,16 +626,16 @@ void BauSystemB::propertyValueRead(ObjectType objectType, uint8_t objectInstance
 
 void BauSystemB::propertyValueWrite(ObjectType objectType, uint8_t objectInstance, uint8_t propertyId,
                                     uint8_t &numberOfElements, uint16_t startIndex,
-                                    uint8_t* data, uint32_t length)
+                                    uint8_t *data, uint32_t length)
 {
-    InterfaceObject* obj =  getInterfaceObject(objectType, objectInstance);
-    if(obj)
+    InterfaceObject *obj = getInterfaceObject(objectType, objectInstance);
+    if (obj)
         obj->writeProperty((PropertyID)propertyId, startIndex, data, numberOfElements);
-    else 
+    else
         numberOfElements = 0;
 }
 
-Memory& BauSystemB::memory()
+Memory &BauSystemB::memory()
 {
     return _memory;
 }
diff --git a/src/knx/bau_systemB_device.cpp b/src/knx/bau_systemB_device.cpp
index 3ee44250..78fa9875 100644
--- a/src/knx/bau_systemB_device.cpp
+++ b/src/knx/bau_systemB_device.cpp
@@ -1,18 +1,18 @@
 #include "bau_systemB_device.h"
 #include "bits.h"
-#include <string.h>
 #include <stdio.h>
+#include <string.h>
 
-BauSystemBDevice::BauSystemBDevice(Platform& platform) :
-    BauSystemB(platform),
-    _addrTable(_memory),
-    _assocTable(_memory), _groupObjTable(_memory),
+BauSystemBDevice::BauSystemBDevice(Platform& platform)
+    : BauSystemB(platform),
+      _addrTable(_memory),
+      _assocTable(_memory), _groupObjTable(_memory),
 #ifdef USE_DATASECURE
-    _appLayer(_deviceObj, _secIfObj, *this),
+      _appLayer(_deviceObj, _secIfObj, *this),
 #else
-    _appLayer(*this),
+      _appLayer(*this),
 #endif
-    _transLayer(_appLayer), _netLayer(_deviceObj, _transLayer)
+      _transLayer(_appLayer), _netLayer(_deviceObj, _transLayer)
 {
     _appLayer.transportLayer(_transLayer);
     _appLayer.associationTableObject(_assocTable);
@@ -53,9 +53,9 @@ void BauSystemBDevice::loop()
 
 void BauSystemBDevice::sendNextGroupTelegram()
 {
-    if(!configured())
+    if (!configured())
         return;
-    
+
     static uint16_t startIdx = 1;
 
     GroupObjectTableObject& table = _groupObjTable;
@@ -69,7 +69,7 @@ void BauSystemBDevice::sendNextGroupTelegram()
         if (flag != ReadRequest && flag != WriteRequest)
             continue;
 
-        if (flag == WriteRequest) 
+        if (flag == WriteRequest)
         {
 #ifdef SMALL_GROUPOBJECT
             GroupObject::processClassCallback(go);
@@ -99,7 +99,7 @@ void BauSystemBDevice::sendNextGroupTelegram()
         {
             uint8_t* data = go.valueRef();
             _appLayer.groupValueWriteRequest(AckRequested, asap, go.priority(), NetworkLayerParameter, goSecurity, data,
-                go.sizeInTelegram());
+                                             go.sizeInTelegram());
         }
         else if (flag == ReadRequest)
         {
@@ -115,7 +115,7 @@ void BauSystemBDevice::sendNextGroupTelegram()
     startIdx = 1;
 }
 
-void BauSystemBDevice::updateGroupObject(GroupObject & go, uint8_t * data, uint8_t length)
+void BauSystemBDevice::updateGroupObject(GroupObject& go, uint8_t* data, uint8_t length)
 {
     uint8_t* goData = go.valueRef();
     if (length != go.valueSize())
@@ -146,17 +146,15 @@ void BauSystemBDevice::updateGroupObject(GroupObject & go, uint8_t * data, uint8
 bool BauSystemBDevice::configured()
 {
     // _configured is set to true initially, if the device was configured with ETS it will be set to true after restart
-    
+
     if (!_configured)
         return false;
-    
-    _configured = _groupObjTable.loadState() == LS_LOADED
-        && _addrTable.loadState() == LS_LOADED
-        && _assocTable.loadState() == LS_LOADED
-        && _appProgram.loadState() == LS_LOADED;
+
+    _configured = _groupObjTable.loadState() == LS_LOADED && _addrTable.loadState() == LS_LOADED && _assocTable.loadState() == LS_LOADED && _appProgram.loadState() == LS_LOADED;
 
 #ifdef USE_DATASECURE
-    _configured &= _secIfObj.loadState() == LS_LOADED;
+    if (_secIfObj.isSecurityModeEnabled())
+        _configured &= _secIfObj.loadState() == LS_LOADED;
 #endif
 
     return _configured;
@@ -174,7 +172,7 @@ void BauSystemBDevice::doMasterReset(EraseCode eraseCode, uint8_t channel)
 #endif
 }
 
-void BauSystemBDevice::groupValueWriteLocalConfirm(AckType ack, uint16_t asap, Priority priority, HopCountType hopType, const SecurityControl &secCtrl, uint8_t * data, uint8_t dataLength, bool status)
+void BauSystemBDevice::groupValueWriteLocalConfirm(AckType ack, uint16_t asap, Priority priority, HopCountType hopType, const SecurityControl& secCtrl, uint8_t* data, uint8_t dataLength, bool status)
 {
     GroupObject& go = _groupObjTable.get(asap);
     if (status)
@@ -183,7 +181,7 @@ void BauSystemBDevice::groupValueWriteLocalConfirm(AckType ack, uint16_t asap, P
         go.commFlag(Error);
 }
 
-void BauSystemBDevice::groupValueReadLocalConfirm(AckType ack, uint16_t asap, Priority priority, HopCountType hopType, const SecurityControl &secCtrl, bool status)
+void BauSystemBDevice::groupValueReadLocalConfirm(AckType ack, uint16_t asap, Priority priority, HopCountType hopType, const SecurityControl& secCtrl, bool status)
 {
     GroupObject& go = _groupObjTable.get(asap);
     if (status)
@@ -192,7 +190,7 @@ void BauSystemBDevice::groupValueReadLocalConfirm(AckType ack, uint16_t asap, Pr
         go.commFlag(Error);
 }
 
-void BauSystemBDevice::groupValueReadIndication(uint16_t asap, Priority priority, HopCountType hopType, const SecurityControl &secCtrl)
+void BauSystemBDevice::groupValueReadIndication(uint16_t asap, Priority priority, HopCountType hopType, const SecurityControl& secCtrl)
 {
 #ifdef USE_DATASECURE
     DataSecurity requiredGoSecurity;
@@ -211,13 +209,13 @@ void BauSystemBDevice::groupValueReadIndication(uint16_t asap, Priority priority
 
     if (!go.communicationEnable() || !go.readEnable())
         return;
-    
+
     uint8_t* data = go.valueRef();
     _appLayer.groupValueReadResponse(AckRequested, asap, priority, hopType, secCtrl, data, go.sizeInTelegram());
 }
 
-void BauSystemBDevice::groupValueReadAppLayerConfirm(uint16_t asap, Priority priority, HopCountType hopType, const SecurityControl &secCtrl, uint8_t* data,
-    uint8_t dataLength)
+void BauSystemBDevice::groupValueReadAppLayerConfirm(uint16_t asap, Priority priority, HopCountType hopType, const SecurityControl& secCtrl, uint8_t* data,
+                                                     uint8_t dataLength)
 {
     GroupObject& go = _groupObjTable.get(asap);
 
@@ -227,7 +225,7 @@ void BauSystemBDevice::groupValueReadAppLayerConfirm(uint16_t asap, Priority pri
     updateGroupObject(go, data, dataLength);
 }
 
-void BauSystemBDevice::groupValueWriteIndication(uint16_t asap, Priority priority, HopCountType hopType, const SecurityControl &secCtrl, uint8_t * data, uint8_t dataLength)
+void BauSystemBDevice::groupValueWriteIndication(uint16_t asap, Priority priority, HopCountType hopType, const SecurityControl& secCtrl, uint8_t* data, uint8_t dataLength)
 {
 #ifdef USE_DATASECURE
     DataSecurity requiredGoSecurity;
diff --git a/src/knx/secure_application_layer.cpp b/src/knx/secure_application_layer.cpp
index 7913c78f..1d889354 100644
--- a/src/knx/secure_application_layer.cpp
+++ b/src/knx/secure_application_layer.cpp
@@ -1,17 +1,17 @@
 #include "config.h"
 #ifdef USE_DATASECURE
 
-#include "secure_application_layer.h"
-#include "transport_layer.h"
-#include "cemi_frame.h"
-#include "association_table_object.h"
 #include "address_table_object.h"
-#include "security_interface_object.h"
-#include "device_object.h"
 #include "apdu.h"
+#include "association_table_object.h"
 #include "bau.h"
-#include "string.h"
 #include "bits.h"
+#include "cemi_frame.h"
+#include "device_object.h"
+#include "secure_application_layer.h"
+#include "security_interface_object.h"
+#include "string.h"
+#include "transport_layer.h"
 
 // Select what cipher modes to include. We need AES128-CBC and AES128-CTR modes.
 #define CBC 1
@@ -23,14 +23,14 @@ static constexpr uint8_t kSecureDataPdu = 0;
 static constexpr uint8_t kSecureSyncRequest = 2;
 static constexpr uint8_t kSecureSyncResponse = 3;
 
-SecureApplicationLayer::SecureApplicationLayer(DeviceObject &deviceObj, SecurityInterfaceObject &secIfObj, BusAccessUnit& bau):
-    ApplicationLayer(bau),
-    _secIfObj(secIfObj),
-    _deviceObj(deviceObj)
+SecureApplicationLayer::SecureApplicationLayer(DeviceObject& deviceObj, SecurityInterfaceObject& secIfObj, BusAccessUnit& bau)
+    : ApplicationLayer(bau),
+      _secIfObj(secIfObj),
+      _deviceObj(deviceObj)
 {
 }
 
-void SecureApplicationLayer::groupAddressTable(AddressTableObject &addrTable)
+void SecureApplicationLayer::groupAddressTable(AddressTableObject& addrTable)
 {
     _addrTab = &addrTable;
 }
@@ -65,7 +65,7 @@ void SecureApplicationLayer::dataGroupIndication(HopCountType hopType, Priority
     ApplicationLayer::dataGroupIndication(hopType, priority, tsap, apdu);
 }
 
-void SecureApplicationLayer::dataGroupConfirm(AckType ack, HopCountType hopType, Priority priority,  uint16_t tsap, APDU& apdu, bool status)
+void SecureApplicationLayer::dataGroupConfirm(AckType ack, HopCountType hopType, Priority priority, uint16_t tsap, APDU& apdu, bool status)
 {
     println("dataGroupConfirm");
 
@@ -395,7 +395,7 @@ void SecureApplicationLayer::dataIndividualRequest(AckType ack, HopCountType hop
     ApplicationLayer::dataIndividualRequest(ack, hopType, priority, destination, apdu, secCtrl);
 }
 
-void SecureApplicationLayer::dataConnectedRequest(uint16_t tsap, Priority priority, APDU& apdu, const SecurityControl &secCtrl)
+void SecureApplicationLayer::dataConnectedRequest(uint16_t tsap, Priority priority, APDU& apdu, const SecurityControl& secCtrl)
 {
     println("dataConnectedRequest");
 
@@ -428,7 +428,7 @@ void SecureApplicationLayer::encryptAesCbc(uint8_t* buffer, uint16_t bufLen, con
     AES_init_ctx_iv(&ctx, key, zeroIv);
 
     // Now encrypt first block B0.
-    AES_CBC_encrypt_buffer(&ctx, (uint8_t*) iv, 16);
+    AES_CBC_encrypt_buffer(&ctx, (uint8_t*)iv, 16);
 
     // Encrypt remaining buffer
     AES_CBC_encrypt_buffer(&ctx, buffer, bufLen);
@@ -467,8 +467,8 @@ uint32_t SecureApplicationLayer::calcAuthOnlyMac(uint8_t* apdu, uint8_t apduLeng
 }
 
 uint32_t SecureApplicationLayer::calcConfAuthMac(uint8_t* associatedData, uint16_t associatedDataLength,
-                            uint8_t* apdu, uint8_t apduLength,
-                            const uint8_t* key, uint8_t* iv)
+                                                 uint8_t* apdu, uint8_t apduLength,
+                                                 const uint8_t* key, uint8_t* iv)
 {
     uint16_t bufLen = 2 + associatedDataLength + apduLength; // 2 bytes for the length field (uint16_t)
     // AES-128 operates on blocks of 16 bytes, add padding
@@ -497,12 +497,12 @@ void SecureApplicationLayer::block0(uint8_t* buffer, uint8_t* seqNum, uint16_t i
     pBuf = pushByteArray(seqNum, 6, pBuf);
     pBuf = pushWord(indSrcAddr, pBuf);
     pBuf = pushWord(dstAddr, pBuf);
-    pBuf = pushByte(0x00, pBuf); // FT: frametype
-    pBuf = pushByte( (dstAddrIsGroupAddr ? 0x80 : 0x00) | (extFrameFormat & 0xf), pBuf); // AT: address type
-    pBuf = pushByte(tpci, pBuf); // TPCI
-    pBuf = pushByte(apci, pBuf); // APCI // draft spec shows something different!
-    pBuf = pushByte(0x00, pBuf); // Reserved: fixed 0x00 (really?)
-    pBuf = pushByte(payloadLength, pBuf); // Payload length
+    pBuf = pushByte(0x00, pBuf);                                                        // FT: frametype
+    pBuf = pushByte((dstAddrIsGroupAddr ? 0x80 : 0x00) | (extFrameFormat & 0xf), pBuf); // AT: address type
+    pBuf = pushByte(tpci, pBuf);                                                        // TPCI
+    pBuf = pushByte(apci, pBuf);                                                        // APCI // draft spec shows something different!
+    pBuf = pushByte(0x00, pBuf);                                                        // Reserved: fixed 0x00 (really?)
+    pBuf = pushByte(payloadLength, pBuf);                                               // Payload length
 }
 
 void SecureApplicationLayer::blockCtr0(uint8_t* buffer, uint8_t* seqNum, uint16_t indSrcAddr, uint16_t dstAddr)
@@ -591,7 +591,7 @@ void SecureApplicationLayer::updateLastValidSequence(bool toolAccess, uint16_t r
     }
 }
 
-void SecureApplicationLayer::sendSyncRequest(uint16_t dstAddr, bool dstAddrIsGroupAddr, const SecurityControl &secCtrl, bool systemBcast)
+void SecureApplicationLayer::sendSyncRequest(uint16_t dstAddr, bool dstAddrIsGroupAddr, const SecurityControl& secCtrl, bool systemBcast)
 {
     if (secCtrl.dataSecurity != DataSecurity::AuthConf)
     {
@@ -608,7 +608,7 @@ void SecureApplicationLayer::sendSyncRequest(uint16_t dstAddr, bool dstAddrIsGro
     sixBytesFromUInt64(challenge, &asdu[0]);
 
     CemiFrame request(2 + 6 + sizeof(asdu) + 4); // 2 bytes (APCI, SCF) + 6 bytes (SeqNum) + 6 bytes (challenge) + 4 bytes (MAC)
-                                                      // Note: additional TPCI byte is already handled internally!
+                                                 // Note: additional TPCI byte is already handled internally!
 
     uint8_t tpci = 0;
     if (!_syncReqBroadcastOutgoing)
@@ -621,7 +621,7 @@ void SecureApplicationLayer::sendSyncRequest(uint16_t dstAddr, bool dstAddrIsGro
     print("sendSyncRequest: TPCI: ");
     println(tpci, HEX);
 
-    if(secure(request.data() + APDU_LPDU_DIFF, kSecureSyncRequest, _deviceObj.individualAddress(), dstAddr, dstAddrIsGroupAddr, tpci, asdu, sizeof(asdu), secCtrl, systemBcast))
+    if (secure(request.data() + APDU_LPDU_DIFF, kSecureSyncRequest, _deviceObj.individualAddress(), dstAddr, dstAddrIsGroupAddr, tpci, asdu, sizeof(asdu), secCtrl, systemBcast))
     {
         println("SyncRequest: ");
         request.apdu().printPDU();
@@ -653,7 +653,7 @@ void SecureApplicationLayer::sendSyncRequest(uint16_t dstAddr, bool dstAddrIsGro
     }
 }
 
-void SecureApplicationLayer::sendSyncResponse(uint16_t dstAddr, bool dstAddrIsGroupAddr, const SecurityControl &secCtrl, uint64_t remoteNextSeqNum, bool systemBcast)
+void SecureApplicationLayer::sendSyncResponse(uint16_t dstAddr, bool dstAddrIsGroupAddr, const SecurityControl& secCtrl, uint64_t remoteNextSeqNum, bool systemBcast)
 {
     if (secCtrl.dataSecurity != DataSecurity::AuthConf)
     {
@@ -668,7 +668,7 @@ void SecureApplicationLayer::sendSyncResponse(uint16_t dstAddr, bool dstAddrIsGr
     sixBytesFromUInt64(remoteNextSeqNum, &asdu[6]);
 
     CemiFrame response(2 + 6 + sizeof(asdu) + 4); // 2 bytes (APCI, SCF) + 6 bytes (SeqNum) + 12 bytes + 4 bytes (MAC)
-                                                      // Note: additional TPCI byte is already handled internally!
+                                                  // Note: additional TPCI byte is already handled internally!
 
     uint8_t tpci = 0;
     if (!_syncReqBroadcastIncoming)
@@ -681,7 +681,7 @@ void SecureApplicationLayer::sendSyncResponse(uint16_t dstAddr, bool dstAddrIsGr
     print("sendSyncResponse: TPCI: ");
     println(tpci, HEX);
 
-    if(secure(response.data() + APDU_LPDU_DIFF, kSecureSyncResponse, _deviceObj.individualAddress(), dstAddr, dstAddrIsGroupAddr, tpci, asdu, sizeof(asdu), secCtrl, systemBcast))
+    if (secure(response.data() + APDU_LPDU_DIFF, kSecureSyncResponse, _deviceObj.individualAddress(), dstAddr, dstAddrIsGroupAddr, tpci, asdu, sizeof(asdu), secCtrl, systemBcast))
     {
         _lastSyncRes = millis();
 
@@ -712,7 +712,7 @@ void SecureApplicationLayer::sendSyncResponse(uint16_t dstAddr, bool dstAddrIsGr
     }
 }
 
-void SecureApplicationLayer::receivedSyncRequest(uint16_t srcAddr, uint16_t dstAddr, bool dstAddrIsGroupAddr, const SecurityControl &secCtrl, uint8_t* seqNum, uint64_t challenge, bool systemBcast)
+void SecureApplicationLayer::receivedSyncRequest(uint16_t srcAddr, uint16_t dstAddr, bool dstAddrIsGroupAddr, const SecurityControl& secCtrl, uint8_t* seqNum, uint64_t challenge, bool systemBcast)
 {
     println("Received SyncRequest:");
 
@@ -728,14 +728,14 @@ void SecureApplicationLayer::receivedSyncRequest(uint16_t srcAddr, uint16_t dstA
     _syncReqBroadcastIncoming = (dstAddr == 0x0000) && dstAddrIsGroupAddr;
 
     // Remember challenge for securing the sync.res later
-    _pendingIncomingSyncRequests.insertOrAssign(_syncReqBroadcastIncoming ? (Addr) GrpAddr(0) : (Addr) IndAddr(srcAddr), challenge);
+    _pendingIncomingSyncRequests.insertOrAssign(_syncReqBroadcastIncoming ? (Addr)GrpAddr(0) : (Addr)IndAddr(srcAddr), challenge);
 
     uint16_t toAddr = _syncReqBroadcastIncoming ? dstAddr : srcAddr;
     bool toIsGroupAddress = _syncReqBroadcastIncoming;
     sendSyncResponse(toAddr, toIsGroupAddress, secCtrl, nextSeqNum, systemBcast);
 }
 
-void SecureApplicationLayer::receivedSyncResponse(uint16_t remote, const SecurityControl &secCtrl, uint8_t* plainApdu)
+void SecureApplicationLayer::receivedSyncResponse(uint16_t remote, const SecurityControl& secCtrl, uint8_t* plainApdu)
 {
     println("Received SyncResponse:");
 
@@ -764,13 +764,14 @@ void SecureApplicationLayer::receivedSyncResponse(uint16_t remote, const Securit
     uint64_t last = lastValidSequenceNumber(secCtrl.toolAccess, remote);
     if (remoteSeq - 1 > last)
     {
-        //logger.debug("sync.res update {} last valid {} seq -> {}", remote, toolAccess ? "tool access" : "p2p", remoteSeq -1);
+        // logger.debug("sync.res update {} last valid {} seq -> {}", remote, toolAccess ? "tool access" : "p2p", remoteSeq -1);
         updateLastValidSequence(secCtrl.toolAccess, remote, remoteSeq - 1);
     }
 
     uint64_t next = nextSequenceNumber(secCtrl.toolAccess);
-    if (localSeq > next) {
-        //logger.debug("sync.res update local next {} seq -> {}", toolAccess ? "tool access" : "p2p", localSeq);
+    if (localSeq > next)
+    {
+        // logger.debug("sync.res update local next {} seq -> {}", toolAccess ? "tool access" : "p2p", localSeq);
         updateSequenceNumber(secCtrl.toolAccess, localSeq);
     }
 
@@ -788,7 +789,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt
     bool toolAccess = ((scf & 0x80) == 0x80);
     bool systemBroadcast = ((scf & 0x08) == 0x08);
     uint8_t sai = (scf >> 4) & 0x07; // sai can only be 0x0 (CCM auth only) or 0x1 (CCM with auth+conf), other values are reserved
-    bool authOnly = ( sai == 0);
+    bool authOnly = (sai == 0);
     uint8_t service = (scf & 0x07); // only 0x0 (S-A_Data-PDU), 0x2 (S-A_Sync_Req-PDU) or 0x3 (S-A_Sync_Rsp-PDU) are valid values
 
     if (systemBroadcast != systemBcast)
@@ -802,8 +803,9 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt
     bool syncReq = service == kSecureSyncRequest;
     bool syncRes = service == kSecureSyncResponse;
 
-    //const uint8_t* key = dstAddrIsGroupAddr ? securityKey(dstAddr, dstAddrIsGroupAddr) : toolAccess ? toolKey() : securityKey(srcAddr, false);
-    const uint8_t* key = dstAddrIsGroupAddr && (dstAddr != 0) ? securityKey(dstAddr, dstAddrIsGroupAddr) : toolAccess ? _secIfObj.toolKey() : securityKey(srcAddr, false);
+    // const uint8_t* key = dstAddrIsGroupAddr ? securityKey(dstAddr, dstAddrIsGroupAddr) : toolAccess ? toolKey() : securityKey(srcAddr, false);
+    const uint8_t* key = dstAddrIsGroupAddr && (dstAddr != 0) ? securityKey(dstAddr, dstAddrIsGroupAddr) : toolAccess ? _secIfObj.toolKey()
+                                                                                                                      : securityKey(srcAddr, false);
     if (key == nullptr)
     {
         print("Error: No key found. toolAccess: ");
@@ -838,7 +840,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt
             }
         }
     }
-    else if(syncReq)
+    else if (syncReq)
     {
         pBuf = popByteArray(knxSerialNumber, 6, pBuf);
         remainingPlainApduLength -= 6;
@@ -847,7 +849,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt
         if (!memcmp(knxSerialNumber, _deviceObj.propertyData(PID_SERIAL_NUMBER), 6))
         {
             uint8_t emptySerialNumber[6] = {0};
-            if (systemBroadcast || dstAddr != _deviceObj.individualAddress() || !memcmp(knxSerialNumber, emptySerialNumber, 6))
+            if (systemBcast || dstAddr != _deviceObj.individualAddress() || !memcmp(knxSerialNumber, emptySerialNumber, 6))
                 return false;
         }
 
@@ -860,7 +862,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt
     else if (syncRes)
     {
         // fetch challenge depending on srcAddr to handle multiple requests
-        uint64_t *challenge = _pendingOutgoingSyncRequests.get(IndAddr(srcAddr));
+        uint64_t* challenge = _pendingOutgoingSyncRequests.get(IndAddr(srcAddr));
         if (challenge == nullptr)
         {
             println("Cannot find matching challenge for source address!");
@@ -885,12 +887,12 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt
     // Clear IV buffer
     uint8_t iv[16] = {0x00};
     // Create first block B0 for AES CBC MAC calculation, used as IV later
-  /*
-    printHex("seq: ", seqNum, 6);
-    printHex("src: ", (uint8_t*) &srcAddr, 2);
-    printHex("dst: ", (uint8_t*) &dstAddr, 2);
-    print("dstAddrisGroup: ");println(dstAddrIsGroupAddr ? "true" : "false");
-*/
+    /*
+      printHex("seq: ", seqNum, 6);
+      printHex("src: ", (uint8_t*) &srcAddr, 2);
+      printHex("dst: ", (uint8_t*) &dstAddr, 2);
+      print("dstAddrisGroup: ");println(dstAddrIsGroupAddr ? "true" : "false");
+  */
     block0(iv, seqNum, srcAddr, dstAddr, dstAddrIsGroupAddr, extendedFrameFormat, tpci | (SecureService >> 8), SecureService & 0x00FF, remainingPlainApduLength);
 
     // Clear block counter0 buffer
@@ -929,15 +931,15 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt
         // AES-128 operates on blocks of 16 bytes, add padding
         uint16_t bufLenPadded = (bufLen + 15) / 16 * 16;
         uint8_t buffer[bufLenPadded];
-        //uint8_t buffer[bufLen];
-        // Make sure to have zeroes everywhere, because of the padding
+        // uint8_t buffer[bufLen];
+        //  Make sure to have zeroes everywhere, because of the padding
         memset(buffer, 0x00, bufLenPadded);
 
         pushInt(mac, &buffer[0]);
         pushByteArray(plainApdu, remainingPlainApduLength, &buffer[4]); // apdu is still encrypted
 
         xcryptAesCtr(buffer, bufLenPadded, ctr0, key);
-        //xcryptAesCtr(buffer, bufLen, ctr0, key);
+        // xcryptAesCtr(buffer, bufLen, ctr0, key);
 
         uint32_t decryptedMac;
         popInt(decryptedMac, &buffer[0]);
@@ -950,11 +952,11 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt
         {
             memcpy(&associatedData[1], knxSerialNumber, 6);
         }
-/*
-        printHex("APDU--------->", plainApdu, remainingPlainApduLength);
-        printHex("Key---------->", key, 16);
-        printHex("ASSOC-------->", associatedData, sizeof(associatedData));
-*/
+        /*
+                printHex("APDU--------->", plainApdu, remainingPlainApduLength);
+                printHex("Key---------->", key, 16);
+                printHex("ASSOC-------->", associatedData, sizeof(associatedData));
+        */
         uint32_t calculatedMac = calcConfAuthMac(associatedData, sizeof(associatedData), plainApdu, remainingPlainApduLength, key, iv);
         if (calculatedMac != decryptedMac)
         {
@@ -976,7 +978,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt
         if (syncReq)
         {
             uint64_t challenge = sixBytesToUInt64(&plainApdu[0]);
-            receivedSyncRequest(srcAddr, dstAddr, dstAddrIsGroupAddr, secCtrl, seqNum, challenge, systemBroadcast);
+            receivedSyncRequest(srcAddr, dstAddr, dstAddrIsGroupAddr, secCtrl, seqNum, challenge, systemBcast);
             return false;
         }
         else if (syncRes)
@@ -993,7 +995,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt
                 print(" seq from ");
                 print(srcAddr, HEX);
                 print(" -> (+1) ");
-                println(receivedSeqNumber,HEX);
+                println(receivedSeqNumber, HEX);
                 updateSequenceNumber(toolAccess, receivedSeqNumber + 1);
             }
             else
@@ -1037,7 +1039,7 @@ bool SecureApplicationLayer::decodeSecureApdu(APDU& secureApdu, APDU& plainApdu,
 
     // FIXME: when cEMI class is refactored, there might be additional info fields in cEMI (fixed APDU_LPDU_DIFF)
     // We are starting from TPCI octet (including): plainApdu.frame().data()+APDU_LPDU_DIFF
-    if (decrypt(plainApdu.frame().data()+APDU_LPDU_DIFF, plainApdu.length()+1, srcAddress, dstAddress, isDstAddrGroupAddr, tpci, secureApdu.data()+1, secCtrl, isSystemBroadcast))
+    if (decrypt(plainApdu.frame().data() + APDU_LPDU_DIFF, plainApdu.length() + 1, srcAddress, dstAddress, isDstAddrGroupAddr, tpci, secureApdu.data() + 1, secCtrl, isSystemBroadcast))
     {
         println("decodeSecureApdu: Plain APDU: ");
         plainApdu.frame().apdu().printPDU();
@@ -1082,8 +1084,8 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
     tpci |= SecureService >> 8; // OR'ing upper two APCI bits
     uint8_t apci = SecureService & 0x00FF;
     uint8_t* pBuf = buffer;
-    pBuf = pushByte(tpci, pBuf);                      // TPCI
-    pBuf = pushByte(apci, pBuf);                      // APCI
+    pBuf = pushByte(tpci, pBuf); // TPCI
+    pBuf = pushByte(apci, pBuf); // APCI
 
     uint8_t scf;
     scf = service;
@@ -1091,7 +1093,7 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
     scf |= confidentiality ? 0x10 : 0;
     scf |= systemBcast ? 0x8 : 0;
 
-    pBuf = pushByte(scf, pBuf);                       // SCF
+    pBuf = pushByte(scf, pBuf); // SCF
 
     uint64_t seqSend = nextSequenceNumber(toolAccess);
     if (seqSend == 0)
@@ -1100,7 +1102,7 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
     uint8_t seq[6];
     sixBytesFromUInt64(seqSend, seq);
     if (!syncRes)
-        pBuf = pushByteArray(seq, 6, pBuf);          // Sequence Number
+        pBuf = pushByteArray(seq, 6, pBuf); // Sequence Number
 
     // Prepare associated data depending on service (SyncRequest, SyncResponse or just DataService)
     uint8_t associatedData[syncReq ? 7 : 1];
@@ -1123,7 +1125,7 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
         Addr remote = _syncReqBroadcastIncoming ? (Addr)GrpAddr(0) : (Addr)IndAddr(dstAddr);
 
         // Get challenge from sync.req
-        uint64_t *challenge = _pendingIncomingSyncRequests.get(remote);
+        uint64_t* challenge = _pendingIncomingSyncRequests.get(remote);
         if (challenge == nullptr)
         {
             println("Cannot send sync.res without corresponding sync.req");
@@ -1135,7 +1137,7 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
         }
         uint8_t challengeSixBytes[6];
         sixBytesFromUInt64(*challenge, challengeSixBytes);
-        //printHex("Decrypted challenge: ", challengeSixBytes, 6);
+        // printHex("Decrypted challenge: ", challengeSixBytes, 6);
 
         // Now XOR the new random SeqNum with the challenge from the SyncRequest
         uint8_t rndXorChallenge[6];
@@ -1153,12 +1155,12 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
     // Clear IV buffer
     uint8_t iv[16] = {0x00};
     // Create first block B0 for AES CBC MAC calculation, used as IV later
-/*
-    printHex("seq: ", seq, 6);
-    printHex("src: ", (uint8_t*) &srcAddr, 2);
-    printHex("dst: ", (uint8_t*) &dstAddr, 2);
-    print("dstAddrisGroup: ");println(dstAddrIsGroupAddr ? "true" : "false");
-*/
+    /*
+        printHex("seq: ", seq, 6);
+        printHex("src: ", (uint8_t*) &srcAddr, 2);
+        printHex("dst: ", (uint8_t*) &dstAddr, 2);
+        print("dstAddrisGroup: ");println(dstAddrIsGroupAddr ? "true" : "false");
+    */
     block0(iv, seq, srcAddr, dstAddr, dstAddrIsGroupAddr, extendedFrameFormat, tpci, apci, apduLength);
 
     // Clear block counter0 buffer
@@ -1169,11 +1171,11 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
     if (confidentiality)
     {
         // Do calculations for Auth+Conf
-/*
-        printHex("APDU--------->", apdu, apduLength);
-        printHex("Key---------->", key, 16);
-        printHex("ASSOC-------->", associatedData, sizeof(associatedData));
-*/
+        /*
+                printHex("APDU--------->", apdu, apduLength);
+                printHex("Key---------->", key, 16);
+                printHex("ASSOC-------->", associatedData, sizeof(associatedData));
+        */
         uint32_t mac = calcConfAuthMac(associatedData, sizeof(associatedData), apdu, apduLength, key, iv);
 
         uint8_t tmpBuffer[4 + apduLength];
@@ -1185,16 +1187,16 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
         pBuf = pushByteArray(tmpBuffer + 4, apduLength, pBuf); // Encrypted APDU
         pBuf = pushByteArray(tmpBuffer + 0, 4, pBuf);          // Encrypted MAC
 
-        //print("MAC(encrypted): ");
-        //println(*((uint32_t*)(tmpBuffer + 0)),HEX);
+        // print("MAC(encrypted): ");
+        // println(*((uint32_t*)(tmpBuffer + 0)),HEX);
     }
     else
     {
         // Do calculations for AuthOnly
         uint32_t tmpMac = calcAuthOnlyMac(apdu, apduLength, key, iv, ctr0);
 
-        pBuf = pushByteArray(apdu, apduLength, pBuf);          // Plain APDU
-        pBuf = pushInt(tmpMac, pBuf);                          // MAC
+        pBuf = pushByteArray(apdu, apduLength, pBuf); // Plain APDU
+        pBuf = pushInt(tmpMac, pBuf);                 // MAC
 
         print("MAC: ");
         println(tmpMac, HEX);
@@ -1232,14 +1234,14 @@ bool SecureApplicationLayer::createSecureApdu(APDU& plainApdu, APDU& secureApdu,
 
     // FIXME: when cEMI class is refactored, there might be additional info fields in cEMI (fixed APDU_LPDU_DIFF)
     // We are starting from TPCI octet (including): plainApdu.frame().data()+APDU_LPDU_DIFF
-    if(secure(secureApdu.frame().data()+APDU_LPDU_DIFF, kSecureDataPdu, srcAddress, dstAddress, isDstAddrGroupAddr, tpci, plainApdu.frame().data()+APDU_LPDU_DIFF, plainApdu.length()+1, secCtrl, isSystemBroadcast))
+    if (secure(secureApdu.frame().data() + APDU_LPDU_DIFF, kSecureDataPdu, srcAddress, dstAddress, isDstAddrGroupAddr, tpci, plainApdu.frame().data() + APDU_LPDU_DIFF, plainApdu.length() + 1, secCtrl, isSystemBroadcast))
     {
         print("Update our next ");
         print(secCtrl.toolAccess ? "tool access" : "");
         print(" seq from ");
         print(srcAddress, HEX);
         print(" -> (+1) ");
-        println(nextSequenceNumber(secCtrl.toolAccess),HEX);
+        println(nextSequenceNumber(secCtrl.toolAccess), HEX);
         updateSequenceNumber(secCtrl.toolAccess, nextSequenceNumber(secCtrl.toolAccess) + 1);
 
         println("createSecureApdu: Secure APDU: ");
@@ -1264,7 +1266,7 @@ void SecureApplicationLayer::loop()
 
 bool SecureApplicationLayer::isSyncService(APDU& secureApdu)
 {
-    uint8_t scf = *(secureApdu.data()+1);
+    uint8_t scf = *(secureApdu.data() + 1);
     uint8_t service = (scf & 0x07); // only 0x0 (S-A_Data-PDU), 0x2 (S-A_Sync_Req-PDU) or 0x3 (S-A_Sync_Rsp-PDU) are valid values
 
     if ((service == kSecureSyncRequest) || (service == kSecureSyncResponse))
diff --git a/src/knx/security_interface_object.cpp b/src/knx/security_interface_object.cpp
index bf0cdde9..831d535c 100644
--- a/src/knx/security_interface_object.cpp
+++ b/src/knx/security_interface_object.cpp
@@ -10,7 +10,7 @@
 #include "function_property.h"
 
 // Our FDSK. It is never changed from ETS. This is the permanent default tool key that is restored on every factory reset of the device.
-const uint8_t SecurityInterfaceObject::_fdsk[] = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F };
+const uint8_t SecurityInterfaceObject::_fdsk[] = {0xFF, 0xEE, 0xDD, 0xCC, 0xBB, 0xAA, 0x99, 0x88, 0x77, 0x66, 0x55, 0x44, 0x33, 0x22, 0x11, 0x00};
 uint8_t SecurityInterfaceObject::_secReport[] = { 0x00, 0x00, 0x00 };
 uint8_t SecurityInterfaceObject::_secReportCtrl[] = { 0x00, 0x00, 0x00 };
 
diff --git a/src/rp2040_arduino_platform.cpp b/src/rp2040_arduino_platform.cpp
index 96c13c91..6b899adf 100644
--- a/src/rp2040_arduino_platform.cpp
+++ b/src/rp2040_arduino_platform.cpp
@@ -312,7 +312,7 @@ uint32_t RP2040ArduinoPlatform::uniqueSerialNumber()
 void RP2040ArduinoPlatform::restart()
 {
     println("restart");
-    watchdog_reboot(0, 0, 0);
+    watchdog_reboot(0, 0, 1000);
 }
 
 #ifdef USE_RP2040_EEPROM_EMULATION