Added generic buffer

Crashes when publishing to more than one stream
This commit is contained in:
Stuart Feichtinger 2016-01-17 19:27:20 -06:00
parent e77be5b9ac
commit 2876ac43dd
2 changed files with 92 additions and 13 deletions

View File

@ -51,7 +51,7 @@ static uint8_t *stringprint(uint8_t *p, char *s) {
uint16_t len = strlen(s); uint16_t len = strlen(s);
p[0] = len >> 8; p++; p[0] = len >> 8; p++;
p[0] = len & 0xFF; p++; p[0] = len & 0xFF; p++;
memcpy(p, s, len); memmove(p, s, len);
return p+len; return p+len;
} }
*/ */
@ -240,17 +240,45 @@ bool Adafruit_MQTT::publish(const char *topic, const char *data, uint8_t qos) {
len = readPacket(buffer, 4, PUBLISH_TIMEOUT_MS); len = readPacket(buffer, 4, PUBLISH_TIMEOUT_MS);
DEBUG_PRINT(F("Publish QOS1+ reply:\t")); DEBUG_PRINT(F("Publish QOS1+ reply:\t"));
DEBUG_PRINTBUFFER(buffer, len); DEBUG_PRINTBUFFER(buffer, len);
if (len != 4) if (len != 4)
return false; return false;
if ((buffer[0] >> 4) != MQTT_CTRL_PUBACK) if ((buffer[0] >> 4) != MQTT_CTRL_PUBACK)
return false; return false;
uint16_t packnum = buffer[2]; uint16_t packnum = buffer[2];
packnum <<= 8; packnum <<= 8;
packnum |= buffer[3]; packnum |= buffer[3];
// we increment the packet_id_counter right after publishing so inc here too to match // we increment the packet_id_counter right after publishing so inc here too to match
packnum++; packnum++;
if (packnum != packet_id_counter) if (packnum != packet_id_counter)
return false;
}
return true;
}
bool Adafruit_MQTT::publish(const char *topic, uint8_t *sData, uint8_t bLen, uint8_t qos) {
// Construct and send publish packet.
uint8_t len = publishPacket(buffer, topic, sData, bLen, qos);
if (!sendPacket(buffer, len))
return false;
// If QOS level is high enough verify the response packet.
if (qos > 0) {
len = readPacket(buffer, 4, PUBLISH_TIMEOUT_MS);
DEBUG_PRINT(F("Publish QOS1+ reply:\t"));
DEBUG_PRINTBUFFER(buffer, len);
if (len != 4)
return false;
if ((buffer[0] >> 4) != MQTT_CTRL_PUBACK)
return false;
uint16_t packnum = buffer[2];
packnum <<= 8;
packnum |= buffer[3];
// we increment the packet_id_counter right after publishing so inc here too to match
packnum++;
if (packnum != packet_id_counter)
return false; return false;
} }
@ -378,7 +406,7 @@ Adafruit_MQTT_Subscribe *Adafruit_MQTT::readSubscription(int16_t timeout) {
datalen = SUBSCRIPTIONDATALEN-1; // cut it off datalen = SUBSCRIPTIONDATALEN-1; // cut it off
} }
// extract out just the data, into the subscription object itself // extract out just the data, into the subscription object itself
memcpy(subscriptions[i]->lastread, buffer+4+topiclen, datalen); memmove(subscriptions[i]->lastread, buffer+4+topiclen, datalen);
subscriptions[i]->datalen = datalen; subscriptions[i]->datalen = datalen;
DEBUG_PRINT(F("Data len: ")); DEBUG_PRINTLN(datalen); DEBUG_PRINT(F("Data len: ")); DEBUG_PRINTLN(datalen);
DEBUG_PRINT(F("Data: ")); DEBUG_PRINTLN((char *)subscriptions[i]->lastread); DEBUG_PRINT(F("Data: ")); DEBUG_PRINTLN((char *)subscriptions[i]->lastread);
@ -401,13 +429,13 @@ bool Adafruit_MQTT::ping(uint8_t num) {
uint8_t len = pingPacket(buffer); uint8_t len = pingPacket(buffer);
if (!sendPacket(buffer, len)) if (!sendPacket(buffer, len))
continue; continue;
// Process ping reply. // Process ping reply.
len = readPacket(buffer, 2, PING_TIMEOUT_MS); len = readPacket(buffer, 2, PING_TIMEOUT_MS);
if (buffer[0] == (MQTT_CTRL_PINGRESP << 4)) if (buffer[0] == (MQTT_CTRL_PINGRESP << 4))
return true; return true;
} }
return false; return false;
} }
@ -502,7 +530,7 @@ uint8_t Adafruit_MQTT::connectPacket(uint8_t *packet) {
} }
// as per http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718040 // as per http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718040
uint8_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic, uint8_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
const char *data, uint8_t qos) { const char *data, uint8_t qos) {
uint8_t *p = packet; uint8_t *p = packet;
uint16_t len; uint16_t len;
@ -524,7 +552,7 @@ uint8_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
packet_id_counter++; packet_id_counter++;
} }
memcpy(p, data, strlen(data)); memmove(p, data, strlen(data));
p+=strlen(data); p+=strlen(data);
len = p - packet; len = p - packet;
packet[1] = len-2; // don't include the 2 bytes of fixed header data packet[1] = len-2; // don't include the 2 bytes of fixed header data
@ -533,7 +561,39 @@ uint8_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
return len; return len;
} }
uint8_t Adafruit_MQTT::subscribePacket(uint8_t *packet, const char *topic, // as per http://docs.oasis-open.org/mqtt/mqtt/v3.1.1/os/mqtt-v3.1.1-os.html#_Toc398718040
uint8_t Adafruit_MQTT::publishPacket(uint8_t *packet, const char *topic,
uint8_t *sData, uint8_t bLen, uint8_t qos) {
uint8_t *p = packet;
uint16_t len;
p[0] = MQTT_CTRL_PUBLISH << 4 | qos << 1;
// fill in packet[1] last
p+=2;
// topic comes before packet identifier
p = stringprint_P(p, topic);
// add packet identifier. used for checking PUBACK in QOS > 0
if(qos > 0) {
p[0] = (packet_id_counter >> 8) & 0xFF;
p[1] = packet_id_counter & 0xFF;
p+=2;
// increment the packet id
packet_id_counter++;
}
memmove(p, sData, bLen);
p+= bLen;
len = p - packet;
packet[1] = len-2; // don't include the 2 bytes of fixed header data
DEBUG_PRINTLN(F("MQTT publish packet:"));
DEBUG_PRINTBUFFER(buffer, len);
return len;
}
uint8_t Adafruit_MQTT::subscribePacket(uint8_t *packet, const char *topic,
uint8_t qos) { uint8_t qos) {
uint8_t *p = packet; uint8_t *p = packet;
uint16_t len; uint16_t len;
@ -562,6 +622,8 @@ uint8_t Adafruit_MQTT::subscribePacket(uint8_t *packet, const char *topic,
return len; return len;
} }
uint8_t Adafruit_MQTT::unsubscribePacket(uint8_t *packet, const char *topic) { uint8_t Adafruit_MQTT::unsubscribePacket(uint8_t *packet, const char *topic) {
uint8_t *p = packet; uint8_t *p = packet;
@ -607,7 +669,7 @@ uint8_t Adafruit_MQTT::disconnectPacket(uint8_t *packet) {
// Adafruit_MQTT_Publish Definition //////////////////////////////////////////// // Adafruit_MQTT_Publish Definition ////////////////////////////////////////////
Adafruit_MQTT_Publish::Adafruit_MQTT_Publish(Adafruit_MQTT *mqttserver, Adafruit_MQTT_Publish::Adafruit_MQTT_Publish(Adafruit_MQTT *mqttserver,
const char *feed, uint8_t q) { const char *feed, uint8_t q) {
mqtt = mqttserver; mqtt = mqttserver;
topic = feed; topic = feed;
@ -643,10 +705,23 @@ bool Adafruit_MQTT_Publish::publish(const char *payload) {
return mqtt->publish(topic, payload, qos); return mqtt->publish(topic, payload, qos);
} }
bool Adafruit_MQTT_Publish::publish(uint8_t *payload, uint8_t bLen) {
/*
Serial.print(F("Publish 1\nbLen:\t"));
Serial.println(bLen);
for(int i = 0; i < bLen; i++){
Serial.print(payload[i], HEX);
}
Serial.println(F("Exit publish 1"));
return 0;
*/
return mqtt->publish(topic, payload, bLen, qos);
}
// Adafruit_MQTT_Subscribe Definition ////////////////////////////////////////// // Adafruit_MQTT_Subscribe Definition //////////////////////////////////////////
Adafruit_MQTT_Subscribe::Adafruit_MQTT_Subscribe(Adafruit_MQTT *mqttserver, Adafruit_MQTT_Subscribe::Adafruit_MQTT_Subscribe(Adafruit_MQTT *mqttserver,
const char *feed, uint8_t q) { const char *feed, uint8_t q) {
mqtt = mqttserver; mqtt = mqttserver;
topic = feed; topic = feed;

View File

@ -160,6 +160,7 @@ class Adafruit_MQTT {
// The topic must be stored in PROGMEM. It can either be a // The topic must be stored in PROGMEM. It can either be a
// char*, or a __FlashStringHelper* (the result of the F() macro). // char*, or a __FlashStringHelper* (the result of the F() macro).
bool publish(const char *topic, const char *payload, uint8_t qos = 0); bool publish(const char *topic, const char *payload, uint8_t qos = 0);
bool publish(const char *topic, uint8_t *payload, uint8_t bLen, uint8_t qos = 0);
bool publish(const __FlashStringHelper *topic, const char *payload, uint8_t qos = 0) { bool publish(const __FlashStringHelper *topic, const char *payload, uint8_t qos = 0) {
return publish((const char *)topic, payload, qos); return publish((const char *)topic, payload, qos);
} }
@ -223,6 +224,7 @@ class Adafruit_MQTT {
uint8_t connectPacket(uint8_t *packet); uint8_t connectPacket(uint8_t *packet);
uint8_t disconnectPacket(uint8_t *packet); uint8_t disconnectPacket(uint8_t *packet);
uint8_t publishPacket(uint8_t *packet, const char *topic, const char *payload, uint8_t qos); uint8_t publishPacket(uint8_t *packet, const char *topic, const char *payload, uint8_t qos);
uint8_t publishPacket(uint8_t *packet, const char *topic, uint8_t *payload, uint8_t bLen, uint8_t qos);
uint8_t subscribePacket(uint8_t *packet, const char *topic, uint8_t qos); uint8_t subscribePacket(uint8_t *packet, const char *topic, uint8_t qos);
uint8_t unsubscribePacket(uint8_t *packet, const char *topic); uint8_t unsubscribePacket(uint8_t *packet, const char *topic);
uint8_t pingPacket(uint8_t *packet); uint8_t pingPacket(uint8_t *packet);
@ -239,6 +241,8 @@ class Adafruit_MQTT_Publish {
// This might be ignored and a higher precision value sent. // This might be ignored and a higher precision value sent.
bool publish(int32_t i); bool publish(int32_t i);
bool publish(uint32_t i); bool publish(uint32_t i);
bool publish(uint8_t *b, uint8_t bLen);
private: private:
Adafruit_MQTT *mqtt; Adafruit_MQTT *mqtt;