linuxwrapper.c

stos Linux 源码示范程序。 可以移植到其他平台

/* ************************************************** */
/*
 * semaphore_delete
 */
/* ************************************************** */
int semaphore_delete (semaphore_t *sem)
{
    SemaphoreList_t   * Current_p = NULL; /* to avoid warning */
    SemaphoreList_t   * Deleted_p = NULL;

#ifdef MODULE
    sem = sem;  /* To avoid warnings */
    
    /* Nothing to do */
#else

	int                 ret;

#ifdef SEMAPHORE_DEBUG
    PrintTrace(("sem_del:%x\n",(unsigned long) sem));
#endif

    /* The mutex must be unlocked to be deleted */
    do
    {
        ret = pthread_mutex_unlock(sem);
    } while ((ret != 0) && (ret != EINVAL));

    if (ret != 0)
    {
        PrintTrace(("COMPAT: semaphore_delete problem: unlock 0x%x (%d)!!!!!\n\n", (U32) sem, ret));
    }

    ret=pthread_mutex_destroy(sem); /* Always returns EBUSY on 2.6 kernel !!! */
#endif /* MODULE */

    /* Frees the semaphore in the list */
    locker_lock(semaphore_mutex);
    if (SemaphoreList_p != NULL)
	{
	    if (SemaphoreList_p->Semaphore_p == sem)
	    {
	        Deleted_p = SemaphoreList_p;
	    }
        else
        {
            Current_p = SemaphoreList_p;
     	 	while (   (Current_p->Next_p != NULL)
     	 	       && (Deleted_p == NULL))
     	 	{
     	 	    if (Current_p->Next_p->Semaphore_p == sem)
     	 	    {
     	 	        /* Element to destroy has been found */
     	 	        Deleted_p = Current_p->Next_p;
     	 	    }
                else
                {
                    /* Goes to next element */
     	   		    Current_p = Current_p->Next_p;
     	   		}
     	   	}
 	   	}

        if (Deleted_p != NULL)
        {
            if (Deleted_p == SemaphoreList_p)
            {
                SemaphoreList_p = Deleted_p->Next_p;
            }
            else
            {
                Current_p->Next_p = Deleted_p->Next_p;
            }

            memory_deallocate(NULL, Deleted_p->Semaphore_p);
            memory_deallocate(NULL, Deleted_p);
        }
        else
        {
#if 0
            /* This trace is deactivated since all semaphore are not allocated */
            /* through semaphore_create_fifo() */
            /* This has to be reactivated when all semaphore_init function will be deactivated */
            PrintTrace(("COMPAT: semaphore_delete problem (0x%x), not found !!!\n", sem));
#endif
        }

 	}
    locker_unlock(semaphore_mutex);
    
    return(0);
}


/* ************************************************** */
/*
 * semaphore_wait
 */
/* ************************************************** */
int semaphore_wait(semaphore_t *sem)
{
#ifdef MODULE
    int     ret;

    ret = wait_event_interruptible(sem->wait_queue,
				   (atomic_read(&sem->Count) <= 0));
    if (ret == 0)
    {
	atomic_inc(&sem->Count);
    }

    return ret;
#else

#ifdef SEMAPHORE_DEBUG
    PrintTrace(("sem_wait:%x\n",(unsigned long) sem));
#endif

    return (pthread_mutex_lock(sem) == 0 ? 0 : -1);

#endif  /* MODULE */
}

/* ************************************************** */
/*
 * semaphore_wait_timeout
 */
/* ************************************************** */
int semaphore_wait_timeout(semaphore_t *sem, clock_t * timeout_p)
{
#ifdef MODULE
    int ret = -1;

    if (TIMEOUT_IMMEDIATE == timeout_p)
    {
        ret = wait_event_interruptible_timeout(sem->wait_queue,
				   (atomic_read(&sem->Count) <= 0), 1);
        if (ret > 0)
    	{
	    atomic_inc(&sem->Count);
            ret = 0;
        }
        else
        if (ret == 0)
    	{
            ret = -1;
        }
    }
    else if (TIMEOUT_INFINITY == timeout_p) /* evil */
    {
        ret = semaphore_wait(sem);
    }
    else
    {
        clock_t timeout;

        timeout = time_minus (*timeout_p, time_now());

        if ((U32)timeout > 0x80000000)
        {
            printk("\n\n\nWARNING!!!\nTimeout may have looped (%d) ...\nPlease check ...\n\n\n", (int)timeout);
        }
    
        ret = wait_event_interruptible_timeout(sem->wait_queue,
				               (atomic_read(&sem->Count) <= 0),
					       timeout);
    	if (ret > 0)
    	{
	    atomic_inc(&sem->Count);
            ret = 0;
        }
        else
        if (ret == 0)
    	{
            ret = -1;
        }
    }

    return ret;

#else   /* MODULE */

     struct timespec abstime;
     struct timeval  tval_timeout;
     int      ret = -1; /* Timeout by default */

 

     if (timeout_p == TIMEOUT_IMMEDIATE)
     {
         ret = pthread_mutex_trylock(sem);
#ifdef SEMAPHORE_DEBUG                    
         PrintTrace(("Timeout_IMMEDIATE\n"));
#endif                 
     }
     else
     if (timeout_p == TIMEOUT_INFINITY)
     {
         ret = pthread_mutex_lock(sem);
#ifdef SEMAPHORE_DEBUG                    
         PrintTrace(("Timeout_INFINITY\n"));
#endif                 
     }
     else
     {
#ifdef SEMAPHORE_DEBUG                    
         PrintTrace(("Timeout_other\n"));
#endif                 
         tval_timeout = Clockt2Timeval(*timeout_p);
         if (   (tval_timeout.tv_sec == 0)
             && (tval_timeout.tv_usec == 0))
         {
             /* Error case current time could not be obtained */
             /* try immediate timeout ... */
#ifdef SEMAPHORE_DEBUG
             PrintTrace(("current time could not be obtained"));
             fflush(stdout);
#endif
             ret = pthread_mutex_trylock(sem);
         }
         else
         {
#ifdef SEMAPHORE_DEBUG
             clock_t now_time;
             struct timeval  tval_now;
             unsigned long calc_time_diff;

             now_time = time_now();
             tval_now = Clockt2Timeval(now_time);
             calc_time_diff = ((tval_timeout.tv_sec % 1000) * 1000) + (tval_timeout.tv_usec / 1000)
                               - (((tval_now.tv_sec % 1000) * 1000) + (tval_now.tv_usec / 1000) + 2) ;

/*             PrintTrace(("diff:%u\n",calc_time_diff));*/

             if (calc_time_diff <=0)
             {
                PrintTrace(("timeout < 2ms !!!\n"));
             }
#endif

             /* Wrap timeval to timespec structures */
             abstime.tv_sec  = tval_timeout.tv_sec;
             abstime.tv_nsec = ((long)tval_timeout.tv_usec*1000);

#ifdef SEMAPHORE_DEBUG
             PrintTrace(("MUTEX timeout at %08u.%06u", abstime.tv_sec, abstime.tv_nsec));
             fflush(stdout);
#endif
             ret = pthread_mutex_timedlock(sem, &abstime);

#ifdef SEMAPHORE_DEBUG
             PrintTrace(("pthread_mutex_timedlock: %d:%x !!!!\n", ret, ret));
#endif                 
 

#ifdef SEMAPHORE_DEBUG                    
             if (ret == ETIMEDOUT)
             {
                PrintTrace(("TIMEOUT!"));
                fflush(stdout);
             }
#endif
         }
     }

     if (   (ret != 0)
         && (ret != ETIMEDOUT)
         && (ret != EBUSY))
     {
         PrintTrace(("semaphore_wait_timeout: pb:%d !!!!\n", ret));
     }

#ifdef SEMAPHORE_DEBUG
     PrintTrace(("sem_waitlockto:%x\n",(unsigned long) sem));
#endif

     return (ret == 0 ? 0 : -1);

#endif  /* MODULE */
}

/* ************************************************** */
/*
 * semaphore_signal
 */
/* ************************************************** */
int semaphore_signal(semaphore_t *sem)
{
#ifdef MODULE

    atomic_dec(&sem->Count);
    wake_up_interruptible(&(sem->wait_queue));

    return 0;

#else
    int mut_ret;

#ifdef SEMAPHORE_DEBUG
    PrintTrace(("sem_sig:%x\n",(unsigned long) sem));
#endif
    
    mut_ret = pthread_mutex_unlock(sem) == 0 ? 0 : -1;
    sched_yield();

    return (mut_ret);
#endif /* MODULE */
}



/****************************************************/
/*                	TASKS							*/
/****************************************************/
/* ************** task delay ********************** */

#ifdef MODULE
        /****************/
        /* kernel mode  */
        /****************/
/* ***************************************** */
int task_delay(int jif)
{
    /* Will perform rescheduling with delay of about the ms */
    
    /* Transforming jiffies (actually HZ) in ms */
    /* msleep((unsigned int) STLINUX_GetClocksPerSecond() * 1000 / jif);*/
    msleep(jif * 1000 / (unsigned int) STLINUX_GetClocksPerSecond());
    /* msleep_interruptible((unsigned int) STLINUX_GetClocksPerSecond() * 1000 / jif);*/

    return -1;
}
/* ***************************************** */
int task_delay_until(clock_t TimeUntil)
{
    int delay_in_jiffies;
    
    if ((delay_in_jiffies = ((unsigned long)TimeUntil - (unsigned long)jiffies)) > 0)
    {
        task_delay(delay_in_jiffies);
    }
    
    return -1;
}
/* ***************************************** */

#else

        /**************/
        /* user mode  */
        /**************/

/* This function was originally replaced by linux_delay */
int task_delay(int ticks)
{
    struct timespec x;

    if (ClkPerSec == 0)
        ClkPerSec = STLINUX_GetClocksPerSecond();

    /* if < 2ms */    
    if (ticks <= (2 * ClkPerSec / 1000 ))
    {
        x.tv_sec  = 0;
        x.tv_nsec = 2000001;
    }
    else
    {
        x.tv_sec  = ticks / ClkPerSec;
        x.tv_nsec = (((ticks % ClkPerSec) * 1000000) / ClkPerSec) * 1000;
    }

    nanosleep (&x, 0);

    return -1;
}
#endif  /* MODULE */

/****************************************************/

task_context_t task_context(task_t **task, int* level)
{
#ifdef MODULE
    if (in_irq())
    {
        /* We are in an interrupt */
        return task_context_interrupt;
    }
    else
#endif
    {
        /* We are in a softirq (even defered) or a kernel thread */
        return task_context_task;
    }
}



/****************************************************/
/*              TASK_LOCK/UNLOCK					*/
/****************************************************/

#ifdef MODULE
/* Already defined in <linux/sched.h> */

#else

void task_lock(void)
{
    locker_lock(compat_mutex);
}

void task_unlock(void)
{
    locker_unlock(compat_mutex);
}
#endif  /* !MODULE */




/****************************************************/
/*                MESSAGES							*/
/****************************************************/

static void Init_Allqueues(void)
{
    MessageElem_t * Elem_p;
    int				Index;

	if (! MessageInitDone)
	{
		MessageInitDone = TRUE;
		for (Index=0 , Elem_p = MessageArray ; Index<QUEUE_NB_MAX ; Index++ , Elem_p++)
        {
			Elem_p->Memory_p = NULL;
			Elem_p->Used_p = NULL;
			Elem_p->Pending_p = NULL;
    		Elem_p->ElementSize = 0;
    		Elem_p->NoElements = 0;
		}

		PrintMessageQ_Debug(("COMPAT: message initialization done !!!\n"));
    }
}

/*
 * message_init_queue
 */
void message_init_queue(message_queue_t * MessageQueue,
                        void * Memory_p,
						size_t ElementSize,
                        unsigned int NoElements)
{
    MessageElem_t * Elem_p;
    int				Index;

    locker_lock(message_mutex);
    Init_Allqueues();

    /* Seeking for unused queue */
	Index = 0;
    Elem_p = MessageArray;
	while ((Index<QUEUE_NB_MAX) && (Elem_p->Memory_p != NULL))
	{
		Index++;
		Elem_p++;;
	}

    MessageQueue->Index = Index;

    if (Index < QUEUE_NB_MAX)   /* if valid unused queue has been found */
	{
	    Elem_p = &MessageArray[ MessageQueue->Index ];

    	Elem_p->Used_p = memory_allocate(NULL, NoElements*sizeof(BOOL));    /* Allocates used/unused element array */
        if (Elem_p->Used_p != NULL)
		{
            memset((void *)Elem_p->Used_p, 0, NoElements*sizeof(BOOL));     /* Sets all elements to unused */

    		Elem_p->Memory_p = Memory_p;
			Elem_p->Pending_p = NULL;
    		Elem_p->ElementSize = ElementSize;
    		Elem_p->NoElements = NoElements;

            MessageQueue->MsgSemaphore_p = semaphore_create_fifo(0);
            MessageQueue->ClaimSemaphore_p = semaphore_create_fifo(0);

			PrintMessageQ_Debug(("COMPAT: Message_queue initialized: %d\n", MessageQueue->Index));
		}
		else
		{
			PrintTrace(("COMPAT: message_init_queue, No memory !!!\n"));
		}
	}
	else
	{
		PrintTrace(("COMPAT: message_init_queue, No memory !!!\n"));
	}
    locker_unlock(message_mutex);
}

/*
 * message_init_queue_timeout
 */
void message_init_queue_timeout(message_queue_t * MessageQueue,
                                void * Memory_p,
								size_t ElementSize,
                                unsigned int NoElements)
{
    MessageElem_t * Elem_p;
    int				Index;

    locker_lock(message_mutex);
    Init_Allqueues();

    /* Seeking for unused queue */
	Index = 0;
    Elem_p = MessageArray;
	while ((Index<QUEUE_NB_MAX) && (Elem_p->Memory_p != NULL))
	{
		Index++;
		Elem_p++;;
	}

    MessageQueue->Index = Index;

    if (Index < QUEUE_NB_MAX)   /* if valid unused queue has been found */
	{
	    Elem_p = &MessageArray[ MessageQueue->Index ];

    	Elem_p->Used_p = memory_allocate(NULL, NoElements*sizeof(BOOL));    /* Allocates used/unused element array */
        if (Elem_p->Used_p != NULL)
		{
            memset((void *)Elem_p->Used_p, 0, NoElements*sizeof(BOOL));     /* Sets all elements to unused */

    		Elem_p->Memory_p = Memory_p;
			Elem_p->Pending_p = NULL;