diff --git a/examples/iec61850_client_example_files/client_example_files.c b/examples/iec61850_client_example_files/client_example_files.c index 33c6750..f321f83 100644 --- a/examples/iec61850_client_example_files/client_example_files.c +++ b/examples/iec61850_client_example_files/client_example_files.c @@ -29,6 +29,9 @@ downloadHandler(void* parameter, uint8_t* buffer, uint32_t bytesRead) if (bufferPosition + bytesRead < MAX_BUFFER_SIZE) { memcpy(downloadBuffer + bufferPosition, buffer, bytesRead); + + bufferPosition += bytesRead; + return true; } else diff --git a/src/mms/asn1/ber_decode.c b/src/mms/asn1/ber_decode.c index 7eca930..4acc260 100644 --- a/src/mms/asn1/ber_decode.c +++ b/src/mms/asn1/ber_decode.c @@ -70,10 +70,65 @@ BerDecoder_decodeString(uint8_t* buffer, int strlen, int bufPos, int maxBufPos) } uint32_t -BerDecoder_decodeUint32(uint8_t* buffer, int intlen, int bufPos) { +BerDecoder_decodeUint32(uint8_t* buffer, int intLen, int bufPos) +{ uint32_t value = 0; int i; + for (i = 0; i < intLen; i++) { + value <<= 8; + value += buffer[bufPos + i]; + } + + return value; +} + +#if 0 +int32_t +BerDecoder_decodeInt32(uint8_t* buffer, int intlen, int bufPos) +{ + int32_t value = 0; + + bool isNegative = ((buffer[bufPos] & 0x80) == 0x80); + + int i = 0; + + if (isNegative) { + for (i = 0; i < 4; i++) { + + value <<= 8; + + if (i < (4 - intlen)) + value += 0xff; + else + value += buffer[bufPos + i - (4 - intlen)]; + } + } + else { + for (i = 0; i < intlen; i++) { + value <<= 8; + value += buffer[bufPos + i]; + } + } + + return value; +} +#endif + +#if 1 +int32_t +BerDecoder_decodeInt32(uint8_t* buffer, int intlen, int bufPos) +{ + int32_t value; + int i; + + bool isNegative = ((buffer[bufPos] & 0x80) == 0x80); + + if (isNegative) + value = -1; + else + value = 0; + for (i = 0; i < intlen; i++) { value <<= 8; value += buffer[bufPos + i]; @@ -81,6 +136,7 @@ BerDecoder_decodeUint32(uint8_t* buffer, int intlen, int bufPos) { return value; } +#endif float BerDecoder_decodeFloat(uint8_t* buffer, int bufPos) diff --git a/src/mms/asn1/ber_encoder.c b/src/mms/asn1/ber_encoder.c index 35953c6..49c9b67 100644 --- a/src/mms/asn1/ber_encoder.c +++ b/src/mms/asn1/ber_encoder.c @@ -250,6 +250,33 @@ BerEncoder_encodeUInt32(uint32_t value, uint8_t* buffer, int bufPos) return bufPos; } +int +BerEncoder_encodeInt32(int32_t value, uint8_t* buffer, int bufPos) +{ + uint8_t* valueArray = (uint8_t*) &value; + uint8_t valueBuffer[4]; + + int i; + +#if (ORDER_LITTLE_ENDIAN == 1) + for (i = 0; i < 4; i++) { + valueBuffer[3 - i] = valueArray[i]; + } +#else + for (i = 0; i < 4; i++) { + valueBuffer[i] = valueArray[i]; + } +#endif + + int size = BerEncoder_compressInteger(valueBuffer, 4); + + for (i = 0; i < size; i++) { + buffer[bufPos++] = valueBuffer[i]; + } + + return bufPos; +} + int BerEncoder_encodeUInt32WithTL(uint8_t tag, uint32_t value, uint8_t* buffer, int bufPos) { diff --git a/src/mms/inc_private/ber_decode.h b/src/mms/inc_private/ber_decode.h index 258f6a2..8bdbcc3 100644 --- a/src/mms/inc_private/ber_decode.h +++ b/src/mms/inc_private/ber_decode.h @@ -34,6 +34,9 @@ BerDecoder_decodeString(uint8_t* buffer, int strlen, int bufPos, int maxBufPos); uint32_t BerDecoder_decodeUint32(uint8_t* buffer, int intlen, int bufPos); +int32_t +BerDecoder_decodeInt32(uint8_t* buffer, int intlen, int bufPos); + float BerDecoder_decodeFloat(uint8_t* buffer, int bufPos); diff --git a/src/mms/inc_private/ber_encoder.h b/src/mms/inc_private/ber_encoder.h index 343f54f..ff4b26d 100644 --- a/src/mms/inc_private/ber_encoder.h +++ b/src/mms/inc_private/ber_encoder.h @@ -55,6 +55,9 @@ BerEncoder_encodeAsn1PrimitiveValue(uint8_t tag, Asn1PrimitiveValue* value, uint int BerEncoder_encodeUInt32(uint32_t value, uint8_t* buffer, int bufPos); +int +BerEncoder_encodeInt32(int32_t value, uint8_t* buffer, int bufPos); + int BerEncoder_encodeUInt32WithTL(uint8_t tag, uint32_t value, uint8_t* buffer, int bufPos); diff --git a/src/mms/inc_private/mms_server_internal.h b/src/mms/inc_private/mms_server_internal.h index a95579a..3393501 100644 --- a/src/mms/inc_private/mms_server_internal.h +++ b/src/mms/inc_private/mms_server_internal.h @@ -271,7 +271,7 @@ mmsServer_handleWriteRequest( void mmsServer_handleIdentifyRequest( MmsServerConnection connection, - int invokeId, + uint32_t invokeId, ByteBuffer* response); void @@ -279,7 +279,7 @@ mmsServer_handleStatusRequest( MmsServerConnection connection, uint8_t* requestBuffer, int bufPos, - int invokeId, + uint32_t invokeId, ByteBuffer* response); void diff --git a/src/mms/iso_mms/client/mms_client_files.c b/src/mms/iso_mms/client/mms_client_files.c index a6603eb..b0e7549 100644 --- a/src/mms/iso_mms/client/mms_client_files.c +++ b/src/mms/iso_mms/client/mms_client_files.c @@ -212,7 +212,7 @@ mmsClient_handleFileReadRequest( uint32_t invokeId, ByteBuffer* response) { - int32_t frsmId = (int32_t) BerDecoder_decodeUint32(buffer, maxBufPos - bufPos, bufPos); + int32_t frsmId = BerDecoder_decodeInt32(buffer, maxBufPos - bufPos, bufPos); if (DEBUG_MMS_CLIENT) printf("MMS_CLIENT: mmsClient_handleFileReadRequest read request for frsmId: %i\n", frsmId); @@ -232,7 +232,7 @@ mmsClient_handleFileCloseRequest( uint32_t invokeId, ByteBuffer* response) { - int32_t frsmId = (int32_t) BerDecoder_decodeUint32(buffer, maxBufPos - bufPos, bufPos); + int32_t frsmId = BerDecoder_decodeInt32(buffer, maxBufPos - bufPos, bufPos); MmsFileReadStateMachine* frsm = getFrsm(connection, frsmId); @@ -698,7 +698,7 @@ mmsMsg_parseFileOpenResponse(uint8_t* buffer, int bufPos, int maxBufPos, int32_t switch (tag) { case 0x80: /* frsmId */ - *frsmId = (int32_t) BerDecoder_decodeUint32(buffer, length, bufPos); + *frsmId = BerDecoder_decodeInt32(buffer, length, bufPos); bufPos += length; break; diff --git a/src/mms/iso_mms/server/mms_file_service.c b/src/mms/iso_mms/server/mms_file_service.c index 9894e9a..ead7571 100644 --- a/src/mms/iso_mms/server/mms_file_service.c +++ b/src/mms/iso_mms/server/mms_file_service.c @@ -213,7 +213,7 @@ mmsMsg_createFileOpenResponse(uint32_t invokeId, ByteBuffer* response, char* ful buffer[bufPos++] = 0xbf; bufPos = BerEncoder_encodeTL(0x48, openFileResponseSize, buffer, bufPos); bufPos = BerEncoder_encodeTL(0x80, frsmIdSize - 2, buffer, bufPos); - bufPos = BerEncoder_encodeUInt32(frsm->frsmId, buffer, bufPos); + bufPos = BerEncoder_encodeInt32(frsm->frsmId, buffer, bufPos); bufPos = encodeFileAttributes(0xa1, frsm->fileSize, gtString, buffer, bufPos); response->size = bufPos; @@ -734,7 +734,7 @@ mmsServer_handleFileReadRequest( uint32_t invokeId, ByteBuffer* response) { - int32_t frsmId = (int32_t) BerDecoder_decodeUint32(buffer, maxBufPos - bufPos, bufPos); + int32_t frsmId = BerDecoder_decodeInt32(buffer, maxBufPos - bufPos, bufPos); if (DEBUG_MMS_SERVER) printf("MMS_SERVER: mmsServer_handleFileReadRequest read request for frsmId: %i\n", frsmId); @@ -760,7 +760,7 @@ mmsServer_handleFileCloseRequest( uint32_t invokeId, ByteBuffer* response) { - int32_t frsmId = (int32_t) BerDecoder_decodeUint32(buffer, maxBufPos - bufPos, bufPos); + int32_t frsmId = BerDecoder_decodeInt32(buffer, maxBufPos - bufPos, bufPos); MmsFileReadStateMachine* frsm = getFrsm(connection, frsmId); @@ -923,7 +923,7 @@ createFileDirectoryResponse(uint32_t invokeId, ByteBuffer* response, int maxPduS bufPos = BerEncoder_encodeTL(0xa1, confirmedResponsePDUSize, buffer, bufPos); bufPos = BerEncoder_encodeTL(0x02, invokeIdSize - 2, buffer, bufPos); - bufPos = BerEncoder_encodeUInt32((uint32_t) invokeId, buffer, bufPos); + bufPos = BerEncoder_encodeUInt32(invokeId, buffer, bufPos); buffer[bufPos++] = 0xbf; bufPos = BerEncoder_encodeTL(0x4d, fileDirectoryResponseSize, buffer, bufPos); diff --git a/src/mms/iso_mms/server/mms_get_namelist_service.c b/src/mms/iso_mms/server/mms_get_namelist_service.c index 4b41f21..676a196 100644 --- a/src/mms/iso_mms/server/mms_get_namelist_service.c +++ b/src/mms/iso_mms/server/mms_get_namelist_service.c @@ -330,7 +330,7 @@ getNamedVariableListAssociationSpecific(MmsServerConnection connection) static void createNameListResponse( MmsServerConnection connection, - int invokeId, + uint32_t invokeId, LinkedList nameList, ByteBuffer* response, char* continueAfter) @@ -406,7 +406,7 @@ createNameListResponse( bufPos = BerEncoder_encodeTL(0xa1, confirmedResponsePDUSize, buffer, bufPos); bufPos = BerEncoder_encodeTL(0x02, invokeIdSize - 2, buffer, bufPos); - bufPos = BerEncoder_encodeUInt32((uint32_t) invokeId, buffer, bufPos); + bufPos = BerEncoder_encodeUInt32(invokeId, buffer, bufPos); bufPos = BerEncoder_encodeTL(0xa1, getNameListSize, buffer, bufPos); bufPos = BerEncoder_encodeTL(0xa0, identifierListSize, buffer, bufPos); diff --git a/src/mms/iso_mms/server/mms_identify_service.c b/src/mms/iso_mms/server/mms_identify_service.c index b8ff70b..0c88007 100644 --- a/src/mms/iso_mms/server/mms_identify_service.c +++ b/src/mms/iso_mms/server/mms_identify_service.c @@ -29,7 +29,7 @@ void mmsServer_handleIdentifyRequest( MmsServerConnection connection, - int invokeId, + uint32_t invokeId, ByteBuffer* response) { int bufPos = 0; diff --git a/src/mms/iso_mms/server/mms_named_variable_list_service.c b/src/mms/iso_mms/server/mms_named_variable_list_service.c index f82dd62..2a48b02 100644 --- a/src/mms/iso_mms/server/mms_named_variable_list_service.c +++ b/src/mms/iso_mms/server/mms_named_variable_list_service.c @@ -252,7 +252,7 @@ createDefineNamedVariableListResponse(uint32_t invokeId, ByteBuffer* response) bufPos = BerEncoder_encodeTL(0xa1, confirmedResponsePDUSize, buffer, bufPos); bufPos = BerEncoder_encodeTL(0x02, invokeIdSize - 2, buffer, bufPos); - bufPos = BerEncoder_encodeUInt32((uint32_t) invokeId, buffer, bufPos); + bufPos = BerEncoder_encodeUInt32(invokeId, buffer, bufPos); bufPos = BerEncoder_encodeTL(0x8b, 0, buffer, bufPos); diff --git a/src/mms/iso_mms/server/mms_server_common.c b/src/mms/iso_mms/server/mms_server_common.c index bfdbedf..e6522bf 100644 --- a/src/mms/iso_mms/server/mms_server_common.c +++ b/src/mms/iso_mms/server/mms_server_common.c @@ -181,7 +181,7 @@ mmsServer_createServiceErrorPduWithServiceSpecificInfo(uint32_t invokeId, ByteBu bufPos = BerEncoder_encodeTL(0xa2, confirmedErrorContentSize, buffer, bufPos); bufPos = BerEncoder_encodeTL(0x80, invokeIdSize - 2, buffer, bufPos); /* invokeID */ - bufPos = BerEncoder_encodeUInt32((uint32_t) invokeId, buffer, bufPos); + bufPos = BerEncoder_encodeUInt32(invokeId, buffer, bufPos); bufPos = BerEncoder_encodeTL(0xa2, serviceErrorContentSize, buffer, bufPos); /* serviceError */ bufPos = BerEncoder_encodeTL(0xa0, 3, buffer, bufPos); /* serviceError */ diff --git a/src/mms/iso_mms/server/mms_status_service.c b/src/mms/iso_mms/server/mms_status_service.c index a547b9f..bccbd34 100644 --- a/src/mms/iso_mms/server/mms_status_service.c +++ b/src/mms/iso_mms/server/mms_status_service.c @@ -31,7 +31,7 @@ mmsServer_handleStatusRequest( MmsServerConnection connection, uint8_t* requestBuffer, int bufPos, - int invokeId, + uint32_t invokeId, ByteBuffer* response) { /* check for extended derivation */