fix(pubsub): Multiple datasetreader with same Publisher ID

 - Get the required Datasetreader even if
   multiple datasetreader has same Publisher ID

Change-Id: I63e3b73b7aed9da73c4848901d671c5f46bc4ea6

src/pubsub/ua_pubsub_reader.c

@@ -158,34 +158,36 @@ UA_ReaderGroupConfig_copy(const UA_ReaderGroupConfig *src,
     return UA_STATUSCODE_GOOD;
 }
 
-static UA_DataSetReader *
-checkReaderIdentifier(UA_Server *server, UA_NetworkMessage *pMsg, UA_DataSetReader *tmpReader) {
+static UA_StatusCode
+checkReaderIdentifier(UA_Server *server, UA_NetworkMessage *pMsg, UA_DataSetReader* dataSetReader, UA_DataSetReader *tmpReader) {
     if(!pMsg->groupHeaderEnabled &&
        !pMsg->groupHeader.writerGroupIdEnabled &&
        !pMsg->payloadHeaderEnabled) {
         UA_LOG_INFO(&server->config.logger, UA_LOGCATEGORY_SERVER,
                     "Cannot process DataSetReader without WriterGroup"
                     "and DataSetWriter identifiers");
-        return NULL;
+        return UA_STATUSCODE_BADNOTIMPLEMENTED;
     }
 
     if((tmpReader->config.writerGroupId == pMsg->groupHeader.writerGroupId) &&
        (tmpReader->config.dataSetWriterId == *pMsg->payloadHeader.dataSetPayloadHeader.dataSetWriterIds)) {
         UA_LOG_DEBUG(&server->config.logger, UA_LOGCATEGORY_SERVER,
                      "DataSetReader found. Process NetworkMessage");
-        return tmpReader;
+        *dataSetReader = *tmpReader;
+        return UA_STATUSCODE_GOOD;
     }
 
-    return NULL;
+    return UA_STATUSCODE_BADNOTFOUND;
 }
 
-static UA_DataSetReader *
+static UA_StatusCode
 getReaderFromIdentifier(UA_Server *server, UA_NetworkMessage *pMsg,
-                        UA_PubSubConnection *pConnection) {
+                        UA_DataSetReader* dataSetReader, UA_PubSubConnection *pConnection) {
+    UA_StatusCode retval = UA_STATUSCODE_GOOD;
     if(!pMsg->publisherIdEnabled) {
         UA_LOG_INFO(&server->config.logger, UA_LOGCATEGORY_SERVER,
                     "Cannot process DataSetReader without PublisherId");
-        return NULL;
+        return UA_STATUSCODE_BADNOTIMPLEMENTED; /* TODO: Handle DSR without PublisherId */
     }
 
     UA_ReaderGroup* readerGroup;
@@ -197,49 +199,56 @@ getReaderFromIdentifier(UA_Server *server, UA_NetworkMessage *pMsg,
                 if(tmpReader->config.publisherId.type == &UA_TYPES[UA_TYPES_BYTE] &&
                    pMsg->publisherIdType == UA_PUBLISHERDATATYPE_BYTE &&
                    pMsg->publisherId.publisherIdByte == *(UA_Byte*)tmpReader->config.publisherId.data) {
-                    UA_DataSetReader* processReader = checkReaderIdentifier(server, pMsg, tmpReader);
-                    return processReader;
+                    retval = checkReaderIdentifier(server, pMsg, dataSetReader, tmpReader);
+                    if (retval == UA_STATUSCODE_GOOD)
+                        return retval;
                 }
                 break;
             case UA_PUBLISHERDATATYPE_UINT16:
                 if(tmpReader->config.publisherId.type == &UA_TYPES[UA_TYPES_UINT16] &&
                    pMsg->publisherIdType == UA_PUBLISHERDATATYPE_UINT16 &&
                    pMsg->publisherId.publisherIdUInt16 == *(UA_UInt16*) tmpReader->config.publisherId.data) {
-                    UA_DataSetReader* processReader = checkReaderIdentifier(server, pMsg, tmpReader);
-                    return processReader;
+                    retval = checkReaderIdentifier(server, pMsg, dataSetReader, tmpReader);
+                    if (retval == UA_STATUSCODE_GOOD)
+                        return retval;
                 }
                 break;
             case UA_PUBLISHERDATATYPE_UINT32:
                 if(tmpReader->config.publisherId.type == &UA_TYPES[UA_TYPES_UINT32] &&
                    pMsg->publisherIdType == UA_PUBLISHERDATATYPE_UINT32 &&
                    pMsg->publisherId.publisherIdUInt32 == *(UA_UInt32*)tmpReader->config.publisherId.data) {
-                    UA_DataSetReader* processReader = checkReaderIdentifier(server, pMsg, tmpReader);
-                    return processReader;
+                    retval = checkReaderIdentifier(server, pMsg, dataSetReader, tmpReader);
+                    if (retval == UA_STATUSCODE_GOOD)
+                        return retval;
                 }
                 break;
             case UA_PUBLISHERDATATYPE_UINT64:
                 if(tmpReader->config.publisherId.type == &UA_TYPES[UA_TYPES_UINT64] &&
                    pMsg->publisherIdType == UA_PUBLISHERDATATYPE_UINT64 &&
                    pMsg->publisherId.publisherIdUInt64 == *(UA_UInt64*)tmpReader->config.publisherId.data) {
-                    UA_DataSetReader* processReader = checkReaderIdentifier(server, pMsg, tmpReader);
-                    return processReader;
+                    retval = checkReaderIdentifier(server, pMsg, dataSetReader, tmpReader);
+                    if (retval == UA_STATUSCODE_GOOD)
+                        return retval;
                 }
                 break;
             case UA_PUBLISHERDATATYPE_STRING:
                 if(tmpReader->config.publisherId.type == &UA_TYPES[UA_TYPES_STRING] &&
                    pMsg->publisherIdType == UA_PUBLISHERDATATYPE_STRING &&
                    UA_String_equal(&pMsg->publisherId.publisherIdString, (UA_String*)tmpReader->config.publisherId.data)) {
-                    UA_DataSetReader* processReader = checkReaderIdentifier(server, pMsg, tmpReader);
-                    return processReader;
+                    retval = checkReaderIdentifier(server, pMsg, dataSetReader, tmpReader);
+                    if (retval == UA_STATUSCODE_GOOD)
+                        return retval;
                 }
                 break;
             default:
-                return NULL;
+                return UA_STATUSCODE_BADINTERNALERROR;
             }
         }
     }
 
-    return NULL;
+    UA_LOG_INFO(&server->config.logger, UA_LOGCATEGORY_SERVER,
+                "Dataset reader not found. Check PublisherID, WriterGroupID and DatasetWriterID");
+    return UA_STATUSCODE_BADNOTFOUND;
 }
 
 UA_ReaderGroup *
@@ -669,6 +678,7 @@ void UA_DataSetReader_delete(UA_Server *server, UA_DataSetReader *dataSetReader)
 UA_StatusCode
 UA_Server_processNetworkMessage(UA_Server *server, UA_NetworkMessage *pMsg,
                                 UA_PubSubConnection *pConnection) {
+    UA_StatusCode retval = UA_STATUSCODE_GOOD;
     if(!pMsg || !pConnection)
         return UA_STATUSCODE_BADINVALIDARGUMENT;
 
@@ -676,11 +686,11 @@ UA_Server_processNetworkMessage(UA_Server *server, UA_NetworkMessage *pMsg,
     /* To Do The condition pMsg->dataSetClassIdEnabled
      * Here some filtering is possible */
 
-    UA_DataSetReader* dataSetReaderErg = getReaderFromIdentifier(server, pMsg, pConnection);
-
-    /* No Reader with the specified id found */
-    if(!dataSetReaderErg) {
-        return UA_STATUSCODE_BADNOTFOUND; /* TODO: Check the return code */
+    UA_DataSetReader dataSetReader;
+    memset(&dataSetReader, 0, sizeof(UA_DataSetReader));
+    retval = getReaderFromIdentifier(server, pMsg, &dataSetReader, pConnection);
+    if(retval != UA_STATUSCODE_GOOD) {
+        return retval;
     }
 
     UA_LOG_DEBUG(&server->config.logger, UA_LOGCATEGORY_SERVER,
@@ -691,7 +701,7 @@ UA_Server_processNetworkMessage(UA_Server *server, UA_NetworkMessage *pMsg,
         anzDataSets = pMsg->payloadHeader.dataSetPayloadHeader.count;
     for(UA_Byte iterator = 0; iterator < anzDataSets; iterator++) {
         UA_LOG_DEBUG(&server->config.logger, UA_LOGCATEGORY_SERVER, "Process Msg with DataSetReader!");
-        UA_Server_DataSetReader_process(server, dataSetReaderErg,
+        UA_Server_DataSetReader_process(server, &dataSetReader,
                                         &pMsg->payload.dataSetPayload.dataSetMessages[iterator]);
     }