static bool
parseInitiateRequestPdu(MmsServerConnection* self, uint8_t* buffer, int bufPos, int maxBufPos)
{
self->maxPduSize = MMS_MAXIMUM_PDU_SIZE;
self->dataStructureNestingLevel =
DEFAULT_DATA_STRUCTURE_NESTING_LEVEL;
self->maxServOutstandingCalled = DEFAULT_MAX_SERV_OUTSTANDING_CALLED;
self->maxServOutstandingCalling = DEFAULT_MAX_SERV_OUTSTANDING_CALLING;
while (bufPos < maxBufPos) {
uint8_t tag = buffer[bufPos++];
uint32_t length;
bufPos = BerDecoder_decodeLength(buffer, (int*) &length, bufPos, maxBufPos);
if (bufPos < 0) {
// TODO write initiate error PDU!
return false;
}
switch (tag) {
case 0x80: /* local-detail-calling */
self->maxPduSize = BerDecoder_decodeUint32(buffer, length, bufPos);
if (self->maxPduSize > MMS_MAXIMUM_PDU_SIZE)
self->maxPduSize = MMS_MAXIMUM_PDU_SIZE;
break;
case 0x81: /* proposed-max-serv-outstanding-calling */
self->maxServOutstandingCalling = BerDecoder_decodeUint32(buffer, length, bufPos);
break;
case 0x82: /* proposed-max-serv-outstanding-called */
self->maxServOutstandingCalled = BerDecoder_decodeUint32(buffer, length, bufPos);
break;
case 0x83: /* proposed-data-structure-nesting-level */
self->dataStructureNestingLevel = BerDecoder_decodeUint32(buffer, length, bufPos);
break;
case 0xa4: /* mms-init-request-detail */
/* we ignore this */
break;
}
bufPos += length;
}
return true;
}
static inline MmsIndication
parseMmsPdu(MmsServerConnection* self, ByteBuffer* message, ByteBuffer* response)
{
MmsIndication retVal;
uint8_t* buffer = message->buffer;
if (message->size < 2)
return MMS_ERROR;
int bufPos = 0;
uint8_t pduType = buffer[bufPos++];
int pduLength;
bufPos = BerDecoder_decodeLength(buffer, &pduLength, bufPos, message->size);
if (bufPos < 0)
return MMS_ERROR;
if (DEBUG) printf("mms_server: recvd MMS-PDU type: %02x size: %u\n", pduType, pduLength);
switch (pduType) {
case 0xa8: /* Initiate request PDU */
handleInitiateRequestPdu(self, buffer, bufPos, bufPos + pduLength, response);
retVal = MMS_INITIATE;
break;
case 0xa0: /* Confirmed request PDU */
handleConfirmedRequestPdu(self, buffer, bufPos, bufPos + pduLength, response);
retVal = MMS_CONFIRMED_REQUEST;
break;
case 0x8b: /* Conclude request PDU */
mmsServer_writeConcludeResponsePdu(response);
IsoConnection_close(self->isoConnection);
retVal = MMS_CONCLUDE;
break;
case 0xa4: /* Reject PDU */
//TODO evaluate reject PDU
printf("received reject PDU!\n");
/* silently ignore */
retVal = MMS_OK;
break;
default:
mmsServer_writeMmsRejectPdu(NULL, MMS_REJECT_UNKNOWN_PDU_TYPE, response);
retVal = MMS_ERROR;
break;
}
parseMmsPdu_exit:
return retVal;
}
void
mmsClient_parseWriteResponse(ByteBuffer* message, int32_t bufPos, MmsError* mmsError)
{
uint8_t* buf = message->buffer;
int size = message->size;
int length;
*mmsError = MMS_ERROR_NONE;
uint8_t tag = buf[bufPos++];
if (tag == 0xa5) {
bufPos = BerDecoder_decodeLength(buf, &length, bufPos, size);
if (bufPos == -1) {
*mmsError = MMS_ERROR_PARSING_RESPONSE;
return;
}
tag = buf[bufPos++];
if (tag == 0x81)
return;
if (tag == 0x80) {
bufPos = BerDecoder_decodeLength(buf, &length, bufPos, size);
uint32_t dataAccessErrorCode =
BerDecoder_decodeUint32(buf, length, bufPos);
*mmsError = mapDataAccessErrorToMmsError(dataAccessErrorCode);
}
}
else
*mmsError = MMS_ERROR_PARSING_RESPONSE;
}
bool
mmsClient_parseGetNameListResponse(LinkedList* nameList, ByteBuffer* message, uint32_t* invokeId)
{
bool moreFollows = true;
uint8_t* buffer = message->buffer;
int maxBufPos = message->size;
int bufPos = 0;
int length;
uint8_t tag = buffer[bufPos++];
if (tag == 0xa2) {
// TODO parse confirmed error PDU
goto exit_error;
}
if (tag != 0xa1) goto exit_error;
bufPos = BerDecoder_decodeLength(buffer, &length, bufPos, maxBufPos);
if (bufPos < 0) goto exit_error;
/* get invokeId */
tag = buffer[bufPos++];
if (tag != 0x02) goto exit_error;
bufPos = BerDecoder_decodeLength(buffer, &length, bufPos, maxBufPos);
if (bufPos < 0) goto exit_error;
if (invokeId != NULL)
*invokeId = BerDecoder_decodeUint32(buffer, length, bufPos);
bufPos += length;
tag = buffer[bufPos++];
if (tag != 0xa1) goto exit_error;
bufPos = BerDecoder_decodeLength(buffer, &length, bufPos, maxBufPos);
if (bufPos < 0) goto exit_error;
tag = buffer[bufPos++];
if (tag != 0xa0) goto exit_error;
bufPos = BerDecoder_decodeLength(buffer, &length, bufPos, maxBufPos);
if (bufPos < 0) goto exit_error;
int listEndPos = bufPos + length;
if (listEndPos > maxBufPos) goto exit_error;
if (*nameList == NULL)
*nameList = LinkedList_create();
LinkedList element = LinkedList_getLastElement(*nameList);
while (bufPos < listEndPos) {
tag = buffer[bufPos++];
if (tag != 0x1a) goto exit_error;
bufPos = BerDecoder_decodeLength(buffer, &length, bufPos, maxBufPos);
if (bufPos < 0) goto exit_error;
char* variableName = createStringFromBuffer(buffer + bufPos, length);
element = LinkedList_insertAfter(element, variableName);
bufPos += length;
}
if (bufPos < maxBufPos) {
tag = buffer[bufPos++];
if (tag != 0x81) goto exit_error;
bufPos = BerDecoder_decodeLength(buffer, &length, bufPos, maxBufPos);
if (bufPos < 0) goto exit_error;
if (length != 1) goto exit_error;
if (buffer[bufPos++] > 0)
moreFollows = true;
else
moreFollows = false;
}
return moreFollows;
exit_error:
if (*nameList != NULL) {
LinkedList_destroy(*nameList);
}
if (DEBUG) printf("parseNameListResponse: error parsing message!\n");
return false;
}
static void
handleConfirmedRequestPdu(
MmsServerConnection* self,
uint8_t* buffer, int bufPos, int maxBufPos,
ByteBuffer* response)
{
uint32_t invokeId = 0;
if (DEBUG) printf("invokeId: %i\n", invokeId);
while (bufPos < maxBufPos) {
uint8_t tag = buffer[bufPos++];
uint32_t length;
bufPos = BerDecoder_decodeLength(buffer, (int*) &length, bufPos, maxBufPos);
if (bufPos < 0) {
writeMmsRejectPdu((int*) &invokeId, REJECT_UNRECOGNIZED_SERVICE, response);
return;
}
if (DEBUG) printf("tag %02x size: %i\n", tag, length);
switch(tag) {
case 0x02: /* invoke Id */
invokeId = BerDecoder_decodeUint32(buffer, length, bufPos);
break;
case 0xa1: /* get-name-list-request */
mmsServer_handleGetNameListRequest(self, buffer, bufPos, bufPos + length,
invokeId, response);
break;
case 0xa4: /* read-request */
mmsServer_handleReadRequest(self, buffer, bufPos, bufPos + length,
invokeId, response);
break;
case 0xa5: /* write-request */
mmsServer_handleWriteRequest(self, buffer, bufPos, bufPos + length,
invokeId, response);
break;
case 0xa6: /* get-variable-access-attributes-request */
mmsServer_handleGetVariableAccessAttributesRequest(self,
buffer, bufPos, bufPos + length,
invokeId, response);
break;
case 0xab: /* define-named-variable-list */
mmsServer_handleDefineNamedVariableListRequest(self,
buffer, bufPos, bufPos + length,
invokeId, response);
break;
case 0xac: /* get-named-variable-list-attributes-request */
mmsServer_handleGetNamedVariableListAttributesRequest(self,
buffer, bufPos, bufPos + length,
invokeId, response);
break;
case 0xad: /* delete-named-variable-list-request */
mmsServer_handleDeleteNamedVariableListRequest(self,
buffer, bufPos, bufPos + length,
invokeId, response);
break;
default:
writeMmsRejectPdu((int*) &invokeId, REJECT_UNRECOGNIZED_SERVICE, response);
return;
break;
}
bufPos += length;
}
}
MmsServerIdentity*
mmsClient_parseIdentifyResponse(MmsConnection self)
{
uint8_t* buffer = self->lastResponse->buffer;
int maxBufPos = self->lastResponse->size;
int bufPos = self->lastResponseBufPos;
int length;
MmsServerIdentity* identityInfo = NULL;
uint8_t tag = buffer[bufPos++];
if (tag != 0xa2)
goto exit_error;
bufPos = BerDecoder_decodeLength(buffer, &length, bufPos, maxBufPos);
if (bufPos < 0) goto exit_error;
int endPos = bufPos + length;
if (endPos > maxBufPos) {
if (DEBUG_MMS_CLIENT)
printf("mmsClient_parseIdentifyResponse: Message to short!\n");
goto exit_error;
}
char* vendorName = NULL;
char* modelName = NULL;
char* revision = NULL;
while (bufPos < endPos) {
tag = buffer[bufPos++];
bufPos = BerDecoder_decodeLength(buffer, &length, bufPos, maxBufPos);
switch (tag) {
case 0x80: /* vendorName */
vendorName = createStringFromBuffer(buffer + bufPos, length);
bufPos += length;
break;
case 0x81: /* modelName */
modelName = createStringFromBuffer(buffer + bufPos, length);
bufPos += length;
break;
case 0x82: /* revision */
revision = createStringFromBuffer(buffer + bufPos, length);
bufPos += length;
break;
case 0x83: /* list of abstract syntaxes */
bufPos += length;
break;
}
}
identityInfo = (MmsServerIdentity*) malloc(sizeof(MmsServerIdentity));
identityInfo->vendorName = vendorName;
identityInfo->modelName = modelName;
identityInfo->revision = revision;
exit_error:
return identityInfo;
}
static int
parseGoosePayload(GooseReceiver self, uint8_t* buffer, int apduLength)
{
int bufPos = 0;
uint32_t timeAllowedToLive = 0;
uint32_t stNum = 0;
uint32_t sqNum = 0;
uint32_t confRev;
bool simulation = false;
bool ndsCom = false;
GooseSubscriber matchingSubscriber = NULL;
uint8_t* timestampBufPos = NULL;
uint8_t* dataSetBufferAddress = NULL;
int dataSetBufferLength = 0;
uint32_t numberOfDatSetEntries = 0;
if (buffer[bufPos++] == 0x61) {
int gooseLength;
bufPos = BerDecoder_decodeLength(buffer, &gooseLength, bufPos, apduLength);
int gooseEnd = bufPos + gooseLength;
while (bufPos < gooseEnd) {
int elementLength;
uint8_t tag = buffer[bufPos++];
bufPos = BerDecoder_decodeLength(buffer, &elementLength, bufPos, apduLength);
if (bufPos + elementLength > apduLength) {
if (DEBUG_GOOSE_SUBSCRIBER)
printf("GOOSE_SUBSCRIBER: Malformed message: sub element is too large!\n");
goto exit_with_fault;
}
if (bufPos == -1)
goto exit_with_fault;
switch(tag) {
case 0x80: /* gocbRef */
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found gocbRef\n");
{
LinkedList element = LinkedList_getNext(self->subscriberList);
while (element != NULL) {
GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
if (subscriber->goCBRefLen == elementLength) {
if (memcmp(subscriber->goCBRef, buffer + bufPos, elementLength) == 0) {
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: gocbRef is matching!\n");
matchingSubscriber = subscriber;
break;
}
}
element = LinkedList_getNext(element);
}
if (matchingSubscriber == NULL)
return 0;
}
break;
case 0x81: /* timeAllowedToLive */
timeAllowedToLive = BerDecoder_decodeUint32(buffer, elementLength, bufPos);
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found timeAllowedToLive %u\n", timeAllowedToLive);
break;
case 0x82:
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found dataSet\n");
break;
case 0x83:
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found goId\n");
break;
case 0x84:
timestampBufPos = buffer + bufPos;
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found timestamp\n");
break;
case 0x85:
stNum = BerDecoder_decodeUint32(buffer, elementLength, bufPos);
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found stNum: %u\n", stNum);
break;
case 0x86:
sqNum = BerDecoder_decodeUint32(buffer, elementLength, bufPos);
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found sqNum: %u\n", sqNum);
break;
case 0x87:
simulation = BerDecoder_decodeBoolean(buffer, bufPos);
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found simulation: %i\n", simulation);
break;
case 0x88:
confRev = BerDecoder_decodeUint32(buffer, elementLength, bufPos);
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found confRev: %u\n", confRev);
break;
case 0x89:
ndsCom = BerDecoder_decodeBoolean(buffer, bufPos);
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found ndsCom: %i\n", ndsCom);
break;
case 0x8a:
numberOfDatSetEntries = BerDecoder_decodeUint32(buffer, elementLength, bufPos);
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found number of entries: %u\n", numberOfDatSetEntries);
break;
case 0xab:
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Found all data with length: %i\n", elementLength);
dataSetBufferAddress = buffer + bufPos;
dataSetBufferLength = elementLength;
break;
default:
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Unknown tag %02x\n", tag);
break;
}
bufPos += elementLength;
}
if (matchingSubscriber != NULL) {
matchingSubscriber->timeAllowedToLive = timeAllowedToLive;
matchingSubscriber->confRev = confRev;
matchingSubscriber->ndsCom = ndsCom;
matchingSubscriber->simulation = simulation;
MmsValue_setUtcTimeByBuffer(matchingSubscriber->timestamp, timestampBufPos);
if (matchingSubscriber->dataSetValues == NULL)
matchingSubscriber->dataSetValues = parseAllDataUnknownValue(matchingSubscriber, dataSetBufferAddress, dataSetBufferLength, false);
else
parseAllData(dataSetBufferAddress, dataSetBufferLength, matchingSubscriber->dataSetValues);
bool isValid = true;
if (matchingSubscriber->stNum == stNum) {
if (matchingSubscriber->sqNum >= sqNum) {
isValid = false;
}
}
matchingSubscriber->stateValid = isValid;
matchingSubscriber->stNum = stNum;
matchingSubscriber->sqNum = sqNum;
matchingSubscriber->invalidityTime = Hal_getTimeInMs() + timeAllowedToLive;
if (matchingSubscriber->listener != NULL)
matchingSubscriber->listener(matchingSubscriber, matchingSubscriber->listenerParameter);
return 1;
}
return 0;
}
exit_with_fault:
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Invalid goose payload\n");
return -1;
}
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;
}
static int
parseAllData(uint8_t* buffer, int allDataLength, MmsValue* dataSetValues)
{
int bufPos = 0;
int elementLength = 0;
int elementIndex = 0;
int maxIndex = MmsValue_getArraySize(dataSetValues) - 1;
while (bufPos < allDataLength) {
uint8_t tag = buffer[bufPos++];
if (elementIndex > maxIndex) {
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: Malformed message: too much elements!\n");
return 0;
}
MmsValue* value = MmsValue_getElement(dataSetValues, elementIndex);
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");
return 0;
}
switch (tag) {
case 0x80: /* reserved for access result */
printf("GOOSE_SUBSCRIBER: found reserved value (tag 0x80)!\n");
break;
case 0xa1: /* array */
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: found array\n");
if (MmsValue_getType(value) == MMS_ARRAY) {
if (!parseAllData(buffer + bufPos, elementLength, value))
return -1;
}
break;
case 0xa2: /* structure */
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: found structure\n");
if (MmsValue_getType(value) == MMS_STRUCTURE) {
if (!parseAllData(buffer + bufPos, elementLength, value))
return -1;
}
break;
case 0x83: /* boolean */
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: found boolean\n");
if (MmsValue_getType(value) == MMS_BOOLEAN) {
MmsValue_setBoolean(value, BerDecoder_decodeBoolean(buffer, bufPos));
}
else
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: message contains value of wrong type!\n");
break;
case 0x84: /* BIT STRING */
if (MmsValue_getType(value) == MMS_BIT_STRING) {
int padding = buffer[bufPos];
int bitStringLength = (8 * (elementLength - 1)) - padding;
if (bitStringLength == value->value.bitString.size) {
memcpy(value->value.bitString.buf, buffer + bufPos + 1,
elementLength - 1);
}
else
if (DEBUG_GOOSE_SUBSCRIBER)
printf("bit-string is of wrong size");
}
break;
case 0x85: /* integer */
if (MmsValue_getType(value) == MMS_INTEGER) {
if (elementLength <= value->value.integer->maxSize) {
value->value.integer->size = elementLength;
memcpy(value->value.integer->octets, buffer + bufPos, elementLength);
}
}
break;
case 0x86: /* unsigned integer */
if (MmsValue_getType(value) == MMS_UNSIGNED) {
if (elementLength <= value->value.integer->maxSize) {
value->value.integer->size = elementLength;
memcpy(value->value.integer->octets, buffer + bufPos, elementLength);
}
}
break;
case 0x87: /* Float */
if (MmsValue_getType(value) == MMS_FLOAT) {
if (elementLength == 9) {
MmsValue_setDouble(value, BerDecoder_decodeDouble(buffer, bufPos));
}
else if (elementLength == 5) {
MmsValue_setFloat(value, BerDecoder_decodeFloat(buffer, bufPos));
}
}
break;
case 0x89: /* octet string */
if (MmsValue_getType(value) == MMS_OCTET_STRING) {
if (elementLength <= value->value.octetString.maxSize) {
value->value.octetString.size = elementLength;
memcpy(value->value.octetString.buf, buffer + bufPos, elementLength);
}
}
break;
case 0x8a: /* visible string */
if (MmsValue_getType(value) == MMS_VISIBLE_STRING) {
if (value->value.visibleString.buf != NULL) {
if ((int32_t) value->value.visibleString.size >= elementLength) {
memcpy(value->value.visibleString.buf, buffer + bufPos, elementLength);
value->value.visibleString.buf[elementLength] = 0;
}
else {
GLOBAL_FREEMEM(value->value.visibleString.buf);
createNewStringFromBufferElement(value, buffer + bufPos, elementLength);
}
}
else
createNewStringFromBufferElement(value, buffer + bufPos, elementLength);
}
break;
case 0x8c: /* binary time */
if (MmsValue_getType(value) == MMS_BINARY_TIME) {
if ((elementLength == 4) || (elementLength == 6)) {
memcpy(value->value.binaryTime.buf, buffer + bufPos, elementLength);
}
}
break;
case 0x91: /* Utctime */
if (elementLength == 8) {
if (MmsValue_getType(value) == MMS_UTC_TIME) {
MmsValue_setUtcTimeByBuffer(value, buffer + bufPos);
}
else
if (DEBUG_GOOSE_SUBSCRIBER) printf("GOOSE_SUBSCRIBER: message contains value of wrong type!\n");
}
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);
break;
}
bufPos += elementLength;
elementIndex++;
}
return 1;
}
void
mmsClient_parseWriteMultipleItemsResponse(ByteBuffer* message, int32_t bufPos, MmsError* mmsError,
int itemCount, LinkedList* accessResults)
{
uint8_t* buf = message->buffer;
int size = message->size;
int length;
*mmsError = MMS_ERROR_NONE;
uint8_t tag = buf[bufPos++];
if (tag == 0xa5) {
bufPos = BerDecoder_decodeLength(buf, &length, bufPos, size);
if (bufPos == -1) {
*mmsError = MMS_ERROR_PARSING_RESPONSE;
return;
}
*accessResults = LinkedList_create();
int endPos = bufPos + length;
int numberOfAccessResults = 0;
while (bufPos < endPos) {
tag = buf[bufPos++];
bufPos = BerDecoder_decodeLength(buf, &length, bufPos, size);
if (bufPos == -1) goto exit_with_error;
if (tag == 0x81) {
MmsValue* value = MmsValue_newDataAccessError(DATA_ACCESS_ERROR_SUCCESS);
LinkedList_add(*accessResults, (void*) value);
}
if (tag == 0x80) {
uint32_t dataAccessErrorCode =
BerDecoder_decodeUint32(buf, length, bufPos);
MmsValue* value = MmsValue_newDataAccessError((MmsDataAccessError) dataAccessErrorCode);
LinkedList_add(*accessResults, (void*) value);
}
bufPos += length;
numberOfAccessResults++;
}
if (itemCount != numberOfAccessResults)
goto exit_with_error;
}
else
*mmsError = MMS_ERROR_PARSING_RESPONSE;
return;
exit_with_error:
*mmsError = MMS_ERROR_PARSING_RESPONSE;
LinkedList_destroyDeep(*accessResults, (LinkedListValueDeleteFunction) MmsValue_delete);
}
