void
IsoServer_destroy(IsoServer self)
{
#if (CONFIG_MMS_THREADLESS_STACK != 1)
if (self->state == ISO_SVR_STATE_RUNNING)
IsoServer_stopListening(self);
#endif
#if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1)
#if (CONFIG_MMS_SINGLE_THREADED == 1)
if (self->openClientConnections != NULL)
LinkedList_destroy(self->openClientConnections);
#else
if (self->openClientConnections != NULL)
LinkedList_destroyStatic(self->openClientConnections);
#endif /* (CONFIG_MMS_SINGLE_THREADED == 1) */
#if (CONFIG_MMS_THREADLESS_STACK != 1)
lockClientConnections(self);
Semaphore_destroy(self->openClientConnectionsMutex);
#endif
#else
GLOBAL_FREEMEM(self->openClientConnections);
#endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
#if (CONFIG_MMS_THREADLESS_STACK != 1)
Semaphore_destroy(self->connectionCounterMutex);
#endif
GLOBAL_FREEMEM(self);
}
static void
freeInitiateRequestPdu(InitiateRequestPdu_t request)
{
GLOBAL_FREEMEM(request.localDetailCalling);
GLOBAL_FREEMEM(request.proposedDataStructureNestingLevel);
GLOBAL_FREEMEM(request.mmsInitRequestDetail.proposedParameterCBB.buf);
}
void
GooseReceiver_destroy(GooseReceiver self)
{
LinkedList_destroyDeep(self->subscriberList,
(LinkedListValueDeleteFunction) GooseSubscriber_destroy);
GLOBAL_FREEMEM(self->buffer);
GLOBAL_FREEMEM(self);
}
void Ethernet_destroySocket(EthernetSocket self)
{
// Close the BPF device.
close(self->bpf);
// Free all dynamic resources used by the ethernet socket.
GLOBAL_FREEMEM(self->bpfBuffer);
GLOBAL_FREEMEM(self->bpfProgram.bf_insns);
GLOBAL_FREEMEM(self);
}
/**
* Request a single value
*/
int
mmsClient_createReadRequest(uint32_t invokeId, const char* domainId, const char* itemId, ByteBuffer* writeBuffer)
{
MmsPdu_t* mmsPdu = mmsClient_createConfirmedRequestPdu(invokeId);
ReadRequest_t* readRequest = createReadRequest(mmsPdu);
readRequest->specificationWithResult = NULL;
readRequest->variableAccessSpecification.present = VariableAccessSpecification_PR_listOfVariable;
readRequest->variableAccessSpecification.choice.listOfVariable.list.array =
(ListOfVariableSeq_t**) GLOBAL_CALLOC(1, sizeof(ListOfVariableSeq_t*));
readRequest->variableAccessSpecification.choice.listOfVariable.list.count = 1;
ListOfVariableSeq_t* listOfVars = (ListOfVariableSeq_t*) GLOBAL_CALLOC(1, sizeof(ListOfVariableSeq_t));
readRequest->variableAccessSpecification.choice.listOfVariable.list.array[0] = listOfVars;
listOfVars->alternateAccess = NULL;
listOfVars->variableSpecification.present = VariableSpecification_PR_name;
if (domainId != NULL) {
listOfVars->variableSpecification.choice.name.present = ObjectName_PR_domainspecific;
listOfVars->variableSpecification.choice.name.choice.domainspecific.domainId.buf = (uint8_t*) domainId;
listOfVars->variableSpecification.choice.name.choice.domainspecific.domainId.size = strlen(domainId);
listOfVars->variableSpecification.choice.name.choice.domainspecific.itemId.buf = (uint8_t*) itemId;
listOfVars->variableSpecification.choice.name.choice.domainspecific.itemId.size = strlen(itemId);
}
else {
listOfVars->variableSpecification.choice.name.present = ObjectName_PR_vmdspecific;
listOfVars->variableSpecification.choice.name.choice.vmdspecific.buf = (uint8_t*) itemId;
listOfVars->variableSpecification.choice.name.choice.vmdspecific.size = strlen(itemId);
}
asn_enc_rval_t rval;
rval = der_encode(&asn_DEF_MmsPdu, mmsPdu,
(asn_app_consume_bytes_f*) mmsClient_write_out, (void*) writeBuffer);
/* clean up data structures */
GLOBAL_FREEMEM(listOfVars);
GLOBAL_FREEMEM(readRequest->variableAccessSpecification.choice.listOfVariable.list.array);
readRequest->variableAccessSpecification.choice.listOfVariable.list.array = NULL;
readRequest->variableAccessSpecification.choice.listOfVariable.list.count = 0;
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
return rval.encoded;
}
int
mmsClient_createWriteRequest(uint32_t invokeId, const char* domainId, const char* itemId, MmsValue* value,
ByteBuffer* writeBuffer)
{
MmsPdu_t* mmsPdu = mmsClient_createConfirmedRequestPdu(invokeId);
mmsPdu->choice.confirmedRequestPdu.confirmedServiceRequest.present =
ConfirmedServiceRequest_PR_write;
WriteRequest_t* request =
&(mmsPdu->choice.confirmedRequestPdu.confirmedServiceRequest.choice.write);
/* Create list of variable specifications */
request->variableAccessSpecification.present = VariableAccessSpecification_PR_listOfVariable;
request->variableAccessSpecification.choice.listOfVariable.list.count = 1;
request->variableAccessSpecification.choice.listOfVariable.list.size = 1;
request->variableAccessSpecification.choice.listOfVariable.list.array =
(ListOfVariableSeq_t**) GLOBAL_CALLOC(1, sizeof(ListOfVariableSeq_t*));
request->variableAccessSpecification.choice.listOfVariable.list.array[0] = (ListOfVariableSeq_t*)
createNewDomainVariableSpecification(domainId, itemId);
/* Create list of typed data values */
request->listOfData.list.count = 1;
request->listOfData.list.size = 1;
request->listOfData.list.array = (Data_t**) GLOBAL_CALLOC(1, sizeof(struct Data*));
request->listOfData.list.array[0] = mmsMsg_createBasicDataElement(value);
asn_enc_rval_t rval;
rval = der_encode(&asn_DEF_MmsPdu, mmsPdu,
(asn_app_consume_bytes_f*) mmsClient_write_out, (void*) writeBuffer);
/* Free ASN structure */
request->variableAccessSpecification.choice.listOfVariable.list.count = 0;
GLOBAL_FREEMEM(request->variableAccessSpecification.choice.listOfVariable.list.array[0]);
GLOBAL_FREEMEM(request->variableAccessSpecification.choice.listOfVariable.list.array);
request->variableAccessSpecification.choice.listOfVariable.list.array = 0;
request->listOfData.list.count = 0;
deleteDataElement(request->listOfData.list.array[0]);
GLOBAL_FREEMEM(request->listOfData.list.array);
request->listOfData.list.array = 0;
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
return rval.encoded;
}
MmsValue*
MmsValueCache_lookupValue(MmsValueCache self, char* itemId)
{
// get value for first matching key substring!
// Then iterate the value for the exact value.
MmsValue* value = NULL;
MmsValueCacheEntry* cacheEntry;
cacheEntry = (MmsValueCacheEntry*) Map_getEntry(self->map, itemId);
if (cacheEntry == NULL) {
char* itemIdCopy = copyString(itemId);
char* parentItemId = getParentSubString(itemIdCopy);
if (parentItemId != NULL) {
value = searchCacheForValue(self, itemId, parentItemId);
}
GLOBAL_FREEMEM(itemIdCopy);
}
if (cacheEntry != NULL)
return cacheEntry->value;
else
return value;
}
void
GooseReceiver_setInterfaceId(GooseReceiver self, const char* interfaceId)
{
if (self->interfaceId != NULL)
GLOBAL_FREEMEM(self->interfaceId);
self->interfaceId = copyString(interfaceId);
}
static void
cacheEntryDelete(MmsValueCacheEntry* entry)
{
if (entry != NULL) {
MmsValue_delete(entry->value);
GLOBAL_FREEMEM(entry);
}
}
void
ClientGooseControlBlock_destroy(ClientGooseControlBlock self)
{
GLOBAL_FREEMEM(self->objectReference);
MmsValue_deleteIfNotNull(self->goEna);
MmsValue_deleteIfNotNull(self->goID);
MmsValue_deleteIfNotNull(self->datSet);
MmsValue_deleteIfNotNull(self->confRev);
MmsValue_deleteIfNotNull(self->ndsCom);
MmsValue_deleteIfNotNull(self->dstAddress);
MmsValue_deleteIfNotNull(self->minTime);
MmsValue_deleteIfNotNull(self->maxTime);
MmsValue_deleteIfNotNull(self->fixedOffs);
GLOBAL_FREEMEM(self);
}
static void
deleteVariableAccessAttributesResponse(
GetVariableAccessAttributesResponse_t* getVarAccessAttr)
{
if (getVarAccessAttr->typeSpecification.present == TypeSpecification_PR_structure) {
int count = getVarAccessAttr->typeSpecification.choice.structure.components.list.count;
int i;
for (i = 0; i < count; i++) {
GLOBAL_FREEMEM(getVarAccessAttr->typeSpecification.choice.structure.components.list.array[i]->componentName->buf);
GLOBAL_FREEMEM(getVarAccessAttr->typeSpecification.choice.structure.components.list.array[i]->componentName);
TypeSpecification_t* typeSpec =
getVarAccessAttr->typeSpecification.choice.structure.components.list.array[i]->componentType;
freeTypeSpecRecursive(typeSpec);
GLOBAL_FREEMEM(typeSpec);
GLOBAL_FREEMEM(getVarAccessAttr->typeSpecification.choice.structure.components.list.array[i]);
}
GLOBAL_FREEMEM(getVarAccessAttr->typeSpecification.choice.structure.components.list.array);
getVarAccessAttr->typeSpecification.choice.structure.components.list.array = NULL;
getVarAccessAttr->typeSpecification.choice.structure.components.list.count = 0;
getVarAccessAttr->typeSpecification.choice.structure.components.list.size = 0;
} else if (getVarAccessAttr->typeSpecification.present == TypeSpecification_PR_array) {
GLOBAL_FREEMEM(getVarAccessAttr->typeSpecification.choice.array.numberOfElements.buf);
getVarAccessAttr->typeSpecification.choice.array.numberOfElements.buf = NULL;
getVarAccessAttr->typeSpecification.choice.array.numberOfElements.size = 0;
freeTypeSpecRecursive(getVarAccessAttr->typeSpecification.choice.array.elementType);
GLOBAL_FREEMEM(getVarAccessAttr->typeSpecification.choice.array.elementType);
getVarAccessAttr->typeSpecification.choice.array.elementType = NULL;
}
}
void
ControlObjectClient_setOrigin(ControlObjectClient self, const char* orIdent, int orCat)
{
if (self->orIdent != NULL)
GLOBAL_FREEMEM(self->orIdent);
self->orIdent = copyString(orIdent);
self->orCat = orCat;
}
void
ControlObjectClient_destroy(ControlObjectClient self)
{
if (self != NULL)
{
GLOBAL_FREEMEM(self->objectReference);
private_IedConnection_removeControlClient(self->connection, self);
if (self->ctlVal != NULL)
MmsValue_delete(self->ctlVal);
if (self->orIdent != NULL)
GLOBAL_FREEMEM(self->orIdent);
GLOBAL_FREEMEM(self);
}
}
/**
* Request multiple values of a single domain
*/
int
mmsClient_createReadRequestMultipleValues(uint32_t invokeId, const char* domainId, LinkedList items,
ByteBuffer* writeBuffer)
{
MmsPdu_t* mmsPdu = mmsClient_createConfirmedRequestPdu(invokeId);
ReadRequest_t* readRequest = createReadRequest(mmsPdu);
readRequest->specificationWithResult = NULL;
int valuesCount = LinkedList_size(items);
ListOfVariableSeq_t** listOfVars = createListOfVariables(readRequest, valuesCount);
LinkedList item = items;
int i = 0;
while ((item = LinkedList_getNext(item)) != NULL) {
listOfVars[i] = createVariableIdentifier(domainId, (char*) (item->data));
i++;
}
asn_enc_rval_t rval;
rval = der_encode(&asn_DEF_MmsPdu, mmsPdu,
(asn_app_consume_bytes_f*) mmsClient_write_out, (void*) writeBuffer);
for (i = 0; i < valuesCount; i++) {
GLOBAL_FREEMEM(listOfVars[i]);
}
GLOBAL_FREEMEM(listOfVars);
readRequest->variableAccessSpecification.choice.listOfVariable.list.count = 0;
readRequest->variableAccessSpecification.choice.listOfVariable.list.size = 0;
readRequest->variableAccessSpecification.choice.listOfVariable.list.array = NULL;
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
return rval.encoded;
}
void
BufferChain_destroy(BufferChain self)
{
BufferChain currentChainElement = self;
while (currentChainElement != NULL) {
BufferChain nextChainElement = currentChainElement->nextPart;
GLOBAL_FREEMEM(currentChainElement);
currentChainElement = nextChainElement;
}
}
static void
handleClientConnections(IsoServer self)
{
#if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1)
#if (CONFIG_MMS_THREADLESS_STACK != 1)
lockClientConnections(self);
#endif
LinkedList openConnection = LinkedList_getNext(self->openClientConnections);
LinkedList lastConnection = self->openClientConnections;
while (openConnection != NULL) {
IsoConnection isoConnection = (IsoConnection) openConnection->data;
if (IsoConnection_isRunning(isoConnection))
IsoConnection_handleTcpConnection(isoConnection);
else {
IsoConnection_destroy(isoConnection);
lastConnection->next = openConnection->next;
GLOBAL_FREEMEM(openConnection);
}
openConnection = LinkedList_getNext(openConnection);
}
#if (CONFIG_MMS_THREADLESS_STACK != 1)
unlockClientConnections(self);
#endif
#else
int i;
for (i = 0; i < CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS; i++) {
if (self->openClientConnections[i] != NULL) {
if (IsoConnection_isRunning(self->openClientConnections[i])) {
IsoConnection_handleTcpConnection(self->openClientConnections[i]);
}
else {
IsoConnection_destroy(self->openClientConnections[i]);
self->openClientConnections[i] = NULL;
}
}
}
#endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
}
void
ControlObject_destroy(ControlObject* self)
{
if (self->mmsValue != NULL)
MmsValue_delete(self->mmsValue);
if (self->sbo != NULL)
MmsValue_delete(self->sbo);
if (self->emptyString != NULL)
MmsValue_delete(self->emptyString);
if (self->ctlObjectName != NULL)
GLOBAL_FREEMEM(self->ctlObjectName);
if (self->error != NULL)
MmsValue_delete(self->error);
if (self->addCause != NULL)
MmsValue_delete(self->addCause);
if (self->ctlVal != NULL)
MmsValue_delete(self->ctlVal);
if (self->ctlNum != NULL)
MmsValue_delete(self->ctlNum);
if (self->origin != NULL)
MmsValue_delete(self->origin);
if (self->name != NULL)
GLOBAL_FREEMEM(self->name);
#if (CONFIG_MMS_THREADLESS_STACK != 1)
if (self->stateLock != NULL)
Semaphore_destroy(self->stateLock);
#endif
GLOBAL_FREEMEM(self);
}
void
mmsMsg_deleteAccessResultList(AccessResult_t** accessResult, int variableCount)
{
int i;
for (i = 0; i < variableCount; i++) {
if (accessResult[i]->present == AccessResult_PR_structure) {
int elementCount = accessResult[i]->choice.structure.list.count;
int j;
for (j = 0; j < elementCount; j++) {
deleteDataElement(accessResult[i]->choice.structure.list.array[j]);
}
GLOBAL_FREEMEM(accessResult[i]->choice.structure.list.array);
}
else if (accessResult[i]->present == AccessResult_PR_array) {
int elementCount = accessResult[i]->choice.array.list.count;
int j;
for (j = 0; j < elementCount; j++) {
deleteDataElement(accessResult[i]->choice.array.list.array[j]);
}
GLOBAL_FREEMEM(accessResult[i]->choice.array.list.array);
}
else if (accessResult[i]->present == AccessResult_PR_integer)
GLOBAL_FREEMEM(accessResult[i]->choice.integer.buf);
else if (accessResult[i]->present == AccessResult_PR_unsigned)
GLOBAL_FREEMEM(accessResult[i]->choice.Unsigned.buf);
else if (accessResult[i]->present == AccessResult_PR_floatingpoint)
GLOBAL_FREEMEM(accessResult[i]->choice.floatingpoint.buf);
else if (accessResult[i]->present == AccessResult_PR_utctime)
GLOBAL_FREEMEM(accessResult[i]->choice.utctime.buf);
else if (accessResult[i]->present == AccessResult_PR_failure)
GLOBAL_FREEMEM(accessResult[i]->choice.failure.buf);
GLOBAL_FREEMEM(accessResult[i]);
}
GLOBAL_FREEMEM(accessResult);
}
void
Socket_destroy(Socket self)
{
int fd = self->fd;
self->fd = -1;
closeAndShutdownSocket(fd);
Thread_sleep(10);
GLOBAL_FREEMEM(self);
}
int
mmsClient_createInitiateRequest(MmsConnection self, ByteBuffer* message)
{
MmsPdu_t* mmsPdu = (MmsPdu_t*) GLOBAL_CALLOC(1, sizeof(MmsPdu_t));
mmsPdu->present = MmsPdu_PR_initiateRequestPdu;
mmsPdu->choice.initiateRequestPdu = createInitiateRequestPdu(self);
asn_enc_rval_t rval = der_encode(&asn_DEF_MmsPdu, mmsPdu,
(asn_app_consume_bytes_f*) mmsClient_write_out, (void*) message);
freeInitiateRequestPdu(mmsPdu->choice.initiateRequestPdu);
GLOBAL_FREEMEM(mmsPdu);
return rval.encoded;
}
static void
deleteDataElement(Data_t* dataElement)
{
if (dataElement == NULL ) {
printf("deleteDataElement NULL argument\n");
return;
}
if (dataElement->present == Data_PR_structure) {
int elementCount = dataElement->choice.structure->list.count;
int i;
for (i = 0; i < elementCount; i++) {
deleteDataElement(dataElement->choice.structure->list.array[i]);
}
GLOBAL_FREEMEM(dataElement->choice.structure->list.array);
GLOBAL_FREEMEM(dataElement->choice.structure);
}
else if (dataElement->present == Data_PR_array) {
int elementCount = dataElement->choice.array->list.count;
int i;
for (i = 0; i < elementCount; i++) {
deleteDataElement(dataElement->choice.array->list.array[i]);
}
GLOBAL_FREEMEM(dataElement->choice.array->list.array);
GLOBAL_FREEMEM(dataElement->choice.array);
}
else if (dataElement->present == Data_PR_floatingpoint) {
GLOBAL_FREEMEM(dataElement->choice.floatingpoint.buf);
}
else if (dataElement->present == Data_PR_utctime) {
GLOBAL_FREEMEM(dataElement->choice.utctime.buf);
}
GLOBAL_FREEMEM(dataElement);
}
static void
freeTypeSpecRecursive(TypeSpecification_t* typeSpec) {
if (typeSpec->present == TypeSpecification_PR_structure) {
int elementCount =
typeSpec->choice.structure.components.list.count;
int i;
for (i = 0; i < elementCount; i++) {
GLOBAL_FREEMEM(typeSpec->choice.structure.components.list.array[i]->componentName->buf);
GLOBAL_FREEMEM(typeSpec->choice.structure.components.list.array[i]->componentName);
freeTypeSpecRecursive(typeSpec->choice.structure.components.list.array[i]->componentType);
GLOBAL_FREEMEM(typeSpec->choice.structure.components.list.array[i]->componentType);
GLOBAL_FREEMEM(typeSpec->choice.structure.components.list.array[i]);
}
GLOBAL_FREEMEM(typeSpec->choice.structure.components.list.array);
}
else if (typeSpec->present == TypeSpecification_PR_array) {
GLOBAL_FREEMEM(typeSpec->choice.array.numberOfElements.buf);
freeTypeSpecRecursive(typeSpec->choice.array.elementType);
GLOBAL_FREEMEM(typeSpec->choice.array.elementType);
}
}
void
FileSystem_closeDirectory(DirectoryHandle directory)
{
closedir(directory->handle);
GLOBAL_FREEMEM(directory);
}
void
MmsValueCache_destroy(MmsValueCache self)
{
Map_deleteDeep(self->map, true, (void (*) (void*)) cacheEntryDelete);
GLOBAL_FREEMEM(self);
}
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;
}
static void
initialize(ControlObject* self)
{
if (!(self->initialized)) {
MmsServer mmsServer = IedServer_getMmsServer(self->iedServer);
self->emptyString = MmsValue_newVisibleString(NULL);
char* ctlModelName = createString(4, self->lnName, "$CF$", self->name, "$ctlModel");
if (DEBUG_IED_SERVER)
printf("initialize control for %s\n", ctlModelName);
MmsValue* ctlModel = MmsServer_getValueFromCache(mmsServer,
self->mmsDomain, ctlModelName);
if (ctlModel == NULL) {
if (DEBUG_IED_SERVER)
printf("No control model found for variable %s\n", ctlModelName);
}
GLOBAL_FREEMEM(ctlModelName);
char* sboClassName = createString(4, self->lnName, "$CF$", self->name, "$sboClass");
self->sboClass = MmsServer_getValueFromCache(mmsServer, self->mmsDomain, sboClassName);
GLOBAL_FREEMEM(sboClassName);
self->ctlObjectName = (char*) GLOBAL_MALLOC(130);
StringUtils_createStringInBuffer(self->ctlObjectName, 5, MmsDomain_getName(self->mmsDomain), "/",
self->lnName, "$CO$", self->name);
self->error = MmsValue_newIntegerFromInt32(0);
self->addCause = MmsValue_newIntegerFromInt32(0);
if (ctlModel != NULL) {
uint32_t ctlModelVal = MmsValue_toInt32(ctlModel);
self->ctlModel = ctlModelVal;
if (DEBUG_IED_SERVER)
printf(" ctlModel: %i\n", ctlModelVal);
if ((ctlModelVal == 2) || (ctlModelVal == 4)) { /* SBO */
char* sboTimeoutName = createString(4, self->lnName, "$CF$", self->name, "$sboTimeout");
char* controlObjectReference = createString(6, self->mmsDomain->domainName, "/", self->lnName, "$",
self->name, "$SBO");
self->sbo = MmsValue_newVisibleString(controlObjectReference);
self->sboTimeout = MmsServer_getValueFromCache(mmsServer,
self->mmsDomain, sboTimeoutName);
updateSboTimeoutValue(self);
setState(self, STATE_UNSELECTED);
if (DEBUG_IED_SERVER)
printf("timeout for %s is %i\n", sboTimeoutName, self->selectTimeout);
GLOBAL_FREEMEM(controlObjectReference);
GLOBAL_FREEMEM(sboTimeoutName);
}
else {
self->sbo = MmsValue_newVisibleString(NULL);
setState(self, STATE_READY);
}
}
self->initialized = true;
}
}
int
mmsServer_handleGetVariableAccessAttributesRequest(
MmsServerConnection connection,
uint8_t* buffer, int bufPos, int maxBufPos,
uint32_t invokeId,
ByteBuffer* response)
{
int retVal = 0;
GetVariableAccessAttributesRequest_t* request = 0;
asn_dec_rval_t rval; /* Decoder return value */
rval = ber_decode(NULL, &asn_DEF_GetVariableAccessAttributesRequest,
(void**) &request, buffer + bufPos, maxBufPos - bufPos);
if (rval.code == RC_OK) {
if (request->present == GetVariableAccessAttributesRequest_PR_name) {
if (request->choice.name.present == ObjectName_PR_domainspecific) {
Identifier_t domainId = request->choice.name.choice.domainspecific.domainId;
Identifier_t nameId = request->choice.name.choice.domainspecific.itemId;
char* domainIdStr = createStringFromBuffer(domainId.buf, domainId.size);
char* nameIdStr = createStringFromBuffer(nameId.buf, nameId.size);
if (DEBUG_MMS_SERVER)
printf("MMS_SERVER: getVariableAccessAttributes domainId: %s nameId: %s\n", domainIdStr, nameIdStr);
createVariableAccessAttributesResponse(connection, domainIdStr, nameIdStr, invokeId, response);
GLOBAL_FREEMEM(domainIdStr);
GLOBAL_FREEMEM(nameIdStr);
}
#if (CONFIG_MMS_SUPPORT_VMD_SCOPE_NAMED_VARIABLES == 1)
else if (request->choice.name.present == ObjectName_PR_vmdspecific) {
Identifier_t nameId = request->choice.name.choice.vmdspecific;
char* nameIdStr = createStringFromBuffer(nameId.buf, nameId.size);
if (DEBUG_MMS_SERVER) printf("MMS_SERVER: getVariableAccessAttributes (VMD specific) nameId: %s\n", nameIdStr);
createVariableAccessAttributesResponse(connection, NULL, nameIdStr, invokeId, response);
GLOBAL_FREEMEM(nameIdStr);
}
#endif /* (CONFIG_MMS_SUPPORT_VMD_SCOPE_NAMED_VARIABLES == 1) */
else {
if (DEBUG_MMS_SERVER) printf("GetVariableAccessAttributesRequest with name other than domainspecific is not supported!\n");
retVal = -1;
}
}
else {
if (DEBUG_MMS_SERVER) printf("GetVariableAccessAttributesRequest with address not supported!\n");
retVal = -1;
}
}
else {
if (DEBUG_MMS_SERVER) printf("GetVariableAccessAttributesRequest parsing request failed!\n");
retVal = -1;
}
asn_DEF_GetVariableAccessAttributesRequest.free_struct(&asn_DEF_GetVariableAccessAttributesRequest, request, 0);
return retVal;
}
void
mmsServer_handleWriteRequest(
MmsServerConnection connection,
uint8_t* buffer, int bufPos, int maxBufPos,
uint32_t invokeId,
ByteBuffer* response)
{
WriteRequest_t* writeRequest = 0;
MmsPdu_t* mmsPdu = 0;
asn_dec_rval_t rval; /* Decoder return value */
rval = ber_decode(NULL, &asn_DEF_MmsPdu, (void**) &mmsPdu, buffer, CONFIG_MMS_MAXIMUM_PDU_SIZE);
if (rval.code != RC_OK) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_INVALID_PDU, response);
return;
}
writeRequest = &(mmsPdu->choice.confirmedRequestPdu.confirmedServiceRequest.choice.write);
int numberOfWriteItems = writeRequest->variableAccessSpecification.choice.listOfVariable.list.count;
if (numberOfWriteItems < 1) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_REQUEST_INVALID_ARGUMENT, response);
return;
}
if (numberOfWriteItems > CONFIG_MMS_WRITE_SERVICE_MAX_NUMBER_OF_WRITE_ITEMS) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_OTHER, response);
return;
}
if (writeRequest->listOfData.list.count != numberOfWriteItems) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_REQUEST_INVALID_ARGUMENT, response);
return;
}
MmsDataAccessError accessResults[CONFIG_MMS_WRITE_SERVICE_MAX_NUMBER_OF_WRITE_ITEMS * sizeof(MmsDataAccessError)];
bool sendResponse = true;
int i;
for (i = 0; i < numberOfWriteItems; i++) {
ListOfVariableSeq_t* varSpec =
writeRequest->variableAccessSpecification.choice.listOfVariable.list.array[i];
if (varSpec->variableSpecification.present != VariableSpecification_PR_name) {
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ACCESS_UNSUPPORTED;
continue;
}
MmsVariableSpecification* variable;
MmsDevice* device = MmsServer_getDevice(connection->server);
MmsDomain* domain = NULL;
char* nameIdStr;
if (varSpec->variableSpecification.choice.name.present == ObjectName_PR_domainspecific) {
Identifier_t domainId = varSpec->variableSpecification.choice.name.choice.domainspecific.domainId;
char* domainIdStr = createStringFromBuffer(domainId.buf, domainId.size);
domain = MmsDevice_getDomain(device, domainIdStr);
GLOBAL_FREEMEM(domainIdStr);
if (domain == NULL) {
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_NONE_EXISTENT;
continue;
}
Identifier_t nameId = varSpec->variableSpecification.choice.name.choice.domainspecific.itemId;
nameIdStr = createStringFromBuffer(nameId.buf, nameId.size);
variable = MmsDomain_getNamedVariable(domain, nameIdStr);
}
#if (CONFIG_MMS_SUPPORT_VMD_SCOPE_NAMED_VARIABLES == 1)
else if (varSpec->variableSpecification.choice.name.present == ObjectName_PR_vmdspecific) {
Identifier_t nameId = varSpec->variableSpecification.choice.name.choice.vmdspecific;
nameIdStr = createStringFromBuffer(nameId.buf, nameId.size);
variable = MmsDevice_getNamedVariable(device, nameIdStr);
}
#endif /* (CONFIG_MMS_SUPPORT_VMD_SCOPE_NAMED_VARIABLES == 1) */
else {
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ACCESS_UNSUPPORTED;
continue;
}
if (variable == NULL) {
GLOBAL_FREEMEM(nameIdStr);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_NONE_EXISTENT;
continue;
}
AlternateAccess_t* alternateAccess = varSpec->alternateAccess;
if (alternateAccess != NULL) {
if (variable->type != MMS_ARRAY) {
GLOBAL_FREEMEM(nameIdStr);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ATTRIBUTE_INCONSISTENT;
continue;
}
if (!mmsServer_isIndexAccess(alternateAccess)) {
GLOBAL_FREEMEM(nameIdStr);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ACCESS_UNSUPPORTED;
continue;
}
}
Data_t* dataElement = writeRequest->listOfData.list.array[i];
MmsValue* value = mmsMsg_parseDataElement(dataElement);
if (value == NULL) {
GLOBAL_FREEMEM(nameIdStr);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ATTRIBUTE_INCONSISTENT;
continue;
}
/* Check for correct type */
if (MmsValue_getType(value) != MmsVariableSpecification_getType(variable)) {
GLOBAL_FREEMEM(nameIdStr);
MmsValue_delete(value);
accessResults[i] = DATA_ACCESS_ERROR_TYPE_INCONSISTENT;
continue;
}
if (alternateAccess != NULL) {
if (domain != NULL)
domain = (MmsDomain*) device;
MmsValue* cachedArray = MmsServer_getValueFromCache(connection->server, domain, nameIdStr);
if (cachedArray == NULL) {
GLOBAL_FREEMEM(nameIdStr);
MmsValue_delete(value);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ATTRIBUTE_INCONSISTENT;
continue;
}
int index = mmsServer_getLowIndex(alternateAccess);
MmsValue* elementValue = MmsValue_getElement(cachedArray, index);
if (elementValue == NULL) {
GLOBAL_FREEMEM(nameIdStr);
MmsValue_delete(value);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ATTRIBUTE_INCONSISTENT;
continue;
}
if (MmsValue_update(elementValue, value) == false) {
GLOBAL_FREEMEM(nameIdStr);
MmsValue_delete(value);
accessResults[i] = DATA_ACCESS_ERROR_TYPE_INCONSISTENT;
continue;
}
GLOBAL_FREEMEM(nameIdStr);
MmsValue_delete(value);
accessResults[i] = DATA_ACCESS_ERROR_SUCCESS;
continue;
}
MmsDataAccessError valueIndication =
mmsServer_setValue(connection->server, domain, nameIdStr, value, connection);
if (valueIndication == DATA_ACCESS_ERROR_NO_RESPONSE)
sendResponse = false;
accessResults[i] = valueIndication;
MmsValue_delete(value);
GLOBAL_FREEMEM(nameIdStr);
}
if (sendResponse) {
mmsServer_createMmsWriteResponse(connection, invokeId, response, numberOfWriteItems, accessResults);
}
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
}
int
IsoServer_waitReady(IsoServer self, unsigned int timeoutMs)
{
int result;
if (self->state == ISO_SVR_STATE_RUNNING) {
HandleSet handles;
handles = Handleset_new();
if (handles != NULL) {
#if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1)
#if (CONFIG_MMS_THREADLESS_STACK != 1)
lockClientConnections(self);
#endif
LinkedList openConnection = LinkedList_getNext(self->openClientConnections);
LinkedList lastConnection = self->openClientConnections;
while (openConnection != NULL) {
IsoConnection isoConnection = (IsoConnection) openConnection->data;
if (IsoConnection_isRunning(isoConnection)) {
IsoConnection_addHandleSet(isoConnection, handles);
openConnection = LinkedList_getNext(openConnection);
} else {
IsoConnection_destroy(isoConnection);
lastConnection->next = openConnection->next;
GLOBAL_FREEMEM(openConnection);
openConnection = lastConnection->next;
}
lastConnection = lastConnection->next;
}
#if (CONFIG_MMS_THREADLESS_STACK != 1)
unlockClientConnections(self);
#endif
#else
int i;
for (i = 0; i < CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS; i++) {
if (self->openClientConnections[i] != NULL) {
if (IsoConnection_isRunning(self->openClientConnections[i])) {
IsoConnection_addHandleSet(self->openClientConnections[i], handles);
} else {
IsoConnection_destroy(self->openClientConnections[i]);
self->openClientConnections[i] = NULL;
}
}
}
#endif /* (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1) */
Handleset_addSocket(handles, self->serverSocket);
result = Handleset_waitReady(handles, timeoutMs);
Handleset_destroy(handles);
} else {
result = -1;
}
} else {
result = -1;
}
return result;
}
EthernetSocket
Ethernet_createSocket(const char* interfaceId, uint8_t* destAddress)
{
char bpfFileStringBuffer[11] = { 0 };
int i;
struct ifreq ifr;
int optval;
struct bpf_insn destAddrFiltCode[] =
{
// Load 0 into accumulator. Change to 1 to enable ethernet address filter.
{0x00, 0, 0, 0x00000000}, // A0: ld #0
{0x15, 4, 0, 0x00000000}, // jeq #0, P0, A1
// Load 4 bytes starting at offest 2 into the accu and compare it with 4 bytes of the destination address.
{0x20, 0, 0, 0x00000002}, // A1: ld [2]
{0x15, 0, 7, 0x00000000}, // jeq #0, A2, KO
// Load 2 bytes starting at offest 0 into the accu and compare it with 2 bytes of the destination address.
{0x28, 0, 0, 0x00000000}, // A2: ldh [0]
{0x15, 0, 5, 0x00000000}, // jeq #0, P0, KO
// Load 0 into accumulator. Change to 1 to enable ethernet protocol filter.
{0x00, 0, 0, 0x00000000}, // P0: ld #0
{0x15, 2, 0, 0x00000000}, // jeq #0, OK, P1
// Load 2 bytes starting at offset 12 into the accu and compare it with the given ethertype.
{0x28, 0, 0, 0x0000000c}, // P1: ldh [12]
{0x15, 0, 1, 0x00000000}, // jeq #0, OK, KO
// Accept packet.
{0x6, 0, 0, 0x0000ffff}, // OK: ret #65535
// Drop packet.
{0x6, 0, 0, 0x00000000} // KO: ret #0
/* The whole BPF VM assembler program compiled with bpfc into the machine code above:
*
* A0: ld #0
* jeq #0, P0, A1
* A1: ld [2]
* jeq #0, A2, KO
* A2: ldh [0]
* jeq #0, P0, KO
* P0: ld #0
* jeq #0, OK, P1
* P1: ldh [12]
* jeq #0, OK, KO
* OK: ret #65535
* KO: ret #0
*/
};
EthernetSocket self = GLOBAL_CALLOC(1, sizeof(struct sEthernetSocket));
if (!self)
{
printf("Could not allocate socket descriptor!\n");
return NULL;
}
// Copy default BPF filter program into descriptor.
self->bpfProgram.bf_insns = GLOBAL_CALLOC(1, sizeof(destAddrFiltCode));
if (!self->bpfProgram.bf_insns)
{
printf("Could not allocate memory for BPF filter program!\n");
return NULL;
}
memcpy(self->bpfProgram.bf_insns, &destAddrFiltCode, sizeof(destAddrFiltCode));
self->bpfProgram.bf_len = 12;
// Find the first unused BPF device node.
self->bpf = -1;
for (i = 0; i < 99; ++i)
{
sprintf(bpfFileStringBuffer, "/dev/bpf%i", i);
self->bpf = open(bpfFileStringBuffer, O_RDWR);
if (self->bpf != -1) break;
}
// Did not found any unused, fail.
if (self->bpf == -1)
{
printf("Error opening BPF file handle!\n");
GLOBAL_FREEMEM(self->bpfProgram.bf_insns);
GLOBAL_FREEMEM(self);
return NULL;
}
// Activate non-blocking operation.
optval = ioctl(self->bpf, F_GETFL);
optval |= O_NONBLOCK;
if (fcntl(self->bpf, F_SETFL, &optval) == -1)
{
printf("Unable to change to non-blocking mode!\n");
GLOBAL_FREEMEM(self->bpfProgram.bf_insns);
GLOBAL_FREEMEM(self);
return NULL;
}
// Select the network interface for the BPF.
strncpy(ifr.ifr_name, interfaceId, IFNAMSIZ);
if (ioctl(self->bpf, BIOCSETIF, &ifr))
{
printf("Unable to select interface %s!\n", interfaceId);
GLOBAL_FREEMEM(self->bpfProgram.bf_insns);
GLOBAL_FREEMEM(self);
return NULL;
}
// Activate immediate mode.
if (ioctl(self->bpf, BIOCIMMEDIATE, &self->bpfBufferSize) == -1)
{
printf("Unable to activate immediate mode!\n");
GLOBAL_FREEMEM(self->bpfProgram.bf_insns);
GLOBAL_FREEMEM(self);
return NULL;
}
// Get the buffer length from the BPF handle.
if (ioctl(self->bpf, BIOCGBLEN, &self->bpfBufferSize) == -1)
{
printf("Unable to get BPF buffer lenght!\n");
GLOBAL_FREEMEM(self->bpfProgram.bf_insns);
GLOBAL_FREEMEM(self);
return NULL;
}
// Allocate a buffer for the message reception.
self->bpfBuffer = GLOBAL_CALLOC(1, self->bpfBufferSize);
if (!self->bpfBuffer)
{
printf("Unable to allocate BPF RX buffer!\n");
GLOBAL_FREEMEM(self->bpfProgram.bf_insns);
GLOBAL_FREEMEM(self);
return NULL;
}
self->bpfPositon = self->bpfBuffer;
self->bpfEnd = self->bpfBuffer;
// Set BPF into promiscous mode.
optval = 1;
if (ioctl(self->bpf, BIOCPROMISC, &optval) == -1)
{
printf("Unable to activate promiscous mode!\n");
GLOBAL_FREEMEM(self->bpfProgram.bf_insns);
GLOBAL_FREEMEM(self->bpfBuffer);
GLOBAL_FREEMEM(self);
return NULL;
}
return self;
}
