Fix FunctionPropertyExt*. Working T_DATA_CONNECTED with FunctionPropertyExt*.

src/knx/application_layer.cpp

@@ -578,9 +578,9 @@ void ApplicationLayer::functionPropertyStateResponse(AckType ack, Priority prior
 void ApplicationLayer::functionPropertyExtStateResponse(AckType ack, Priority priority, HopCountType hopType, uint16_t asap, const SecurityControl& secCtrl,
                                                         uint16_t objectType, uint8_t objectInstance, uint16_t propertyId, uint8_t* resultData, uint8_t resultLength)
 {
-    CemiFrame frame(6 + resultLength + 1);
+    CemiFrame frame(5 + resultLength + 1);
     APDU& apdu = frame.apdu();
-    apdu.type(FunctionPropertyStateResponse);
+    apdu.type(FunctionPropertyExtStateResponse);
     uint8_t* data = apdu.data() + 1;
 
     data[0] = ((uint16_t)objectType) >> 8;
@@ -875,18 +875,18 @@ void ApplicationLayer::individualIndication(HopCountType hopType, Priority prior
         {
             ObjectType objectType = (ObjectType)(((data[1] & 0xff) << 8) | (data[2] & 0xff));
             uint8_t objectInstance = ((data[3] & 0xff) << 4) | ((data[4] & 0xff) >> 4);
-            uint16_t propertyId = ((data[5] & 0xf) << 8) | (data[6] & 0xff);
+            uint16_t propertyId = ((data[4] & 0xf) << 8) | (data[5] & 0xff);
-            uint8_t* functionInput = &data[7];
+            uint8_t* functionInput = &data[6];
-            _bau.functionPropertyExtCommandIndication(priority, hopType, tsap, secCtrl, objectType, objectInstance, propertyId, functionInput, apdu.length() - 8); //TODO: check length
+            _bau.functionPropertyExtCommandIndication(priority, hopType, tsap, secCtrl, objectType, objectInstance, propertyId, functionInput, apdu.length() - 6);
             break;
         }
         case FunctionPropertyExtState:
         {
             ObjectType objectType = (ObjectType)(((data[1] & 0xff) << 8) | (data[2] & 0xff));
             uint8_t objectInstance = ((data[3] & 0xff) << 4) | ((data[4] & 0xff) >> 4);
-            uint16_t propertyId = ((data[5] & 0xf) << 8) | (data[6] & 0xff);
+            uint16_t propertyId = ((data[4] & 0xf) << 8) | (data[5] & 0xff);
-            uint8_t* functionInput = &data[7];
+            uint8_t* functionInput = &data[6];
-            _bau.functionPropertyExtStateIndication(priority, hopType, tsap, secCtrl, objectType, objectInstance, propertyId, functionInput, apdu.length() - 8); //TODO: check length
+            _bau.functionPropertyExtStateIndication(priority, hopType, tsap, secCtrl, objectType, objectInstance, propertyId, functionInput, apdu.length() - 6);
             break;
         }
         case PropertyDescriptionRead:

src/knx/secure_application_layer.cpp

@@ -341,6 +341,10 @@ void SecureApplicationLayer::dataSystemBroadcastRequest(AckType ack, HopCountTyp
 
     if (secCtrl.dataSecurity != DataSecurity::none)
     {
+        apdu.frame().sourceAddress(_deviceObj.induvidualAddress());
+        apdu.frame().destinationAddress(0x0000);
+        apdu.frame().addressType(GroupAddress);
+
         uint16_t secureApduLength = apdu.length() + 3 + 6 + 4; // 3(TPCI,APCI,SCF) + sizeof(seqNum) + apdu.length() + 4
         CemiFrame secureFrame(secureApduLength);
         // create secure APDU
@@ -360,6 +364,10 @@ void SecureApplicationLayer::dataIndividualRequest(AckType ack, HopCountType hop
 
     if (secCtrl.dataSecurity != DataSecurity::none)
     {
+        apdu.frame().sourceAddress(_deviceObj.induvidualAddress());
+        apdu.frame().destinationAddress(destination);
+        apdu.frame().addressType(InduvidualAddress);
+
         uint16_t secureApduLength = apdu.length() + 3 + 6 + 4; // 3(TPCI,APCI,SCF) + sizeof(seqNum) + apdu.length() + 4
         CemiFrame secureFrame(secureApduLength);
         // create secure APDU
@@ -379,6 +387,10 @@ void SecureApplicationLayer::dataConnectedRequest(uint16_t tsap, Priority priori
 
     if (secCtrl.dataSecurity != DataSecurity::none)
     {
+        apdu.frame().sourceAddress(_deviceObj.induvidualAddress());
+        apdu.frame().destinationAddress(_transportLayer->getConnectionAddress());
+        apdu.frame().addressType(InduvidualAddress);
+
         uint16_t secureApduLength = apdu.length() + 3 + 6 + 4; // 3(TPCI,APCI,SCF) + sizeof(seqNum) + apdu.length() + 4
         CemiFrame secureFrame(secureApduLength);
         // create secure APDU

src/knx/security_interface_object.cpp

@@ -56,6 +56,7 @@ SecurityInterfaceObject::SecurityInterfaceObject()
                     resultData[0] = ReturnCodes::Success;
                     resultData[1] = serviceId;
                     resultLength = 2;
+                    return;
                 }
                 resultData[0] = ReturnCodes::GenericError;
                 resultLength = 1;

src/knx/transport_layer.cpp

@@ -530,6 +530,7 @@ void TransportLayer::ackTimeoutIndication()
     }
 }
 
+// Note: we should probably also add the TSAP as argument if would support multiple concurrent connections
 uint8_t TransportLayer::getTpciSeqNum()
 {
     // Return seqNum that would be used for sending next frame
@@ -537,6 +538,12 @@ uint8_t TransportLayer::getTpciSeqNum()
     return ((_seqNoSend & 0xF) << 2);
 }
 
+// Note: we should probably also add the TSAP as argument if would support multiple concurrent connections
+uint16_t TransportLayer::getConnectionAddress()
+{
+    return _connectionAddress;
+}
+
 void TransportLayer::loop()
 {
     uint32_t milliseconds = millis();

src/knx/transport_layer.h

@@ -58,6 +58,7 @@ public:
     void dataConnectedRequest(uint16_t tsap, Priority priority, APDU& apdu);
 
     uint8_t getTpciSeqNum();
+    uint16_t getConnectionAddress();
 #pragma endregion
 
 #pragma region other