This is an automated email from the ASF dual-hosted git repository. janc pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/mynewt-nimble.git
commit 427f3b5abbd4c61aa7e88a287df249e154d60b81 Author: Szymon Janc <[email protected]> AuthorDate: Fri Jul 25 18:54:05 2025 +0200 porting: Sync OS components with Mynewt There were some fixes in core that should be also used in NimBLE ports. Some msys code qwere also synchronized with core for easier future updates. --- porting/nimble/include/os/os_mbuf.h | 190 +++--- porting/nimble/include/os/os_mempool.h | 98 ++- porting/nimble/include/os/queue.h | 788 +++++++++++++++++------ porting/nimble/src/os_mbuf.c | 204 +----- porting/nimble/src/os_mempool.c | 71 +- porting/nimble/src/{os_msys_init.c => os_msys.c} | 153 ++++- 6 files changed, 1000 insertions(+), 504 deletions(-) diff --git a/porting/nimble/include/os/os_mbuf.h b/porting/nimble/include/os/os_mbuf.h index 6c05b2a73..81fcab9d0 100644 --- a/porting/nimble/include/os/os_mbuf.h +++ b/porting/nimble/include/os/os_mbuf.h @@ -53,6 +53,9 @@ struct os_mbuf_pool { */ struct os_mempool *omp_pool; + /** + * Next mbuf pool in the list + */ STAILQ_ENTRY(os_mbuf_pool) omp_next; }; @@ -70,6 +73,9 @@ struct os_mbuf_pkthdr { */ uint16_t omp_flags; + /** + * Next mbuf packet header in the list + */ STAILQ_ENTRY(os_mbuf_pkthdr) omp_next; }; @@ -99,6 +105,9 @@ struct os_mbuf { */ struct os_mbuf_pool *om_omp; + /** + * Next mbuf in the list + */ SLIST_ENTRY(os_mbuf) om_next; /** @@ -111,34 +120,34 @@ struct os_mbuf { * Structure representing a queue of mbufs. */ struct os_mqueue { + /** A queue of mbuf packet headers. */ STAILQ_HEAD(, os_mbuf_pkthdr) mq_head; /** Event to post when new buffers are available on the queue. */ struct ble_npl_event mq_ev; }; -/* +/** * Given a flag number, provide the mask for it * - * @param __n The number of the flag in the mask + * @param __n The number of the flag in the mask */ #define OS_MBUF_F_MASK(__n) (1 << (__n)) -/* +/** * Checks whether a given mbuf is a packet header mbuf * - * @param __om The mbuf to check + * @param __om The mbuf to check */ #define OS_MBUF_IS_PKTHDR(__om) \ ((__om)->om_pkthdr_len >= sizeof (struct os_mbuf_pkthdr)) /** Get a packet header pointer given an mbuf pointer */ -#define OS_MBUF_PKTHDR(__om) ((struct os_mbuf_pkthdr *)(uintptr_t) \ - (void *)((uint8_t *)&(__om)->om_data \ - + sizeof(struct os_mbuf))) +#define OS_MBUF_PKTHDR(__om) ((struct os_mbuf_pkthdr *) \ + ((uint8_t *)&(__om)->om_data + sizeof(struct os_mbuf))) /** Given a mbuf packet header pointer, return a pointer to the mbuf */ #define OS_MBUF_PKTHDR_TO_MBUF(__hdr) \ - (struct os_mbuf *)(uintptr_t)((uint8_t *)(__hdr) - sizeof(struct os_mbuf)) + (struct os_mbuf *)((uint8_t *)(__hdr) - sizeof(struct os_mbuf)) /** * Gets the length of an entire mbuf chain. The specified mbuf must have a @@ -149,8 +158,8 @@ struct os_mqueue { /** * Access the data of a mbuf, and cast it to type * - * @param __om The mbuf to access, and cast - * @param __type The type to cast it to + * @param __om The mbuf to access, and cast + * @param __type The type to cast it to */ #define OS_MBUF_DATA(__om, __type) \ (__type) ((__om)->om_data) @@ -190,7 +199,7 @@ _os_mbuf_leadingspace(struct os_mbuf *om) } leadingspace = (uint16_t) (OS_MBUF_DATA(om, uint8_t *) - - ((uint8_t *) &om->om_databuf[0] + startoff)); + ((uint8_t *) &om->om_databuf[0] + startoff)); return (leadingspace); } @@ -202,10 +211,10 @@ _os_mbuf_leadingspace(struct os_mbuf *om) * Works on both packet header, and regular mbufs, as it accounts * for the additional space allocated to the packet header. * - * @param __omp Is the mbuf pool (which contains packet header length.) - * @param __om Is the mbuf in that pool to get the leadingspace for + * @param __om The mbuf in that pool to get the leading + * space for * - * @return Amount of leading space available in the mbuf + * @return Amount of leading space available in the mbuf */ #define OS_MBUF_LEADINGSPACE(__om) _os_mbuf_leadingspace(__om) @@ -221,7 +230,7 @@ _os_mbuf_trailingspace(struct os_mbuf *om) omp = om->om_omp; return (&om->om_databuf[0] + omp->omp_databuf_len) - - (om->om_data + om->om_len); + (om->om_data + om->om_len); } /** @endcond */ @@ -230,10 +239,10 @@ _os_mbuf_trailingspace(struct os_mbuf *om) * Returns the trailing space (space at the end) of the mbuf. * Works on both packet header and regular mbufs. * - * @param __omp The mbuf pool for this mbuf - * @param __om Is the mbuf in that pool to get trailing space for + * @param __om The mbuf in that pool to get the trailing + * space for * - * @return The amount of trailing space available in the mbuf + * @return Amount of trailing space available in the mbuf */ #define OS_MBUF_TRAILINGSPACE(__om) _os_mbuf_trailingspace(__om) @@ -260,11 +269,12 @@ int os_mqueue_init(struct os_mqueue *mq, ble_npl_event_fn *ev_cb, void *arg); /** * Remove and return a single mbuf from the mbuf queue. Does not block. * - * @param mq The mbuf queue to pull an element off of. + * @param mq The mbuf queue to pull an element off of. * - * @return The next mbuf in the queue, or NULL if queue has no mbufs. + * @return The next mbuf in the queue; + * NULL if queue has no mbufs. */ -struct os_mbuf *os_mqueue_get(struct os_mqueue *); +struct os_mbuf *os_mqueue_get(struct os_mqueue *mq); /** * Adds a packet (i.e. packet header mbuf) to an mqueue. The event associated @@ -272,11 +282,12 @@ struct os_mbuf *os_mqueue_get(struct os_mqueue *); * * @param mq The mbuf queue to append the mbuf to. * @param evq The event queue to post an event to. - * @param m The mbuf to append to the mbuf queue. + * @param om The mbuf to append to the mbuf queue. * * @return 0 on success, non-zero on failure. */ -int os_mqueue_put(struct os_mqueue *, struct ble_npl_eventq *, struct os_mbuf *); +int os_mqueue_put(struct os_mqueue *mq, struct ble_npl_eventq *evq, + struct os_mbuf *om); /** * MSYS is a system level mbuf registry. Allows the system to share @@ -290,20 +301,24 @@ int os_mqueue_put(struct os_mqueue *, struct ble_npl_eventq *, struct os_mbuf *) * os_msys_register() registers a mbuf pool with MSYS, and allows MSYS to * allocate mbufs out of it. * - * @param new_pool The pool to register with MSYS + * @param new_pool The pool to register with MSYS * - * @return 0 on success, non-zero on failure + * @return 0 on success; + * non-zero on failure */ -int os_msys_register(struct os_mbuf_pool *); +int os_msys_register(struct os_mbuf_pool *new_pool); /** * Allocate a mbuf from msys. Based upon the data size requested, * os_msys_get() will choose the mbuf pool that has the best fit. * - * @param dsize The estimated size of the data being stored in the mbuf - * @param leadingspace The amount of leadingspace to allocate in the mbuf + * @param dsize The estimated size of the data being stored in + * the mbuf + * @param leadingspace The amount of leadingspace to allocate in + * the mbuf * - * @return A freshly allocated mbuf on success, NULL on failure. + * @return A freshly allocated mbuf on success; + * NULL on failure. */ struct os_mbuf *os_msys_get(uint16_t dsize, uint16_t leadingspace); @@ -316,72 +331,77 @@ void os_msys_reset(void); * Allocate a packet header structure from the MSYS pool. See * os_msys_register() for a description of MSYS. * - * @param dsize The estimated size of the data being stored in the mbuf - * @param user_hdr_len The length to allocate for the packet header structure + * @param dsize The estimated size of the data being stored in + * the mbuf + * @param user_hdr_len The length to allocate for the packet header + * structure * - * @return A freshly allocated mbuf on success, NULL on failure. + * @return A freshly allocated mbuf on success; + * NULL on failure. */ struct os_mbuf *os_msys_get_pkthdr(uint16_t dsize, uint16_t user_hdr_len); /** * Count the number of blocks in all the mbuf pools that are allocated. * - * @return total number of blocks allocated in Msys + * @return total number of blocks allocated in Msys */ int os_msys_count(void); /** * Return the number of free blocks in Msys * - * @return Number of free blocks available in Msys + * @return Number of free blocks available in Msys */ int os_msys_num_free(void); /** * Initialize a pool of mbufs. * - * @param omp The mbuf pool to initialize - * @param mp The memory pool that will hold this mbuf pool - * @param buf_len The length of the buffer itself. - * @param nbufs The number of buffers in the pool + * @param omp The mbuf pool to initialize + * @param mp The memory pool that will hold this mbuf pool + * @param buf_len The length of the buffer itself. + * @param nbufs The number of buffers in the pool * - * @return 0 on success, error code on failure. + * @return 0 on success; + * error code on failure. */ -int os_mbuf_pool_init(struct os_mbuf_pool *, struct os_mempool *mp, - uint16_t, uint16_t); - +int os_mbuf_pool_init(struct os_mbuf_pool *omp, struct os_mempool *mp, + uint16_t buf_len, uint16_t nbufs); /** * Get an mbuf from the mbuf pool. The mbuf is allocated, and initialized * prior to being returned. * - * @param omp The mbuf pool to return the packet from - * @param leadingspace The amount of leadingspace to put before the data - * section by default. + * @param omp The mbuf pool to return the packet from + * @param leadingspace The amount of leadingspace to put before the + * data section by default. * - * @return An initialized mbuf on success, and NULL on failure. + * @return An initialized mbuf on success; + * NULL on failure. */ -struct os_mbuf *os_mbuf_get(struct os_mbuf_pool *omp, uint16_t); +struct os_mbuf *os_mbuf_get(struct os_mbuf_pool *omp, uint16_t leadingspace); /** * Allocate a new packet header mbuf out of the os_mbuf_pool. * - * @param omp The mbuf pool to allocate out of - * @param user_pkthdr_len The packet header length to reserve for the caller. + * @param omp The mbuf pool to allocate out of + * @param user_pkthdr_len The packet header length to reserve for the + * caller. * - * @return A freshly allocated mbuf on success, NULL on failure. + * @return A freshly allocated mbuf on success; + * NULL on failure. */ struct os_mbuf *os_mbuf_get_pkthdr(struct os_mbuf_pool *omp, - uint8_t pkthdr_len); + uint8_t user_pkthdr_len); /** * Duplicate a chain of mbufs. Return the start of the duplicated chain. * - * @param omp The mbuf pool to duplicate out of - * @param om The mbuf chain to duplicate + * @param om The mbuf chain to duplicate * - * @return A pointer to the new chain of mbufs + * @return A pointer to the new chain of mbufs */ -struct os_mbuf *os_mbuf_dup(struct os_mbuf *m); +struct os_mbuf *os_mbuf_dup(struct os_mbuf *om); /** * Locates the specified absolute offset within an mbuf chain. The offset @@ -400,11 +420,11 @@ struct os_mbuf *os_mbuf_off(const struct os_mbuf *om, int off, uint16_t *out_off); -/* +/** * Copy data from an mbuf chain starting "off" bytes from the beginning, * continuing for "len" bytes, into the indicated buffer. * - * @param m The mbuf chain to copy from + * @param om The mbuf chain to copy from * @param off The offset into the mbuf chain to begin copying from * @param len The length of the data to copy * @param dst The destination buffer to copy into @@ -412,7 +432,7 @@ struct os_mbuf *os_mbuf_off(const struct os_mbuf *om, int off, * @return 0 on success; * -1 if the mbuf does not contain enough data. */ -int os_mbuf_copydata(const struct os_mbuf *m, int off, int len, void *dst); +int os_mbuf_copydata(const struct os_mbuf *om, int off, int len, void *dst); /** * @brief Calculates the length of an mbuf chain. @@ -431,13 +451,14 @@ uint16_t os_mbuf_len(const struct os_mbuf *om); /** * Append data onto a mbuf * - * @param om The mbuf to append the data onto - * @param data The data to append onto the mbuf - * @param len The length of the data to append + * @param om The mbuf to append the data onto + * @param data The data to append onto the mbuf + * @param len The length of the data to append * - * @return 0 on success, and an error code on failure + * @return 0 on success; + * an error code on failure */ -int os_mbuf_append(struct os_mbuf *m, const void *, uint16_t); +int os_mbuf_append(struct os_mbuf *om, const void *data, uint16_t len); /** * Reads data from one mbuf and appends it to another. On error, the specified @@ -460,20 +481,21 @@ int os_mbuf_appendfrom(struct os_mbuf *dst, const struct os_mbuf *src, /** * Release a mbuf back to the pool * - * @param omp The Mbuf pool to release back to - * @param om The Mbuf to release back to the pool + * @param om The Mbuf to release back to the pool * - * @return 0 on success, -1 on failure + * @return 0 on success; + * -1 on failure */ -int os_mbuf_free(struct os_mbuf *mb); +int os_mbuf_free(struct os_mbuf *om); /** * Free a chain of mbufs * - * @param omp The mbuf pool to free the chain of mbufs into - * @param om The starting mbuf of the chain to free back into the pool + * @param om The starting mbuf of the chain to free back into + * the pool * - * @return 0 on success, -1 on failure + * @return 0 on success; + * -1 on failure */ int os_mbuf_free_chain(struct os_mbuf *om); @@ -481,12 +503,12 @@ int os_mbuf_free_chain(struct os_mbuf *om); * Adjust the length of a mbuf, trimming either from the head or the tail * of the mbuf. * - * @param mp The mbuf chain to adjust - * @param req_len The length to trim from the mbuf. If positive, trims - * from the head of the mbuf, if negative, trims from the - * tail of the mbuf. + * @param om The mbuf chain to adjust + * @param req_len The length to trim from the mbuf. If positive, + * trims from the head of the mbuf, if + * negative, trims from the tail of the mbuf. */ -void os_mbuf_adj(struct os_mbuf *mp, int req_len); +void os_mbuf_adj(struct os_mbuf *om, int req_len); /** @@ -536,7 +558,6 @@ int os_mbuf_cmpm(const struct os_mbuf *om1, uint16_t offset1, * * The specified mbuf chain does not need to contain a packet header. * - * @param omp The mbuf pool to allocate from. * @param om The head of the mbuf chain. * @param len The number of bytes to prepend. * @@ -564,7 +585,6 @@ struct os_mbuf *os_mbuf_prepend_pullup(struct os_mbuf *om, uint16_t len); * it is extended as necessary. If the destination mbuf contains a packet * header, the header length is updated. * - * @param omp The mbuf pool to allocate from. * @param om The mbuf chain to copy into. * @param off The offset within the chain to copy to. * @param src The source buffer to copy from. @@ -591,7 +611,6 @@ void os_mbuf_concat(struct os_mbuf *first, struct os_mbuf *second); * appended to the chain. It is an error to request more data than can fit in * a single buffer. * - * @param omp * @param om The head of the chain to extend. * @param len The number of bytes to extend by. * @@ -610,11 +629,12 @@ void *os_mbuf_extend(struct os_mbuf *om, uint16_t len); * extra bytes to the contiguous region, in an attempt to avoid being * called next time. * - * @param omp The mbuf pool to take the mbufs out of - * @param om The mbuf chain to make contiguous - * @param len The number of bytes in the chain to make contiguous + * @param om The mbuf chain to make contiguous + * @param len The number of bytes in the chain to make + * contiguous * - * @return The contiguous mbuf chain on success, NULL on failure. + * @return The contiguous mbuf chain on success; + * NULL on failure. */ struct os_mbuf *os_mbuf_pullup(struct os_mbuf *om, uint16_t len); @@ -657,10 +677,10 @@ int os_mbuf_widen(struct os_mbuf *om, uint16_t off, uint16_t len); * header it is discarded. If m1 is NULL, NULL is returned and * m2 is left untouched. * - * @param m1 Pointer to first mbuf chain to pack - * @param m2 Pointer to second mbuf chain to pack + * @param m1 Pointer to first mbuf chain to pack + * @param m2 Pointer to second mbuf chain to pack * - * @return struct os_mbuf* Pointer to resulting mbuf chain + * @return Pointer to resulting mbuf chain */ struct os_mbuf *os_mbuf_pack_chains(struct os_mbuf *m1, struct os_mbuf *m2); diff --git a/porting/nimble/include/os/os_mempool.h b/porting/nimble/include/os/os_mempool.h index 74bc43fe0..d4c3313a3 100644 --- a/porting/nimble/include/os/os_mempool.h +++ b/porting/nimble/include/os/os_mempool.h @@ -43,6 +43,7 @@ extern "C" { * caller. */ struct os_memblock { + /** Next memory block in the list. */ SLIST_ENTRY(os_memblock) mb_next; }; @@ -66,8 +67,10 @@ struct os_mempool { /** Bitmap of OS_MEMPOOL_F_[...] values. */ uint8_t mp_flags; /** Address of memory buffer used by pool */ - uint32_t mp_membuf_addr; + uintptr_t mp_membuf_addr; + /** Next memory pool in the list. */ STAILQ_ENTRY(os_mempool) mp_list; + /** Head of the list of memory blocks. */ SLIST_HEAD(,os_memblock); /** Name for memory block */ char *name; @@ -102,14 +105,19 @@ struct os_mempool_ext; typedef os_error_t os_mempool_put_fn(struct os_mempool_ext *ome, void *data, void *arg); +/** Extended memory pool. */ struct os_mempool_ext { + /** Standard memory pool. */ struct os_mempool mpe_mp; - /* Callback that is executed immediately when a block is freed. */ + /** Callback that is executed immediately when a block is freed. */ os_mempool_put_fn *mpe_put_cb; + + /** Optional argument passed to the callback function. */ void *mpe_put_arg; }; +/** Length of the name of memory pool */ #define OS_MEMPOOL_INFO_NAME_LEN (32) /** @@ -132,15 +140,30 @@ struct os_mempool_info { /** * Get information about the next system memory pool. * - * @param mempool The current memory pool, or NULL if starting iteration. - * @param info A pointer to the structure to return memory pool information - * into. + * @param mp The current memory pool, or NULL if starting + * iteration. + * @param omi A pointer to the structure to return memory pool + * information into. * - * @return The next memory pool in the list to get information about, or NULL - * when at the last memory pool. + * @return The next memory pool in the list to get + * information about; + * NULL when at the last memory pool. */ -struct os_mempool *os_mempool_info_get_next(struct os_mempool *, - struct os_mempool_info *); +struct os_mempool *os_mempool_info_get_next(struct os_mempool *mp, + struct os_mempool_info *omi); + +/** + * Get information system memory pool by name. + * + * @param mempool_name The name of mempool. + * @param info A pointer to the structure to return memory pool + * information into, can be NULL. + * + * @return The memory pool found; + * NULL when there is no such memory pool. + */ +struct os_mempool *os_mempool_get(const char *mempool_name, + struct os_mempool_info *info); /* * To calculate size of the memory buffer needed for the pool. NOTE: This size @@ -148,11 +171,10 @@ struct os_mempool *os_mempool_info_get_next(struct os_mempool *, * the memory pool. */ #if MYNEWT_VAL(OS_MEMPOOL_GUARD) -/* - * Leave extra 4 bytes of guard area at the end. - */ +/** Leave extra 4 bytes of guard area at the end. */ #define OS_MEMPOOL_BLOCK_SZ(sz) ((sz) + sizeof(os_membuf_t)) #else +/** Size of a memory pool block. */ #define OS_MEMPOOL_BLOCK_SZ(sz) (sz) #endif #if (OS_ALIGNMENT == 4) @@ -164,6 +186,8 @@ typedef __uint128_t os_membuf_t; #else #error "Unhandled `OS_ALIGNMENT` for `os_membuf_t`" #endif /* OS_ALIGNMENT == * */ + +/** The total size of a memory pool, including alignment. */ #define OS_MEMPOOL_SIZE(n,blksize) (((OS_MEMPOOL_BLOCK_SZ(blksize) + ((OS_ALIGNMENT)-1)) / (OS_ALIGNMENT)) * (n)) /** Calculates the number of bytes required to initialize a memory pool. */ @@ -173,13 +197,14 @@ typedef __uint128_t os_membuf_t; /** * Initialize a memory pool. * - * @param mp Pointer to a pointer to a mempool - * @param blocks The number of blocks in the pool - * @param blocks_size The size of the block, in bytes. - * @param membuf Pointer to memory to contain blocks. - * @param name Name of the pool. + * @param mp Pointer to a pointer to a mempool + * @param blocks The number of blocks in the pool + * @param block_size The size of the block, in bytes. + * @param membuf Pointer to memory to contain blocks. + * @param name Name of the pool. * - * @return os_error_t + * @return 0 on success; + * Non-zero error code on failure. */ os_error_t os_mempool_init(struct os_mempool *mp, uint16_t blocks, uint32_t block_size, void *membuf, char *name); @@ -189,13 +214,14 @@ os_error_t os_mempool_init(struct os_mempool *mp, uint16_t blocks, * are not specified when this function is called; they are assigned manually * after initialization. * - * @param mpe The extended memory pool to initialize. - * @param blocks The number of blocks in the pool. - * @param block_size The size of each block, in bytes. - * @param membuf Pointer to memory to contain blocks. - * @param name Name of the pool. + * @param mpe The extended memory pool to initialize. + * @param blocks The number of blocks in the pool. + * @param block_size The size of each block, in bytes. + * @param membuf Pointer to memory to contain blocks. + * @param name Name of the pool. * - * @return os_error_t + * @return 0 on success; + * Non-zero error code on failure. */ os_error_t os_mempool_ext_init(struct os_mempool_ext *mpe, uint16_t blocks, uint32_t block_size, void *membuf, char *name); @@ -214,9 +240,10 @@ os_error_t os_mempool_unregister(struct os_mempool *mp); /** * Clears a memory pool. * - * @param mp The mempool to clear. + * @param mp The mempool to clear. * - * @return os_error_t + * @return 0 on success; + * Non-zero error code on failure. */ os_error_t os_mempool_clear(struct os_mempool *mp); @@ -246,9 +273,10 @@ int os_memblock_from(const struct os_mempool *mp, const void *block_addr); /** * Get a memory block from a memory pool * - * @param mp Pointer to the memory pool + * @param mp Pointer to the memory pool * - * @return void* Pointer to block if available; NULL otherwise + * @return Pointer to block if available; + * NULL otherwise */ void *os_memblock_get(struct os_mempool *mp); @@ -257,20 +285,22 @@ void *os_memblock_get(struct os_mempool *mp); * This function should only be called from a put callback to free a block * without causing infinite recursion. * - * @param mp Pointer to memory pool - * @param block_addr Pointer to memory block + * @param mp Pointer to memory pool + * @param block_addr Pointer to memory block * - * @return os_error_t + * @return 0 on success; + * Non-zero error code on failure. */ os_error_t os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr); /** * Puts the memory block back into the pool * - * @param mp Pointer to memory pool - * @param block_addr Pointer to memory block + * @param mp Pointer to memory pool + * @param block_addr Pointer to memory block * - * @return os_error_t + * @return 0 on success; + * Non-zero error code on failure. */ os_error_t os_memblock_put(struct os_mempool *mp, void *block_addr); diff --git a/porting/nimble/include/os/queue.h b/porting/nimble/include/os/queue.h index c19edb113..ed05c619d 100644 --- a/porting/nimble/include/os/queue.h +++ b/porting/nimble/include/os/queue.h @@ -1,4 +1,6 @@ -/* +/*- + * SPDX-License-Identifier: BSD-3-Clause + * * Copyright (c) 1991, 1993 * The Regents of the University of California. All rights reserved. * @@ -10,7 +12,7 @@ * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. - * 4. Neither the name of the University nor the names of its contributors + * 3. Neither the name of the University nor the names of its contributors * may be used to endorse or promote products derived from this software * without specific prior written permission. * @@ -27,19 +29,16 @@ * SUCH DAMAGE. * * @(#)queue.h 8.5 (Berkeley) 8/20/94 - * $FreeBSD: src/sys/sys/queue.h,v 1.32.2.7 2002/04/17 14:21:02 des Exp $ */ -#ifndef _QUEUE_H_ -#define _QUEUE_H_ +#ifndef _SYS_QUEUE_H_ +#define _SYS_QUEUE_H_ -#ifdef __cplusplus -extern "C" { -#endif +#include <os/util.h> /* - * This file defines five types of data structures: singly-linked lists, - * singly-linked tail queues, lists, tail queues, and circular queues. + * This file defines four types of data structures: singly-linked lists, + * singly-linked tail queues, lists and tail queues. * * A singly-linked list is headed by a single forward pointer. The elements * are singly linked for minimum space and pointer manipulation overhead at @@ -67,7 +66,7 @@ extern "C" { * so that an arbitrary element can be removed without a need to * traverse the list. New elements can be added to the list before * or after an existing element or at the head of the list. A list - * may only be traversed in the forward direction. + * may be traversed in either direction. * * A tail queue is headed by a pair of pointers, one to the head of the * list and the other to the tail of the list. The elements are doubly @@ -76,99 +75,246 @@ extern "C" { * after an existing element, at the head of the list, or at the end of * the list. A tail queue may be traversed in either direction. * - * A circle queue is headed by a pair of pointers, one to the head of the - * list and the other to the tail of the list. The elements are doubly - * linked so that an arbitrary element can be removed without a need to - * traverse the list. New elements can be added to the list before or after - * an existing element, at the head of the list, or at the end of the list. - * A circle queue may be traversed in either direction, but has a more - * complex end of list detection. - * * For details on the use of these macros, see the queue(3) manual page. * + * Below is a summary of implemented functions where: + * + means the macro is available + * - means the macro is not available + * s means the macro is available but is slow (runs in O(n) time) * - * SLIST LIST STAILQ TAILQ CIRCLEQ - * _HEAD + + + + + - * _HEAD_INITIALIZER + + + + + - * _ENTRY + + + + + - * _INIT + + + + + - * _EMPTY + + + + + - * _FIRST + + + + + - * _NEXT + + + + + - * _PREV - - - + + - * _LAST - - + + + - * _FOREACH + + + + + - * _FOREACH_REVERSE - - - + + - * _INSERT_HEAD + + + + + - * _INSERT_BEFORE - + - + + - * _INSERT_AFTER + + + + + - * _INSERT_TAIL - - + + + - * _REMOVE_HEAD + - + - - - * _REMOVE + + + + + + * SLIST LIST STAILQ TAILQ + * _HEAD + + + + + * _CLASS_HEAD + + + + + * _HEAD_INITIALIZER + + + + + * _ENTRY + + + + + * _CLASS_ENTRY + + + + + * _INIT + + + + + * _EMPTY + + + + + * _END + + + + + * _FIRST + + + + + * _NEXT + + + + + * _PREV - + - + + * _LAST - - + + + * _LAST_FAST - - - + + * _FOREACH + + + + + * _FOREACH_FROM + + + + + * _FOREACH_SAFE + + + + + * _FOREACH_FROM_SAFE + + + + + * _FOREACH_REVERSE - - - + + * _FOREACH_REVERSE_FROM - - - + + * _FOREACH_REVERSE_SAFE - - - + + * _FOREACH_REVERSE_FROM_SAFE - - - + + * _INSERT_HEAD + + + + + * _INSERT_BEFORE - + - + + * _INSERT_AFTER + + + + + * _INSERT_TAIL - - + + + * _CONCAT s s + + + * _REMOVE_AFTER + - + - + * _REMOVE_HEAD + + + + + * _REMOVE s + s + + * _REPLACE - + - + + * _SWAP + + + + * */ +#ifndef __containerof +#define __containerof CONTAINER_OF +#endif + +#ifdef QUEUE_MACRO_DEBUG +#warn Use QUEUE_MACRO_DEBUG_TRACE and/or QUEUE_MACRO_DEBUG_TRASH +#define QUEUE_MACRO_DEBUG_TRACE +#define QUEUE_MACRO_DEBUG_TRASH +#endif + +#ifdef QUEUE_MACRO_DEBUG_TRACE +/* Store the last 2 places the queue element or head was altered */ +struct qm_trace { + unsigned long lastline; + unsigned long prevline; + const char *lastfile; + const char *prevfile; +}; + +#define TRACEBUF struct qm_trace trace; +#define TRACEBUF_INITIALIZER { __LINE__, 0, __FILE__, NULL } , + +#define QMD_TRACE_HEAD(head) do { \ + (head)->trace.prevline = (head)->trace.lastline; \ + (head)->trace.prevfile = (head)->trace.lastfile; \ + (head)->trace.lastline = __LINE__; \ + (head)->trace.lastfile = __FILE__; \ +} while (0) + +#define QMD_TRACE_ELEM(elem) do { \ + (elem)->trace.prevline = (elem)->trace.lastline; \ + (elem)->trace.prevfile = (elem)->trace.lastfile; \ + (elem)->trace.lastline = __LINE__; \ + (elem)->trace.lastfile = __FILE__; \ +} while (0) + +#else /* !QUEUE_MACRO_DEBUG_TRACE */ +#define QMD_TRACE_ELEM(elem) +#define QMD_TRACE_HEAD(head) +#define TRACEBUF +#define TRACEBUF_INITIALIZER +#endif /* QUEUE_MACRO_DEBUG_TRACE */ + +#ifdef QUEUE_MACRO_DEBUG_TRASH +#define QMD_SAVELINK(name, link) void **name = (void *)&(link) +#define TRASHIT(x) do {(x) = (void *)-1;} while (0) +#define QMD_IS_TRASHED(x) ((x) == (void *)(intptr_t)-1) +#else /* !QUEUE_MACRO_DEBUG_TRASH */ +#define QMD_SAVELINK(name, link) +#define TRASHIT(x) +#define QMD_IS_TRASHED(x) 0 +#endif /* QUEUE_MACRO_DEBUG_TRASH */ + +#ifdef __cplusplus +/* + * In C++ there can be structure lists and class lists: + */ +#define QUEUE_TYPEOF(type) type +#else +#define QUEUE_TYPEOF(type) struct type +#endif + /* * Singly-linked List declarations. */ -#define SLIST_HEAD(name, type) \ -struct name { \ - struct type *slh_first; /* first element */ \ +#define SLIST_HEAD(name, type) \ +struct name { \ + struct type *slh_first; /* first element */ \ +} + +#define SLIST_CLASS_HEAD(name, type) \ +struct name { \ + class type *slh_first; /* first element */ \ } -#define SLIST_HEAD_INITIALIZER(head) \ - { NULL } +#define SLIST_HEAD_INITIALIZER(head) \ + { NULL } -#define SLIST_ENTRY(type) \ -struct { \ - struct type *sle_next; /* next element */ \ +#define SLIST_ENTRY(type) \ +struct { \ + struct type *sle_next; /* next element */ \ +} + +#define SLIST_CLASS_ENTRY(type) \ +struct { \ + class type *sle_next; /* next element */ \ } /* * Singly-linked List functions. */ -#define SLIST_EMPTY(head) ((head)->slh_first == NULL) +#if (defined(_KERNEL) && defined(INVARIANTS)) +#define QMD_SLIST_CHECK_PREVPTR(prevp, elm) do { \ + if (*(prevp) != (elm)) \ + panic("Bad prevptr *(%p) == %p != %p", \ + (prevp), *(prevp), (elm)); \ +} while (0) +#else +#define QMD_SLIST_CHECK_PREVPTR(prevp, elm) +#endif -#define SLIST_FIRST(head) ((head)->slh_first) +#define SLIST_CONCAT(head1, head2, type, field) do { \ + QUEUE_TYPEOF(type) *curelm = SLIST_FIRST(head1); \ + if (curelm == NULL) { \ + if ((SLIST_FIRST(head1) = SLIST_FIRST(head2)) != NULL) \ + SLIST_INIT(head2); \ + } else if (SLIST_FIRST(head2) != NULL) { \ + while (SLIST_NEXT(curelm, field) != NULL) \ + curelm = SLIST_NEXT(curelm, field); \ + SLIST_NEXT(curelm, field) = SLIST_FIRST(head2); \ + SLIST_INIT(head2); \ + } \ +} while (0) + +#define SLIST_EMPTY(head) ((head)->slh_first == NULL) -#define SLIST_FOREACH(var, head, field) \ - for ((var) = SLIST_FIRST((head)); \ - (var); \ - (var) = SLIST_NEXT((var), field)) +#define SLIST_FIRST(head) ((head)->slh_first) -#define SLIST_INIT(head) do { \ - SLIST_FIRST((head)) = NULL; \ +#define SLIST_FOREACH(var, head, field) \ + for ((var) = SLIST_FIRST((head)); \ + (var); \ + (var) = SLIST_NEXT((var), field)) + +#define SLIST_FOREACH_FROM(var, head, field) \ + for ((var) = ((var) ? (var) : SLIST_FIRST((head))); \ + (var); \ + (var) = SLIST_NEXT((var), field)) + +#define SLIST_FOREACH_SAFE(var, head, field, tvar) \ + for ((var) = SLIST_FIRST((head)); \ + (var) && ((tvar) = SLIST_NEXT((var), field), 1); \ + (var) = (tvar)) + +#define SLIST_FOREACH_FROM_SAFE(var, head, field, tvar) \ + for ((var) = ((var) ? (var) : SLIST_FIRST((head))); \ + (var) && ((tvar) = SLIST_NEXT((var), field), 1); \ + (var) = (tvar)) + +#define SLIST_FOREACH_PREVPTR(var, varp, head, field) \ + for ((varp) = &SLIST_FIRST((head)); \ + ((var) = *(varp)) != NULL; \ + (varp) = &SLIST_NEXT((var), field)) + +#define SLIST_INIT(head) do { \ + SLIST_FIRST((head)) = NULL; \ +} while (0) + +#define SLIST_INSERT_AFTER(slistelm, elm, field) do { \ + SLIST_NEXT((elm), field) = SLIST_NEXT((slistelm), field); \ + SLIST_NEXT((slistelm), field) = (elm); \ } while (0) -#define SLIST_INSERT_AFTER(slistelm, elm, field) do { \ - SLIST_NEXT((elm), field) = SLIST_NEXT((slistelm), field); \ - SLIST_NEXT((slistelm), field) = (elm); \ +#define SLIST_INSERT_HEAD(head, elm, field) do { \ + SLIST_NEXT((elm), field) = SLIST_FIRST((head)); \ + SLIST_FIRST((head)) = (elm); \ +} while (0) + +#define SLIST_NEXT(elm, field) ((elm)->field.sle_next) + +#define SLIST_REMOVE(head, elm, type, field) do { \ + if (SLIST_FIRST((head)) == (elm)) { \ + SLIST_REMOVE_HEAD((head), field); \ + } \ + else { \ + QUEUE_TYPEOF(type) *curelm = SLIST_FIRST(head); \ + while (SLIST_NEXT(curelm, field) != (elm)) \ + curelm = SLIST_NEXT(curelm, field); \ + SLIST_REMOVE_AFTER(curelm, field); \ + } \ } while (0) -#define SLIST_INSERT_HEAD(head, elm, field) do { \ - SLIST_NEXT((elm), field) = SLIST_FIRST((head)); \ - SLIST_FIRST((head)) = (elm); \ +#define SLIST_REMOVE_AFTER(elm, field) do { \ + QMD_SAVELINK(oldnext, SLIST_NEXT(elm, field)->field.sle_next); \ + SLIST_NEXT(elm, field) = \ + SLIST_NEXT(SLIST_NEXT(elm, field), field); \ + TRASHIT(*oldnext); \ } while (0) -#define SLIST_NEXT(elm, field) ((elm)->field.sle_next) +#define SLIST_REMOVE_HEAD(head, field) do { \ + QMD_SAVELINK(oldnext, SLIST_FIRST(head)->field.sle_next); \ + SLIST_FIRST((head)) = SLIST_NEXT(SLIST_FIRST((head)), field); \ + TRASHIT(*oldnext); \ +} while (0) -#define SLIST_REMOVE(head, elm, type, field) do { \ - if (SLIST_FIRST((head)) == (elm)) { \ - SLIST_REMOVE_HEAD((head), field); \ - } \ - else { \ - struct type *curelm = SLIST_FIRST((head)); \ - while (SLIST_NEXT(curelm, field) != (elm)) \ - curelm = SLIST_NEXT(curelm, field); \ - SLIST_NEXT(curelm, field) = \ - SLIST_NEXT(SLIST_NEXT(curelm, field), field); \ - } \ +#define SLIST_REMOVE_PREVPTR(prevp, elm, field) do { \ + QMD_SLIST_CHECK_PREVPTR(prevp, elm); \ + *(prevp) = SLIST_NEXT(elm, field); \ + TRASHIT((elm)->field.sle_next); \ } while (0) -#define SLIST_REMOVE_HEAD(head, field) do { \ - SLIST_FIRST((head)) = SLIST_NEXT(SLIST_FIRST((head)), field); \ +#define SLIST_SWAP(head1, head2, type) do { \ + QUEUE_TYPEOF(type) *swap_first = SLIST_FIRST(head1); \ + SLIST_FIRST(head1) = SLIST_FIRST(head2); \ + SLIST_FIRST(head2) = swap_first; \ } while (0) +#define SLIST_END(head) NULL + /* * Singly-linked Tail queue declarations. */ @@ -178,6 +324,12 @@ struct name { \ struct type **stqh_last;/* addr of last next element */ \ } +#define STAILQ_CLASS_HEAD(name, type) \ +struct name { \ + class type *stqh_first; /* first element */ \ + class type **stqh_last; /* addr of last next element */ \ +} + #define STAILQ_HEAD_INITIALIZER(head) \ { NULL, &(head).stqh_first } @@ -186,9 +338,22 @@ struct { \ struct type *stqe_next; /* next element */ \ } +#define STAILQ_CLASS_ENTRY(type) \ +struct { \ + class type *stqe_next; /* next element */ \ +} + /* * Singly-linked Tail queue functions. */ +#define STAILQ_CONCAT(head1, head2) do { \ + if (!STAILQ_EMPTY((head2))) { \ + *(head1)->stqh_last = (head2)->stqh_first; \ + (head1)->stqh_last = (head2)->stqh_last; \ + STAILQ_INIT((head2)); \ + } \ +} while (0) + #define STAILQ_EMPTY(head) ((head)->stqh_first == NULL) #define STAILQ_FIRST(head) ((head)->stqh_first) @@ -198,6 +363,21 @@ struct { \ (var); \ (var) = STAILQ_NEXT((var), field)) +#define STAILQ_FOREACH_FROM(var, head, field) \ + for ((var) = ((var) ? (var) : STAILQ_FIRST((head))); \ + (var); \ + (var) = STAILQ_NEXT((var), field)) + +#define STAILQ_FOREACH_SAFE(var, head, field, tvar) \ + for ((var) = STAILQ_FIRST((head)); \ + (var) && ((tvar) = STAILQ_NEXT((var), field), 1); \ + (var) = (tvar)) + +#define STAILQ_FOREACH_FROM_SAFE(var, head, field, tvar) \ + for ((var) = ((var) ? (var) : STAILQ_FIRST((head))); \ + (var) && ((tvar) = STAILQ_NEXT((var), field), 1); \ + (var) = (tvar)) + #define STAILQ_INIT(head) do { \ STAILQ_FIRST((head)) = NULL; \ (head)->stqh_last = &STAILQ_FIRST((head)); \ @@ -222,25 +402,30 @@ struct { \ } while (0) #define STAILQ_LAST(head, type, field) \ - (STAILQ_EMPTY(head) ? \ - NULL : \ - ((struct type *) \ - ((char *)((head)->stqh_last) - offsetof(struct type, field)))) + (STAILQ_EMPTY((head)) ? NULL : \ + __containerof((head)->stqh_last, \ + QUEUE_TYPEOF(type), field.stqe_next)) #define STAILQ_NEXT(elm, field) ((elm)->field.stqe_next) #define STAILQ_REMOVE(head, elm, type, field) do { \ + QMD_SAVELINK(oldnext, (elm)->field.stqe_next); \ if (STAILQ_FIRST((head)) == (elm)) { \ - STAILQ_REMOVE_HEAD(head, field); \ + STAILQ_REMOVE_HEAD((head), field); \ } \ else { \ - struct type *curelm = STAILQ_FIRST((head)); \ + QUEUE_TYPEOF(type) *curelm = STAILQ_FIRST(head); \ while (STAILQ_NEXT(curelm, field) != (elm)) \ curelm = STAILQ_NEXT(curelm, field); \ - if ((STAILQ_NEXT(curelm, field) = \ - STAILQ_NEXT(STAILQ_NEXT(curelm, field), field)) == NULL)\ - (head)->stqh_last = &STAILQ_NEXT((curelm), field);\ + STAILQ_REMOVE_AFTER(head, curelm, field); \ } \ + TRASHIT(*oldnext); \ +} while (0) + +#define STAILQ_REMOVE_AFTER(head, elm, field) do { \ + if ((STAILQ_NEXT(elm, field) = \ + STAILQ_NEXT(STAILQ_NEXT(elm, field), field)) == NULL) \ + (head)->stqh_last = &STAILQ_NEXT((elm), field); \ } while (0) #define STAILQ_REMOVE_HEAD(head, field) do { \ @@ -249,16 +434,21 @@ struct { \ (head)->stqh_last = &STAILQ_FIRST((head)); \ } while (0) -#define STAILQ_REMOVE_HEAD_UNTIL(head, elm, field) do { \ - if ((STAILQ_FIRST((head)) = STAILQ_NEXT((elm), field)) == NULL) \ - (head)->stqh_last = &STAILQ_FIRST((head)); \ +#define STAILQ_SWAP(head1, head2, type) do { \ + QUEUE_TYPEOF(type) *swap_first = STAILQ_FIRST(head1); \ + QUEUE_TYPEOF(type) **swap_last = (head1)->stqh_last; \ + STAILQ_FIRST(head1) = STAILQ_FIRST(head2); \ + (head1)->stqh_last = (head2)->stqh_last; \ + STAILQ_FIRST(head2) = swap_first; \ + (head2)->stqh_last = swap_last; \ + if (STAILQ_EMPTY(head1)) \ + (head1)->stqh_last = &STAILQ_FIRST(head1); \ + if (STAILQ_EMPTY(head2)) \ + (head2)->stqh_last = &STAILQ_FIRST(head2); \ } while (0) -#define STAILQ_REMOVE_AFTER(head, elm, field) do { \ - if ((STAILQ_NEXT(elm, field) = \ - STAILQ_NEXT(STAILQ_NEXT(elm, field), field)) == NULL) \ - (head)->stqh_last = &STAILQ_NEXT((elm), field); \ -} while (0) +#define STAILQ_END(head) NULL + /* * List declarations. @@ -268,6 +458,11 @@ struct name { \ struct type *lh_first; /* first element */ \ } +#define LIST_CLASS_HEAD(name, type) \ +struct name { \ + class type *lh_first; /* first element */ \ +} + #define LIST_HEAD_INITIALIZER(head) \ { NULL } @@ -277,10 +472,75 @@ struct { \ struct type **le_prev; /* address of previous next element */ \ } +#define LIST_CLASS_ENTRY(type) \ +struct { \ + class type *le_next; /* next element */ \ + class type **le_prev; /* address of previous next element */ \ +} + /* * List functions. */ +#if (defined(_KERNEL) && defined(INVARIANTS)) +/* + * QMD_LIST_CHECK_HEAD(LIST_HEAD *head, LIST_ENTRY NAME) + * + * If the list is non-empty, validates that the first element of the list + * points back at 'head.' + */ +#define QMD_LIST_CHECK_HEAD(head, field) do { \ + if (LIST_FIRST((head)) != NULL && \ + LIST_FIRST((head))->field.le_prev != \ + &LIST_FIRST((head))) \ + panic("Bad list head %p first->prev != head", (head)); \ +} while (0) + +/* + * QMD_LIST_CHECK_NEXT(TYPE *elm, LIST_ENTRY NAME) + * + * If an element follows 'elm' in the list, validates that the next element + * points back at 'elm.' + */ +#define QMD_LIST_CHECK_NEXT(elm, field) do { \ + if (LIST_NEXT((elm), field) != NULL && \ + LIST_NEXT((elm), field)->field.le_prev != \ + &((elm)->field.le_next)) \ + panic("Bad link elm %p next->prev != elm", (elm)); \ +} while (0) + +/* + * QMD_LIST_CHECK_PREV(TYPE *elm, LIST_ENTRY NAME) + * + * Validates that the previous element (or head of the list) points to 'elm.' + */ +#define QMD_LIST_CHECK_PREV(elm, field) do { \ + if (*(elm)->field.le_prev != (elm)) \ + panic("Bad link elm %p prev->next != elm", (elm)); \ +} while (0) +#else +#define QMD_LIST_CHECK_HEAD(head, field) +#define QMD_LIST_CHECK_NEXT(elm, field) +#define QMD_LIST_CHECK_PREV(elm, field) +#endif /* (_KERNEL && INVARIANTS) */ + +#define LIST_CONCAT(head1, head2, type, field) do { \ + QUEUE_TYPEOF(type) *curelm = LIST_FIRST(head1); \ + if (curelm == NULL) { \ + if ((LIST_FIRST(head1) = LIST_FIRST(head2)) != NULL) { \ + LIST_FIRST(head2)->field.le_prev = \ + &LIST_FIRST((head1)); \ + LIST_INIT(head2); \ + } \ + } else if (LIST_FIRST(head2) != NULL) { \ + while (LIST_NEXT(curelm, field) != NULL) \ + curelm = LIST_NEXT(curelm, field); \ + LIST_NEXT(curelm, field) = LIST_FIRST(head2); \ + LIST_FIRST(head2)->field.le_prev = &LIST_NEXT(curelm, field);\ + LIST_INIT(head2); \ + } \ +} while (0) + #define LIST_EMPTY(head) ((head)->lh_first == NULL) #define LIST_FIRST(head) ((head)->lh_first) @@ -290,11 +550,27 @@ struct { \ (var); \ (var) = LIST_NEXT((var), field)) +#define LIST_FOREACH_FROM(var, head, field) \ + for ((var) = ((var) ? (var) : LIST_FIRST((head))); \ + (var); \ + (var) = LIST_NEXT((var), field)) + +#define LIST_FOREACH_SAFE(var, head, field, tvar) \ + for ((var) = LIST_FIRST((head)); \ + (var) && ((tvar) = LIST_NEXT((var), field), 1); \ + (var) = (tvar)) + +#define LIST_FOREACH_FROM_SAFE(var, head, field, tvar) \ + for ((var) = ((var) ? (var) : LIST_FIRST((head))); \ + (var) && ((tvar) = LIST_NEXT((var), field), 1); \ + (var) = (tvar)) + #define LIST_INIT(head) do { \ LIST_FIRST((head)) = NULL; \ } while (0) #define LIST_INSERT_AFTER(listelm, elm, field) do { \ + QMD_LIST_CHECK_NEXT(listelm, field); \ if ((LIST_NEXT((elm), field) = LIST_NEXT((listelm), field)) != NULL)\ LIST_NEXT((listelm), field)->field.le_prev = \ &LIST_NEXT((elm), field); \ @@ -303,6 +579,7 @@ struct { \ } while (0) #define LIST_INSERT_BEFORE(listelm, elm, field) do { \ + QMD_LIST_CHECK_PREV(listelm, field); \ (elm)->field.le_prev = (listelm)->field.le_prev; \ LIST_NEXT((elm), field) = (listelm); \ *(listelm)->field.le_prev = (elm); \ @@ -310,6 +587,7 @@ struct { \ } while (0) #define LIST_INSERT_HEAD(head, elm, field) do { \ + QMD_LIST_CHECK_HEAD((head), field); \ if ((LIST_NEXT((elm), field) = LIST_FIRST((head))) != NULL) \ LIST_FIRST((head))->field.le_prev = &LIST_NEXT((elm), field);\ LIST_FIRST((head)) = (elm); \ @@ -318,13 +596,54 @@ struct { \ #define LIST_NEXT(elm, field) ((elm)->field.le_next) +#define LIST_PREV(elm, head, type, field) \ + ((elm)->field.le_prev == &LIST_FIRST((head)) ? NULL : \ + __containerof((elm)->field.le_prev, \ + QUEUE_TYPEOF(type), field.le_next)) + +#define LIST_REMOVE_HEAD(head, field) \ + LIST_REMOVE(LIST_FIRST(head), field) + #define LIST_REMOVE(elm, field) do { \ + QMD_SAVELINK(oldnext, (elm)->field.le_next); \ + QMD_SAVELINK(oldprev, (elm)->field.le_prev); \ + QMD_LIST_CHECK_NEXT(elm, field); \ + QMD_LIST_CHECK_PREV(elm, field); \ if (LIST_NEXT((elm), field) != NULL) \ - LIST_NEXT((elm), field)->field.le_prev = \ + LIST_NEXT((elm), field)->field.le_prev = \ (elm)->field.le_prev; \ *(elm)->field.le_prev = LIST_NEXT((elm), field); \ + TRASHIT(*oldnext); \ + TRASHIT(*oldprev); \ } while (0) +#define LIST_REPLACE(elm, elm2, field) do { \ + QMD_SAVELINK(oldnext, (elm)->field.le_next); \ + QMD_SAVELINK(oldprev, (elm)->field.le_prev); \ + QMD_LIST_CHECK_NEXT(elm, field); \ + QMD_LIST_CHECK_PREV(elm, field); \ + LIST_NEXT((elm2), field) = LIST_NEXT((elm), field); \ + if (LIST_NEXT((elm2), field) != NULL) \ + LIST_NEXT((elm2), field)->field.le_prev = \ + &(elm2)->field.le_next; \ + (elm2)->field.le_prev = (elm)->field.le_prev; \ + *(elm2)->field.le_prev = (elm2); \ + TRASHIT(*oldnext); \ + TRASHIT(*oldprev); \ +} while (0) + +#define LIST_SWAP(head1, head2, type, field) do { \ + QUEUE_TYPEOF(type) *swap_tmp = LIST_FIRST(head1); \ + LIST_FIRST((head1)) = LIST_FIRST((head2)); \ + LIST_FIRST((head2)) = swap_tmp; \ + if ((swap_tmp = LIST_FIRST((head1))) != NULL) \ + swap_tmp->field.le_prev = &LIST_FIRST((head1)); \ + if ((swap_tmp = LIST_FIRST((head2))) != NULL) \ + swap_tmp->field.le_prev = &LIST_FIRST((head2)); \ +} while (0) + +#define LIST_END(head) NULL + /* * Tail queue declarations. */ @@ -332,20 +651,100 @@ struct { \ struct name { \ struct type *tqh_first; /* first element */ \ struct type **tqh_last; /* addr of last next element */ \ + TRACEBUF \ +} + +#define TAILQ_CLASS_HEAD(name, type) \ +struct name { \ + class type *tqh_first; /* first element */ \ + class type **tqh_last; /* addr of last next element */ \ + TRACEBUF \ } #define TAILQ_HEAD_INITIALIZER(head) \ - { NULL, &(head).tqh_first } + { NULL, &(head).tqh_first, TRACEBUF_INITIALIZER } #define TAILQ_ENTRY(type) \ struct { \ struct type *tqe_next; /* next element */ \ struct type **tqe_prev; /* address of previous next element */ \ + TRACEBUF \ +} + +#define TAILQ_CLASS_ENTRY(type) \ +struct { \ + class type *tqe_next; /* next element */ \ + class type **tqe_prev; /* address of previous next element */ \ + TRACEBUF \ } /* * Tail queue functions. */ +#if (defined(_KERNEL) && defined(INVARIANTS)) +/* + * QMD_TAILQ_CHECK_HEAD(TAILQ_HEAD *head, TAILQ_ENTRY NAME) + * + * If the tailq is non-empty, validates that the first element of the tailq + * points back at 'head.' + */ +#define QMD_TAILQ_CHECK_HEAD(head, field) do { \ + if (!TAILQ_EMPTY(head) && \ + TAILQ_FIRST((head))->field.tqe_prev != \ + &TAILQ_FIRST((head))) \ + panic("Bad tailq head %p first->prev != head", (head)); \ +} while (0) + +/* + * QMD_TAILQ_CHECK_TAIL(TAILQ_HEAD *head, TAILQ_ENTRY NAME) + * + * Validates that the tail of the tailq is a pointer to pointer to NULL. + */ +#define QMD_TAILQ_CHECK_TAIL(head, field) do { \ + if (*(head)->tqh_last != NULL) \ + panic("Bad tailq NEXT(%p->tqh_last) != NULL", (head)); \ +} while (0) + +/* + * QMD_TAILQ_CHECK_NEXT(TYPE *elm, TAILQ_ENTRY NAME) + * + * If an element follows 'elm' in the tailq, validates that the next element + * points back at 'elm.' + */ +#define QMD_TAILQ_CHECK_NEXT(elm, field) do { \ + if (TAILQ_NEXT((elm), field) != NULL && \ + TAILQ_NEXT((elm), field)->field.tqe_prev != \ + &((elm)->field.tqe_next)) \ + panic("Bad link elm %p next->prev != elm", (elm)); \ +} while (0) + +/* + * QMD_TAILQ_CHECK_PREV(TYPE *elm, TAILQ_ENTRY NAME) + * + * Validates that the previous element (or head of the tailq) points to 'elm.' + */ +#define QMD_TAILQ_CHECK_PREV(elm, field) do { \ + if (*(elm)->field.tqe_prev != (elm)) \ + panic("Bad link elm %p prev->next != elm", (elm)); \ +} while (0) +#else +#define QMD_TAILQ_CHECK_HEAD(head, field) +#define QMD_TAILQ_CHECK_TAIL(head, headname) +#define QMD_TAILQ_CHECK_NEXT(elm, field) +#define QMD_TAILQ_CHECK_PREV(elm, field) +#endif /* (_KERNEL && INVARIANTS) */ + +#define TAILQ_CONCAT(head1, head2, field) do { \ + if (!TAILQ_EMPTY(head2)) { \ + *(head1)->tqh_last = (head2)->tqh_first; \ + (head2)->tqh_first->field.tqe_prev = (head1)->tqh_last; \ + (head1)->tqh_last = (head2)->tqh_last; \ + TAILQ_INIT((head2)); \ + QMD_TRACE_HEAD(head1); \ + QMD_TRACE_HEAD(head2); \ + } \ +} while (0) + #define TAILQ_EMPTY(head) ((head)->tqh_first == NULL) #define TAILQ_FIRST(head) ((head)->tqh_first) @@ -355,34 +754,74 @@ struct { \ (var); \ (var) = TAILQ_NEXT((var), field)) +#define TAILQ_FOREACH_FROM(var, head, field) \ + for ((var) = ((var) ? (var) : TAILQ_FIRST((head))); \ + (var); \ + (var) = TAILQ_NEXT((var), field)) + +#define TAILQ_FOREACH_SAFE(var, head, field, tvar) \ + for ((var) = TAILQ_FIRST((head)); \ + (var) && ((tvar) = TAILQ_NEXT((var), field), 1); \ + (var) = (tvar)) + +#define TAILQ_FOREACH_FROM_SAFE(var, head, field, tvar) \ + for ((var) = ((var) ? (var) : TAILQ_FIRST((head))); \ + (var) && ((tvar) = TAILQ_NEXT((var), field), 1); \ + (var) = (tvar)) + #define TAILQ_FOREACH_REVERSE(var, head, headname, field) \ for ((var) = TAILQ_LAST((head), headname); \ (var); \ (var) = TAILQ_PREV((var), headname, field)) +#define TAILQ_FOREACH_REVERSE_FROM(var, head, headname, field) \ + for ((var) = ((var) ? (var) : TAILQ_LAST((head), headname)); \ + (var); \ + (var) = TAILQ_PREV((var), headname, field)) + +#define TAILQ_FOREACH_REVERSE_SAFE(var, head, headname, field, tvar) \ + for ((var) = TAILQ_LAST((head), headname); \ + (var) && ((tvar) = TAILQ_PREV((var), headname, field), 1); \ + (var) = (tvar)) + +#define TAILQ_FOREACH_REVERSE_FROM_SAFE(var, head, headname, field, tvar)\ + for ((var) = ((var) ? (var) : TAILQ_LAST((head), headname)); \ + (var) && ((tvar) = TAILQ_PREV((var), headname, field), 1); \ + (var) = (tvar)) + #define TAILQ_INIT(head) do { \ TAILQ_FIRST((head)) = NULL; \ (head)->tqh_last = &TAILQ_FIRST((head)); \ + QMD_TRACE_HEAD(head); \ } while (0) #define TAILQ_INSERT_AFTER(head, listelm, elm, field) do { \ + QMD_TAILQ_CHECK_NEXT(listelm, field); \ if ((TAILQ_NEXT((elm), field) = TAILQ_NEXT((listelm), field)) != NULL)\ - TAILQ_NEXT((elm), field)->field.tqe_prev = \ + TAILQ_NEXT((elm), field)->field.tqe_prev = \ &TAILQ_NEXT((elm), field); \ - else \ + else { \ (head)->tqh_last = &TAILQ_NEXT((elm), field); \ + QMD_TRACE_HEAD(head); \ + } \ TAILQ_NEXT((listelm), field) = (elm); \ (elm)->field.tqe_prev = &TAILQ_NEXT((listelm), field); \ + QMD_TRACE_ELEM(&(elm)->field); \ + QMD_TRACE_ELEM(&(listelm)->field); \ } while (0) #define TAILQ_INSERT_BEFORE(listelm, elm, field) do { \ + QMD_TAILQ_CHECK_PREV(listelm, field); \ (elm)->field.tqe_prev = (listelm)->field.tqe_prev; \ TAILQ_NEXT((elm), field) = (listelm); \ *(listelm)->field.tqe_prev = (elm); \ (listelm)->field.tqe_prev = &TAILQ_NEXT((elm), field); \ + QMD_TRACE_ELEM(&(elm)->field); \ + QMD_TRACE_ELEM(&(listelm)->field); \ } while (0) #define TAILQ_INSERT_HEAD(head, elm, field) do { \ + QMD_TAILQ_CHECK_HEAD(head, field); \ if ((TAILQ_NEXT((elm), field) = TAILQ_FIRST((head))) != NULL) \ TAILQ_FIRST((head))->field.tqe_prev = \ &TAILQ_NEXT((elm), field); \ @@ -390,133 +829,98 @@ struct { \ (head)->tqh_last = &TAILQ_NEXT((elm), field); \ TAILQ_FIRST((head)) = (elm); \ (elm)->field.tqe_prev = &TAILQ_FIRST((head)); \ + QMD_TRACE_HEAD(head); \ + QMD_TRACE_ELEM(&(elm)->field); \ } while (0) #define TAILQ_INSERT_TAIL(head, elm, field) do { \ + QMD_TAILQ_CHECK_TAIL(head, field); \ TAILQ_NEXT((elm), field) = NULL; \ (elm)->field.tqe_prev = (head)->tqh_last; \ *(head)->tqh_last = (elm); \ (head)->tqh_last = &TAILQ_NEXT((elm), field); \ + QMD_TRACE_HEAD(head); \ + QMD_TRACE_ELEM(&(elm)->field); \ } while (0) #define TAILQ_LAST(head, headname) \ (*(((struct headname *)((head)->tqh_last))->tqh_last)) +/* + * The FAST function is fast in that it causes no data access other + * then the access to the head. The standard LAST function above + * will cause a data access of both the element you want and + * the previous element. FAST is very useful for instances when + * you may want to prefetch the last data element. + */ +#define TAILQ_LAST_FAST(head, type, field) \ + (TAILQ_EMPTY(head) ? NULL : __containerof((head)->tqh_last, QUEUE_TYPEOF(type), field.tqe_next)) + #define TAILQ_NEXT(elm, field) ((elm)->field.tqe_next) #define TAILQ_PREV(elm, headname, field) \ (*(((struct headname *)((elm)->field.tqe_prev))->tqh_last)) +#define TAILQ_PREV_FAST(elm, head, type, field) \ + ((elm)->field.tqe_prev == &(head)->tqh_first ? NULL : \ + __containerof((elm)->field.tqe_prev, QUEUE_TYPEOF(type), field.tqe_next)) + +#define TAILQ_REMOVE_HEAD(head, field) \ + TAILQ_REMOVE(head, TAILQ_FIRST(head), field) + #define TAILQ_REMOVE(head, elm, field) do { \ + QMD_SAVELINK(oldnext, (elm)->field.tqe_next); \ + QMD_SAVELINK(oldprev, (elm)->field.tqe_prev); \ + QMD_TAILQ_CHECK_NEXT(elm, field); \ + QMD_TAILQ_CHECK_PREV(elm, field); \ if ((TAILQ_NEXT((elm), field)) != NULL) \ - TAILQ_NEXT((elm), field)->field.tqe_prev = \ + TAILQ_NEXT((elm), field)->field.tqe_prev = \ (elm)->field.tqe_prev; \ - else \ + else { \ (head)->tqh_last = (elm)->field.tqe_prev; \ + QMD_TRACE_HEAD(head); \ + } \ *(elm)->field.tqe_prev = TAILQ_NEXT((elm), field); \ + TRASHIT(*oldnext); \ + TRASHIT(*oldprev); \ + QMD_TRACE_ELEM(&(elm)->field); \ } while (0) -/* - * Circular queue declarations. - */ -#define CIRCLEQ_HEAD(name, type) \ -struct name { \ - struct type *cqh_first; /* first element */ \ - struct type *cqh_last; /* last element */ \ -} - -#define CIRCLEQ_HEAD_INITIALIZER(head) \ - { (void *)&(head), (void *)&(head) } - -#define CIRCLEQ_ENTRY(type) \ -struct { \ - struct type *cqe_next; /* next element */ \ - struct type *cqe_prev; /* previous element */ \ -} - -/* - * Circular queue functions. - */ -#define CIRCLEQ_EMPTY(head) ((head)->cqh_first == (void *)(head)) - -#define CIRCLEQ_FIRST(head) ((head)->cqh_first) - -#define CIRCLEQ_FOREACH(var, head, field) \ - for ((var) = CIRCLEQ_FIRST((head)); \ - (var) != (void *)(head) || ((var) = NULL); \ - (var) = CIRCLEQ_NEXT((var), field)) - -#define CIRCLEQ_FOREACH_REVERSE(var, head, field) \ - for ((var) = CIRCLEQ_LAST((head)); \ - (var) != (void *)(head) || ((var) = NULL); \ - (var) = CIRCLEQ_PREV((var), field)) - -#define CIRCLEQ_INIT(head) do { \ - CIRCLEQ_FIRST((head)) = (void *)(head); \ - CIRCLEQ_LAST((head)) = (void *)(head); \ -} while (0) - -#define CIRCLEQ_INSERT_AFTER(head, listelm, elm, field) do { \ - CIRCLEQ_NEXT((elm), field) = CIRCLEQ_NEXT((listelm), field); \ - CIRCLEQ_PREV((elm), field) = (listelm); \ - if (CIRCLEQ_NEXT((listelm), field) == (void *)(head)) \ - CIRCLEQ_LAST((head)) = (elm); \ - else \ - CIRCLEQ_PREV(CIRCLEQ_NEXT((listelm), field), field) = (elm);\ - CIRCLEQ_NEXT((listelm), field) = (elm); \ -} while (0) - -#define CIRCLEQ_INSERT_BEFORE(head, listelm, elm, field) do { \ - CIRCLEQ_NEXT((elm), field) = (listelm); \ - CIRCLEQ_PREV((elm), field) = CIRCLEQ_PREV((listelm), field); \ - if (CIRCLEQ_PREV((listelm), field) == (void *)(head)) \ - CIRCLEQ_FIRST((head)) = (elm); \ - else \ - CIRCLEQ_NEXT(CIRCLEQ_PREV((listelm), field), field) = (elm);\ - CIRCLEQ_PREV((listelm), field) = (elm); \ +#define TAILQ_REPLACE(head, elm, elm2, field) do { \ + QMD_SAVELINK(oldnext, (elm)->field.tqe_next); \ + QMD_SAVELINK(oldprev, (elm)->field.tqe_prev); \ + QMD_TAILQ_CHECK_NEXT(elm, field); \ + QMD_TAILQ_CHECK_PREV(elm, field); \ + TAILQ_NEXT((elm2), field) = TAILQ_NEXT((elm), field); \ + if (TAILQ_NEXT((elm2), field) != TAILQ_END(head)) \ + TAILQ_NEXT((elm2), field)->field.tqe_prev = \ + &(elm2)->field.tqe_next; \ + else \ + (head)->tqh_last = &(elm2)->field.tqe_next; \ + (elm2)->field.tqe_prev = (elm)->field.tqe_prev; \ + *(elm2)->field.tqe_prev = (elm2); \ + TRASHIT(*oldnext); \ + TRASHIT(*oldprev); \ + QMD_TRACE_ELEM(&(elm)->field); \ } while (0) -#define CIRCLEQ_INSERT_HEAD(head, elm, field) do { \ - CIRCLEQ_NEXT((elm), field) = CIRCLEQ_FIRST((head)); \ - CIRCLEQ_PREV((elm), field) = (void *)(head); \ - if (CIRCLEQ_LAST((head)) == (void *)(head)) \ - CIRCLEQ_LAST((head)) = (elm); \ +#define TAILQ_SWAP(head1, head2, type, field) do { \ + QUEUE_TYPEOF(type) *swap_first = (head1)->tqh_first; \ + QUEUE_TYPEOF(type) **swap_last = (head1)->tqh_last; \ + (head1)->tqh_first = (head2)->tqh_first; \ + (head1)->tqh_last = (head2)->tqh_last; \ + (head2)->tqh_first = swap_first; \ + (head2)->tqh_last = swap_last; \ + if ((swap_first = (head1)->tqh_first) != NULL) \ + swap_first->field.tqe_prev = &(head1)->tqh_first; \ else \ - CIRCLEQ_PREV(CIRCLEQ_FIRST((head)), field) = (elm); \ - CIRCLEQ_FIRST((head)) = (elm); \ -} while (0) - -#define CIRCLEQ_INSERT_TAIL(head, elm, field) do { \ - CIRCLEQ_NEXT((elm), field) = (void *)(head); \ - CIRCLEQ_PREV((elm), field) = CIRCLEQ_LAST((head)); \ - if (CIRCLEQ_FIRST((head)) == (void *)(head)) \ - CIRCLEQ_FIRST((head)) = (elm); \ - else \ - CIRCLEQ_NEXT(CIRCLEQ_LAST((head)), field) = (elm); \ - CIRCLEQ_LAST((head)) = (elm); \ -} while (0) - -#define CIRCLEQ_LAST(head) ((head)->cqh_last) - -#define CIRCLEQ_NEXT(elm,field) ((elm)->field.cqe_next) - -#define CIRCLEQ_PREV(elm,field) ((elm)->field.cqe_prev) - -#define CIRCLEQ_REMOVE(head, elm, field) do { \ - if (CIRCLEQ_NEXT((elm), field) == (void *)(head)) \ - CIRCLEQ_LAST((head)) = CIRCLEQ_PREV((elm), field); \ - else \ - CIRCLEQ_PREV(CIRCLEQ_NEXT((elm), field), field) = \ - CIRCLEQ_PREV((elm), field); \ - if (CIRCLEQ_PREV((elm), field) == (void *)(head)) \ - CIRCLEQ_FIRST((head)) = CIRCLEQ_NEXT((elm), field); \ + (head1)->tqh_last = &(head1)->tqh_first; \ + if ((swap_first = (head2)->tqh_first) != NULL) \ + swap_first->field.tqe_prev = &(head2)->tqh_first; \ else \ - CIRCLEQ_NEXT(CIRCLEQ_PREV((elm), field), field) = \ - CIRCLEQ_NEXT((elm), field); \ + (head2)->tqh_last = &(head2)->tqh_first; \ } while (0) -#ifdef __cplusplus -} -#endif +#define TAILQ_END(head) NULL #endif /* !_SYS_QUEUE_H_ */ diff --git a/porting/nimble/src/os_mbuf.c b/porting/nimble/src/os_mbuf.c index 6d84be1f4..829d79858 100644 --- a/porting/nimble/src/os_mbuf.c +++ b/porting/nimble/src/os_mbuf.c @@ -49,15 +49,12 @@ #define max(a, b) ((a) > (b) ? (a) : (b)) #endif -/** - * @addtogroup OSKernel - * @{ - * @defgroup OSMqueue Queue of Mbufs - * @{ - */ +#include "syscfg/syscfg.h" +#if !MYNEWT_VAL(OS_SYSVIEW_TRACE_MBUF) +#define OS_TRACE_DISABLE_FILE_API +#endif + -STAILQ_HEAD(, os_mbuf_pool) g_msys_pool_list = - STAILQ_HEAD_INITIALIZER(g_msys_pool_list); int @@ -97,19 +94,19 @@ os_mqueue_get(struct os_mqueue *mq) } int -os_mqueue_put(struct os_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf *m) +os_mqueue_put(struct os_mqueue *mq, struct ble_npl_eventq *evq, struct os_mbuf *om) { struct os_mbuf_pkthdr *mp; os_sr_t sr; int rc; /* Can only place the head of a chained mbuf on the queue. */ - if (!OS_MBUF_IS_PKTHDR(m)) { + if (!OS_MBUF_IS_PKTHDR(om)) { rc = OS_EINVAL; goto err; } - mp = OS_MBUF_PKTHDR(m); + mp = OS_MBUF_PKTHDR(om); OS_ENTER_CRITICAL(sr); STAILQ_INSERT_TAIL(&mq->mq_head, mp, omp_next); @@ -125,141 +122,6 @@ err: return (rc); } -int -os_msys_register(struct os_mbuf_pool *new_pool) -{ - struct os_mbuf_pool *pool; - - pool = NULL; - STAILQ_FOREACH(pool, &g_msys_pool_list, omp_next) { - if (new_pool->omp_databuf_len > pool->omp_databuf_len) { - break; - } - } - - if (pool) { - STAILQ_INSERT_AFTER(&g_msys_pool_list, pool, new_pool, omp_next); - } else { - STAILQ_INSERT_TAIL(&g_msys_pool_list, new_pool, omp_next); - } - - return (0); -} - -void -os_msys_reset(void) -{ - STAILQ_INIT(&g_msys_pool_list); -} - -static struct os_mbuf_pool * -os_msys_find_pool(uint16_t dsize) -{ - struct os_mbuf_pool *pool; - struct os_mbuf_pool *pool_with_free_blocks = NULL; - uint16_t pool_free_blocks; - - STAILQ_FOREACH(pool, &g_msys_pool_list, omp_next) { - pool_free_blocks = pool->omp_pool->mp_num_free; - if (pool_free_blocks != 0) { - pool_with_free_blocks = pool; - if (dsize <= pool->omp_databuf_len) { - break; - } - } - } - - return pool_with_free_blocks; -} - -static struct os_mbuf_pool * -os_msys_find_biggest_pool(void) -{ - return os_msys_find_pool(0xFFFF); -} - -struct os_mbuf * -os_msys_get(uint16_t dsize, uint16_t leadingspace) -{ - struct os_mbuf *m; - struct os_mbuf_pool *pool; - - /* If dsize = 0 that means user has no idea how big block size is needed, - * therefore lets find for him the biggest one - */ - if (dsize == 0) { - pool = os_msys_find_biggest_pool(); - } else { - pool = os_msys_find_pool(dsize); - } - - if (!pool) { - goto err; - } - - m = os_mbuf_get(pool, leadingspace); - return (m); -err: - return (NULL); -} - -struct os_mbuf * -os_msys_get_pkthdr(uint16_t dsize, uint16_t user_hdr_len) -{ - uint16_t total_pkthdr_len; - struct os_mbuf *m; - struct os_mbuf_pool *pool; - - total_pkthdr_len = user_hdr_len + sizeof(struct os_mbuf_pkthdr); - - /* If dsize = 0 that means user has no idea how big block size is needed, - * therefore lets find for him the biggest one - */ - if (dsize == 0) { - pool = os_msys_find_biggest_pool(); - } else { - pool = os_msys_find_pool(dsize + total_pkthdr_len); - } - - if (!pool) { - goto err; - } - - m = os_mbuf_get_pkthdr(pool, user_hdr_len); - return (m); -err: - return (NULL); -} - -int -os_msys_count(void) -{ - struct os_mbuf_pool *omp; - int total; - - total = 0; - STAILQ_FOREACH(omp, &g_msys_pool_list, omp_next) { - total += omp->omp_pool->mp_num_blocks; - } - - return total; -} - -int -os_msys_num_free(void) -{ - struct os_mbuf_pool *omp; - int total; - - total = 0; - STAILQ_FOREACH(omp, &g_msys_pool_list, omp_next) { - total += omp->omp_pool->mp_num_free; - } - - return total; -} - - int os_mbuf_pool_init(struct os_mbuf_pool *omp, struct os_mempool *mp, uint16_t buf_len, uint16_t nbufs) @@ -275,8 +137,8 @@ os_mbuf_get(struct os_mbuf_pool *omp, uint16_t leadingspace) { struct os_mbuf *om; - os_trace_api_u32x2(OS_TRACE_ID_MBUF_GET, (uint32_t)omp, - (uint32_t)(uintptr_t)leadingspace); + os_trace_api_u32x2(OS_TRACE_ID_MBUF_GET, (uintptr_t)omp, + (uint32_t)leadingspace); if (leadingspace > omp->omp_databuf_len) { om = NULL; @@ -296,7 +158,7 @@ os_mbuf_get(struct os_mbuf_pool *omp, uint16_t leadingspace) om->om_omp = omp; done: - os_trace_api_ret_u32(OS_TRACE_ID_MBUF_GET, (uint32_t)(uintptr_t)om); + os_trace_api_ret_u32(OS_TRACE_ID_MBUF_GET, (uintptr_t)om); return om; } @@ -307,7 +169,7 @@ os_mbuf_get_pkthdr(struct os_mbuf_pool *omp, uint8_t user_pkthdr_len) struct os_mbuf_pkthdr *pkthdr; struct os_mbuf *om; - os_trace_api_u32x2(OS_TRACE_ID_MBUF_GET_PKTHDR, (uint32_t)(uintptr_t)omp, + os_trace_api_u32x2(OS_TRACE_ID_MBUF_GET_PKTHDR, (uintptr_t)omp, (uint32_t)user_pkthdr_len); /* User packet header must fit inside mbuf */ @@ -329,7 +191,7 @@ os_mbuf_get_pkthdr(struct os_mbuf_pool *omp, uint8_t user_pkthdr_len) } done: - os_trace_api_ret_u32(OS_TRACE_ID_MBUF_GET_PKTHDR, (uint32_t)(uintptr_t)om); + os_trace_api_ret_u32(OS_TRACE_ID_MBUF_GET_PKTHDR, (uintptr_t)om); return om; } @@ -338,7 +200,7 @@ os_mbuf_free(struct os_mbuf *om) { int rc; - os_trace_api_u32(OS_TRACE_ID_MBUF_FREE, (uint32_t)(uintptr_t)om); + os_trace_api_u32(OS_TRACE_ID_MBUF_FREE, (uintptr_t)om); if (om->om_omp != NULL) { rc = os_memblock_put(om->om_omp->omp_pool, om); @@ -360,7 +222,7 @@ os_mbuf_free_chain(struct os_mbuf *om) struct os_mbuf *next; int rc; - os_trace_api_u32(OS_TRACE_ID_MBUF_FREE_CHAIN, (uint32_t)(uintptr_t)om); + os_trace_api_u32(OS_TRACE_ID_MBUF_FREE_CHAIN, (uintptr_t)om); while (om != NULL) { next = SLIST_NEXT(om, om_next); @@ -588,7 +450,7 @@ os_mbuf_off(const struct os_mbuf *om, int off, uint16_t *out_off) } int -os_mbuf_copydata(const struct os_mbuf *m, int off, int len, void *dst) +os_mbuf_copydata(const struct os_mbuf *om, int off, int len, void *dst) { unsigned int count; uint8_t *udst; @@ -600,36 +462,38 @@ os_mbuf_copydata(const struct os_mbuf *m, int off, int len, void *dst) udst = dst; while (off > 0) { - if (!m) { + if (!om) { return (-1); } - if (off < m->om_len) + if (off < om->om_len) { break; - off -= m->om_len; - m = SLIST_NEXT(m, om_next); + } + off -= om->om_len; + om = SLIST_NEXT(om, om_next); } - while (len > 0 && m != NULL) { - count = min(m->om_len - off, len); - memcpy(udst, m->om_data + off, count); + while (len > 0 && om != NULL) { + count = min(om->om_len - off, len); + memcpy(udst, om->om_data + off, count); len -= count; udst += count; off = 0; - m = SLIST_NEXT(m, om_next); + om = SLIST_NEXT(om, om_next); } return (len > 0 ? -1 : 0); } void -os_mbuf_adj(struct os_mbuf *mp, int req_len) +os_mbuf_adj(struct os_mbuf *om, int req_len) { int len = req_len; struct os_mbuf *m; int count; - if ((m = mp) == NULL) + if ((m = om) == NULL) { return; + } if (len >= 0) { /* * Trim from head. @@ -645,8 +509,9 @@ os_mbuf_adj(struct os_mbuf *mp, int req_len) len = 0; } } - if (OS_MBUF_IS_PKTHDR(mp)) - OS_MBUF_PKTHDR(mp)->omp_len -= (req_len - len); + if (OS_MBUF_IS_PKTHDR(om)) { + OS_MBUF_PKTHDR(om)->omp_len -= (req_len - len); + } } else { /* * Trim from tail. Scan the mbuf chain, @@ -665,8 +530,9 @@ os_mbuf_adj(struct os_mbuf *mp, int req_len) } if (m->om_len >= len) { m->om_len -= len; - if (OS_MBUF_IS_PKTHDR(mp)) - OS_MBUF_PKTHDR(mp)->omp_len -= len; + if (OS_MBUF_IS_PKTHDR(om)) { + OS_MBUF_PKTHDR(om)->omp_len -= len; + } return; } count -= len; @@ -677,7 +543,7 @@ os_mbuf_adj(struct os_mbuf *mp, int req_len) * Find the mbuf with last data, adjust its length, * and toss data from remaining mbufs on chain. */ - m = mp; + m = om; if (OS_MBUF_IS_PKTHDR(m)) OS_MBUF_PKTHDR(m)->omp_len = count; for (; m; m = SLIST_NEXT(m, om_next)) { diff --git a/porting/nimble/src/os_mempool.c b/porting/nimble/src/os_mempool.c index 09d5e3526..9800d8808 100644 --- a/porting/nimble/src/os_mempool.c +++ b/porting/nimble/src/os_mempool.c @@ -32,8 +32,8 @@ #if MYNEWT_VAL(OS_MEMPOOL_GUARD) #define OS_MEMPOOL_TRUE_BLOCK_SIZE(mp) \ (((mp)->mp_flags & OS_MEMPOOL_F_EXT) ? \ - OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size) : \ - (OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size) + sizeof(os_membuf_t))) + OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size) : \ + (OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size) + sizeof(os_membuf_t))) #else #define OS_MEMPOOL_TRUE_BLOCK_SIZE(mp) OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size) #endif @@ -95,7 +95,7 @@ os_mempool_guard(const struct os_mempool *mp, void *start) if ((mp->mp_flags & OS_MEMPOOL_F_EXT) == 0) { tgt = (uint32_t *)((uintptr_t)start + - OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size)); + OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size)); *tgt = OS_MEMPOOL_GUARD_PATTERN; } } @@ -107,7 +107,7 @@ os_mempool_guard_check(const struct os_mempool *mp, void *start) if ((mp->mp_flags & OS_MEMPOOL_F_EXT) == 0) { tgt = (uint32_t *)((uintptr_t)start + - OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size)); + OS_MEM_TRUE_BLOCK_SIZE(mp->mp_block_size)); assert(*tgt == OS_MEMPOOL_GUARD_PATTERN); } } @@ -139,7 +139,7 @@ os_mempool_init_internal(struct os_mempool *mp, uint16_t blocks, /* Blocks need to be sized properly and memory buffer should be * aligned */ - if (((uint32_t)(uintptr_t)membuf & (OS_ALIGNMENT - 1)) != 0) { + if (((uintptr_t)membuf & (OS_ALIGNMENT - 1)) != 0) { return OS_MEM_NOT_ALIGNED; } } @@ -150,7 +150,7 @@ os_mempool_init_internal(struct os_mempool *mp, uint16_t blocks, mp->mp_min_free = blocks; mp->mp_flags = flags; mp->mp_num_blocks = blocks; - mp->mp_membuf_addr = (uint32_t)(uintptr_t)membuf; + mp->mp_membuf_addr = (uintptr_t)membuf; mp->name = name; SLIST_FIRST(mp) = membuf; @@ -261,10 +261,10 @@ os_mempool_clear(struct os_mempool *mp) mp->mp_min_free = mp->mp_num_blocks; os_mempool_poison(mp, (void *)mp->mp_membuf_addr); os_mempool_guard(mp, (void *)mp->mp_membuf_addr); - SLIST_FIRST(mp) = (void *)(uintptr_t)mp->mp_membuf_addr; + SLIST_FIRST(mp) = (void *)mp->mp_membuf_addr; /* Chain the memory blocks to the free list */ - block_addr = (uint8_t *)(uintptr_t)mp->mp_membuf_addr; + block_addr = (uint8_t *)mp->mp_membuf_addr; block_ptr = (struct os_memblock *)block_addr; blocks = mp->mp_num_blocks; @@ -304,23 +304,20 @@ int os_memblock_from(const struct os_mempool *mp, const void *block_addr) { uint32_t true_block_size; - uintptr_t baddr32; - uint32_t end; + uintptr_t baddr; + uintptr_t end; - static_assert(sizeof block_addr == sizeof baddr32, - "Pointer to void must be 32-bits."); - - baddr32 = (uint32_t)(uintptr_t)block_addr; + baddr = (uintptr_t)block_addr; true_block_size = OS_MEMPOOL_TRUE_BLOCK_SIZE(mp); end = mp->mp_membuf_addr + (mp->mp_num_blocks * true_block_size); /* Check that the block is in the memory buffer range. */ - if ((baddr32 < mp->mp_membuf_addr) || (baddr32 >= end)) { + if ((baddr < mp->mp_membuf_addr) || (baddr >= end)) { return 0; } /* All freed blocks should be on true block size boundaries! */ - if (((baddr32 - mp->mp_membuf_addr) % true_block_size) != 0) { + if (((baddr - mp->mp_membuf_addr) % true_block_size) != 0) { return 0; } @@ -333,7 +330,7 @@ os_memblock_get(struct os_mempool *mp) os_sr_t sr; struct os_memblock *block; - os_trace_api_u32(OS_TRACE_ID_MEMBLOCK_GET, (uint32_t)(uintptr_t)mp); + os_trace_api_u32(OS_TRACE_ID_MEMBLOCK_GET, (uintptr_t)mp); /* Check to make sure they passed in a memory pool (or something) */ block = NULL; @@ -361,7 +358,7 @@ os_memblock_get(struct os_mempool *mp) } } - os_trace_api_ret_u32(OS_TRACE_ID_MEMBLOCK_GET, (uint32_t)(uintptr_t)block); + os_trace_api_ret_u32(OS_TRACE_ID_MEMBLOCK_GET, (uintptr_t)block); return (void *)block; } @@ -372,8 +369,8 @@ os_memblock_put_from_cb(struct os_mempool *mp, void *block_addr) os_sr_t sr; struct os_memblock *block; - os_trace_api_u32x2(OS_TRACE_ID_MEMBLOCK_PUT_FROM_CB, (uint32_t)(uintptr_t)mp, - (uint32_t)(uintptr_t)block_addr); + os_trace_api_u32x2(OS_TRACE_ID_MEMBLOCK_PUT_FROM_CB, (uintptr_t)mp, + (uintptr_t)block_addr); os_mempool_guard_check(mp, block_addr); os_mempool_poison(mp, block_addr); @@ -403,10 +400,11 @@ os_memblock_put(struct os_mempool *mp, void *block_addr) os_error_t ret; #if MYNEWT_VAL(OS_MEMPOOL_CHECK) struct os_memblock *block; + int sr; #endif - os_trace_api_u32x2(OS_TRACE_ID_MEMBLOCK_PUT, (uint32_t)(uintptr_t)mp, - (uint32_t)(uintptr_t)block_addr); + os_trace_api_u32x2(OS_TRACE_ID_MEMBLOCK_PUT, (uintptr_t)mp, + (uintptr_t)block_addr); /* Make sure parameters are valid */ if ((mp == NULL) || (block_addr == NULL)) { @@ -421,9 +419,12 @@ os_memblock_put(struct os_mempool *mp, void *block_addr) /* * Check for duplicate free. */ + OS_ENTER_CRITICAL(sr); SLIST_FOREACH(block, mp, mb_next) { assert(block != (struct os_memblock *)block_addr); } + OS_EXIT_CRITICAL(sr); + #endif /* If this is an extended mempool with a put callback, call the callback * instead of freeing the block directly. @@ -469,8 +470,34 @@ os_mempool_info_get_next(struct os_mempool *mp, struct os_mempool_info *omi) return (cur); } +struct os_mempool * +os_mempool_get(const char *mempool_name, struct os_mempool_info *info) +{ + struct os_mempool *mp; + + mp = STAILQ_FIRST(&g_os_mempool_list); + while (mp) { + if (strcmp(mempool_name, mp->name) == 0) { + break; + } + mp = STAILQ_NEXT(mp, mp_list); + } + + if (mp != NULL && info != NULL) { + info->omi_block_size = mp->mp_block_size; + info->omi_num_blocks = mp->mp_num_blocks; + info->omi_num_free = mp->mp_num_free; + info->omi_min_free = mp->mp_min_free; + info->omi_name[0] = '\0'; + strncat(info->omi_name, mp->name, sizeof(info->omi_name) - 1); + } + + return mp; +} + void os_mempool_module_init(void) { STAILQ_INIT(&g_os_mempool_list); } + diff --git a/porting/nimble/src/os_msys_init.c b/porting/nimble/src/os_msys.c similarity index 59% rename from porting/nimble/src/os_msys_init.c rename to porting/nimble/src/os_msys.c index d22ae351f..7f1069fab 100644 --- a/porting/nimble/src/os_msys_init.c +++ b/porting/nimble/src/os_msys.c @@ -22,7 +22,7 @@ #include "mem/mem.h" #include "sysinit/sysinit.h" -static STAILQ_HEAD(, os_mbuf_pool) g_msys_pool_list = +static STAILQ_HEAD(os_mbuf_list, os_mbuf_pool) g_msys_pool_list = STAILQ_HEAD_INITIALIZER(g_msys_pool_list); #if MYNEWT_VAL(MSYS_1_BLOCK_COUNT) > 0 @@ -55,6 +55,147 @@ static struct os_mempool os_msys_2_mempool; static struct os_sanity_check os_msys_sc; #endif +int +os_msys_register(struct os_mbuf_pool *new_pool) +{ + struct os_mbuf_pool *pool; + struct os_mbuf_pool *prev; + + /* We want to have order from smallest to biggest mempool. */ + prev = NULL; + pool = NULL; + STAILQ_FOREACH(pool, &g_msys_pool_list, omp_next) { + if (new_pool->omp_databuf_len < pool->omp_databuf_len) { + break; + } + prev = pool; + } + + if (prev) { + STAILQ_INSERT_AFTER(&g_msys_pool_list, prev, new_pool, omp_next); + } else { + STAILQ_INSERT_HEAD(&g_msys_pool_list, new_pool, omp_next); + } + + return (0); +} + +void +os_msys_reset(void) +{ + STAILQ_INIT(&g_msys_pool_list); +} + +static struct os_mbuf_pool *os_msys_find_pool(uint16_t dsize); + +static struct os_mbuf_pool * +os_msys_find_biggest_pool(void) +{ + return os_msys_find_pool(0xFFFF); +} + +static struct os_mbuf_pool * +os_msys_find_pool(uint16_t dsize) +{ + struct os_mbuf_pool *pool; + struct os_mbuf_pool *pool_with_free_blocks = NULL; + uint16_t pool_free_blocks; + + STAILQ_FOREACH(pool, &g_msys_pool_list, omp_next) { + pool_free_blocks = pool->omp_pool->mp_num_free; + if (pool_free_blocks != 0) { + pool_with_free_blocks = pool; + if (dsize <= pool->omp_databuf_len) { + break; + } + } + } + + return pool_with_free_blocks; +} + + +struct os_mbuf * +os_msys_get(uint16_t dsize, uint16_t leadingspace) +{ + struct os_mbuf *m; + struct os_mbuf_pool *pool; + + /* If dsize = 0 that means user has no idea how big block size is needed, + * therefore lets find for him the biggest one + */ + if (dsize == 0) { + pool = os_msys_find_biggest_pool(); + } else { + pool = os_msys_find_pool(dsize); + } + + if (!pool) { + goto err; + } + + m = os_mbuf_get(pool, leadingspace); + return (m); +err: + return (NULL); +} + +struct os_mbuf * +os_msys_get_pkthdr(uint16_t dsize, uint16_t user_hdr_len) +{ + uint16_t total_pkthdr_len; + struct os_mbuf *m; + struct os_mbuf_pool *pool; + + total_pkthdr_len = user_hdr_len + sizeof(struct os_mbuf_pkthdr); + + /* If dsize = 0 that means user has no idea how big block size is needed, + * therefore lets find for him the biggest one + */ + if (dsize == 0) { + pool = os_msys_find_biggest_pool(); + } else { + pool = os_msys_find_pool(dsize + total_pkthdr_len); + } + + if (!pool) { + goto err; + } + + m = os_mbuf_get_pkthdr(pool, user_hdr_len); + return (m); +err: + return (NULL); +} + +int +os_msys_count(void) +{ + struct os_mbuf_pool *omp; + int total; + + total = 0; + STAILQ_FOREACH(omp, &g_msys_pool_list, omp_next) { + total += omp->omp_pool->mp_num_blocks; + } + + return total; +} + +int +os_msys_num_free(void) +{ + struct os_mbuf_pool *omp; + int total; + + total = 0; + STAILQ_FOREACH(omp, &g_msys_pool_list, omp_next) { + total += omp->omp_pool->mp_num_free; + } + + return total; +} + #if OS_MSYS_SANITY_ENABLED /** @@ -153,4 +294,12 @@ os_msys_init(void) rc = os_sanity_check_register(&os_msys_sc); SYSINIT_PANIC_ASSERT(rc == 0); #endif -} \ No newline at end of file +} + +#if MYNEWT_VAL(SELFTEST) +struct os_mbuf_list * +get_msys_pool_list(void) +{ + return &g_msys_pool_list; +} +#endif
