From 060123dc8d4936c4d3c7c0be68d7c782eedb7875 Mon Sep 17 00:00:00 2001
From: Jonathan Reichelt Gjertsen <jonath.re@gmail.com>
Date: Mon, 24 May 2021 23:31:30 +0200
Subject: [PATCH] Optional feature to get the max level that has ever been held
 by a queue (#444)

I'm using this in my program to get a better idea of how many entries I
need to allocate to avoid blocking or losing data, and to debug performance
issues.
---
 .../pico_util/include/pico/util/queue.h       | 34 +++++++++++++++++++
 src/common/pico_util/queue.c                  |  8 +++++
 2 files changed, 42 insertions(+)

diff --git a/src/common/pico_util/include/pico/util/queue.h b/src/common/pico_util/include/pico/util/queue.h
index e83ecd4..6fba9af 100644
--- a/src/common/pico_util/include/pico/util/queue.h
+++ b/src/common/pico_util/include/pico/util/queue.h
@@ -10,6 +10,11 @@
 #include "pico.h"
 #include "hardware/sync.h"
 
+// PICO_CONFIG: PICO_QUEUE_MAX_LEVEL, Maintain a field for the highest level that has been reached by a queue, type=bool, default=0, advanced=true, group=queue
+#ifndef PICO_QUEUE_MAX_LEVEL
+#define PICO_QUEUE_MAX_LEVEL 0
+#endif
+
 /** \file queue.h
  * \defgroup queue queue
  * Multi-core and IRQ safe queue implementation.
@@ -31,6 +36,9 @@ typedef struct {
     uint16_t rptr;
     uint16_t element_size;
     uint16_t element_count;
+#if PICO_QUEUE_MAX_LEVEL
+    uint16_t max_level;
+#endif
 } queue_t;
 
 /*! \brief Initialise a queue with a specific spinlock for concurrency protection
@@ -93,6 +101,32 @@ static inline uint queue_get_level(queue_t *q) {
     return level;
 }
 
+/*! \brief Returns the highest level reached by the specified queue since it was created
+ *         or since the max level was reset
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \return Maximum level of the queue
+ */
+#if PICO_QUEUE_MAX_LEVEL
+static inline uint queue_get_max_level(queue_t *q) {
+    return q->max_level;
+}
+#endif
+
+/*! \brief Reset the highest level reached of the specified queue.
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ */
+#if PICO_QUEUE_MAX_LEVEL
+static inline void queue_reset_max_level(queue_t *q) {
+    uint32_t save = spin_lock_blocking(q->core.spin_lock);
+    q->max_level = queue_get_level_unsafe(q);
+    spin_unlock(q->core.spin_lock, save);
+}
+#endif
+
 /*! \brief Check if queue is empty
  *  \ingroup queue
  *
diff --git a/src/common/pico_util/queue.c b/src/common/pico_util/queue.c
index bc667ac..8e9866d 100644
--- a/src/common/pico_util/queue.c
+++ b/src/common/pico_util/queue.c
@@ -30,6 +30,14 @@ static inline uint16_t inc_index(queue_t *q, uint16_t index) {
     if (++index > q->element_count) { // > because we have element_count + 1 elements
         index = 0;
     }
+
+#if PICO_QUEUE_MAX_LEVEL
+    uint16_t level = queue_get_level_unsafe(q);
+    if (level > q->max_level) {
+        q->max_level = level;
+    }
+#endif
+
     return index;
 }