2 files changed, 52 insertions, 0 deletions

diff --git a/src/client.c b/src/client.c
index f790f42..aaebad3 100644
--- a/src/client.c
+++ b/src/client.c
@@ -72,6 +72,10 @@ struct MqttClient
     int maxQueued;
     /* 1 if PINGREQ is sent and we are waiting for PINGRESP, 0 otherwise */
     int pingSent;
+    StringBuf willTopic;
+    StringBuf willMessage;
+    int willQos;
+    int willRetain;
 };
 
 enum MessageState
@@ -277,6 +281,24 @@ int MqttClientConnect(MqttClient *client, const char *host, short port,
         return -1;
     }
 
+    if (client->willTopic.data != NULL)
+    {
+        packet->connectFlags |= 0x04;
+
+        memcpy(&packet->willTopic, &client->willTopic,
+               sizeof(packet->willTopic));
+
+        memset(&client->willTopic, 0, sizeof(packet->willTopic));
+
+        memcpy(&packet->willMessage, &client->willMessage,
+               sizeof(packet->willMessage));
+
+        memset(&client->willMessage, 0, sizeof(packet->willMessage));
+
+        packet->connectFlags |= (client->willQos & 3) << 3;
+        packet->connectFlags |= (client->willRetain & 1) << 5;
+    }
+
     MqttClientQueuePacket(client, &packet->base);
 
     return 0;
@@ -576,6 +598,33 @@ void MqttClientSetMaxQueuedMessages(MqttClient *client, int max)
     client->maxQueued = max;
 }
 
+int MqttClientSetWill(MqttClient *client, const char *topic, const void *msg,
+                      size_t size, int qos, int retain)
+{
+    assert(client != NULL);
+
+    if (client->stream.sock != -1)
+    {
+        LOG_ERROR("MqttClientSetWill must be called before MqttClientConnect");
+        return -1;
+    }
+
+    if (StringBufInitFromCString(&client->willTopic, topic, -1) == -1)
+    {
+        return -1;
+    }
+
+    if (StringBufInitFromData(&client->willMessage, msg, size) == -1)
+    {
+        return -1;
+    }
+
+    client->willQos = qos;
+    client->willRetain = retain;
+
+    return 0;
+}
+
 static void MqttClientQueuePacket(MqttClient *client, MqttPacket *packet)
 {
     assert(client != NULL);
diff --git a/src/mqtt.h b/src/mqtt.h
index 84b42be..6c16ea6 100644
--- a/src/mqtt.h
+++ b/src/mqtt.h
@@ -91,6 +91,9 @@ void MqttClientSetMaxMessagesInflight(MqttClient *client, int max);
 
 void MqttClientSetMaxQueuedMessages(MqttClient *client, int max);
 
+int MqttClientSetWill(MqttClient *client, const char *topic, const void *msg,
+                      size_t size, int qos, int retain);
+
 #if defined(__cplusplus)
 }
 #endif