diff --git a/.vscode/settings.json b/.vscode/settings.json
index 822f36d..38c93d2 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -26,6 +26,7 @@
         "mqtt_handler.h": "c",
         "cstdlib": "c",
         "modbus_master.h": "c",
-        "ch32v003_gpio_branchless.h": "c"
+        "ch32v003_gpio_branchless.h": "c",
+        "mqttpacket.h": "c"
     }
 }
\ No newline at end of file
diff --git a/include/gpio.h b/include/gpio.h
index ca8be97..5851d5f 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -3,7 +3,20 @@
 
 #include <stdint.h>
 
-// Function prototype for initializing GPIO
+// Status states
+typedef enum {
+  LED_STATE_OFF,
+  LED_STATE_ON,       // ok
+  LED_STATE_WARNING,  // slow blink
+  LED_STATE_ERROR,    // fast blink
+  LED_STATE_BUSY      // "breathing"
+} led_state_t;
+
+// Initialize GPIO
 void init_gpio(void);
 
+// LED status handling
+void led_status_set(led_state_t state);
+void led_status_process(void);
+
 #endif  // GPIO_H
\ No newline at end of file
diff --git a/include/mqtt_handler.h b/include/mqtt_handler.h
index 740445e..aed0b47 100644
--- a/include/mqtt_handler.h
+++ b/include/mqtt_handler.h
@@ -25,6 +25,7 @@ typedef struct {
   uint32_t last_yield;
   bool is_connected;
   char base_topic[64];
+  bool discovery_published;
 } mqtt_state_t;
 
 extern char nodes_list[MAX_PAYLOAD_LENGTH];
diff --git a/lib/ioLibrary_Driver/MQTT/MQTTClient.c b/lib/ioLibrary_Driver/MQTT/MQTTClient.c
index 5fdfd39..21d309a 100644
--- a/lib/ioLibrary_Driver/MQTT/MQTTClient.c
+++ b/lib/ioLibrary_Driver/MQTT/MQTTClient.c
@@ -42,7 +42,7 @@ static int sendPacket(MQTTClient* c, int length, Timer* timer)
     if (sent == length)
     {
         TimerCountdown(&c->ping_timer, c->keepAliveInterval); // record the fact that we have successfully sent the packet
-        rc = SUCCESSS;
+        rc = SUCCESS;
     }
     else
         rc = FAILURE;
@@ -111,8 +111,13 @@ static int readPacket(MQTTClient* c, Timer* timer)
     int rem_len = 0;
 
     /* 1. read the header byte. This has the packet type in it */
-    if (c->ipstack->mqttread(c->ipstack, c->readbuf, 1, TimerLeftMS(timer)) != 1)
-        goto exit;
+    int read_len = c->ipstack->mqttread(c->ipstack, c->readbuf, 1, TimerLeftMS(timer));
+    if (read_len == 0) {  // Timeout/no data
+        return SUCCESS;
+    }
+    if (read_len < 0) {
+        return FAILURE;
+    }
 
     len = 1;
     /* 2. read the remaining length. This is variable in itself */
@@ -177,7 +182,7 @@ int deliverMessage(MQTTClient* c, MQTTString* topicName, MQTTMessage* message)
                 MessageData md;
                 NewMessageData(&md, topicName, message);
                 c->messageHandlers[i].fp(&md);
-                rc = SUCCESSS;
+                rc = SUCCESS;
             }
         }
     }
@@ -187,7 +192,7 @@ int deliverMessage(MQTTClient* c, MQTTString* topicName, MQTTMessage* message)
         MessageData md;
         NewMessageData(&md, topicName, message);
         c->defaultMessageHandler(&md);
-        rc = SUCCESSS;
+        rc = SUCCESS;
     }
 
     return rc;
@@ -196,14 +201,16 @@ int deliverMessage(MQTTClient* c, MQTTString* topicName, MQTTMessage* message)
 
 int keepalive(MQTTClient* c)
 {
-    int rc = SUCCESSS;
+    int rc = SUCCESS;
 
     if (c->keepAliveInterval == 0)
+    {
         goto exit;
+    }
 
     if (!c->isconnected)
     {
-        rc = FAILURE;
+        // rc = FAILURE;
         goto exit;
     }
 
@@ -223,7 +230,7 @@ int keepalive(MQTTClient* c)
             }
 
             rc = sendPacket(c, len, &timer);
-            if (rc == SUCCESSS)
+            if (rc == SUCCESS)
             {
                 c->ping_outstanding = 1;
                 TimerCountdown(&c->ping_timer, c->keepAliveInterval);
@@ -239,14 +246,16 @@ exit:
     return rc;
 }
 
-
 int cycle(MQTTClient* c, Timer* timer)
 {
     // read the socket, see what work is due
-    unsigned short packet_type = readPacket(c, timer);
+    int packet_type = readPacket(c, timer);
+    if (packet_type == FAILURE) {
+        return FAILURE;
+    }
 
     int len = 0,
-        rc = SUCCESSS;
+        rc = SUCCESS;
 
     switch (packet_type)
     {
@@ -287,7 +296,7 @@ int cycle(MQTTClient* c, Timer* timer)
                 rc = FAILURE;
             else if ((len = MQTTSerialize_ack(c->buf, c->buf_size, PUBREL, 0, mypacketid)) <= 0)
                 rc = FAILURE;
-            else if ((rc = sendPacket(c, len, timer)) != SUCCESSS) // send the PUBREL packet
+            else if ((rc = sendPacket(c, len, timer)) != SUCCESS) // send the PUBREL packet
                 rc = FAILURE; // there was a problem
             if (rc == FAILURE)
                 goto exit; // there was a problem
@@ -299,9 +308,12 @@ int cycle(MQTTClient* c, Timer* timer)
             c->ping_outstanding = 0;
             break;
     }
-    keepalive(c);
+    rc = keepalive(c);  // Check keepalive return value
+    if (rc != SUCCESS) {
+        return rc;
+    }
 exit:
-    if (rc == SUCCESSS)
+    if (rc == SUCCESS)
         rc = packet_type;
     return rc;
 }
@@ -309,7 +321,7 @@ exit:
 
 int MQTTYield(MQTTClient* c, int timeout_ms)
 {
-    int rc = SUCCESSS;
+    int rc = SUCCESS;
     Timer timer;
 
     TimerInit(&timer);
@@ -391,7 +403,7 @@ int MQTTConnect(MQTTClient* c, MQTTPacket_connectData* options)
     TimerCountdown(&c->ping_timer, c->keepAliveInterval);
     if ((len = MQTTSerialize_connect(c->buf, c->buf_size, options)) <= 0)
         goto exit;
-    if ((rc = sendPacket(c, len, &connect_timer)) != SUCCESSS)  // send the connect packet
+    if ((rc = sendPacket(c, len, &connect_timer)) != SUCCESS)  // send the connect packet
         goto exit; // there was a problem
 
     // this will be a blocking call, wait for the connack
@@ -408,7 +420,7 @@ int MQTTConnect(MQTTClient* c, MQTTPacket_connectData* options)
         rc = FAILURE;
 
 exit:
-    if (rc == SUCCESSS)
+    if (rc == SUCCESS)
         c->isconnected = 1;
 
 #if defined(MQTT_TASK)
@@ -442,7 +454,7 @@ int MQTTSubscribe(MQTTClient* c, const char* topicFilter, enum QoS qos, messageH
     len = MQTTSerialize_subscribe(c->buf, c->buf_size, 0, getNextPacketId(c), 1, &topic, &charQos);
     if (len <= 0)
         goto exit;
-    if ((rc = sendPacket(c, len, &timer)) != SUCCESSS) // send the subscribe packet
+    if ((rc = sendPacket(c, len, &timer)) != SUCCESS) // send the subscribe packet
         goto exit;             // there was a problem
 
     if (waitfor(c, SUBACK, &timer) == SUBACK)      // wait for suback
@@ -497,7 +509,7 @@ int MQTTUnsubscribe(MQTTClient* c, const char* topicFilter)
 
     if ((len = MQTTSerialize_unsubscribe(c->buf, c->buf_size, 0, getNextPacketId(c), 1, &topic)) <= 0)
         goto exit;
-    if ((rc = sendPacket(c, len, &timer)) != SUCCESSS) // send the subscribe packet
+    if ((rc = sendPacket(c, len, &timer)) != SUCCESS) // send the subscribe packet
         goto exit; // there was a problem
 
     if (waitfor(c, UNSUBACK, &timer) == UNSUBACK)
@@ -541,7 +553,7 @@ int MQTTPublish(MQTTClient* c, const char* topicName, MQTTMessage* message)
               topic, (unsigned char*)message->payload, message->payloadlen);
     if (len <= 0)
         goto exit;
-    if ((rc = sendPacket(c, len, &timer)) != SUCCESSS) // send the subscribe packet
+    if ((rc = sendPacket(c, len, &timer)) != SUCCESS) // send the subscribe packet
         goto exit; // there was a problem
 
     if (message->qos == QOS1)
diff --git a/lib/ioLibrary_Driver/MQTT/MQTTClient.h b/lib/ioLibrary_Driver/MQTT/MQTTClient.h
index dfc372c..9e2ab7a 100644
--- a/lib/ioLibrary_Driver/MQTT/MQTTClient.h
+++ b/lib/ioLibrary_Driver/MQTT/MQTTClient.h
@@ -48,7 +48,14 @@
 enum QoS { QOS0, QOS1, QOS2 };
 
 /* all failure return codes must be negative */
-enum returnCode { BUFFER_OVERFLOW = -2, FAILURE = -1, SUCCESSS = 0 };
+enum returnCode { 
+    NO_DATA = -100,          /* No data available (timeout) */
+    PROTOCOL_ERROR = -4,     /* Invalid packet type or format */
+    DECODE_ERROR = -3,       /* Failed to decode packet length */
+    BUFFER_OVERFLOW = -2,    /* Buffer too small */
+    FAILURE = -1,           /* Generic failure */
+    SUCCESS = 0 
+};
 
 /* The Platform specific header must define the Network and Timer structures and functions
  * which operate on them.
diff --git a/lib/onewire/onewire.c b/lib/onewire/onewire.c
index 9728d36..b42454a 100644
--- a/lib/onewire/onewire.c
+++ b/lib/onewire/onewire.c
@@ -289,19 +289,19 @@ uint8_t OneWireReset(void) {
   // wait until the wire is high... just in case
   do {
     if (--retries == 0) return 0;
-    Delay_Us(2);
+    Delay_Us(ONEWIRE_RESET_RETRY_TIME);
   } while (!DIRECT_READ());
 
   DIRECT_WRITE_LOW();
   DIRECT_MODE_OUTPUT();  // drive output low
 
-  Delay_Us(480);
+  Delay_Us(ONEWIRE_RESET_LOW_TIME);
 
   DIRECT_MODE_INPUT();  // allow it to float
-  Delay_Us(70);
+  Delay_Us(ONEWIRE_RESET_SAMPLE_TIME);
   r = !DIRECT_READ();
 
-  Delay_Us(410);
+  Delay_Us(ONEWIRE_RESET_POST_TIME);
   return r;
 }
 
@@ -313,17 +313,17 @@ void OneWireWriteBit(uint8_t v) {
   if (v & 1) {
     DIRECT_WRITE_LOW();
     DIRECT_MODE_OUTPUT();  // drive output low
-    Delay_Us(10);
+    Delay_Us(ONEWIRE_WRITE_1_LOW_TIME);
     DIRECT_WRITE_HIGH();  // drive output high
 
-    Delay_Us(55);
+    Delay_Us(ONEWIRE_WRITE_1_TOTAL_TIME - ONEWIRE_WRITE_1_LOW_TIME);
   } else {
     DIRECT_WRITE_LOW();
     DIRECT_MODE_OUTPUT();  // drive output low
-    Delay_Us(65);
+    Delay_Us(ONEWIRE_WRITE_0_LOW_TIME);
     DIRECT_WRITE_HIGH();  // drive output high
 
-    Delay_Us(5);
+    Delay_Us(ONEWIRE_WRITE_0_TOTAL_TIME - ONEWIRE_WRITE_0_LOW_TIME);
   }
 }
 
@@ -336,12 +336,17 @@ uint8_t OneWireReadBit(void) {
 
   DIRECT_MODE_OUTPUT();
   DIRECT_WRITE_LOW();
-  Delay_Us(3);
-  DIRECT_MODE_INPUT();  // let pin float, pull up will raise
-  Delay_Us(10);
+  Delay_Us(ONEWIRE_READ_INIT_LOW_TIME);  // 6us initial low pulse
+
+  DIRECT_MODE_INPUT();                 // let pin float, pull up will raise
+  Delay_Us(ONEWIRE_READ_SAMPLE_TIME);  // 8us until sample point
   r = DIRECT_READ();
 
-  Delay_Us(53);
+  // Wait for remainder of the read timeslot
+  // Total - init_low - sample = 64 - 6 - 8 = 50us
+  Delay_Us(ONEWIRE_READ_TOTAL_TIME - ONEWIRE_READ_INIT_LOW_TIME -
+           ONEWIRE_READ_SAMPLE_TIME);
+
   return r;
 }
 
diff --git a/lib/onewire/onewire.h b/lib/onewire/onewire.h
index 078a53e..9cd792b 100644
--- a/lib/onewire/onewire.h
+++ b/lib/onewire/onewire.h
@@ -14,14 +14,14 @@ static inline __attribute__((always_inline)) uint8_t directRead() {
   return (GPIOB->INDR & (1 << 9)) ? 1 : 0;
 }
 
-static inline __attribute__((always_inline)) void directModeInput() {
+static inline void directModeInput() {
   GPIOB->CFGHR &= ~(0xF << (4 * (9 - 8)));
-  GPIOB->CFGHR |= (0x4 << (4 * (9 - 8)));
+  GPIOB->CFGHR |= (GPIO_CNF_IN_FLOATING << (4 * (9 - 8)));
 }
 
-static inline __attribute__((always_inline)) void directModeOutput() {
+static inline void directModeOutput() {
   GPIOB->CFGHR &= ~(0xF << (4 * (9 - 8)));
-  GPIOB->CFGHR |= (0x3 << (4 * (9 - 8)));
+  GPIOB->CFGHR |= ((GPIO_Speed_50MHz | GPIO_CNF_OUT_PP) << (4 * (9 - 8)));
 }
 
 static inline __attribute__((always_inline)) void directWriteLow() {
@@ -39,6 +39,27 @@ static inline __attribute__((always_inline)) void directWriteHigh() {
 #define DIRECT_MODE_OUTPUT() directModeOutput()
 #endif
 
+// timing configuration
+// time between line check retries
+#define ONEWIRE_RESET_RETRY_TIME 2
+// reset cycle
+#define ONEWIRE_RESET_LOW_TIME 480
+#define ONEWIRE_RESET_SAMPLE_TIME 60
+#define ONEWIRE_RESET_POST_TIME 410
+
+// write 1 bit
+#define ONEWIRE_WRITE_1_LOW_TIME 6
+#define ONEWIRE_WRITE_1_TOTAL_TIME 64
+
+// write 0 bit
+#define ONEWIRE_WRITE_0_LOW_TIME 80
+#define ONEWIRE_WRITE_0_TOTAL_TIME 84
+
+// read bit
+#define ONEWIRE_READ_INIT_LOW_TIME 6
+#define ONEWIRE_READ_SAMPLE_TIME 8
+#define ONEWIRE_READ_TOTAL_TIME 64
+
 // OneWire Function Declarations
 
 // Initialize the OneWire bus
diff --git a/src/gpio.c b/src/gpio.c
index 9be9636..4a87d0e 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -1,6 +1,28 @@
 #include "gpio.h"
 
+#include <stdbool.h>
+
 #include "ch32v003fun.h"
+#include "systick.h"
+
+#define LED_BLINK_SLOW 1000     // Warning blink interval (ms)
+#define LED_BLINK_FAST 500      // Error blink interval (ms)
+#define LED_BREATH_PERIOD 2000  // Breathing effect period (ms)
+#define STATE_STABILITY 500    // Minimum time before state change (ms)
+
+#define LED_G (1 << 4)
+#define LED_B (1 << 3)
+
+typedef struct {
+  uint32_t last_update;
+  uint32_t stable_since;
+  led_state_t current_state;
+  led_state_t target_state;
+  uint8_t blink_state;
+  uint32_t last_blink;
+} led_status_t;
+
+static led_status_t led_status = {0};
 
 void init_gpio(void) {
   // Enable clock for GPIOB
@@ -11,8 +33,66 @@ void init_gpio(void) {
   GPIOB->CFGLR |= ((GPIO_Speed_10MHz | GPIO_CNF_OUT_PP) << (4 * 3)) |
                   ((GPIO_Speed_10MHz | GPIO_CNF_OUT_PP) << (4 * 4));
 
-  // XXX: SysTick debug
   // GPIOB: Pin 9 as Output, Push-Pull, 10MHz
   GPIOB->CFGHR &= ~(0xF << (4 * (9 - 8)));
   GPIOB->CFGHR |= (GPIO_Speed_10MHz | GPIO_CNF_OUT_PP) << (4 * (9 - 8));
+
+  led_status.current_state = LED_STATE_OFF;
+  led_status.target_state = LED_STATE_OFF;
+
+  GPIOB->BSHR = LED_G | LED_B;
 }
+
+void led_status_set(led_state_t state) {
+  uint32_t now = millis();
+
+  // stability timer upd on state change
+  if (led_status.target_state != state) {
+    led_status.stable_since = now;
+    led_status.target_state = state;
+  }
+}
+
+static inline void leds(bool g, bool b) {
+  uint32_t val = (g ? LED_G << 16 : LED_G) | (b ? LED_B << 16 : LED_B);
+  GPIOB->BSHR = val;
+}
+
+void led_status_process(void) {
+  uint32_t now = millis();
+
+  if (now - led_status.stable_since < STATE_STABILITY) {
+    return;
+  }
+
+  switch (led_status.target_state) {
+    case LED_STATE_OFF:
+      GPIOB->BSHR = LED_G | LED_B;
+      break;
+
+    case LED_STATE_ON:
+      leds(1, 0);  // green on, blue off
+      break;
+
+    case LED_STATE_WARNING:
+      if (now - led_status.last_blink >= LED_BLINK_SLOW) {
+        led_status.blink_state = !led_status.blink_state;
+        led_status.last_blink = now;
+        leds(led_status.blink_state, 0);
+      }
+      break;
+
+    case LED_STATE_ERROR:
+      if (now - led_status.last_blink >= LED_BLINK_FAST) {
+        led_status.blink_state = !led_status.blink_state;
+        led_status.last_blink = now;
+        leds(led_status.blink_state, !led_status.blink_state);
+      }
+      break;
+
+    case LED_STATE_BUSY:
+      bool blue_on = (now % LED_BREATH_PERIOD) < LED_BREATH_PERIOD / 2;
+      leds(0, blue_on);
+      break;
+  }
+}
\ No newline at end of file
diff --git a/src/main.c b/src/main.c
index aa72cf1..4bc14c6 100644
--- a/src/main.c
+++ b/src/main.c
@@ -4,6 +4,7 @@
 #include "config.h"
 #include "debug.h"
 #include "dhcp.h"
+#include "gpio.h"
 #include "modbus_handler.h"
 #include "mqtt_handler.h"
 #include "onewire_temp.h"
@@ -29,12 +30,8 @@ int main(void) {
 
   configure_network();
   dhcp_init();
-
-  if (!wait_for_dhcp()) {
-    while (1) {
-      // TODO: Implement proper error handling
-    }
-  }
+  // block forever until dhcp resolves
+  wait_for_dhcp();
 
   // init handlers
   // init sensors before mqtt so we can add them to discovery
@@ -51,17 +48,19 @@ int main(void) {
   while (1) {
     uint32_t current_time = millis();
 
+    led_status_process();
     dhcp_process();
     mqtt_process(&mqtt_state);
     modbus_handler_process();
 
-    // TODO: doesn't make sense to convert every 1s, should be the same interval as publish
+    // TODO: doesn't make sense to convert every 1s, should be the same interval
+    // as publish
     if (current_time - last_temp_conversion >= CONVERSION_INTERVAL) {
       onewire_temp_start_parallel();
       last_temp_conversion = current_time;
     }
 
-    onewire_temp_process();  // Process all sensors
+    onewire_temp_process();  // process all sensors
 
     if (current_time - last_temp_publish >= TEMP_PUBLISH_INTERVAL) {
       if (mqtt_state.is_connected) {
diff --git a/src/mqtt_handler.c b/src/mqtt_handler.c
index 22ae04a..3164bb4 100644
--- a/src/mqtt_handler.c
+++ b/src/mqtt_handler.c
@@ -5,13 +5,14 @@
 #include <string.h>
 
 #include "debug.h"
+#include "gpio.h"
 #include "modbus_handler.h"
 #include "onewire_temp.h"
 #include "systick.h"
 
 // MQTT
 #define MQTT_YIELD_INTERVAL 100  // 100ms between yields in main loop
-#define MQTT_MAX_PACKET_WAIT 20  // Only wait up to 20ms for packet processing
+#define MQTT_MAX_PACKET_WAIT 20   // Only wait up to 20ms for packet processing
 #define MQTT_RECONNECT_INTERVAL 5000  // 5 seconds between reconnection attempts
 
 // Homie convention constants
@@ -26,9 +27,9 @@ char nodes_list[MAX_PAYLOAD_LENGTH];
 
 // Parse Homie topic format: homie/node-id/device-name/property/[set|get]
 static bool parse_homie_topic(const char* topic, size_t topic_len,
-                                 char* device_name, size_t name_max,
-                                 char* property, size_t prop_max,
-                                 uint8_t* is_set) {
+                              char* device_name, size_t name_max,
+                              char* property, size_t prop_max,
+                              uint8_t* is_set) {
   const char* segment_start = topic;
   const char* topic_end = topic + topic_len;
   uint8_t segment = 0;
@@ -359,16 +360,15 @@ void message_arrived(MessageData* md) {
 void mqtt_process(mqtt_state_t* state) {
   uint32_t now = millis();
   int rc;
-  static uint8_t discovery_published = 0;
 
   if (!state->is_connected) {
     if (now - state->last_reconnect >= MQTT_RECONNECT_INTERVAL) {
       rc = setup_mqtt_client(&state->network, &state->opts, &state->client);
 
-      if (rc == 0) {
+      if (rc == SUCCESS) {
         state->is_connected = true;
 
-        if (!discovery_published) {
+        if (!state->discovery_published) {
           publish_device_attributes(&state->client);
 
           for (int i = 0; i < RS485_DEVICE_COUNT; i++) {
@@ -379,7 +379,7 @@ void mqtt_process(mqtt_state_t* state) {
           // is it worth implementing?
           onewire_temp_publish_discovery(&state->client, NODE_CONFIG.id);
 
-          discovery_published = 1;
+          state->discovery_published = 1;
         }
 
         char sub_topic[MAX_TOPIC_LENGTH];
@@ -388,17 +388,23 @@ void mqtt_process(mqtt_state_t* state) {
 
         rc = subscribe_to_topic(&state->client, sub_topic, QOS1,
                                 message_arrived);
-        if (rc != 0) {
+        if (rc != SUCCESS) {
           state->is_connected = false;
         }
       }
       state->last_reconnect = now;
     }
+    led_status_set(LED_STATE_BUSY);
+
   } else if (now - state->last_yield >= MQTT_YIELD_INTERVAL) {
     rc = MQTTYield(&state->client, MQTT_MAX_PACKET_WAIT);
-    if (rc != 0) {
+    if (rc != SUCCESS) {
+      DEBUG_PRINT("MQTT Yield failed with rc=%d, ping_outstanding=%d\n", rc,
+                  state->client.ping_outstanding);
       state->is_connected = false;
+      state->discovery_published = false;
     }
     state->last_yield = now;
+    led_status_set(LED_STATE_ON);
   }
 }
\ No newline at end of file
diff --git a/src/system_init.c b/src/system_init.c
index 67547fe..d187d4d 100644
--- a/src/system_init.c
+++ b/src/system_init.c
@@ -25,12 +25,12 @@ void init_system(void) {
 }
 
 bool wait_for_dhcp(void) {
-  uint32_t start = millis();
+  // uint32_t start = millis();
   while (dhcp_get_state() != DHCP_STATE_LEASED) {
-    if (millis() - start >= DHCP_TIMEOUT_MS) {
-      DEBUG_PRINT("DHCP timeout\n");
-      return false;
-    }
+    // if (millis() - start >= DHCP_TIMEOUT_MS) {
+    // DEBUG_PRINT("DHCP timeout\n");
+    // return false;
+    // }
     dhcp_process();
   }
   return true;