save work

src/knx/application_layer.h

@@ -71,12 +71,12 @@ class ApplicationLayer
     virtual void dataSystemBroadcastConfirm(HopCountType hopType, Priority priority, APDU& apdu, bool status);
     virtual void dataIndividualIndication(HopCountType hopType, Priority priority, uint16_t source, APDU& apdu);
     virtual void dataIndividualConfirm(AckType ack, HopCountType hopType, Priority priority, uint16_t tsap, APDU& apdu, bool status);
-    virtual void connectIndication(uint16_t tsap);
-    virtual void connectConfirm(uint16_t destination, uint16_t tsap, bool status);
-    virtual void disconnectIndication(uint16_t tsap);
-    virtual void disconnectConfirm(Priority priority, uint16_t tsap, bool status);
     virtual void dataConnectedIndication(Priority priority, uint16_t tsap, APDU& apdu);
     virtual void dataConnectedConfirm(uint16_t tsap);
+    void connectIndication(uint16_t tsap);
+    void connectConfirm(uint16_t destination, uint16_t tsap, bool status);
+    void disconnectIndication(uint16_t tsap);
+    void disconnectConfirm(Priority priority, uint16_t tsap, bool status);
 #pragma endregion
 
 #pragma region from bau

src/knx/secure_application_layer.cpp

@@ -41,14 +41,19 @@ void SecureApplicationLayer::dataGroupIndication(HopCountType hopType, Priority
     if (apdu.type() == SecureService)
     {
         // Decrypt secure APDU
+        // Somehow ugly that we need to know the size in advance here at this point
+        uint16_t plainApduLength = apdu.length() - 1 - 6 - 4; // secureAdsuLength - sizeof(scf) - sizeof(seqNum) - sizeof(mac)
+        CemiFrame plainFrame(plainApduLength);
+        // Decrypt secure APDU
+        if (decryptSecureApdu(apdu, plainFrame.apdu()))
+        {
+            // Process decrypted inner APDU
+            ApplicationLayer::dataGroupIndication(hopType, priority, tsap, plainFrame.apdu());
+        }
+        return;
+    }
 
-        // Process decrypted inner APDU
+    ApplicationLayer::dataGroupIndication(hopType, priority, tsap, apdu);
-        ApplicationLayer::dataGroupIndication(hopType, priority, tsap, apdu);
-    }
-    else
-    {
-        ApplicationLayer::dataGroupIndication(hopType, priority, tsap, apdu);
-    }
 }
 
 void SecureApplicationLayer::dataGroupConfirm(AckType ack, HopCountType hopType, Priority priority,  uint16_t tsap, APDU& apdu, bool status)
@@ -56,31 +61,58 @@ void SecureApplicationLayer::dataGroupConfirm(AckType ack, HopCountType hopType,
     if (apdu.type() == SecureService)
     {
         // Decrypt secure APDU
+        // Somehow ugly that we need to know the size in advance here at this point
+        uint16_t plainApduLength = apdu.length() - 1 - 6 - 4; // secureAdsuLength - sizeof(scf) - sizeof(seqNum) - sizeof(mac)
+        CemiFrame plainFrame(plainApduLength);
+        // Decrypt secure APDU
+        if (decryptSecureApdu(apdu, plainFrame.apdu()))
+        {
+            // Process decrypted inner APDU
+            ApplicationLayer::dataGroupConfirm(ack, hopType, priority, tsap, plainFrame.apdu(), status);
+        }
+        return;
+    }
 
-        // Process decrypted inner APDU
+    ApplicationLayer::dataGroupConfirm(ack, hopType, priority, tsap, apdu, status);
-        ApplicationLayer::dataGroupConfirm(ack, hopType, priority, tsap, apdu, status);
-    }
-    else
-    {
-        ApplicationLayer::dataGroupConfirm(ack, hopType, priority, tsap, apdu, status);
-    }
 }
 
 void SecureApplicationLayer::dataBroadcastIndication(HopCountType hopType, Priority priority, uint16_t source, APDU& apdu)
 {
     if (apdu.type() == SecureService)
     {
-        // Secure APDU is not allowed in Broadcast
+        // Decrypt secure APDU
-        println("Secure APDU in Broadcast not allowed!");
+        // Somehow ugly that we need to know the size in advance here at this point
-    }
+        uint16_t plainApduLength = apdu.length() - 1 - 6 - 4; // secureAdsuLength - sizeof(scf) - sizeof(seqNum) - sizeof(mac)
-    else
+        CemiFrame plainFrame(plainApduLength);
-    {
+        // Decrypt secure APDU
-        ApplicationLayer::dataBroadcastIndication(hopType, priority, source, apdu);
+        if (decryptSecureApdu(apdu, plainFrame.apdu()))
+        {
+            // Process decrypted inner APDU
+            ApplicationLayer::dataBroadcastIndication(hopType, priority, source, plainFrame.apdu());
+        }
+        return;
     }
+
+    ApplicationLayer::dataBroadcastIndication(hopType, priority, source, apdu);
 }
 
 void SecureApplicationLayer::dataBroadcastConfirm(AckType ack, HopCountType hopType, Priority priority, APDU& apdu, bool status)
 {
+    if (apdu.type() == SecureService)
+    {
+        // Decrypt secure APDU
+        // Somehow ugly that we need to know the size in advance here at this point
+        uint16_t plainApduLength = apdu.length() - 1 - 6 - 4; // secureAdsuLength - sizeof(scf) - sizeof(seqNum) - sizeof(mac)
+        CemiFrame plainFrame(plainApduLength);
+        // Decrypt secure APDU
+        if (decryptSecureApdu(apdu, plainFrame.apdu()))
+        {
+            // Process decrypted inner APDU
+            ApplicationLayer::dataBroadcastConfirm(ack, hopType, priority, plainFrame.apdu(), status);
+        }
+        return;
+    }
+
     ApplicationLayer::dataBroadcastConfirm(ack, hopType, priority, apdu, status);
 }
 
@@ -88,17 +120,39 @@ void SecureApplicationLayer::dataSystemBroadcastIndication(HopCountType hopType,
 {
     if (apdu.type() == SecureService)
     {
-        // Secure APDU is not allowed in SystemBroadcast
+        // Decrypt secure APDU
-        println("Secure APDU in SystemBroadcast not allowed!");
+        // Somehow ugly that we need to know the size in advance here at this point
-    }
+        uint16_t plainApduLength = apdu.length() - 1 - 6 - 4; // secureAdsuLength - sizeof(scf) - sizeof(seqNum) - sizeof(mac)
-    else
+        CemiFrame plainFrame(plainApduLength);
-    {
+        // Decrypt secure APDU
-        ApplicationLayer::dataSystemBroadcastIndication(hopType, priority, source, apdu);
+        if (decryptSecureApdu(apdu, plainFrame.apdu()))
+        {
+            // Process decrypted inner APDU
+            ApplicationLayer::dataSystemBroadcastIndication(hopType, priority, source, plainFrame.apdu());
+        }
+        return;
     }
+
+    ApplicationLayer::dataSystemBroadcastIndication(hopType, priority, source, apdu);
 }
 
 void SecureApplicationLayer::dataSystemBroadcastConfirm(HopCountType hopType, Priority priority, APDU& apdu, bool status)
 {
+    if (apdu.type() == SecureService)
+    {
+        // Decrypt secure APDU
+        // Somehow ugly that we need to know the size in advance here at this point
+        uint16_t plainApduLength = apdu.length() - 1 - 6 - 4; // secureAdsuLength - sizeof(scf) - sizeof(seqNum) - sizeof(mac)
+        CemiFrame plainFrame(plainApduLength);
+        // Decrypt secure APDU
+        if (decryptSecureApdu(apdu, plainFrame.apdu()))
+        {
+            // Process decrypted inner APDU
+            ApplicationLayer::dataSystemBroadcastConfirm(hopType, priority, plainFrame.apdu(), status);
+        }
+        return;
+    }
+
     ApplicationLayer::dataSystemBroadcastConfirm(hopType, priority, apdu, status);
 }
 
@@ -107,14 +161,19 @@ void SecureApplicationLayer::dataIndividualIndication(HopCountType hopType, Prio
     if (apdu.type() == SecureService)
     {
         // Decrypt secure APDU
+        // Somehow ugly that we need to know the size in advance here at this point
+        uint16_t plainApduLength = apdu.length() - 1 - 6 - 4; // secureAdsuLength - sizeof(scf) - sizeof(seqNum) - sizeof(mac)
+        CemiFrame plainFrame(plainApduLength);
+        // Decrypt secure APDU
+        if (decryptSecureApdu(apdu, plainFrame.apdu()))
+        {
+            // Process decrypted inner APDU
+            ApplicationLayer::dataIndividualIndication(hopType, priority, tsap, plainFrame.apdu());
+        }
+        return;
+    }
 
-        // Process decrypted inner APDU
+    ApplicationLayer::dataIndividualIndication(hopType, priority, tsap, apdu);
-        ApplicationLayer::dataIndividualIndication(hopType, priority, tsap, apdu);
-    }
-    else
-    {
-        ApplicationLayer::dataIndividualIndication(hopType, priority, tsap, apdu);
-    }
 }
 
 void SecureApplicationLayer::dataIndividualConfirm(AckType ack, HopCountType hopType, Priority priority, uint16_t tsap, APDU& apdu, bool status)
@@ -132,68 +191,23 @@ void SecureApplicationLayer::dataIndividualConfirm(AckType ack, HopCountType hop
     }
 }
 
-void SecureApplicationLayer::connectIndication(uint16_t tsap)
-{
-    ApplicationLayer::connectIndication(tsap);
-}
-
-void SecureApplicationLayer::connectConfirm(uint16_t destination, uint16_t tsap, bool status)
-{
-    ApplicationLayer::connectConfirm(destination, tsap, status);
-}
-
-void SecureApplicationLayer::disconnectIndication(uint16_t tsap)
-{
-    ApplicationLayer::disconnectIndication(tsap);
-}
-
-void SecureApplicationLayer::disconnectConfirm(Priority priority, uint16_t tsap, bool status)
-{
-    ApplicationLayer::disconnectConfirm(priority, tsap, status);
-}
-
 void SecureApplicationLayer::dataConnectedIndication(Priority priority, uint16_t tsap, APDU& apdu)
 {
     if (apdu.type() == SecureService)
     {
-        // Decrypt secure APDU
-
-        println("Secure APDU: ");
-        apdu.printPDU();
-
         // Somehow ugly that we need to know the size in advance here at this point
-        // Same length calculation is also in the decrypt() function
         uint16_t plainApduLength = apdu.length() - 1 - 6 - 4; // secureAdsuLength - sizeof(scf) - sizeof(seqNum) - sizeof(mac)
         CemiFrame plainFrame(plainApduLength);
-
+        // Decrypt secure APDU
-        uint16_t srcAddress = apdu.frame().sourceAddress();
+        if (decryptSecureApdu(apdu, plainFrame.apdu()))
-        uint16_t dstAddress = apdu.frame().destinationAddress();
-        uint8_t tpci = apdu.frame().data()[TPDU_LPDU_DIFF]; // FIXME: when cEMI class is refactored, there might be additional info fields in cEMI [fixed TPDU_LPDU_DIFF]
-        print("Secure Debug: TPCI: ");
-        println(tpci, HEX);
-        // Note:
-        // The TPCI is also included in the MAC calculation to provide authenticity for this field.
-        // However, a secure APDU (with a MAC) is only included in transport layer PDUs T_DATA_GROUP, T_DATA_TAG_GROUP, T_DATA_INDIVIDUAL, T_DATA_CONNECTED
-        // and not in T_CONNECT, T_DISCONNECT, T_ACK, T_NACK.
-        // This means the DATA/CONTROL flag is always 0(=DATA). The flag "NUMBERED" differentiates between T_DATA_INDIVIDUAL and T_DATA_CONNECTED.
-        // The seqNumber is only in T_DATA_CONNECTED and 0 in case of T_DATA_GROUP and T_DATA_GROUP (leaving out T_DATA_TAG_GROUP).
-        // Summary: effectively only the "NUMBERED" flag (bit6) and the SeqNumber (bit5-2) are used from transport layer.
-        //          In T_DATA_* services the bits1-0 of TPCI octet are used as bits9-8 for APCI type which is fixed to 0x03. SecureService APCI is 0x03F1.
-
-        // FIXME: when cEMI class is refactored, there might be additional info fields in cEMI (fixed APDU_LPDU_DIFF)
-        if (decrypt(plainFrame.data()+APDU_LPDU_DIFF, srcAddress, dstAddress, false, tpci, apdu.data(), apdu.length()))
         {
-            println("Plain APDU: ");
-            plainFrame.apdu().printPDU();
-
             // Process decrypted inner APDU
             ApplicationLayer::dataConnectedIndication(priority, tsap, plainFrame.apdu());
         }
+        return;
     }
-    else
+
-    {
+    ApplicationLayer::dataConnectedIndication(priority, tsap, apdu);
-        ApplicationLayer::dataConnectedIndication(priority, tsap, apdu);
-    }
 }
 
 void SecureApplicationLayer::dataConnectedConfirm(uint16_t tsap)
@@ -401,31 +415,69 @@ uint64_t SecureApplicationLayer::nextSequenceNumber(bool toolAccess)
 void SecureApplicationLayer::updateSequenceNumber(bool toolAccess, uint64_t seqNum)
 {
     if (toolAccess)
+    {
         sequenceNumberToolAccess = seqNum;
+        //TODO: securityInterface.set(Pid.ToolSequenceNumberSending, sixBytes(seqNo).array());
+    }
     else
+    {
         sequenceNumber = seqNum;
+        //TODO: securityInterface.set(Pid.SequenceNumberSending, sixBytes(seqNo).array());
+    }
 }
 
 uint64_t SecureApplicationLayer::lastValidSequenceNumber(bool toolAcces, uint16_t srcAddr)
 {
     if (toolAcces)
     {
+        // TODO: add map to handle multiplpe lastValidSequenceNumberTool for each srcAddr
+        // lastValidSequence.getOrDefault(remote, 0L);
         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));
+        }
+*/
+    }
 
-    // TODO
     return 0;
 }
 
 void SecureApplicationLayer::updateLastValidSequence(bool toolAccess, uint16_t remoteAddr, uint64_t seqNo)
 {
     if (toolAccess)
-        // TODO
+        // TODO: add map to handle multiplpe lastValidSequenceNumberTool for each srcAddr
         //lastValidSequenceToolAccess.put(remoteAddr, seqNo);
         lastValidSequenceNumberTool = seqNo;
-    //else
+    else
-        // TODO
+    {
-        //lastValidSequence.put(remoteAddr, seqNo);
+/*
+ * 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;
+            }
+        }
+*/
+    }
 }
 
 void SecureApplicationLayer::sendSyncResponse(uint16_t dstAddr, bool dstAddrIsGroupAddr, bool toolAccess, uint64_t remoteNextSeqNum)
@@ -475,6 +527,11 @@ void SecureApplicationLayer::receivedSyncResponse(uint16_t remoteAddr, bool tool
 //    final var request = pendingSyncRequests.get(remote);
 //    if (request == null)
 //        return;
+    if (_challengeSrcAddr != remoteAddr)
+    {
+        println("receivedSyncResponse(): Did not find matching challenge for remoteAddr!");
+        return;
+    }
 
     // Bytes 0-5 in the "APDU" buffer contain the remote sequence number
     // Bytes 6-11 in the "APDU" buffer contain the local sequence number
@@ -497,10 +554,8 @@ void SecureApplicationLayer::receivedSyncResponse(uint16_t remoteAddr, bool tool
     //syncRequestCompleted(request);
 }
 
-bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t srcAddr, uint16_t dstAddr, bool dstAddrIsGroupAddr, uint8_t tpci, uint8_t* secureAsdu, uint16_t secureAdsuLength)
+bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t plainApduLength, uint16_t srcAddr, uint16_t dstAddr, bool dstAddrIsGroupAddr, uint8_t tpci, uint8_t* secureAsdu)
 {
-    uint8_t extendedFrameFormat = 0;
-
     const uint8_t* pBuf;
     uint8_t scf;
 
@@ -577,13 +632,15 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t srcAddr, uint1
         }
     }
 
-    uint16_t apduLength = secureAdsuLength - 1 - 6 - 4; // secureAdsuLength - sizeof(scf) - sizeof(seqNum) - sizeof(mac)
+    pBuf = popByteArray(plainApdu, plainApduLength, pBuf);
-    pBuf = popByteArray(plainApdu, apduLength, pBuf);
 
+    // No LTE-HEE for now
+    // Data Secure always uses extended frame format with APDU length > 15 octets (long messages), no standard frames
+    uint8_t extendedFrameFormat = 0;
     // Clear IV buffer
     uint8_t iv[16] = {0x00};
     // Create first block B0 for AES CBC MAC calculation, used as IV later
-    block0(iv, seqNum, srcAddr, dstAddr, dstAddrIsGroupAddr, extendedFrameFormat, tpci | (SecureService >> 8), SecureService & 0x00FF, apduLength);
+    block0(iv, seqNum, srcAddr, dstAddr, dstAddrIsGroupAddr, extendedFrameFormat, tpci | (SecureService >> 8), SecureService & 0x00FF, plainApduLength);
 
     // Clear block counter0 buffer
     uint8_t ctr0[16] = {0x00};
@@ -598,7 +655,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t srcAddr, uint1
         // APDU is already plain, no decryption needed
 
         // Only check the MAC
-        uint32_t calculatedMac = calcAuthOnlyMac(plainApdu, apduLength, key, iv, ctr0);
+        uint32_t calculatedMac = calcAuthOnlyMac(plainApdu, plainApduLength, key, iv, ctr0);
         if (calculatedMac != mac)
         {
             // security failure
@@ -610,13 +667,13 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t srcAddr, uint1
             return false;
         }
 
-        memcpy(plainApdu, secureAsdu, apduLength);
+        memcpy(plainApdu, secureAsdu, plainApduLength);
     }
     else
     {
         // APDU is encrypted and needs decryption
 
-        uint16_t bufLen = 4 + apduLength;
+        uint16_t bufLen = 4 + plainApduLength;
         // AES-128 operates on blocks of 16 bytes, add padding
         //uint16_t bufLenPadded = (bufLen + 15) / 16 * 16;
         //uint8_t buffer[bufLenPadded];
@@ -625,7 +682,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t srcAddr, uint1
         //memset(buffer, 0x00, bufLenPadded);
 
         pushInt(mac, &buffer[0]);
-        pushByteArray(plainApdu, apduLength, &buffer[4]); // apdu is still encrypted
+        pushByteArray(plainApdu, plainApduLength, &buffer[4]); // apdu is still encrypted
 
         struct AES_ctx ctx;
         AES_init_ctx_iv(&ctx, key, ctr0);
@@ -634,7 +691,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t srcAddr, uint1
 
         uint32_t decryptedMac;
         popInt(decryptedMac, &buffer[0]);
-        popByteArray(plainApdu, apduLength, &buffer[4]); // apdu is now decrypted (overwritten)
+        popByteArray(plainApdu, plainApduLength, &buffer[4]); // apdu is now decrypted (overwritten)
 
         // Do calculations for Auth+Conf
         uint8_t associatedData[syncReq ? 7 : 1];
@@ -643,7 +700,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t srcAddr, uint1
         {
             memcpy(&associatedData[1], knxSerialNumber, 6);
         }
-        uint32_t calculatedMac = calcConfAuthMac(associatedData, sizeof(associatedData), plainApdu, apduLength, key, iv);
+        uint32_t calculatedMac = calcConfAuthMac(associatedData, sizeof(associatedData), plainApdu, plainApduLength, key, iv);
         if (calculatedMac != decryptedMac)
         {
             // security failure
@@ -681,6 +738,7 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t srcAddr, uint1
                 updateLastValidSequence(toolAccess, srcAddr, receivedSeqNumber);
             }
         }
+        // In case it was a sync.req or a sync.res we not have anything for the application layer to send to
         if (syncReq || syncRes)
             return false;
     }
@@ -688,6 +746,40 @@ bool SecureApplicationLayer::decrypt(uint8_t* plainApdu, uint16_t srcAddr, uint1
     return true;
 }
 
+bool SecureApplicationLayer::decryptSecureApdu(APDU& secureApdu, APDU& plainApdu)
+{
+    // Decrypt secure APDU
+
+    println("Secure APDU: ");
+    secureApdu.printPDU();
+
+    uint16_t srcAddress = secureApdu.frame().sourceAddress();
+    uint16_t dstAddress = secureApdu.frame().destinationAddress();
+    bool isDstAddrGroupAddr = secureApdu.frame().addressType() == GroupAddress;
+    uint8_t tpci = secureApdu.frame().data()[TPDU_LPDU_DIFF]; // FIXME: when cEMI class is refactored, there might be additional info fields in cEMI [fixed TPDU_LPDU_DIFF]
+    print("Secure Debug: TPCI: ");
+    println(tpci, HEX);
+    // Note:
+    // The TPCI is also included in the MAC calculation to provide authenticity for this field.
+    // However, a secure APDU (with a MAC) is only included in transport layer PDUs T_DATA_GROUP, T_DATA_TAG_GROUP, T_DATA_INDIVIDUAL, T_DATA_CONNECTED
+    // and not in T_CONNECT, T_DISCONNECT, T_ACK, T_NACK.
+    // This means the DATA/CONTROL flag is always 0(=DATA). The flag "NUMBERED" differentiates between T_DATA_INDIVIDUAL and T_DATA_CONNECTED.
+    // The seqNumber is only used in T_DATA_CONNECTED and 0 in case of T_DATA_GROUP and T_DATA_GROUP (leaving out T_DATA_TAG_GROUP).
+    // Summary: effectively only the "NUMBERED" flag (bit6) and the SeqNumber (bit5-2) are used from transport layer.
+    //          In T_DATA_* services the bits1-0 of TPCI octet are used as bits9-8 for APCI type which is fixed to 0x03. SecureService APCI is 0x03F1.
+
+    // FIXME: when cEMI class is refactored, there might be additional info fields in cEMI (fixed APDU_LPDU_DIFF)
+    if (decrypt(plainApdu.frame().data()+APDU_LPDU_DIFF, plainApdu.length(), srcAddress, dstAddress, isDstAddrGroupAddr, tpci, secureApdu.data()))
+    {
+        println("Plain APDU: ");
+        plainApdu.frame().apdu().printPDU();
+
+        return true;
+    }
+
+    return false;
+}
+
 bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t srcAddr, uint16_t dstAddr, bool dstAddrIsGroupAddr,
                                     uint8_t* apdu, uint16_t apduLength, bool toolAccess, bool confidentiality)
 {
@@ -789,7 +881,8 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
         pBuf = pushByteArray(rndXorChallenge, 6, pBuf);
     }
 
-    // For now only 0
+    // No LTE-HEE for now
+    // Data Secure always uses extended frame format with APDU length > 15 octets (long messages), no standard frames
     uint8_t extendedFrameFormat = 0;
     // Clear IV buffer
     uint8_t iv[16] = {0x00};

src/knx/secure_application_layer.h

@@ -36,10 +36,6 @@ class SecureApplicationLayer :  public ApplicationLayer
     virtual void dataSystemBroadcastConfirm(HopCountType hopType, Priority priority, APDU& apdu, bool status) override;
     virtual void dataIndividualIndication(HopCountType hopType, Priority priority, uint16_t source, APDU& apdu) override;
     virtual void dataIndividualConfirm(AckType ack, HopCountType hopType, Priority priority, uint16_t tsap, APDU& apdu, bool status) override;
-    virtual void connectIndication(uint16_t tsap) override;
-    virtual void connectConfirm(uint16_t destination, uint16_t tsap, bool status) override;
-    virtual void disconnectIndication(uint16_t tsap) override;
-    virtual void disconnectConfirm(Priority priority, uint16_t tsap, bool status) override;
     virtual void dataConnectedIndication(Priority priority, uint16_t tsap, APDU& apdu) override;
     virtual void dataConnectedConfirm(uint16_t tsap) override;
 
@@ -82,9 +78,11 @@ class SecureApplicationLayer :  public ApplicationLayer
     void receivedSyncRequest(uint16_t srcAddr, uint16_t dstAddr, bool dstAddrIsGroupAddr, bool toolAccess, uint8_t* seq, long challenge);
     void receivedSyncResponse(uint16_t remoteAddr, bool toolAccess, uint8_t* plainApdu);
 
-    bool decrypt(uint8_t* plainApdu, uint16_t srcAddr, uint16_t dstAddr, bool dstAddrIsGroupAddr, uint8_t tpci, uint8_t* secureAsdu, uint16_t secureAdsuLength);
+    bool decrypt(uint8_t* plainApdu, uint16_t plainapduLength, uint16_t srcAddr, uint16_t dstAddr, bool dstAddrIsGroupAddr, uint8_t tpci, uint8_t* secureAsdu);
     bool secure(uint8_t* buffer, uint16_t service, uint16_t srcAddr, uint16_t dstAddr, bool dstAddrIsGroupAddr, uint8_t* apdu, uint16_t apduLength, bool toolAccess, bool confidentiality);
 
+    bool decryptSecureApdu(APDU& secureApdu, APDU &plainApdu);
+
     bool _syncReqBroadcast;
     uint32_t _lastSyncRes;
     uint8_t _challenge[6];

src/knx/security_interface_object.cpp

@@ -33,31 +33,107 @@ SecurityInterfaceObject::SecurityInterfaceObject()
         new FunctionProperty<SecurityInterfaceObject>(this, PID_SECURITY_MODE, ReadLv3 | WriteLv0,
             // Command Callback of PID_SECURITY_MODE
             [](SecurityInterfaceObject* obj, uint8_t* data, uint8_t length, uint8_t* resultData, uint8_t& resultLength) -> void {
-                // TODO
+                uint8_t serviceId = data[1] & 0xff;
+                resultLength = 1;
+                if (serviceId != 0)
+                {
+                    resultData[0] = 0xF2; // InvalidCommand
+                    return;
+                }
+                if (length == 3)
+                {
+                    uint8_t mode = data[2];
+                    if (mode > 1)
+                    {
+                        resultData[0] = 0xF8; // DataVoid
+                        return;
+                    }
+                    // TODO
+                    //setSecurityMode(mode == 1);
+                    resultData[0] = serviceId;
+                }
             },
             // State Callback of PID_SECURITY_MODE
             [](SecurityInterfaceObject* obj, uint8_t* data, uint8_t length, uint8_t* resultData, uint8_t& resultLength) -> void {
-                // TODO
+                uint8_t serviceId = data[1] & 0xff;
+                resultLength = 2;
+                if (serviceId != 0)
+                {
+                    resultData[0] = 0xF2; // InvalidCommand
+                    resultLength = 1;
+                    return;
+                }
+                if (length == 2)
+                {
+                    // TODO
+                    resultData[0] = serviceId;
+                    //resultData[1] = isSecurityModeEnabled() ? 1 : 0;
+                    resultLength = 2;
+                }
             }),
-        new DataProperty( PID_P2P_KEY_TABLE, true, PDT_GENERIC_20, 1, ReadLv3 | WriteLv0, (uint16_t)0 ), // TODO: value
+        new DataProperty( PID_P2P_KEY_TABLE, true, PDT_GENERIC_20, 1, ReadLv3 | WriteLv0 ), // written by ETS
-        new DataProperty( PID_GRP_KEY_TABLE, true, PDT_GENERIC_18, 1, ReadLv3 | WriteLv0, (uint16_t)0 ), // TODO: value
+        new DataProperty( PID_GRP_KEY_TABLE, true, PDT_GENERIC_18, 1, ReadLv3 | WriteLv0 ), // written by ETS
-        new DataProperty( PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE, true, PDT_GENERIC_08, 1, ReadLv3 | WriteLv0, (uint16_t)0 ), // TODO: value
+        new DataProperty( PID_SECURITY_INDIVIDUAL_ADDRESS_TABLE, true, PDT_GENERIC_08, 1, ReadLv3 | WriteLv0 ), // written by ETS
         new FunctionProperty<SecurityInterfaceObject>(this, PID_SECURITY_FAILURES_LOG, ReadLv3 | WriteLv0,
             // Command Callback of PID_SECURITY_FAILURES_LOG
             [](SecurityInterfaceObject* obj, uint8_t* data, uint8_t length, uint8_t* resultData, uint8_t& resultLength) -> void {
-                // TODO
+                if (length != 3)
+                {
+                    resultData[0] = 0xF8; // DataVoid
+                    resultLength = 1;
+                    return;
+                }
+                uint8_t id = data[1];
+                uint8_t info = data[2];
+                if (id == 0 && info == 0)
+                {
+                    //TODO: clearFailureLog();
+                    resultData[0] = id;
+                    resultLength = 1;
+                }
             },
             // State Callback of PID_SECURITY_FAILURES_LOG
             [](SecurityInterfaceObject* obj, uint8_t* data, uint8_t length, uint8_t* resultData, uint8_t& resultLength) -> void {
-                // TODO
+                if (length != 3)
+                {
+                    resultData[0] = 0xF8; // DataVoid
+                    resultLength = 1;
+                    return;
+                }
+                uint8_t id = data[1];
+                uint8_t info = data[2];
+
+                // failure counters
+                if (id == 0 && info == 0)
+                {
+                    //TODO:
+                    // var counters = ByteBuffer.allocate(10).put((byte) id).put((byte) info).put(failureCountersArray());
+                    // return new ServiceResult(counters.array());
+                }
+                // query latest failure by index
+                else if(id == 1)
+                {
+                    // TODO:
+                    //int index = info;
+                    //int i = 0;
+                    //for (var msgInfo : lastFailures) {
+                    //    if (i++ == index)
+                    //        return new ServiceResult(ByteBuffer.allocate(2 + msgInfo.length).put((byte) id)
+                    //                .put((byte) index).put(msgInfo).array());
+                    //}
+                    resultData[0] = 0xF8; // DataVoid
+                    resultData[1] = id;
+                    resultLength = 2;
+                    return;
+                }
             }),
         new DataProperty( PID_TOOL_KEY, true, PDT_GENERIC_16, 1, ReadLv3 | WriteLv0, (uint8_t*) _fdsk ), // default is FDSK
         new DataProperty( PID_SECURITY_REPORT, true, PDT_BITSET8, 1, ReadLv3 | WriteLv0, (uint16_t)0 ), // TODO: value
         new DataProperty( PID_SECURITY_REPORT_CONTROL, true, PDT_BINARY_INFORMATION, 1, ReadLv3 | WriteLv0, (uint16_t)0 ), // TODO: value
         new DataProperty( PID_SEQUENCE_NUMBER_SENDING, true, PDT_GENERIC_06, 1, ReadLv3 | WriteLv0, (uint16_t)0 ), // TODO: value
-        new DataProperty( PID_ZONE_KEY_TABLE, true, PDT_GENERIC_19, 1, ReadLv3 | WriteLv0, (uint16_t)0 ), // TODO: value
+        new DataProperty( PID_ZONE_KEY_TABLE, true, PDT_GENERIC_19, 1, ReadLv3 | WriteLv0 ), // written by ETS
-        new DataProperty( PID_GO_SECURITY_FLAGS, true, PDT_GENERIC_01, 1, ReadLv3 | WriteLv0, (uint16_t)0 ), // TODO: value
+        new DataProperty( PID_GO_SECURITY_FLAGS, true, PDT_GENERIC_01, 1, ReadLv3 | WriteLv0 ), // written by ETS
-        new DataProperty( PID_ROLE_TABLE, true, PDT_GENERIC_01, 1, ReadLv3 | WriteLv0, (uint16_t)0 ), // TODO: value
+        new DataProperty( PID_ROLE_TABLE, true, PDT_GENERIC_01, 1, ReadLv3 | WriteLv0 ), // written by ETS
     };
     initializeProperties(sizeof(properties), properties);
 }