diff --git a/examples/goose_publisher/goose_publisher_example.c b/examples/goose_publisher/goose_publisher_example.c index f46b3fe7..52c13c6b 100644 --- a/examples/goose_publisher/goose_publisher_example.c +++ b/examples/goose_publisher/goose_publisher_example.c @@ -51,21 +51,26 @@ main(int argc, char** argv) */ GoosePublisher publisher = GoosePublisher_create(&gooseCommParameters, interface); - GoosePublisher_setGoCbRef(publisher, "simpleIOGenericIO/LLN0$GO$gcbAnalogValues"); - GoosePublisher_setConfRev(publisher, 1); - GoosePublisher_setDataSetRef(publisher, "simpleIOGenericIO/LLN0$AnalogValues"); + if (publisher) { + GoosePublisher_setGoCbRef(publisher, "simpleIOGenericIO/LLN0$GO$gcbAnalogValues"); + GoosePublisher_setConfRev(publisher, 1); + GoosePublisher_setDataSetRef(publisher, "simpleIOGenericIO/LLN0$AnalogValues"); - int i = 0; + int i = 0; - for (i = 0; i < 3; i++) { - Thread_sleep(1000); + for (i = 0; i < 3; i++) { + Thread_sleep(1000); - if (GoosePublisher_publish(publisher, dataSetValues) == -1) { - printf("Error sending message!\n"); - } - } + if (GoosePublisher_publish(publisher, dataSetValues) == -1) { + printf("Error sending message!\n"); + } + } - GoosePublisher_destroy(publisher); + GoosePublisher_destroy(publisher); + } + else { + printf("Failed to create GOOSE publisher. Reason can be that the Ethernet interface doesn't exist or you need root permissions.\n"); + } LinkedList_destroyDeep(dataSetValues, (LinkedListValueDeleteFunction) MmsValue_delete); } diff --git a/src/goose/goose_publisher.c b/src/goose/goose_publisher.c index 97732268..4ba245da 100644 --- a/src/goose/goose_publisher.c +++ b/src/goose/goose_publisher.c @@ -35,7 +35,7 @@ #define GOOSE_MAX_MESSAGE_SIZE 1518 -static void +static bool prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char* interfaceID, bool useVlanTags); struct sGoosePublisher { @@ -68,11 +68,19 @@ GoosePublisher_createEx(CommParameters* parameters, const char* interfaceID, boo { GoosePublisher self = (GoosePublisher) GLOBAL_CALLOC(1, sizeof(struct sGoosePublisher)); - prepareGooseBuffer(self, parameters, interfaceID, useVlanTag); + if (self) { - self->timestamp = MmsValue_newUtcTimeByMsTime(Hal_getTimeInMs()); + if (prepareGooseBuffer(self, parameters, interfaceID, useVlanTag)) { + self->timestamp = MmsValue_newUtcTimeByMsTime(Hal_getTimeInMs()); - GoosePublisher_reset(self); + GoosePublisher_reset(self); + } + else { + GoosePublisher_destroy(self); + self = NULL; + } + + } return self; } @@ -86,7 +94,9 @@ GoosePublisher_create(CommParameters* parameters, const char* interfaceID) void GoosePublisher_destroy(GoosePublisher self) { - Ethernet_destroySocket(self->ethernetSocket); + if (self->ethernetSocket) { + Ethernet_destroySocket(self->ethernetSocket); + } MmsValue_delete(self->timestamp); @@ -99,7 +109,9 @@ GoosePublisher_destroy(GoosePublisher self) if (self->dataSetRef != NULL) GLOBAL_FREEMEM(self->dataSetRef); - GLOBAL_FREEMEM(self->buffer); + if (self->buffer) + GLOBAL_FREEMEM(self->buffer); + GLOBAL_FREEMEM(self); } @@ -164,7 +176,7 @@ GoosePublisher_setTimeAllowedToLive(GoosePublisher self, uint32_t timeAllowedToL self->timeAllowedToLive = timeAllowedToLive; } -static void +static bool prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char* interfaceID, bool useVlanTags) { uint8_t srcAddr[6]; @@ -199,50 +211,57 @@ prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char* else self->ethernetSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, dstAddr); - self->buffer = (uint8_t*) GLOBAL_MALLOC(GOOSE_MAX_MESSAGE_SIZE); + if (self->ethernetSocket) { + self->buffer = (uint8_t*) GLOBAL_MALLOC(GOOSE_MAX_MESSAGE_SIZE); - memcpy(self->buffer, dstAddr, 6); - memcpy(self->buffer + 6, srcAddr, 6); + memcpy(self->buffer, dstAddr, 6); + memcpy(self->buffer + 6, srcAddr, 6); - int bufPos = 12; + int bufPos = 12; - if (useVlanTags) { - /* Priority tag - IEEE 802.1Q */ - self->buffer[bufPos++] = 0x81; - self->buffer[bufPos++] = 0x00; + if (useVlanTags) { + /* Priority tag - IEEE 802.1Q */ + self->buffer[bufPos++] = 0x81; + self->buffer[bufPos++] = 0x00; - uint8_t tci1 = priority << 5; - tci1 += vlanId / 256; + uint8_t tci1 = priority << 5; + tci1 += vlanId / 256; - uint8_t tci2 = vlanId % 256; + uint8_t tci2 = vlanId % 256; - self->buffer[bufPos++] = tci1; /* Priority + VLAN-ID */ - self->buffer[bufPos++] = tci2; /* VLAN-ID */ - } + self->buffer[bufPos++] = tci1; /* Priority + VLAN-ID */ + self->buffer[bufPos++] = tci2; /* VLAN-ID */ + } - /* EtherType GOOSE */ - self->buffer[bufPos++] = 0x88; - self->buffer[bufPos++] = 0xB8; + /* EtherType GOOSE */ + self->buffer[bufPos++] = 0x88; + self->buffer[bufPos++] = 0xB8; - /* APPID */ - self->buffer[bufPos++] = appId / 256; - self->buffer[bufPos++] = appId % 256; + /* APPID */ + self->buffer[bufPos++] = appId / 256; + self->buffer[bufPos++] = appId % 256; - self->lengthField = bufPos; + self->lengthField = bufPos; - /* Length */ - self->buffer[bufPos++] = 0x00; - self->buffer[bufPos++] = 0x08; + /* Length */ + self->buffer[bufPos++] = 0x00; + self->buffer[bufPos++] = 0x08; - /* Reserved1 */ - self->buffer[bufPos++] = 0x00; - self->buffer[bufPos++] = 0x00; + /* Reserved1 */ + self->buffer[bufPos++] = 0x00; + self->buffer[bufPos++] = 0x00; + + /* Reserved2 */ + self->buffer[bufPos++] = 0x00; + self->buffer[bufPos++] = 0x00; - /* Reserved2 */ - self->buffer[bufPos++] = 0x00; - self->buffer[bufPos++] = 0x00; + self->payloadStart = bufPos; - self->payloadStart = bufPos; + return true; + } + else { + return false; + } } static int32_t