diff --git a/include/sys/util.h b/include/sys/util.h index e1aa0b59b6..742253fbc2 100644 --- a/include/sys/util.h +++ b/include/sys/util.h @@ -257,6 +257,29 @@ static inline void bytecpy(void *dst, const void *src, size_t size) } } +/** + * @brief byte by byte swap. + * + * Swap @a size bytes between memory regions @a a and @a b. This is + * guaranteed to be done byte by byte. + * + * @param a Pointer to the the first memory region. + * @param b Pointer to the the second memory region. + * @param size The number of bytes to swap. + */ +static inline void byteswp(void *a, void *b, size_t size) +{ + uint8_t t; + uint8_t *aa = (uint8_t *)a; + uint8_t *bb = (uint8_t *)b; + + for (; size > 0; --size) { + t = *aa; + *aa++ = *bb; + *bb++ = t; + } +} + /** * @brief Convert a single character into a hexadecimal nibble. * diff --git a/lib/libc/minimal/CMakeLists.txt b/lib/libc/minimal/CMakeLists.txt index b7f9e55774..bfeb71bbe1 100644 --- a/lib/libc/minimal/CMakeLists.txt +++ b/lib/libc/minimal/CMakeLists.txt @@ -11,6 +11,7 @@ zephyr_library_sources( source/stdlib/malloc.c source/stdlib/bsearch.c source/stdlib/exit.c + source/stdlib/qsort.c source/string/strncasecmp.c source/string/strstr.c source/string/string.c diff --git a/lib/libc/minimal/include/stdlib.h b/lib/libc/minimal/include/stdlib.h index d2486efb0d..2b804db7ce 100644 --- a/lib/libc/minimal/include/stdlib.h +++ b/lib/libc/minimal/include/stdlib.h @@ -30,6 +30,9 @@ void *bsearch(const void *key, const void *array, size_t count, size_t size, int (*cmp)(const void *key, const void *element)); +void qsort_r(void *base, size_t nmemb, size_t size, + int (*compar)(const void *, const void *, void *), void *arg); + #define EXIT_SUCCESS 0 #define EXIT_FAILURE 1 void _exit(int status); @@ -60,6 +63,14 @@ static inline long long llabs(long long __n) return (__n < 0LL) ? -__n : __n; } +static inline void qsort(void *base, size_t nmemb, size_t size, + int (*compar)(const void *, const void *)) +{ + typedef int (*compar3)(const void *, const void *, void *); + + qsort_r(base, nmemb, size, (compar3)compar, NULL); +} + #ifdef __cplusplus } #endif diff --git a/lib/libc/minimal/source/stdlib/qsort.c b/lib/libc/minimal/source/stdlib/qsort.c new file mode 100644 index 0000000000..e9d616aea8 --- /dev/null +++ b/lib/libc/minimal/source/stdlib/qsort.c @@ -0,0 +1,86 @@ +/* + * Copyright (c) 2021 Friedt Professional Engineering Services, Inc + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include + +typedef int (*comp3_t)(const void *, const void *, void *); + +/* + * Normally parent is defined parent(k) = floor((k-1) / 2) but we can avoid a + * divide by noticing that floor((k-1) / 2) = ((k - 1) >> 1). + */ + +#define parent(k) (((k) - 1) >> 1) +/* + * Normally left is defined left(k) = (2 * k + 1) but we can avoid a + * multiply by noticing that (2 * k + 1) = ((k << 1) + 1). + */ + +#define left(k) (((k) << 1) + 1) + +/* + * Normally right is defined right(k) = (2 * k + 2) but we can avoid a + * multiply by noticing that right(k) = left(k) + 1 + */ +#define right(k) (left(k) + 1) + +#define A(k) ((uint8_t *)base + size * (k)) + +static void sift_down(void *base, int start, int end, size_t size, comp3_t comp, void *comp_arg) +{ + int root; + int child; + int swap; + + for (swap = start, root = swap; left(root) < end; root = swap) { + child = left(root); + + /* if root < left */ + if (comp(A(swap), A(child), comp_arg) < 0) { + swap = child; + } + + /* right exists and min(A(root),A(left)) < A(right) */ + if (right(root) < end && comp(A(swap), A(right(root)), comp_arg) < 0) { + swap = right(root); + } + + if (swap == root) { + return; + } + + byteswp(A(root), A(swap), size); + } +} + +static void heapify(void *base, int nmemb, size_t size, comp3_t comp, void *comp_arg) +{ + int start; + + for (start = parent(nmemb - 1); start >= 0; --start) { + sift_down(base, start, nmemb, size, comp, comp_arg); + } +} + +static void heap_sort(void *base, int nmemb, size_t size, comp3_t comp, void *comp_arg) +{ + int end; + + heapify(base, nmemb, size, comp, comp_arg); + + for (end = nmemb - 1; end > 0; --end) { + byteswp(A(end), A(0), size); + sift_down(base, 0, end, size, comp, comp_arg); + } +} + +void qsort_r(void *base, size_t nmemb, size_t size, comp3_t comp, void *arg) +{ + heap_sort(base, nmemb, size, comp, arg); +} diff --git a/samples/subsys/shell/shell_module/CMakeLists.txt b/samples/subsys/shell/shell_module/CMakeLists.txt index 50e902b84c..ecf1c025f7 100644 --- a/samples/subsys/shell/shell_module/CMakeLists.txt +++ b/samples/subsys/shell/shell_module/CMakeLists.txt @@ -5,5 +5,5 @@ find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) project(shell_module) target_sources(app PRIVATE src/main.c src/test_module.c) -target_sources_ifdef(CONFIG_SHELL_DYNAMIC_CMDS app PRIVATE src/dynamic_cmd.c src/qsort.c) +target_sources_ifdef(CONFIG_SHELL_DYNAMIC_CMDS app PRIVATE src/dynamic_cmd.c) target_sources_ifdef(CONFIG_SHELL_BACKEND_SERIAL app PRIVATE src/uart_reinit.c) diff --git a/samples/subsys/shell/shell_module/src/qsort.c b/samples/subsys/shell/shell_module/src/qsort.c deleted file mode 100644 index d27a7b7f95..0000000000 --- a/samples/subsys/shell/shell_module/src/qsort.c +++ /dev/null @@ -1,202 +0,0 @@ -/*- - * SPDX-License-Identifier: BSD-3-Clause - * - * Copyright (c) 1992, 1993 - * The Regents of the University of California. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 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. - * 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. - * - * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY - * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF - * SUCH DAMAGE. - */ - -#if defined(LIBC_SCCS) && !defined(lint) -static const char sccsid[] = "@(#)qsort.c 8.1 (Berkeley) 6/4/93"; -#endif /* LIBC_SCCS and not lint */ -#include -#include - -#ifdef I_AM_QSORT_R -typedef int cmp_t(void *, const void *, const void *); -#else -typedef int cmp_t(const void *, const void *); -#endif -static inline char *med3(char *, char *, char *, cmp_t *, void *); - -#define MIN(a, b) ((a) < (b) ? a : b) - -/* - * Qsort routine from Bentley & McIlroy's "Engineering a Sort Function". - */ - -static inline void -swapfunc(char *a, char *b, size_t es) -{ - char t; - - do { - t = *a; - *a++ = *b; - *b++ = t; - } while (--es > 0); -} - -#define vecswap(a, b, n) \ - do { \ - if ((n) > 0) { \ - swapfunc(a, b, n); \ - } \ - } while (0) - -#ifdef I_AM_QSORT_R -#define CMP(t, x, y) (cmp((t), (x), (y))) -#else -#define CMP(t, x, y) (cmp((x), (y))) -#endif - -static inline char * -med3(char *a, char *b, char *c, cmp_t *cmp, void *thunk) -{ - return CMP(thunk, a, b) < 0 ? - (CMP(thunk, b, c) < 0 ? b : (CMP(thunk, a, c) < 0 ? c : a)) - : (CMP(thunk, b, c) > 0 ? b : (CMP(thunk, a, c) < 0 ? a : c)); -} - -#ifdef I_AM_QSORT_R -void qsort_r(void *a, size_t n, size_t es, void *thunk, cmp_t *cmp) -#else -#define thunk NULL -void qsort(void *a, size_t n, size_t es, cmp_t *cmp) -#endif -{ - char *pa, *pb, *pc, *pd, *pl, *pm, *pn; - size_t d1, d2; - int cmp_result; - int swap_cnt; - -loop: - swap_cnt = 0; - if (n < 7) { - for (pm = (char *)a + es; pm < (char *)a + n * es; pm += es) { - for (pl = pm; - pl > (char *)a && CMP(thunk, pl - es, pl) > 0; - pl -= es) { - swapfunc(pl, pl - es, es); - } - } - return; - } - pm = (char *)a + (n / 2) * es; - if (n > 7) { - pl = a; - pn = (char *)a + (n - 1) * es; - if (n > 40) { - size_t d = (n / 8) * es; - - pl = med3(pl, pl + d, pl + 2 * d, cmp, thunk); - pm = med3(pm - d, pm, pm + d, cmp, thunk); - pn = med3(pn - 2 * d, pn - d, pn, cmp, thunk); - } - pm = med3(pl, pm, pn, cmp, thunk); - } - swapfunc(a, pm, es); - pa = pb = (char *)a + es; - - pc = pd = (char *)a + (n - 1) * es; - for (;;) { - while (pb <= pc && (cmp_result = CMP(thunk, pb, a)) <= 0) { - if (cmp_result == 0) { - swap_cnt = 1; - swapfunc(pa, pb, es); - pa += es; - } - pb += es; - } - while (pb <= pc && (cmp_result = CMP(thunk, pc, a)) >= 0) { - if (cmp_result == 0) { - swap_cnt = 1; - swapfunc(pc, pd, es); - pd -= es; - } - pc -= es; - } - if (pb > pc) { - break; - } - - swapfunc(pb, pc, es); - swap_cnt = 1; - pb += es; - pc -= es; - } - if (swap_cnt == 0) { /* Switch to insertion sort */ - for (pm = (char *)a + es; pm < (char *)a + n * es; pm += es) { - for (pl = pm; - pl > (char *)a && CMP(thunk, pl - es, pl) > 0; - pl -= es) { - swapfunc(pl, pl - es, es); - } - } - return; - } - - pn = (char *)a + n * es; - d1 = MIN(pa - (char *)a, pb - pa); - vecswap(a, pb - d1, d1); - d1 = MIN(pd - pc, pn - pd - es); - vecswap(pb, pn - d1, d1); - - d1 = pb - pa; - d2 = pd - pc; - if (d1 <= d2) { - /* Recurse on left partition, then iterate on right partition */ - if (d1 > es) { -#ifdef I_AM_QSORT_R - qsort_r(a, d1 / es, es, thunk, cmp); -#else - qsort(a, d1 / es, es, cmp); -#endif - } - if (d2 > es) { - /* Iterate rather than recurse to save stack space */ - /* qsort(pn - d2, d2 / es, es, cmp); */ - a = pn - d2; - n = d2 / es; - goto loop; - } - } else { - /* Recurse on right partition, then iterate on left partition */ - if (d2 > es) { -#ifdef I_AM_QSORT_R - qsort_r(pn - d2, d2 / es, es, thunk, cmp); -#else - qsort(pn - d2, d2 / es, es, cmp); -#endif - } - if (d1 > es) { - /* Iterate rather than recurse to save stack space */ - /* qsort(a, d1 / es, es, cmp); */ - n = d1 / es; - goto loop; - } - } -}