Pārlūkot izejas kodu

Merge branch 'master' into unsigned_array_size

Julius Pfrommer 9 gadi atpakaļ
vecāks
revīzija
e9437b5c34

+ 1 - 1
doc/building.rst

@@ -135,7 +135,7 @@ This group contains build options related to the supported OPC UA features.
    Enable method calls in server and client
 **ENABLE_MULTITHREADING**
    Enable multi-threading support (experimental)
-**ENABLE_SUBSCRIPTONS**
+**ENABLE_SUBSCRIPTIONS**
    Enable subscriptions
 **GENERATE_SELFSIGNED**
    Generate a self-signed certificate for the server (openSSL required)

+ 5 - 1
examples/server.c

@@ -228,7 +228,11 @@ int main(int argc, char** argv) {
 
 #ifndef _WIN32
     /* cpu temperature monitoring for linux machines */
-    if((temperatureFile = fopen("/sys/class/thermal/thermal_zone0/temp", "r"))) {
+    const char *temperatureFileName = "/sys/class/thermal/thermal_zone0/temp"; // RaspberryPi
+    // const char *temperatureFileName = "/sys/class/hwmon/hwmon0/device/temp1_input"; // Beaglebone
+    // const char *temperatureFileName = "/sys/class/thermal/thermal_zone3/temp"; // Intel Edison Alternative 1
+    // const char *temperatureFileName = "/sys/class/thermal/thermal_zone4/temp"; // Intel Edison Alternative 2
+    if((temperatureFile = fopen(temperatureFileName, "r"))) {
         // add node with the data source
         UA_DataSource temperatureDataSource = (UA_DataSource) {
             .handle = NULL, .read = readTemperature, .write = NULL};

+ 234 - 213
src/server/ua_server_binary.c

@@ -146,270 +146,289 @@ static void init_response_header(const UA_RequestHeader *p, UA_ResponseHeader *r
     r->timestamp = UA_DateTime_now();
 }
 
-/* The request/response are casted to the header (first element of their struct) */
-static void invoke_service(UA_Server *server, UA_SecureChannel *channel, UA_UInt32 requestId,
-                           UA_RequestHeader *request, const UA_DataType *responseType,
-                           void (*service)(UA_Server*, UA_Session*, void*, void*)) {
-    UA_ResponseHeader *response = UA_alloca(responseType->memSize);
-    UA_init(response, responseType);
-    init_response_header(request, response);
-    /* try to get the session from the securechannel first */
-    UA_Session *session = UA_SecureChannel_getSession(channel, &request->authenticationToken);
-#ifdef EXTENSION_STATELESS
-    if(request->authenticationToken.namespaceIndex == 0
-            && request->authenticationToken.identifierType == UA_NODEIDTYPE_NUMERIC
-            && request->authenticationToken.identifier.numeric == 0
-       && (responseType->typeIndex == UA_TYPES_READRESPONSE
-           || responseType->typeIndex == UA_TYPES_WRITERESPONSE
-           || responseType->typeIndex == UA_TYPES_BROWSERESPONSE)) {
-        session = &anonymousSession;
-        service(server, session, request, response);
-    } else {
-#endif
-    if(!session || session->channel != channel) {
-        response->serviceResult = UA_STATUSCODE_BADSESSIONIDINVALID;
-    } else if(session->activated == UA_FALSE) {
-        response->serviceResult = UA_STATUSCODE_BADSESSIONNOTACTIVATED;
-        /* /\* the session is invalidated FIXME: do this delayed*\/ */
-        /* UA_SessionManager_removeSession(&server->sessionManager, server, &request->authenticationToken); */
-    } else {
-        UA_Session_updateLifetime(session);
-        service(server, session, request, response);
-    }
-#ifdef EXTENSION_STATELESS
-    }
-#endif
-    UA_StatusCode retval = UA_SecureChannel_sendBinaryMessage(channel, requestId, response, responseType);
-    if(retval != UA_STATUSCODE_GOOD) {
-        if(retval == UA_STATUSCODE_BADENCODINGLIMITSEXCEEDED)
-            response->serviceResult = UA_STATUSCODE_BADRESPONSETOOLARGE;
-        else
-            response->serviceResult = retval;
-        UA_SecureChannel_sendBinaryMessage(channel, requestId, response, &UA_TYPES[UA_TYPES_SERVICEFAULT]);
-    }
-    UA_deleteMembers(response, responseType);
-}
-
-#define INVOKE_SERVICE(REQUEST, RESPONSETYPE) do {                      \
-        UA_##REQUEST##Request p;                                        \
-        if(UA_##REQUEST##Request_decodeBinary(msg, pos, &p))            \
-            return;                                                     \
-        invoke_service(server, clientChannel, sequenceHeader.requestId, \
-                       &p.requestHeader, &UA_TYPES[RESPONSETYPE],       \
-                       (void (*)(UA_Server*, UA_Session*, void*,void*))Service_##REQUEST); \
-        UA_##REQUEST##Request_deleteMembers(&p);                        \
-} while(0)
-
-static void processMSG(UA_Connection *connection, UA_Server *server, const UA_ByteString *msg, size_t *pos) {
-    /* Read in the securechannel */
-    UA_UInt32 secureChannelId;
-    UA_StatusCode retval = UA_UInt32_decodeBinary(msg, pos, &secureChannelId);
-    if(retval != UA_STATUSCODE_GOOD)
-        return;
-
-    /* the anonymous channel is used e.g. to allow getEndpoints without a channel */
-    UA_SecureChannel *clientChannel = connection->channel;
-    UA_SecureChannel anonymousChannel;
-    if(!clientChannel) {
-        UA_SecureChannel_init(&anonymousChannel);
-        anonymousChannel.connection = connection;
-        clientChannel = &anonymousChannel;
-    }
-
-    /* Read the security header */
-    UA_UInt32 tokenId = 0;
-    UA_SequenceHeader sequenceHeader;
-    retval = UA_UInt32_decodeBinary(msg, pos, &tokenId);
-    retval |= UA_SequenceHeader_decodeBinary(msg, pos, &sequenceHeader);
-    if(retval != UA_STATUSCODE_GOOD)
-        return;
-
-#ifndef EXTENSION_STATELESS
-    if(tokenId == 0) // 0 is invalid
-        return;
-#endif
-
-    if(clientChannel != &anonymousChannel && tokenId != clientChannel->securityToken.tokenId) {
-        if(tokenId != clientChannel->nextSecurityToken.tokenId) {
-            /* close the securechannel but keep the connection open */
-            UA_LOG_INFO(server->logger, UA_LOGCATEGORY_SECURECHANNEL,
-                        "Request with a wrong security token. Closing the SecureChannel %i.",
-                        clientChannel->securityToken.channelId);
-            Service_CloseSecureChannel(server, clientChannel->securityToken.channelId);
-            return;
-        }
-        UA_SecureChannel_revolveTokens(clientChannel);
-    }
-
-    /* Read the request type */
-    UA_NodeId requestType;
-    if(UA_NodeId_decodeBinary(msg, pos, &requestType) != UA_STATUSCODE_GOOD)
-        return;
-    if(requestType.identifierType != UA_NODEIDTYPE_NUMERIC) {
-        UA_NodeId_deleteMembers(&requestType);
-        return;
-    }
-
-    switch(requestType.identifier.numeric - UA_ENCODINGOFFSET_BINARY) {
-    case UA_NS0ID_GETENDPOINTSREQUEST: {
-        if(clientChannel == &anonymousChannel)
-            UA_LOG_DEBUG(server->logger, UA_LOGCATEGORY_NETWORK, "Processing GetEndpointsRequest on Connection %i",
-                         connection->sockfd);
-        else
-            UA_LOG_DEBUG(server->logger, UA_LOGCATEGORY_SECURECHANNEL, "Processing GetEndpointsRequest on SecureChannel %i",
-                         clientChannel->securityToken.channelId);
-        UA_GetEndpointsRequest p;
-        UA_GetEndpointsResponse r;
-        if(UA_GetEndpointsRequest_decodeBinary(msg, pos, &p))
-            return;
-        UA_GetEndpointsResponse_init(&r);
-        init_response_header(&p.requestHeader, &r.responseHeader);
-        Service_GetEndpoints(server, &p, &r);
-        UA_GetEndpointsRequest_deleteMembers(&p);
-        UA_SecureChannel_sendBinaryMessage(clientChannel, sequenceHeader.requestId, &r,
-                                           &UA_TYPES[UA_TYPES_GETENDPOINTSRESPONSE]);
-        UA_GetEndpointsResponse_deleteMembers(&r);
+static void
+getServicePointers(UA_UInt32 requestTypeId, const UA_DataType **requestType,
+                   const UA_DataType **responseType, UA_Service *service) {
+    switch(requestTypeId - UA_ENCODINGOFFSET_BINARY) {
+    case UA_NS0ID_GETENDPOINTSREQUEST:
+        *service = (UA_Service)Service_GetEndpoints;
+        *requestType = &UA_TYPES[UA_TYPES_GETENDPOINTSREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_GETENDPOINTSRESPONSE];
         break;
-    }
-
-    case UA_NS0ID_FINDSERVERSREQUEST: {
-        if(clientChannel == &anonymousChannel)
-            UA_LOG_DEBUG(server->logger, UA_LOGCATEGORY_NETWORK, "Processing FindServerRequest on Connection %i",
-                         connection->sockfd);
-        else
-            UA_LOG_DEBUG(server->logger, UA_LOGCATEGORY_SECURECHANNEL, "Processing FindServerRequest on SecureChannel %i",
-                         clientChannel->securityToken.channelId);
-        UA_FindServersRequest  p;
-        UA_FindServersResponse r;
-        if(UA_FindServersRequest_decodeBinary(msg, pos, &p))
-            return;
-        UA_FindServersResponse_init(&r);
-        init_response_header(&p.requestHeader, &r.responseHeader);
-        Service_FindServers(server, &p, &r);
-        UA_FindServersRequest_deleteMembers(&p);
-        UA_SecureChannel_sendBinaryMessage(clientChannel, sequenceHeader.requestId, &r,
-                                           &UA_TYPES[UA_TYPES_FINDSERVERSRESPONSE]);
-        UA_FindServersResponse_deleteMembers(&r);
+    case UA_NS0ID_FINDSERVERSREQUEST:
+        *service = (UA_Service)Service_FindServers;
+        *requestType = &UA_TYPES[UA_TYPES_FINDSERVERSREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_FINDSERVERSRESPONSE];
         break;
-    }
-
-    case UA_NS0ID_CREATESESSIONREQUEST: {
-        UA_CreateSessionRequest  p;
-        UA_CreateSessionResponse r;
-        if(UA_CreateSessionRequest_decodeBinary(msg, pos, &p))
-            return;
-        UA_CreateSessionResponse_init(&r);
-        init_response_header(&p.requestHeader, &r.responseHeader);
-        Service_CreateSession(server, clientChannel, &p, &r);
-        UA_CreateSessionRequest_deleteMembers(&p);
-        UA_SecureChannel_sendBinaryMessage(clientChannel, sequenceHeader.requestId, &r,
-                                           &UA_TYPES[UA_TYPES_CREATESESSIONRESPONSE]);
-        UA_CreateSessionResponse_deleteMembers(&r);
+    case UA_NS0ID_CREATESESSIONREQUEST:
+        *service = (UA_Service)Service_CreateSession;
+        *requestType = &UA_TYPES[UA_TYPES_CREATESESSIONREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_CREATESESSIONRESPONSE];
         break;
-    }
-
-    case UA_NS0ID_ACTIVATESESSIONREQUEST: {
-        UA_ActivateSessionRequest  p;
-        UA_ActivateSessionResponse r;
-        if(UA_ActivateSessionRequest_decodeBinary(msg, pos, &p))
-            return;
-        UA_ActivateSessionResponse_init(&r);
-        init_response_header(&p.requestHeader, &r.responseHeader);
-        Service_ActivateSession(server, clientChannel, &p, &r);
-        UA_ActivateSessionRequest_deleteMembers(&p);
-        UA_SecureChannel_sendBinaryMessage(clientChannel, sequenceHeader.requestId, &r,
-                                           &UA_TYPES[UA_TYPES_ACTIVATESESSIONRESPONSE]);
-        UA_ActivateSessionResponse_deleteMembers(&r);
+    case UA_NS0ID_ACTIVATESESSIONREQUEST:
+        *service = (UA_Service)Service_ActivateSession;
+        *requestType = &UA_TYPES[UA_TYPES_ACTIVATESESSIONREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_ACTIVATESESSIONRESPONSE];
         break;
-    }
-    
     case UA_NS0ID_CLOSESESSIONREQUEST:
-        INVOKE_SERVICE(CloseSession, UA_TYPES_CLOSESESSIONRESPONSE);
+        *service = (UA_Service)Service_CloseSession;
+        *requestType = &UA_TYPES[UA_TYPES_CLOSESESSIONREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_CLOSESESSIONRESPONSE];
         break;
     case UA_NS0ID_READREQUEST:
-        INVOKE_SERVICE(Read, UA_TYPES_READRESPONSE);
+        *service = (UA_Service)Service_Read;
+        *requestType = &UA_TYPES[UA_TYPES_READREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_READRESPONSE];
         break;
     case UA_NS0ID_WRITEREQUEST:
-        INVOKE_SERVICE(Write, UA_TYPES_WRITERESPONSE);
+        *service = (UA_Service)Service_Write;
+        *requestType = &UA_TYPES[UA_TYPES_WRITEREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_WRITERESPONSE];
         break;
     case UA_NS0ID_BROWSEREQUEST:
-        INVOKE_SERVICE(Browse, UA_TYPES_BROWSERESPONSE);
+        *service = (UA_Service)Service_Browse;
+        *requestType = &UA_TYPES[UA_TYPES_BROWSEREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_BROWSERESPONSE];
         break;
     case UA_NS0ID_BROWSENEXTREQUEST:
-        INVOKE_SERVICE(BrowseNext, UA_TYPES_BROWSENEXTRESPONSE);
+        *service = (UA_Service)Service_BrowseNext;
+        *requestType = &UA_TYPES[UA_TYPES_BROWSENEXTREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_BROWSENEXTRESPONSE];
         break;
     case UA_NS0ID_REGISTERNODESREQUEST:
-        INVOKE_SERVICE(RegisterNodes, UA_TYPES_REGISTERNODESRESPONSE);
+        *service = (UA_Service)Service_RegisterNodes;
+        *requestType = &UA_TYPES[UA_TYPES_REGISTERNODESREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_REGISTERNODESRESPONSE];
         break;
     case UA_NS0ID_UNREGISTERNODESREQUEST:
-        INVOKE_SERVICE(UnregisterNodes, UA_TYPES_UNREGISTERNODESRESPONSE);
+        *service = (UA_Service)Service_UnregisterNodes;
+        *requestType = &UA_TYPES[UA_TYPES_UNREGISTERNODESREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_UNREGISTERNODESRESPONSE];
         break;
     case UA_NS0ID_TRANSLATEBROWSEPATHSTONODEIDSREQUEST:
-        INVOKE_SERVICE(TranslateBrowsePathsToNodeIds, UA_TYPES_TRANSLATEBROWSEPATHSTONODEIDSRESPONSE);
+        *service = (UA_Service)Service_TranslateBrowsePathsToNodeIds;
+        *requestType = &UA_TYPES[UA_TYPES_TRANSLATEBROWSEPATHSTONODEIDSREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_TRANSLATEBROWSEPATHSTONODEIDSRESPONSE];
         break;
+
 #ifdef ENABLE_SUBSCRIPTIONS
     case UA_NS0ID_CREATESUBSCRIPTIONREQUEST:
-        INVOKE_SERVICE(CreateSubscription, UA_TYPES_CREATESUBSCRIPTIONRESPONSE);
+        *service = (UA_Service)Service_CreateSubscription;
+        *requestType = &UA_TYPES[UA_TYPES_CREATESUBSCRIPTIONREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_CREATESUBSCRIPTIONRESPONSE];
         break;
     case UA_NS0ID_PUBLISHREQUEST:
-        INVOKE_SERVICE(Publish, UA_TYPES_PUBLISHRESPONSE);
+        *service = (UA_Service)Service_Publish;
+        *requestType = &UA_TYPES[UA_TYPES_PUBLISHREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_PUBLISHRESPONSE];
+        break;
+    case UA_NS0ID_REPUBLISHREQUEST:
+        *service = (UA_Service)Service_Republish;
+        *requestType = &UA_TYPES[UA_TYPES_REPUBLISHREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_REPUBLISHRESPONSE];
         break;
     case UA_NS0ID_MODIFYSUBSCRIPTIONREQUEST:
-        INVOKE_SERVICE(ModifySubscription, UA_TYPES_MODIFYSUBSCRIPTIONRESPONSE);
+        *service = (UA_Service)Service_ModifySubscription;
+        *requestType = &UA_TYPES[UA_TYPES_MODIFYSUBSCRIPTIONREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_MODIFYSUBSCRIPTIONRESPONSE];
         break;
     case UA_NS0ID_DELETESUBSCRIPTIONSREQUEST:
-        INVOKE_SERVICE(DeleteSubscriptions, UA_TYPES_DELETESUBSCRIPTIONSRESPONSE);
+        *service = (UA_Service)Service_DeleteSubscriptions;
+        *requestType = &UA_TYPES[UA_TYPES_DELETESUBSCRIPTIONSREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_DELETESUBSCRIPTIONSRESPONSE];
         break;
     case UA_NS0ID_CREATEMONITOREDITEMSREQUEST:
-        INVOKE_SERVICE(CreateMonitoredItems, UA_TYPES_CREATEMONITOREDITEMSRESPONSE);
+        *service = (UA_Service)Service_CreateMonitoredItems;
+        *requestType = &UA_TYPES[UA_TYPES_CREATEMONITOREDITEMSREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_CREATEMONITOREDITEMSRESPONSE];
         break;
     case UA_NS0ID_DELETEMONITOREDITEMSREQUEST:
-        INVOKE_SERVICE(DeleteMonitoredItems, UA_TYPES_DELETEMONITOREDITEMSRESPONSE);
+        *service = (UA_Service)Service_DeleteMonitoredItems;
+        *requestType = &UA_TYPES[UA_TYPES_DELETEMONITOREDITEMSREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_DELETEMONITOREDITEMSRESPONSE];
         break;
 #endif
+
 #ifdef ENABLE_METHODCALLS
     case UA_NS0ID_CALLREQUEST:
-        INVOKE_SERVICE(Call, UA_TYPES_CALLRESPONSE);
+        *service = (UA_Service)Service_Call;
+        *requestType = &UA_TYPES[UA_TYPES_CALLREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_CALLRESPONSE];
 	break;
 #endif
+
 #ifdef ENABLE_NODEMANAGEMENT
     case UA_NS0ID_ADDNODESREQUEST:
-        INVOKE_SERVICE(AddNodes, UA_TYPES_ADDNODESRESPONSE);
+        *service = (UA_Service)Service_AddNodes;
+        *requestType = &UA_TYPES[UA_TYPES_ADDNODESREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_ADDNODESRESPONSE];
         break;
     case UA_NS0ID_ADDREFERENCESREQUEST:
-        INVOKE_SERVICE(AddReferences, UA_TYPES_ADDREFERENCESRESPONSE);
+        *service = (UA_Service)Service_AddReferences;
+        *requestType = &UA_TYPES[UA_TYPES_ADDREFERENCESREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_ADDREFERENCESRESPONSE];
         break;
     case UA_NS0ID_DELETENODESREQUEST:
-        INVOKE_SERVICE(DeleteNodes, UA_TYPES_DELETENODESRESPONSE);
+        *service = (UA_Service)Service_DeleteNodes;
+        *requestType = &UA_TYPES[UA_TYPES_DELETENODESREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_DELETENODESRESPONSE];
         break;
     case UA_NS0ID_DELETEREFERENCESREQUEST:
-        INVOKE_SERVICE(DeleteReferences, UA_TYPES_DELETEREFERENCESRESPONSE);
+        *service = (UA_Service)Service_DeleteReferences;
+        *requestType = &UA_TYPES[UA_TYPES_DELETEREFERENCESREQUEST];
+        *responseType = &UA_TYPES[UA_TYPES_DELETEREFERENCESRESPONSE];
         break;
 #endif
-    default: {
-        if(requestType.namespaceIndex == 0 && requestType.identifier.numeric==787)
-            UA_LOG_INFO(server->logger, UA_LOGCATEGORY_NETWORK,
-                        "Client requested a subscription that are not supported, the message will be skipped");
-        else
-            UA_LOG_INFO(server->logger, UA_LOGCATEGORY_NETWORK, "Unknown request: NodeId(ns=%d, i=%d)",
-                        requestType.namespaceIndex, requestType.identifier.numeric);
-        UA_RequestHeader p;
-        UA_ServiceFault r;
-        if(UA_RequestHeader_decodeBinary(msg, pos, &p) != UA_STATUSCODE_GOOD)
-            return;
-        UA_ServiceFault_init(&r);
-        init_response_header(&p, &r.responseHeader);
-        r.responseHeader.serviceResult = UA_STATUSCODE_BADSERVICEUNSUPPORTED;
-        UA_SecureChannel_sendBinaryMessage(clientChannel, sequenceHeader.requestId, &r,
-                                           &UA_TYPES[UA_TYPES_SERVICEFAULT]);
-        UA_RequestHeader_deleteMembers(&p);
-        UA_ServiceFault_deleteMembers(&r);
+
+    default:
         break;
     }
+}
+
+static void
+sendError(UA_SecureChannel *channel, const UA_ByteString *msg, size_t pos,
+          UA_UInt32 requestId, UA_StatusCode error) {
+    UA_RequestHeader p;
+    if(UA_RequestHeader_decodeBinary(msg, &pos, &p) != UA_STATUSCODE_GOOD)
+        return;
+    UA_ResponseHeader r;
+    UA_ResponseHeader_init(&r);
+    init_response_header(&p, &r);
+    r.serviceResult = error;
+    UA_SecureChannel_sendBinaryMessage(channel, requestId, &r,
+                                       &UA_TYPES[UA_TYPES_SERVICEFAULT]);
+    UA_RequestHeader_deleteMembers(&p);
+    UA_ResponseHeader_deleteMembers(&r);
+}
+
+static void
+processMSG(UA_Connection *connection, UA_Server *server, const UA_ByteString *msg, size_t *pos) {
+    /* If we cannot decode these, don't respond */
+    UA_UInt32 secureChannelId = 0;
+    UA_UInt32 tokenId = 0;
+    UA_SequenceHeader sequenceHeader;
+    UA_NodeId requestTypeId;
+    UA_StatusCode retval = UA_UInt32_decodeBinary(msg, pos, &secureChannelId);
+    retval |= UA_UInt32_decodeBinary(msg, pos, &tokenId);
+    retval |= UA_SequenceHeader_decodeBinary(msg, pos, &sequenceHeader);
+    retval = UA_NodeId_decodeBinary(msg, pos, &requestTypeId);
+    if(retval != UA_STATUSCODE_GOOD)
+        return;
+
+    UA_SecureChannel *channel = connection->channel;
+    UA_SecureChannel anonymousChannel;
+    if(!channel) {
+        UA_SecureChannel_init(&anonymousChannel);
+        anonymousChannel.connection = connection;
+        channel = &anonymousChannel;
+    }
+
+    /* Test if the secure channel is ok */
+    if(secureChannelId != channel->securityToken.channelId)
+        return;
+    if(tokenId != channel->securityToken.tokenId) {
+        if(tokenId != channel->nextSecurityToken.tokenId) {
+            /* close the securechannel but keep the connection open */
+            UA_LOG_INFO(server->logger, UA_LOGCATEGORY_SECURECHANNEL,
+                        "Request with a wrong security token. Closing the SecureChannel %i.",
+                        channel->securityToken.channelId);
+            Service_CloseSecureChannel(server, channel->securityToken.channelId);
+            return;
+        }
+        UA_SecureChannel_revolveTokens(channel);
+    }
+
+    /* Test if the service type nodeid has the right format */
+    if(requestTypeId.identifierType != UA_NODEIDTYPE_NUMERIC ||
+       requestTypeId.namespaceIndex != 0) {
+        UA_NodeId_deleteMembers(&requestTypeId);
+        sendError(channel, msg, *pos, sequenceHeader.requestId, UA_STATUSCODE_BADSERVICEUNSUPPORTED);
+        return;
+    }
+
+    /* Get the service pointers */
+    UA_Service service = NULL;
+    const UA_DataType *requestType = NULL;
+    const UA_DataType *responseType = NULL;
+    getServicePointers(requestTypeId.identifier.numeric, &requestType, &responseType, &service);
+    if(!service) {
+        /* The service is not supported */
+        if(requestTypeId.identifier.numeric==787)
+            UA_LOG_INFO(server->logger, UA_LOGCATEGORY_SERVER,
+                        "Client requested a subscription that are not supported, "
+                        "the message will be skipped");
+        else
+            UA_LOG_INFO(server->logger, UA_LOGCATEGORY_SERVER, "Unknown request: NodeId(ns=%d, i=%d)",
+                        requestTypeId.namespaceIndex, requestTypeId.identifier.numeric);
+        sendError(channel, msg, *pos, sequenceHeader.requestId, UA_STATUSCODE_BADSERVICEUNSUPPORTED);
+        return;
+    }
+
+    /* Most services can only be called with a valid securechannel */
+#ifndef EXTENSION_STATELESS
+    if(channel == &anonymousChannel &&
+       requestType->typeIndex > UA_TYPES_OPENSECURECHANNELREQUEST) {
+        sendError(channel, msg, *pos, sequenceHeader.requestId, UA_STATUSCODE_BADSECURECHANNELIDINVALID);
+        return;
+    }
+#endif
+
+    /* Decode the request */
+    void *request = UA_alloca(requestType->memSize);
+    size_t oldpos = *pos;
+    retval = UA_decodeBinary(msg, pos, request, requestType);
+    if(retval != UA_STATUSCODE_GOOD) {
+        sendError(channel, msg, oldpos, sequenceHeader.requestId, retval);
+        return;
+    }
+
+    /* Find the matching session */
+    UA_Session *session =
+        UA_SecureChannel_getSession(channel, &((UA_RequestHeader*)request)->authenticationToken);
+    UA_Session anonymousSession;
+    if(!session) {
+        UA_Session_init(&anonymousSession);
+        anonymousSession.channel = channel;
+        anonymousSession.activated = UA_TRUE;
+        session = &anonymousSession;
+    }
+
+    /* Test if the session is valid */
+    if(!session->activated && requestType->typeIndex != UA_TYPES_ACTIVATESESSIONREQUEST) {
+        sendError(channel, msg, *pos, sequenceHeader.requestId, UA_STATUSCODE_BADSESSIONNOTACTIVATED);
+        return;
+    }
+#ifndef EXTENSION_STATELESS
+    if(session == &anonymousSession &&
+       requestType->typeIndex > UA_TYPES_CREATESESSIONREQUEST) {
+        sendError(channel, msg, *pos, sequenceHeader.requestId, UA_STATUSCODE_BADSESSIONIDINVALID);
+        return;
+    }
+#endif
+
+    /* Call the service */
+    UA_Session_updateLifetime(session);
+    void *response = UA_alloca(responseType->memSize);
+    UA_init(response, responseType);
+    init_response_header(request, response);
+    service(server, session, request, response);
+
+    /* Send the response */
+    retval = UA_SecureChannel_sendBinaryMessage(channel, sequenceHeader.requestId,
+                                                response, responseType);
+    if(retval != UA_STATUSCODE_GOOD) {
+        /* e.g. UA_STATUSCODE_BADENCODINGLIMITSEXCEEDED */
+        sendError(channel, msg, oldpos, sequenceHeader.requestId, retval);
     }
+
+    /* Clean up */
+    UA_deleteMembers(request, requestType);
+    UA_deleteMembers(response, responseType);
+    return;
 }
 
-static void processCLO(UA_Connection *connection, UA_Server *server, const UA_ByteString *msg, size_t *pos) {
+static void
+processCLO(UA_Connection *connection, UA_Server *server, const UA_ByteString *msg, size_t *pos) {
     UA_UInt32 secureChannelId;
     UA_StatusCode retval = UA_UInt32_decodeBinary(msg, pos, &secureChannelId);
     if(retval != UA_STATUSCODE_GOOD || !connection->channel ||
@@ -455,13 +474,15 @@ void UA_Server_processBinaryMessage(UA_Server *server, UA_Connection *connection
             connection->close(connection);
             return;
         default:
-            UA_LOG_INFO(server->logger, UA_LOGCATEGORY_NETWORK, "Unknown request type on Connection %i", connection->sockfd);
+            UA_LOG_INFO(server->logger, UA_LOGCATEGORY_NETWORK,
+                        "Unknown request type on Connection %i", connection->sockfd);
         }
 
         UA_TcpMessageHeader_deleteMembers(&tcpMessageHeader);
         if(pos != targetpos) {
             UA_LOG_INFO(server->logger, UA_LOGCATEGORY_NETWORK,
-                        "Message on Connection %i was not entirely processed. Arrived at position %i, skip after the announced length to position %i",
+                        "Message on Connection %i was not entirely processed. "
+                        "Arrived at position %i, skip after the announced length to position %i",
                         connection->sockfd, pos, targetpos);
             pos = targetpos;
         }

+ 8 - 1
src/server/ua_server_internal.h

@@ -85,7 +85,7 @@ void UA_Server_addExistingNode(UA_Server *server, UA_Session *session, UA_Node *
                                const UA_NodeId *parentNodeId, const UA_NodeId *referenceTypeId,
                                UA_AddNodesResult *result);
 
-typedef UA_StatusCode (*UA_EditNodeCallback)(UA_Server *server, UA_Session*, UA_Node*, const void*);
+typedef UA_StatusCode (*UA_EditNodeCallback)(UA_Server*, UA_Session*, UA_Node*, const void*);
 
 /* Calls callback on the node. In the multithreaded case, the node is copied before and replaced in
    the nodestore. */
@@ -98,4 +98,11 @@ UA_StatusCode UA_Server_addDelayedJob(UA_Server *server, UA_Job job);
 
 void UA_Server_deleteAllRepeatedJobs(UA_Server *server);
 
+typedef void (*UA_SendResponseCallback)(UA_Server*, UA_Session*, const void*, const UA_DataType*);
+
+void UA_Server_processRequest(UA_Server *server, UA_Session *session,
+                              const void *request, const UA_DataType *requestType,
+                              void *response, const UA_DataType *responseType,
+                              UA_SendResponseCallback *send);
+
 #endif /* UA_SERVER_INTERNAL_H_ */

+ 80 - 45
src/server/ua_services.h

@@ -8,6 +8,8 @@
 #include "ua_session.h"
 #include "ua_nodes.h"
 
+typedef void (*UA_Service)(UA_Server*, UA_Session*, const void*, void*);
+
 /**
  * @ingroup server
  * @defgroup services Services
@@ -25,13 +27,15 @@
  *
  * @{
  */
-void Service_FindServers(UA_Server *server, const UA_FindServersRequest *request,
+void Service_FindServers(UA_Server *server, UA_Session *session,
+                         const UA_FindServersRequest *request,
                          UA_FindServersResponse *response);
 /**
  * Returns the Endpoints supported by a Server and all of the configuration
  * information required to establish a SecureChannel and a Session.
  */
-void Service_GetEndpoints(UA_Server *server, const UA_GetEndpointsRequest *request,
+void Service_GetEndpoints(UA_Server *server, UA_Session *session,
+                          const UA_GetEndpointsRequest *request,
                           UA_GetEndpointsResponse *response);
 // Service_RegisterServer
 /** @} */
@@ -75,8 +79,9 @@ void Service_CloseSecureChannel(UA_Server *server, UA_Int32 channelId);
  * address space. The second is the authenticationToken which is used to
  * associate an incoming request with a Session.
  */
-void Service_CreateSession(UA_Server *server, UA_SecureChannel *channel,
-                           const UA_CreateSessionRequest *request, UA_CreateSessionResponse *response);
+void Service_CreateSession(UA_Server *server, UA_Session *session,
+                           const UA_CreateSessionRequest *request,
+                           UA_CreateSessionResponse *response);
 
 /**
  * Used by the Client to submit its SoftwareCertificates to the Server for
@@ -85,11 +90,13 @@ void Service_CreateSession(UA_Server *server, UA_SecureChannel *channel,
  * any other Service request after CreateSession. Failure to do so shall cause
  * the Server to close the Session.
  */
-void Service_ActivateSession(UA_Server *server, UA_SecureChannel *channel,
-                             const UA_ActivateSessionRequest *request, UA_ActivateSessionResponse *response);
+void Service_ActivateSession(UA_Server *server, UA_Session *session,
+                             const UA_ActivateSessionRequest *request,
+                             UA_ActivateSessionResponse *response);
 
 /** Used to terminate a Session. */
-void Service_CloseSession(UA_Server *server, UA_Session *session, const UA_CloseSessionRequest *request,
+void Service_CloseSession(UA_Server *server, UA_Session *session,
+                          const UA_CloseSessionRequest *request,
                           UA_CloseSessionResponse *response);
 // Service_Cancel
 /** @} */
@@ -105,19 +112,22 @@ void Service_CloseSession(UA_Server *server, UA_Session *session, const UA_Close
  */
 
 /** Used to add one or more Nodes into the AddressSpace hierarchy. */
-void Service_AddNodes(UA_Server *server, UA_Session *session, const UA_AddNodesRequest *request,
+void Service_AddNodes(UA_Server *server, UA_Session *session,
+                      const UA_AddNodesRequest *request,
                       UA_AddNodesResponse *response);
 void Service_AddNodes_single(UA_Server *server, UA_Session *session, const UA_AddNodesItem *item,
                              UA_AddNodesResult *result);
 
 /** Used to add one or more References to one or more Nodes. */
-void Service_AddReferences(UA_Server *server, UA_Session *session, const UA_AddReferencesRequest *request,
+void Service_AddReferences(UA_Server *server, UA_Session *session,
+                           const UA_AddReferencesRequest *request,
                            UA_AddReferencesResponse *response);
 UA_StatusCode Service_AddReferences_single(UA_Server *server, UA_Session *session,
                                            const UA_AddReferencesItem *item);
 
 /** Used to delete one or more Nodes from the AddressSpace. */
-void Service_DeleteNodes(UA_Server *server, UA_Session *session, const UA_DeleteNodesRequest *request,
+void Service_DeleteNodes(UA_Server *server, UA_Session *session,
+                         const UA_DeleteNodesRequest *request,
                          UA_DeleteNodesResponse *response);
 UA_StatusCode Service_DeleteNodes_single(UA_Server *server, UA_Session *session, const UA_NodeId *nodeId,
                                          UA_Boolean deleteReferences);
@@ -145,11 +155,13 @@ UA_StatusCode Service_DeleteReferences_single(UA_Server *server, UA_Session *ses
  * further limited by the use of a View. This Browse Service also supports a
  * primitive filtering capability.
  */
-void Service_Browse(UA_Server *server, UA_Session *session, const UA_BrowseRequest *request,
+void Service_Browse(UA_Server *server, UA_Session *session,
+                    const UA_BrowseRequest *request,
                     UA_BrowseResponse *response);
 
-void Service_Browse_single(UA_Server *server, UA_Session *session, struct ContinuationPointEntry *cp,
-                           const UA_BrowseDescription *descr, UA_UInt32 maxrefs, UA_BrowseResult *result);
+void Service_Browse_single(UA_Server *server, UA_Session *session,
+                           struct ContinuationPointEntry *cp, const UA_BrowseDescription *descr,
+                           UA_UInt32 maxrefs, UA_BrowseResult *result);
 
 /**
  * Used to request the next set of Browse or BrowseNext response information
@@ -158,7 +170,8 @@ void Service_Browse_single(UA_Server *server, UA_Session *session, struct Contin
  * the number of results to return exceeds the maximum number of results to
  * return that was specified by the Client in the original Browse request.
  */
-void Service_BrowseNext(UA_Server *server, UA_Session *session, const UA_BrowseNextRequest *request,
+void Service_BrowseNext(UA_Server *server, UA_Session *session,
+                        const UA_BrowseNextRequest *request,
                         UA_BrowseNextResponse *response);
 
 /** Used to translate textual node paths to their respective ids. */
@@ -168,9 +181,11 @@ void Service_TranslateBrowsePathsToNodeIds(UA_Server *server, UA_Session *sessio
 void Service_TranslateBrowsePathsToNodeIds_single(UA_Server *server, UA_Session *session,
                                                   const UA_BrowsePath *path, UA_BrowsePathResult *result);
 
-void Service_RegisterNodes(UA_Server *server, UA_Session *session, const UA_RegisterNodesRequest *request,
+void Service_RegisterNodes(UA_Server *server, UA_Session *session,
+                           const UA_RegisterNodesRequest *request,
                            UA_RegisterNodesResponse *response);
-void Service_UnregisterNodes(UA_Server *server, UA_Session *session, const UA_UnregisterNodesRequest *request,
+void Service_UnregisterNodes(UA_Server *server, UA_Session *session,
+                             const UA_UnregisterNodesRequest *request,
                              UA_UnregisterNodesResponse *response);
 /** @} */
 
@@ -211,10 +226,14 @@ UA_StatusCode parse_numericrange(const UA_String *str, UA_NumericRange *range);
  * allows Clients to read the entire set of indexed values as a composite, to
  * read individual elements or to read ranges of elements of the composite.
  */
-void Service_Read(UA_Server *server, UA_Session *session, const UA_ReadRequest *request,
-                  UA_ReadResponse *response);
-void Service_Read_single(UA_Server *server, UA_Session *session, UA_TimestampsToReturn timestamps,
-                         const UA_ReadValueId *id, UA_DataValue *v);
+void
+Service_Read(UA_Server *server, UA_Session *session,
+             const UA_ReadRequest *request,
+             UA_ReadResponse *response);
+void
+Service_Read_single(UA_Server *server, UA_Session *session,
+                    UA_TimestampsToReturn timestamps,
+                    const UA_ReadValueId *id, UA_DataValue *v);
 
 // Service_HistoryRead
 /**
@@ -223,11 +242,14 @@ void Service_Read_single(UA_Server *server, UA_Session *session, UA_TimestampsTo
  * allows Clients to write the entire set of indexed values as a composite, to
  * write individual elements or to write ranges of elements of the composite.
  */
-void Service_Write(UA_Server *server, UA_Session *session, const UA_WriteRequest *request,
-                   UA_WriteResponse *response);
+void
+Service_Write(UA_Server *server, UA_Session *session,
+              const UA_WriteRequest *request,
+              UA_WriteResponse *response);
 
 /** Single attribute writes are exposed to the userspace. The wvalue may be destroyed (deleteMembers) */
-UA_StatusCode Service_Write_single(UA_Server *server, UA_Session *session, const UA_WriteValue *wvalue);
+UA_StatusCode
+Service_Write_single(UA_Server *server, UA_Session *session, const UA_WriteValue *wvalue);
 
 // Service_HistoryUpdate
 /** @} */
@@ -241,8 +263,10 @@ UA_StatusCode Service_Write_single(UA_Server *server, UA_Session *session, const
  * @{
  */
 #ifdef ENABLE_METHODCALLS
-void Service_Call(UA_Server *server, UA_Session *session, const UA_CallRequest *request,
-                  UA_CallResponse *response);
+void
+Service_Call(UA_Server *server, UA_Session *session,
+             const UA_CallRequest *request,
+             UA_CallResponse *response);
 #endif
 /** @} */
 
@@ -264,15 +288,17 @@ void Service_Call(UA_Server *server, UA_Session *session, const UA_CallRequest *
  * links to be deleted, but has no effect on the MonitoredItems referenced by
  * the triggered items.
  */
-void Service_CreateMonitoredItems(UA_Server *server, UA_Session *session,
-                                       const UA_CreateMonitoredItemsRequest *request, 
-                                       UA_CreateMonitoredItemsResponse *response);
+void
+Service_CreateMonitoredItems(UA_Server *server, UA_Session *session,
+                             const UA_CreateMonitoredItemsRequest *request, 
+                             UA_CreateMonitoredItemsResponse *response);
 // Service_ModifyMonitoredItems
 // Service_SetMonitoringMode
 // Service_SetTriggering
-void Service_DeleteMonitoredItems(UA_Server *server, UA_Session *session,
-                                  const UA_DeleteMonitoredItemsRequest *request,
-                                  UA_DeleteMonitoredItemsResponse *response);
+void
+Service_DeleteMonitoredItems(UA_Server *server, UA_Session *session,
+                             const UA_DeleteMonitoredItemsRequest *request,
+                             UA_DeleteMonitoredItemsResponse *response);
                                       
 /** @} */
 
@@ -284,27 +310,36 @@ void Service_DeleteMonitoredItems(UA_Server *server, UA_Session *session,
  * @{
  */
     
-void Service_CreateSubscription(UA_Server *server, UA_Session *session,
-                                const UA_CreateSubscriptionRequest *request,
-                                UA_CreateSubscriptionResponse *response);
+void
+Service_CreateSubscription(UA_Server *server, UA_Session *session,
+                           const UA_CreateSubscriptionRequest *request,
+                           UA_CreateSubscriptionResponse *response);
+
+void
+Service_ModifySubscription(UA_Server *server, UA_Session *session,
+                           const UA_ModifySubscriptionRequest *request,
+                           UA_ModifySubscriptionResponse *response);
+
+void
+Service_DeleteSubscriptions(UA_Server *server, UA_Session *session,
+                            const UA_DeleteSubscriptionsRequest *request,
+                            UA_DeleteSubscriptionsResponse *response);
+                                     
+void
+Service_Publish(UA_Server *server, UA_Session *session,
+                const UA_PublishRequest *request,
+                UA_PublishResponse *response);
 
-void Service_ModifySubscription(UA_Server *server, UA_Session *session,
-                                const UA_ModifySubscriptionRequest *request,
-                                UA_ModifySubscriptionResponse *response);
+void
+Service_Republish(UA_Server *server, UA_Session *session,
+                  const UA_RepublishRequest *request,
+                  UA_RepublishResponse *response);
 
-void Service_DeleteSubscriptions(UA_Server *server, UA_Session *session,
-                                 const UA_DeleteSubscriptionsRequest *request,
-                                 UA_DeleteSubscriptionsResponse *response);
-                                     
-void Service_Publish(UA_Server *server, UA_Session *session,
-                     const UA_PublishRequest *request, UA_PublishResponse *response);
-                         
 // Service_ModifySubscription
 // Service_SetPublishingMode
 // UA_Int32 Service_SetPublishingMode(UA_Server *server, UA_Session *session,
                                   // const UA_SetPublishingModeRequest *request,
                                   // UA_SetPublishingModeResponse *response);
-// Service_Republish
 // Service_TransferSubscription
 // Service_DeleteSubscription
 /** @} */

+ 4 - 4
src/server/ua_services_discovery.c

@@ -2,8 +2,8 @@
 #include "ua_services.h"
 #include "ua_util.h"
 
-void Service_FindServers(UA_Server *server, const UA_FindServersRequest *request,
-                         UA_FindServersResponse *response) {
+void Service_FindServers(UA_Server *server, UA_Session *session,
+                         const UA_FindServersRequest *request, UA_FindServersResponse *response) {
     response->servers = UA_malloc(sizeof(UA_ApplicationDescription));
     if(!response->servers) {
         response->responseHeader.serviceResult = UA_STATUSCODE_BADOUTOFMEMORY;
@@ -17,8 +17,8 @@ void Service_FindServers(UA_Server *server, const UA_FindServersRequest *request
 	response->serversSize = 1;
 }
 
-void Service_GetEndpoints(UA_Server *server, const UA_GetEndpointsRequest *request,
-                          UA_GetEndpointsResponse *response) {
+void Service_GetEndpoints(UA_Server *server, UA_Session *session,
+                          const UA_GetEndpointsRequest *request, UA_GetEndpointsResponse *response) {
     /* test if the supported binary profile shall be returned */
 #ifdef NO_ALLOCA
 	UA_Boolean relevant_endpoints[server->endpointDescriptionsSize];

+ 10 - 3
src/server/ua_services_session.c

@@ -3,8 +3,13 @@
 #include "ua_session_manager.h"
 #include "ua_types_generated_encoding_binary.h"
 
-void Service_CreateSession(UA_Server *server, UA_SecureChannel *channel, const UA_CreateSessionRequest *request,
+void Service_CreateSession(UA_Server *server, UA_Session *session, const UA_CreateSessionRequest *request,
                            UA_CreateSessionResponse *response) {
+    UA_SecureChannel *channel = session->channel;
+    if(channel->securityToken.channelId == 0) {
+        response->responseHeader.serviceResult = UA_STATUSCODE_BADSECURECHANNELIDINVALID;
+        return;
+    }
     response->responseHeader.serviceResult =
         UA_Array_copy(server->endpointDescriptions, server->endpointDescriptionsSize,
                       (void**)&response->serverEndpoints, &UA_TYPES[UA_TYPES_ENDPOINTDESCRIPTION]);
@@ -41,8 +46,10 @@ void Service_CreateSession(UA_Server *server, UA_SecureChannel *channel, const U
                  response->sessionId.identifier.numeric);
 }
 
-void Service_ActivateSession(UA_Server *server, UA_SecureChannel *channel,
-                             const UA_ActivateSessionRequest *request, UA_ActivateSessionResponse *response) {
+void
+Service_ActivateSession(UA_Server *server, UA_Session *session, const UA_ActivateSessionRequest *request,
+                        UA_ActivateSessionResponse *response) {
+    UA_SecureChannel *channel = session->channel;
     // make the channel know about the session
 	UA_Session *foundSession =
         UA_SessionManager_getSession(&server->sessionManager, &request->requestHeader.authenticationToken);

+ 54 - 4
src/server/ua_services_subscription.c

@@ -170,10 +170,22 @@ void Service_Publish(UA_Server *server, UA_Session *session, const UA_PublishReq
         if(sub->unpublishedNotificationsSize == 0)
             continue;
         
+        // This subscription has notifications in its queue (top NotificationMessage exists in the queue). 
+        // Due to republish, we need to check if there are any unplublished notifications first ()
+        UA_unpublishedNotification *notification = NULL;
+        LIST_FOREACH(notification, &sub->unpublishedNotifications, listEntry) {
+            if (notification->publishedOnce == UA_FALSE)
+                break;
+        }
+        if (notification == NULL)
+            continue;
+    
+        // We found an unpublished notification message in this subscription, which we will now publish.
         response->subscriptionId = sub->subscriptionID;
-        Subscription_copyTopNotificationMessage(&response->notificationMessage, sub);
-        UA_unpublishedNotification *firstUnPublished = LIST_FIRST(&sub->unpublishedNotifications);
-        if(firstUnPublished->notification.sequenceNumber > sub->sequenceNumber) {
+        Subscription_copyNotificationMessage(&response->notificationMessage, notification);
+        // Mark this notification as published
+        notification->publishedOnce = UA_TRUE;
+        if(notification->notification.sequenceNumber > sub->sequenceNumber) {
             // If this is a keepalive message, its seqNo is the next seqNo to be used for an actual msg.
             response->availableSequenceNumbersSize = 0;
             // .. and must be deleted
@@ -196,7 +208,7 @@ void Service_Publish(UA_Server *server, UA_Session *session, const UA_PublishReq
         response->subscriptionId = sub->subscriptionID;
         sub->keepAliveCount.currentValue=sub->keepAliveCount.minValue;
         Subscription_generateKeepAlive(sub);
-        Subscription_copyTopNotificationMessage(&response->notificationMessage, sub);
+        Subscription_copyNotificationMessage(&response->notificationMessage, sub->unpublishedNotifications.lh_first);
         Subscription_deleteUnpublishedNotification(sub->sequenceNumber + 1, false, sub);
     }
     
@@ -277,3 +289,41 @@ void Service_DeleteMonitoredItems(UA_Server *server, UA_Session *session,
             SubscriptionManager_deleteMonitoredItem(manager, sub->subscriptionID,
                                                     request->monitoredItemIds[i]);
 }
+
+void Service_Republish(UA_Server *server, UA_Session *session,
+                                const UA_RepublishRequest *request,
+                                UA_RepublishResponse *response) {
+    UA_SubscriptionManager *manager = &session->subscriptionManager;
+    UA_Subscription *sub = SubscriptionManager_getSubscriptionByID(manager, request->subscriptionId);
+    if (!sub) {
+        response->responseHeader.serviceResult = UA_STATUSCODE_BADSUBSCRIPTIONIDINVALID;
+        return;
+    }
+    
+    // Find the notification in question
+    UA_unpublishedNotification *notification;
+    LIST_FOREACH(notification, &sub->unpublishedNotifications, listEntry) {
+      if (notification->notification.sequenceNumber == request->retransmitSequenceNumber)
+	break;
+    }
+    if (!notification) {
+      response->responseHeader.serviceResult = UA_STATUSCODE_BADSEQUENCENUMBERINVALID;
+      return;
+    }
+    
+    // FIXME: By spec, this notification has to be in the "retransmit queue", i.e. publishedOnce must be
+    //        true. If this is not tested, the client just gets what he asks for... hence this part is
+    //        commented:
+    /* Check if the notification is in the published queue
+    if (notification->publishedOnce == UA_FALSE) {
+      response->responseHeader.serviceResult = UA_STATUSCODE_BADSUBSCRIPTIONIDINVALID;
+      return;
+    }
+    */
+    // Retransmit 
+    Subscription_copyNotificationMessage(&response->notificationMessage, notification);
+    // Mark this notification as published
+    notification->publishedOnce = UA_TRUE;
+    
+    return;
+}

+ 4 - 9
src/server/ua_subscription.c

@@ -115,6 +115,7 @@ void Subscription_updateNotifications(UA_Subscription *subscription) {
     for(size_t notmsgn = 0; notmsgn < msg->notification.notificationDataSize; notmsgn++) {
         // Set the notification message type and encoding for each of 
         //   the three possible NotificationData Types
+
         /* msg->notification->notificationData[notmsgn].encoding = 1; // Encoding is always binary */
         /* msg->notification->notificationData[notmsgn].typeId = UA_NODEID_NUMERIC(0, 811); */
       
@@ -160,24 +161,18 @@ UA_UInt32 *Subscription_getAvailableSequenceNumbers(UA_Subscription *sub) {
     return seqArray;
 }
 
-void Subscription_copyTopNotificationMessage(UA_NotificationMessage *dst, UA_Subscription *sub) {
+void Subscription_copyNotificationMessage(UA_NotificationMessage *dst, UA_unpublishedNotification *src) {
     if(!dst)
         return;
     
-    if(sub->unpublishedNotificationsSize == 0) {
-        dst->notificationDataSize = 0;
-        dst->publishTime = UA_DateTime_now();
-        dst->sequenceNumber = 0;
-        return;
-    }
-    
-    UA_NotificationMessage *latest = &LIST_FIRST(&sub->unpublishedNotifications)->notification;
+    UA_NotificationMessage *latest = &src->notification;
     dst->notificationDataSize = latest->notificationDataSize;
     dst->publishTime = latest->publishTime;
     dst->sequenceNumber = latest->sequenceNumber;
     
     if(latest->notificationDataSize == 0)
         return;
+
     dst->notificationData = UA_ExtensionObject_new();
     UA_ExtensionObject_copy(latest->notificationData, dst->notificationData);
 }

+ 2 - 0
src/server/ua_subscription.h

@@ -66,6 +66,7 @@ int MonitoredItem_QueueToDataChangeNotifications(UA_MonitoredItemNotification *d
 /****************/
 
 typedef struct UA_unpublishedNotification {
+    UA_Boolean publishedOnce;
     LIST_ENTRY(UA_unpublishedNotification) listEntry;
     UA_NotificationMessage notification;
 } UA_unpublishedNotification;
@@ -96,6 +97,7 @@ UA_UInt32 *Subscription_getAvailableSequenceNumbers(UA_Subscription *sub);
 void Subscription_generateKeepAlive(UA_Subscription *subscription);
 void Subscription_copyTopNotificationMessage(UA_NotificationMessage *dst, UA_Subscription *sub);
 UA_UInt32 Subscription_deleteUnpublishedNotification(UA_UInt32 seqNo, UA_Boolean bDeleteAll, UA_Subscription *sub);
+void Subscription_copyNotificationMessage(UA_NotificationMessage *dst, UA_unpublishedNotification *src);
 UA_StatusCode Subscription_createdUpdateJob(UA_Server *server, UA_Guid jobId, UA_Subscription *sub);
 UA_StatusCode Subscription_registerUpdateJob(UA_Server *server, UA_Subscription *sub);
 UA_StatusCode Subscription_unregisterUpdateJob(UA_Server *server, UA_Subscription *sub);

+ 1 - 1
tools/generate_datatypes.py

@@ -69,7 +69,7 @@ minimal_types = ["InvalidType", "Node", "NodeClass", "ReferenceNode", "Applicati
 subscription_types = ["CreateSubscriptionRequest", "CreateSubscriptionResponse",
                       "DeleteMonitoredItemsRequest", "DeleteMonitoredItemsResponse", "NotificationMessage",
                       "MonitoredItemNotification", "DataChangeNotification", "ModifySubscriptionRequest",
-                      "ModifySubscriptionResponse"]
+                      "ModifySubscriptionResponse", "RepublishRequest", "RepublishResponse"]
 
 class TypeDescription(object):
     def __init__(self, name, nodeid, namespaceid):