diff --git a/src/sampled_values/sv_publisher.c b/src/sampled_values/sv_publisher.c index 75e48c72..d5ee8d05 100644 --- a/src/sampled_values/sv_publisher.c +++ b/src/sampled_values/sv_publisher.c @@ -84,16 +84,9 @@ struct sSVPublisher { }; -static void -preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* interfaceID) +static bool +preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* interfaceId) { - uint8_t srcAddr[6]; - - if (interfaceID != NULL) - Ethernet_getInterfaceMACAddress(interfaceID, srcAddr); - else - Ethernet_getInterfaceMACAddress(CONFIG_ETHERNET_INTERFACE_ID, srcAddr); - uint8_t defaultDstAddr[] = CONFIG_SV_DEFAULT_DST_ADDRESS; uint8_t* dstAddr; @@ -101,6 +94,13 @@ preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* in uint16_t vlanId; uint16_t appId; + uint8_t srcAddr[6]; + + if (interfaceId != NULL) + Ethernet_getInterfaceMACAddress(interfaceId, srcAddr); + else + Ethernet_getInterfaceMACAddress(CONFIG_ETHERNET_INTERFACE_ID, srcAddr); + if (parameters == NULL) { dstAddr = defaultDstAddr; priority = CONFIG_SV_DEFAULT_PRIORITY; @@ -114,11 +114,19 @@ preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* in appId = parameters->appId; } - if (interfaceID != NULL) - self->ethernetSocket = Ethernet_createSocket(interfaceID, dstAddr); + if (interfaceId != NULL) + self->ethernetSocket = Ethernet_createSocket(interfaceId, dstAddr); else self->ethernetSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, dstAddr); + if (self->ethernetSocket == NULL) { + + if (DEBUG_SV_PUBLISHER) + printf("SV_PUBLISHER: Failed to allocate Ethernet interface\n"); + + return false; + } + self->buffer = (uint8_t*) GLOBAL_MALLOC(SV_MAX_MESSAGE_SIZE); memcpy(self->buffer, dstAddr, 6); @@ -161,6 +169,8 @@ preparePacketBuffer(SVPublisher self, CommParameters* parameters, const char* in self->buffer[bufPos++] = 0x00; self->payloadStart = bufPos; + + return true; } @@ -287,9 +297,15 @@ SVPublisher_create(CommParameters* parameters, const char* interfaceId) { SVPublisher self = (SVPublisher) GLOBAL_CALLOC(1, sizeof(struct sSVPublisher)); - self->asduList = NULL; + if (self) { + self->asduList = NULL; - preparePacketBuffer(self, parameters, interfaceId); + if (preparePacketBuffer(self, parameters, interfaceId) == false) { + GLOBAL_FREEMEM(self); + self = NULL; + } + + } return self; } diff --git a/src/sampled_values/sv_subscriber.c b/src/sampled_values/sv_subscriber.c index 9e03607c..3e3fe093 100644 --- a/src/sampled_values/sv_subscriber.c +++ b/src/sampled_values/sv_subscriber.c @@ -160,15 +160,20 @@ svReceiverLoop(void* threadParameter) self->running = true; self->stopped = false; - SVReceiver_startThreadless(self); + if (SVReceiver_startThreadless(self)) { - while (self->running) { + while (self->running) { - if (SVReceiver_tick(self) == false) - Thread_sleep(1); - } + if (SVReceiver_tick(self) == false) + Thread_sleep(1); + } - SVReceiver_stopThreadless(self); + SVReceiver_stopThreadless(self); + } + else { + if (DEBUG_SV_SUBSCRIBER) + printf("SV_SUBSCRIBER: Failed to start SV receiver\n"); + } self->stopped = true; } @@ -229,9 +234,12 @@ SVReceiver_startThreadless(SVReceiver self) else self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL); - Ethernet_setProtocolFilter(self->ethSocket, ETH_P_SV); + if (self->ethSocket) { - self->running = true; + Ethernet_setProtocolFilter(self->ethSocket, ETH_P_SV); + + self->running = true; + } return self->ethSocket; }