Fix publish acknowledge

diff --git a/mqtt_sub/mqtt_comm.c b/mqtt_sub/mqtt_comm.c
index bc2fbcc..ecbb3eb 100644 (file)
--- a/mqtt_sub/mqtt_comm.c
+++ b/mqtt_sub/mqtt_comm.c
@@ -217,16 +217,24 @@ int mqtt_handle_packet(char* buf, int comm_uart, int dbg_uart)
                case MQTT_WAITING_PUBACK:
                case MQTT_WAITING_PUBREC:
                case MQTT_WAITING_PUBREL: {
-                       /* Check for this being a pubrel (release) and send pubcomp (complete */
-                       struct mqtt_publish_reply_pkt* ack = (struct mqtt_publish_reply_pkt*)(buf + HEADER_SIZE);
-                       int ret = mqtt_check_publish_reply_pkt(ack, MQTT_CONTROL_PUBREL);
+                       /* Check for this being a pubrel (release) and send pubcomp (complete) */
+                       struct mqtt_publish_reply_pkt* pub_comp = (struct mqtt_publish_reply_pkt*)(txbuf + HEADER_SIZE);
+                       struct mqtt_publish_reply_pkt* pub_rel = (struct mqtt_publish_reply_pkt*)(buf + HEADER_SIZE);
+                       int ret = mqtt_check_publish_reply_pkt(pub_rel, MQTT_CONTROL_PUBREL);
                        int len = 0;
                        if (ret < 0) {
                                uprintf(dbg_uart, "MQTT packet is not a valid pubrel.\n");
                                return -7;
                        }
-                       len = mqtt_pack_publish_reply_pkt((uint8_t*)ack, htons(ack->acked_pkt_id), MQTT_CONTROL_PUBCOMP);
-                       ret = serial_write(comm_uart, (char*)txbuf, (len + HEADER_SIZE));
+                       uprintf(dbg_uart, "MQTT pubrel received, sending pubcomp\n");
+                       len = mqtt_pack_publish_reply_pkt((uint8_t*)pub_comp, ntohs(pub_rel->acked_pkt_id), MQTT_CONTROL_PUBCOMP);
+                       if (len != 4) {
+                               uprintf(dbg_uart, "MQTT pack pubrel error: %d\n", len);
+                               return len;
+                       }
+                       /* Add our packet header */
+                       add_packet_header(len);
+                       ret = serial_write(comm_uart, (char*)&txbuf, (len + HEADER_SIZE));
                        if (ret < 0) {
                                uprintf(dbg_uart, "MQTT send error: %d\n", ret);
                                return ret;
@@ -239,7 +247,7 @@ int mqtt_handle_packet(char* buf, int comm_uart, int dbg_uart)
                case MQTT_WAITING_PINGRESP:
                case MQTT_IDLE: {
                        struct mqtt_publish_pkt pub_rx;
-                       struct mqtt_publish_reply_pkt pub_ack;
+                       struct mqtt_publish_reply_pkt* pub_ack = (struct mqtt_publish_reply_pkt*)(txbuf + HEADER_SIZE);
                        int ret = 0, len = 0;
 
                        /* Make sure this is a publish packet */
@@ -257,16 +265,20 @@ int mqtt_handle_packet(char* buf, int comm_uart, int dbg_uart)
                                if (pub_rx.topic.QoS == MQTT_QoS_2) {
                                        type = MQTT_CONTROL_PUBREC;
                                }
-                               len = mqtt_pack_publish_reply_pkt((uint8_t*)&pub_ack, pub_rx.packet_id, type);
-                               ret = serial_write(comm_uart, (char*)txbuf, (len + HEADER_SIZE));
+                               len = mqtt_pack_publish_reply_pkt((uint8_t*)pub_ack, pub_rx.packet_id, type);
+                               if (len != 4) {
+                                       uprintf(dbg_uart, "MQTT pack puback error: %d\n", len);
+                                       return len;
+                               }
+                               /* Add our packet header */
+                               add_packet_header(len);
+                               ret = serial_write(comm_uart, (char*)&txbuf, (len + HEADER_SIZE));
                                if (ret < 0) {
                                        uprintf(dbg_uart, "MQTT send error: %d\n", ret);
                                        return ret;
                                }
                                if (pub_rx.topic.QoS == MQTT_QoS_2) {
                                        mqtt_comm_state = MQTT_WAITING_PUBREL;
-                               } else {
-                                       mqtt_comm_state = MQTT_IDLE;
                                }
                        }
                        /* Perform something with the packet ... */