- SV publisher/subscriber: improved error handling when configuration is wrong

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

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