1103581Smini/*- 2103581Smini * Copyright (c) 2002 Jonathan Mini (mini@freebsd.org). 3103581Smini * All rights reserved. 4103581Smini * 5103581Smini * Redistribution and use in source and binary forms, with or without 6103581Smini * modification, are permitted provided that the following conditions 7103581Smini * are met: 8103581Smini * 1. Redistributions of source code must retain the above copyright 9103581Smini * notice, this list of conditions and the following disclaimer. 10103581Smini * 2. Redistributions in binary form must reproduce the above copyright 11103581Smini * notice, this list of conditions and the following disclaimer in the 12103581Smini * documentation and/or other materials provided with the distribution. 13103581Smini * 14103581Smini * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15103581Smini * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16103581Smini * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17103581Smini * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18103581Smini * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19103581Smini * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20103581Smini * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21103581Smini * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22103581Smini * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23103581Smini * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24103581Smini * SUCH DAMAGE. 25103581Smini * 2699073Sjulian * $FreeBSD: releng/10.3/tools/KSE/ksetest/kse_threads_test.c 228975 2011-12-30 00:04:11Z uqs $ 2799073Sjulian */ 2899073Sjulian 29103581Smini#include <sys/types.h> 30103581Smini#include <sys/signal.h> 31103581Smini#include <sys/signalvar.h> 32103581Smini#include <sys/sysctl.h> 3399073Sjulian#include <sys/kse.h> 34103581Smini#include <sys/ucontext.h> 3599073Sjulian 36103581Smini#include <stdarg.h> 37103581Smini#include <stddef.h> 38103581Smini#include <stdlib.h> 39103581Smini#include <string.h> 40103581Smini#include <sysexits.h> 41103581Smini#include <time.h> 42103581Smini#include <unistd.h> 43103840Sjulian#include "simplelock.h" 4499073Sjulian 45103581Smini#undef TRACE_UTS 46103840Sjulian//#define TRACE_KSE 4799073Sjulian 48103581Smini#ifdef TRACE_UTS 49103581Smini#define UPFMT(fmt...) pfmt(#fmt) 50103581Smini#define UPSTR(s) pstr(s) 51103581Smini#define UPCHAR(c) pchar(c) 52103581Smini#else 53103581Smini#define UPFMT(fmt...) /* Nothing. */ 54103581Smini#define UPSTR(s) /* Nothing. */ 55103581Smini#define UPCHAR(c) /* Nothing. */ 56103581Smini#endif 5799073Sjulian 58103581Smini#define MAIN_STACK_SIZE (1024 * 1024) 59103581Smini#define THREAD_STACK_SIZE (32 * 1024) 6099073Sjulian 61103840Sjulianstruct uts_runq { 62103973Sarchie struct kse_thr_mailbox *head; 63103840Sjulian struct simplelock lock; 64103840Sjulian}; 65103840Sjulian 66103840Sjulianstruct uts_data { 67103840Sjulian struct kse_mailbox mb; 68103840Sjulian struct uts_runq *runq; 69103973Sarchie struct kse_thr_mailbox *cur_thread; 70103840Sjulian}; 71103840Sjulian 72103840Sjulianstatic struct uts_runq runq1; 73103840Sjulianstatic struct uts_data data1, data2; 74103840Sjulianstatic struct uts_runq runq2; 75103840Sjulianstatic struct uts_data data3, data4; 76103973Sarchiestatic struct kse_thr_mailbox *aa; 7799073Sjulian 78104380Sarchie#ifdef TRACE_UTS 79103581Sministatic int progress = 0; 80104380Sarchie#endif 8199073Sjulian 82103840Sjulianstatic void init_uts(struct uts_data *data, struct uts_runq *q); 83103840Sjulianstatic void start_uts(struct uts_data *data, int newgrp); 84103840Sjulianstatic void enter_uts(struct uts_data *); 85103581Sministatic void pchar(char c); 86103581Sministatic void pfmt(const char *fmt, ...); 87103581Sministatic void pstr(const char *s); 88103840Sjulianstatic void runq_init(struct uts_runq *q); 89103973Sarchiestatic void runq_insert(struct uts_runq *q, struct kse_thr_mailbox *tm); 90104380Sarchiestatic struct kse_thr_mailbox *runq_remove(struct uts_runq *q); 91104380Sarchiestatic struct kse_thr_mailbox *runq_remove_nolock(struct uts_runq *q); 92103840Sjulianstatic void thread_start(struct uts_data *data, const void *func, int arg); 93103581Sministatic void uts(struct kse_mailbox *km); 9499073Sjulian 95104380Sarchie/* Functions implemented in assembly */ 96104380Sarchieextern int uts_to_thread(struct kse_thr_mailbox *tdp, 97104380Sarchie struct kse_thr_mailbox **curthreadp); 98104380Sarchieextern int thread_to_uts(struct kse_thr_mailbox *tm, 99104380Sarchie struct kse_mailbox *km); 10099073Sjulian 10199073Sjulianstatic void 102103581Smininano(int len) 10399073Sjulian{ 104103581Smini struct timespec time_to_sleep; 105103581Smini struct timespec time_remaining; 10699073Sjulian 107103581Smini time_to_sleep.tv_sec = 0; 108103581Smini time_to_sleep.tv_nsec = len * 10000; 109103581Smini nanosleep(&time_to_sleep, &time_remaining); 11099073Sjulian} 11199073Sjulian 11299073Sjulianvoid 113103581Sminiaaaa(int c) 11499073Sjulian{ 115103581Smini for (;;) { 116103581Smini pchar(c); 117103581Smini nano(1); 11899073Sjulian } 119103581Smini} 12099073Sjulian 121103581Sministatic void 122103581Sminifoof(int sig) 12399073Sjulian{ 124103581Smini pfmt("\n[%d]\n", sig); 125103840Sjulian// thread_start(aaaa, '0' + progress++); 12699073Sjulian} 12799073Sjulian 128103840Sjulianstatic void 129103840Sjuliannewkse(int v) 130103840Sjulian{ 131103840Sjulian start_uts(&data4, 0); 132103840Sjulian} 133103840Sjulian 134103840Sjulian#if 0 135103581Sminivoid 136103581Sminispin(int arg) 13799073Sjulian{ 138103581Smini for (;;) enter_uts(); sched_yield(); 13999073Sjulian} 140103840Sjulian#endif 141103581Smini/* 142103581Smini * Test Userland Thread Scheduler (UTS) suite for KSE. 143103581Smini */ 144103581Sminiint 145103581Sminimain(void) 14699073Sjulian{ 147103581Smini int i; 14899073Sjulian 149103840Sjulian runq_init(&runq1); 150103840Sjulian init_uts(&data1, &runq1); 151103840Sjulian init_uts(&data2, &runq1); 152103840Sjulian thread_start(&data1, aaaa, '+'); 153103840Sjulian thread_start(&data1, aaaa, '-'); 154103840Sjulian start_uts(&data1, 0); 155103840Sjulian start_uts(&data2, 0); 156103840Sjulian 157103840Sjulian// start second ksegrp 158103840Sjulian runq_init(&runq2); 159103840Sjulian init_uts(&data3, &runq2); 160103840Sjulian init_uts(&data4, &runq2); 161103840Sjulian thread_start(&data3, newkse, 0); 162103840Sjulian thread_start(&data3, aaaa, '*'); 163103840Sjulian thread_start(&data3, aaaa, '.'); 164103840Sjulian start_uts(&data3, 1); 165103840Sjulian 166103581Smini for (i = 0;1;i++) { 167103581Smini// if (i < 1000) 168103581Smini// thread_start(aaaa, 'a' + (i % 26)); 169103581Smini pchar('A' + (i % 26)); 170103581Smini nano(5); 171103581Smini } 172103581Smini pstr("\n** main() exiting **\n"); 173103581Smini return (EX_OK); 17499073Sjulian} 17599073Sjulian 176103581Smini 177103581Smini/* 178103581Smini * Enter the UTS from a thread. 179103581Smini */ 180103581Sministatic void 181103840Sjulianenter_uts(struct uts_data *data) 18299073Sjulian{ 183103973Sarchie struct kse_thr_mailbox *td; 18499073Sjulian 185103581Smini /* XXX: We should atomically exchange these two. */ 186103840Sjulian td = data->mb.km_curthread; 187103840Sjulian data->mb.km_curthread = NULL; 188103581Smini 189103840Sjulian thread_to_uts(td, &data->mb); 19099073Sjulian} 19199073Sjulian 192103581Smini/* 193103581Smini * Initialise threading. 194103581Smini */ 195103581Sministatic void 196103840Sjulianinit_uts(struct uts_data *data, struct uts_runq *q) 19799073Sjulian{ 198103973Sarchie struct kse_thr_mailbox *tm; 199103581Smini int mib[2]; 200103581Smini char *p; 201104380Sarchie#if 0 202103581Smini size_t len; 203104380Sarchie#endif 20499073Sjulian 205103581Smini /* 206103581Smini * Create initial thread. 207103581Smini */ 208103973Sarchie tm = (struct kse_thr_mailbox *)calloc(1, sizeof(struct kse_thr_mailbox)); 20999073Sjulian 210103581Smini /* Throw us into its context. */ 211103581Smini getcontext(&tm->tm_context); 21299073Sjulian 213103581Smini /* Find our stack. */ 214103581Smini mib[0] = CTL_KERN; 215103581Smini mib[1] = KERN_USRSTACK; 216103840Sjulian#if 0 217103581Smini len = sizeof(p); 218103581Smini if (sysctl(mib, 2, &p, &len, NULL, 0) == -1) 219103581Smini pstr("sysctl(CTL_KER.KERN_USRSTACK) failed.\n"); 220103840Sjulian#endif 221103840Sjulian p = (char *)malloc(MAIN_STACK_SIZE) + MAIN_STACK_SIZE; 222103581Smini pfmt("main() : 0x%x\n", tm); 223103581Smini pfmt("eip -> 0x%x\n", tm->tm_context.uc_mcontext.mc_eip); 224103581Smini tm->tm_context.uc_stack.ss_sp = p - MAIN_STACK_SIZE; 225103581Smini tm->tm_context.uc_stack.ss_size = MAIN_STACK_SIZE; 22699073Sjulian 227103581Smini /* 228103581Smini * Create KSE mailbox. 229103581Smini */ 230103581Smini p = (char *)malloc(THREAD_STACK_SIZE); 231103840Sjulian bzero(&data->mb, sizeof(struct kse_mailbox)); 232103840Sjulian data->mb.km_stack.ss_sp = p; 233103840Sjulian data->mb.km_stack.ss_size = THREAD_STACK_SIZE; 234103840Sjulian data->mb.km_func = (void *)uts; 235103840Sjulian data->mb.km_udata = data; 236103840Sjulian data->cur_thread = tm; 237103840Sjulian data->runq = q; 238103581Smini pfmt("uts() at : 0x%x\n", uts); 239103581Smini pfmt("uts stack at : 0x%x - 0x%x\n", p, p + THREAD_STACK_SIZE); 240103840Sjulian} 24199073Sjulian 242103840Sjulianstatic void 243103840Sjulianstart_uts(struct uts_data *data, int newgrp) 244103840Sjulian{ 245103581Smini /* 246103581Smini * Start KSE scheduling. 247103581Smini */ 248103973Sarchie pfmt("kse_create() -> %d\n", kse_create(&data->mb, newgrp)); 249103840Sjulian data->mb.km_curthread = data->cur_thread; 250103581Smini 251103581Smini /* 252103581Smini * Arrange to deliver signals via KSE. 253103581Smini */ 254103581Smini signal(SIGURG, foof); 25599073Sjulian} 25699073Sjulian 257103581Smini/* 258103581Smini * Write a single character to stdout, in a thread-safe manner. 259103581Smini */ 260103581Sministatic void 261103581Sminipchar(char c) 26299073Sjulian{ 263103581Smini 264103581Smini write(STDOUT_FILENO, &c, 1); 26599073Sjulian} 26699073Sjulian 267103581Smini/* 268103581Smini * Write formatted output to stdout, in a thread-safe manner. 269103581Smini * 270103581Smini * Recognises the following conversions: 271103581Smini * %c -> char 272103581Smini * %d -> signed int (base 10) 273103581Smini * %s -> string 274103581Smini * %u -> unsigned int (base 10) 275103581Smini * %x -> unsigned int (base 16) 276103581Smini */ 277103581Sministatic void 278103581Sminipfmt(const char *fmt, ...) 27999073Sjulian{ 280103581Smini static const char digits[16] = "0123456789abcdef"; 281103581Smini va_list ap; 282103581Smini char buf[10]; 283103581Smini char *s; 284103581Smini unsigned r, u; 285103581Smini int c, d; 286103581Smini 287103581Smini va_start(ap, fmt); 288103581Smini while ((c = *fmt++)) { 289103581Smini if (c == '%') { 290103581Smini c = *fmt++; 291103581Smini switch (c) { 292103581Smini case 'c': 293103581Smini pchar(va_arg(ap, int)); 294103581Smini continue; 295103581Smini case 's': 296103581Smini pstr(va_arg(ap, char *)); 297103581Smini continue; 298103581Smini case 'd': 299103581Smini case 'u': 300103581Smini case 'x': 301103581Smini r = ((c == 'u') || (c == 'd')) ? 10 : 16; 302103581Smini if (c == 'd') { 303103581Smini d = va_arg(ap, unsigned); 304103581Smini if (d < 0) { 305103581Smini pchar('-'); 306103581Smini u = (unsigned)(d * -1); 307103581Smini } else 308103581Smini u = (unsigned)d; 309103581Smini } else 310103581Smini u = va_arg(ap, unsigned); 311103581Smini s = buf; 312103581Smini do { 313103581Smini *s++ = digits[u % r]; 314103581Smini } while (u /= r); 315103581Smini while (--s >= buf) 316103581Smini pchar(*s); 317103581Smini continue; 318103581Smini } 319103581Smini } 320103581Smini pchar(c); 32199073Sjulian } 322103581Smini va_end(ap); 32399073Sjulian} 32499073Sjulian 325103581Sministatic void 326103581Sminipstr(const char *s) 32799073Sjulian{ 328103581Smini 329103581Smini write(STDOUT_FILENO, s, strlen(s)); 33099073Sjulian} 33199073Sjulian 332103840Sjulianstatic void 333103840Sjulianrunq_init(struct uts_runq *q) 334103840Sjulian{ 335103840Sjulian q->head = NULL; 336103840Sjulian simplelock_init(&q->lock); 337103840Sjulian} 338103840Sjulian 339103581Smini/* 340103581Smini * Insert a thread into the run queue. 341103581Smini */ 342103581Sministatic void 343103973Sarchierunq_insert(struct uts_runq *q, struct kse_thr_mailbox *tm) 344103581Smini{ 345103840Sjulian simplelock_lock(&q->lock); 346103840Sjulian tm->tm_next = q->head; 347103840Sjulian q->head = tm; 348103840Sjulian simplelock_unlock(&q->lock); 349103581Smini} 35099073Sjulian 351103581Smini/* 352103581Smini * Select and remove a thread from the run queue. 353103581Smini */ 354103973Sarchiestatic struct kse_thr_mailbox * 355103840Sjulianrunq_remove(struct uts_runq *q) 35699073Sjulian{ 357103973Sarchie struct kse_thr_mailbox *tm; 358103840Sjulian 359103840Sjulian simplelock_lock(&q->lock); 360103840Sjulian tm = runq_remove_nolock(q); 361103840Sjulian simplelock_unlock(&q->lock); 362103840Sjulian return tm; 363103840Sjulian} 364103840Sjulian 365103973Sarchiestatic struct kse_thr_mailbox * 366103840Sjulianrunq_remove_nolock(struct uts_runq *q) 367103840Sjulian{ 368103973Sarchie struct kse_thr_mailbox *p, *p1; 369103840Sjulian 370103840Sjulian if (q->head == NULL) 371103581Smini return (NULL); 372103581Smini p1 = NULL; 373103840Sjulian for (p = q->head; p->tm_next != NULL; p = p->tm_next) 374103581Smini p1 = p; 375103581Smini if (p1 == NULL) 376103840Sjulian q->head = NULL; 377103581Smini else 378103581Smini p1->tm_next = NULL; 379103581Smini return (p); 380103581Smini} 38199073Sjulian 382103581Smini/* 383103581Smini * Userland thread scheduler. 384103581Smini */ 385103581Sministatic void 386103581Sminiuts(struct kse_mailbox *km) 387103581Smini{ 388104380Sarchie#ifdef TRACE_KSE 389103840Sjulian static struct uts_data *prev_data; 390104380Sarchie#endif 391103973Sarchie struct kse_thr_mailbox *tm, *p; 392103840Sjulian struct uts_data *data; 393104380Sarchie int i; 39499073Sjulian 395103581Smini UPSTR("\n--uts() start--\n"); 396103581Smini UPFMT("mailbox -> %x\n", km); 39799073Sjulian 398103581Smini /* 399103581Smini * Insert any processes back from being blocked 400103581Smini * in the kernel into the run queue. 401103581Smini */ 402103840Sjulian data = km->km_udata; 403103581Smini p = km->km_completed; 404103840Sjulian km->km_completed = NULL; 405103581Smini UPFMT("km_completed -> 0x%x", p); 406103840Sjulian#ifdef TRACE_KSE 407103840Sjulian if (data != prev_data) { 408103840Sjulian prev_data = data; 409103840Sjulian pfmt("uts data: 0x%x\n", data); 410103840Sjulian } 411103840Sjulian#endif 412103581Smini while ((tm = p) != NULL) { 413103581Smini p = tm->tm_next; 414103581Smini UPFMT(" 0x%x", p); 415103840Sjulian runq_insert(data->runq, tm); 41699073Sjulian } 417103581Smini UPCHAR('\n'); 41899073Sjulian 419103840Sjulian simplelock_lock(&data->runq->lock); 420103581Smini /* 421228975Suqs * Process any signals we've received (but only if we have 422103581Smini * somewhere to deliver them to). 423103581Smini */ 424103840Sjulian if ((data->runq->head != NULL) && SIGNOTEMPTY(km->km_sigscaught)) { 425103581Smini for (i = 0;i < _SIG_MAXSIG;i++) 426103581Smini if (SIGISMEMBER(km->km_sigscaught, i)) { 427103840Sjulian signalcontext(&data->runq->head->tm_context, 428103840Sjulian i, foof); 429103581Smini break; 430103581Smini } 431103581Smini bzero(&km->km_sigscaught, sizeof(sigset_t)); 432103581Smini } 43399073Sjulian 434103581Smini /* 435103581Smini * Pull a thread off the run queue. 436103581Smini */ 437103840Sjulian p = runq_remove_nolock(data->runq); 438103840Sjulian simplelock_unlock(&data->runq->lock); 439103581Smini#if 0 440103581Smini if ((p == aa) && (progress > 0)) { 441103581Smini --progress; 442103581Smini signalcontext(&p->tm_context, 1, foof); 443103581Smini } 444103581Smini#endif 445103581Smini 446103581Smini /* 447103581Smini * Either schedule a thread, or idle if none ready to run. 448103581Smini */ 449103581Smini if (p != NULL) { 450103581Smini UPFMT("\n-- uts() scheduling 0x%x--\n", p); 451103581Smini UPFMT("eip -> 0x%x progress -> %d\n", 452103581Smini p->tm_context.uc_mcontext.mc_eip, progress); 453103581Smini UPSTR("curthread set\n"); 454103581Smini uts_to_thread(p, &km->km_curthread); 455103581Smini UPSTR("\n-- uts_to_thread() failed --\n"); 456103581Smini } 457111171Sdavidxu kse_release(NULL); 458103581Smini pstr("** uts() exiting **\n"); 459103581Smini exit(EX_SOFTWARE); 46099073Sjulian} 46199073Sjulian 462103581Smini/* 463103581Smini * Start a thread. 464103581Smini */ 465103973Sarchiestatic struct kse_thr_mailbox * 466103840Sjulianthread_create(const void *func, int arg) 467103581Smini{ 468103973Sarchie struct kse_thr_mailbox *tm; 469103581Smini char *p; 470103581Smini 471103973Sarchie aa = tm = (struct kse_thr_mailbox *)calloc(1, sizeof(struct kse_thr_mailbox)); 472103581Smini getcontext(&tm->tm_context); 473103581Smini p = (char *)malloc(THREAD_STACK_SIZE); 474103581Smini tm->tm_context.uc_stack.ss_sp = p; 475103581Smini tm->tm_context.uc_stack.ss_size = THREAD_STACK_SIZE; 476103581Smini makecontext(&tm->tm_context, func, 2, arg); 477103581Smini // setcontext(&tm->tm_context); 478103840Sjulian return tm; 479103581Smini} 480103840Sjulian 481103840Sjulianstatic void 482103840Sjulianthread_start(struct uts_data *data, const void *func, int arg) 483103840Sjulian{ 484103973Sarchie struct kse_thr_mailbox *tm; 485103973Sarchie struct kse_thr_mailbox *tm2; 486103840Sjulian 487103840Sjulian tm = thread_create(func, arg); 488103840Sjulian tm2 = thread_create(enter_uts, (int)data); 489103840Sjulian tm->tm_context.uc_link = &tm2->tm_context; 490103840Sjulian runq_insert(data->runq, tm); 491103840Sjulian pfmt("thread_start() : 0x%x %x\n", tm, &tm->tm_context); 492103840Sjulian} 493