linuxwrapper.c

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

    		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_timeout, No memory !!!\n"));
	    }
	}
	else
	{
		PrintTrace(("COMPAT: message_init_queue_timeout, No memory !!!\n"));
	}
    locker_unlock(message_mutex);
}

/*
 * message_create_queue_timeout
 */
message_queue_t * message_create_queue_timeout(size_t ElementSize, unsigned int NoElements)
{
    MessageQueueList_t  * Current_p;
    MessageQueueList_t  * New_p;
    message_queue_t     * MesQ_p = NULL;

    if ((New_p = memory_allocate(NULL, sizeof(MessageQueueList_t))) != NULL)
    {
        if ((New_p->MessageQueue_p = memory_allocate(NULL, sizeof(message_queue_t))) != NULL)
        {
            if ((New_p->Memory_p = memory_allocate(NULL, MESSAGE_MEMSIZE_QUEUE(ElementSize, NoElements))) == NULL)
            {
                /* Memory allocation pb, deallocate list element */
                memory_deallocate(NULL, New_p->MessageQueue_p);
                memory_deallocate(NULL, New_p);
                New_p = NULL;
            }
        }
        else
        {
            /* Memory allocation pb, deallocate list element */
            memory_deallocate(NULL, New_p);
            New_p = NULL;
        }
    }

    if (New_p != NULL)
    {
        message_init_queue_timeout( New_p->MessageQueue_p, New_p->Memory_p,
                                    ElementSize, NoElements);

        if (MessageArray[ New_p->MessageQueue_p->Index ].Used_p != NULL)
        {
            /* At this stage, we know that all allocations have been correctly done */
            New_p->Next_p = NULL;

            locker_lock(message_mutex);
            Current_p = MessageQueueList_p;
            if (Current_p != NULL)
            {
                while (Current_p->Next_p != NULL)
                    Current_p = Current_p->Next_p;

                Current_p->Next_p = New_p;
            }
            else
            {
                MessageQueueList_p = New_p;
            }
            locker_unlock(message_mutex);

            MesQ_p = New_p->MessageQueue_p;

	        PrintMessageQ_Debug(("COMPAT: message_create_queue_timeout done (0x%x)!!!\n", MesQ_p));
        }
        else
        {
            message_delete_queue(New_p->MessageQueue_p);

            memory_deallocate(NULL, New_p->MessageQueue_p);
            memory_deallocate(NULL, New_p->Memory_p);
            memory_deallocate(NULL, New_p);
            PrintTrace(("COMPAT: message_create_queue_timeout, message queue init problem !!!\n"));
        }
    }
    else
    {
        PrintTrace(("COMPAT: message_create_queue_timeout, no memory !!!\n"));
    }

    /* NULL if Message Queue creation problem, message queue pointer otherwise */
    return(MesQ_p);
}

/*
 * message_delete_queue
 */
void message_delete_queue(message_queue_t * MessageQueue)
{
    MessageQueueList_t  * Current_p = NULL; /* to avoid warning */
    MessageQueueList_t  * Deleted_p = NULL;
	MessageSend_t       * Pending_p;
    MessageElem_t       * Elem_p;

	if (! MessageInitDone)
	{
		PrintTrace(("COMPAT: message_delete_queue, not initialized !!!\n"));
		return;
	}

    if (MessageQueue->Index < QUEUE_NB_MAX)
	{
        Elem_p = &MessageArray[ MessageQueue->Index ];

        /* Delete associated semaphores */
        semaphore_delete(MessageQueue->MsgSemaphore_p);
        semaphore_delete(MessageQueue->ClaimSemaphore_p);

        locker_lock(message_mutex);

	    Elem_p->Memory_p = NULL;
		Pending_p = Elem_p->Pending_p;
		while (Pending_p != NULL)
		{
			Elem_p->Pending_p = Elem_p->Pending_p->Next_p;
	    	memory_deallocate(NULL, Pending_p);
			Pending_p = Elem_p->Pending_p;
		}
    	Elem_p->ElementSize = 0;
    	Elem_p->NoElements = 0;
    	if (Elem_p->Used_p != NULL)
    	{
    	    memory_deallocate(NULL, Elem_p->Used_p);
    	    Elem_p->Used_p = NULL;
        }

        locker_unlock(message_mutex);
	}
	MessageQueue->Index = QUEUE_NB_MAX;

    /* Frees the message queue in the list */
    locker_lock(message_mutex);
    if (MessageQueueList_p != NULL)
    {
        if (MessageQueueList_p->MessageQueue_p == MessageQueue)
        {
            Deleted_p = MessageQueueList_p;
        }
        else
        {
            Current_p = MessageQueueList_p;
            while (   (Current_p->Next_p != NULL)
                   && (Deleted_p == NULL))
            {
                if (Current_p->Next_p->MessageQueue_p == MessageQueue)
                {
                    /* 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 == MessageQueueList_p)
            {
                MessageQueueList_p = Deleted_p->Next_p;
            }
            else
            {
                Current_p->Next_p = Deleted_p->Next_p;
            }

            memory_deallocate(NULL, Deleted_p->Memory_p);
            memory_deallocate(NULL, Deleted_p->MessageQueue_p);
            memory_deallocate(NULL, Deleted_p);
        }
        else
        {
            PrintTrace(("COMPAT: message_delete problem (0x%x), not found !!!\n", (U32)MessageQueue));
        }
    }
    locker_unlock(message_mutex);

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

/*
 * message_claim_timeout
 */
int message_claim_timeout(message_queue_t * MessageQueue, clock_t * time_end_p)
{
    MessageElem_t * Elem_p;
    int				i;
    BOOL            SemWait = FALSE;

	if (! MessageInitDone)
	{
		PrintTrace(("COMPAT: message_claim_timeout, not initialized !!!\n"));
		return((int)NULL);
	}

    if (MessageQueue->Index < QUEUE_NB_MAX)
	{
		PrintMessageQ_Debug(("COMPAT: Claiming (%d)\n", MessageQueue->Index));

        Elem_p = &MessageArray[ MessageQueue->Index ];
        
		do
		{
		    if (SemWait)
		    {
                PrintMessageQ_FullDebug(("COMPAT: Semaphore unlocked (%d)\n", MessageQueue->Index));
		    }

            /* Scan every element to find first free one */
            locker_lock(message_mutex);
			for (i=0 ; i<Elem_p->NoElements ; i++)
			{
				if (! Elem_p->Used_p[i])
				{
		            PrintMessageQ_Debug(("COMPAT: Claiming done, elem:%d\n", i));

					Elem_p->Used_p[i] = TRUE;

                    locker_unlock(message_mutex);
					return( (int)(Elem_p->Memory_p) + (int)(i * Elem_p->ElementSize) );
				}
			}
            locker_unlock(message_mutex);

           /* No free message has been found, waiting for message release */
            SemWait = TRUE;
            PrintMessageQ_FullDebug(("COMPAT: message_claim, No free message, waiting ...\n"));
		}
		while (semaphore_wait_timeout(MessageQueue->ClaimSemaphore_p, time_end_p) == 0);

		PrintMessageQ_FullDebug(("COMPAT: Claiming timeout\n"));
	}

	return((int)NULL);
}

/*
 * message_claim
 */
int message_claim(message_queue_t * MessageQueue)
{
	if (! MessageInitDone)
	{
		PrintTrace(("COMPAT: message_claim, not initialized !!!\n"));
		return((int)NULL);
	}

    return( message_claim_timeout(MessageQueue, TIMEOUT_INFINITY) );
}

/*
 * message_release
 */
void message_release (message_queue_t* MessageQueue, void* Message)
{
    MessageElem_t * Elem_p;
	U32				Index;

	if (! MessageInitDone)
	{
		PrintTrace(("COMPAT: message_release, not initialized !!!\n"));
		return;
	}

    if (MessageQueue->Index < QUEUE_NB_MAX)
	{
		PrintMessageQ_Debug(("COMPAT: Releasing (%d)\n", MessageQueue->Index));

        Elem_p = &MessageArray[ MessageQueue->Index ];

        locker_lock(message_mutex);
        if (   ((U32)Message >= (U32)(Elem_p->Memory_p))
		    && ((U32)Message < (U32)(Elem_p->Memory_p) + (Elem_p->ElementSize*Elem_p->NoElements)) )
		{
			Index = ((U32)Message - (U32)(Elem_p->Memory_p))/((U32)(Elem_p->ElementSize));
			Elem_p->Used_p[Index] = FALSE;

			PrintMessageQ_Debug(("COMPAT: %d released\n", Index));

            semaphore_signal(MessageQueue->ClaimSemaphore_p);
		}
		else
		{
			PrintTrace(("COMPAT: message_release pb !!!\n"));
		}
        locker_unlock(message_mutex);
	}
}


/*
 * message_send
 */
int message_send(message_queue_t * MessageQueue, void * message)
{
	MessageSend_t  * Pending_p;
	MessageSend_t  * New_p;

	if (! MessageInitDone)
	{
		PrintTrace(("COMPAT: message_send, not initialized !!!\n"));
		return 0;
	}

    if (MessageQueue->Index < QUEUE_NB_MAX)
	{
		PrintMessageQ_Debug(("COMPAT: Sending (%d)\n", MessageQueue->Index));

    	New_p = memory_allocate(NULL, sizeof(MessageSend_t));
        if (New_p != NULL)
		{
			New_p->Next_p = NULL;
			New_p->Message_p = message;

	    	PrintMessageQ_Debug(("COMPAT: Sending (0x%x)\n", New_p->Message_p));

            locker_lock(message_mutex);
			Pending_p = MessageArray[ MessageQueue->Index ].Pending_p;
	        if (Pending_p != NULL)
			{
				while (Pending_p->Next_p != NULL)
					Pending_p = Pending_p->Next_p;

				Pending_p->Next_p = New_p;
			}
			else
			{
				MessageArray[ MessageQueue->Index ].Pending_p = New_p;
			}
            locker_unlock(message_mutex);

	    	semaphore_signal(MessageQueue->MsgSemaphore_p);
		}
		else
		{
			PrintTrace(("COMPAT: message_send, no memory !!!\n"));
		}
    }

	return 0;
}

/*
 * message_receive_timeout
 */
void * message_receive_timeout (message_queue_t* MessageQueue, clock_t * ticks_p)
{
	MessageSend_t  * Pending_p;
    MessageElem_t  * Elem_p;
    void 		   * Message_p = NULL;
    BOOL             SemWait = FALSE;

	if (! MessageInitDone)
	{
		PrintTrace(("COMPAT: message_receive_timeout, not initialized !!!\n"));
		return (Message_p);
	}

    if (MessageQueue->Index < QUEUE_NB_MAX)
	{
		PrintMessageQ_FullDebug(("COMPAT: Receiving (%d)\n", MessageQueue->Index));

#ifdef MESSAGE_QUEUE_DEBUG
		Elem_p = &MessageArray[ MessageQueue->Index ];

        locker_lock(message_mutex);
		Pending_p = Elem_p->Pending_p;
		if (Pending_p != NULL)
		{
			PrintMessageQ_Debug(("COMPAT: Queue %d: ", MessageQueue->Index));
			while (Pending_p != NULL)
			{
			   PrintMessageQ_Debug(("-> 0x%x ", Pending_p->Message_p));
			   Pending_p = Pending_p->Next_p;
			}
			PrintMessageQ_Debug(("-> NULL\n"));
		}
        locker_unlock(message_mutex);
#endif

	    do
		{
		    if (SemWait)
		    {
                PrintMessageQ_FullDebug(("COMPAT: Semaphore unlocked (%d)\n", MessageQueue->Index));
		    }

	        Elem_p = &MessageArray[ MessageQueue->Index ];

            locker_lock(message_mutex);
			Pending_p = Elem_p->Pending_p;
			if (Pending_p != NULL)
			{
				Elem_p->Pending_p = Pending_p->Next_p;
				Message_p = Pending_p->Message_p;
    			memory_deallocate(NULL, Pending_p);

				PrintMessageQ_Debug(("COMPAT: Queue %d: Received 0x%x \n", MessageQueue->Index, Message_p));

                locker_unlock(message_mutex);
                return (Message_p);
			}
            locker_unlock(message_mutex);

            SemWait = TRUE;
            PrintMessageQ_FullDebug(("COMPAT: message_receive (%d), No message available, waiting ...\n", MessageQueue->Index));
		}
		while (semaphore_wait_timeout(MessageQueue->MsgSemaphore_p, ticks_p) == 0);

        PrintMessageQ_FullDebug(("COMPAT: Receive timeout (%d) \n", MessageQueue->Index));
    }

	return (Message_p);
}

/*
 * message_receive
 */
void * message_receive (message_queue_t* MessageQueue)
{
	if (! MessageInitDone)
	{
		PrintTrace(("COMPAT: message_receive, not initialized !!!\n"));
		return ((void *)NULL);
	}

    return( message_receive_timeout(MessageQueue, TIMEOUT_INFINITY) );
}




/****************************************************/
/*                		TIME   						*/
/****************************************************/

#ifdef MODULE
    /* 
       LM: time must be used in jiffies everywhere, so clock_t should be replaced or redefined by jiffies
       As it is not possible without warning (drivers are using OS20/OS21 clock_t type where the linux kernel
       is using it for a user purpose), I made type conversion only.
       A clean implementation should be to introduce STOS_Clock_t in each driver.
       clock_t is actually (long) jiffies in this implementation.
    */
    
/* ***** */
clock_t  time_now (void)
{
    return((long)(jiffies));
}
/* ***** */
clock_t time_plus (clock_t Time1, clock_t Time2)
{
/*    printk("Time1: %d, Time2: %d, Res: %d\n", (int)Time1, (int)Time2, (int)(Time1 + Time2));*/

    return ((long)((unsigned long)Time1 + (unsigned long)Time2)); /* roundup needed ? */
}
/* ***** */
int time_after_STAPI (clock_t Time1, clock_t Time2)
{
    return (time_after((unsigned long)Time1, (unsigned long)Time2));
}
/* ***** */
clock_t time_minus (clock_t Time1, clock_t Time2)
{
    return ((long)((unsigned long)Time1 - (unsigned long)Time2));
}
/* ***** */
#endif  /* MODULE */

/* ******** End of kernel 2.6 section ********* */

#if !defined(MODULE)

void linux_delay(int a) /* a in ms */
{
    struct timespec x;

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

    nanosleep (&x, 0);
}

/*
 * Clockt2Timeval
 */
struct timeval Clockt2Timeval(clock_t clock)
{
    struct timeval	tval;
    int				USecPerClk;    /* Micro seconds per clock (typically 64) */

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

	if (   (gettimeofday(&tval, NULL) != 0)
	    || (ClkPerSec == 0))
    {
		PrintTrace(("Clockt2Timeval Failed: gettimeofday or STLINUX_GetClocksPerSecond failed !!!!\n"));

		tval.tv_sec  = 0;
		tval.tv_usec = 0;

		return(tval);
	}

    USecPerClk = 1000000/ClkPerSec;

	/* Retrieve seconds which were deleted due to restriction on seconds in time_now() and add seconds of clock to translate */
	tval.tv_sec  = (tval.tv_sec / (TIME_NOW_CONSTRAINTS_ON_SECONDS-1))*(TIME_NOW_CONSTRAINTS_ON_SECONDS-1) + clock/ClkPerSec;
	tval.tv_usec = (clock % ClkPerSec)*USecPerClk;

	return(tval);
}

/*
 * time_now