pthread.c

这是leon3处理器的交叉编译链

/* Copyright (C) 1992-2000 the Florida State University
   Distributed by the Florida State University under the terms of the
   GNU Library General Public License.

This file is part of Pthreads.

Pthreads is free software; you can redistribute it and/or
modify it under the terms of the GNU Library General Public
License as published by the Free Software Foundation (version 2).

Pthreads is distributed "AS IS" in the hope that it will be
useful, but WITHOUT ANY WARRANTY; without even the implied
warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Library General Public License for more details.

You should have received a copy of the GNU Library General Public
License along with Pthreads; see the file COPYING.  If not, write
to the Free Software Foundation, 675 Mass Ave, Cambridge,
MA 02139, USA.

Report problems and direct all questions to:

  [e-mail]

  @(#)pthread.c	3.14 11/8/00

*/

/*
 * Implementation of fork, join, exit, etc.
 */

#if defined (__FreeBSD__) || defined (_M_UNIX) || defined (__dos__)
# include<stdlib.h> /* why was this here for __linux__ ? */
#endif

#include "internals.h"
#include "setjmp.h"
#ifdef TDI_SUPPORT
#include "tdi.h"
#include "tdi-aux.h"
#include "tdi-dl.h"
#endif
#include <fsu_pthread/asm.h>
#include <fsu_pthread/debug.h>
#include <fsu_pthread/malloc.h>
#include <asm-leon/clock.h>

volatile int pthread_started = FALSE;
static volatile int n_pthreads = 0;
static volatile pthread_key_t n_keys = 0;
static void (*key_destructor[_POSIX_DATAKEYS_MAX])();

/*------------------------------------------------------------*/
/*
 * pthread_alloc - allocate pthread structure
 */
static pthread_t pthread_alloc(func, arg)
     pthread_func_t func;
     any_t arg;
{
  pthread_t t;
  
  t = (pthread_t) calloc(1, sizeof(struct pthread));
  if (t != NO_PTHREAD) {
    t->func = func;
    t->arg = arg;
    t->state = T_RUNNING | T_CONTROLLED;
  }
  PDEBUG(PDBG_ALLOC,"allocated: 0x%x",t);
  
  return(t);
}

/*------------------------------------------------------------*/
/*
 * pthread_self - returns immutable thread ID of calling thread
 */
pthread_t pthread_self()
{
  PTRACEIN;
  return(mac_pthread_self());
}

/*------------------------------------------------------------*/
/*
 * pthread_idle - idle function
 */
any_t pthread_idle(any_t __arg) {
  while(1);
}

void _pthread_init()
{
  pthread_t t;
  int i;
  threadctx_t env __attribute__((aligned(8)));
  PTRACEIN;

  PDEBUG(PDBG_START,"kernel: 0x%x, idle=0x%x",&pthread_kern,&k_idle);

  TAILQ_INIT(&ready);
  TAILQ_INIT(&all);
  TAILQ_INIT(&suspend_q);
  TAILQ_INIT(&pthread_timeout_q);

  if (!(pthread_tempstack = (kernel_stack_t) malloc(sizeof(struct kernel_stack))))
    return;
  
  pthread_init_signals();

  pthread_mutexattr_init(&pthread_mutexattr_default);
  pthread_condattr_init(&pthread_condattr_default);
  pthread_attr_init(&pthread_attr_default);

  t = pthread_alloc((pthread_func_t) 0, (any_t *) NULL);
  t->pt_name = "main";
  t->state |= T_MAIN | T_CONTROLLED;

  t->num_timers = 0;
  t->interval.tv_sec = 0;
  t->interval.tv_usec = 0;

  thread_setjmp(env, FALSE);

  process_stack_base = (char *) env[THREAD_JB_SP];
  pthread_stack_init(t);
  
  pthread_attr_init(&t->attr);
  t->attr.param.sched_priority = MIN_PRIORITY;
#ifdef _POSIX_THREADS_PRIO_PROTECT
  t->base_prio = MIN_PRIORITY;
#ifdef SRP
  t->max_ceiling_prio = NO_PRIO;
  t->new_prio = NO_PRIO;
#endif
#endif
#ifdef DEF_RR
  t->attr.sched = SCHED_RR;
#else
  t->attr.sched = SCHED_FIFO;
#endif
  /*
   * 1st thread inherits signal mask from process
   */
  pthread_sigcpyset2set(&t->mask, &proc_mask);
  t->pt_reentp = _impure_ptr;

  mac_pthread_self() = t;	
  pthread_q_all_enq(&all, t);
  pthread_q_primary_enq(&ready, t);
  state_change = FALSE;

  pthreadleon_init_ticks();
  
  pthread_started = TRUE;
#ifdef DEF_RR
  pthread_timed_sigwait(t, NULL, RR_TIME, NULL, t);
#endif

  
  n_pthreads = 1; /* main thread counts, fd_server does not count */

}


void pthread_init_attr(pthread_attr_t *attr) {
  
  PTRACEIN;
  if (pthread_started)
    return;
  
  _pthread_init();
  attr->name = "idle";
  pthread_create( &k_idle, attr, (void*)&pthread_idle, NULL);
}
  

/*------------------------------------------------------------*/
/*
 * pthread_init - initialize the threads package. This function is 
 * the first function that be called by any program using this package.
 * It initializes the main thread and sets up a stackspace for all the
 * threads to use. Its main purpose is to setup the mutexes and 
 * condition variables and allow one to access them.
 */

/*
 * the `constructor' attribute causes `pthread_init()' to be called 
 * automatically before execution enters `main()'.
 * (For all ports except SunOS and Solaris: see p_aux.S for init hook)
 */

/*void pthread_init(void) __attribute__ ((constructor));*/

void pthread_init() {
  pthread_attr_t attr;
  PTRACEIN;
  pthread_attr_init(&attr);
  attr.stacksize = DEFAULT_STACKSIZE_IDLE;
  attr.param.sched_priority = MIN_PRIORITY;
  pthread_init_attr(&attr);
}

/*------------------------------------------------------------*/
/*
 * pthread_body - base of the pthreads implementation.
 * Procedure invoked at the base of each pthread (as a wrapper).
 */
void pthread_body()
{
  pthread_t t = mac_pthread_self();
#ifdef REAL_TIME
  struct timespec tp;
#endif /* REAL_TIME */
  static void pthread_terminate();
#ifdef DEBUG
#ifdef STAND_ALONE
pthread_timer_q_t pthread_timer;              /* timer queue                 */
#else
pthread_timer_q pthread_timer;                /* timer queue                 */
#endif
#endif /* DEBUG */

  PTRACEIN;
  PDEBUG(PDBG_START,"body-args 0x%x",t->arg);
  
  CLEAR_KERNEL_FLAG;

#ifdef REAL_TIME
  NTIMERCLEAR(tp);
  if (ISNTIMERSET(t->attr.starttime)) {
    tp.tv_sec = t->attr.starttime.tv_sec;
    tp.tv_nsec = t->attr.starttime.tv_nsec;
  }

  if (ISNTIMERSET(t->attr.deadline)) {
    PDEBUG(PDBG_START,"REAL_TIME: wait deadline [0x%x,0x%x]",t->attr.deadline.ts_sec,t->attr.deadline.ts_nsec);
    SET_KERNEL_FLAG;
    pthread_timed_sigwait(t, &t->attr.deadline, ABS_TIME, pthread_exit, -1);
    CLEAR_KERNEL_FLAG;
  }

  if (ISNTIMERSET(tp)) {
    PDEBUG(PDBG_START,"REAL_TIME: wait startime [0x%x,0x%x]",tp.tv_sec,tp.tv_nsec);
    pthread_absnanosleep(&tp);
  }
  
  do {
    if (ISNTIMERSET(tp) && !LE0_NTIME(t->attr.period)) {
      PDEBUG(PDBG_START,"REAL_TIME: > LESS ZERO [0x%x,0x%x]",t->attr.period.ts_sec,t->attr.period.ts_nsec);
      PLUS_NTIME(tp, tp, t->attr.period);
      SET_KERNEL_FLAG;
      pthread_timed_sigwait(t, &tp, ABS_TIME, pthread_exit, -1);
      CLEAR_KERNEL_FLAG;
    }
    if (!thread_setjmp(t->body, FALSE))
#endif /* REAL_TIME */
      
      t->result = (*(t->func))(t->arg);

#ifdef REAL_TIME
    if (ISNTIMERSET(tp) && !LE0_NTIME(t->attr.period)) {
      PDEBUG(PDBG_START,"REAL_TIME: < LESS ZERO [0x%x,0x%x]",t->attr.period.ts_sec,t->attr.period.ts_nsec);
      SET_KERNEL_FLAG;
      pthread_cancel_timed_sigwait(t, FALSE, ALL_TIME, FALSE);
      CLEAR_KERNEL_FLAG;
      pthread_absnanosleep(&tp);
    }
  } while (!LE0_NTIME(t->attr.period));
#endif /* REAL_TIME */

  pthread_terminate();
}

/*------------------------------------------------------------*/
/*
 * pthread_terminate - terminate thread: call cleanup handlers and
 * destructor functions, allow no more signals, dequeue from ready,
 * and switch context
 */
static void pthread_terminate()
{
  register pthread_t p, t = mac_pthread_self();
  register pthread_key_t i;
  register pthread_cleanup_t new;
#ifdef CLEANUP_HEAP
  register pthread_cleanup_t old;
#endif
  sigset_t abs_all_signals;

  PTRACEIN;
  
  SET_KERNEL_FLAG;
  if (!(t->state & T_EXITING)) {
    t->state |= T_EXITING;
    CLEAR_KERNEL_FLAG;
  }
  else {
    CLEAR_KERNEL_FLAG;
    return;
  }

  /*
   * call cleanup handlers in LIFO
   */
#ifdef CLEANUP_HEAP
  for (old = (pthread_cleanup_t) NULL, new = t->cleanup_top; new;
       old = new, new = new->next) {
    (new->func)(new->arg);
    if (old)
      free(old);
  }
  if (old)
    free(old);
#else
  for (new = t->cleanup_top; new; new = new->next)
    (new->func)(new->arg);
#endif

  /*
   * call destructor functions for data keys (if both are defined)
   */
  for (i = 0; i < n_keys; i++)
    if (t->key[i] && key_destructor[i])
      (key_destructor[i])(t->key[i]);

  /*
   * No more signals, also remove from queue of all threads
   */
  SET_KERNEL_FLAG;
  pthread_sigcpyset2set(&abs_all_signals, &all_signals);
  sigaddset(&abs_all_signals, SIGCANCEL);
  if (!pthread_siggeset2set(&t->mask, &abs_all_signals)) {

    if (t->state & (T_SIGWAIT | T_SIGSUSPEND)) {
      t->state &= ~(T_SIGWAIT | T_SIGSUSPEND);
      sigemptyset(&t->sigwaitset);
    }

    TAILQ_REMOVE(&all,t,pt_qelem[K_QUEUES_ALL]);
    pthread_sigcpyset2set(&t->mask, &abs_all_signals);

  }
  PTRACE_ALLQUEUE;
  
  /*
   * dequeue thread and schedule someone else
   */

  if (t->state & (T_SYNCTIMER | T_ASYNCTIMER)) {
    pthread_cancel_timed_sigwait(t, FALSE, ALL_TIME, FALSE);
  }
  t->state &= ~T_RUNNING;
  t->state &= ~T_EXITING;
  t->state |= T_RETURNED;

  /*
   * Terminating thread has to be detached if anyone tries to join
   * but the memory is not freed until the dispatcher is called.
   * This is required by pthread_join().
   * The result is copied into the TCB of the joining threads to
   * allow the memory of the current thread to be reclaimed before
   * the joining thread accesses the result.
   */
  if (TAILQ_FIRST(&t->joinq)) {
    t->state |= T_DETACHED;
    TAILQ_FOREACH(p,&t->joinq,pt_qelem[PRIMARY_QUEUE]) {
      p->result = t->result;
    }
    pthread_q_wakeup_all(&t->joinq, PRIMARY_QUEUE);
  }
  
#ifdef STACK_CHECK
  if (!(t->state & T_MAIN))
    pthread_unlock_all_stack(t);
#endif

  /*
   * The last threads switches off the light and calls UNIX exit
   */
  SIM_SYSCALL(TRUE);
  if (--n_pthreads) {
    pthread_q_deq(&ready, t, PRIMARY_QUEUE);
    CLEAR_KERNEL_FLAG;
  }
  else {
#ifdef STAND_ALONE
    exit(0);
#else
    pthread_clear_sighandler();
    pthread_process_exit(0);
#endif
  }
  
}

/*------------------------------------------------------------*/
/*
 * pthread_create - Create a new thread of execution. the thread 
 * structure and a queue and bind them together.
 * The completely created pthread is then put on the active list before
 * it is allowed to execute. Caution: The current implementation uses 
 * pointers to the thread structure as thread ids. If a thread is not 
 * valid it's pointer becomes a dangling reference but may still be 
 * used for thread operations. It's up to the user to make sure he 
 * never uses dangling thread ids. If, for example, the created thread
 * has a higher priority than the caller of pthread_create() and the 
 * created thread does not block, the caller will suspend until the 
 * child has terminated and receives a DANGLING REFERENCE as the 
 * thread id in the return value of pthread_create()! This 
 * implementation could be enhanced by modifying the type pthread_t of
 * a pointer to the thread control block and a serial number which had
 * to be compared with the serial number in the thread control block 
 * for each thread operation. Also, threads had to be allocated from a
 * fixed-size thread control pool or the serial number would become a 
 * "magic number".
 */
int pthread_create(thread, attr, func, arg)
     pthread_t *thread;
     pthread_attr_t *attr;
     pthread_func_t func;
     any_t arg;
{
  register pthread_t t;
  pthread_t parent_t = mac_pthread_self();
  PTRACEIN;

  if (!attr)
    attr = &pthread_attr_default;

  if (!attr->flags || thread == NULL)
    return(EINVAL);

#ifdef REAL_TIME
  {
    struct timespec now;

    clock_gettime(CLOCK_REALTIME, &now);
    if ((ISNTIMERSET(attr->starttime) && !GTEQ_NTIME(attr->starttime, now)) ||
	(ISNTIMERSET(attr->deadline)  && !GTEQ_NTIME(attr->deadline , now)) ||
	(ISNTIMERSET(attr->period) && !ISNTIMERSET(attr->starttime)))
      return(EINVAL);
  }
#endif /* REAL_TIME */

  t = pthread_alloc(func, arg);
  if (t == NO_PTHREAD)
    return(EAGAIN);

  t->attr.stacksize = attr->stacksize;
  t->attr.stack = attr->stack;
  t->attr.flags = attr->flags;
#ifdef _POSIX_THREADS_PRIO_PROTECT
#ifdef SRP
  t->max_ceiling_prio = NO_PRIO;
  t->new_prio = NO_PRIO;
#endif
#endif
  if (attr->inheritsched == PTHREAD_EXPLICIT_SCHED) {
    t->attr.contentionscope = attr->contentionscope;
    t->attr.inheritsched = attr->inheritsched;
    t->attr.sched = attr->sched;
    t->attr.param.sched_priority = attr->param.sched_priority;
#ifdef _POSIX_THREADS_PRIO_PROTECT
    t->base_prio = attr->param.sched_priority;
#endif
  }
  else {
    t->attr.contentionscope = parent_t->attr.contentionscope;
    t->attr.inheritsched = parent_t->attr.inheritsched;
    t->attr.sched = parent_t->attr.sched;
#ifdef _POSIX_THREADS_PRIO_PROTECT
    t->attr.param.sched_priority = t->base_prio = parent_t->base_prio;
#else
    t->attr.param.sched_priority = parent_t->attr.param.sched_priority;
#endif
  }
#ifdef REAL_TIME
  t->attr.starttime.tv_sec = attr->starttime.tv_sec;
  t->attr.starttime.tv_nsec = attr->starttime.tv_nsec;
  t->attr.deadline.tv_sec = attr->deadline.tv_sec;
  t->attr.deadline.tv_nsec = attr->deadline.tv_nsec;
  t->attr.period.tv_sec = attr->period.tv_sec;
  t->attr.period.tv_nsec = attr->period.tv_nsec;
#endif /* REAL_TIME */
#ifdef DEF_RR
  t->num_timers = 0;
  t->interval.tv_sec = 0;
  t->interval.tv_usec = 0;
#endif
  pthread_queue_init(&t->joinq);
  pthread_queue_init(&t->pt_active_timers);
  pthread_queue_init(&t->pt_free_timers);
  t->pt_name = attr->name;
  t->pt_reentp = &(t->pt_reent);
  _REENT_INIT_PTR(t->pt_reentp);
  
  /*
   * inherit the parent's signal mask
   */
  pthread_sigcpyset2set(&t->mask, &parent_t->mask);
  if (attr->detachstate == PTHREAD_CREATE_DETACHED)
    t->state |= T_DETACHED;
  *thread= t;

  ++n_pthreads; 
  if (!pthread_alloc_stack(t)) {
    return(ENOMEM);
  }
  
  pthread_initialize(t);

  SET_KERNEL_FLAG;
  pthread_q_all_enq(&all, t);