7 files changed, 182 insertions, 4 deletions
diff --git a/lib/bt/esp_ble_mesh/common/atomic.c b/lib/bt/esp_ble_mesh/common/atomic.c
index 723ce7e3..9c856cc3 100644
--- a/lib/bt/esp_ble_mesh/common/atomic.c
+++ b/lib/bt/esp_ble_mesh/common/atomic.c
@@ -13,7 +13,7 @@
 /*
  * SPDX-FileCopyrightText: 2016 Intel Corporation
  * SPDX-FileCopyrightText: 2011-2014 Wind River Systems, Inc.
- * SPDX-FileContributor: 2018-2021 Espressif Systems (Shanghai) CO LTD
+ * SPDX-FileContributor: 2018-2024 Espressif Systems (Shanghai) CO LTD
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -170,4 +170,18 @@ bt_mesh_atomic_val_t bt_mesh_atomic_inc(bt_mesh_atomic_t *target)
     return ret;
 }
 
+bool bt_mesh_atomic_cas(bt_mesh_atomic_t *target, bt_mesh_atomic_val_t excepted, bt_mesh_atomic_val_t new_val)
+{
+    bt_mesh_atomic_lock();
+
+    if (*target == excepted) {
+        *target = new_val;
+        bt_mesh_atomic_unlock();
+        return true;
+    }
+
+    bt_mesh_atomic_unlock();
+    return false;
+}
+
 #endif /* #ifndef CONFIG_ATOMIC_OPERATIONS_BUILTIN */
diff --git a/lib/bt/esp_ble_mesh/common/include/mesh/atomic.h b/lib/bt/esp_ble_mesh/common/include/mesh/atomic.h
index f7283436..b2849743 100644
--- a/lib/bt/esp_ble_mesh/common/include/mesh/atomic.h
+++ b/lib/bt/esp_ble_mesh/common/include/mesh/atomic.h
@@ -148,6 +148,33 @@ extern bt_mesh_atomic_val_t bt_mesh_atomic_and(bt_mesh_atomic_t *target, bt_mesh
 #endif
 
 /**
+ * @brief Atomic CAS operation.
+ *
+ * This compares the contents of @a *target
+ * with the contents of @a excepted. If equal,
+ * the operation is a read-modify-write operation
+ * that writes @a new_val into @a *target and return true.
+ * If they are not equal, the operation is a read
+ * and return false.
+ *
+ * @param target Address of atomic variable.
+ * @param excepted Value of excepted.
+ * @param new_val Write if target value is equal to expected one.
+ *
+ * @return
+ *         - true: Target value updated.
+ *         - false: Target value not updated.
+ */
+#ifdef CONFIG_ATOMIC_OPERATIONS_BUILTIN
+static inline bool bt_mesh_atomic_cas(bt_mesh_atomic_t *target, bt_mesh_atomic_val_t excepted, bt_mesh_atomic_val_t new_val)
+{
+    return __atomic_compare_exchange_n(target, &excepted, &new_val, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST);
+}
+#else
+extern bool bt_mesh_atomic_cas(bt_mesh_atomic_t *target, bt_mesh_atomic_val_t excepted, bt_mesh_atomic_val_t new_val);
+#endif
+
+/**
  * @cond INTERNAL_HIDDEN
  */
 
diff --git a/lib/bt/esp_ble_mesh/common/include/mesh/mutex.h b/lib/bt/esp_ble_mesh/common/include/mesh/mutex.h
index ee897500..0cc47eb0 100644
--- a/lib/bt/esp_ble_mesh/common/include/mesh/mutex.h
+++ b/lib/bt/esp_ble_mesh/common/include/mesh/mutex.h
@@ -1,5 +1,5 @@
 /*
- * SPDX-FileCopyrightText: 2017-2021 Espressif Systems (Shanghai) CO LTD
+ * SPDX-FileCopyrightText: 2017-2024 Espressif Systems (Shanghai) CO LTD
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -32,6 +32,11 @@ void bt_mesh_r_mutex_free(bt_mesh_mutex_t *mutex);
 void bt_mesh_r_mutex_lock(bt_mesh_mutex_t *mutex);
 void bt_mesh_r_mutex_unlock(bt_mesh_mutex_t *mutex);
 
+void bt_mesh_c_semaphore_create(bt_mesh_mutex_t *mutex, int max, int init);
+void bt_mesh_c_semaphore_free(bt_mesh_mutex_t *mutex);
+void bt_mesh_c_semaphore_give(bt_mesh_mutex_t *mutex);
+void bt_mesh_c_semaphore_take(bt_mesh_mutex_t *mutex, uint32_t timeout);
+
 void bt_mesh_alarm_lock(void);
 void bt_mesh_alarm_unlock(void);
 
diff --git a/lib/bt/esp_ble_mesh/common/include/mesh/queue.h b/lib/bt/esp_ble_mesh/common/include/mesh/queue.h
new file mode 100644
index 00000000..021c99ba
--- /dev/null
+++ b/lib/bt/esp_ble_mesh/common/include/mesh/queue.h
@@ -0,0 +1,33 @@
+/*
+ * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef _BLE_MESH_QUEUE_H_
+#define _BLE_MESH_QUEUE_H_
+
+#include "mesh/kernel.h"
+#include "mesh/slist.h"
+#include "mesh/atomic.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+    QueueHandle_t handle;
+#if CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC
+    StaticQueue_t *buffer;
+    uint8_t *storage;
+#endif
+} bt_mesh_queue_t;
+
+int bt_mesh_queue_init(bt_mesh_queue_t *queue, uint16_t queue_size, uint8_t item_size);
+int bt_mesh_queue_deinit(bt_mesh_queue_t *queue);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _BLE_MESH_QUEUE_H_ */
diff --git a/lib/bt/esp_ble_mesh/common/include/mesh/utils.h b/lib/bt/esp_ble_mesh/common/include/mesh/utils.h
index 98243483..967fed2e 100644
--- a/lib/bt/esp_ble_mesh/common/include/mesh/utils.h
+++ b/lib/bt/esp_ble_mesh/common/include/mesh/utils.h
@@ -200,7 +200,7 @@ extern "C" {
  *    { MY_PWM0 , MY_PWM1 }
  *
  * @param LEN The length of the sequence. Must be an integer literal less
- *            than 255.
+ *            than 255 (ref: utils_loops.h).
  * @param F A macro function that accepts at least two arguments:
  *          <tt>F(i, ...)</tt>. @p F is called repeatedly in the expansion.
  *          Its first argument @p i is the index in the sequence, and
diff --git a/lib/bt/esp_ble_mesh/common/mutex.c b/lib/bt/esp_ble_mesh/common/mutex.c
index 6c3b2bf6..3a3dedb8 100644
--- a/lib/bt/esp_ble_mesh/common/mutex.c
+++ b/lib/bt/esp_ble_mesh/common/mutex.c
@@ -1,5 +1,5 @@
 /*
- * SPDX-FileCopyrightText: 2017-2021 Espressif Systems (Shanghai) CO LTD
+ * SPDX-FileCopyrightText: 2017-2024 Espressif Systems (Shanghai) CO LTD
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -125,6 +125,57 @@ void bt_mesh_r_mutex_unlock(bt_mesh_mutex_t *mutex)
     }
 }
 
+void bt_mesh_c_semaphore_free(bt_mesh_mutex_t *mutex)
+{
+    bt_mesh_mutex_free(mutex);
+}
+
+void bt_mesh_c_semaphore_create(bt_mesh_mutex_t *mutex, int max, int init)
+{
+    if (!mutex) {
+        BT_ERR("Create, invalid mutex");
+        return;
+    }
+
+#if CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC
+#if CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC_EXTERNAL
+    mutex->buffer = heap_caps_calloc_prefer(1, sizeof(StaticQueue_t), 2, MALLOC_CAP_SPIRAM|MALLOC_CAP_8BIT, MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+#elif CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC_IRAM_8BIT
+    mutex->buffer = heap_caps_calloc_prefer(1, sizeof(StaticQueue_t), 2, MALLOC_CAP_INTERNAL|MALLOC_CAP_IRAM_8BIT, MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+#endif
+    __ASSERT(mutex->buffer, "Failed to create counting semaphore buffer");
+    mutex->mutex = xSemaphoreCreateCountingStatic(max, init, mutex->buffer);
+    __ASSERT(mutex->mutex, "Failed to create static counting semaphore");
+#else /* CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC */
+    mutex->mutex = xSemaphoreCreateCounting(max, init);
+    __ASSERT(mutex->mutex, "Failed to create counting semaphore");
+#endif /* CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC */
+}
+
+void bt_mesh_c_semaphore_take(bt_mesh_mutex_t *mutex, uint32_t timeout)
+{
+    if (!mutex) {
+        BT_ERR("Lock, invalid counting semaphore");
+        return;
+    }
+
+    if (mutex->mutex) {
+        xSemaphoreTake(mutex->mutex, timeout / portTICK_PERIOD_MS);
+    }
+}
+
+void bt_mesh_c_semaphore_give(bt_mesh_mutex_t *mutex)
+{
+    if (!mutex) {
+        BT_ERR("Unlock, invalid counting semaphore");
+        return;
+    }
+
+    if (mutex->mutex) {
+        xSemaphoreGive(mutex->mutex);
+    }
+}
+
 void bt_mesh_alarm_lock(void)
 {
     bt_mesh_mutex_lock(&alarm_lock);
diff --git a/lib/bt/esp_ble_mesh/common/queue.c b/lib/bt/esp_ble_mesh/common/queue.c
new file mode 100644
index 00000000..1fc3d66a
--- /dev/null
+++ b/lib/bt/esp_ble_mesh/common/queue.c
@@ -0,0 +1,48 @@
+/*
+ * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "mesh/common.h"
+#include "mesh/queue.h"
+
+int bt_mesh_queue_init(bt_mesh_queue_t *queue, uint16_t queue_size, uint8_t item_size)
+{
+    __ASSERT(queue && queue_size && item_size, "Invalid queue init parameters");
+
+#if !CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC
+    queue->handle = xQueueCreate(queue_size, item_size);
+    __ASSERT(queue->handle, "Failed to create queue");
+#else /* !CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC */
+#if CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC_EXTERNAL
+    queue->buffer = heap_caps_calloc_prefer(1, sizeof(StaticQueue_t), 2, MALLOC_CAP_SPIRAM|MALLOC_CAP_8BIT, MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+#elif CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC_IRAM_8BIT
+    queue->buffer = heap_caps_calloc_prefer(1, sizeof(StaticQueue_t), 2, MALLOC_CAP_INTERNAL|MALLOC_CAP_IRAM_8BIT, MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+#endif
+    __ASSERT(queue->buffer, "Failed to create queue buffer");
+#if CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC_EXTERNAL
+    queue->storage = heap_caps_calloc_prefer(1, (queue_size * item_size), 2, MALLOC_CAP_SPIRAM|MALLOC_CAP_8BIT, MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+#elif CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC_IRAM_8BIT
+    queue->storage = heap_caps_calloc_prefer(1, (queue_size * item_size), 2, MALLOC_CAP_INTERNAL|MALLOC_CAP_IRAM_8BIT, MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT);
+#endif
+    __ASSERT(queue->storage, "Failed to create queue storage");
+    queue->handle = xQueueCreateStatic(queue_size, item_size, (uint8_t*)queue->storage, queue->buffer);
+    __ASSERT(queue->handle, "Failed to create static queue");
+#endif /* !CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC */
+    return 0;
+}
+
+int bt_mesh_queue_deinit(bt_mesh_queue_t *queue)
+{
+    __ASSERT(queue, "Invalid queue init parameters");
+    vQueueDelete(queue->handle);
+    queue->handle = NULL;
+#if CONFIG_BLE_MESH_FREERTOS_STATIC_ALLOC
+    heap_caps_free(queue->buffer);
+    queue->buffer = NULL;
+    heap_caps_free(queue->storage);
+    queue->storage = NULL;
+#endif
+    return 0;
+}