MmsValue* mmsMsg_parseDataElement(Data_t* dataElement) { MmsValue* value = NULL; if (dataElement->present == Data_PR_structure) { value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue)); int componentCount = dataElement->choice.structure->list.count; value->type = MMS_STRUCTURE; value->value.structure.size = componentCount; value->value.structure.components = (MmsValue**) GLOBAL_CALLOC(componentCount, sizeof(MmsValue*)); int i; for (i = 0; i < componentCount; i++) { value->value.structure.components[i] = mmsMsg_parseDataElement(dataElement->choice.structure->list.array[i]); } } else if (dataElement->present == Data_PR_array) { value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue)); int componentCount = dataElement->choice.array->list.count; value->type = MMS_ARRAY; value->value.structure.size = componentCount; value->value.structure.components = (MmsValue**) GLOBAL_CALLOC(componentCount, sizeof(MmsValue*)); int i; for (i = 0; i < componentCount; i++) { value->value.structure.components[i] = mmsMsg_parseDataElement(dataElement->choice.array->list.array[i]); } } else { if (dataElement->present == Data_PR_integer) { Asn1PrimitiveValue* berInteger = BerInteger_createFromBuffer( dataElement->choice.integer.buf, dataElement->choice.integer.size); value = MmsValue_newIntegerFromBerInteger(berInteger); } else if (dataElement->present == Data_PR_unsigned) { Asn1PrimitiveValue* berInteger = BerInteger_createFromBuffer( dataElement->choice.Unsigned.buf, dataElement->choice.Unsigned.size); value = MmsValue_newUnsignedFromBerInteger(berInteger); } else if (dataElement->present == Data_PR_visiblestring) { value = MmsValue_newVisibleStringFromByteArray(dataElement->choice.visiblestring.buf, dataElement->choice.visiblestring.size); } else if (dataElement->present == Data_PR_mMSString) { value = MmsValue_newMmsStringFromByteArray(dataElement->choice.mMSString.buf, dataElement->choice.mMSString.size); } else if (dataElement->present == Data_PR_bitstring) { value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue)); value->type = MMS_BIT_STRING; int size = dataElement->choice.bitstring.size; value->value.bitString.size = (size * 8) - dataElement->choice.bitstring.bits_unused; value->value.bitString.buf = (uint8_t*) GLOBAL_MALLOC(size); memcpy(value->value.bitString.buf, dataElement->choice.bitstring.buf, size); } else if (dataElement->present == Data_PR_floatingpoint) { value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue)); int size = dataElement->choice.floatingpoint.size; value->type = MMS_FLOAT; if (size == 5) { /* FLOAT32 */ value->value.floatingPoint.formatWidth = 32; value->value.floatingPoint.exponentWidth = dataElement->choice.floatingpoint.buf[0]; uint8_t* floatBuf = (dataElement->choice.floatingpoint.buf + 1); value->value.floatingPoint.buf = (uint8_t*) GLOBAL_MALLOC(4); #if (ORDER_LITTLE_ENDIAN == 1) memcpyReverseByteOrder(value->value.floatingPoint.buf, floatBuf, 4); #else memcpy(value->value.floatingPoint.buf, floatBuf, 4); #endif } if (size == 9) { /* FLOAT64 */ value->value.floatingPoint.formatWidth = 64; value->value.floatingPoint.exponentWidth = dataElement->choice.floatingpoint.buf[0]; uint8_t* floatBuf = (dataElement->choice.floatingpoint.buf + 1); value->value.floatingPoint.buf = (uint8_t*) GLOBAL_MALLOC(8); #if (ORDER_LITTLE_ENDIAN == 1) memcpyReverseByteOrder(value->value.floatingPoint.buf, floatBuf, 8); #else memcpy(value->value.floatingPoint.buf, floatBuf, 8); #endif } } else if (dataElement->present == Data_PR_utctime) { value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue)); value->type = MMS_UTC_TIME; memcpy(value->value.utcTime, dataElement->choice.utctime.buf, 8); } else if (dataElement->present == Data_PR_octetstring) { value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue)); value->type = MMS_OCTET_STRING; int size = dataElement->choice.octetstring.size; value->value.octetString.size = size; value->value.octetString.maxSize = size; value->value.octetString.buf = (uint8_t*) GLOBAL_MALLOC(size); memcpy(value->value.octetString.buf, dataElement->choice.octetstring.buf, size); } else if (dataElement->present == Data_PR_binarytime) { int size = dataElement->choice.binarytime.size; if (size <= 6) { value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue)); value->type = MMS_BINARY_TIME; value->value.binaryTime.size = size; memcpy(value->value.binaryTime.buf, dataElement->choice.binarytime.buf, size); } } else if (dataElement->present == Data_PR_boolean) { value = MmsValue_newBoolean(dataElement->choice.boolean); } } return value; }
static MmsValue* parseAllDataUnknownValue(GooseSubscriber self, uint8_t* buffer, int allDataLength, bool isStructure) { int bufPos = 0; int elementLength = 0; int elementIndex = 0; MmsValue* dataSetValues = NULL; while (bufPos < allDataLength) { uint8_t tag = buffer[bufPos++]; bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength); if (bufPos + elementLength > allDataLength) { if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Malformed message: sub element is too large!\n"); goto exit_with_error; } switch (tag) { case 0x80: /* reserved for access result */ break; case 0xa1: /* array */ break; case 0xa2: /* structure */ break; case 0x83: /* boolean */ break; case 0x84: /* BIT STRING */ break; case 0x85: /* integer */ break; case 0x86: /* unsigned integer */ break; case 0x87: /* Float */ break; case 0x89: /* octet string */ break; case 0x8a: /* visible string */ break; case 0x8c: /* binary time */ break; case 0x91: /* Utctime */ break; default: if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: found unknown tag %02x\n", tag); goto exit_with_error; } bufPos += elementLength; elementIndex++; } if (isStructure) dataSetValues = MmsValue_createEmptyStructure(elementIndex); else dataSetValues = MmsValue_createEmptyArray(elementIndex); elementIndex = 0; bufPos = 0; while (bufPos < allDataLength) { uint8_t tag = buffer[bufPos++]; bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, allDataLength); if (bufPos + elementLength > allDataLength) { if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Malformed message: sub element is too large!\n"); goto exit_with_error; } MmsValue* value = NULL; switch (tag) { case 0xa1: /* array */ if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: found array\n"); value = parseAllDataUnknownValue(self, buffer + bufPos, elementLength, false); if (value == NULL) goto exit_with_error; break; case 0xa2: /* structure */ if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: found structure\n"); value = parseAllDataUnknownValue(self, buffer + bufPos, elementLength, true); if (value == NULL) goto exit_with_error; break; case 0x83: /* boolean */ if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: found boolean\n"); value = MmsValue_newBoolean(BerDecoder_decodeBoolean(buffer, bufPos)); break; case 0x84: /* BIT STRING */ { int padding = buffer[bufPos]; int bitStringLength = (8 * (elementLength - 1)) - padding; value = MmsValue_newBitString(bitStringLength); memcpy(value->value.bitString.buf, buffer + bufPos + 1, elementLength - 1); } break; case 0x85: /* integer */ value = MmsValue_newInteger(elementLength * 8); memcpy(value->value.integer->octets, buffer + bufPos, elementLength); value->value.integer->size = elementLength; break; case 0x86: /* unsigned integer */ value = MmsValue_newUnsigned(elementLength * 8); memcpy(value->value.integer->octets, buffer + bufPos, elementLength); value->value.integer->size = elementLength; break; case 0x87: /* Float */ if (elementLength == 9) value = MmsValue_newDouble(BerDecoder_decodeDouble(buffer, bufPos)); else if (elementLength == 5) value = MmsValue_newFloat(BerDecoder_decodeFloat(buffer, bufPos)); break; case 0x89: /* octet string */ value = MmsValue_newOctetString(elementLength, elementLength); memcpy(value->value.octetString.buf, buffer + bufPos, elementLength); break; case 0x8a: /* visible string */ value = MmsValue_newVisibleStringFromByteArray(buffer + bufPos, elementLength); break; case 0x8c: /* binary time */ if (elementLength == 4) value = MmsValue_newBinaryTime(true); else if (elementLength == 6) value = MmsValue_newBinaryTime(false); if ((elementLength == 4) || (elementLength == 6)) memcpy(value->value.binaryTime.buf, buffer + bufPos, elementLength); break; case 0x91: /* Utctime */ if (elementLength == 8) { value = MmsValue_newUtcTime(0); MmsValue_setUtcTimeByBuffer(value, buffer + bufPos); } else if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: UTCTime element is of wrong size!\n"); break; default: if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: found unkown tag %02x\n", tag); goto exit_with_error; } bufPos += elementLength; if (value != NULL) { MmsValue_setElement(dataSetValues, elementIndex, value); elementIndex++; } } self->dataSetValuesSelfAllocated = true; return dataSetValues; exit_with_error: if (dataSetValues != NULL) MmsValue_delete(dataSetValues); return NULL; }