From ec3999357e7e80d7a0302f17b3b4e94a44e9c5a5 Mon Sep 17 00:00:00 2001 From: Nanosonde <2073569+nanosonde@users.noreply.github.com> Date: Wed, 1 Jul 2020 21:48:28 +0200 Subject: [PATCH] save work --- src/knx/bau_systemB.cpp | 2 +- src/knx/bits.cpp | 21 +++ src/knx/bits.h | 3 + src/knx/data_property.cpp | 10 ++ src/knx/data_property.h | 1 + src/knx/interface_object.cpp | 6 + src/knx/interface_object.h | 1 + src/knx/secure_application_layer.cpp | 159 ++++++----------------- src/knx/secure_application_layer.h | 17 +-- src/knx/security_interface_object.cpp | 177 ++++++++++++++++++++++++++ src/knx/security_interface_object.h | 12 ++ 11 files changed, 278 insertions(+), 131 deletions(-) diff --git a/src/knx/bau_systemB.cpp b/src/knx/bau_systemB.cpp index 213a890..eb05f1b 100644 --- a/src/knx/bau_systemB.cpp +++ b/src/knx/bau_systemB.cpp @@ -18,7 +18,7 @@ BauSystemB::BauSystemB(Platform& platform): _memory(platform, _deviceObj), _addr _assocTable(_memory), _groupObjTable(_memory), _appProgram(_memory), _platform(platform), #ifdef USE_DATASECURE - _appLayer(_deviceObj, _secIfObj, _assocTable, *this), + _appLayer(_deviceObj, _secIfObj, _assocTable, _addrTable, *this), #else _appLayer(_assocTable, *this), #endif diff --git a/src/knx/bits.cpp b/src/knx/bits.cpp index 551fbd2..b47d65f 100644 --- a/src/knx/bits.cpp +++ b/src/knx/bits.cpp @@ -84,3 +84,24 @@ uint32_t getInt(const uint8_t * data) { return (data[0] << 24) + (data[1] << 16) + (data[2] << 8) + data[3]; } + +void sixBytesFromUInt64(uint64_t num, uint8_t* toByteArray) +{ + toByteArray[0] = ((num >> 40) & 0xff); + toByteArray[1] = ((num >> 32) & 0xff); + toByteArray[2] = ((num >> 24) & 0xff); + toByteArray[3] = ((num >> 16) & 0xff); + toByteArray[4] = ((num >> 8) & 0xff); + toByteArray[5] = (num & 0xff); +} + +uint64_t sixBytesToUInt64(uint8_t* data) +{ + uint64_t l = 0; + + for (uint8_t i = 0; i < 6; i++) + { + l = (l << 8) + data[i]; + } + return l; +} diff --git a/src/knx/bits.h b/src/knx/bits.h index 565d24c..dd4b568 100644 --- a/src/knx/bits.h +++ b/src/knx/bits.h @@ -85,3 +85,6 @@ uint8_t* pushByteArray(const uint8_t* src, uint32_t size, uint8_t* data); uint16_t getWord(const uint8_t* data); uint32_t getInt(const uint8_t* data); void printHex(const char* suffix, const uint8_t *data, size_t length); + +void sixBytesFromUInt64(uint64_t num, uint8_t* toByteArray); +uint64_t sixBytesToUInt64(uint8_t* data); diff --git a/src/knx/data_property.cpp b/src/knx/data_property.cpp index 408ac32..c1ee851 100644 --- a/src/knx/data_property.cpp +++ b/src/knx/data_property.cpp @@ -151,3 +151,13 @@ const uint8_t* DataProperty::data() { return _data; } + +const uint8_t* DataProperty::data(uint16_t elementIndex) +{ + if ((elementIndex == 0) || (elementIndex > _currentElements)) + return nullptr; + + elementIndex -= 1; // Starting from 0 + uint16_t offset = elementIndex * ElementSize(); + return _data + offset; +} diff --git a/src/knx/data_property.h b/src/knx/data_property.h index e430eeb..12c202f 100644 --- a/src/knx/data_property.h +++ b/src/knx/data_property.h @@ -17,6 +17,7 @@ class DataProperty : public Property virtual const uint8_t* restore(const uint8_t* buffer) override; virtual uint16_t saveSize() override; const uint8_t* data(); + const uint8_t* data(uint16_t elementIndex); private: uint16_t _currentElements = 0; diff --git a/src/knx/interface_object.cpp b/src/knx/interface_object.cpp index ec8379b..6b6aa32 100644 --- a/src/knx/interface_object.cpp +++ b/src/knx/interface_object.cpp @@ -190,3 +190,9 @@ const uint8_t* InterfaceObject::propertyData(PropertyID id) DataProperty* prop = (DataProperty*)property(id); return prop->data(); } + +const uint8_t* InterfaceObject::propertyData(PropertyID id, uint16_t elementIndex) +{ + DataProperty* prop = (DataProperty*)property(id); + return prop->data(elementIndex); +} diff --git a/src/knx/interface_object.h b/src/knx/interface_object.h index daa96fc..375d14d 100644 --- a/src/knx/interface_object.h +++ b/src/knx/interface_object.h @@ -172,6 +172,7 @@ class InterfaceObject : public SaveRestore } const uint8_t* propertyData(PropertyID id); + const uint8_t* propertyData(PropertyID id, uint16_t elementIndex); /** * Gets const property with PropertyID id if it exists and nullptr otherwise. */ diff --git a/src/knx/secure_application_layer.cpp b/src/knx/secure_application_layer.cpp index 2615b6d..31abae7 100644 --- a/src/knx/secure_application_layer.cpp +++ b/src/knx/secure_application_layer.cpp @@ -2,6 +2,7 @@ #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" @@ -15,22 +16,16 @@ #define ECB 0 #include "aes.hpp" -const uint8_t SecureDataPdu = 0; -const uint8_t SecureSyncRequest = 2; -const uint8_t SecureSyncResponse = 3; +static constexpr uint8_t kSecureDataPdu = 0; +static constexpr uint8_t kSecureSyncRequest = 2; +static constexpr uint8_t kSecureSyncResponse = 3; -uint64_t sequenceNumberToolAccess = 50; -uint64_t sequenceNumber = 0; - -uint64_t lastValidSequenceNumberTool = 0; -uint64_t lastValidSequenceNumber = 0; - -SecureApplicationLayer::SecureApplicationLayer(DeviceObject &deviceObj, SecurityInterfaceObject &secIfObj, AssociationTableObject& assocTable, BusAccessUnit& bau): +SecureApplicationLayer::SecureApplicationLayer(DeviceObject &deviceObj, SecurityInterfaceObject &secIfObj, AssociationTableObject& assocTable, AddressTableObject &addrTab, BusAccessUnit& bau): ApplicationLayer(assocTable, bau), _secIfObj(secIfObj), - _deviceObj(deviceObj) + _deviceObj(deviceObj), + _addrTab(addrTab) { - } /* from transport layer */ @@ -501,62 +496,9 @@ void SecureApplicationLayer::blockCtr0(uint8_t* buffer, uint8_t* seqNum, uint16_ pBuf = pushByte(0x01, pBuf); } -void SecureApplicationLayer::sixBytesFromUInt64(uint64_t num, uint8_t* toByteArray) -{ - toByteArray[0] = ((num >> 40) & 0xff); - toByteArray[1] = ((num >> 32) & 0xff); - toByteArray[2] = ((num >> 24) & 0xff); - toByteArray[3] = ((num >> 16) & 0xff); - toByteArray[4] = ((num >> 8) & 0xff); - toByteArray[5] = (num & 0xff); -} - -uint64_t SecureApplicationLayer::sixBytesToUInt64(uint8_t* data) -{ - uint64_t l = 0; - - for (uint8_t i = 0; i < 6; i++) - { - l = (l << 8) + data[i]; - } - return l; -} - -const uint8_t* SecureApplicationLayer::toolKey(uint16_t devAddr) -{ - //TODO: multiple tool keys possible - const uint8_t* toolKey = _secIfObj.propertyData(PID_TOOL_KEY); - return toolKey; -} - -const uint8_t* SecureApplicationLayer::p2pKey(uint16_t addressIndex) -{ - if (!_secIfObj.isLoaded()) - return nullptr; - - // TODO - return _secIfObj.propertyData(PID_P2P_KEY_TABLE); -} - -const uint8_t* SecureApplicationLayer::groupKey(uint16_t addressIndex) -{ - if (!_secIfObj.isLoaded()) - return nullptr; - - // TODO - return _secIfObj.propertyData(PID_GRP_KEY_TABLE); -} - uint16_t SecureApplicationLayer::groupAddressIndex(uint16_t groupAddr) { - // TODO - return 0; -} - -uint16_t SecureApplicationLayer::indAddressIndex(uint16_t indAddr) -{ - // TODO - return 0; + return _addrTab.getTsap(groupAddr); } const uint8_t* SecureApplicationLayer::securityKey(uint16_t addr, bool isGroupAddress) @@ -565,13 +507,13 @@ const uint8_t* SecureApplicationLayer::securityKey(uint16_t addr, bool isGroupAd { uint16_t gaIndex = groupAddressIndex(addr); if (gaIndex > 0) - return groupKey(gaIndex); + return _secIfObj.groupKey(gaIndex); } else { - uint16_t iaIndex = indAddressIndex(addr); + uint16_t iaIndex = _secIfObj.indAddressIndex(addr); if (iaIndex > 0) - return p2pKey(iaIndex); + return _secIfObj.p2pKey(iaIndex); } return nullptr; @@ -580,7 +522,7 @@ const uint8_t* SecureApplicationLayer::securityKey(uint16_t addr, bool isGroupAd // returns next outgoing sequence number for secure communication uint64_t SecureApplicationLayer::nextSequenceNumber(bool toolAccess) { - return toolAccess ? sequenceNumberToolAccess : sequenceNumber; + return toolAccess ? _sequenceNumberToolAccess : _sequenceNumber; } // stores next outgoing sequence number for secure communication @@ -588,40 +530,29 @@ void SecureApplicationLayer::updateSequenceNumber(bool toolAccess, uint64_t seqN { if (toolAccess) { - sequenceNumberToolAccess = seqNum; - //TODO: securityInterface.set(Pid.ToolSequenceNumberSending, sixBytes(seqNo).array()); + _sequenceNumberToolAccess = seqNum; } else { - sequenceNumber = seqNum; - //TODO: securityInterface.set(Pid.SequenceNumberSending, sixBytes(seqNo).array()); + _sequenceNumber = seqNum; } + + // Also update the properties accordingly + _secIfObj.setSequenceNumber(toolAccess, seqNum); } -uint64_t SecureApplicationLayer::lastValidSequenceNumber(bool toolAcces, uint16_t srcAddr) +uint64_t SecureApplicationLayer::lastValidSequenceNumber(bool toolAccess, uint16_t srcAddr) { - if (toolAcces) + if (toolAccess) { - // TODO: add map to handle multiplpe lastValidSequenceNumberTool for each srcAddr - // lastValidSequence.getOrDefault(remote, 0L); + // TODO: check if we really have to support multiple tools at the same time if (srcAddr == _deviceObj.induvidualAddress()) - return sequenceNumberToolAccess; - return lastValidSequenceNumberTool; + return _sequenceNumberToolAccess; + return _lastValidSequenceNumberTool; } else { -/* - * TODO: - byte[] addresses = securityInterface.get(Pid.SecurityIndividualAddressTable); - var addr = remote.toByteArray(); - // precondition: array size is multiple of entrySize - int entrySize = 2 + 6; // Address and SeqNum - for (int offset = 0; offset < addresses.length; offset += entrySize) - { - if (Arrays.equals(addr, 0, addr.length, addresses, offset, offset + 2)) - return unsigned(Arrays.copyOfRange(addresses, offset + 2, offset + 2 + 6)); - } -*/ + return _secIfObj.getLastValidSequenceNumber(srcAddr); } return 0; @@ -630,27 +561,11 @@ uint64_t SecureApplicationLayer::lastValidSequenceNumber(bool toolAcces, uint16_ void SecureApplicationLayer::updateLastValidSequence(bool toolAccess, uint16_t remoteAddr, uint64_t seqNo) { if (toolAccess) - // TODO: add map to handle multiple lastValidSequenceNumberTool for each srcAddr - //lastValidSequenceToolAccess.put(remoteAddr, seqNo); - lastValidSequenceNumberTool = seqNo; + // TODO: check if we really have to support multiple tools at the same time + _lastValidSequenceNumberTool = seqNo; else { -/* - * TODO: - byte[] addresses = securityInterface.get(Pid.SecurityIndividualAddressTable); - var addr = remote.toByteArray(); - - int entrySize = addr.length + 6; // Address + SeqNum - // precondition: array size is multiple of entrySize - for (int offset = 0; offset < addresses.length; offset += entrySize) { - if (Arrays.equals(addr, 0, addr.length, addresses, offset, offset + 2)) { - final var start = 1 + offset / entrySize; - final var data = ByteBuffer.allocate(8).put(addr).put(sixBytes(seqNo)); - securityInterface.set(Pid.SecurityIndividualAddressTable, start, 1, data.array()); - break; - } - } -*/ + _secIfObj.setLastValidSequenceNumber(remoteAddr, seqNo); } } @@ -684,7 +599,7 @@ void SecureApplicationLayer::sendSyncRequest(uint16_t dstAddr, bool dstAddrIsGro print("sendSyncRequest: TPCI: "); println(tpci, HEX); - if(secure(request.data() + APDU_LPDU_DIFF, SecureSyncRequest, _deviceObj.induvidualAddress(), dstAddr, dstAddrIsGroupAddr, tpci, asdu, sizeof(asdu), secCtrl)) + if(secure(request.data() + APDU_LPDU_DIFF, kSecureSyncRequest, _deviceObj.induvidualAddress(), dstAddr, dstAddrIsGroupAddr, tpci, asdu, sizeof(asdu), secCtrl)) { println("SyncRequest: "); request.apdu().printPDU(); @@ -744,7 +659,7 @@ void SecureApplicationLayer::sendSyncResponse(uint16_t dstAddr, bool dstAddrIsGr print("sendSyncResponse: TPCI: "); println(tpci, HEX); - if(secure(response.data() + APDU_LPDU_DIFF, SecureSyncResponse, _deviceObj.induvidualAddress(), dstAddr, dstAddrIsGroupAddr, tpci, asdu, sizeof(asdu), secCtrl)) + if(secure(response.data() + APDU_LPDU_DIFF, kSecureSyncResponse, _deviceObj.induvidualAddress(), dstAddr, dstAddrIsGroupAddr, tpci, asdu, sizeof(asdu), secCtrl)) { _lastSyncRes = millis(); @@ -857,11 +772,11 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt secCtrl.toolAccess = toolAccess; secCtrl.dataSecurity = authOnly ? DataSecurity::auth : DataSecurity::authConf; - bool syncReq = service == SecureSyncRequest; - bool syncRes = service == SecureSyncResponse; + bool syncReq = service == kSecureSyncRequest; + bool syncRes = service == kSecureSyncResponse; //const uint8_t* key = dstAddrIsGroupAddr ? securityKey(dstAddr, dstAddrIsGroupAddr) : toolAccess ? toolKey(srcAddr == _deviceObj.induvidualAddress() ? dstAddr : srcAddr) : securityKey(srcAddr, false); - const uint8_t* key = dstAddrIsGroupAddr && (dstAddr != 0) ? securityKey(dstAddr, dstAddrIsGroupAddr) : toolAccess ? toolKey(srcAddr == _deviceObj.induvidualAddress() ? dstAddr : srcAddr) : securityKey(srcAddr, false); + const uint8_t* key = dstAddrIsGroupAddr && (dstAddr != 0) ? securityKey(dstAddr, dstAddrIsGroupAddr) : toolAccess ? _secIfObj.toolKey(srcAddr == _deviceObj.induvidualAddress() ? dstAddr : srcAddr) : securityKey(srcAddr, false); if (key == nullptr) { print("Error: No key found. toolAccess: "); @@ -879,7 +794,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLengt uint16_t remainingPlainApduLength = plainApduLength; - if (service == SecureDataPdu) + if (service == kSecureDataPdu) { if (srcAddr != _deviceObj.induvidualAddress()) { @@ -1125,7 +1040,7 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t } } - const uint8_t* key = toolAccess ? toolKey(_syncReqBroadcastIncoming ? _deviceObj.induvidualAddress() : dstAddr) : securityKey(dstAddr, dstAddrIsGroupAddr); + const uint8_t* key = toolAccess ? _secIfObj.toolKey(_syncReqBroadcastIncoming ? _deviceObj.induvidualAddress() : dstAddr) : securityKey(dstAddr, dstAddrIsGroupAddr); if (key == nullptr) { print("Error: No key found. toolAccess: "); @@ -1133,8 +1048,8 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t return false; } - bool syncReq = service == SecureSyncRequest; - bool syncRes = service == SecureSyncResponse; + bool syncReq = service == kSecureSyncRequest; + bool syncRes = service == kSecureSyncResponse; tpci |= SecureService >> 8; // OR'ing upper two APCI bits uint8_t apci = SecureService & 0x00FF; @@ -1290,7 +1205,7 @@ 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, SecureDataPdu, srcAddress, dstAddress, isDstAddrGroupAddr, tpci, plainApdu.frame().data()+APDU_LPDU_DIFF, plainApdu.length()+1, secCtrl)) + if(secure(secureApdu.frame().data()+APDU_LPDU_DIFF, kSecureDataPdu, srcAddress, dstAddress, isDstAddrGroupAddr, tpci, plainApdu.frame().data()+APDU_LPDU_DIFF, plainApdu.length()+1, secCtrl)) { print("Update our next "); print(secCtrl.toolAccess ? "tool access" : ""); @@ -1353,7 +1268,7 @@ bool SecureApplicationLayer::isSyncService(APDU& secureApdu) 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 == SecureSyncRequest) || (service == SecureSyncResponse)) + if ((service == kSecureSyncRequest) || (service == kSecureSyncResponse)) { return true; } diff --git a/src/knx/secure_application_layer.h b/src/knx/secure_application_layer.h index faeb158..2e7270e 100644 --- a/src/knx/secure_application_layer.h +++ b/src/knx/secure_application_layer.h @@ -9,6 +9,7 @@ class DeviceObject; class SecurityInterfaceObject; class AssociationTableObject; +class AddressTableObject; class BusAccessUnit; /** * This is an implementation of the application layer as specified in @cite knx:3/5/1. @@ -26,7 +27,7 @@ class SecureApplicationLayer : public ApplicationLayer * @param assocTable The AssociationTable is used to translate between asap (i.e. group objects) and group addresses. * @param bau methods are called here depending of the content of the APDU */ - SecureApplicationLayer(DeviceObject& deviceObj, SecurityInterfaceObject& secIfObj, AssociationTableObject& assocTable, BusAccessUnit& bau); + SecureApplicationLayer(DeviceObject& deviceObj, SecurityInterfaceObject& secIfObj, AssociationTableObject& assocTable, AddressTableObject& addrTab, BusAccessUnit& bau); void setSecurityMode(bool enabled); bool isSecurityModeEnabled(); @@ -225,17 +226,10 @@ class SecureApplicationLayer : public ApplicationLayer void block0(uint8_t* buffer, uint8_t* seqNum, uint16_t indSrcAddr, uint16_t dstAddr, bool dstAddrIsGroupAddr, uint8_t extFrameFormat, uint8_t tpci, uint8_t apci, uint8_t payloadLength); void blockCtr0(uint8_t* buffer, uint8_t* seqNum, uint16_t indSrcAddr, uint16_t dstAddr); - void sixBytesFromUInt64(uint64_t num, uint8_t* toByteArray); - uint64_t sixBytesToUInt64(uint8_t* data); - - const uint8_t *toolKey(uint16_t devAddr); const uint8_t* securityKey(uint16_t addr, bool isGroupAddress); - uint16_t indAddressIndex(uint16_t indAddr); // returns 1-based index of address in security IA table uint16_t groupAddressIndex(uint16_t groupAddr); // returns 1-based index of address in group address table uint16_t groupObjectIndex(uint16_t groupAddrIndex); // returns 1-based index of object in association table - const uint8_t* p2pKey(uint16_t addressIndex); // returns p2p key for IA index - const uint8_t* groupKey(uint16_t addressIndex); // returns group key for group address index uint8_t groupObjectSecurity(uint16_t groupObjectIndex); @@ -272,6 +266,13 @@ class SecureApplicationLayer : public ApplicationLayer Map _pendingOutgoingSyncRequests; // Store challenges for outgoing sync requests Map _pendingIncomingSyncRequests; // Store challenges for incoming sync requests + uint64_t _sequenceNumberToolAccess = 50; + uint64_t _sequenceNumber = 0; + + uint64_t _lastValidSequenceNumberTool = 0; + uint64_t _lastValidSequenceNumber = 0; + SecurityInterfaceObject& _secIfObj; DeviceObject& _deviceObj; + AddressTableObject& _addrTab; }; diff --git a/src/knx/security_interface_object.cpp b/src/knx/security_interface_object.cpp index 43ed63a..c985ff2 100644 --- a/src/knx/security_interface_object.cpp +++ b/src/knx/security_interface_object.cpp @@ -314,5 +314,182 @@ void SecurityInterfaceObject::factoryReset() property(PID_TOOL_KEY)->write(1, 1, _fdsk); } +const uint8_t* SecurityInterfaceObject::toolKey(uint16_t devAddr) +{ + //TODO: check if multiple tool keys possible, leave it for now + const uint8_t* toolKey = propertyData(PID_TOOL_KEY); + return toolKey; +} + +const uint8_t* SecurityInterfaceObject::p2pKey(uint16_t addressIndex) +{ + if (!isLoaded()) + return nullptr; + + // Get number of entries for this property + uint16_t numElements = getNumberOfElements(PID_P2P_KEY_TABLE); + + if (numElements > 0) + { + uint8_t elementSize = propertySize(PID_P2P_KEY_TABLE); + + // Search for address index + uint8_t entry[elementSize]; // 2 bytes index + keysize (16 bytes) + 2 bytes(for what?) = 20 bytes + for (int i = 1; i <= numElements; i++) + { + property(PID_P2P_KEY_TABLE)->read(i, 1, entry); + uint16_t index = (entry[0] << 8) | entry[1]; + if (index > addressIndex) + { + return nullptr; + } + if (index == addressIndex) + { + return propertyData(PID_P2P_KEY_TABLE, i) + sizeof(index); + } + } + } + + return nullptr; +} + +const uint8_t* SecurityInterfaceObject::groupKey(uint16_t addressIndex) +{ + if (!isLoaded()) + return nullptr; + + // Get number of entries for this property + uint16_t numElements = getNumberOfElements(PID_GRP_KEY_TABLE); + + if (numElements > 0) + { + uint8_t elementSize = propertySize(PID_GRP_KEY_TABLE); + + // Search for address index + uint8_t entry[elementSize]; // 2 bytes index + keysize (16 bytes) = 18 bytes + for (int i = 1; i <= numElements; i++) + { + property(PID_P2P_KEY_TABLE)->read(i, 1, entry); + uint16_t index = (entry[0] << 8) | entry[1]; + if (index > addressIndex) + { + return nullptr; + } + if (index == addressIndex) + { + return propertyData(PID_GRP_KEY_TABLE, i) + sizeof(index); + } + } + } + + return nullptr; +} + +uint16_t SecurityInterfaceObject::indAddressIndex(uint16_t indAddr) +{ + // Get number of entries for this property + uint16_t numElements = getNumberOfElements(PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE); + + if (numElements > 0) + { + uint8_t elementSize = propertySize(PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE); + + // Search for individual address + uint8_t entry[elementSize]; // 2 bytes address + 6 bytes seqno = 8 bytes + for (int i = 1; i <= numElements; i++) + { + property(PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE)->read(i, 1, entry); + uint16_t addr = (entry[0] << 8) | entry[1]; + if (addr == indAddr) + { + return i; + } + } + } + + // Not found + return 0; +} + +void SecurityInterfaceObject::setSequenceNumber(bool toolAccess, uint64_t seqNum) +{ + uint8_t seqBytes[6] = {0x00}; + sixBytesFromUInt64(seqNum, seqBytes); + + if (toolAccess) + { + property(PID_TOOL_SEQUENCE_NUMBER_SENDING)->write(1, 1, seqBytes); + } + else + { + property(PID_SEQUENCE_NUMBER_SENDING)->write(1, 1, seqBytes); + } +} + +uint16_t SecurityInterfaceObject::getNumberOfElements(PropertyID propId) +{ + // Get number of entries for this property + uint16_t numElements = 0; + + uint8_t data[sizeof(uint16_t)]; // is sizeof(_currentElements) which is uint16_t + uint8_t count = property(propId)->read(0, 1, data); + + if (count > 0) + { + popWord(numElements, data); + } + + return numElements; +} + +uint64_t SecurityInterfaceObject::getLastValidSequenceNumber(uint16_t deviceAddr) +{ + + // Get number of entries for this property + uint16_t numElements = getNumberOfElements(PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE); + + if (numElements > 0) + { + uint8_t elementSize = propertySize(PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE); + + // Search for individual address + uint8_t entry[elementSize]; // 2 bytes address + 6 bytes seqno = 8 bytes + for (int i = 1; i <= numElements; i++) + { + property(PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE)->read(i, 1, entry); + uint16_t addr = (entry[0] << 8) | entry[1]; + if (addr == deviceAddr) + { + return sixBytesToUInt64(&entry[2]); + } + } + } + return 0; +} + +void SecurityInterfaceObject::setLastValidSequenceNumber(uint16_t deviceAddr, uint64_t seqNum) +{ + // Get number of entries for this property + uint16_t numElements = getNumberOfElements(PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE); + + if (numElements > 0) + { + uint8_t elementSize = propertySize(PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE); + + // Search for individual address + uint8_t entry[elementSize]; // 2 bytes address + 6 bytes seqno = 8 bytes + for (int i = 1; i <= numElements; i++) + { + property(PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE)->read(i, 1, entry); + uint16_t addr = (entry[0] << 8) | entry[1]; + if (addr == deviceAddr) + { + sixBytesFromUInt64(seqNum, &entry[2]); + property(PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE)->write(i, 1, entry); + } + } + } +} + #endif diff --git a/src/knx/security_interface_object.h b/src/knx/security_interface_object.h index e1f565a..e56f519 100644 --- a/src/knx/security_interface_object.h +++ b/src/knx/security_interface_object.h @@ -22,6 +22,16 @@ public: bool isLoaded(); + const uint8_t* toolKey(uint16_t devAddr); // returns single tool key (ETS) + const uint8_t* p2pKey(uint16_t addressIndex); // returns p2p key for IA index + const uint8_t* groupKey(uint16_t addressIndex); // returns group key for group address index + + uint16_t indAddressIndex(uint16_t indAddr); // returns 1-based index of address in security IA table + + void setSequenceNumber(bool toolAccess, uint64_t seqNum); + uint64_t getLastValidSequenceNumber(uint16_t deviceAddr); + void setLastValidSequenceNumber(uint16_t deviceAddr, uint64_t seqNum); + private: SecureApplicationLayer* _secAppLayer = nullptr; @@ -36,6 +46,8 @@ private: void loadState(LoadState newState); LoadState _state = LS_UNLOADED; + uint16_t getNumberOfElements(PropertyID propId); + // Our FDSK static const uint8_t _fdsk[]; static uint8_t _secReport[];