1/*-
2 * Copyright (c) 2002 Jonathan Mini (mini@freebsd.org).
3 * 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 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 *
26 * $FreeBSD$
27 */
28
29#include <sys/types.h>
30#include <sys/signal.h>
31#include <sys/signalvar.h>
32#include <sys/sysctl.h>
33#include <sys/kse.h>
34#include <sys/ucontext.h>
35
36#include <stdarg.h>
37#include <stddef.h>
38#include <stdlib.h>
39#include <string.h>
40#include <sysexits.h>
41#include <time.h>
42#include <unistd.h>
43#include "simplelock.h"
44
45#undef TRACE_UTS
46//#define TRACE_KSE
47
48#ifdef TRACE_UTS
49#define	UPFMT(fmt...)	pfmt(#fmt)
50#define	UPSTR(s)	pstr(s)
51#define	UPCHAR(c)	pchar(c)
52#else
53#define	UPFMT(fmt...)	/* Nothing. */
54#define	UPSTR(s)	/* Nothing. */
55#define	UPCHAR(c)	/* Nothing. */
56#endif
57
58#define MAIN_STACK_SIZE			(1024 * 1024)
59#define THREAD_STACK_SIZE		(32 * 1024)
60
61struct uts_runq {
62	struct kse_thr_mailbox	*head;
63	struct simplelock	lock;
64};
65
66struct uts_data {
67	struct kse_mailbox	mb;
68	struct uts_runq		*runq;
69	struct kse_thr_mailbox	*cur_thread;
70};
71
72static struct uts_runq runq1;
73static struct uts_data data1, data2;
74static struct uts_runq runq2;
75static struct uts_data data3, data4;
76static struct kse_thr_mailbox	*aa;
77
78#ifdef TRACE_UTS
79static int progress = 0;
80#endif
81
82static void	init_uts(struct uts_data *data, struct uts_runq *q);
83static void	start_uts(struct uts_data *data, int newgrp);
84static void	enter_uts(struct uts_data *);
85static void	pchar(char c);
86static void	pfmt(const char *fmt, ...);
87static void	pstr(const char *s);
88static void	runq_init(struct uts_runq *q);
89static void	runq_insert(struct uts_runq *q, struct kse_thr_mailbox *tm);
90static struct	kse_thr_mailbox *runq_remove(struct uts_runq *q);
91static struct	kse_thr_mailbox *runq_remove_nolock(struct uts_runq *q);
92static void	thread_start(struct uts_data *data, const void *func, int arg);
93static void	uts(struct kse_mailbox *km);
94
95/* Functions implemented in assembly */
96extern int	uts_to_thread(struct kse_thr_mailbox *tdp,
97			struct kse_thr_mailbox **curthreadp);
98extern int	thread_to_uts(struct kse_thr_mailbox *tm,
99			struct kse_mailbox *km);
100
101static void
102nano(int len)
103{
104	struct timespec time_to_sleep;
105	struct timespec time_remaining;
106
107	time_to_sleep.tv_sec = 0;
108	time_to_sleep.tv_nsec = len * 10000;
109	nanosleep(&time_to_sleep, &time_remaining);
110}
111
112void
113aaaa(int c)
114{
115	for (;;) {
116		pchar(c);
117		nano(1);
118	}
119}
120
121static void
122foof(int sig)
123{
124	pfmt("\n[%d]\n", sig);
125//	thread_start(aaaa, '0' + progress++);
126}
127
128static void
129newkse(int v)
130{
131	start_uts(&data4, 0);
132}
133
134#if 0
135void
136spin(int arg)
137{
138	for (;;) enter_uts(); sched_yield();
139}
140#endif
141/*
142 * Test Userland Thread Scheduler (UTS) suite for KSE.
143 */
144int
145main(void)
146{
147	int i;
148
149	runq_init(&runq1);
150	init_uts(&data1, &runq1);
151	init_uts(&data2, &runq1);
152	thread_start(&data1, aaaa, '+');
153	thread_start(&data1, aaaa, '-');
154	start_uts(&data1, 0);
155	start_uts(&data2, 0);
156
157//	start second ksegrp
158	runq_init(&runq2);
159	init_uts(&data3, &runq2);
160	init_uts(&data4, &runq2);
161	thread_start(&data3, newkse, 0);
162	thread_start(&data3, aaaa, '*');
163	thread_start(&data3, aaaa, '.');
164	start_uts(&data3, 1);
165
166	for (i = 0;1;i++) {
167//		if (i < 1000)
168//			thread_start(aaaa, 'a' + (i % 26));
169		pchar('A' + (i % 26));
170		nano(5);
171	}
172	pstr("\n** main() exiting **\n");
173	return (EX_OK);
174}
175
176
177/*
178 * Enter the UTS from a thread.
179 */
180static void
181enter_uts(struct uts_data *data)
182{
183	struct kse_thr_mailbox	*td;
184
185	/* XXX: We should atomically exchange these two. */
186	td = data->mb.km_curthread;
187	data->mb.km_curthread = NULL;
188
189	thread_to_uts(td, &data->mb);
190}
191
192/*
193 * Initialise threading.
194 */
195static void
196init_uts(struct uts_data *data, struct uts_runq *q)
197{
198	struct kse_thr_mailbox *tm;
199	int mib[2];
200	char	*p;
201#if 0
202	size_t len;
203#endif
204
205	/*
206	 * Create initial thread.
207	 */
208	tm = (struct kse_thr_mailbox *)calloc(1, sizeof(struct kse_thr_mailbox));
209
210	/* Throw us into its context. */
211	getcontext(&tm->tm_context);
212
213	/* Find our stack. */
214	mib[0] = CTL_KERN;
215	mib[1] = KERN_USRSTACK;
216#if 0
217	len = sizeof(p);
218	if (sysctl(mib, 2, &p, &len, NULL, 0) == -1)
219		pstr("sysctl(CTL_KER.KERN_USRSTACK) failed.\n");
220#endif
221	p = (char *)malloc(MAIN_STACK_SIZE) + MAIN_STACK_SIZE;
222	pfmt("main() : 0x%x\n", tm);
223	pfmt("eip -> 0x%x\n", tm->tm_context.uc_mcontext.mc_eip);
224	tm->tm_context.uc_stack.ss_sp = p - MAIN_STACK_SIZE;
225	tm->tm_context.uc_stack.ss_size = MAIN_STACK_SIZE;
226
227	/*
228	 * Create KSE mailbox.
229	 */
230	p = (char *)malloc(THREAD_STACK_SIZE);
231	bzero(&data->mb, sizeof(struct kse_mailbox));
232	data->mb.km_stack.ss_sp = p;
233	data->mb.km_stack.ss_size = THREAD_STACK_SIZE;
234	data->mb.km_func = (void *)uts;
235	data->mb.km_udata = data;
236	data->cur_thread = tm;
237	data->runq = q;
238	pfmt("uts() at : 0x%x\n", uts);
239	pfmt("uts stack at : 0x%x - 0x%x\n", p, p + THREAD_STACK_SIZE);
240}
241
242static void
243start_uts(struct uts_data *data, int newgrp)
244{
245	/*
246	 * Start KSE scheduling.
247	 */
248	pfmt("kse_create() -> %d\n", kse_create(&data->mb, newgrp));
249	data->mb.km_curthread = data->cur_thread;
250
251	/*
252	 * Arrange to deliver signals via KSE.
253	 */
254	signal(SIGURG, foof);
255}
256
257/*
258 * Write a single character to stdout, in a thread-safe manner.
259 */
260static void
261pchar(char c)
262{
263
264	write(STDOUT_FILENO, &c, 1);
265}
266
267/*
268 * Write formatted output to stdout, in a thread-safe manner.
269 *
270 * Recognises the following conversions:
271 *	%c	-> char
272 *	%d	-> signed int (base 10)
273 *	%s	-> string
274 *	%u	-> unsigned int (base 10)
275 *	%x	-> unsigned int (base 16)
276 */
277static void
278pfmt(const char *fmt, ...)
279{
280	static const char digits[16] = "0123456789abcdef";
281	va_list	 ap;
282	char buf[10];
283	char *s;
284	unsigned r, u;
285	int c, d;
286
287	va_start(ap, fmt);
288	while ((c = *fmt++)) {
289		if (c == '%') {
290			c = *fmt++;
291			switch (c) {
292			case 'c':
293				pchar(va_arg(ap, int));
294				continue;
295			case 's':
296				pstr(va_arg(ap, char *));
297				continue;
298			case 'd':
299			case 'u':
300			case 'x':
301				r = ((c == 'u') || (c == 'd')) ? 10 : 16;
302				if (c == 'd') {
303					d = va_arg(ap, unsigned);
304					if (d < 0) {
305						pchar('-');
306						u = (unsigned)(d * -1);
307					} else
308						u = (unsigned)d;
309				} else
310					u = va_arg(ap, unsigned);
311				s = buf;
312				do {
313					*s++ = digits[u % r];
314				} while (u /= r);
315				while (--s >= buf)
316					pchar(*s);
317				continue;
318			}
319		}
320		pchar(c);
321	}
322	va_end(ap);
323}
324
325static void
326pstr(const char *s)
327{
328
329	write(STDOUT_FILENO, s, strlen(s));
330}
331
332static void
333runq_init(struct uts_runq *q)
334{
335	q->head = NULL;
336	simplelock_init(&q->lock);
337}
338
339/*
340 * Insert a thread into the run queue.
341 */
342static void
343runq_insert(struct uts_runq *q, struct kse_thr_mailbox *tm)
344{
345	simplelock_lock(&q->lock);
346	tm->tm_next = q->head;
347	q->head = tm;
348	simplelock_unlock(&q->lock);
349}
350
351/*
352 * Select and remove a thread from the run queue.
353 */
354static struct kse_thr_mailbox *
355runq_remove(struct uts_runq *q)
356{
357	struct kse_thr_mailbox *tm;
358
359	simplelock_lock(&q->lock);
360	tm = runq_remove_nolock(q);
361	simplelock_unlock(&q->lock);
362	return tm;
363}
364
365static struct kse_thr_mailbox *
366runq_remove_nolock(struct uts_runq *q)
367{
368	struct kse_thr_mailbox *p, *p1;
369
370	if (q->head == NULL)
371		return (NULL);
372	p1 = NULL;
373	for (p = q->head; p->tm_next != NULL; p = p->tm_next)
374		p1 = p;
375	if (p1 == NULL)
376		q->head = NULL;
377	else
378		p1->tm_next = NULL;
379	return (p);
380}
381
382/*
383 * Userland thread scheduler.
384 */
385static void
386uts(struct kse_mailbox *km)
387{
388#ifdef TRACE_KSE
389	static struct uts_data *prev_data;
390#endif
391	struct kse_thr_mailbox *tm, *p;
392	struct uts_data *data;
393	int i;
394
395	UPSTR("\n--uts() start--\n");
396	UPFMT("mailbox -> %x\n", km);
397
398	/*
399	 * Insert any processes back from being blocked
400	 * in the kernel into the run queue.
401	 */
402	data = km->km_udata;
403	p = km->km_completed;
404	km->km_completed = NULL;
405	UPFMT("km_completed -> 0x%x", p);
406#ifdef TRACE_KSE
407	if (data != prev_data) {
408		prev_data = data;
409		pfmt("uts data: 0x%x\n", data);
410	}
411#endif
412	while ((tm = p) != NULL) {
413		p = tm->tm_next;
414		UPFMT(" 0x%x", p);
415		runq_insert(data->runq, tm);
416	}
417	UPCHAR('\n');
418
419	simplelock_lock(&data->runq->lock);
420	/*
421	 * Process any signals we've received (but only if we have
422	 * somewhere to deliver them to).
423	 */
424	if ((data->runq->head != NULL) && SIGNOTEMPTY(km->km_sigscaught)) {
425		for (i = 0;i < _SIG_MAXSIG;i++)
426			if (SIGISMEMBER(km->km_sigscaught, i)) {
427				signalcontext(&data->runq->head->tm_context,
428				 i, foof);
429				break;
430			}
431		bzero(&km->km_sigscaught, sizeof(sigset_t));
432	}
433
434	/*
435	 * Pull a thread off the run queue.
436	 */
437	p = runq_remove_nolock(data->runq);
438	simplelock_unlock(&data->runq->lock);
439#if 0
440	if ((p == aa) && (progress > 0)) {
441		--progress;
442		signalcontext(&p->tm_context, 1, foof);
443	}
444#endif
445
446	/*
447	 * Either schedule a thread, or idle if none ready to run.
448	 */
449	if (p != NULL) {
450		UPFMT("\n-- uts() scheduling 0x%x--\n", p);
451		UPFMT("eip -> 0x%x progress -> %d\n",
452		    p->tm_context.uc_mcontext.mc_eip, progress);
453		UPSTR("curthread set\n");
454		uts_to_thread(p, &km->km_curthread);
455		UPSTR("\n-- uts_to_thread() failed --\n");
456	}
457	kse_release(NULL);
458	pstr("** uts() exiting **\n");
459	exit(EX_SOFTWARE);
460}
461
462/*
463 * Start a thread.
464 */
465static struct kse_thr_mailbox *
466thread_create(const void *func, int arg)
467{
468	struct kse_thr_mailbox *tm;
469	char *p;
470
471	aa = tm = (struct kse_thr_mailbox *)calloc(1, sizeof(struct kse_thr_mailbox));
472	getcontext(&tm->tm_context);
473	p = (char *)malloc(THREAD_STACK_SIZE);
474	tm->tm_context.uc_stack.ss_sp = p;
475	tm->tm_context.uc_stack.ss_size = THREAD_STACK_SIZE;
476	makecontext(&tm->tm_context, func, 2, arg);
477	// setcontext(&tm->tm_context);
478	return tm;
479}
480
481static void
482thread_start(struct uts_data *data, const void *func, int arg)
483{
484	struct kse_thr_mailbox *tm;
485	struct kse_thr_mailbox *tm2;
486
487	tm = thread_create(func, arg);
488	tm2 = thread_create(enter_uts, (int)data);
489	tm->tm_context.uc_link = &tm2->tm_context;
490	runq_insert(data->runq, tm);
491	pfmt("thread_start() : 0x%x %x\n", tm, &tm->tm_context);
492}
493