diff --git a/src/iec61850/client/client_control.c b/src/iec61850/client/client_control.c index 94d203f7..4ef0576f 100644 --- a/src/iec61850/client/client_control.c +++ b/src/iec61850/client/client_control.c @@ -456,7 +456,7 @@ ControlObjectClient_operate(ControlObjectClient self, MmsValue* ctlVal, uint64_t if (mmsError != MMS_ERROR_NONE) { if (DEBUG_IED_CLIENT) - printf("IED_CLIENT: operate failed!\n"); + printf("IED_CLIENT: operate failed! (error=%i)\n", mmsError); goto exit_function; } diff --git a/src/mms/inc_private/mms_common_internal.h b/src/mms/inc_private/mms_common_internal.h index 2d899037..18ed88ef 100644 --- a/src/mms/inc_private/mms_common_internal.h +++ b/src/mms/inc_private/mms_common_internal.h @@ -91,10 +91,10 @@ void mmsMsg_createMmsRejectPdu(uint32_t* invokeId, int reason, ByteBuffer* response); int -mmsMsg_parseConfirmedErrorPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32_t* invokeId, MmsServiceError* serviceError); +mmsMsg_parseConfirmedErrorPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32_t* invokeId, bool* hasInvokeId, MmsServiceError* serviceError); int -mmsMsg_parseRejectPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32_t* invokeId, int* rejectType, int* rejectReason); +mmsMsg_parseRejectPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32_t* invokeId, bool* hasInvokeId, int* rejectType, int* rejectReason); MmsValue* mmsMsg_parseDataElement(Data_t* dataElement); diff --git a/src/mms/iso_mms/client/mms_client_connection.c b/src/mms/iso_mms/client/mms_client_connection.c index 1540d118..6daea12d 100644 --- a/src/mms/iso_mms/client/mms_client_connection.c +++ b/src/mms/iso_mms/client/mms_client_connection.c @@ -391,7 +391,7 @@ sendRequestAndWaitForResponse(MmsConnection self, uint32_t invokeId, ByteBuffer* Semaphore_post(self->lastResponseLock); - Thread_sleep(10); + Thread_sleep(2); currentTime = Hal_getTimeInMs(); } @@ -409,7 +409,7 @@ sendRequestAndWaitForResponse(MmsConnection self, uint32_t invokeId, ByteBuffer* receivedMessage = NULL; } - connection_lost: +connection_lost: removeFromOutstandingCalls(self, invokeId); @@ -440,7 +440,7 @@ getResponseInvokeId(MmsConnection self) } static void -waitUntilLastResponseHasBeenProcessed(MmsConnection self) +waitUntilResponseHasBeenProcessed(MmsConnection self) { uint32_t currentInvokeId = getResponseInvokeId(self); @@ -635,10 +635,13 @@ parseServiceError(uint8_t* buffer, int bufPos, int maxLength, MmsServiceError* e } int -mmsMsg_parseConfirmedErrorPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32_t* invokeId, MmsServiceError* serviceError) +mmsMsg_parseConfirmedErrorPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32_t* invokeId, bool* hasInvokeId, MmsServiceError* serviceError) { int length; + if (hasInvokeId) + *hasInvokeId = false; + uint8_t tag = buffer[bufPos++]; if (tag != 0xa2) goto exit_error; @@ -662,6 +665,8 @@ mmsMsg_parseConfirmedErrorPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32 switch (tag) { case 0x80: /* invoke Id */ + if (hasInvokeId) + *hasInvokeId = true; if (invokeId != NULL) *invokeId = BerDecoder_decodeUint32(buffer, length, bufPos); bufPos += length; @@ -690,10 +695,13 @@ mmsMsg_parseConfirmedErrorPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32 } int -mmsMsg_parseRejectPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32_t* invokeId, int* rejectType, int* rejectReason) +mmsMsg_parseRejectPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32_t* invokeId, bool* hasInvokeId, int* rejectType, int* rejectReason) { int length; + if (hasInvokeId) + *hasInvokeId = false; + uint8_t tag = buffer[bufPos++]; if (tag != 0xa4) @@ -719,6 +727,9 @@ mmsMsg_parseRejectPDU(uint8_t* buffer, int bufPos, int maxBufPos, uint32_t* invo goto exit_error; if (tag == 0x80) { /* invoke id */ + if (hasInvokeId) + *hasInvokeId = true; + if (invokeId != NULL) *invokeId = BerDecoder_decodeUint32(buffer, length, bufPos); } @@ -838,26 +849,29 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) else if (tag == 0xa2) { /* confirmed error PDU */ if (DEBUG_MMS_CLIENT) printf("MMS_CLIENT: Confirmed error PDU!\n"); + uint32_t invokeId; - MmsServiceError serviceError = - { 0, 0 }; + bool hasInvokeId = false; + + MmsServiceError serviceError = { 0, 0 }; - if (mmsMsg_parseConfirmedErrorPDU(payload->buffer, 0, payload->size, &invokeId, &serviceError) < 0) { + if (mmsMsg_parseConfirmedErrorPDU(payload->buffer, 0, payload->size, &invokeId, &hasInvokeId, &serviceError) < 0) { if (DEBUG_MMS_CLIENT) printf("MMS_CLIENT: Error parsing confirmedErrorPDU!\n"); goto exit_with_error; } else { - if (checkForOutstandingCall(self, invokeId)) { - /* wait for application thread to handle last received response */ - waitUntilLastResponseHasBeenProcessed(self); + if (hasInvokeId && checkForOutstandingCall(self, invokeId)) { Semaphore_wait(self->lastResponseLock); self->lastResponseError = convertServiceErrorToMmsError(serviceError); self->responseInvokeId = invokeId; Semaphore_post(self->lastResponseLock); + + /* wait for application thread to handle received response */ + waitUntilResponseHasBeenProcessed(self); } else { if (DEBUG_MMS_CLIENT) @@ -871,24 +885,25 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) if (DEBUG_MMS_CLIENT) printf("MMS_CLIENT: reject PDU!\n"); - uint32_t invokeId; + bool hasInvokeId = false; + uint32_t invokeId = 0; int rejectType; int rejectReason; - if (mmsMsg_parseRejectPDU(payload->buffer, 0, payload->size, &invokeId, &rejectType, &rejectReason) >= 0) { + if (mmsMsg_parseRejectPDU(payload->buffer, 0, payload->size, &invokeId, &hasInvokeId, &rejectType, &rejectReason) >= 0) { if (DEBUG_MMS_CLIENT) - printf("MMS_CLIENT: reject PDU invokeID: %i type: %i reason: %i\n", (int) invokeId, rejectType, rejectReason); - - if (checkForOutstandingCall(self, invokeId)) { + printf("MMS_CLIENT: reject PDU invokeID: %u type: %i reason: %i\n", invokeId, rejectType, rejectReason); - /* wait for application thread to handle last received response */ - waitUntilLastResponseHasBeenProcessed(self); + if (hasInvokeId && checkForOutstandingCall(self, invokeId)) { Semaphore_wait(self->lastResponseLock); self->lastResponseError = convertRejectCodesToMmsError(rejectType, rejectReason); self->responseInvokeId = invokeId; Semaphore_post(self->lastResponseLock); + + /* wait for application thread to handle received response */ + waitUntilResponseHasBeenProcessed(self); } else { IsoClientConnection_releaseReceiveBuffer(self->isoClient); @@ -925,13 +940,14 @@ mmsIsoCallback(IsoIndication indication, void* parameter, ByteBuffer* payload) if (checkForOutstandingCall(self, invokeId)) { - waitUntilLastResponseHasBeenProcessed(self); - Semaphore_wait(self->lastResponseLock); + self->lastResponse = payload; self->lastResponseBufPos = bufPos; self->responseInvokeId = invokeId; Semaphore_post(self->lastResponseLock); + + waitUntilResponseHasBeenProcessed(self); } else { if (DEBUG_MMS_CLIENT) diff --git a/src/mms/iso_mms/server/mms_server_connection.c b/src/mms/iso_mms/server/mms_server_connection.c index 6d84fe32..ffb76041 100644 --- a/src/mms/iso_mms/server/mms_server_connection.c +++ b/src/mms/iso_mms/server/mms_server_connection.c @@ -411,26 +411,29 @@ handleConfirmedErrorPdu( uint8_t* buffer, int bufPos, int maxBufPos, ByteBuffer* response) { - uint32_t invokeId; + uint32_t invokeId = 0; + bool hasInvokeId = false; MmsServiceError serviceError; - if (mmsMsg_parseConfirmedErrorPDU(buffer, bufPos, maxBufPos, &invokeId, &serviceError)) { + if (mmsMsg_parseConfirmedErrorPDU(buffer, bufPos, maxBufPos, &invokeId, &hasInvokeId, &serviceError)) { if (DEBUG_MMS_SERVER) - printf("MMS_SERVER: Handle confirmed error PDU: invokeID: %i\n", invokeId); + printf("MMS_SERVER: Handle confirmed error PDU: invokeID: %u\n", invokeId); + if (hasInvokeId) { /* check if message is related to an existing file upload task */ - int i; - for (i = 0; i < CONFIG_MMS_SERVER_MAX_GET_FILE_TASKS; i++) { + int i; + for (i = 0; i < CONFIG_MMS_SERVER_MAX_GET_FILE_TASKS; i++) { - if (self->server->fileUploadTasks[i].state != MMS_FILE_UPLOAD_STATE_NOT_USED) { + if (self->server->fileUploadTasks[i].state != MMS_FILE_UPLOAD_STATE_NOT_USED) { - if (self->server->fileUploadTasks[i].lastRequestInvokeId == invokeId) { + if (self->server->fileUploadTasks[i].lastRequestInvokeId == invokeId) { - self->server->fileUploadTasks[i].state = MMS_FILE_UPLOAD_STATE_SEND_OBTAIN_FILE_ERROR_SOURCE; - return; - } + self->server->fileUploadTasks[i].state = MMS_FILE_UPLOAD_STATE_SEND_OBTAIN_FILE_ERROR_SOURCE; + return; + } + } } } }