C++ ASSERT_ERRNO_AND_EXIT函数代码示例

/*!
 * Create global message queue
 */
int sys__create_msg_queue ( void *p )
{
	/* parameters on thread stack */
	msg_q *msgq;
	uint min_prio;
	/* local variables */
	kgmsg_q *gmsgq;

	msgq = *( (msg_q **) p );	p += sizeof (msg_q *);
	min_prio = *( (uint *) p );

	ASSERT_ERRNO_AND_EXIT ( msgq, E_INVALID_HANDLE );

	msgq = U2K_GET_ADR ( msgq, k_get_active_process () );

	gmsgq = kmalloc ( sizeof ( kgmsg_q ) );
	ASSERT_ERRNO_AND_EXIT ( gmsgq, E_NO_MEMORY );

	list_init ( &gmsgq->mq.msgs ); /* list for messages */
	k_threadq_init ( &gmsgq->mq.thrq ); /* list for blocked threads */

	gmsgq->mq.min_prio = min_prio;
	msgq->id = gmsgq->id = k_new_unique_id ();
	msgq->handle = gmsgq;

	list_append ( &kmsg_qs, gmsgq, &gmsgq->all ); /* all msg.q. list */

	EXIT ( SUCCESS );
}

/*!
 * Delete global message queue
 */
int sys__delete_msg_queue ( void *p )
{
	/* parameter on thread stack */
	msg_q *msgq;
	/* local variables */
	kgmsg_q *gmsgq;

	msgq = *( (msg_q **) p );

	ASSERT_ERRNO_AND_EXIT ( msgq, E_INVALID_HANDLE );
	msgq = U2K_GET_ADR ( msgq, k_get_active_process () );

	gmsgq = msgq->handle;
	ASSERT_ERRNO_AND_EXIT ( gmsgq->id == msgq->id, E_INVALID_HANDLE );

	k_msgq_clean ( &gmsgq->mq );

	k_release_all_threads ( &gmsgq->mq.thrq );

	k_free_unique_id ( gmsgq->id );

	kfree ( gmsgq );

	msgq->id = 0;
	msgq->handle = NULL;

	EXIT ( SUCCESS );
}

/*!
 * Decrement (lock) semaphore value by 1 (if not 0 when thread is blocked)
 * \param sem Semaphore descriptor (user level descriptor)
 * \return 0 if successful, -1 otherwise and appropriate error number is set
 */
int sys__sem_wait ( sem_t *sem )
{
	ksem_t *ksem;
	kobject_t *kobj;
	kthread_t *kthread;

	SYS_ENTRY();

	ASSERT_ERRNO_AND_EXIT ( sem, EINVAL );

	kthread = kthread_get_active ();

	kobj = sem->ptr;
	ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
	ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
				EINVAL );
	ksem = kobj->kobject;
	ASSERT_ERRNO_AND_EXIT ( ksem && ksem->id == sem->id, EINVAL );

	kthread_set_errno ( kthread, EXIT_SUCCESS );
	kthread_set_syscall_retval ( kthread, EXIT_SUCCESS );

	if ( ksem->sem_value > 0 )
	{
		ksem->sem_value--;
		ksem->last_lock = kthread;
	}
	else {
		kthread_enqueue ( kthread, &ksem->queue );
		kthreads_schedule ();
	}

	SYS_EXIT ( kthread_get_errno(NULL), kthread_get_syscall_retval(NULL) );
}

/*!
 * Initialize semaphore object
 * \param sem Semaphore descriptor (user level descriptor)
 * \param pshared Shall semaphore object be shared between processes
 * \param value Initial semaphore value
 * \return 0 if successful, -1 otherwise and appropriate error number is set
 */
int sys__sem_init ( sem_t *sem, int pshared, int value )
{
	ksem_t *ksem;
	kobject_t *kobj;

	SYS_ENTRY();

	ASSERT_ERRNO_AND_EXIT ( sem, EINVAL );

	kobj = kmalloc_kobject ( sizeof (ksem_t) );
	ASSERT_ERRNO_AND_EXIT ( kobj, ENOMEM );
	ksem = kobj->kobject;

	ksem->id = k_new_id ();
	ksem->sem_value = value;
	ksem->last_lock = NULL;
	ksem->flags = 0;
	ksem->ref_cnt = 1;
	kthreadq_init ( &ksem->queue );

	if ( pshared )
		ksem->flags |= PTHREAD_PROCESS_SHARED;

	sem->ptr = kobj;
	sem->id = ksem->id;

	SYS_EXIT ( EXIT_SUCCESS, EXIT_SUCCESS );
}

/*!
 * Lock mutex object
 * \param mutex Mutex descriptor (user level descriptor)
 * \return 0 if successful, -1 otherwise and appropriate error number is set
 */
int sys__pthread_mutex_lock ( pthread_mutex_t *mutex )
{
	kpthread_mutex_t *kmutex;
	kobject_t *kobj;
	int retval = EXIT_SUCCESS;

	SYS_ENTRY();

	ASSERT_ERRNO_AND_EXIT ( mutex, EINVAL );

	kobj = mutex->ptr;
	ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
	ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
				EINVAL );
	kmutex = kobj->kobject;
	ASSERT_ERRNO_AND_EXIT ( kmutex && kmutex->id == mutex->id, EINVAL );

	retval = mutex_lock ( kmutex, kthread_get_active () );

	if ( retval == 1 )
		kthreads_schedule ();

	 /* errno is already set */
	if ( retval != -1 )
		SYS_RETURN ( EXIT_SUCCESS );
	else
		SYS_RETURN ( EXIT_FAILURE );
}

/*!
 * Send signal to thread (from thread)
 * \param pid Thread descriptor (user level descriptor)
 * \param signo Signal number
 * \param sigval Parameter to send with signal
 * \return 0 if successful, -1 otherwise and appropriate error number is set
 */
int sys__sigqueue ( void *p )
{
	pid_t pid;
	int signo;
	sigval_t sigval;

	pthread_t thread, sender;
	kthread_t *kthread;
	siginfo_t sig;

	pid =    *( (pid_t *) p );	p += sizeof (pid_t);
	signo =  *( (int *) p );	p += sizeof (int);
	sigval = *( (sigval_t *) p );

	ASSERT_ERRNO_AND_EXIT ( signo > 0 && signo <= SIGMAX, EINVAL );

	thread = (pthread_t) pid; /* pid_t should be pthread_t */
	ASSERT_ERRNO_AND_EXIT ( thread.ptr, EINVAL );

	kthread = thread.ptr;
	ASSERT_ERRNO_AND_EXIT ( kthread_get_id (kthread) == thread.id, EINVAL );
	ASSERT_ERRNO_AND_EXIT ( kthread_check_kthread ( kthread ), EINVAL );

	sender.id = kthread_get_id (NULL);
	sender.ptr = kthread_get_active ();

	sig.si_signo = signo;
	sig.si_value = sigval;
	sig.si_pid = sender;
	sig.si_code = SI_USER;
	sig.si_errno = 0;

	EXIT ( ksignal_queue ( kthread, &sig ) );
}

/*!
 * Change scheduling parameters
 * \param thread User level thread descriptor
 * \param policy Thread scheduling policy
 * \param param Additional scheduling parameters (when policy != SCHED_FIFO)
 * \return 0
 */
int sys__pthread_setschedparam ( void *p )
{
	pthread_t *thread;
	int policy;
	sched_param_t *param;

	kthread_t *kthread;

	thread = *( (pthread_t **) p );		p += sizeof (pthread_t *);
	policy = *( (int *) p );		p += sizeof (int);
	param =  *( (sched_param_t **) p );

	kthread = thread->ptr;
	ASSERT_ERRNO_AND_EXIT ( kthread, EINVAL );
	ASSERT_ERRNO_AND_EXIT ( kthread_get_id (kthread) == thread->id, ESRCH );
	ASSERT_ERRNO_AND_EXIT ( kthread_is_alive (kthread), ESRCH );

	ASSERT_ERRNO_AND_EXIT ( policy >= 0 && policy < SCHED_NUM, EINVAL );

	if ( param )
	{
		ASSERT_ERRNO_AND_EXIT (
			param->sched_priority >= THREAD_MIN_PRIO &&
			param->sched_priority <= THREAD_MAX_PRIO, EINVAL );
	}

	return kthread_setschedparam ( kthread, policy, param );
}

/*!
 * Block thread (on conditional variable) and release monitor
 */
int sys__monitor_wait ( monitor_t *monitor, monitor_q *queue )
{
	kmonitor_t *kmonitor;
	kmonitor_q *kqueue;

	ASSERT_ERRNO_AND_EXIT ( monitor && monitor->ptr, E_INVALID_HANDLE );
	ASSERT_ERRNO_AND_EXIT ( queue && queue->ptr, E_INVALID_HANDLE );

	kmonitor = monitor->ptr;
	kqueue = queue->ptr;

	ASSERT_ERRNO_AND_EXIT ( kmonitor->owner == k_get_active_thread (),
				E_NOT_OWNER );

	disable_interrupts ();

	SET_ERRNO ( SUCCESS );

	k_set_thread_qdata ( NULL, kmonitor );
	k_enqueue_thread ( NULL, &kqueue->queue );

	kmonitor->owner = k_threadq_get ( &kmonitor->queue );
	if ( !k_release_thread ( &kmonitor->queue ) )
		kmonitor->lock = FALSE;

	k_schedule_threads ();

	enable_interrupts ();

	EXIT ( SUCCESS );
}

/*! Change thread scheduling parameters ------------------------------------- */
int kthread_setschedparam (kthread_t *kthread, int policy, sched_param_t *param)
{
	int sched_priority;

	ASSERT_ERRNO_AND_EXIT ( kthread, EINVAL );
	ASSERT_ERRNO_AND_EXIT ( kthread_is_alive (kthread), ESRCH );
	ASSERT_ERRNO_AND_EXIT ( policy >= 0 && policy < SCHED_NUM, EINVAL );

	if ( param )
	{
		ASSERT_ERRNO_AND_EXIT (
			param->sched_priority >= THREAD_MIN_PRIO &&
			param->sched_priority <= THREAD_MAX_PRIO, EINVAL );

		if ( param->sched_priority )
			sched_priority = param->sched_priority;
		else
			sched_priority = kthread->sched_priority;
	}
	else {
		sched_priority = kthread->sched_priority;
	}

	/* change in priority? */
	if ( kthread->sched_priority != sched_priority )
		kthread_set_prio ( kthread, sched_priority );

	return EXIT_SUCCESS;
}

/*!
 * Unlock mutex object
 * \param mutex Mutex descriptor (user level descriptor)
 * \return 0 if successful, -1 otherwise and appropriate error number is set
 */
int sys__pthread_mutex_unlock ( void *p )
{
	pthread_mutex_t *mutex;

	kpthread_mutex_t *kmutex;
	kobject_t *kobj;

	mutex = *( (pthread_mutex_t **) p );
	ASSERT_ERRNO_AND_EXIT ( mutex, EINVAL );

	kobj = mutex->ptr;
	ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
	ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
				EINVAL );
	kmutex = kobj->kobject;
	ASSERT_ERRNO_AND_EXIT ( kmutex && kmutex->id == mutex->id, EINVAL );

	if ( kmutex->owner != kthread_get_active() )
	{
		SET_ERRNO ( EPERM );
		return EXIT_FAILURE;
	}

	SET_ERRNO ( EXIT_SUCCESS );

	kmutex->owner = kthreadq_get ( &kmutex->queue );
	if ( kmutex->owner )
	{
		kthreadq_release ( &kmutex->queue );
		kthreads_schedule ();
	}

	return EXIT_SUCCESS;
}

/*!
 * Change scheduling parameters
 * \param thread User level thread descriptor
 * \param policy Thread scheduling policy
 * \param param Additional scheduling parameters (when policy != SCHED_FIFO)
 * \return 0
 */
int sys__pthread_setschedparam ( pthread_t *thread, int policy,
				 sched_param_t *param )
{
	kthread_t *kthread;
	int retval;

	SYS_ENTRY();

	kthread = thread->ptr;
	ASSERT_ERRNO_AND_EXIT ( kthread, EINVAL );
	ASSERT_ERRNO_AND_EXIT ( kthread_get_id (kthread) == thread->id, ESRCH );
	ASSERT_ERRNO_AND_EXIT ( kthread_is_alive (kthread), ESRCH );

	ASSERT_ERRNO_AND_EXIT ( policy == SCHED_FIFO, ENOTSUP );

	if ( param )
	{
		ASSERT_ERRNO_AND_EXIT (
			param->sched_priority >= THREAD_MIN_PRIO &&
			param->sched_priority <= THREAD_MAX_PRIO, EINVAL );
	}

	retval = kthread_setschedparam ( kthread, policy, param );
	if ( retval == EXIT_SUCCESS )
		SYS_EXIT ( EXIT_SUCCESS, retval );
	else
		SYS_EXIT ( retval, EXIT_FAILURE );
}

/*!
 * Destroy semaphore object
 * \param sem Semaphore descriptor (user level descriptor)
 * \return 0 if successful, -1 otherwise and appropriate error number is set
 */
int sys__sem_destroy ( void *p )
{
	sem_t *sem;

	ksem_t *ksem;
	kobject_t *kobj;

	sem = *( (sem_t **) p );

	ASSERT_ERRNO_AND_EXIT ( sem, EINVAL );

	kobj = sem->ptr;
	ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
	ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
				EINVAL );
	ksem = kobj->kobject;
	ASSERT_ERRNO_AND_EXIT ( ksem && ksem->id == sem->id, EINVAL );

	ASSERT_ERRNO_AND_EXIT (kthreadq_get (&ksem->queue) == NULL, ENOTEMPTY);

	ksem->ref_cnt--;

	/* additional cleanup here (e.g. if semaphore is shared leave it) */
	if ( ksem->ref_cnt )
		EXIT2 ( EBUSY, EXIT_FAILURE );

	kfree_kobject ( kobj );

	sem->ptr = NULL;
	sem->id = 0;

	EXIT2 ( EXIT_SUCCESS, EXIT_SUCCESS );
}

/*!
 * Decrement (lock) semaphore value by 1 (if not 0 when thread is blocked)
 * \param sem Semaphore descriptor (user level descriptor)
 * \return 0 if successful, -1 otherwise and appropriate error number is set
 */
int sys__sem_wait ( void *p )
{
	sem_t *sem;

	ksem_t *ksem;
	kobject_t *kobj;
	kthread_t *kthread;

	sem = *( (sem_t **) p );
	ASSERT_ERRNO_AND_EXIT ( sem, EINVAL );

	kthread = kthread_get_active ();

	kobj = sem->ptr;
	ASSERT_ERRNO_AND_EXIT ( kobj, EINVAL );
	ASSERT_ERRNO_AND_EXIT ( list_find ( &kobjects, &kobj->list ),
				EINVAL );
	ksem = kobj->kobject;
	ASSERT_ERRNO_AND_EXIT ( ksem && ksem->id == sem->id, EINVAL );

	kthread_set_errno ( kthread, EXIT_SUCCESS );

	if ( ksem->sem_value > 0 )
	{
		ksem->sem_value--;
		ksem->last_lock = kthread;
	}
	else {
		kthread_enqueue ( kthread, &ksem->queue, 1, NULL, NULL );
		kthreads_schedule ();
	}

	return EXIT_SUCCESS;
}

/*!
 * Create new timer
 * \param clockid	Clock used in timer
 * \param evp		Timer expiration action
 * \param timerid	Timer descriptor is returned in this variable
 * \return status	0 for success
 */
int sys__timer_create ( void *p )
{
	clockid_t clockid;
	sigevent_t *evp;
	timer_t *timerid;

	ktimer_t *ktimer;
	int retval = EXIT_SUCCESS;
	kobject_t *kobj;

	clockid =	*( (clockid_t *) p );	p += sizeof (clockid_t);
	evp =		*( (sigevent_t **) p );	p += sizeof (sigevent_t *);
	timerid =	*( (timer_t **) p );

	ASSERT_ERRNO_AND_EXIT (
	clockid == CLOCK_REALTIME || clockid == CLOCK_MONOTONIC, EINVAL );
	ASSERT_ERRNO_AND_EXIT ( evp && timerid, EINVAL );

	retval = ktimer_create ( clockid, evp, &ktimer, kthread_get_active() );
	if ( retval == EXIT_SUCCESS )
	{
		kobj = kmalloc_kobject ( 0 );
		kobj->kobject = ktimer;
		timerid->id = ktimer->id;
		timerid->ptr = kobj;
	}
	EXIT ( retval );
}

/*!
 * Initialize semaphore object
 * \param sem Semaphore descriptor (user level descriptor)
 * \param pshared Shall semaphore object be shared between processes
 * \param value Initial semaphore value
 * \return 0 if successful, -1 otherwise and appropriate error number is set
 */
int sys__sem_init ( void *p )
{
	sem_t *sem;
	int pshared;
	int value;

	ksem_t *ksem;
	kobject_t *kobj;

	sem =		*( (sem_t **) p );	p += sizeof (sem_t *);
	pshared =	*( (int *) p );		p += sizeof (int);
	value =		*( (uint *) p );

	ASSERT_ERRNO_AND_EXIT ( sem, EINVAL );

	kobj = kmalloc_kobject ( sizeof (ksem_t) );
	ASSERT_ERRNO_AND_EXIT ( kobj, ENOMEM );
	ksem = kobj->kobject;

	ksem->id = k_new_id ();
	ksem->sem_value = value;
	ksem->last_lock = NULL;
	ksem->flags = 0;
	ksem->ref_cnt = 1;
	kthreadq_init ( &ksem->queue );

	if ( pshared )
		ksem->flags |= PTHREAD_PROCESS_SHARED;

	sem->ptr = kobj;
	sem->id = ksem->id;

	EXIT2 ( EXIT_SUCCESS, EXIT_SUCCESS );
}