| /** @file |
| @brief Network buffers for IP stack |
| |
| Network data is passed between components using nbuf. |
| */ |
| |
| /* |
| * Copyright (c) 2016 Intel Corporation |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| #define SYS_LOG_DOMAIN "net/nbuf" |
| #define NET_LOG_ENABLED 1 |
| #endif |
| |
| #include <kernel.h> |
| #include <toolchain.h> |
| #include <string.h> |
| #include <stdint.h> |
| #include <sys/types.h> |
| |
| #include <misc/util.h> |
| |
| #include <net/net_core.h> |
| #include <net/net_ip.h> |
| #include <net/buf.h> |
| #include <net/nbuf.h> |
| |
| #include "net_private.h" |
| |
| /* Available (free) buffers queue */ |
| #define NBUF_RX_COUNT CONFIG_NET_NBUF_RX_COUNT |
| #define NBUF_TX_COUNT CONFIG_NET_NBUF_TX_COUNT |
| #define NBUF_DATA_COUNT CONFIG_NET_NBUF_DATA_COUNT |
| #define NBUF_DATA_LEN CONFIG_NET_NBUF_DATA_SIZE |
| #define NBUF_USER_DATA_LEN CONFIG_NET_NBUF_USER_DATA_SIZE |
| |
| #if defined(CONFIG_NET_TCP) |
| #define APP_PROTO_LEN NET_TCPH_LEN |
| #else |
| #if defined(CONFIG_NET_UDP) |
| #define APP_PROTO_LEN NET_UDPH_LEN |
| #else |
| #define APP_PROTO_LEN 0 |
| #endif /* UDP */ |
| #endif /* TCP */ |
| |
| #if defined(CONFIG_NET_IPV6) || defined(CONFIG_NET_L2_RAW_CHANNEL) |
| #define IP_PROTO_LEN NET_IPV6H_LEN |
| #else |
| #if defined(CONFIG_NET_IPV4) |
| #define IP_PROTO_LEN NET_IPV4H_LEN |
| #else |
| #error "Either IPv6 or IPv4 needs to be selected." |
| #endif /* IPv4 */ |
| #endif /* IPv6 */ |
| |
| #define EXTRA_PROTO_LEN NET_ICMPH_LEN |
| |
| /* Make sure that IP + TCP/UDP header fit into one |
| * fragment. This makes possible to cast a protocol header |
| * struct into memory area. |
| */ |
| #if NBUF_DATA_LEN < (IP_PROTO_LEN + APP_PROTO_LEN) |
| #if defined(STRING2) |
| #undef STRING2 |
| #endif |
| #if defined(STRING) |
| #undef STRING |
| #endif |
| #define STRING2(x) #x |
| #define STRING(x) STRING2(x) |
| #pragma message "Data len " STRING(NBUF_DATA_LEN) |
| #pragma message "Minimum len " STRING(IP_PROTO_LEN + APP_PROTO_LEN) |
| #error "Too small net_buf fragment size" |
| #endif |
| |
| /* The RX and TX pools do not store any data. Only bearer / protocol |
| * related data is stored here. |
| */ |
| static inline void free_rx_bufs_func(struct net_buf *buf); |
| static inline void free_tx_bufs_func(struct net_buf *buf); |
| static inline void free_data_bufs_func(struct net_buf *buf); |
| |
| NET_BUF_POOL_DEFINE(rx_buffers, NBUF_RX_COUNT, 0, sizeof(struct net_nbuf), |
| free_rx_bufs_func); |
| NET_BUF_POOL_DEFINE(tx_buffers, NBUF_TX_COUNT, 0, sizeof(struct net_nbuf), |
| free_tx_bufs_func); |
| |
| /* The data fragment pool is for storing network data. */ |
| NET_BUF_POOL_DEFINE(data_buffers, NBUF_DATA_COUNT, NBUF_DATA_LEN, |
| NBUF_USER_DATA_LEN, free_data_bufs_func); |
| |
| /* We need to know the name of the pool in order to figure out |
| * how much data it is consuming. |
| */ |
| #define rx_buffers_pool _net_buf_pool_rx_buffers |
| #define tx_buffers_pool _net_buf_pool_tx_buffers |
| #define data_buffers_pool _net_buf_pool_data_buffers |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| |
| #define NET_BUF_CHECK_IF_IN_USE(buf, ref) \ |
| do { \ |
| if (ref) { \ |
| NET_ERR("**ERROR** buf %p in use (%s:%s():%d)", \ |
| buf, __FILE__, __func__, __LINE__); \ |
| } \ |
| } while (0) |
| |
| #define NET_BUF_CHECK_IF_NOT_IN_USE(buf, ref) \ |
| do { \ |
| if (!(ref)) { \ |
| NET_ERR("**ERROR** buf %p not in use (%s:%s():%d)", \ |
| buf, __FILE__, __func__, __LINE__); \ |
| } \ |
| } while (0) |
| |
| static int num_free_rx_bufs = NBUF_RX_COUNT; |
| static int num_free_tx_bufs = NBUF_TX_COUNT; |
| static int num_free_data_bufs = NBUF_DATA_COUNT; |
| |
| static inline void dec_free_rx_bufs(struct net_buf *buf) |
| { |
| if (!buf) { |
| return; |
| } |
| |
| num_free_rx_bufs--; |
| if (num_free_rx_bufs < 0) { |
| NET_DBG("*** ERROR *** Invalid RX buffer count."); |
| num_free_rx_bufs = 0; |
| } |
| } |
| |
| static inline void inc_free_rx_bufs(struct net_buf *buf) |
| { |
| if (!buf) { |
| return; |
| } |
| |
| if (num_free_rx_bufs > NBUF_RX_COUNT) { |
| num_free_rx_bufs = NBUF_RX_COUNT; |
| } else { |
| num_free_rx_bufs++; |
| } |
| } |
| |
| static inline void dec_free_tx_bufs(struct net_buf *buf) |
| { |
| if (!buf) { |
| return; |
| } |
| |
| num_free_tx_bufs--; |
| if (num_free_tx_bufs < 0) { |
| NET_DBG("*** ERROR *** Invalid TX buffer count."); |
| num_free_tx_bufs = 0; |
| } |
| } |
| |
| static inline void inc_free_tx_bufs(struct net_buf *buf) |
| { |
| if (!buf) { |
| return; |
| } |
| |
| if (num_free_tx_bufs > NBUF_TX_COUNT) { |
| num_free_tx_bufs = NBUF_TX_COUNT; |
| } else { |
| num_free_tx_bufs++; |
| } |
| } |
| |
| static inline void dec_free_data_bufs(struct net_buf *buf) |
| { |
| if (!buf) { |
| return; |
| } |
| |
| num_free_data_bufs--; |
| if (num_free_data_bufs < 0) { |
| NET_DBG("*** ERROR *** Invalid data buffer count."); |
| num_free_data_bufs = 0; |
| } |
| } |
| |
| static inline void inc_free_data_bufs(struct net_buf *buf) |
| { |
| if (!buf) { |
| return; |
| } |
| |
| if (num_free_data_bufs > NBUF_DATA_COUNT) { |
| num_free_data_bufs = NBUF_DATA_COUNT; |
| } else { |
| num_free_data_bufs++; |
| } |
| } |
| |
| static inline void dec_free_bufs(struct net_buf_pool *pool, |
| struct net_buf *buf) |
| { |
| if (pool == &rx_buffers) { |
| dec_free_rx_bufs(buf); |
| } else if (pool == &tx_buffers) { |
| dec_free_tx_bufs(buf); |
| } else { |
| dec_free_data_bufs(buf); |
| } |
| } |
| |
| static inline int get_frees(struct net_buf_pool *pool) |
| { |
| if (pool == &rx_buffers) { |
| return num_free_rx_bufs; |
| } else if (pool == &tx_buffers) { |
| return num_free_tx_bufs; |
| } else if (pool == &data_buffers) { |
| return num_free_data_bufs; |
| } |
| |
| return 0xffffffff; |
| } |
| |
| #else /* CONFIG_NET_DEBUG_NET_BUF */ |
| #define dec_free_rx_bufs(...) |
| #define inc_free_rx_bufs(...) |
| #define dec_free_tx_bufs(...) |
| #define inc_free_tx_bufs(...) |
| #define dec_free_data_bufs(...) |
| #define inc_free_data_bufs(...) |
| #define dec_free_bufs(...) |
| #define NET_BUF_CHECK_IF_IN_USE(buf, ref) |
| #define NET_BUF_CHECK_IF_NOT_IN_USE(buf, ref) |
| #endif /* CONFIG_NET_DEBUG_NET_BUF */ |
| |
| static inline bool is_from_data_pool(struct net_buf *buf) |
| { |
| return (buf->pool == &data_buffers); |
| } |
| |
| static inline void free_rx_bufs_func(struct net_buf *buf) |
| { |
| inc_free_rx_bufs(buf); |
| |
| net_buf_destroy(buf); |
| } |
| |
| static inline void free_tx_bufs_func(struct net_buf *buf) |
| { |
| inc_free_tx_bufs(buf); |
| |
| net_buf_destroy(buf); |
| } |
| |
| static inline void free_data_bufs_func(struct net_buf *buf) |
| { |
| inc_free_data_bufs(buf); |
| |
| net_buf_destroy(buf); |
| } |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| static inline const char *pool2str(struct net_buf_pool *pool) |
| { |
| if (pool == &rx_buffers) { |
| return "RX"; |
| } else if (pool == &tx_buffers) { |
| return "TX"; |
| } else if (pool == &data_buffers) { |
| return "DATA"; |
| } |
| |
| return "EXTERNAL"; |
| } |
| |
| void net_nbuf_print_frags(struct net_buf *buf) |
| { |
| struct net_buf *frag; |
| size_t total = 0; |
| int count = 0, frag_size = 0, ll_overhead = 0; |
| |
| NET_DBG("Buf %p frags %p", buf, buf->frags); |
| |
| NET_ASSERT(buf->frags); |
| |
| frag = buf->frags; |
| |
| while (frag) { |
| total += frag->len; |
| |
| frag_size = frag->size; |
| ll_overhead = net_buf_headroom(frag); |
| |
| NET_DBG("[%d] frag %p len %d size %d reserve %d", |
| count, frag, frag->len, frag_size, ll_overhead); |
| |
| count++; |
| |
| frag = frag->frags; |
| } |
| |
| NET_DBG("Total data size %zu, occupied %d bytes, ll overhead %d, " |
| "utilization %zu%%", |
| total, count * frag_size - count * ll_overhead, |
| count * ll_overhead, (total * 100) / (count * frag_size)); |
| } |
| |
| static struct net_buf *net_nbuf_get_reserve_debug(struct net_buf_pool *pool, |
| uint16_t reserve_head, |
| int32_t timeout, |
| const char *caller, |
| int line) |
| #else /* CONFIG_NET_DEBUG_NET_BUF */ |
| static struct net_buf *net_nbuf_get_reserve(struct net_buf_pool *pool, |
| uint16_t reserve_head, |
| int32_t timeout) |
| #endif /* CONFIG_NET_DEBUG_NET_BUF */ |
| { |
| struct net_buf *buf = NULL; |
| |
| /* |
| * The reserve_head variable in the function will tell |
| * the size of the link layer headers if there are any. |
| */ |
| |
| if (k_is_in_isr()) { |
| buf = net_buf_alloc(pool, K_NO_WAIT); |
| } else { |
| buf = net_buf_alloc(pool, timeout); |
| } |
| |
| if (!buf) { |
| return NULL; |
| } |
| |
| if (pool == &data_buffers) { |
| /* The buf->data will point to the start of the L3 |
| * header (like IPv4 or IPv6 packet header). |
| */ |
| net_buf_reserve(buf, reserve_head); |
| } else { |
| memset(net_buf_user_data(buf), 0, sizeof(struct net_nbuf)); |
| } |
| |
| dec_free_bufs(pool, buf); |
| |
| NET_BUF_CHECK_IF_NOT_IN_USE(buf, buf->ref + 1); |
| |
| NET_DBG("%s [%d] buf %p reserve %u ref %d (%s():%d)", |
| pool2str(pool), get_frees(pool), |
| buf, reserve_head, buf->ref, caller, line); |
| |
| return buf; |
| } |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| struct net_buf *net_nbuf_get_reserve_rx_debug(uint16_t reserve_head, |
| int32_t timeout, |
| const char *caller, int line) |
| { |
| return net_nbuf_get_reserve_debug(&rx_buffers, reserve_head, timeout, |
| caller, line); |
| } |
| |
| struct net_buf *net_nbuf_get_reserve_tx_debug(uint16_t reserve_head, |
| int32_t timeout, |
| const char *caller, int line) |
| { |
| return net_nbuf_get_reserve_debug(&tx_buffers, reserve_head, timeout, |
| caller, line); |
| } |
| |
| struct net_buf *net_nbuf_get_reserve_data_debug(uint16_t reserve_head, |
| int32_t timeout, |
| const char *caller, int line) |
| { |
| return net_nbuf_get_reserve_debug(&data_buffers, reserve_head, |
| timeout, caller, line); |
| } |
| |
| #else /* CONFIG_NET_DEBUG_NET_BUF */ |
| |
| struct net_buf *net_nbuf_get_reserve_rx(uint16_t reserve_head, |
| int32_t timeout) |
| { |
| return net_nbuf_get_reserve(&rx_buffers, reserve_head, timeout); |
| } |
| |
| struct net_buf *net_nbuf_get_reserve_tx(uint16_t reserve_head, |
| int32_t timeout) |
| { |
| return net_nbuf_get_reserve(&tx_buffers, reserve_head, timeout); |
| } |
| |
| struct net_buf *net_nbuf_get_reserve_data(uint16_t reserve_head, |
| int32_t timeout) |
| { |
| return net_nbuf_get_reserve(&data_buffers, reserve_head, timeout); |
| } |
| |
| #endif /* CONFIG_NET_DEBUG_NET_BUF */ |
| |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| static struct net_buf *net_nbuf_get_debug(struct net_buf_pool *pool, |
| struct net_context *context, |
| int32_t timeout, |
| const char *caller, int line) |
| #else |
| static struct net_buf *net_nbuf_get(struct net_buf_pool *pool, |
| struct net_context *context, |
| int32_t timeout) |
| #endif /* CONFIG_NET_DEBUG_NET_BUF */ |
| { |
| struct in6_addr *addr6 = NULL; |
| struct net_if *iface; |
| struct net_buf *buf; |
| uint16_t reserve; |
| |
| if (!context) { |
| return NULL; |
| } |
| |
| iface = net_context_get_iface(context); |
| |
| NET_ASSERT(iface); |
| |
| if (net_context_get_family(context) == AF_INET6) { |
| addr6 = &((struct sockaddr_in6 *) &context->remote)->sin6_addr; |
| } |
| |
| reserve = net_if_get_ll_reserve(iface, addr6); |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| buf = net_nbuf_get_reserve_debug(pool, reserve, timeout, caller, line); |
| #else |
| buf = net_nbuf_get_reserve(pool, reserve, timeout); |
| #endif |
| if (!buf) { |
| return buf; |
| } |
| |
| if (pool != &data_buffers) { |
| net_nbuf_set_context(buf, context); |
| net_nbuf_set_ll_reserve(buf, reserve); |
| net_nbuf_set_iface(buf, iface); |
| |
| if (context) { |
| net_nbuf_set_family(buf, |
| net_context_get_family(context)); |
| } |
| } |
| |
| return buf; |
| } |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| struct net_buf *net_nbuf_get_rx_debug(struct net_context *context, |
| int32_t timeout, |
| const char *caller, int line) |
| { |
| return net_nbuf_get_debug(&rx_buffers, context, timeout, caller, line); |
| } |
| |
| struct net_buf *net_nbuf_get_tx_debug(struct net_context *context, |
| int32_t timeout, |
| const char *caller, int line) |
| { |
| return net_nbuf_get_debug(&tx_buffers, context, timeout, caller, line); |
| } |
| |
| struct net_buf *net_nbuf_get_data_debug(struct net_context *context, |
| int32_t timeout, |
| const char *caller, int line) |
| { |
| return net_nbuf_get_debug(&data_buffers, context, timeout, |
| caller, line); |
| } |
| |
| #else /* CONFIG_NET_DEBUG_NET_BUF */ |
| |
| struct net_buf *net_nbuf_get_rx(struct net_context *context, int32_t timeout) |
| { |
| NET_ASSERT_INFO(context, "RX context not set"); |
| |
| return net_nbuf_get(&rx_buffers, context, timeout); |
| } |
| |
| struct net_buf *net_nbuf_get_tx(struct net_context *context, int32_t timeout) |
| { |
| NET_ASSERT_INFO(context, "TX context not set"); |
| |
| return net_nbuf_get(&tx_buffers, context, timeout); |
| } |
| |
| struct net_buf *net_nbuf_get_data(struct net_context *context, int32_t timeout) |
| { |
| NET_ASSERT_INFO(context, "Data context not set"); |
| |
| return net_nbuf_get(&data_buffers, context, timeout); |
| } |
| |
| #endif /* CONFIG_NET_DEBUG_NET_BUF */ |
| |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| void net_nbuf_unref_debug(struct net_buf *buf, const char *caller, int line) |
| { |
| struct net_buf *frag; |
| |
| #else |
| void net_nbuf_unref(struct net_buf *buf) |
| { |
| #endif /* CONFIG_NET_DEBUG_NET_BUF */ |
| if (!buf) { |
| NET_DBG("*** ERROR *** buf %p (%s():%d)", buf, caller, line); |
| return; |
| } |
| |
| if (!buf->ref) { |
| NET_DBG("*** ERROR *** buf %p is freed already (%s():%d)", |
| buf, caller, line); |
| return; |
| } |
| |
| NET_DBG("%s [%d] buf %p ref %d frags %p (%s():%d)", |
| pool2str(buf->pool), get_frees(buf->pool), |
| buf, buf->ref - 1, buf->frags, caller, line); |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| if (buf->ref > 1) { |
| goto done; |
| } |
| |
| /* Only remove fragments if debug is enabled since net_buf_unref takes |
| * care of removing all the fragments. |
| */ |
| frag = buf->frags; |
| while (frag) { |
| NET_DBG("%s [%d] buf %p ref %d frags %p (%s():%d)", |
| pool2str(frag->pool), get_frees(frag->pool), |
| frag, frag->ref - 1, frag->frags, caller, line); |
| |
| if (!frag->ref) { |
| NET_DBG("*** ERROR *** frag %p is freed already " |
| "(%s():%d)", frag, caller, line); |
| } |
| |
| frag = net_buf_frag_del(buf, frag); |
| } |
| |
| done: |
| #endif /* CONFIG_NET_DEBUG_NET_BUF */ |
| net_buf_unref(buf); |
| } |
| |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| struct net_buf *net_nbuf_ref_debug(struct net_buf *buf, const char *caller, |
| int line) |
| #else |
| struct net_buf *net_nbuf_ref(struct net_buf *buf) |
| #endif /* CONFIG_NET_DEBUG_NET_BUF */ |
| { |
| if (!buf) { |
| NET_DBG("*** ERROR *** buf %p (%s():%d)", buf, caller, line); |
| return NULL; |
| } |
| |
| NET_DBG("%s [%d] buf %p ref %d (%s():%d)", |
| pool2str(buf->pool), get_frees(buf->pool), |
| buf, buf->ref + 1, caller, line); |
| |
| return net_buf_ref(buf); |
| } |
| |
| struct net_buf *net_nbuf_copy(struct net_buf *orig, size_t amount, |
| size_t reserve, int32_t timeout) |
| { |
| uint16_t ll_reserve = net_buf_headroom(orig); |
| struct net_buf *frag, *first; |
| |
| if (!is_from_data_pool(orig)) { |
| NET_ERR("Buffer %p is not a data fragment", orig); |
| return NULL; |
| } |
| |
| frag = net_nbuf_get_reserve_data(ll_reserve, timeout); |
| if (!frag) { |
| return NULL; |
| } |
| |
| if (reserve > net_buf_tailroom(frag)) { |
| NET_ERR("Reserve %zu is too long, max is %zu", |
| reserve, net_buf_tailroom(frag)); |
| net_nbuf_unref(frag); |
| return NULL; |
| } |
| |
| net_buf_add(frag, reserve); |
| |
| first = frag; |
| |
| NET_DBG("Copying frag %p with %zu bytes and reserving %zu bytes", |
| first, amount, reserve); |
| |
| if (!orig->len) { |
| /* No data in the first fragment in the original message */ |
| NET_DBG("Original buffer empty!"); |
| return frag; |
| } |
| |
| while (orig && amount) { |
| int left_len = net_buf_tailroom(frag); |
| int copy_len; |
| |
| if (amount > orig->len) { |
| copy_len = orig->len; |
| } else { |
| copy_len = amount; |
| } |
| |
| if ((copy_len - left_len) >= 0) { |
| /* Just copy the data from original fragment |
| * to new fragment. The old data will fit the |
| * new fragment and there could be some space |
| * left in the new fragment. |
| */ |
| amount -= left_len; |
| |
| memcpy(net_buf_add(frag, left_len), orig->data, |
| left_len); |
| |
| if (!net_buf_tailroom(frag)) { |
| /* There is no space left in copy fragment. |
| * We must allocate a new one. |
| */ |
| struct net_buf *new_frag = |
| net_nbuf_get_reserve_data(ll_reserve, |
| timeout); |
| if (!new_frag) { |
| net_nbuf_unref(first); |
| return NULL; |
| } |
| |
| net_buf_frag_add(frag, new_frag); |
| |
| frag = new_frag; |
| } |
| |
| net_buf_pull(orig, left_len); |
| |
| continue; |
| } else { |
| /* We should be at the end of the original buf |
| * fragment list. |
| */ |
| amount -= copy_len; |
| |
| memcpy(net_buf_add(frag, copy_len), orig->data, |
| copy_len); |
| net_buf_pull(orig, copy_len); |
| } |
| |
| orig = orig->frags; |
| } |
| |
| return first; |
| } |
| |
| int net_nbuf_linear_copy(struct net_buf *dst, struct net_buf *src, |
| uint16_t offset, uint16_t len) |
| { |
| uint16_t to_copy; |
| uint16_t copied; |
| |
| if (dst->size < len) { |
| return -ENOMEM; |
| } |
| |
| /* find the right fragment to start copying from */ |
| while (src && offset >= src->len) { |
| offset -= src->len; |
| src = src->frags; |
| } |
| |
| /* traverse the fragment chain until len bytes are copied */ |
| copied = 0; |
| while (src && len > 0) { |
| to_copy = min(len, src->len - offset); |
| memcpy(dst->data + copied, src->data + offset, to_copy); |
| |
| copied += to_copy; |
| /* to_copy is always <= len */ |
| len -= to_copy; |
| src = src->frags; |
| /* after the first iteration, this value will be 0 */ |
| offset = 0; |
| } |
| |
| if (len > 0) { |
| return -ENOMEM; |
| } |
| |
| dst->len = copied; |
| |
| return 0; |
| } |
| |
| bool net_nbuf_is_compact(struct net_buf *buf) |
| { |
| struct net_buf *last; |
| size_t total = 0, calc; |
| int count = 0; |
| |
| last = NULL; |
| |
| if (!is_from_data_pool(buf)) { |
| /* Skip the first element that does not contain any data. |
| */ |
| buf = buf->frags; |
| } |
| |
| while (buf) { |
| total += buf->len; |
| count++; |
| |
| last = buf; |
| buf = buf->frags; |
| } |
| |
| NET_ASSERT(last); |
| |
| if (!last) { |
| return false; |
| } |
| |
| calc = count * last->size - net_buf_tailroom(last) - |
| count * net_buf_headroom(last); |
| |
| if (total == calc) { |
| return true; |
| } |
| |
| NET_DBG("Not compacted total %zu real %zu", total, calc); |
| |
| return false; |
| } |
| |
| bool net_nbuf_compact(struct net_buf *buf) |
| { |
| struct net_buf *prev; |
| |
| if (is_from_data_pool(buf)) { |
| NET_DBG("Buffer %p is a data fragment", buf); |
| return false; |
| } |
| |
| NET_DBG("Compacting data to buf %p", buf); |
| |
| buf = buf->frags; |
| prev = NULL; |
| |
| while (buf) { |
| if (buf->frags) { |
| /* Copy amount of data from next fragment to this |
| * fragment. |
| */ |
| size_t copy_len; |
| |
| copy_len = buf->frags->len; |
| if (copy_len > net_buf_tailroom(buf)) { |
| copy_len = net_buf_tailroom(buf); |
| } |
| |
| memcpy(net_buf_tail(buf), buf->frags->data, copy_len); |
| net_buf_add(buf, copy_len); |
| |
| memmove(buf->frags->data, |
| buf->frags->data + copy_len, |
| buf->frags->len - copy_len); |
| |
| buf->frags->len -= copy_len; |
| |
| /* Is there any more space in this fragment */ |
| if (net_buf_tailroom(buf)) { |
| /* There is. This also means that the next |
| * fragment is empty as otherwise we could |
| * not have copied all data. Remove next |
| * fragment as there is no data in it any more. |
| */ |
| net_buf_frag_del(buf, buf->frags); |
| |
| /* Then check next fragment */ |
| continue; |
| } |
| } else { |
| if (!buf->len) { |
| /* Remove the last fragment because there is no |
| * data in it. |
| */ |
| net_buf_frag_del(prev, buf); |
| |
| break; |
| } |
| } |
| |
| prev = buf; |
| buf = buf->frags; |
| } |
| |
| return true; |
| } |
| |
| struct net_buf *net_nbuf_pull(struct net_buf *buf, size_t amount) |
| { |
| struct net_buf *first; |
| ssize_t count = amount; |
| |
| if (amount == 0) { |
| NET_DBG("No data to remove."); |
| return buf; |
| } |
| |
| first = buf; |
| |
| if (!is_from_data_pool(buf)) { |
| NET_DBG("Buffer %p is not a data fragment", buf); |
| buf = buf->frags; |
| } |
| |
| NET_DBG("Removing first %zd bytes from the fragments (%zu bytes)", |
| count, net_buf_frags_len(buf)); |
| |
| while (buf && count > 0) { |
| if (count < buf->len) { |
| /* We can remove the data from this single fragment */ |
| net_buf_pull(buf, count); |
| return first; |
| } |
| |
| if (buf->len == count) { |
| if (buf == first) { |
| struct net_buf tmp; |
| |
| tmp.frags = buf; |
| first = buf->frags; |
| net_buf_frag_del(&tmp, buf); |
| } else { |
| net_buf_frag_del(first, buf); |
| } |
| |
| return first; |
| } |
| |
| count -= buf->len; |
| |
| if (buf == first) { |
| struct net_buf tmp; |
| |
| tmp.frags = buf; |
| first = buf->frags; |
| net_buf_frag_del(&tmp, buf); |
| buf = first; |
| } else { |
| net_buf_frag_del(first, buf); |
| buf = first->frags; |
| } |
| } |
| |
| if (count > 0) { |
| NET_ERR("Not enough data in the fragments"); |
| } |
| |
| return first; |
| } |
| |
| /* This helper routine will append multiple bytes, if there is no place for |
| * the data in current fragment then create new fragment and add it to |
| * the buffer. It assumes that the buffer has at least one fragment. |
| */ |
| static inline bool net_nbuf_append_bytes(struct net_buf *buf, |
| const uint8_t *value, |
| uint16_t len, int32_t timeout) |
| { |
| struct net_buf *frag = net_buf_frag_last(buf); |
| uint16_t ll_reserve = net_nbuf_ll_reserve(buf); |
| |
| do { |
| uint16_t count = min(len, net_buf_tailroom(frag)); |
| void *data = net_buf_add(frag, count); |
| |
| memcpy(data, value, count); |
| len -= count; |
| value += count; |
| |
| if (len == 0) { |
| return true; |
| } |
| |
| frag = net_nbuf_get_reserve_data(ll_reserve, timeout); |
| if (!frag) { |
| return false; |
| } |
| |
| net_buf_frag_add(buf, frag); |
| } while (1); |
| |
| return false; |
| } |
| |
| bool net_nbuf_append(struct net_buf *buf, uint16_t len, const uint8_t *data, |
| int32_t timeout) |
| { |
| struct net_buf *frag; |
| |
| if (!buf || !data) { |
| return false; |
| } |
| |
| if (is_from_data_pool(buf)) { |
| /* The buf needs to be a net_buf that has user data |
| * part. Otherwise we cannot use the net_nbuf_ll_reserve() |
| * function to figure out the reserve amount. |
| */ |
| NET_DBG("Buffer %p is a data fragment", buf); |
| return false; |
| } |
| |
| if (!buf->frags) { |
| frag = net_nbuf_get_reserve_data(net_nbuf_ll_reserve(buf), |
| timeout); |
| if (!frag) { |
| return false; |
| } |
| |
| net_buf_frag_add(buf, frag); |
| } |
| |
| return net_nbuf_append_bytes(buf, data, len, timeout); |
| } |
| |
| /* Helper routine to retrieve single byte from fragment and move |
| * offset. If required byte is last byte in framgent then return |
| * next fragment and set offset = 0. |
| */ |
| static inline struct net_buf *net_nbuf_read_byte(struct net_buf *buf, |
| uint16_t offset, |
| uint16_t *pos, |
| uint8_t *data) |
| { |
| if (data) { |
| *data = buf->data[offset]; |
| } |
| |
| *pos = offset + 1; |
| |
| if (*pos >= buf->len) { |
| *pos = 0; |
| |
| return buf->frags; |
| } |
| |
| return buf; |
| } |
| |
| /* Helper function to adjust offset in net_nbuf_read() call |
| * if given offset is more than current fragment length. |
| */ |
| static inline struct net_buf *adjust_offset(struct net_buf *buf, |
| uint16_t offset, uint16_t *pos) |
| { |
| if (!buf || !is_from_data_pool(buf)) { |
| NET_ERR("Invalid buffer or buffer is not a fragment"); |
| return NULL; |
| } |
| |
| while (buf) { |
| if (offset == buf->len) { |
| *pos = 0; |
| |
| return buf->frags; |
| } else if (offset < buf->len) { |
| *pos = offset; |
| |
| return buf; |
| } |
| |
| offset -= buf->len; |
| buf = buf->frags; |
| } |
| |
| NET_ERR("Invalid offset, failed to adjust"); |
| |
| return NULL; |
| } |
| |
| struct net_buf *net_nbuf_read(struct net_buf *buf, uint16_t offset, |
| uint16_t *pos, uint16_t len, uint8_t *data) |
| { |
| uint16_t copy = 0; |
| |
| buf = adjust_offset(buf, offset, pos); |
| if (!buf) { |
| goto error; |
| } |
| |
| while (len-- > 0 && buf) { |
| if (data) { |
| buf = net_nbuf_read_byte(buf, *pos, pos, data + copy++); |
| } else { |
| buf = net_nbuf_read_byte(buf, *pos, pos, NULL); |
| } |
| |
| /* Error: Still reamining length to be read, but no data. */ |
| if (!buf && len) { |
| NET_ERR("Not enough data to read"); |
| goto error; |
| } |
| } |
| |
| return buf; |
| |
| error: |
| *pos = 0xffff; |
| |
| return NULL; |
| } |
| |
| struct net_buf *net_nbuf_read_be16(struct net_buf *buf, uint16_t offset, |
| uint16_t *pos, uint16_t *value) |
| { |
| struct net_buf *retbuf; |
| uint8_t v16[2]; |
| |
| retbuf = net_nbuf_read(buf, offset, pos, sizeof(uint16_t), v16); |
| |
| *value = v16[0] << 8 | v16[1]; |
| |
| return retbuf; |
| } |
| |
| struct net_buf *net_nbuf_read_be32(struct net_buf *buf, uint16_t offset, |
| uint16_t *pos, uint32_t *value) |
| { |
| struct net_buf *retbuf; |
| uint8_t v32[4]; |
| |
| retbuf = net_nbuf_read(buf, offset, pos, sizeof(uint32_t), v32); |
| |
| *value = v32[0] << 24 | v32[1] << 16 | v32[2] << 8 | v32[3]; |
| |
| return retbuf; |
| } |
| |
| static inline struct net_buf *check_and_create_data(struct net_buf *buf, |
| struct net_buf *data, |
| int32_t timeout) |
| { |
| struct net_buf *frag; |
| |
| if (data) { |
| return data; |
| } |
| |
| frag = net_nbuf_get_reserve_data(net_nbuf_ll_reserve(buf), |
| timeout); |
| if (!frag) { |
| return NULL; |
| } |
| |
| net_buf_frag_add(buf, frag); |
| |
| return frag; |
| } |
| |
| static inline struct net_buf *adjust_write_offset(struct net_buf *buf, |
| struct net_buf *frag, |
| uint16_t offset, |
| uint16_t *pos, |
| int32_t timeout) |
| { |
| uint16_t tailroom; |
| |
| do { |
| frag = check_and_create_data(buf, frag, timeout); |
| if (!frag) { |
| return NULL; |
| } |
| |
| /* Offset is less than current fragment length, so new data |
| * will start from this "offset". |
| */ |
| if (offset < frag->len) { |
| *pos = offset; |
| |
| return frag; |
| } |
| |
| /* Offset is equal to fragment length. If some tailtoom exists, |
| * offset start from same fragment otherwise offset starts from |
| * beginning of next fragment. |
| */ |
| if (offset == frag->len) { |
| if (net_buf_tailroom(frag)) { |
| *pos = offset; |
| |
| return frag; |
| } |
| |
| *pos = 0; |
| |
| return check_and_create_data(buf, frag->frags, |
| timeout); |
| } |
| |
| /* If the offset is more than current fragment length, remove |
| * current fragment length and verify with tailroom in the |
| * current fragment. From here on create empty (space/fragments) |
| * to reach proper offset. |
| */ |
| if (offset > frag->len) { |
| |
| offset -= frag->len; |
| tailroom = net_buf_tailroom(frag); |
| |
| if (offset < tailroom) { |
| /* Create empty space */ |
| net_buf_add(frag, offset); |
| |
| *pos = frag->len; |
| |
| return frag; |
| } |
| |
| if (offset == tailroom) { |
| /* Create empty space */ |
| net_buf_add(frag, tailroom); |
| |
| *pos = 0; |
| |
| return check_and_create_data(buf, |
| frag->frags, |
| timeout); |
| } |
| |
| if (offset > tailroom) { |
| /* Creating empty space */ |
| net_buf_add(frag, tailroom); |
| offset -= tailroom; |
| |
| frag = check_and_create_data(buf, |
| frag->frags, |
| timeout); |
| } |
| } |
| |
| } while (1); |
| |
| return NULL; |
| } |
| |
| struct net_buf *net_nbuf_write(struct net_buf *buf, struct net_buf *frag, |
| uint16_t offset, uint16_t *pos, |
| uint16_t len, uint8_t *data, |
| int32_t timeout) |
| { |
| uint16_t ll_reserve; |
| |
| if (!buf || is_from_data_pool(buf)) { |
| NET_ERR("Invalid buffer or it is data fragment"); |
| goto error; |
| } |
| |
| ll_reserve = net_nbuf_ll_reserve(buf); |
| |
| frag = adjust_write_offset(buf, frag, offset, &offset, timeout); |
| if (!frag) { |
| NET_DBG("Failed to adjust offset"); |
| goto error; |
| } |
| |
| do { |
| uint16_t space = frag->size - net_buf_headroom(frag) - offset; |
| uint16_t count = min(len, space); |
| int size_to_add; |
| |
| memcpy(frag->data + offset, data, count); |
| |
| /* If we are overwriting on already available space then need |
| * not to update the length, otherwise increase it. |
| */ |
| size_to_add = offset + count - frag->len; |
| if (size_to_add > 0) { |
| net_buf_add(frag, size_to_add); |
| } |
| |
| len -= count; |
| if (len == 0) { |
| *pos = offset + count; |
| |
| return frag; |
| } |
| |
| data += count; |
| offset = 0; |
| frag = frag->frags; |
| |
| if (!frag) { |
| frag = net_nbuf_get_reserve_data(ll_reserve, timeout); |
| if (!frag) { |
| goto error; |
| } |
| |
| net_buf_frag_add(buf, frag); |
| } |
| } while (1); |
| |
| error: |
| *pos = 0xffff; |
| |
| return NULL; |
| } |
| |
| static inline bool insert_data(struct net_buf *buf, struct net_buf *frag, |
| struct net_buf *temp, uint16_t offset, |
| uint16_t len, uint8_t *data, |
| int32_t timeout) |
| { |
| struct net_buf *insert; |
| |
| do { |
| uint16_t count = min(len, net_buf_tailroom(frag)); |
| |
| /* Copy insert data */ |
| memcpy(frag->data + offset, data, count); |
| net_buf_add(frag, count); |
| |
| len -= count; |
| if (len == 0) { |
| /* Once insertion is done, then add the data if |
| * there is anything after original insertion |
| * offset. |
| */ |
| if (temp) { |
| net_buf_frag_insert(frag, temp); |
| } |
| |
| /* As we are creating temporary buffers to cache, |
| * compact the fragments to save space. |
| */ |
| net_nbuf_compact(buf->frags); |
| |
| return true; |
| } |
| |
| data += count; |
| offset = 0; |
| |
| insert = net_nbuf_get_reserve_data(net_nbuf_ll_reserve(buf), |
| timeout); |
| if (!insert) { |
| return false; |
| } |
| |
| net_buf_frag_insert(frag, insert); |
| frag = insert; |
| |
| } while (1); |
| |
| return false; |
| } |
| |
| static inline struct net_buf *adjust_insert_offset(struct net_buf *buf, |
| uint16_t offset, |
| uint16_t *pos) |
| { |
| if (!buf || !is_from_data_pool(buf)) { |
| NET_ERR("Invalid buffer or buffer is not a fragment"); |
| |
| return NULL; |
| } |
| |
| while (buf) { |
| if (offset == buf->len) { |
| *pos = 0; |
| |
| return buf->frags; |
| } |
| |
| if (offset < buf->len) { |
| *pos = offset; |
| |
| return buf; |
| } |
| |
| if (offset > buf->len) { |
| if (buf->frags) { |
| offset -= buf->len; |
| buf = buf->frags; |
| } else { |
| return NULL; |
| } |
| } |
| } |
| |
| NET_ERR("Invalid offset, failed to adjust"); |
| |
| return NULL; |
| } |
| |
| bool net_nbuf_insert(struct net_buf *buf, struct net_buf *frag, |
| uint16_t offset, uint16_t len, uint8_t *data, |
| int32_t timeout) |
| { |
| struct net_buf *temp = NULL; |
| uint16_t bytes; |
| |
| if (!buf || is_from_data_pool(buf)) { |
| return false; |
| } |
| |
| frag = adjust_insert_offset(frag, offset, &offset); |
| if (!frag) { |
| return false; |
| } |
| |
| /* If there is any data after offset, store in temp fragment and |
| * add it after insertion is completed. |
| */ |
| bytes = frag->len - offset; |
| if (bytes) { |
| temp = net_nbuf_get_reserve_data(net_nbuf_ll_reserve(buf), |
| timeout); |
| if (!temp) { |
| return false; |
| } |
| |
| memcpy(net_buf_add(temp, bytes), frag->data + offset, bytes); |
| |
| frag->len -= bytes; |
| } |
| |
| /* Insert data into current(frag) fragment from "offset". */ |
| return insert_data(buf, frag, temp, offset, len, data, timeout); |
| } |
| |
| void net_nbuf_get_info(size_t *tx_size, size_t *rx_size, size_t *data_size, |
| int *tx, int *rx, int *data) |
| { |
| if (tx_size) { |
| *tx_size = sizeof(tx_buffers_pool); |
| } |
| |
| if (rx_size) { |
| *rx_size = sizeof(rx_buffers_pool); |
| } |
| |
| if (data_size) { |
| *data_size = sizeof(data_buffers_pool); |
| } |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| *tx = get_frees(&tx_buffers); |
| *rx = get_frees(&rx_buffers); |
| *data = get_frees(&data_buffers); |
| #else |
| *tx = BIT(31); |
| *rx = BIT(31); |
| *data = BIT(31); |
| #endif /* CONFIG_NET_DEBUG_NET_BUF */ |
| } |
| |
| #if defined(CONFIG_NET_DEBUG_NET_BUF) |
| void net_nbuf_print(void) |
| { |
| int tx, rx, data; |
| |
| net_nbuf_get_info(NULL, NULL, NULL, &tx, &rx, &data); |
| |
| NET_DBG("TX %d RX %d DATA %d", tx, rx, data); |
| } |
| #endif /* CONFIG_NET_DEBUG_NET_BUF */ |
| |
| void net_nbuf_init(void) |
| { |
| NET_DBG("Allocating %u RX (%zu bytes), %u TX (%zu bytes) " |
| "and %u data (%zu bytes) buffers", |
| NBUF_RX_COUNT, sizeof(rx_buffers_pool), |
| NBUF_TX_COUNT, sizeof(tx_buffers_pool), |
| NBUF_DATA_COUNT, sizeof(data_buffers_pool)); |
| } |