1 files changed, 222 insertions, 0 deletions
diff --git a/firmware/libc/qsort.c b/firmware/libc/qsort.c
new file mode 100644
index 0000000000..8c4d1ad511
--- /dev/null
+++ b/firmware/libc/qsort.c
@@ -0,0 +1,222 @@
+/*
+FUNCTION
+<<qsort>>---sort an array
+INDEX
+        qsort
+ANSI_SYNOPSIS
+        #include <stdlib.h>
+        void qsort(void *<[base]>, size_t <[nmemb]>, size_t <[size]>,
+                   int (*<[compar]>)(const void *, const void *) );
+TRAD_SYNOPSIS
+        #include <stdlib.h>
+        qsort(<[base]>, <[nmemb]>, <[size]>, <[compar]> )
+        char *<[base]>;
+        size_t <[nmemb]>;
+        size_t <[size]>;
+        int (*<[compar]>)();
+DESCRIPTION
+<<qsort>> sorts an array (beginning at <[base]>) of <[nmemb]> objects.
+<[size]> describes the size of each element of the array.
+You must supply a pointer to a comparison function, using the argument
+shown as <[compar]>.  (This permits sorting objects of unknown
+properties.)  Define the comparison function to accept two arguments,
+each a pointer to an element of the array starting at <[base]>.  The
+result of <<(*<[compar]>)>> must be negative if the first argument is
+less than the second, zero if the two arguments match, and positive if
+the first argument is greater than the second (where ``less than'' and
+``greater than'' refer to whatever arbitrary ordering is appropriate).
+The array is sorted in place; that is, when <<qsort>> returns, the
+array elements beginning at <[base]> have been reordered.
+RETURNS
+<<qsort>> does not return a result.
+PORTABILITY
+<<qsort>> is required by ANSI (without specifying the sorting algorithm).
+*/
+/*-
+ * 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. All advertising materials mentioning features or use of this software
+ *    must display the following acknowledgement:
+ *      This product includes software developed by the University of
+ *      California, Berkeley and its contributors.
+ * 4. 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.
+ */
+#include <_ansi.h>
+#include <stdlib.h>
+#ifndef __GNUC__
+#define inline
+#endif
+static inline char      *med3 _PARAMS((char *, char *, char *, int (*cmp)(const _PTR,const _PTR)));
+static inline void       swapfunc _PARAMS((char *, char *, int, int));
+#define min(a, b)       (a) < (b) ? a : b
+/*
+ * Qsort routine from Bentley & McIlroy's "Engineering a Sort Function".
+ */
+#define swapcode(TYPE, parmi, parmj, n) {               \
+        long i = (n) / sizeof (TYPE);                   \
+        register TYPE *pi = (TYPE *) (parmi);           \
+        register TYPE *pj = (TYPE *) (parmj);           \
+        do {                                            \
+                register TYPE   t = *pi;                \
+                *pi++ = *pj;                            \
+                *pj++ = t;                              \
+        } while (--i > 0);                              \
+}
+#define SWAPINIT(a, es) swaptype = ((char *)a - (char *)0) % sizeof(long) || \
+        es % sizeof(long) ? 2 : es == sizeof(long)? 0 : 1;
+static inline void
+_DEFUN(swapfunc, (a, b, n, swaptype),
+        char *a _AND
+        char *b _AND
+        int n _AND
+        int swaptype)
+{
+        if(swaptype <= 1) 
+                swapcode(long, a, b, n)
+        else
+                swapcode(char, a, b, n)
+}
+#define swap(a, b)                                      \
+        if (swaptype == 0) {                            \
+                long t = *(long *)(a);                  \
+                *(long *)(a) = *(long *)(b);            \
+                *(long *)(b) = t;                       \
+        } else                                          \
+                swapfunc(a, b, es, swaptype)
+#define vecswap(a, b, n)        if ((n) > 0) swapfunc(a, b, n, swaptype)
+static inline char *
+_DEFUN(med3, (a, b, c, cmp),
+        char *a _AND
+        char *b _AND
+        char *c _AND
+        int (*cmp)(const _PTR,const _PTR))
+{
+        return cmp(a, b) < 0 ?
+               (cmp(b, c) < 0 ? b : (cmp(a, c) < 0 ? c : a ))
+              :(cmp(b, c) > 0 ? b : (cmp(a, c) < 0 ? a : c ));
+}
+void
+_DEFUN(qsort, (a, n, es, cmp),
+        void *a _AND
+        size_t n _AND
+        size_t es _AND
+        int (*cmp)(const _PTR,const _PTR))
+{
+        char *pa, *pb, *pc, *pd, *pl, *pm, *pn;
+        int d, r, swaptype, swap_cnt;
+loop:   SWAPINIT(a, es);
+        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(pl - es, pl) > 0;
+                             pl -= es)
+                                swap(pl, pl - es);
+                return;
+        }
+        pm = (char *) a + (n / 2) * es;
+        if (n > 7) {
+                pl = a;
+                pn = (char *) a + (n - 1) * es;
+                if (n > 40) {
+                        d = (n / 8) * es;
+                        pl = med3(pl, pl + d, pl + 2 * d, cmp);
+                        pm = med3(pm - d, pm, pm + d, cmp);
+                        pn = med3(pn - 2 * d, pn - d, pn, cmp);
+                }
+                pm = med3(pl, pm, pn, cmp);
+        }
+        swap(a, pm);
+        pa = pb = (char *) a + es;
+        pc = pd = (char *) a + (n - 1) * es;
+        for (;;) {
+                while (pb <= pc && (r = cmp(pb, a)) <= 0) {
+                        if (r == 0) {
+                                swap_cnt = 1;
+                                swap(pa, pb);
+                                pa += es;
+                        }
+                        pb += es;
+                }
+                while (pb <= pc && (r = cmp(pc, a)) >= 0) {
+                        if (r == 0) {
+                                swap_cnt = 1;
+                                swap(pc, pd);
+                                pd -= es;
+                        }
+                        pc -= es;
+                }
+                if (pb > pc)
+                        break;
+                swap(pb, pc);
+                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(pl - es, pl) > 0; 
+                             pl -= es)
+                                swap(pl, pl - es);
+                return;
+        }
+        pn = (char *) a + n * es;
+        r = min(pa - (char *)a, pb - pa);
+        vecswap(a, pb - r, r);
+        r = min((unsigned int)(pd - pc), pn - pd - es);
+        vecswap(pb, pn - r, r);
+        if ((unsigned int)(r = pb - pa) > es)
+                qsort(a, r / es, es, cmp);
+        if ((unsigned int)(r = pd - pc) > es) { 
+                /* Iterate rather than recurse to save stack space */
+                a = pn - r;
+                n = r / es;
+                goto loop;
+        }
+/*              qsort(pn - r, r / es, es, cmp);*/
+}

diff --git a/firmware/libc/qsort.c b/firmware/libc/qsort.c new file mode 100644 index 0000000000..8c4d1ad511 --- /dev/null +++ b/firmware/libc/qsort.c
@@ -0,0 +1,222 @@
	1	/*
	2	FUNCTION
	3	<<qsort>>---sort an array
	4
	5	INDEX
	6	qsort
	7
	8	ANSI_SYNOPSIS
	9	#include <stdlib.h>
	10	void qsort(void *<[base]>, size_t <[nmemb]>, size_t <[size]>,
	11	int (<[compar]>)(const void , const void *) );
	12
	13	TRAD_SYNOPSIS
	14	#include <stdlib.h>
	15	qsort(<[base]>, <[nmemb]>, <[size]>, <[compar]> )
	16	char *<[base]>;
	17	size_t <[nmemb]>;
	18	size_t <[size]>;
	19	int (*<[compar]>)();
	20
	21	DESCRIPTION
	22	<<qsort>> sorts an array (beginning at <[base]>) of <[nmemb]> objects.
	23	<[size]> describes the size of each element of the array.
	24
	25	You must supply a pointer to a comparison function, using the argument
	26	shown as <[compar]>. (This permits sorting objects of unknown
	27	properties.) Define the comparison function to accept two arguments,
	28	each a pointer to an element of the array starting at <[base]>. The
	29	result of <<(*<[compar]>)>> must be negative if the first argument is
	30	less than the second, zero if the two arguments match, and positive if
	31	the first argument is greater than the second (where ``less than'' and
	32	``greater than'' refer to whatever arbitrary ordering is appropriate).
	33
	34	The array is sorted in place; that is, when <<qsort>> returns, the
	35	array elements beginning at <[base]> have been reordered.
	36
	37	RETURNS
	38	<<qsort>> does not return a result.
	39
	40	PORTABILITY
	41	<<qsort>> is required by ANSI (without specifying the sorting algorithm).
	42	*/
	43
	44	/*-
	45	* Copyright (c) 1992, 1993
	46	* The Regents of the University of California. All rights reserved.
	47	*
	48	* Redistribution and use in source and binary forms, with or without
	49	* modification, are permitted provided that the following conditions
	50	* are met:
	51	* 1. Redistributions of source code must retain the above copyright
	52	* notice, this list of conditions and the following disclaimer.
	53	* 2. Redistributions in binary form must reproduce the above copyright
	54	* notice, this list of conditions and the following disclaimer in the
	55	* documentation and/or other materials provided with the distribution.
	56	* 3. All advertising materials mentioning features or use of this software
	57	* must display the following acknowledgement:
	58	* This product includes software developed by the University of
	59	* California, Berkeley and its contributors.
	60	* 4. Neither the name of the University nor the names of its contributors
	61	* may be used to endorse or promote products derived from this software
	62	* without specific prior written permission.
	63	*
	64	* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
	65	* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	66	* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	67	* ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
	68	* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
	69	* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
	70	* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
	71	* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
	72	* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
	73	* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
	74	* SUCH DAMAGE.
	75	*/
	76
	77	#include <_ansi.h>
	78	#include <stdlib.h>
	79
	80	#ifndef __GNUC__
	81	#define inline
	82	#endif
	83
	84	static inline char med3 _PARAMS((char , char , char , int (*cmp)(const _PTR,const _PTR)));
	85	static inline void swapfunc _PARAMS((char , char , int, int));
	86
	87	#define min(a, b) (a) < (b) ? a : b
	88
	89	/*
	90	* Qsort routine from Bentley & McIlroy's "Engineering a Sort Function".
	91	*/
	92	#define swapcode(TYPE, parmi, parmj, n) { \
	93	long i = (n) / sizeof (TYPE); \
	94	register TYPE pi = (TYPE ) (parmi); \
	95	register TYPE pj = (TYPE ) (parmj); \
	96	do { \
	97	register TYPE t = *pi; \
	98	pi++ = pj; \
	99	*pj++ = t; \
	100	} while (--i > 0); \
	101	}
	102
	103	#define SWAPINIT(a, es) swaptype = ((char )a - (char )0) % sizeof(long) \|\| \
	104	es % sizeof(long) ? 2 : es == sizeof(long)? 0 : 1;
	105
	106	static inline void
	107	_DEFUN(swapfunc, (a, b, n, swaptype),
	108	char *a _AND
	109	char *b _AND
	110	int n _AND
	111	int swaptype)
	112	{
	113	if(swaptype <= 1)
	114	swapcode(long, a, b, n)
	115	else
	116	swapcode(char, a, b, n)
	117	}
	118
	119	#define swap(a, b) \
	120	if (swaptype == 0) { \
	121	long t = (long )(a); \
	122	(long )(a) = (long )(b); \
	123	(long )(b) = t; \
	124	} else \
	125	swapfunc(a, b, es, swaptype)
	126
	127	#define vecswap(a, b, n) if ((n) > 0) swapfunc(a, b, n, swaptype)
	128
	129	static inline char *
	130	_DEFUN(med3, (a, b, c, cmp),
	131	char *a _AND
	132	char *b _AND
	133	char *c _AND
	134	int (*cmp)(const _PTR,const _PTR))
	135	{
	136	return cmp(a, b) < 0 ?
	137	(cmp(b, c) < 0 ? b : (cmp(a, c) < 0 ? c : a ))
	138	:(cmp(b, c) > 0 ? b : (cmp(a, c) < 0 ? a : c ));
	139	}
	140
	141	void
	142	_DEFUN(qsort, (a, n, es, cmp),
	143	void *a _AND
	144	size_t n _AND
	145	size_t es _AND
	146	int (*cmp)(const _PTR,const _PTR))
	147	{
	148	char pa, pb, pc, pd, pl, pm, *pn;
	149	int d, r, swaptype, swap_cnt;
	150
	151	loop: SWAPINIT(a, es);
	152	swap_cnt = 0;
	153	if (n < 7) {
	154	for (pm = (char ) a + es; pm < (char ) a + n * es; pm += es)
	155	for (pl = pm; pl > (char *) a && cmp(pl - es, pl) > 0;
	156	pl -= es)
	157	swap(pl, pl - es);
	158	return;
	159	}
	160	pm = (char ) a + (n / 2) es;
	161	if (n > 7) {
	162	pl = a;
	163	pn = (char ) a + (n - 1) es;
	164	if (n > 40) {
	165	d = (n / 8) * es;
	166	pl = med3(pl, pl + d, pl + 2 * d, cmp);
	167	pm = med3(pm - d, pm, pm + d, cmp);
	168	pn = med3(pn - 2 * d, pn - d, pn, cmp);
	169	}
	170	pm = med3(pl, pm, pn, cmp);
	171	}
	172	swap(a, pm);
	173	pa = pb = (char *) a + es;
	174
	175	pc = pd = (char ) a + (n - 1) es;
	176	for (;;) {
	177	while (pb <= pc && (r = cmp(pb, a)) <= 0) {
	178	if (r == 0) {
	179	swap_cnt = 1;
	180	swap(pa, pb);
	181	pa += es;
	182	}
	183	pb += es;
	184	}
	185	while (pb <= pc && (r = cmp(pc, a)) >= 0) {
	186	if (r == 0) {
	187	swap_cnt = 1;
	188	swap(pc, pd);
	189	pd -= es;
	190	}
	191	pc -= es;
	192	}
	193	if (pb > pc)
	194	break;
	195	swap(pb, pc);
	196	swap_cnt = 1;
	197	pb += es;
	198	pc -= es;
	199	}
	200	if (swap_cnt == 0) { /* Switch to insertion sort */
	201	for (pm = (char ) a + es; pm < (char ) a + n * es; pm += es)
	202	for (pl = pm; pl > (char *) a && cmp(pl - es, pl) > 0;
	203	pl -= es)
	204	swap(pl, pl - es);
	205	return;
	206	}
	207
	208	pn = (char ) a + n es;
	209	r = min(pa - (char *)a, pb - pa);
	210	vecswap(a, pb - r, r);
	211	r = min((unsigned int)(pd - pc), pn - pd - es);
	212	vecswap(pb, pn - r, r);
	213	if ((unsigned int)(r = pb - pa) > es)
	214	qsort(a, r / es, es, cmp);
	215	if ((unsigned int)(r = pd - pc) > es) {
	216	/* Iterate rather than recurse to save stack space */
	217	a = pn - r;
	218	n = r / es;
	219	goto loop;
	220	}
	221	/* qsort(pn - r, r / es, es, cmp);*/
	222	}