1/*-
2 * Copyright (c) 1992, 1993
3 * The Regents of the University of California. All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 * 4. Neither the name of the University nor the names of its contributors
14 * may be used to endorse or promote products derived from this software
15 * without specific prior written permission.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
18 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
21 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
23 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
24 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
26 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
27 * SUCH DAMAGE.
28 */
29
30#include <mem/alloc.h>
31#include <stdint.h>
32#include <string.h>
33
34/*
35 * Swap two areas of size number of bytes. Although qsort(3) permits random
36 * blocks of memory to be sorted, sorting pointers is almost certainly the
37 * common case (and, were it not, could easily be made so). Regardless, it
38 * isn't worth optimizing; the SWAP's get sped up by the cache, and pointer
39 * arithmetic gets lost in the time required for comparison function calls.
40 */
41#define SWAP(a, b, count, size, tmp) \
42 { \
43 count = size; \
44 do { \
45 tmp = *a; \
46 *a++ = *b; \
47 *b++ = tmp; \
48 } while (--count); \
49 }
50
51/* Copy one block of size size to another. */
52#define COPY(a, b, count, size, tmp1, tmp2) \
53 { \
54 count = size; \
55 tmp1 = a; \
56 tmp2 = b; \
57 do { \
58 *tmp1++ = *tmp2++; \
59 } while (--count); \
60 }
61
62/*
63 * Build the list into a heap, where a heap is defined such that for
64 * the records K1 ... KN, Kj/2 >= Kj for 1 <= j/2 <= j <= N.
65 *
66 * There two cases. If j == nmemb, select largest of Ki and Kj. If
67 * j < nmemb, select largest of Ki, Kj and Kj+1.
68 */
69#define CREATE(initval, nmemb, par_i, child_i, par, child, size, count, tmp) \
70 { \
71 for (par_i = initval; (child_i = par_i * 2) <= nmemb; \
72 par_i = child_i) { \
73 child = base + child_i * size; \
74 if (child_i < nmemb && compar(child, child + size) < 0) { \
75 child += size; \
76 ++child_i; \
77 } \
78 par = base + par_i * size; \
79 if (compar(child, par) <= 0) \
80 break; \
81 SWAP(par, child, count, size, tmp); \
82 } \
83 }
84
85/*
86 * Select the top of the heap and 'heapify'. Since by far the most expensive
87 * action is the call to the compar function, a considerable optimization
88 * in the average case can be achieved due to the fact that k, the displaced
89 * elememt, is ususally quite small, so it would be preferable to first
90 * heapify, always maintaining the invariant that the larger child is copied
91 * over its parent's record.
92 *
93 * Then, starting from the *bottom* of the heap, finding k's correct place,
94 * again maintianing the invariant. As a result of the invariant no element
95 * is 'lost' when k is assigned its correct place in the heap.
96 *
97 * The time savings from this optimization are on the order of 15-20% for the
98 * average case. See Knuth, Vol. 3, page 158, problem 18.
99 *
100 * XXX Don't break the #define SELECT line, below. Reiser cpp gets upset.
101 */
102#define SELECT(par_i, child_i, nmemb, par, child, size, k, count, tmp1, tmp2) \
103 { \
104 for (par_i = 1; (child_i = par_i * 2) <= nmemb; par_i = child_i) { \
105 child = base + child_i * size; \
106 if (child_i < nmemb && compar(child, child + size) < 0) { \
107 child += size; \
108 ++child_i; \
109 } \
110 par = base + par_i * size; \
111 COPY(par, child, count, size, tmp1, tmp2); \
112 } \
113 while (true) { \
114 child_i = par_i; \
115 par_i = child_i / 2; \
116 child = base + child_i * size; \
117 par = base + par_i * size; \
118 if (child_i == 1 || compar(k, par) < 0) { \
119 COPY(child, k, count, size, tmp1, tmp2); \
120 break; \
121 } \
122 COPY(child, par, count, size, tmp1, tmp2); \
123 } \
124 }
125
126int heapsort(void *vbase, size_t nmemb, size_t size,
127 int (*compar)(const void *, const void *)) {
128 size_t cnt;
129 size_t i;
130 size_t j;
131 size_t l;
132 char tmp;
133 char *tmp1;
134 char *tmp2;
135 char *base;
136 char *k;
137 char *p;
138 char *t;
139
140 if (nmemb <= 1) {
141 return (0);
142 }
143
144 if (!size) {
145 // errno = EINVAL;
146 return (-1);
147 }
148
149 if ((k = kmalloc(size)) == NULL) {
150 return (-1);
151 }
152
153 /*
154 * Items are numbered from 1 to nmemb, so offset from size bytes
155 * below the starting address.
156 */
157 base = (char *) vbase - size;
158
159 for (l = nmemb / 2 + 1; --l;) {
160 CREATE(l, nmemb, i, j, t, p, size, cnt, tmp);
161 }
162
163 /*
164 * For each element of the heap, save the largest element into its
165 * final slot, save the displaced element (k), then recreate the
166 * heap.
167 */
168 while (nmemb > 1) {
169 COPY(k, base + nmemb * size, cnt, size, tmp1, tmp2);
170 COPY(base + nmemb * size, base + size, cnt, size, tmp1, tmp2);
171 --nmemb;
172 SELECT(i, j, nmemb, t, p, size, k, cnt, tmp1, tmp2);
173 }
174
175 kfree(k);
176
177 return (0);
178}
179#ifdef I_AM_QSORT_R
180typedef int cmp_t(void *, const void *, const void *);
181#else
182typedef int cmp_t(const void *, const void *);
183#endif
184
185static inline char *med3(char *a, char *b, char *c, cmp_t *cmd, void *thunk)
186 __attribute__((always_inline));
187static inline void swapfunc(char *a, char *b, size_t n, size_t swaptype)
188 __attribute__((always_inline));
189
190#define min(a, b) (a) < (b) ? a : b
191
192/*
193 * Qsort routine from Bentley & McIlroy's "Engineering a Sort Function".
194 */
195#define swapcode(TYPE, parmi, parmj, n) \
196 { \
197 size_t i = (n) / sizeof(TYPE); \
198 TYPE *pi = (TYPE *) (parmi); \
199 TYPE *pj = (TYPE *) (parmj); \
200 do { \
201 TYPE t = *pi; \
202 *pi++ = *pj; \
203 *pj++ = t; \
204 } while (--i > 0); \
205 }
206
207#define SWAPINIT(a, es) \
208 swaptype = (uintptr_t) a % sizeof(long) || es % sizeof(long) ? 2 \
209 : es == sizeof(long) ? 0 \
210 : 1;
211
212static inline void swapfunc(char *a, char *b, size_t n, size_t swaptype) {
213 if (swaptype == 0) {
214 long t = *(long *) (void *) (a);
215 *(long *) (void *) (a) = *(long *) (void *) (b);
216 *(long *) (void *) (b) = t;
217 } else if (swaptype == 1) {
218 swapcode(long, (void *) a, (void *) b, n)
219 } else {
220 swapcode(char, a, b, n)
221 }
222}
223
224#define swap(a, b) swapfunc(a, b, (size_t) es, (size_t) swaptype)
225
226// this switch exists because we needed to clean up
227// the swap() macro, and vecswap has a different 0 meaning.
228// To force the right swapfunc behavior we force it to 1 if it's
229#define vecswap(a, b, n) \
230 if ((n) > 0) \
231 swapfunc(a, b, n, swaptype ? (size_t) swaptype : 1)
232
233#ifdef I_AM_QSORT_R
234#define CMP(t, x, y) (cmp((t), (x), (y)))
235#else
236#define CMP(t, x, y) (cmp((x), (y)))
237#endif
238
239static inline char *med3(char *a, char *b, char *c, cmp_t *cmp,
240 void *thunk
241#ifndef I_AM_QSORT_R
242 __attribute__((unused))
243#endif
244) {
245 return CMP(thunk, a, b) < 0
246 ? (CMP(thunk, b, c) < 0 ? b : (CMP(thunk, a, c) < 0 ? c : a))
247 : (CMP(thunk, b, c) > 0 ? b : (CMP(thunk, a, c) < 0 ? a : c));
248}
249int flsl_like(unsigned long x) {
250 if (x == 0)
251 return 0;
252 return (sizeof(x) * 8) - __builtin_clzl(x);
253}
254
255#ifdef __LP64__
256#define DEPTH(x) (2 * flsl_like(((long) (x)) - 1))
257#else /* !__LP64__ */
258#define DEPTH(x) (2 * (fls((int) (x)) - 1))
259#endif /* __LP64__ */
260
261static void _qsort(void *a, size_t n, size_t es,
262#ifdef I_AM_QSORT_R
263 void *thunk,
264#else
265#define thunk NULL
266#endif
267 cmp_t *cmp, int depth_limit) {
268 char *pa;
269 char *pb;
270 char *pc;
271 char *pd;
272 char *pl;
273 char *pm;
274 char *pn;
275 size_t d;
276 size_t r;
277 size_t swap_cnt;
278 int cmp_result;
279 int swaptype;
280
281loop:
282 if (depth_limit-- <= 0) {
283#ifdef I_AM_QSORT_R
284 heapsort_r(a, n, es, thunk, cmp);
285#else
286 heapsort(vbase: a, nmemb: n, size: es, compar: cmp);
287#endif
288 return;
289 }
290 SWAPINIT(a, es);
291 swap_cnt = 0;
292 if (n < 7) {
293 for (pm = (char *) a + es; pm < (char *) a + n * es; pm += es) {
294 for (pl = pm; pl > (char *) a && CMP(thunk, pl - es, pl) > 0;
295 pl -= es) {
296 swap(pl, pl - es);
297 }
298 }
299 return;
300 }
301 pm = (char *) a + (n / 2) * es;
302 if (n > 7) {
303 pl = a;
304 pn = (char *) a + (n - 1) * es;
305 if (n > 40) {
306 d = (n / 8) * es;
307 pl = med3(a: pl, b: pl + d, c: pl + 2 * d, cmp, thunk);
308 pm = med3(a: pm - d, b: pm, c: pm + d, cmp, thunk);
309 pn = med3(a: pn - 2 * d, b: pn - d, c: pn, cmp, thunk);
310 }
311 pm = med3(a: pl, b: pm, c: pn, cmp, thunk);
312 }
313 swap(a, (void *) pm);
314 pa = pb = (char *) a + es;
315
316 pc = pd = (char *) a + (n - 1) * es;
317 while (true) {
318 while (pb <= pc && (cmp_result = CMP(thunk, pb, a)) <= 0) {
319 if (cmp_result == 0) {
320 swap_cnt = 1;
321 swap(pa, pb);
322 pa += es;
323 }
324 pb += es;
325 }
326 while (pb <= pc && (cmp_result = CMP(thunk, pc, a)) >= 0) {
327 if (cmp_result == 0) {
328 swap_cnt = 1;
329 swap(pc, pd);
330 pd -= es;
331 }
332 pc -= es;
333 }
334 if (pb > pc) {
335 break;
336 }
337 swap(pb, pc);
338 swap_cnt = 1;
339 pb += es;
340 pc -= es;
341 }
342
343 pn = (char *) a + n * es;
344 r = min((uintptr_t) pa - (uintptr_t) a, (uintptr_t) pb - (uintptr_t) pa);
345 vecswap(a, pb - r, r);
346 r = min((uintptr_t) pd - (uintptr_t) pc,
347 (uintptr_t) pn - (uintptr_t) pd - (uintptr_t) es);
348 vecswap(pb, pn - r, r);
349
350 if (swap_cnt == 0) { /* Switch to insertion sort */
351 r = 1 + n / 4; /* n >= 7, so r >= 2 */
352 for (pm = (char *) a + es; pm < (char *) a + n * es; pm += es) {
353 for (pl = pm; pl > (char *) a && CMP(thunk, pl - es, pl) > 0;
354 pl -= es) {
355 swap(pl, pl - es);
356 if (++swap_cnt > r) {
357 goto nevermind;
358 }
359 }
360 }
361 return;
362 }
363
364nevermind:
365 if ((r = (uintptr_t) pb - (uintptr_t) pa) > es) {
366#ifdef I_AM_QSORT_R
367 _qsort(a, r / es, es, thunk, cmp, depth_limit);
368#else
369 _qsort(a, n: r / es, es, cmp, depth_limit);
370#endif
371 }
372 if ((r = (uintptr_t) pd - (uintptr_t) pc) > es) {
373 /* Iterate rather than recurse to save stack space */
374 a = pn - r;
375 n = r / es;
376 goto loop;
377 }
378 /* qsort(pn - r, r / es, es, cmp);*/
379}
380
381void
382#ifdef I_AM_QSORT_R
383qsort_r(void *a, size_t n, size_t es, void *thunk, cmp_t *cmp)
384#else
385qsort(void *a, size_t n, size_t es, cmp_t *cmp)
386#endif
387{
388 _qsort(a, n, es,
389#ifdef I_AM_QSORT_R
390 thunk,
391#endif
392 cmp, DEPTH(n));
393}
394