From 80abcbf3a9b8e9b35f8345a8656b3076b3d9bc47 Mon Sep 17 00:00:00 2001
From: st0ff3r <stoffer@skulp.net>
Date: Fri, 18 Nov 2016 05:32:06 +0100
Subject: [PATCH] PROTO_AddRb(): rollback inserts into ringbuffer done by
 RINGBUF_Put() when there was not eneough space free

---
 mqtt/queue.c | 20 +++++++++++++++++++-
 1 file changed, 19 insertions(+), 1 deletion(-)

diff --git a/mqtt/queue.c b/mqtt/queue.c
index 7fa50aa..4a7e146 100644
--- a/mqtt/queue.c
+++ b/mqtt/queue.c
@@ -34,6 +34,11 @@
 #include "os_type.h"
 #include "mem.h"
 #include "proto.h"
+
+uint8_t *last_rb_p_r;
+uint8_t *last_rb_p_w;
+uint32_t last_fill_cnt;
+
 void ICACHE_FLASH_ATTR QUEUE_Init(QUEUE *queue, int bufferSize)
 {
   queue->buf = (uint8_t*)os_zalloc(bufferSize);
@@ -41,7 +46,20 @@ void ICACHE_FLASH_ATTR QUEUE_Init(QUEUE *queue, int bufferSize)
 }
 int32_t ICACHE_FLASH_ATTR QUEUE_Puts(QUEUE *queue, uint8_t* buffer, uint16_t len)
 {
-  return PROTO_AddRb(&queue->rb, buffer, len);
+  uint32_t ret;
+  
+  last_rb_p_r = queue->rb.p_r;
+  last_rb_p_w = queue->rb.p_w;
+  last_fill_cnt = queue->rb.fill_cnt;
+
+  ret = PROTO_AddRb(&queue->rb, buffer, len);
+  if (ret == -1) {
+    // rolling ring buffer back
+    queue->rb.p_r = last_rb_p_r;
+    queue->rb.p_w = last_rb_p_w;
+    queue->rb.fill_cnt = last_fill_cnt;
+  }
+  return ret;
 }
 int32_t ICACHE_FLASH_ATTR QUEUE_Gets(QUEUE *queue, uint8_t* buffer, uint16_t* len, uint16_t maxLen)
 {