1 /*
2 * Copyright (c) 2021 Friedt Professional Engineering Services, Inc
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stddef.h>
8 #include <stdint.h>
9 #include <stdlib.h>
10 #include <sys/types.h>
11 #include <zephyr/sys/util.h>
12
13 /*
14 * Normally parent is defined parent(k) = floor((k-1) / 2) but we can avoid a
15 * divide by noticing that floor((k-1) / 2) = ((k - 1) >> 1).
16 */
17
18 #define parent(k) (((k) - 1) >> 1)
19 /*
20 * Normally left is defined left(k) = (2 * k + 1) but we can avoid a
21 * multiply by noticing that (2 * k + 1) = ((k << 1) + 1).
22 */
23
24 #define left(k) (((k) << 1) + 1)
25
26 /*
27 * Normally right is defined right(k) = (2 * k + 2) but we can avoid a
28 * multiply by noticing that right(k) = left(k) + 1
29 */
30 #define right(k) (left(k) + 1)
31
32 #define A(k) ((uint8_t *)base + size * (k))
33
34 struct qsort_comp {
35 bool has3;
36 void *arg;
37 union {
38 int (*comp2)(const void *a, const void *b);
39 int (*comp3)(const void *a, const void *b, void *arg);
40 };
41 };
42
compare(struct qsort_comp * cmp,void * a,void * b)43 static inline int compare(struct qsort_comp *cmp, void *a, void *b)
44 {
45 if (cmp->has3) {
46 return cmp->comp3(a, b, cmp->arg);
47 }
48
49 return cmp->comp2(a, b);
50 }
51
sift_down(void * base,int start,int end,size_t size,struct qsort_comp * cmp)52 static void sift_down(void *base, int start, int end, size_t size, struct qsort_comp *cmp)
53 {
54 int root;
55 int child;
56 int swap;
57
58 for (swap = start, root = swap; left(root) < end; root = swap) {
59 child = left(root);
60
61 /* if root < left */
62 if (compare(cmp, A(swap), A(child)) < 0) {
63 swap = child;
64 }
65
66 /* right exists and min(A(root),A(left)) < A(right) */
67 if (right(root) < end && compare(cmp, A(swap), A(right(root))) < 0) {
68 swap = right(root);
69 }
70
71 if (swap == root) {
72 return;
73 }
74
75 byteswp(A(root), A(swap), size);
76 }
77 }
78
heapify(void * base,int nmemb,size_t size,struct qsort_comp * cmp)79 static void heapify(void *base, int nmemb, size_t size, struct qsort_comp *cmp)
80 {
81 int start;
82
83 for (start = parent(nmemb - 1); start >= 0; --start) {
84 sift_down(base, start, nmemb, size, cmp);
85 }
86 }
87
heap_sort(void * base,int nmemb,size_t size,struct qsort_comp * cmp)88 static void heap_sort(void *base, int nmemb, size_t size, struct qsort_comp *cmp)
89 {
90 int end;
91
92 heapify(base, nmemb, size, cmp);
93
94 for (end = nmemb - 1; end > 0; --end) {
95 byteswp(A(end), A(0), size);
96 sift_down(base, 0, end, size, cmp);
97 }
98 }
99
qsort_r(void * base,size_t nmemb,size_t size,int (* comp3)(const void * a,const void * b,void * arg),void * arg)100 void qsort_r(void *base, size_t nmemb, size_t size,
101 int (*comp3)(const void *a, const void *b, void *arg), void *arg)
102 {
103 struct qsort_comp cmp = {
104 .has3 = true,
105 .arg = arg,
106 {
107 .comp3 = comp3
108 }
109 };
110
111 heap_sort(base, nmemb, size, &cmp);
112 }
113
qsort(void * base,size_t nmemb,size_t size,int (* comp2)(const void * a,const void * b))114 void qsort(void *base, size_t nmemb, size_t size,
115 int (*comp2)(const void *a, const void *b))
116 {
117 struct qsort_comp cmp = {
118 .has3 = false,
119 .arg = NULL,
120 {
121 .comp2 = comp2
122 }
123 };
124
125 heap_sort(base, nmemb, size, &cmp);
126 }
127
