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;
}
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;
}
}
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;
}
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);
}