save work

src/knx/secure_application_layer.cpp

@@ -245,12 +245,11 @@ void SecureApplicationLayer::dataGroupRequest(AckType ack, HopCountType hopType,
 {
     // TODO:
     // get flags for this TSAP from PID_GO_SECURITY_FLAGS from SecIntObj
-    bool needsEncryption = true;
+    bool needsEncryption = false;
 
     if (needsEncryption)
     {
-        //ByteBuffer secureApdu = ByteBuffer.allocate(3 + SeqSize + apdu.length + MacSize);
+        uint16_t secureApduLength = apdu.length() + 2 + 6 + 4; // 2(TPCI,APCI,SCF) + sizeof(seqNum) + apdu.length() + 4
-        uint16_t secureApduLength = apdu.length() + 3 + 6 + 4; // 3(TPCI,APCI,SCF) + sizeof(seqNum) + apdu.length() + 4
         CemiFrame secureFrame(secureApduLength);
         // create secure APDU
         if (createSecureApdu(apdu, secureFrame.apdu(), true, true)) // TODO: toolAccess, confidentialty
@@ -265,21 +264,85 @@ void SecureApplicationLayer::dataGroupRequest(AckType ack, HopCountType hopType,
 
 void SecureApplicationLayer::dataBroadcastRequest(AckType ack, HopCountType hopType, Priority priority, APDU& apdu)
 {
+    // TODO:
+    // get flags for this TSAP from PID_GO_SECURITY_FLAGS from SecIntObj
+    bool needsEncryption = true;
+
+    if (needsEncryption)
+    {
+        uint16_t secureApduLength = apdu.length() + 2 + 6 + 4; // 2(TPCI,APCI,SCF) + sizeof(seqNum) + apdu.length() + 4
+        CemiFrame secureFrame(secureApduLength);
+        // create secure APDU
+        if (createSecureApdu(apdu, secureFrame.apdu(), true, true)) // TODO: toolAccess, confidentialty
+        {
+            ApplicationLayer::dataBroadcastRequest(ack, hopType, SystemPriority, secureFrame.apdu());
+        }
+        return;
+    }
+
     ApplicationLayer::dataBroadcastRequest(ack, hopType, SystemPriority, apdu);
 }
 
 void SecureApplicationLayer::dataSystemBroadcastRequest(AckType ack, HopCountType hopType, Priority priority, APDU& apdu)
 {
+    // TODO:
+    // get flags for this TSAP from PID_GO_SECURITY_FLAGS from SecIntObj
+    bool needsEncryption = true;
+
+    if (needsEncryption)
+    {
+        uint16_t secureApduLength = apdu.length() + 2 + 6 + 4; // 2(TPCI,APCI,SCF) + sizeof(seqNum) + apdu.length() + 4
+        CemiFrame secureFrame(secureApduLength);
+        // create secure APDU
+        if (createSecureApdu(apdu, secureFrame.apdu(), true, true)) // TODO: toolAccess, confidentialty
+        {
+            ApplicationLayer::dataSystemBroadcastRequest(ack, hopType, SystemPriority, secureFrame.apdu());
+        }
+        return;
+    }
+
     ApplicationLayer::dataSystemBroadcastRequest(ack, hopType, SystemPriority, apdu);
 }
 
 void SecureApplicationLayer::dataIndividualRequest(AckType ack, HopCountType hopType, Priority priority, uint16_t destination, APDU& apdu)
 {
+    // TODO:
+    // get flags for this TSAP from PID_GO_SECURITY_FLAGS from SecIntObj
+    bool needsEncryption = true;
+
+    if (needsEncryption)
+    {
+        uint16_t secureApduLength = apdu.length() + 2 + 6 + 4; // 2(TPCI,APCI,SCF) + sizeof(seqNum) + apdu.length() + 4
+        CemiFrame secureFrame(secureApduLength);
+        // create secure APDU
+        if (createSecureApdu(apdu, secureFrame.apdu(), true, true)) // TODO: toolAccess, confidentialty
+        {
+            ApplicationLayer::dataIndividualRequest(ack, hopType, priority, destination, secureFrame.apdu());
+        }
+        return;
+    }
+
     ApplicationLayer::dataIndividualRequest(ack, hopType, priority, destination, apdu);
 }
 
 void SecureApplicationLayer::dataConnectedRequest(uint16_t tsap, Priority priority, APDU& apdu)
 {
+    // TODO:
+    // get flags for this TSAP from PID_GO_SECURITY_FLAGS from SecIntObj
+    bool needsEncryption = true;
+
+    if (needsEncryption)
+    {
+        uint16_t secureApduLength = apdu.length() + 2 + 6 + 4; // 2(TPCI,APCI,SCF) + sizeof(seqNum) + apdu.length() + 4
+        CemiFrame secureFrame(secureApduLength);
+        // create secure APDU
+        if (createSecureApdu(apdu, secureFrame.apdu(), true, true)) // TODO: toolAccess, confidentialty
+        {
+            ApplicationLayer::dataConnectedRequest(tsap, priority, secureFrame.apdu());
+        }
+        return;
+    }
+
     // apdu must be valid until it was confirmed
     ApplicationLayer::dataConnectedRequest(tsap, priority, apdu);
 }
@@ -933,18 +996,9 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
     }
     else if (syncRes)
     {
-        // Just for testing
+        // use random number in SyncResponse
-        if (testSeq)
+        uint64_t randomNumber = 0x000102030405; // TODO: generate random number
-        {
+        sixBytesFromUInt64(randomNumber, seq);
-            // Do not use a random number, but a well-known one
-            sixBytesFromUInt64(0xaaaaaaaaaaaa, seq);
-        }
-        else
-        {
-            // use random number in SyncResponse
-            uint64_t randomNumber = 0x000102030405; // TODO: generate random number
-            sixBytesFromUInt64(randomNumber, seq);
-        }
 
         // TODO: maybe implement something like std::map for pending SyncRequests?
         //final var request = pendingSyncRequests.remove(dst);
@@ -961,7 +1015,7 @@ bool SecureApplicationLayer::secure(uint8_t* buffer, uint16_t service, uint16_t
             println("sending sync.res without corresponding .req");
         }
 
-        printHex("Decrypted challenge: ", _challenge, 6);
+        //printHex("Decrypted challenge: ", _challenge, 6);
 
         // Now XOR the new random SeqNum with the challenge from the SyncRequest
         uint8_t rndXorChallenge[6];

src/knx/secure_application_layer.h

@@ -18,6 +18,161 @@ class BusAccessUnit;
  */
 class SecureApplicationLayer :  public ApplicationLayer
 {
+    template <typename K, typename V, int SIZE>
+    class Map
+    {
+    public:
+        Map()
+        {
+            static_assert (SIZE <= 64, "Map is too big! Max. 64 elements.");
+        }
+
+        void setInvalidValue(V value)
+        {
+            _invalidValue = value;
+        }
+
+        void clear()
+        {
+            _validEntries = 0;
+        }
+
+        bool empty()
+        {
+            return (_validEntries == 0);
+        }
+
+        uint8_t size()
+        {
+            uint8_t size = 0;
+
+            for (uint8_t i = 0; i < SIZE; i++)
+            {
+                size += (((_validEntries >> i) & 0x01) == 0x01) ? 1 : 0;
+            }
+
+            return size;
+        }
+
+        bool insert(K key, V value)
+        {
+            uint8_t index = getNextFreeIndex();
+            if (index != noFreeEntryFoundIndex)
+            {
+                keys[index] = key;
+                values[index] = value;
+
+                _validEntries |= 1 << index;
+                return true;
+            }
+
+            // No free space
+            return false;
+        }
+
+        bool insertOrAssign(K key, V value)
+        {
+            // Try to find the key
+            for (uint8_t i = 0; i < SIZE; i++)
+            {
+                // Check if this array slot is occupied
+                if ((_validEntries >> i) & 0x01)
+                {
+                    // Key found?
+                    if (keys[i] == key)
+                    {
+                        values[i] = value;
+                        return true;
+                    }
+                }
+            }
+
+            // Key does not exist, add it if enough space
+            return insert(key, value);
+        }
+
+        bool erase(K key)
+        {
+            for (uint8_t i = 0; i < SIZE; i++)
+            {
+                if ((_validEntries >> i) & 0x01)
+                {
+                    if (keys[i] == key)
+                    {
+                        _validEntries &= ~(1 << i);
+                        return true;
+                    }
+                }
+            }
+            return false;
+        }
+
+        V operator[](K key) const
+        {
+            // Try to find the key
+            for (uint8_t i = 0; i < SIZE; i++)
+            {
+                // Check if this array slot is occupied
+                if ((_validEntries >> i) & 0x01)
+                {
+                    // Key found?
+                    if (keys[i] == key)
+                    {
+                        return values[i];
+                    }
+                }
+            }
+            return _invalidValue;
+        }
+
+        V &operator[](K key)
+        {
+            // Try to find the key
+            for (uint8_t i = 0; i < SIZE; i++)
+            {
+                // Check if this array slot is occupied
+                if ((_validEntries >> i) & 0x01)
+                {
+                    // Key found?
+                    if (keys[i] == key)
+                    {
+                        return values[i];
+                    }
+                }
+            }
+
+            // Key was not found, so add key and value
+            uint8_t index = getNextFreeIndex();
+            if (index != noFreeEntryFoundIndex)
+            {
+                _validEntries |= 1 << index;
+                keys[index] = key;
+                return values[index];
+            }
+            return _invalidValue;
+        }
+
+    private:
+        uint8_t getNextFreeIndex()
+        {
+            for (uint8_t i = 0; i < SIZE; i++)
+            {
+                if (((_validEntries >> i) & 0x01) == 0)
+                {
+                    return i;
+                }
+            }
+
+            return noFreeEntryFoundIndex;
+        }
+
+        uint64_t _validEntries{0};
+        K keys[SIZE];
+        V values[SIZE];
+        static constexpr uint8_t noFreeEntryFoundIndex = 255;
+        V _invalidValue;
+    };
+
   public:
     /**
      * The constructor.
@@ -54,6 +209,19 @@ class SecureApplicationLayer :  public ApplicationLayer
     virtual void dataConnectedRequest(uint16_t tsap, Priority priority, APDU& apdu) override; // apdu must be valid until it was confirmed
 
   private:
+    enum class DataSecurity
+    {
+        none,
+        auth,
+        authConf
+    };
+
+    struct SecurityControl
+    {
+        bool toolAccess;
+        DataSecurity dataSecurity;
+    };
+
     uint32_t calcAuthOnlyMac(uint8_t* apdu, uint8_t apduLength, const uint8_t *key, uint8_t* iv, uint8_t* ctr0);
     uint32_t calcConfAuthMac(uint8_t* associatedData, uint16_t associatedDataLength, uint8_t* apdu, uint8_t apduLength, const uint8_t* key, uint8_t* iv);
 
@@ -101,8 +269,8 @@ class SecureApplicationLayer :  public ApplicationLayer
     uint16_t _challengeSrcAddr;
     bool _isChallengeValid {false};
 
+    Map<uint16_t, SecurityControl, 8> _pendingRequests;
+
     SecurityInterfaceObject& _secIfObj;
     DeviceObject& _deviceObj;
-
-    bool testSeq {false};
 };