cbd31d720b
In order to bring consistency in-tree, migrate all lib code to the new prefix <zephyr/...>. Note that the conversion has been scripted, refer to zephyrproject-rtos#45388 for more details. Signed-off-by: Gerard Marull-Paretas <gerard.marull@nordicsemi.no>
127 lines
2.5 KiB
C
127 lines
2.5 KiB
C
/*
|
|
* Copyright (c) 2021 Friedt Professional Engineering Services, Inc
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <stddef.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <sys/types.h>
|
|
#include <zephyr/sys/util.h>
|
|
|
|
/*
|
|
* 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))
|
|
|
|
struct qsort_comp {
|
|
bool has3;
|
|
void *arg;
|
|
union {
|
|
int (*comp2)(const void *a, const void *b);
|
|
int (*comp3)(const void *a, const void *b, void *arg);
|
|
};
|
|
};
|
|
|
|
static inline int compare(struct qsort_comp *cmp, void *a, void *b)
|
|
{
|
|
if (cmp->has3) {
|
|
return cmp->comp3(a, b, cmp->arg);
|
|
}
|
|
|
|
return cmp->comp2(a, b);
|
|
}
|
|
|
|
static void sift_down(void *base, int start, int end, size_t size, struct qsort_comp *cmp)
|
|
{
|
|
int root;
|
|
int child;
|
|
int swap;
|
|
|
|
for (swap = start, root = swap; left(root) < end; root = swap) {
|
|
child = left(root);
|
|
|
|
/* if root < left */
|
|
if (compare(cmp, A(swap), A(child)) < 0) {
|
|
swap = child;
|
|
}
|
|
|
|
/* right exists and min(A(root),A(left)) < A(right) */
|
|
if (right(root) < end && compare(cmp, A(swap), A(right(root))) < 0) {
|
|
swap = right(root);
|
|
}
|
|
|
|
if (swap == root) {
|
|
return;
|
|
}
|
|
|
|
byteswp(A(root), A(swap), size);
|
|
}
|
|
}
|
|
|
|
static void heapify(void *base, int nmemb, size_t size, struct qsort_comp *cmp)
|
|
{
|
|
int start;
|
|
|
|
for (start = parent(nmemb - 1); start >= 0; --start) {
|
|
sift_down(base, start, nmemb, size, cmp);
|
|
}
|
|
}
|
|
|
|
static void heap_sort(void *base, int nmemb, size_t size, struct qsort_comp *cmp)
|
|
{
|
|
int end;
|
|
|
|
heapify(base, nmemb, size, cmp);
|
|
|
|
for (end = nmemb - 1; end > 0; --end) {
|
|
byteswp(A(end), A(0), size);
|
|
sift_down(base, 0, end, size, cmp);
|
|
}
|
|
}
|
|
|
|
void qsort_r(void *base, size_t nmemb, size_t size,
|
|
int (*comp3)(const void *a, const void *b, void *arg), void *arg)
|
|
{
|
|
struct qsort_comp cmp = {
|
|
.has3 = true,
|
|
.arg = arg,
|
|
{
|
|
.comp3 = comp3
|
|
}
|
|
};
|
|
|
|
heap_sort(base, nmemb, size, &cmp);
|
|
}
|
|
|
|
void qsort(void *base, size_t nmemb, size_t size,
|
|
int (*comp2)(const void *a, const void *b))
|
|
{
|
|
struct qsort_comp cmp = {
|
|
.has3 = false,
|
|
.arg = NULL,
|
|
{
|
|
.comp2 = comp2
|
|
}
|
|
};
|
|
|
|
heap_sort(base, nmemb, size, &cmp);
|
|
}
|