lib: net_buf: Add a way to create pool with aligned data

Add a new macro NET_BUF_POOL_VAR_ALIGN_DEFINE() which can be
used to create a pool with aligned data where both the start
of the data and the length of the data is aligned.

Signed-off-by: Jukka Rissanen <jukka.rissanen@nordicsemi.no>

Author: jukkar

include/zephyr/net_buf.h

@@ -1066,6 +1066,7 @@ struct net_buf_data_alloc {
 	const struct net_buf_data_cb *cb;
 	void *alloc_data;
 	size_t max_alloc_size;
+	size_t alignment;
 };
 
 /** @endcond */
@@ -1279,6 +1280,51 @@ extern const struct net_buf_data_cb net_buf_var_cb;
 					 _net_buf_##_name, _count, _ud_size,   \
 					 _destroy)
 
+/**
+ *
+ * @brief Define a new pool for buffers with variable size payloads. Align the
+ *        length and start of the buffer to the specified alignment.
+ *
+ * Defines a net_buf_pool struct and the necessary memory storage (array of
+ * structs) for the needed amount of buffers. After this, the buffers can be
+ * accessed from the pool through net_buf_alloc. The pool is defined as a
+ * static variable, so if it needs to be exported outside the current module
+ * this needs to happen with the help of a separate pointer rather than an
+ * extern declaration.
+ *
+ * The data payload of the buffers will be based on a memory pool from which
+ * variable size payloads may be allocated.
+ *
+ * If provided with a custom destroy callback, this callback is
+ * responsible for eventually calling net_buf_destroy() to complete the
+ * process of returning the buffer to the pool.
+ *
+ * Both the length and start of the buffer will be aligned to the specified
+ * alignment. The alignment must be a power of 2 and the size of the
+ * alignment must be less than or equal to the size of the data payload.
+ *
+ * @param _name      Name of the pool variable.
+ * @param _count     Number of buffers in the pool.
+ * @param _data_size Total amount of memory available for data payloads.
+ * @param _ud_size   User data space to reserve per buffer.
+ * @param _destroy   Optional destroy callback when buffer is freed.
+ * @param _align     Alignment of the length and start of the buffer.
+ */
+#define NET_BUF_POOL_VAR_ALIGN_DEFINE(_name, _count, _data_size, _ud_size,     \
+				      _destroy, _align)			       \
+	_NET_BUF_ARRAY_DEFINE(_name, _count, _ud_size);                        \
+	K_HEAP_DEFINE(net_buf_mem_pool_##_name, _data_size);                   \
+	static const struct net_buf_data_alloc net_buf_data_alloc_##_name = {  \
+		.cb = &net_buf_var_cb,					       \
+		.alloc_data = &net_buf_mem_pool_##_name,                       \
+		.max_alloc_size = 0,                                           \
+		.alignment = _align,					       \
+	};                                                                     \
+	static STRUCT_SECTION_ITERABLE(net_buf_pool, _name) =                  \
+		NET_BUF_POOL_INITIALIZER(_name, &net_buf_data_alloc_##_name,   \
+					 _net_buf_##_name, _count, _ud_size,   \
+					 _destroy)
+
 /**
  *
  * @brief Define a new pool for buffers

lib/net_buf/buf.c

@@ -42,6 +42,8 @@ LOG_MODULE_REGISTER(LOG_MODULE_NAME);
 #define WARN_ALLOC_INTERVAL K_FOREVER
 #endif
 
+#define GET_ALIGN(pool) MAX(sizeof(void *), pool->alloc->alignment)
+
 /* Linker-defined symbol bound to the static pool structs */
 STRUCT_SECTION_START_EXTERN(net_buf_pool);
 
@@ -95,9 +97,10 @@ void net_buf_reset(struct net_buf *buf)
 
 static uint8_t *generic_data_ref(struct net_buf *buf, uint8_t *data)
 {
+	struct net_buf_pool *buf_pool = net_buf_pool_get(buf->pool_id);
 	uint8_t *ref_count;
 
-	ref_count = data - sizeof(void *);
+	ref_count = data - GET_ALIGN(buf_pool);
 	(*ref_count)++;
 
 	return data;
@@ -109,9 +112,26 @@ static uint8_t *mem_pool_data_alloc(struct net_buf *buf, size_t *size,
 	struct net_buf_pool *buf_pool = net_buf_pool_get(buf->pool_id);
 	struct k_heap *pool = buf_pool->alloc->alloc_data;
 	uint8_t *ref_count;
+	void *b;
+
+	if (buf_pool->alloc->alignment == 0) {
+		/* Reserve extra space for a ref-count (uint8_t) */
+		b = k_heap_alloc(pool, sizeof(void *) + *size, timeout);
+
+	} else {
+		if (*size < buf_pool->alloc->alignment) {
+			NET_BUF_DBG("Requested size %zu is smaller than alignment %zu",
+				    *size, buf_pool->alloc->alignment);
+			return NULL;
+		}
 
-	/* Reserve extra space for a ref-count (uint8_t) */
-	void *b = k_heap_alloc(pool, sizeof(void *) + *size, timeout);
+		/* Reserve extra space for a ref-count (uint8_t) */
+		b = k_heap_aligned_alloc(pool,
+					 buf_pool->alloc->alignment,
+					 GET_ALIGN(buf_pool) +
+					 ROUND_UP(*size, buf_pool->alloc->alignment),
+					 timeout);
+	}
 
 	if (b == NULL) {
 		return NULL;
@@ -121,7 +141,7 @@ static uint8_t *mem_pool_data_alloc(struct net_buf *buf, size_t *size,
 	*ref_count = 1U;
 
 	/* Return pointer to the byte following the ref count */
-	return ref_count + sizeof(void *);
+	return ref_count + GET_ALIGN(buf_pool);
 }
 
 static void mem_pool_data_unref(struct net_buf *buf, uint8_t *data)
@@ -130,7 +150,7 @@ static void mem_pool_data_unref(struct net_buf *buf, uint8_t *data)
 	struct k_heap *pool = buf_pool->alloc->alloc_data;
 	uint8_t *ref_count;
 
-	ref_count = data - sizeof(void *);
+	ref_count = data - GET_ALIGN(buf_pool);
 	if (--(*ref_count)) {
 		return;
 	}
@@ -171,23 +191,25 @@ const struct net_buf_data_cb net_buf_fixed_cb = {
 static uint8_t *heap_data_alloc(struct net_buf *buf, size_t *size,
 			     k_timeout_t timeout)
 {
+	struct net_buf_pool *buf_pool = net_buf_pool_get(buf->pool_id);
 	uint8_t *ref_count;
 
-	ref_count = k_malloc(sizeof(void *) + *size);
+	ref_count = k_malloc(GET_ALIGN(buf_pool) + *size);
 	if (!ref_count) {
 		return NULL;
 	}
 
 	*ref_count = 1U;
 
-	return ref_count + sizeof(void *);
+	return ref_count + GET_ALIGN(buf_pool);
 }
 
 static void heap_data_unref(struct net_buf *buf, uint8_t *data)
 {
+	struct net_buf_pool *buf_pool = net_buf_pool_get(buf->pool_id);
 	uint8_t *ref_count;
 
-	ref_count = data - sizeof(void *);
+	ref_count = data - GET_ALIGN(buf_pool);
 	if (--(*ref_count)) {
 		return;
 	}