[Retain] Fix bug in last retained msg and add retain commands to tinytests

src/TinyMqtt.cpp

@@ -957,14 +957,19 @@ void MqttBroker::retainDrop()
 
 void MqttBroker::retain(const Topic& topic, const MqttMessage& msg)
 {
-  debug("MqttBroker::retain msg_type=" << _HEX(msg.type()));
+  debug("MqttBroker::retain msg_type=" << _HEX(msg.type()) << ", retain_size=" << retain_size);
   if (retain_size==0 or msg.type() != MqttMessage::Publish) return;
   if (msg.flags() & 1)  // flag RETAIN
   {
     debug("  retaining " << topic.str());
-    if (retained.find(topic) == retained.end()) retainDrop();
+    auto old = retained.find(topic);
+    if (old == retained.end())
+      retainDrop();
+    else
+      retained.erase(old);
     // FIXME if payload size == 0 remove message from retained
     Retain r(micros(), msg);
+    r.msg.retained();
     retained.insert({ topic, std::move(r)});
   }
 }

src/TinyMqtt.h

@@ -133,6 +133,7 @@ class MqttMessage
     const char* end() const { return &buffer[0]+buffer.size(); }
     const char* getVHeader() const { return &buffer[vheader]; }
     void complete() { encodeLength(); }
+    void retained() { if ((buffer[0] & 0xF)==Publish) buffer[0] |= 1; }
 
     void reset();
 
@@ -346,7 +347,9 @@ class MqttBroker
     bool connected() const { return remote_broker ? remote_broker->connected() : false; }
 
     size_t clientsCount() const { return clients.size(); }
-    void retain(uint8_t size) { retain_size = size; }
+    uint8_t retain() { return retain_size; }
+    void retain(uint8_t size) { retain_size = size; if (size==0) retained.clear(); }
+    uint8_t retainCount() const { return retained.size(); }
 
     void dump(string indent="")
     {