sys_arch.c

最新rtlinux内核源码

  struct thread_start_param *thread_param;
  pthread_attr_t attr;

  thread = sys_malloc(sizeof(struct sys_thread));

  if(thread != NULL){

    thread->next = threads;

    SET(thread->flags, 0x00);

    thread->stack = (char *) sys_malloc(sizeof(char)*THREAD_STACK_SIZE);

    thread->timeouts = NULL;

#ifdef THREAD_DEBUG
    thread->name = (char *) sys_malloc(20);
    bcopy(name, thread->name, name_len);
#endif

    if(thread->stack == NULL){
      sys_free(thread);
      rtl_printf("ERROR: Not enough memory to create new thread's stack\n");
      return NULL;
    }

    threads = thread;

    pthread_attr_init(&attr);


    /* Because it's a thread which is trying to create another thread, RTLinux
       only allows that by passing to the new thread it's stack */    
    pthread_attr_setstackaddr(&attr, thread->stack);
    pthread_attr_setstacksize(&attr, THREAD_STACK_SIZE);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
    
    thread_param = sys_malloc(sizeof(struct thread_start_param));
    
    if(thread_param == NULL){
      sys_free(thread->stack);
      sys_free(thread);
      rtl_printf("ERROR: Not enough memory to create thread start param\n");
      return NULL;
    }
    
    thread_param->function = function;
    thread_param->arg = arg;
    thread_param->thread = thread;
    
    if(pthread_create(&(thread->pthread),&attr, thread_start, thread_param)) {
      rtl_printf("\nsys_thread_new: pthread_create  0x%x 0x%x\n", pthread_self(),thread->pthread);
      rtl_printf("Kernel Panic\n");
      return NULL;
    }

    if(period != 0)
      pthread_make_periodic_np(thread->pthread, gethrtime(), period);

    return (void *) thread->pthread;
  }
  
  rtl_printf("ERROR: Not enough memory to create thread\n"); 
  return NULL;
}

/*-----------------------------------------------------------------------------------*/
struct sys_mbox *
sys_mbox_new()
{
  struct sys_mbox *mbox;

  mbox = sys_malloc(sizeof(struct sys_mbox));

  if(mbox != NULL){
    
    mbox->first = mbox->last = 0;
    mbox->mail = sys_sem_new_(0);
    mbox->mutex = sys_sem_new_(1);

    return mbox;
  }else{
    rtl_printf("ERROR: Not enough memory to create mbox\n");
    return NULL;
  }
}

/*-----------------------------------------------------------------------------------*/
void
sys_mbox_free(struct sys_mbox *mbox)
{

  if(mbox != SYS_MBOX_NULL) {

    sys_sem_wait(mbox->mutex);
    
    sys_sem_free(mbox->mail);
    sys_sem_free(mbox->mutex);
    mbox->mail = mbox->mutex = NULL;

    sys_free(mbox);
  }
}
/*-----------------------------------------------------------------------------------*/
void
sys_mbox_post(struct sys_mbox *mbox, void *msg)
{
  u8_t first;
  unsigned int state;

  sys_sem_wait(mbox->mutex);
  
  mbox->msgs[mbox->last] = msg;

  if(mbox->last == mbox->first) {
    first = 1;
  } else {
    first = 0;
  }
  
  mbox->last++;
  if(mbox->last == SYS_MBOX_SIZE) {
    mbox->last = 0;
  }

  sys_stop_interrupts(&state);
  
  if(first)
    sys_sem_signal_pre(mbox->mail);

  sys_sem_signal(mbox->mutex);
  
  sys_allow_interrupts(&state);

}
/*-----------------------------------------------------------------------------------*/
u16_t
sys_arch_mbox_fetch(struct sys_mbox *mbox, void **msg, u16_t timeout)
{
  u16_t time = 1;
  
  /* The mutex lock is quick so we don't bother with the timeout
     stuff here. */
  sys_arch_sem_wait(mbox->mutex, 0);
  while(mbox->first == mbox->last) {
    sys_sem_signal(mbox->mutex);
    /* We block while waiting for a mail to arrive in the mailbox. We
       must be prepared to timeout. */
    if(timeout != 0) {

      time = sys_arch_sem_wait(mbox->mail, timeout);
      
      /* If time == 0, the sem_wait timed out, and we return 0. */
      if(time == 0) {
	return 0;
      }
    } else
      sys_arch_sem_wait(mbox->mail, 0);

    sys_arch_sem_wait(mbox->mutex, 0);
  }
  
  if(msg != NULL) {
    *msg = mbox->msgs[mbox->first];
  }
  
  mbox->first++;
  if(mbox->first == SYS_MBOX_SIZE) {
    mbox->first = 0;
  }    
  
  sys_sem_signal(mbox->mutex);
  
  return time;
}

/*-----------------------------------------------------------------------------------*/
struct sys_sem *
sys_sem_new(u8_t count)
{
  return sys_sem_new_(count);
}

/*-----------------------------------------------------------------------------------*/
static struct sys_sem *
sys_sem_new_(u8_t count)
{
  struct sys_sem *sem;

  sem = sys_malloc(sizeof(struct sys_sem));

  if(sem != NULL){
    sem->c = count;
    sem_init(&(sem->sem),0,count);
    return sem;
  }else{
    rtl_printf("ERROR: Not enough memory to create semaphore\n");
    return NULL;
  }
}

/*-----------------------------------------------------------------------------------*/
static u16_t wait_for_semaphore(struct sys_sem *sem, u16_t timeout){
  unsigned int tdiff;
  unsigned long sec, usec;
  struct timeval rtime1, rtime2;
  struct timespec ts;
  struct timezone tz;
  int retval;

  if(timeout > 0) {
    /* Get a timestamp and add the timeout value. */
    gettimeofday(&rtime1, &tz);

    sec = rtime1.tv_sec;
    usec = rtime1.tv_usec;
    usec += timeout % 1000 * 1000;  
    sec += (int)(timeout / 1000) + (int)(usec / 1000000);
    usec = usec % 1000000;
    ts.tv_nsec = usec * 1000;
    ts.tv_sec = sec;
 
    retval = sem_timedwait(&(sem->sem),&ts);   

    if(retval == -1) {
      return 0;
    } else {
      /* Calculate for how long we waited for the cond. */
      gettimeofday(&rtime2, &tz);
      tdiff = (rtime2.tv_sec - rtime1.tv_sec) * 1000 +	
	(rtime2.tv_usec - rtime1.tv_usec) / 1000;
      if(tdiff == 0) {
	return 1;
      }
      return tdiff;
    }
  } else {
    sem_wait(&(sem->sem));
    return 0;
  }
}

/*-----------------------------------------------------------------------------------*/
u16_t
sys_arch_sem_wait(struct sys_sem *sem, u16_t timeout)
{
  u16_t time = 1;

  sem->sem.value = sem->c;

  while(sem->c <= 0) {
    
    if(timeout > 0) {

      time = wait_for_semaphore(sem, timeout);
      if(time == 0) 
	return 0;
    } else {
      wait_for_semaphore(sem,0);
    }
  }

  sem->c--;
  sem->sem.value = sem->c;

  return time;
}

/*-----------------------------------------------------------------------------------*/
int
sys_sem_signal(struct sys_sem *sem)
{

  sem->c++;
  if(sem->c > 1)
    sem->c = 1;

  sem->sem.value = sem->c;

  return sys_sem_post(&(sem->sem));
}

/*-----------------------------------------------------------------------------------*/
void
sys_sem_free(struct sys_sem *sem)
{
  if(sem != NULL)
    sem_destroy(&(sem->sem));

  sys_free(sem);
}

/*-----------------------------------------------------------------------------------*/
void sys_arch_close(void)
{
  struct sys_thread *st;

  for(st = threads; st != NULL; st = st->next) {
    if(AND(st->flags,0x81)== 0x80){ //i.e. It is a standalone thread finished
      sys_free(st->stack);
    }else if(AND(st->flags,0xff)==0x00){ //i.e A thread still working
      pthread_delete_np(st->pthread);
      //This line is necessary because the thread could be standalone.
      //If it is not, nothing happens
      sys_free(st->stack);               
    }else if(AND(st->flags,0xff)==0x01){ //i.e A registered thread still working
      pthread_cancel(st->pthread);
      pthread_join(st->pthread,NULL);
    }
  }

  free_all_resources();

  return;
}

/*-----------------------------------------------------------------------------------*/
void
sys_init(void)
{
  int i,retval;

  for(i=0; i<=500; i++)
    mallocs[i] = NULL;

  for(i=0; i<MAX_TIMERS; i++){
    vector_of_timers[i].signal = RTL_SIGRTMIN + signal++;
    vector_of_timers[i].timer_event_spec.sigev_notify = SIGEV_SIGNAL;
    vector_of_timers[i].timer_event_spec.sigev_signo = vector_of_timers[i].signal;
    vector_of_timers[i].timer_event_spec.sigev_value.sival_int = 13;
    
    retval = timer_create(CLOCK_REALTIME, &(vector_of_timers[i].timer_event_spec), &(vector_of_timers[i].timer));
    
    if (retval) {
      rtl_printf("timer_create(CLOCK_REALTIME) failed:\n");
    }
  }
}

/*-----------------------------------------------------------------------------------*/
void
sys_sem_wait(sys_sem_t sem)
{
  sys_arch_sem_wait(sem, 0);
}

/*-----------------------------------------------------------------------------------*/
int sys_sem_wait_timeout(sys_sem_t sem, u32_t timeout)
{
  return sys_arch_sem_wait(sem, timeout);
}

/*-----------------------------------------------------------------------------------*/
void
sys_mbox_fetch(sys_mbox_t mbox, void **msg)
{
  sys_arch_mbox_fetch(mbox, msg, 0);
}

/*-----------------------------------------------------------------------------------*/
void sys_timeout(u16_t msecs, sys_timeout_handler h, void *arg)
{
  struct sys_thread *thread;
  int err, sec = 0, nsec = 0;

  thread = sys_current_thread();

  if(thread->timeouts == NULL){
    thread->timeouts = &vector_of_timers[timer_index++];
  }

  thread->timeouts->sa.sa_handler = h;
  thread->timeouts->sa.sa_mask=0;
  thread->timeouts->sa.sa_flags=0;
  thread->timeouts->sa.sa_focus=0;

  rtl_sigemptyset(&(thread->timeouts->sa.sa_mask));

  if (sigaction(thread->timeouts->signal, &(thread->timeouts->sa), NULL)) {
    rtl_printf("sigaction failed");
  }  

  if(msecs >= 100){
    sec = msecs / 1000;
    nsec = (msecs % 1000) * 1000000 ;
  }else{
    nsec = msecs * 1000000;
  }

  thread->timeouts->ospec.it_value.tv_sec = sec;
  thread->timeouts->ospec.it_value.tv_nsec = nsec;
  thread->timeouts->ospec.it_interval.tv_sec = 0;
  thread->timeouts->ospec.it_interval.tv_nsec = 0;

  err = timer_settime(thread->timeouts->timer, 0, &(thread->timeouts->ospec), &(thread->timeouts->old_setting));

  return;  
}

/*-----------------------------------------------------------------------------------*/
void sys_untimeout(sys_timeout_handler h, void *arg){
  struct sys_thread *thread;

  thread = sys_current_thread();

  if(thread->timeouts != NULL)
    timer_delete(thread->timeouts->timer);
}