IsoServer
IsoServer_create()
{
IsoServer self = (IsoServer) GLOBAL_CALLOC(1, sizeof(struct sIsoServer));
self->state = ISO_SVR_STATE_IDLE;
self->tcpPort = TCP_PORT;
#if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1)
self->openClientConnections = LinkedList_create();
#else
self->openClientConnections = (IsoConnection*)
GLOBAL_CALLOC(CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS, sizeof(IsoConnection));
#endif
#if (CONFIG_MMS_THREADLESS_STACK != 1)
self->connectionCounterMutex = Semaphore_create(1);
#if (CONFIG_MAXIMUM_TCP_CLIENT_CONNECTIONS == -1)
self->openClientConnectionsMutex = Semaphore_create(1);
#endif
#endif /* (CONFIG_MMS_THREADLESS_STACK != 1) */
self->connectionCounter = 0;
return self;
}
static InitiateRequestPdu_t
createInitiateRequestPdu(MmsConnection self)
{
InitiateRequestPdu_t request;
request.localDetailCalling = (Integer32_t*) GLOBAL_CALLOC(1, sizeof(Integer32_t));
*(request.localDetailCalling) = self->parameters.maxPduSize;
request.proposedMaxServOutstandingCalled = DEFAULT_MAX_SERV_OUTSTANDING_CALLED;
request.proposedMaxServOutstandingCalling = DEFAULT_MAX_SERV_OUTSTANDING_CALLING;
request.proposedDataStructureNestingLevel = (Integer8_t*) GLOBAL_CALLOC(1, sizeof(Integer8_t));
*(request.proposedDataStructureNestingLevel) = DEFAULT_DATA_STRUCTURE_NESTING_LEVEL;
request.mmsInitRequestDetail.proposedParameterCBB.size = 2;
request.mmsInitRequestDetail.proposedParameterCBB.bits_unused = 5;
request.mmsInitRequestDetail.proposedParameterCBB.buf = (uint8_t*) GLOBAL_CALLOC(2, sizeof(uint8_t));
request.mmsInitRequestDetail.proposedParameterCBB.buf[0] = 0xf1;
request.mmsInitRequestDetail.proposedParameterCBB.buf[1] = 0x00;
request.mmsInitRequestDetail.proposedVersionNumber = 1;
request.mmsInitRequestDetail.servicesSupportedCalling.size = 11;
request.mmsInitRequestDetail.servicesSupportedCalling.bits_unused = 3;
request.mmsInitRequestDetail.servicesSupportedCalling.buf = servicesSupported;
return request;
}
Data_t*
mmsMsg_createDataElement(MmsValue* value)
{
if (value->type == MMS_STRUCTURE) {
Data_t* dataElement = (Data_t*) GLOBAL_CALLOC(1, sizeof(Data_t));
dataElement->present = Data_PR_structure;
int elementCount = value->value.structure.size;
dataElement->choice.structure = (DataSequence_t*) GLOBAL_CALLOC(1, sizeof(DataSequence_t));
dataElement->choice.structure->list.size = elementCount;
dataElement->choice.structure->list.count = elementCount;
dataElement->choice.structure->list.array = (Data_t**) GLOBAL_CALLOC(elementCount, sizeof(Data_t*));
int i;
for (i = 0; i < elementCount; i++) {
dataElement->choice.structure->list.array[i] =
mmsMsg_createDataElement(value->value.structure.components[i]);
}
return dataElement;
}
else {
return mmsMsg_createBasicDataElement(value);
}
}
/**
* 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;
}
ClientGooseControlBlock
ClientGooseControlBlock_create(const char* objectReference)
{
ClientGooseControlBlock self = (ClientGooseControlBlock) GLOBAL_CALLOC(1, sizeof(struct sClientGooseControlBlock));
self->objectReference = copyString(objectReference);
return self;
}
MmsValueCache
MmsValueCache_create(MmsDomain* domain)
{
MmsValueCache self = (MmsValueCache) GLOBAL_CALLOC(1, sizeof(struct sMmsValueCache));
self->domain = domain;
self->map = StringMap_create();
return self;
}
int
mmsClient_createGetNameListRequestDomainOrVMDSpecific(long invokeId, const char* domainName,
ByteBuffer* writeBuffer, MmsObjectClass objectClass, const char* continueAfter)
{
MmsPdu_t* mmsPdu = mmsClient_createConfirmedRequestPdu(invokeId);
mmsPdu->choice.confirmedRequestPdu.confirmedServiceRequest.present =
ConfirmedServiceRequest_PR_getNameList;
GetNameListRequest_t* request;
request = &(mmsPdu->choice.confirmedRequestPdu.confirmedServiceRequest.choice.getNameList);
if (continueAfter != NULL) {
request->continueAfter = (Identifier_t*) GLOBAL_CALLOC(1, sizeof(Identifier_t));
request->continueAfter->buf = (uint8_t*) copyString(continueAfter);
request->continueAfter->size = strlen(continueAfter);
}
else
request->continueAfter = NULL;
if (domainName != NULL) {
request->objectScope.present = GetNameListRequest__objectScope_PR_domainSpecific;
request->objectScope.choice.domainSpecific.buf = (uint8_t*) domainName;
request->objectScope.choice.domainSpecific.size = strlen(domainName);
}
else {
request->objectScope.present = GetNameListRequest__objectScope_PR_vmdSpecific;
}
request->objectClass.present = ObjectClass_PR_basicObjectClass;
if (objectClass == MMS_NAMED_VARIABLE)
asn_long2INTEGER(&request->objectClass.choice.basicObjectClass,
ObjectClass__basicObjectClass_namedVariable);
else if (objectClass == MMS_NAMED_VARIABLE_LIST)
asn_long2INTEGER(&request->objectClass.choice.basicObjectClass,
ObjectClass__basicObjectClass_namedVariableList);
asn_enc_rval_t rval;
rval = der_encode(&asn_DEF_MmsPdu, mmsPdu,
(asn_app_consume_bytes_f*) mmsClient_write_out, (void*) writeBuffer);
request->objectScope.choice.domainSpecific.buf = 0;
request->objectScope.choice.domainSpecific.size = 0;
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
return rval.encoded;
}
static AlternateAccess_t*
createAlternateAccess(uint32_t index, uint32_t elementCount)
{
AlternateAccess_t* alternateAccess = (AlternateAccess_t*) GLOBAL_CALLOC(1, sizeof(AlternateAccess_t));
alternateAccess->list.count = 1;
alternateAccess->list.array = (struct AlternateAccess__Member**) GLOBAL_CALLOC(1, sizeof(struct AlternateAccess__Member*));
alternateAccess->list.array[0] = (struct AlternateAccess__Member*) GLOBAL_CALLOC(1, sizeof(struct AlternateAccess__Member));
alternateAccess->list.array[0]->present = AlternateAccess__Member_PR_unnamed;
alternateAccess->list.array[0]->choice.unnamed = (AlternateAccessSelection_t*) GLOBAL_CALLOC(1, sizeof(AlternateAccessSelection_t));
alternateAccess->list.array[0]->choice.unnamed->present = AlternateAccessSelection_PR_selectAccess;
if (elementCount > 0) {
alternateAccess->list.array[0]->choice.unnamed->choice.selectAccess.present =
AlternateAccessSelection__selectAccess_PR_indexRange;
INTEGER_t* asnIndex =
&(alternateAccess->list.array[0]->choice.unnamed->choice.selectAccess.choice.indexRange.lowIndex);
asn_long2INTEGER(asnIndex, index);
asnIndex =
&(alternateAccess->list.array[0]->choice.unnamed->choice.selectAccess.choice.indexRange.numberOfElements);
asn_long2INTEGER(asnIndex, elementCount);
}
else {
alternateAccess->list.array[0]->choice.unnamed->choice.selectAccess.present =
AlternateAccessSelection__selectAccess_PR_index;
INTEGER_t* asnIndex =
&(alternateAccess->list.array[0]->choice.unnamed->choice.selectAccess.choice.index);
asn_long2INTEGER(asnIndex, index);
}
return alternateAccess;
}
static VariableSpecification_t*
createNewDomainVariableSpecification(const char* domainId, const char* itemId)
{
VariableSpecification_t* varSpec = (VariableSpecification_t*) GLOBAL_CALLOC(1, sizeof(ListOfVariableSeq_t));
varSpec->present = VariableSpecification_PR_name;
varSpec->choice.name.present = ObjectName_PR_domainspecific;
varSpec->choice.name.choice.domainspecific.domainId.buf = (uint8_t*) domainId;
varSpec->choice.name.choice.domainspecific.domainId.size = strlen(domainId);
varSpec->choice.name.choice.domainspecific.itemId.buf = (uint8_t*) itemId;
varSpec->choice.name.choice.domainspecific.itemId.size = strlen(itemId);
return varSpec;
}
static ListOfVariableSeq_t*
createVariableIdentifier(const char* domainId, const char* itemId)
{
ListOfVariableSeq_t* variableIdentifier = (ListOfVariableSeq_t*) GLOBAL_CALLOC(1, sizeof(ListOfVariableSeq_t));
variableIdentifier->variableSpecification.present = VariableSpecification_PR_name;
variableIdentifier->variableSpecification.choice.name.present = ObjectName_PR_domainspecific;
variableIdentifier->variableSpecification.choice.name.choice.domainspecific.domainId.buf = (uint8_t*) domainId;
variableIdentifier->variableSpecification.choice.name.choice.domainspecific.domainId.size = strlen(domainId);
variableIdentifier->variableSpecification.choice.name.choice.domainspecific.itemId.buf = (uint8_t*) itemId;
variableIdentifier->variableSpecification.choice.name.choice.domainspecific.itemId.size = strlen(itemId);
return variableIdentifier;
}
int
mmsServer_createMmsWriteResponse(MmsServerConnection connection,
int invokeId, ByteBuffer* response, int numberOfItems, MmsDataAccessError* accessResults)
{
MmsPdu_t* mmsPdu = mmsServer_createConfirmedResponse(invokeId);
mmsPdu->choice.confirmedResponsePdu.confirmedServiceResponse.present =
ConfirmedServiceResponse_PR_write;
WriteResponse_t* writeResponse =
&(mmsPdu->choice.confirmedResponsePdu.confirmedServiceResponse.choice.write);
writeResponse->list.count = numberOfItems;
writeResponse->list.size = numberOfItems;
writeResponse->list.array = (struct WriteResponse__Member**) GLOBAL_CALLOC(numberOfItems,
sizeof(struct WriteResponse__Member*));
int i;
for (i = 0; i < numberOfItems; i++) {
writeResponse->list.array[i] = (struct WriteResponse__Member*) GLOBAL_CALLOC(1, sizeof(struct WriteResponse__Member));
if (accessResults[i] == DATA_ACCESS_ERROR_SUCCESS)
writeResponse->list.array[i]->present = WriteResponse__Member_PR_success;
else {
writeResponse->list.array[i]->present = WriteResponse__Member_PR_failure;
asn_long2INTEGER(&writeResponse->list.array[i]->choice.failure, (long) accessResults[i]);
}
}
der_encode(&asn_DEF_MmsPdu, mmsPdu, mmsServer_write_out, (void*) response);
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
return 0;
}
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;
}
int
mmsClient_createMmsGetNameListRequestAssociationSpecific(long invokeId, ByteBuffer* writeBuffer,
const char* continueAfter)
{
MmsPdu_t* mmsPdu = mmsClient_createConfirmedRequestPdu(invokeId);
mmsPdu->choice.confirmedRequestPdu.confirmedServiceRequest.present =
ConfirmedServiceRequest_PR_getNameList;
GetNameListRequest_t* request;
request = &(mmsPdu->choice.confirmedRequestPdu.confirmedServiceRequest.choice.getNameList);
if (continueAfter != NULL) {
request->continueAfter = (Identifier_t*) GLOBAL_CALLOC(1, sizeof(Identifier_t));
request->continueAfter->buf = (uint8_t*) copyString(continueAfter);
request->continueAfter->size = strlen(continueAfter);
}
else
request->continueAfter = NULL;
request->objectScope.present = GetNameListRequest__objectScope_PR_aaSpecific;
request->objectClass.present = ObjectClass_PR_basicObjectClass;
asn_long2INTEGER(&request->objectClass.choice.basicObjectClass,
ObjectClass__basicObjectClass_namedVariableList);
asn_enc_rval_t rval;
rval = der_encode(&asn_DEF_MmsPdu, mmsPdu,
(asn_app_consume_bytes_f*) mmsClient_write_out, (void*) writeBuffer);
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
return rval.encoded;
}
ControlObject*
ControlObject_create(IedServer iedServer, MmsDomain* domain, char* lnName, char* name)
{
ControlObject* self = (ControlObject*) GLOBAL_CALLOC(1, sizeof(ControlObject));
if (self == NULL)
goto exit_function;
if (DEBUG_IED_SERVER)
printf("create control object for LD: %s, LN: %s, name: %s\n", domain->domainName, lnName, name);
#if (CONFIG_MMS_THREADLESS_STACK != 1)
self->stateLock = Semaphore_create(1);
if (self->stateLock == NULL) {
ControlObject_destroy(self);
self = NULL;
goto exit_function;
}
#endif
self->name = copyString(name);
if (self->name == NULL) {
ControlObject_destroy(self);
self = NULL;
goto exit_function;
}
self->lnName = lnName;
self->mmsDomain = domain;
self->iedServer = iedServer;
exit_function:
return self;
}
int
mmsClient_createReadAssociationSpecificNamedVariableListRequest(
uint32_t invokeId,
const char* itemId,
ByteBuffer* writeBuffer,
bool specWithResult)
{
MmsPdu_t* mmsPdu = mmsClient_createConfirmedRequestPdu(invokeId);
ReadRequest_t* readRequest = createReadRequest(mmsPdu);
if (specWithResult) {
readRequest->specificationWithResult = (BOOLEAN_t*) GLOBAL_CALLOC(1, sizeof(BOOLEAN_t));
(*(readRequest->specificationWithResult)) = true;
}
else
readRequest->specificationWithResult = NULL;
readRequest->variableAccessSpecification.present = VariableAccessSpecification_PR_variableListName;
ObjectName_t* objectName = &(readRequest->variableAccessSpecification.choice.variableListName);
objectName->present = ObjectName_PR_aaspecific;
objectName->choice.aaspecific.buf = (uint8_t*) copyString(itemId);
objectName->choice.aaspecific.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);
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
return rval.encoded;
}
static int
createTypeSpecification (
MmsVariableSpecification* namedVariable,
TypeSpecification_t* typeSpec)
{
if (namedVariable->type == MMS_ARRAY) {
typeSpec->present = TypeSpecification_PR_array;
asn_long2INTEGER(&(typeSpec->choice.array.numberOfElements),
(long) namedVariable->typeSpec.array.elementCount);
typeSpec->choice.array.packed = NULL;
typeSpec->choice.array.elementType = (TypeSpecification_t*) GLOBAL_CALLOC(1, sizeof(TypeSpecification_t));
createTypeSpecification(namedVariable->typeSpec.array.elementTypeSpec,
typeSpec->choice.array.elementType);
}
else if (namedVariable->type == MMS_STRUCTURE) {
typeSpec->present = TypeSpecification_PR_structure;
int componentCount = namedVariable->typeSpec.structure.elementCount;
typeSpec->choice.structure.components.list.count = componentCount;
typeSpec->choice.structure.components.list.size = componentCount;
typeSpec->choice.structure.components.list.array
= (StructComponent_t**) GLOBAL_CALLOC(componentCount, sizeof(StructComponent_t*));
int i;
for (i = 0; i < componentCount; i++) {
typeSpec->choice.structure.components.list.array[i] =
(StructComponent_t*) GLOBAL_CALLOC(1, sizeof(StructComponent_t));
typeSpec->choice.structure.components.list.array[i]->componentName =
(Identifier_t*) GLOBAL_CALLOC(1, sizeof(Identifier_t));
typeSpec->choice.structure.components.list.array[i]->componentName->buf =
(uint8_t*) copyString(namedVariable->typeSpec.structure.elements[i]->name);
typeSpec->choice.structure.components.list.array[i]->componentName->size =
strlen(namedVariable->typeSpec.structure.elements[i]->name);
typeSpec->choice.structure.components.list.array[i]->componentType =
(TypeSpecification_t*) GLOBAL_CALLOC(1, sizeof(TypeSpecification_t));
createTypeSpecification(namedVariable->typeSpec.structure.elements[i],
typeSpec->choice.structure.components.list.array[i]->componentType);
}
}
else {
switch (namedVariable->type) {
case MMS_BOOLEAN:
typeSpec->present = TypeSpecification_PR_boolean;
break;
case MMS_BIT_STRING:
typeSpec->present = TypeSpecification_PR_bitstring;
typeSpec->choice.bitstring = namedVariable->typeSpec.bitString;
break;
case MMS_INTEGER:
typeSpec->present = TypeSpecification_PR_integer;
typeSpec->choice.integer = namedVariable->typeSpec.integer;
break;
case MMS_UNSIGNED:
typeSpec->present = TypeSpecification_PR_unsigned;
typeSpec->choice.Unsigned = namedVariable->typeSpec.unsignedInteger;
break;
case MMS_FLOAT:
typeSpec->present = TypeSpecification_PR_floatingpoint;
typeSpec->choice.floatingpoint.exponentwidth =
namedVariable->typeSpec.floatingpoint.exponentWidth;
typeSpec->choice.floatingpoint.formatwidth =
namedVariable->typeSpec.floatingpoint.formatWidth;
break;
case MMS_OCTET_STRING:
typeSpec->present = TypeSpecification_PR_octetstring;
typeSpec->choice.octetstring = namedVariable->typeSpec.octetString;
break;
case MMS_VISIBLE_STRING:
typeSpec->present = TypeSpecification_PR_visiblestring;
typeSpec->choice.visiblestring = namedVariable->typeSpec.visibleString;
break;
case MMS_STRING:
typeSpec->present = TypeSpecification_PR_mMSString;
typeSpec->choice.mMSString = namedVariable->typeSpec.mmsString;
break;
case MMS_UTC_TIME:
typeSpec->present = TypeSpecification_PR_utctime;
break;
case MMS_BINARY_TIME:
typeSpec->present = TypeSpecification_PR_binarytime;
if (namedVariable->typeSpec.binaryTime == 6)
typeSpec->choice.binarytime = 1;
else
typeSpec->choice.binarytime = 0;
break;
default:
if (DEBUG_MMS_SERVER)
printf("MMS-SERVER: Unsupported type %i!\n", namedVariable->type);
return -1;
break;
}
}
return 1;
}
static MmsVariableSpecification*
createTypeSpecification(TypeSpecification_t* asnTypeSpec) {
MmsVariableSpecification* typeSpec = (MmsVariableSpecification*)
GLOBAL_CALLOC(1, sizeof(MmsVariableSpecification));
switch (asnTypeSpec->present) {
case TypeSpecification_PR_structure:
{
typeSpec->type = MMS_STRUCTURE;
int elementCount = asnTypeSpec->choice.structure.components.list.count;
typeSpec->typeSpec.structure.elementCount = elementCount;
typeSpec->typeSpec.structure.elements = (MmsVariableSpecification**)
GLOBAL_CALLOC(elementCount, sizeof(MmsVariableSpecification*));
int i;
for (i = 0; i < elementCount; i++) {
char* name = createStringFromBuffer(
asnTypeSpec->choice.structure.components.list.array[i]->componentName->buf,
asnTypeSpec->choice.structure.components.list.array[i]->componentName->size);
typeSpec->typeSpec.structure.elements[i] =
createTypeSpecification(asnTypeSpec->choice.structure.components.
list.array[i]->componentType);
typeSpec->typeSpec.structure.elements[i]->name = name;
}
}
break;
case TypeSpecification_PR_array:
{
typeSpec->type = MMS_ARRAY;
long elementCount;
asn_INTEGER2long(&asnTypeSpec->choice.array.numberOfElements, &elementCount);
typeSpec->typeSpec.array.elementCount = elementCount;
typeSpec->typeSpec.array.elementTypeSpec =
createTypeSpecification(asnTypeSpec->choice.array.elementType);
}
break;
case TypeSpecification_PR_boolean:
typeSpec->type = MMS_BOOLEAN;
break;
case TypeSpecification_PR_bitstring:
typeSpec->type = MMS_BIT_STRING;
typeSpec->typeSpec.bitString = asnTypeSpec->choice.bitstring;
break;
case TypeSpecification_PR_integer:
typeSpec->type = MMS_INTEGER;
typeSpec->typeSpec.integer = asnTypeSpec->choice.integer;
break;
case TypeSpecification_PR_unsigned:
typeSpec->type = MMS_UNSIGNED;
typeSpec->typeSpec.unsignedInteger = asnTypeSpec->choice.Unsigned;
break;
case TypeSpecification_PR_floatingpoint:
typeSpec->type = MMS_FLOAT;
typeSpec->typeSpec.floatingpoint.exponentWidth =
asnTypeSpec->choice.floatingpoint.exponentwidth;
typeSpec->typeSpec.floatingpoint.formatWidth =
asnTypeSpec->choice.floatingpoint.formatwidth;
break;
case TypeSpecification_PR_octetstring:
typeSpec->type = MMS_OCTET_STRING;
typeSpec->typeSpec.octetString = asnTypeSpec->choice.octetstring;
break;
case TypeSpecification_PR_visiblestring:
typeSpec->type = MMS_VISIBLE_STRING;
typeSpec->typeSpec.visibleString = asnTypeSpec->choice.visiblestring;
break;
case TypeSpecification_PR_mMSString:
typeSpec->type = MMS_STRING;
typeSpec->typeSpec.mmsString = asnTypeSpec->choice.mMSString;
break;
case TypeSpecification_PR_utctime:
typeSpec->type = MMS_UTC_TIME;
break;
case TypeSpecification_PR_binarytime:
typeSpec->type = MMS_BINARY_TIME;
if (asnTypeSpec->choice.binarytime == 0)
typeSpec->typeSpec.binaryTime = 4;
else
typeSpec->typeSpec.binaryTime = 6;
break;
default:
printf("ERROR: unknown type in type specification\n");
break;
}
return typeSpec;
}
ControlObjectClient
ControlObjectClient_create(const char* objectReference, IedConnection connection)
{
ControlObjectClient self = NULL;
/* request control model from server */
char domainId[65];
char itemId[65];
char* domainName = MmsMapping_getMmsDomainFromObjectReference(objectReference, domainId);
if (domainName == NULL)
goto exit_function;
convertToMmsAndInsertFC(itemId, objectReference + strlen(domainId) + 1, "CF");
int controlObjectItemIdLen = strlen(itemId);
strncat(itemId, "$ctlModel", 64 - controlObjectItemIdLen);
MmsError mmsError;
MmsValue* ctlModel = MmsConnection_readVariable(IedConnection_getMmsConnection(connection),
&mmsError, domainId, itemId);
if (ctlModel == NULL) {
if (DEBUG_IED_CLIENT)
printf("IED_CLIENT: ControlObjectClient_create: failed to get ctlModel from server\n");
goto exit_function;
}
int ctlModelVal = MmsValue_toUint32(ctlModel);
MmsValue_delete(ctlModel);
IedClientError error;
LinkedList dataDirectory =
IedConnection_getDataDirectory(connection, &error, objectReference);
if (dataDirectory == NULL) {
if (DEBUG_IED_CLIENT)
printf("IED_CLIENT: ControlObjectClient_create: failed to get data directory of control object\n");
goto exit_function;
}
/* check what control elements are available */
bool hasOper = false;
LinkedList element = LinkedList_getNext(dataDirectory);
while (element != NULL) {
char* objectName = (char*) element->data;
if (strcmp(objectName, "Oper") == 0)
hasOper = true;
element = LinkedList_getNext(element);
}
LinkedList_destroy(dataDirectory);
if (hasOper == false) {
if (DEBUG_IED_CLIENT)
printf("IED_CLIENT: control is missing required element \"Oper\"\n");
goto exit_function;
}
/* check for time activated control */
bool hasTimeActivatedControl = false;
strcpy(itemId, objectReference);
strcat(itemId, ".Oper");
dataDirectory = IedConnection_getDataDirectory(connection, &error, itemId);
if (dataDirectory == NULL)
goto exit_function;
element = LinkedList_getNext(dataDirectory);
while (element != NULL) {
char* objectName = (char*) element->data;
if (strcmp(objectName, "operTm") == 0) {
hasTimeActivatedControl = true;
break;
}
element = LinkedList_getNext(element);
}
LinkedList_destroy(dataDirectory);
/* get default parameters for Oper control variable */
MmsValue* oper = IedConnection_readObject(connection, &error, itemId, IEC61850_FC_CO);
if (oper == NULL) {
if (DEBUG_IED_CLIENT)
printf("IED_CLIENT: reading \"Oper\" failed!\n");
goto exit_function;
}
self = (ControlObjectClient) GLOBAL_CALLOC(1, sizeof(struct sControlObjectClient));
if (self == NULL)
goto exit_function;
self->objectReference = copyString(objectReference);
self->connection = connection;
self->ctlModel = (ControlModel) ctlModelVal;
self->hasTimeActivatedMode = hasTimeActivatedControl;
self->ctlVal = MmsValue_getElement(oper, 0);
/* Check for T element type (EntryTime -> Ed.1, Timestamp -> Ed.2) */
MmsValue* t;
if (hasTimeActivatedControl)
t = MmsValue_getElement(oper, 4);
else
t = MmsValue_getElement(oper, 3);
if (MmsValue_getType(t) == MMS_BINARY_TIME)
self->edition = 1;
else
self->edition = 2;
if (DEBUG_IED_CLIENT)
printf("IED_CLIENT: Detected edition %i control\n", self->edition);
MmsValue_setElement(oper, 0, NULL);
MmsValue_delete(oper);
private_IedConnection_addControlClient(connection, self);
exit_function:
return self;
}
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 void
createGetNamedVariableListAttributesResponse(int invokeId, ByteBuffer* response,
MmsNamedVariableList variableList)
{
MmsPdu_t* mmsPdu = mmsServer_createConfirmedResponse(invokeId);
mmsPdu->choice.confirmedResponsePdu.confirmedServiceResponse.present =
ConfirmedServiceResponse_PR_getNamedVariableListAttributes;
GetNamedVariableListAttributesResponse_t* varListResponse =
&(mmsPdu->choice.confirmedResponsePdu.confirmedServiceResponse.
choice.getNamedVariableListAttributes);
varListResponse->mmsDeletable = MmsNamedVariableList_isDeletable(variableList);
LinkedList variables = MmsNamedVariableList_getVariableList(variableList);
int variableCount = LinkedList_size(variables);
varListResponse->listOfVariable.list.count = variableCount;
varListResponse->listOfVariable.list.size = variableCount;
varListResponse->listOfVariable.list.array = (struct GetNamedVariableListAttributesResponse__listOfVariable__Member**)
GLOBAL_CALLOC(variableCount, sizeof(void*));
LinkedList variable = LinkedList_getNext(variables);
int i;
for (i = 0; i < variableCount; i++) {
MmsNamedVariableListEntry variableEntry = (MmsNamedVariableListEntry) variable->data;
varListResponse->listOfVariable.list.array[i] = (struct GetNamedVariableListAttributesResponse__listOfVariable__Member*)
GLOBAL_CALLOC(1, sizeof(struct GetNamedVariableListAttributesResponse__listOfVariable__Member));
varListResponse->listOfVariable.list.array[i]->variableSpecification.present =
VariableSpecification_PR_name;
varListResponse->listOfVariable.list.array[i]->variableSpecification.choice.name.present =
ObjectName_PR_domainspecific;
char* variableDomainName = MmsDomain_getName(variableEntry->domain);
varListResponse->listOfVariable.list.array[i]->variableSpecification.choice.name.choice.
domainspecific.domainId.buf = (uint8_t*) copyString(variableDomainName);
varListResponse->listOfVariable.list.array[i]->variableSpecification.choice.name.choice.
domainspecific.domainId.size = strlen(variableDomainName);
varListResponse->listOfVariable.list.array[i]->variableSpecification.choice.name.choice.
domainspecific.itemId.buf = (uint8_t*) copyString(variableEntry->variableName);
varListResponse->listOfVariable.list.array[i]->variableSpecification.choice.name.choice.
domainspecific.itemId.size = strlen(variableEntry->variableName);
variable = LinkedList_getNext(variable);
}
der_encode(&asn_DEF_MmsPdu, mmsPdu, mmsServer_write_out, (void*) response);
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
}
int
mmsClient_createReadRequestAlternateAccessIndex(uint32_t invokeId, const char* domainId, const char* itemId,
uint32_t index, uint32_t elementCount, 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* variableIdentifier = createVariableIdentifier(domainId, itemId);
readRequest->variableAccessSpecification.choice.listOfVariable.list.array[0] = variableIdentifier;
variableIdentifier->alternateAccess = createAlternateAccess(index, elementCount);
asn_enc_rval_t rval;
rval = der_encode(&asn_DEF_MmsPdu, mmsPdu,
(asn_app_consume_bytes_f*) mmsClient_write_out, (void*) writeBuffer);
variableIdentifier->variableSpecification.choice.name.choice.domainspecific.domainId.buf = 0;
variableIdentifier->variableSpecification.choice.name.choice.domainspecific.domainId.size = 0;
variableIdentifier->variableSpecification.choice.name.choice.domainspecific.itemId.buf = 0;
variableIdentifier->variableSpecification.choice.name.choice.domainspecific.itemId.size = 0;
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
return rval.encoded;
}
int
mmsClient_createWriteMultipleItemsRequest(uint32_t invokeId, const char* domainId, LinkedList itemIds, LinkedList values,
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);
int numberOfItems = LinkedList_size(itemIds);
/* Create list of variable specifications */
request->variableAccessSpecification.present = VariableAccessSpecification_PR_listOfVariable;
request->variableAccessSpecification.choice.listOfVariable.list.count = numberOfItems;
request->variableAccessSpecification.choice.listOfVariable.list.size = numberOfItems;
request->variableAccessSpecification.choice.listOfVariable.list.array =
(ListOfVariableSeq_t**) GLOBAL_CALLOC(numberOfItems, sizeof(ListOfVariableSeq_t*));
/* Create list of data values */
request->listOfData.list.count = numberOfItems;
request->listOfData.list.size = numberOfItems;
request->listOfData.list.array = (Data_t**) GLOBAL_CALLOC(numberOfItems, sizeof(struct Data*));
int i;
LinkedList item = LinkedList_getNext(itemIds);
LinkedList valueElement = LinkedList_getNext(values);
for (i = 0; i < numberOfItems; i++) {
if (item == NULL) return -1;
if (valueElement == NULL) return -1;
char* itemId = (char*) item->data;
MmsValue* value = (MmsValue*) valueElement->data;
request->variableAccessSpecification.choice.listOfVariable.list.array[i] = (ListOfVariableSeq_t*)
createNewDomainVariableSpecification(domainId, itemId);
request->listOfData.list.array[i] = mmsMsg_createBasicDataElement(value);
item = LinkedList_getNext(item);
valueElement = LinkedList_getNext(valueElement);
}
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;
for (i = 0; i < numberOfItems; i++) {
GLOBAL_FREEMEM(request->variableAccessSpecification.choice.listOfVariable.list.array[i]);
deleteDataElement(request->listOfData.list.array[i]);
}
GLOBAL_FREEMEM(request->variableAccessSpecification.choice.listOfVariable.list.array);
request->variableAccessSpecification.choice.listOfVariable.list.array = 0;
request->listOfData.list.count = 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;
}
void
mmsMsg_addResultToResultList(AccessResult_t* accessResult, MmsValue* value)
{
if (value == NULL) {
accessResult->present = AccessResult_PR_failure;
asn_long2INTEGER(&accessResult->choice.failure, DataAccessError_objectnonexistent);
if (DEBUG)
printf("ACCESS ERROR\n");
}
else {
switch (value->type) {
case MMS_ARRAY:
{
int size = value->value.structure.size;
accessResult->present = AccessResult_PR_array;
accessResult->choice.array.list.count = size;
accessResult->choice.array.list.size = size;
accessResult->choice.array.list.array = (Data_t**) GLOBAL_CALLOC(size, sizeof(Data_t*));
int i;
for (i = 0; i < size; i++) {
accessResult->choice.array.list.array[i] =
mmsMsg_createDataElement(value->value.structure.components[i]);
}
}
break;
case MMS_STRUCTURE:
{
int size = value->value.structure.size;
accessResult->present = AccessResult_PR_structure;
accessResult->choice.structure.list.count = size;
accessResult->choice.structure.list.size = size;
accessResult->choice.structure.list.array = (Data_t**) GLOBAL_CALLOC(size, sizeof(Data_t*));
int i;
for (i = 0; i < size; i++) {
accessResult->choice.structure.list.array[i] =
mmsMsg_createDataElement(value->value.structure.components[i]);
}
}
break;
case MMS_BIT_STRING:
{
accessResult->present = AccessResult_PR_bitstring;
accessResult->choice.bitstring.buf = value->value.bitString.buf;
int size = (value->value.bitString.size / 8) + ((value->value.bitString.size % 8) > 0);
int unused = 8 - (value->value.bitString.size % 8);
accessResult->choice.bitstring.size = size; /* size in bytes */
accessResult->choice.bitstring.bits_unused = unused;
}
break;
case MMS_BOOLEAN:
accessResult->present = AccessResult_PR_boolean;
accessResult->choice.boolean = value->value.boolean;
break;
case MMS_FLOAT:
accessResult->present = AccessResult_PR_floatingpoint;
mmsMsg_createFloatData(value, &accessResult->choice.floatingpoint.size,
&accessResult->choice.floatingpoint.buf);
break;
case MMS_UTC_TIME:
accessResult->present = AccessResult_PR_utctime;
accessResult->choice.utctime.buf = (uint8_t*) GLOBAL_MALLOC(8);
memcpy(accessResult->choice.utctime.buf, value->value.utcTime, 8);
accessResult->choice.utctime.size = 8;
break;
case MMS_INTEGER:
accessResult->present = AccessResult_PR_integer;
asn_long2INTEGER(&accessResult->choice.integer, (long) MmsValue_toInt32(value));
break;
case MMS_UNSIGNED:
accessResult->present = AccessResult_PR_unsigned;
asn_long2INTEGER(&accessResult->choice.Unsigned, (long) MmsValue_toInt32(value));
break;
case MMS_VISIBLE_STRING:
accessResult->present = AccessResult_PR_visiblestring;
if (value->value.visibleString.buf == NULL )
accessResult->choice.visiblestring.size = 0;
else {
accessResult->choice.visiblestring.buf = (uint8_t*) value->value.visibleString.buf;
accessResult->choice.visiblestring.size = strlen(value->value.visibleString.buf);
}
break;
case MMS_STRING:
accessResult->present = AccessResult_PR_mMSString;
if (value->value.visibleString.buf == NULL ) {
accessResult->choice.mMSString.size = 0;
}
else {
accessResult->choice.mMSString.buf = (uint8_t*) value->value.visibleString.buf;
accessResult->choice.mMSString.size = strlen(value->value.visibleString.buf);
}
break;
case MMS_BINARY_TIME:
accessResult->present = AccessResult_PR_binarytime;
accessResult->choice.binarytime.size = value->value.binaryTime.size;
accessResult->choice.binarytime.buf = value->value.binaryTime.buf;
break;
case MMS_OCTET_STRING:
accessResult->present = AccessResult_PR_octetstring;
if (value->value.octetString.buf != NULL ) {
accessResult->choice.octetstring.buf = value->value.octetString.buf;
accessResult->choice.octetstring.size = value->value.octetString.size;
}
else
accessResult->choice.octetstring.size = 0;
break;
default:
accessResult->present = AccessResult_PR_failure;
asn_long2INTEGER(&accessResult->choice.failure, DataAccessError_typeinconsistent);
break;
}
}
}
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;
}
MmsValue*
mmsClient_parseListOfAccessResults(AccessResult_t** accessResultList, int listSize, bool createArray)
{
MmsValue* valueList = NULL;
MmsValue* value = NULL;
int elementCount = listSize;
if ((elementCount > 1) || createArray)
valueList = MmsValue_createEmptyArray(elementCount);
int i = 0;
for (i = 0; i < elementCount; i++) {
AccessResult_PR presentType = accessResultList[i]->present;
if (presentType == AccessResult_PR_failure) {
if (DEBUG_MMS_CLIENT) printf("access error!\n");
if (accessResultList[i]->choice.failure.size > 0) {
int errorCode = (int) accessResultList[i]->choice.failure.buf[0];
MmsDataAccessError dataAccessError = DATA_ACCESS_ERROR_UNKNOWN;
if ((errorCode >= 0) && (errorCode < 12))
dataAccessError = (MmsDataAccessError) errorCode;
value = MmsValue_newDataAccessError(dataAccessError);
}
else
value = MmsValue_newDataAccessError(DATA_ACCESS_ERROR_UNKNOWN);
}
else if (presentType == AccessResult_PR_array) {
value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue));
value->type = MMS_ARRAY;
int arrayElementCount =
accessResultList[i]->choice.array.list.count;
value->value.structure.size = arrayElementCount;
value->value.structure.components = (MmsValue**) GLOBAL_CALLOC(arrayElementCount, sizeof(MmsValue*));
int j;
for (j = 0; j < arrayElementCount; j++) {
value->value.structure.components[j] = mmsMsg_parseDataElement(
accessResultList[i]->choice.array.list.array[j]);
}
}
else if (presentType == AccessResult_PR_structure) {
value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue));
value->type = MMS_STRUCTURE;
int componentCount =
accessResultList[i]->choice.structure.list.count;
value->value.structure.size = componentCount;
value->value.structure.components = (MmsValue**) GLOBAL_CALLOC(componentCount, sizeof(MmsValue*));
int j;
for (j = 0; j < componentCount; j++) {
value->value.structure.components[j] = mmsMsg_parseDataElement(
accessResultList[i]->choice.structure.list.array[j]);
}
}
else if (presentType == AccessResult_PR_bitstring) {
value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue));
value->type = MMS_BIT_STRING;
int size = accessResultList[i]->choice.bitstring.size;
value->value.bitString.size = (size * 8)
- accessResultList[i]->choice.bitstring.bits_unused;
value->value.bitString.buf = (uint8_t*) GLOBAL_MALLOC(size);
memcpy(value->value.bitString.buf,
accessResultList[i]->choice.bitstring.buf, size);
}
else if (presentType == AccessResult_PR_integer) {
Asn1PrimitiveValue* berInteger =
BerInteger_createFromBuffer(accessResultList[i]->choice.integer.buf,
accessResultList[i]->choice.integer.size);
value = MmsValue_newIntegerFromBerInteger(berInteger);
}
else if (presentType == AccessResult_PR_unsigned) {
Asn1PrimitiveValue* berInteger =
BerInteger_createFromBuffer(accessResultList[i]->choice.Unsigned.buf,
accessResultList[i]->choice.Unsigned.size);
value = MmsValue_newUnsignedFromBerInteger(berInteger);
}
else if (presentType == AccessResult_PR_floatingpoint) {
int size = accessResultList[i]->choice.floatingpoint.size;
value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue));
value->type = MMS_FLOAT;
if (size == 5) { /* FLOAT32 */
value->value.floatingPoint.formatWidth = 32;
value->value.floatingPoint.exponentWidth = accessResultList[i]->choice.floatingpoint.buf[0];
uint8_t* floatBuf = (accessResultList[i]->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 = accessResultList[i]->choice.floatingpoint.buf[0];
uint8_t* floatBuf = (accessResultList[i]->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 (presentType == AccessResult_PR_visiblestring) {
value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue));
value->type = MMS_VISIBLE_STRING;
int strSize = accessResultList[i]->choice.visiblestring.size;
value->value.visibleString.buf = (char*) GLOBAL_MALLOC(strSize + 1);
value->value.visibleString.size = strSize;
memcpy(value->value.visibleString.buf,
accessResultList[i]->choice.visiblestring.buf,
strSize);
value->value.visibleString.buf[strSize] = 0;
}
else if (presentType == AccessResult_PR_mMSString) {
value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue));
value->type = MMS_STRING;
int strSize = accessResultList[i]->choice.mMSString.size;
value->value.visibleString.buf = (char*) GLOBAL_MALLOC(strSize + 1);
value->value.visibleString.size = strSize;
memcpy(value->value.visibleString.buf,
accessResultList[i]->choice.mMSString.buf, strSize);
value->value.visibleString.buf[strSize] = 0;
}
else if (presentType == AccessResult_PR_utctime) {
value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue));
value->type = MMS_UTC_TIME;
memcpy(value->value.utcTime,
accessResultList[i]->choice.utctime.buf, 8);
}
else if (presentType == AccessResult_PR_boolean) {
value = MmsValue_newBoolean(accessResultList[i]->choice.boolean);
}
else if (presentType == AccessResult_PR_binarytime) {
int size = accessResultList[i]->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, accessResultList[i]->choice.binarytime.buf, size);
}
}
else if (presentType == AccessResult_PR_octetstring) {
int size = accessResultList[i]->choice.octetstring.size;
value = (MmsValue*) GLOBAL_CALLOC(1, sizeof(MmsValue));
value->type = MMS_OCTET_STRING;
value->value.octetString.maxSize = size;
value->value.octetString.size = size;
value->value.octetString.buf = (uint8_t*) GLOBAL_MALLOC(size);
memcpy(value->value.octetString.buf, accessResultList[i]->choice.octetstring.buf, size);
}
else {
printf("unknown type %i\n", presentType);
value = MmsValue_newDataAccessError(DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID);
}
if ((elementCount > 1) || createArray)
MmsValue_setElement(valueList, i, value);
}
if (valueList == NULL)
valueList = value;
return valueList;
}
Data_t*
mmsMsg_createBasicDataElement(MmsValue* value)
{
Data_t* dataElement = (Data_t*) GLOBAL_CALLOC(1, sizeof(Data_t));
switch (value->type) {
case MMS_ARRAY:
{
int size = MmsValue_getArraySize(value);
dataElement->present = Data_PR_array;
dataElement->choice.array = (DataSequence_t*) GLOBAL_CALLOC(1, sizeof(DataSequence_t));
dataElement->choice.array->list.count = size;
dataElement->choice.array->list.size = size;
dataElement->choice.array->list.array = (Data_t**) GLOBAL_CALLOC(size, sizeof(Data_t*));
int i;
for (i = 0; i < size; i++) {
dataElement->choice.array->list.array[i] =
mmsMsg_createBasicDataElement(MmsValue_getElement(value, i));
}
}
break;
case MMS_STRUCTURE:
{
int size = value->value.structure.size;
dataElement->present = Data_PR_structure;
dataElement->choice.structure = (DataSequence_t*) GLOBAL_CALLOC(1, sizeof(DataSequence_t));
dataElement->choice.structure->list.count = size;
dataElement->choice.structure->list.size = size;
dataElement->choice.structure->list.array = (Data_t**) GLOBAL_CALLOC(size, sizeof(Data_t*));
int i;
for (i = 0; i < size; i++) {
dataElement->choice.structure->list.array[i] = mmsMsg_createBasicDataElement(
value->value.structure.components[i]);
}
}
break;
case MMS_BIT_STRING:
{
dataElement->present = Data_PR_bitstring;
dataElement->choice.bitstring.buf = value->value.bitString.buf;
int size = (value->value.bitString.size / 8) + ((value->value.bitString.size % 8) > 0);
int unused = 8 - (value->value.bitString.size % 8);
dataElement->choice.bitstring.size = size; /* size in bytes */
dataElement->choice.bitstring.bits_unused = unused;
}
break;
case MMS_BOOLEAN:
dataElement->present = Data_PR_boolean;
dataElement->choice.boolean = value->value.boolean;
break;
case MMS_FLOAT:
dataElement->present = Data_PR_floatingpoint;
mmsMsg_createFloatData(value, &dataElement->choice.floatingpoint.size,
&dataElement->choice.floatingpoint.buf);
break;
case MMS_UTC_TIME:
dataElement->present = Data_PR_utctime;
dataElement->choice.utctime.buf = (uint8_t*) GLOBAL_MALLOC(8);
memcpy(dataElement->choice.utctime.buf, value->value.utcTime, 8);
dataElement->choice.utctime.size = 8;
break;
case MMS_INTEGER:
dataElement->present = Data_PR_integer;
dataElement->choice.integer.size = value->value.integer->size;
dataElement->choice.integer.buf = value->value.integer->octets;
break;
case MMS_UNSIGNED:
dataElement->present = Data_PR_unsigned;
dataElement->choice.Unsigned.size = value->value.integer->size;
dataElement->choice.Unsigned.buf = value->value.integer->octets;
break;
case MMS_VISIBLE_STRING:
dataElement->present = Data_PR_visiblestring;
if (value->value.visibleString.buf != NULL ) {
dataElement->choice.visiblestring.buf = (uint8_t*) value->value.visibleString.buf;
dataElement->choice.visiblestring.size = strlen(value->value.visibleString.buf);
} else
dataElement->choice.visiblestring.size = 0;
break;
case MMS_OCTET_STRING:
dataElement->present = Data_PR_octetstring;
if (value->value.octetString.buf != NULL ) {
dataElement->choice.octetstring.buf = value->value.octetString.buf;
dataElement->choice.octetstring.size =
value->value.octetString.size;
} else
dataElement->choice.octetstring.size = 0;
break;
case MMS_STRING:
dataElement->present = Data_PR_mMSString;
if (value->value.visibleString.buf != NULL ) {
dataElement->choice.mMSString.buf = (uint8_t*) value->value.visibleString.buf;
dataElement->choice.mMSString.size = strlen(value->value.visibleString.buf);
} else
dataElement->choice.mMSString.size = 0;
break;
case MMS_BINARY_TIME:
dataElement->present = Data_PR_binarytime;
dataElement->choice.binarytime.size = value->value.binaryTime.size;
dataElement->choice.binarytime.buf = value->value.binaryTime.buf;
break;
default:
dataElement->present = Data_PR_NOTHING;
break;
}
return dataElement;
}