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