Merge pull request #2066 from cesanta/queue

Refactor queue

src/arch_newlib.h

@@ -19,6 +19,5 @@
 #define MG_PATH_MAX 100
 #define MG_ENABLE_SOCKET 0
 #define MG_ENABLE_DIRLIST 0
-#define MG_ENABLE_ATOMIC 1
 
 #endif

src/arch_rp2040.h

@@ -12,5 +12,4 @@
 
 #include <pico/stdlib.h>
 int mkdir(const char *, mode_t);
-#define MG_ENABLE_ATOMIC 1
 #endif

src/config.h

@@ -8,10 +8,6 @@
 #define MG_ENABLE_TCPIP 0  // Mongoose built-in network stack
 #endif
 
-#ifndef MG_ENABLE_ATOMIC
-#define MG_ENABLE_ATOMIC 0  // Required by mg_queue
-#endif
-
 #ifndef MG_ENABLE_LWIP
 #define MG_ENABLE_LWIP 0  // lWIP network stack
 #endif
@@ -85,6 +81,10 @@
 #define MG_ENABLE_PACKED_FS 0
 #endif
 
+#ifndef MG_ENABLE_ASSERT
+#define MG_ENABLE_ASSERT 0
+#endif
+
 #ifndef MG_IO_SIZE
 #define MG_IO_SIZE 2048  // Granularity of the send/recv IO buffer growth
 #endif

src/printf.c

@@ -6,7 +6,7 @@
 size_t mg_queue_vprintf(struct mg_queue *q, const char *fmt, va_list *ap) {
   size_t len = mg_snprintf(NULL, 0, fmt, ap);
   char *buf;
-  if (mg_queue_space(q, &buf) < len + 1) {
+  if (len == 0 || mg_queue_book(q, &buf, len + 1) < len + 1) {
     len = 0;  // Nah. Not enough space
   } else {
     len = mg_vsnprintf((char *)buf, len + 1, fmt, ap);

src/queue.c

@@ -1,40 +1,82 @@
 #include "queue.h"
+#include "util.h"
 
-// Every message in the queue is prepended by the message length (ML)
-// ML is sizeof(size_t) in size
-// Tail points to the message data
+#if defined(__GNUC__) || defined(__clang__)
+#define MG_MEMORY_BARRIER() __sync_synchronize()
+#elif defined(_MSC_VER) && _MSC_VER >= 1700
+#define MG_MEMORY_BARRIER() MemoryBarrier()
+#elif !defined(MG_MEMORY_BARRIER)
+#define MG_MEMORY_BARRIER()
+#endif
+
+// Every message in a queue is prepended by a 32-bit message length (ML).
+// If ML is 0, then it is the end, and reader must wrap to the beginning.
 //
-//    |------| ML | message1 |  ML | message2 |--- free space ---|
-//    ^      ^                                ^                  ^
-//   buf    tail                             head               len
+//  Queue when q->tail <= q->head:
+//  |----- free -----| ML | message1 | ML | message2 |  ----- free ------|
+//  ^                ^                               ^                   ^
+// buf              tail                            head                len
+//
+//  Queue when q->tail > q->head:
+//  | ML | message2 |----- free ------| ML | message1 | 0 |---- free ----|
+//  ^               ^                 ^                                  ^
+// buf             head              tail                               len
 
-size_t mg_queue_space(struct mg_queue *q, char **buf) {
-  size_t ofs;
-  if (q->head > 0 && q->tail >= q->head) {  // All messages read?
-    q->head = 0;                            // Yes. Reset head first
-    q->tail = 0;                            // Now reset the tail
+void mg_queue_init(struct mg_queue *q, char *buf, size_t size) {
+  q->size = size;
+  q->buf = buf;
+  q->head = q->tail = 0;
+}
+
+static size_t mg_queue_read_len(struct mg_queue *q) {
+  uint32_t n = 0;
+  MG_MEMORY_BARRIER();
+  memcpy(&n, q->buf + q->tail, sizeof(n));
+  assert(q->tail + n + sizeof(n) <= q->size);
+  return n;
+}
+
+static void mg_queue_write_len(struct mg_queue *q, size_t len) {
+  uint32_t n = (uint32_t) len;
+  memcpy(q->buf + q->head, &n, sizeof(n));
+  MG_MEMORY_BARRIER();
+}
+
+size_t mg_queue_book(struct mg_queue *q, char **buf, size_t len) {
+  size_t space = 0, hs = sizeof(uint32_t) * 2;  // *2 is for the 0 marker
+  if (q->head >= q->tail && q->head + len + hs <= q->size) {
+    space = q->size - q->head - hs;  // There is enough space
+  } else if (q->head >= q->tail && q->tail > hs) {
+    mg_queue_write_len(q, 0);  // Not enough space ahead
+    q->head = 0;               // Wrap head to the beginning
   }
-  ofs = q->head + sizeof(size_t);
-  if (buf != NULL) *buf = q->buf + ofs;
-  return ofs > q->len ? 0 : q->len - ofs;
+  if (q->head + hs + len < q->tail) space = q->tail - q->head - hs;
+  if (buf != NULL) *buf = q->buf + q->head + sizeof(uint32_t);
+  return space;
 }
 
 size_t mg_queue_next(struct mg_queue *q, char **buf) {
-  size_t len = MG_QUEUE_EMPTY;
-  if (q->tail < q->head) memcpy(&len, &q->buf[q->tail], sizeof(len));
-  if (buf != NULL) *buf = &q->buf[q->tail + sizeof(len)];
+  size_t len = 0;
+  if (q->tail != q->head) {
+    len = mg_queue_read_len(q);
+    if (len == 0) {  // Zero (head wrapped) ?
+      q->tail = 0;   // Reset tail to the start
+      if (q->head > q->tail) len = mg_queue_read_len(q);  // Read again
+    }
+  }
+  if (buf != NULL) *buf = q->buf + q->tail + sizeof(uint32_t);
+  assert(q->tail + len <= q->size);
   return len;
 }
 
 void mg_queue_add(struct mg_queue *q, size_t len) {
-  size_t head = q->head + len + (size_t) sizeof(head);  // New head
-  if (head <= q->len) {                                 // Have space ?
-    memcpy(q->buf + q->head, &len, sizeof(len));        // Yes. Store ML
-    q->head = head;                                     // Advance head
-  }
+  assert(len > 0);
+  mg_queue_write_len(q, len);
+  assert(q->head + sizeof(uint32_t) * 2 + len <= q->size);
+  q->head += len + sizeof(uint32_t);
 }
 
-void mg_queue_del(struct mg_queue *q) {
-  size_t len = mg_queue_next(q, NULL), tail = q->tail + len + sizeof(size_t);
-  if (len != MG_QUEUE_EMPTY) q->tail = tail;
+void mg_queue_del(struct mg_queue *q, size_t len) {
+  q->tail += len + sizeof(uint32_t);
+  assert(q->tail + sizeof(uint32_t) <= q->size);
 }

src/queue.h

@@ -1,38 +1,30 @@
 #pragma once
 
-#include "arch.h"    // For size_t
-#include "config.h"  // For MG_ENABLE_ATOMIC
-
-#if MG_ENABLE_ATOMIC
-#include <stdatomic.h>
-#elif !defined(_Atomic)
-#define _Atomic
-#endif
+#include "arch.h"  // For size_t
 
 // Single producer, single consumer non-blocking queue
 //
 // Producer:
-//    void *buf;
-//    while (mg_queue_space(q, &buf) < len) WAIT();  // Wait for free space
-//    memcpy(buf, data, len);  // Copy data to the queue
-//    mg_queue_add(q, len);    // Advance q->head
+//    char *buf;
+//    while (mg_queue_book(q, &buf) < len) WAIT();  // Wait for space
+//    memcpy(buf, my_data, len);   // Copy data to the queue
+//    mg_queue_add(q, len);
 //
 // Consumer:
-//    void *buf;
-//    while ((len = mg_queue_next(q, &buf)) == MG_QUEUE_EMPTY) WAIT();
-//    mg_hexdump(buf, len);    // Handle message
-//    mg_queue_del(q);         // Delete message
+//    char *buf;
+//    while ((len = mg_queue_get(q, &buf)) == 0) WAIT();
+//    mg_hexdump(buf, len); // Handle message
+//    mg_queue_del(q, len);
 //
 struct mg_queue {
   char *buf;
-  size_t len;
-  volatile _Atomic size_t tail;
-  volatile _Atomic size_t head;
+  size_t size;
+  volatile size_t tail;
+  volatile size_t head;
 };
 
-#define MG_QUEUE_EMPTY ((size_t) ~0ul)  // Next message size when queue is empty
-
-void mg_queue_add(struct mg_queue *, size_t len);       // Advance head
-void mg_queue_del(struct mg_queue *);                   // Advance tail
-size_t mg_queue_space(struct mg_queue *, char **);      // Get free space
-size_t mg_queue_next(struct mg_queue *, char **);       // Get next message size
+void mg_queue_init(struct mg_queue *, char *, size_t);        // Init queue
+size_t mg_queue_book(struct mg_queue *, char **buf, size_t);  // Reserve space
+void mg_queue_add(struct mg_queue *, size_t);                 // Add new message
+size_t mg_queue_next(struct mg_queue *, char **);  // Get oldest message
+void mg_queue_del(struct mg_queue *, size_t);      // Delete oldest message

src/sha1.c

@@ -48,7 +48,7 @@ static uint32_t blk0(union char64long16 *block, int i) {
   w = rol(w, 30);
 
 static void mg_sha1_transform(uint32_t state[5],
-                              const unsigned char buffer[64]) {
+                              const unsigned char *buffer) {
   uint32_t a, b, c, d, e;
   union char64long16 block[1];
 

src/util.h

@@ -6,6 +6,12 @@
 #include "net.h"
 #include "str.h"
 
+#if MG_ENABLE_ASSERT
+#include <assert.h>
+#elif !defined(assert)
+#define assert(x)
+#endif
+
 void mg_random(void *buf, size_t len);
 char *mg_random_str(char *buf, size_t len);
 uint16_t mg_ntohs(uint16_t net);