compiler/pthread/pthread.c

   1 /*
   2   Copyright (C) 2014 Szilard Biro
   3
   4   This software is provided 'as-is', without any express or implied
   5   warranty.  In no event will the authors be held liable for any damages
   6   arising from the use of this software.
   7
   8   Permission is granted to anyone to use this software for any purpose,
   9   including commercial applications, and to alter it and redistribute it
  10   freely, subject to the following restrictions:
  11
  12   1. The origin of this software must not be misrepresented; you must not
  13      claim that you wrote the original software. If you use this software
  14      in a product, an acknowledgment in the product documentation would be
  15      appreciated but is not required.
  16   2. Altered source versions must be plainly marked as such, and must not be
  17      misrepresented as being the original software.
  18   3. This notice may not be removed or altered from any source distribution.
  19 */
  20
  21 #ifdef __MORPHOS__
  22 #include <sys/time.h>
  23 #endif
  24 #include <dos/dostags.h>
  25 #include <clib/alib_protos.h>
  26 #include <proto/exec.h>
  27 #include <proto/dos.h>
  28 #include <proto/timer.h>
  29 #ifdef __AROS__
  30 #include <aros/symbolsets.h>
  31 #else
  32 #include <constructor.h>
  33 #define StackSwapArgs PPCStackSwapArgs
  34 #define NewStackSwap NewPPCStackSwap
  35 #endif
  36
  37 #include <setjmp.h>
  38 #include <string.h>
  39 #include <stdio.h>
  40 #include <signal.h>
  41 #include <stdlib.h>
  42
  43 #include "pthread.h"
  44 #include "debug.h"
  45
  46 #define FALLBACKSIGNAL SIGBREAKB_CTRL_E
  47 #define NAMELEN 32
  48 #define PTHREAD_INVALID_ID ((pthread_t)-1)
  49
  50 typedef struct
  51 {
  52     struct MinNode node;
  53     struct Task *task;
  54     ULONG sigmask;
  55 } CondWaiter;
  56
  57 typedef struct
  58 {
  59     void *value;
  60     void (*destructor)(void *);
  61     BOOL used;
  62 } TLSKey;
  63
  64 typedef struct
  65 {
  66     struct MinNode node;
  67     void (*routine)(void *);
  68     void *arg;
  69 } CleanupHandler;
  70
  71
  72 typedef struct
  73 {
  74     void *(*start)(void *);
  75     void *arg;
  76     struct MsgPort *msgport;
  77     struct Message msg;
  78     struct Process *process;
  79     //pthread_t id;
  80     void *ret;
  81     jmp_buf jmp;
  82     pthread_attr_t attr;
  83     //char name[256];
  84     //size_t oldlen;
  85     TLSKey tls[PTHREAD_KEYS_MAX];
  86     struct MinList cleanup;
  87 } ThreadInfo;
  88
  89 #define PTHREAD_FIRST_THREAD_ID (1) /* First thread id will be 1 so that it is different than default value of pthread_t */
  90
  91 static ThreadInfo threads[PTHREAD_THREADS_MAX];
  92 //static volatile pthread_t nextid = 0;
  93 static struct SignalSemaphore thread_sem;
  94
  95 //
  96 // Helper functions
  97 //
  98
  99 static int SemaphoreIsInvalid(struct SignalSemaphore *sem)
 100 {
 101     return (!sem || sem->ss_Link.ln_Type != NT_SIGNALSEM || sem->ss_WaitQueue.mlh_Tail != NULL);
 102 }
 103
 104 static ThreadInfo *GetThreadInfo(pthread_t thread)
 105 {
 106     ThreadInfo *inf = NULL;
 107
 108     //if (thread < nextid)
 109     // TODO: more robust error handling?
 110     if (thread < PTHREAD_THREADS_MAX)
 111         inf = &threads[thread];
 112
 113     return inf;
 114 }
 115
 116 static pthread_t GetThreadId(struct Task *task)
 117 {
 118     pthread_t i;
 119
 120     ObtainSemaphore(&thread_sem);
 121
 122     for (i = PTHREAD_FIRST_THREAD_ID; i < PTHREAD_THREADS_MAX; i++)
 123     {
 124         if ((struct Task *)threads[i].process == task)
 125             break;
 126     }
 127
 128     ReleaseSemaphore(&thread_sem);
 129
 130     if (i >= PTHREAD_THREADS_MAX)
 131         i = PTHREAD_INVALID_ID;
 132
 133     return i;
 134 }
 135
 136 //
 137 // Thread specific data functions
 138 //
 139
 140 int pthread_key_create(pthread_key_t *key, void (*destructor)(void *))
 141 {
 142     pthread_t thread;
 143     ThreadInfo *inf;
 144     TLSKey *tls;
 145     int i;
 146
 147     D(bug("%s(%p, %p)\n", __FUNCTION__, key, destructor));
 148
 149     if (key == NULL)
 150         return EINVAL;
 151
 152     thread = pthread_self();
 153     inf = GetThreadInfo(thread);
 154
 155     for (i = 0; i < PTHREAD_KEYS_MAX; i++)
 156     {
 157         if (inf->tls[i].used == FALSE)
 158             break;
 159     }
 160
 161     if (i >= PTHREAD_KEYS_MAX)
 162         return EAGAIN;
 163
 164     tls = &inf->tls[i];
 165     tls->used = TRUE;
 166     tls->destructor = destructor;
 167
 168     *key = i;
 169
 170     return 0;
 171 }
 172
 173 int pthread_key_delete(pthread_key_t key)
 174 {
 175     pthread_t thread;
 176     ThreadInfo *inf;
 177     TLSKey *tls;
 178
 179     D(bug("%s(%u)\n", __FUNCTION__, key));
 180
 181     thread = pthread_self();
 182     inf = GetThreadInfo(thread);
 183     tls = &inf->tls[key];
 184
 185     if (tls->used == FALSE)
 186         return EINVAL;
 187
 188     if (tls->destructor)
 189         tls->destructor(tls->value);
 190
 191     tls->used = FALSE;
 192     tls->destructor = NULL;
 193
 194     return 0;
 195 }
 196
 197 int pthread_setspecific(pthread_key_t key, const void *value)
 198 {
 199     pthread_t thread;
 200     ThreadInfo *inf;
 201     TLSKey *tls;
 202
 203     D(bug("%s(%u)\n", __FUNCTION__, key));
 204
 205     thread = pthread_self();
 206     inf = GetThreadInfo(thread);
 207     tls = &inf->tls[key];
 208
 209     if (tls->used == FALSE)
 210         return EINVAL;
 211
 212     tls->value = (void *)value;
 213
 214     return 0;
 215 }
 216
 217 void *pthread_getspecific(pthread_key_t key)
 218 {
 219     pthread_t thread;
 220     ThreadInfo *inf;
 221     TLSKey *tls;
 222     void *value = NULL;
 223
 224     D(bug("%s(%u)\n", __FUNCTION__, key));
 225
 226     thread = pthread_self();
 227     inf = GetThreadInfo(thread);
 228     tls = &inf->tls[key];
 229
 230     if (tls->used == TRUE)
 231         value = tls->value;
 232
 233     return value;
 234 }
 235
 236 //
 237 // Mutex attribute functions
 238 //
 239
 240 int pthread_mutexattr_init(pthread_mutexattr_t *attr)
 241 {
 242     D(bug("%s(%p)\n", __FUNCTION__, attr));
 243
 244     if (attr == NULL)
 245         return EINVAL;
 246
 247     memset(attr, 0, sizeof(pthread_mutexattr_t));
 248
 249     return 0;
 250 }
 251
 252 int pthread_mutexattr_settype(pthread_mutexattr_t *attr, int kind)
 253 {
 254     D(bug("%s(%p)\n", __FUNCTION__, attr));
 255
 256     if (attr == NULL)
 257         return EINVAL;
 258
 259     attr->kind = kind;
 260
 261     return 0;
 262 }
 263
 264 //
 265 // Mutex functions
 266 //
 267
 268 int pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *attr)
 269 {
 270     D(bug("%s(%p, %p)\n", __FUNCTION__, mutex, attr));
 271
 272     if (mutex == NULL)
 273         return EINVAL;
 274
 275     InitSemaphore(&mutex->semaphore);
 276
 277     return 0;
 278 }
 279
 280 int pthread_mutex_destroy(pthread_mutex_t *mutex)
 281 {
 282     //D(bug("%s(%p)\n", __FUNCTION__, mutex));
 283
 284     if (mutex == NULL)
 285         return EINVAL;
 286
 287     memset(mutex, 0, sizeof(pthread_mutex_t));
 288
 289     return 0;
 290 }
 291
 292 int pthread_mutex_lock(pthread_mutex_t *mutex)
 293 {
 294     //D(bug("%s(%p)\n", __FUNCTION__, mutex));
 295
 296     if (mutex == NULL)
 297         return EINVAL;
 298
 299     // initialize static mutexes
 300     if (SemaphoreIsInvalid(&mutex->semaphore))
 301         pthread_mutex_init(mutex, NULL);
 302
 303     ObtainSemaphore(&mutex->semaphore);
 304
 305     return 0;
 306 }
 307
 308 int pthread_mutex_trylock(pthread_mutex_t *mutex)
 309 {
 310     ULONG ret;
 311
 312     //D(bug("%s(%p)\n", __FUNCTION__, mutex));
 313
 314     if (mutex == NULL)
 315         return EINVAL;
 316
 317     // initialize static mutexes
 318     if (SemaphoreIsInvalid(&mutex->semaphore))
 319         pthread_mutex_init(mutex, NULL);
 320
 321     ret = AttemptSemaphore(&mutex->semaphore);
 322
 323     return (ret == TRUE) ? 0 : EBUSY;
 324 }
 325
 326 int pthread_mutex_unlock(pthread_mutex_t *mutex)
 327 {
 328     //D(bug("%s(%p)\n", __FUNCTION__, mutex));
 329
 330     if (mutex == NULL)
 331         return EINVAL;
 332
 333     // initialize static mutexes
 334     if (SemaphoreIsInvalid(&mutex->semaphore))
 335         pthread_mutex_init(mutex, NULL);
 336
 337     ReleaseSemaphore(&mutex->semaphore);
 338
 339     return 0;
 340 }
 341
 342 //
 343 // Condition variable functions
 344 //
 345
 346 int pthread_cond_init(pthread_cond_t *cond, const pthread_condattr_t *attr)
 347 {
 348     D(bug("%s(%p, %p)\n", __FUNCTION__, cond, attr));
 349
 350     if (cond == NULL)
 351         return EINVAL;
 352
 353     InitSemaphore(&cond->semaphore);
 354     NewList((struct List *)&cond->waiters);
 355     cond->waiting = 0;
 356
 357     return 0;
 358 }
 359
 360 int pthread_cond_destroy(pthread_cond_t *cond)
 361 {
 362     D(bug("%s(%p)\n", __FUNCTION__, cond));
 363
 364     if (cond == NULL)
 365         return EINVAL;
 366
 367     if (cond->waiting > 0)
 368         return EBUSY;
 369
 370     memset(cond, 0, sizeof(pthread_cond_t));
 371
 372     return 0;
 373 }
 374
 375 int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime)
 376 {
 377     CondWaiter waiter;
 378     BYTE signal;
 379     ULONG sigs = 0;
 380     ULONG timermask = 0;
 381     struct MsgPort *timermp = NULL;
 382     struct timerequest *timerio = NULL;
 383     struct Device *TimerBase = NULL;
 384
 385     //D(bug("%s(%p, %p, %p)\n", __FUNCTION__, cond, mutex, abstime));
 386
 387     if (cond == NULL || mutex == NULL)
 388         return EINVAL;
 389
 390     // initialize static conditions
 391     if (SemaphoreIsInvalid(&cond->semaphore))
 392         pthread_cond_init(cond, NULL);
 393
 394     if (abstime)
 395     {
 396         if (!(timermp = CreateMsgPort()))
 397             return EINVAL;
 398
 399         if (!(timerio = (struct timerequest *)CreateIORequest(timermp, sizeof(struct timerequest))))
 400         {
 401             DeleteMsgPort(timermp);
 402             return EINVAL;
 403         }
 404
 405         if (OpenDevice(TIMERNAME, UNIT_MICROHZ, &timerio->tr_node, 0) != 0)
 406         {
 407             DeleteMsgPort(timermp);
 408             DeleteIORequest((struct IORequest *)timerio);
 409             return EINVAL;
 410         }
 411
 412         TimerBase = timerio->tr_node.io_Device;
 413     }
 414
 415     if (TimerBase)
 416     {
 417         struct timeval starttime;
 418
 419         gettimeofday(&starttime, NULL);
 420         timerio->tr_node.io_Command = TR_ADDREQUEST;
 421         timerio->tr_time.tv_secs = abstime->tv_sec;
 422         timerio->tr_time.tv_micro = abstime->tv_nsec / 1000;
 423         SubTime(&timerio->tr_time, &starttime);
 424         timermask = 1 << timermp->mp_SigBit;
 425         sigs |= timermask;
 426         SendIO((struct IORequest *)timerio);
 427     }
 428
 429     waiter.task = FindTask(NULL);
 430     signal = AllocSignal(-1);
 431     if (signal == -1)
 432     {
 433         signal = FALLBACKSIGNAL;
 434         SetSignal(1 << FALLBACKSIGNAL, 0);
 435     }
 436     waiter.sigmask = 1 << signal;
 437     sigs |= waiter.sigmask;
 438     ObtainSemaphore(&cond->semaphore);
 439     AddTail((struct List *)&cond->waiters, (struct Node *)&waiter);
 440     cond->waiting++;
 441     ReleaseSemaphore(&cond->semaphore);
 442
 443     pthread_mutex_unlock(mutex);
 444     sigs = Wait(sigs);
 445     pthread_mutex_lock(mutex);
 446
 447     ObtainSemaphore(&cond->semaphore);
 448     Remove((struct Node *)&waiter);
 449     cond->waiting--;
 450     ReleaseSemaphore(&cond->semaphore);
 451
 452     if (signal != FALLBACKSIGNAL)
 453         FreeSignal(signal);
 454
 455     if (TimerBase)
 456     {
 457         if (!CheckIO((struct IORequest *)timerio))
 458         {
 459             AbortIO((struct IORequest *)timerio);
 460             WaitIO((struct IORequest *)timerio);
 461         }
 462         CloseDevice((struct IORequest *)timerio);
 463         DeleteIORequest((struct IORequest *)timerio);
 464         DeleteMsgPort(timermp);
 465
 466         if (sigs & timermask)
 467             return ETIMEDOUT;
 468     }
 469
 470     return 0;
 471 }
 472
 473 int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex)
 474 {
 475     //D(bug("%s(%p)\n", __FUNCTION__, cond));
 476
 477     return pthread_cond_timedwait(cond, mutex, NULL);
 478 }
 479
 480 static int _pthread_cond_broadcast(pthread_cond_t *cond, BOOL onlyfirst)
 481 {
 482     CondWaiter *waiter;
 483
 484     //D(bug("%s(%p, %d)\n", __FUNCTION__, cond, onlyfirst));
 485
 486     if (cond == NULL)
 487         return EINVAL;
 488
 489     // initialize static conditions
 490     if (SemaphoreIsInvalid(&cond->semaphore))
 491         pthread_cond_init(cond, NULL);
 492
 493     ObtainSemaphore(&cond->semaphore);
 494     if (cond->waiting > 0)
 495     {
 496         ForeachNode(&cond->waiters, waiter)
 497         {
 498             Signal(waiter->task, waiter->sigmask);
 499             if (onlyfirst) break;
 500         }
 501     }
 502     ReleaseSemaphore(&cond->semaphore);
 503
 504     return 0;
 505 }
 506
 507 int pthread_cond_signal(pthread_cond_t *cond)
 508 {
 509     //D(bug("%s(%p)\n", __FUNCTION__, cond));
 510
 511     return _pthread_cond_broadcast(cond, TRUE);
 512 }
 513
 514 int pthread_cond_broadcast(pthread_cond_t *cond)
 515 {
 516     //D(bug("%s(%p)\n", __FUNCTION__, cond));
 517
 518     return _pthread_cond_broadcast(cond, FALSE);
 519 }
 520
 521 //
 522 // Thread attribute functions
 523 //
 524
 525 int pthread_attr_init(pthread_attr_t *attr)
 526 {
 527     struct Task *task = FindTask(NULL);
 528
 529     D(bug("%s(%p)\n", __FUNCTION__, attr));
 530
 531     if (attr == NULL)
 532         return EINVAL;
 533
 534     memset(attr, 0, sizeof(pthread_attr_t));
 535     // inherit the priority and stack size of the parent thread
 536     attr->param.sched_priority = task->tc_Node.ln_Pri;
 537     attr->stacksize = (UBYTE *)task->tc_SPUpper - (UBYTE *)task->tc_SPLower;
 538
 539     return 0;
 540 }
 541
 542 int pthread_attr_destroy(pthread_attr_t *attr)
 543 {
 544     D(bug("%s(%p)\n", __FUNCTION__, attr));
 545
 546     if (attr == NULL)
 547         return EINVAL;
 548
 549     memset(attr, 0, sizeof(pthread_attr_t));
 550
 551     return 0;
 552 }
 553
 554 int pthread_attr_getstack(const pthread_attr_t *attr, void **stackaddr, size_t *stacksize)
 555 {
 556     D(bug("%s(%p, %p)\n", __FUNCTION__, attr, stackaddr));
 557
 558     if (attr == NULL)
 559         return EINVAL;
 560
 561     if (stackaddr != NULL)
 562         *stackaddr = attr->stackaddr;
 563
 564     if (stacksize != NULL)
 565         *stacksize = attr->stacksize;
 566
 567     return 0;
 568 }
 569
 570 int pthread_attr_setstack(pthread_attr_t *attr, void *stackaddr, size_t stacksize)
 571 {
 572     D(bug("%s(%p, %p)\n", __FUNCTION__, attr, stackaddr));
 573
 574     if (attr == NULL || (stackaddr != NULL && stacksize == 0))
 575         return EINVAL;
 576
 577     attr->stackaddr = stackaddr;
 578     attr->stacksize = stacksize;
 579
 580     return 0;
 581 }
 582
 583 int pthread_attr_getstacksize(const pthread_attr_t *attr, size_t *stacksize)
 584 {
 585     D(bug("%s(%p, %p)\n", __FUNCTION__, attr, stacksize));
 586
 587     return pthread_attr_getstack(attr, NULL, stacksize);
 588 }
 589
 590 int pthread_attr_setstacksize(pthread_attr_t *attr, size_t stacksize)
 591 {
 592     D(bug("%s(%p, %u)\n", __FUNCTION__, attr, stacksize));
 593
 594     return pthread_attr_setstack(attr, NULL, stacksize);
 595 }
 596
 597 int pthread_attr_getschedparam(const pthread_attr_t *attr, struct sched_param *param)
 598 {
 599     D(bug("%s(%p, %p)\n", __FUNCTION__, attr, param));
 600
 601     if (attr == NULL || param == NULL)
 602         return EINVAL;
 603
 604     *param = attr->param;
 605
 606     return 0;
 607 }
 608
 609 int pthread_attr_setschedparam(pthread_attr_t *attr, const struct sched_param *param)
 610 {
 611     D(bug("%s(%p, %p)\n", __FUNCTION__, attr, param));
 612
 613     if (attr == NULL || param == NULL)
 614         return EINVAL;
 615
 616     attr->param = *param;
 617
 618     return 0;
 619 }
 620
 621 //
 622 // Thread functions
 623 //
 624
 625 static void StarterFunc(void)
 626 {
 627     ThreadInfo *inf;
 628
 629     D(bug("%s()\n", __FUNCTION__));
 630
 631     inf = (ThreadInfo *)FindTask(NULL)->tc_UserData;
 632     // trim the name
 633     //inf->process->pr_Task.tc_Node.ln_Name[inf->oldlen];
 634
 635     // we have to set the priority here to avoid race conditions
 636     SetTaskPri((struct Task *)inf->process, inf->attr.param.sched_priority);
 637
 638     if (!setjmp(inf->jmp))
 639     {
 640         if (inf->attr.stackaddr != NULL && inf->attr.stacksize > 0)
 641         {
 642             struct StackSwapArgs swapargs;
 643             struct StackSwapStruct stack;
 644
 645             swapargs.Args[0] = (IPTR)inf->arg;
 646             stack.stk_Lower = inf->attr.stackaddr;
 647             stack.stk_Upper = (APTR)((IPTR)stack.stk_Lower + inf->attr.stacksize);
 648             stack.stk_Pointer = stack.stk_Upper;
 649
 650             inf->ret = (void *)NewStackSwap(&stack, inf->start, &swapargs);
 651         }
 652         else
 653         {
 654             inf->ret = inf->start(inf->arg);
 655         }
 656     }
 657
 658     Forbid();
 659     ReplyMsg(&inf->msg);
 660 }
 661
 662 int pthread_create(pthread_t *thread, const pthread_attr_t *attr, void *(*start)(void *), void *arg)
 663 {
 664     ThreadInfo *inf;
 665     char buf[NAMELEN];
 666     char name[NAMELEN];
 667     size_t oldlen;
 668     pthread_t threadnew;
 669
 670     D(bug("%s(%p, %p, %p, %p)\n", __FUNCTION__, thread, attr, start, arg));
 671
 672     if (thread == NULL || start == NULL)
 673         return EINVAL;
 674
 675     ObtainSemaphore(&thread_sem);
 676
 677     //threadnew = nextid++; //__sync_add_and_fetch(&nextid, 1);
 678     threadnew = GetThreadId(NULL);
 679     if (threadnew == PTHREAD_INVALID_ID)
 680     {
 681         ReleaseSemaphore(&thread_sem);
 682         return EAGAIN;
 683     }
 684
 685     inf = GetThreadInfo(threadnew);
 686     memset(inf, 0, sizeof(ThreadInfo));
 687     inf->start = start;
 688     if (attr)
 689         inf->attr = *attr;
 690     else
 691         pthread_attr_init(&inf->attr);
 692     inf->arg = arg;
 693     NewList((struct List *)&inf->cleanup);
 694
 695     // let's trick CreateNewProc into allocating a larger buffer for the name
 696     snprintf(buf, sizeof(buf), "pthread thread #%d", threadnew);
 697     oldlen = strlen(buf);
 698     memset(name, ' ', sizeof(name));
 699     memcpy(name, buf, oldlen);
 700     name[sizeof(name) - 1] = '\0';
 701
 702     inf->msgport = CreateMsgPort();
 703     if (!inf->msgport)
 704     {
 705         ReleaseSemaphore(&thread_sem);
 706         return EAGAIN;
 707     }
 708
 709     inf->msg.mn_Node.ln_Type = NT_MESSAGE;
 710     inf->msg.mn_ReplyPort = inf->msgport;
 711     inf->msg.mn_Length = sizeof(inf->msg);
 712
 713     inf->process = CreateNewProcTags(NP_Entry, StarterFunc,
 714 #ifdef __MORPHOS__
 715         NP_CodeType, CODETYPE_PPC,
 716         (inf->attr.stackaddr == NULL && inf->attr.stacksize > 0) ? NP_PPCStackSize : TAG_IGNORE, inf->attr.stacksize,
 717 #else
 718         (inf->attr.stackaddr == NULL && inf->attr.stacksize > 0) ? NP_StackSize : TAG_IGNORE, inf->attr.stacksize,
 719 #endif
 720         NP_UserData, inf,
 721         NP_Name, name,
 722         TAG_DONE);
 723
 724     ReleaseSemaphore(&thread_sem);
 725
 726     if (!inf->process)
 727     {
 728         DeleteMsgPort(inf->msgport);
 729         inf->msgport = NULL;
 730         return EAGAIN;
 731     }
 732
 733     *thread = threadnew;
 734
 735     return 0;
 736 }
 737
 738 int pthread_detach(pthread_t thread)
 739 {
 740     D(bug("%s(%u) not implemented\n", __FUNCTION__, thread));
 741
 742     return ESRCH;
 743 }
 744
 745 int pthread_join(pthread_t thread, void **value_ptr)
 746 {
 747     ThreadInfo *inf;
 748
 749     D(bug("%s(%u, %p)\n", __FUNCTION__, thread, value_ptr));
 750
 751     inf = GetThreadInfo(thread);
 752
 753     if (!inf->msgport)
 754         return ESRCH;
 755
 756     //while (!GetMsg(inf->msgport))
 757         WaitPort(inf->msgport);
 758
 759     DeleteMsgPort(inf->msgport);
 760     //inf->msgport = NULL;
 761
 762     if (value_ptr)
 763         *value_ptr = inf->ret;
 764
 765     ObtainSemaphore(&thread_sem);
 766     memset(inf, 0, sizeof(ThreadInfo));
 767     ReleaseSemaphore(&thread_sem);
 768
 769     return 0;
 770 }
 771
 772 int pthread_equal(pthread_t t1, pthread_t t2)
 773 {
 774     D(bug("%s(%u, %u)\n", __FUNCTION__, t1, t2));
 775
 776     return (t1 == t2);
 777 }
 778
 779 pthread_t pthread_self(void)
 780 {
 781     struct Task *task;
 782     pthread_t thread;
 783
 784     D(bug("%s()\n", __FUNCTION__));
 785
 786     task = FindTask(NULL);
 787     thread = GetThreadId(task);
 788
 789     // add non-pthread processes to our list, so we can handle the main thread
 790     if (thread == PTHREAD_INVALID_ID)
 791     {
 792         ThreadInfo *inf;
 793
 794         ObtainSemaphore(&thread_sem);
 795         thread = GetThreadId(NULL);
 796         //thread = nextid++; //__sync_add_and_fetch(&nextid, 1);
 797         inf = GetThreadInfo(thread);
 798         memset(inf, 0, sizeof(ThreadInfo));
 799         NewList((struct List *)&inf->cleanup);
 800         inf->process = (struct Process *)task;
 801         ReleaseSemaphore(&thread_sem);
 802     }
 803
 804     return thread;
 805 }
 806
 807 int pthread_cancel(pthread_t thread)
 808 {
 809     D(bug("%s(%u) not implemented\n", __FUNCTION__, thread));
 810
 811     // TODO: should I do a pthread_join here?
 812     return ESRCH;
 813 }
 814
 815 void pthread_exit(void *value_ptr)
 816 {
 817     ThreadInfo *inf;
 818     CleanupHandler *handler;
 819
 820     D(bug("%s(%p)\n", __FUNCTION__, value_ptr));
 821
 822     inf = GetThreadInfo(pthread_self());
 823     inf->ret = value_ptr;
 824
 825     while ((handler = (CleanupHandler *)RemTail((struct List *)&inf->cleanup)))
 826         if (handler->routine)
 827             handler->routine(handler->arg);
 828
 829     longjmp(inf->jmp, 1);
 830 }
 831
 832 #if defined __mc68000__
 833 /* No CAS instruction on m68k */
 834 static int __m68k_sync_val_compare_and_swap(int *v, int o, int n)
 835 {
 836     int ret;
 837
 838     Disable();
 839     if ((*v) == (o))
 840         (*v) = (n);
 841     ret = (*v);
 842     Enable();
 843
 844     return ret;
 845 }
 846 #undef __sync_val_compare_and_swap
 847 #define __sync_val_compare_and_swap(v, o, n) __m68k_sync_val_compare_and_swap(v, o, n)
 848 #endif
 849
 850 int pthread_once(pthread_once_t *once_control, void (*init_routine)(void))
 851 {
 852     if (once_control == NULL || init_routine == NULL)
 853         return EINVAL;
 854
 855     if (__sync_val_compare_and_swap(&once_control->started, FALSE, TRUE))
 856     {
 857         if (!once_control->done)
 858         {
 859             (*init_routine)();
 860             once_control->done = TRUE;
 861         }
 862     }
 863
 864     return 0;
 865 }
 866
 867 //
 868 // Scheduling functions
 869 //
 870
 871 int pthread_setschedparam(pthread_t thread, int policy, const struct sched_param *param)
 872 {
 873     ThreadInfo *inf;
 874
 875     D(bug("%s(%u, %d, %p)\n", __FUNCTION__, thread, policy, param));
 876
 877     if (param == NULL)
 878         return EINVAL;
 879
 880     inf = GetThreadInfo(thread);
 881     SetTaskPri((struct Task *)inf->process, param->sched_priority);
 882
 883     return 0;
 884 }
 885
 886 //
 887 // NP
 888 //
 889 int pthread_setname_np(pthread_t thread, const char *name)
 890 {
 891     ThreadInfo *inf;
 892     char *currentName;
 893
 894     D(bug("%s(%u, %s)\n", __FUNCTION__, thread, name));
 895
 896     if (name == NULL)
 897         return ERANGE;
 898
 899     inf = GetThreadInfo(thread);
 900     currentName = inf->process->pr_Task.tc_Node.ln_Name;
 901
 902     if (strlen(name) + 1 > NAMELEN)
 903         return ERANGE;
 904
 905     strncpy(currentName, name, NAMELEN);
 906
 907     return 0;
 908 }
 909
 910 int pthread_getname_np(pthread_t thread, char *name, size_t len)
 911 {
 912     ThreadInfo *inf;
 913     char *currentName;
 914
 915     D(bug("%s(%u, %p, %u)\n", __FUNCTION__, thread, name, len));
 916
 917     if (name == NULL || len == 0)
 918         return ERANGE;
 919
 920     inf = GetThreadInfo(thread);
 921     currentName = inf->process->pr_Task.tc_Node.ln_Name;
 922
 923     if (strlen(currentName) + 1 > len)
 924         return ERANGE;
 925     // TODO: partially copy the name?
 926     strncpy(name, currentName, len);
 927
 928     return 0;
 929 }
 930
 931 //
 932 // Cancellation cleanup
 933 //
 934
 935 // theads can't be cancelled, but they can still call pthread_exit, which
 936 // will execute these clean-up handlers
 937 void pthread_cleanup_push(void (*routine)(void *), void *arg)
 938 {
 939     pthread_t thread;
 940     ThreadInfo *inf;
 941     CleanupHandler *handler;
 942
 943     D(bug("%s(%p, %p)\n", __FUNCTION__, routine, arg));
 944
 945     handler = calloc(1, sizeof(CleanupHandler));
 946
 947     if (routine == NULL || handler == NULL)
 948         return;
 949
 950     thread = pthread_self();
 951     inf = GetThreadInfo(thread);
 952
 953     AddTail((struct List *)&inf->cleanup, (struct Node *)handler);
 954 }
 955
 956 void pthread_cleanup_pop(int execute)
 957 {
 958     pthread_t thread;
 959     ThreadInfo *inf;
 960     CleanupHandler *handler;
 961
 962     D(bug("%s(%d)\n", __FUNCTION__, execute));
 963
 964     thread = pthread_self();
 965     inf = GetThreadInfo(thread);
 966     handler = (CleanupHandler *)RemTail((struct List *)&inf->cleanup);
 967
 968     if (handler && handler->routine && execute)
 969         handler->routine(handler->arg);
 970
 971     free(handler);
 972 }
 973
 974 //
 975 // Signalling
 976 //
 977
 978 int pthread_kill(pthread_t thread, int sig)
 979 {
 980
 981     ThreadInfo *inf;
 982     struct ETask *et;
 983
 984     D(bug("%s(%u, %d)\n", __FUNCTION__, thread, sig));
 985
 986 #ifdef __AROS__
 987     inf = GetThreadInfo(thread);
 988     et = GetETask((struct Task *)inf->process);
 989
 990     if (et == NULL)
 991         return EINVAL;
 992
 993     return kill((pid_t)et->et_UniqueID, sig);
 994 #else
 995     return EINVAL;
 996 #endif
 997 }
 998
 999 //
1000 // Constructors, destructors
1001 //
1002
1003 static int _Init_Func(void)
1004 {
1005     D(bug("%s()\n", __FUNCTION__));
1006
1007     memset(&threads, 0, sizeof(threads));
1008     InitSemaphore(&thread_sem);
1009
1010     return TRUE;
1011 }
1012
1013 static void _Exit_Func(void)
1014 {
1015 #if 0
1016     pthread_t i;
1017 #endif
1018
1019     D(bug("%s()\n", __FUNCTION__));
1020
1021     // wait for the threads?
1022 #if 0
1023     for (i = 0; i < PTHREAD_THREADS_MAX; i++)
1024         pthread_join(i, NULL);
1025 #endif
1026 }
1027
1028 #ifdef __AROS__
1029 ADD2INIT(_Init_Func, 0);
1030 ADD2EXIT(_Exit_Func, 0);
1031 #else
1032 static CONSTRUCTOR_P(_Init_Func, 100)
1033 {
1034     return !_Init_Func();
1035 }
1036
1037 static DESTRUCTOR_P(_Exit_Func, 100)
1038 {
1039     _Exit_Func();
1040 }
1041 #endif