rt-thread/components/pthreads/semaphore.c

#include <errno.h>
#include <sys/fcntl.h>

#include <rtthread.h>
#include "semaphore.h"

static posix_sem_t* posix_sem_list = RT_NULL;
static struct rt_semaphore posix_sem_lock;
void posix_sem_system_init()
{
	rt_sem_init(&posix_sem_lock, "psem", 1, RT_IPC_FLAG_FIFO);
}

rt_inline void posix_sem_insert(posix_sem_t *psem)
{
	psem->next = posix_sem_list;
	posix_sem_list = psem;
}

static void posix_sem_delete(posix_sem_t *psem)
{
	posix_sem_t *iter;
	if (posix_sem_list == psem)
	{
		posix_sem_list = psem->next;

		rt_sem_delete(psem->sem);
		rt_free(psem);

		return;
	}
	for (iter = posix_sem_list; iter->next != RT_NULL; iter = iter->next)
	{
		if (iter->next == psem)
		{
			/* delete this mq */
			if (psem->next != RT_NULL)
				iter->next = psem->next;
			else
				iter->next = RT_NULL;

			/* delete RT-Thread mqueue */
			rt_sem_delete(psem->sem);
			rt_free(psem);
			return ;
		}
	}
}

static posix_sem_t *posix_sem_find(const char* name)
{
	posix_sem_t *iter;
	rt_object_t object;

	for (iter = posix_sem_list; iter != RT_NULL; iter = iter->next)
	{
		object = (rt_object_t)&(iter->sem);

		if (strncmp(object->name, name, RT_NAME_MAX) == 0)
		{
			return iter;
		}
	}
}

int sem_close(sem_t *sem)
{
	if (sem == RT_NULL)
	{
		rt_set_errno(EINVAL);
		return -1;
	}

    /* lock posix semaphore list */
    rt_sem_take(&posix_sem_lock, RT_WAITING_FOREVER);
    sem->sem->refcount --;
    if (sem->sem->refcount == 0)
    {
    	/* delete from posix semaphore list */
    	if (sem->sem->unlinked)
    		posix_sem_delete(sem->sem);
    	sem->sem = RT_NULL;
    }
    rt_sem_release(&posix_sem_lock);

    rt_free(sem);
    return 0;
}

int sem_destroy(sem_t *sem)
{
	rt_err_t result;

	if ((!sem) || !(sem->sem->unamed))
	{
		rt_set_errno(EINVAL);
		return -1;
	}

    /* lock posix semaphore list */
    rt_sem_take(&posix_sem_lock, RT_WAITING_FOREVER);
    result = rt_sem_trytake(sem->sem->sem);
    if (result != RT_EOK)
    {
        rt_sem_release(&posix_sem_lock);
        rt_set_errno(EBUSY);
    	return -1;
    }

    /* destroy an unamed posix semaphore */
   	posix_sem_delete(sem->sem);
    rt_sem_release(&posix_sem_lock);

    rt_free(sem);
    return 0;
}

int sem_unlink(const char *name)
{
	posix_sem_t *psem;

    /* lock posix semaphore list */
    rt_sem_take(&posix_sem_lock, RT_WAITING_FOREVER);
    psem = posix_sem_find(name);
    if (psem != RT_NULL)
    {
    	psem->unlinked = 1;
    	if (psem->refcount == 0)
    	{
    		/* remove this semaphore */
    		posix_sem_delete(psem);
    	}
        rt_sem_release(&posix_sem_lock);
        return 0;
    }
    rt_sem_release(&posix_sem_lock);

    /* no this entry */
    rt_set_errno(ENOENT);
    return -1;
}

int sem_getvalue(sem_t *sem, int *sval)
{
	if (!sem || !sval)
	{
		rt_set_errno(EINVAL);
		return -1;
	}
	*sval = sem->sem->sem->value;
	return 0;
}

int sem_init(sem_t *sem, int pshared, unsigned int value)
{
	rt_err_t result;
	char name[RT_NAME_MAX];
	static rt_uint16_t psem_number = 0;

	RT_ASSERT(sem != RT_NULL);

	rt_snprintf(name, sizeof(name), "psem%02d", psem_number++);
	sem->sem = (struct posix_sem*) rt_malloc (sizeof(struct posix_sem));
	if (sem->sem == RT_NULL)
	{
		rt_set_errno(EINVAL);
		return -1;
	}
	sem->sem->sem = rt_sem_create(name, value, RT_IPC_FLAG_FIFO);
	if (sem->sem->sem == RT_NULL)
	{
		rt_free(sem->sem);
		sem->sem = RT_NULL;
		rt_set_errno(ENOMEM);
		return -1;
	}

	/* initialize posix semaphore */
	sem->sem->refcount = 1;
	sem->sem->unlinked = 0;
	sem->sem->unamed = 1;
    /* lock posix semaphore list */
    rt_sem_take(&posix_sem_lock, RT_WAITING_FOREVER);
    posix_sem_insert(sem->sem);
    rt_sem_release(&posix_sem_lock);
	sem->flags = 0;

	return 0;
}

sem_t *sem_open(const char *name, int oflag, ...)
{
	sem_t* sem;
	va_list arg;
	mode_t mode;
	unsigned int value;

    sem = RT_NULL;

    /* lock posix semaphore list */
    rt_sem_take(&posix_sem_lock, RT_WAITING_FOREVER);
	if (oflag & O_CREAT)
	{
		va_start(arg, oflag);
		mode = (mode_t) va_arg( arg, unsigned int);
		value = va_arg( arg, unsigned int);
		va_end(arg);

	    if (oflag & O_EXCL)
	    {
	    	if (posix_sem_find(name) != RT_NULL)
	    	{
	    		rt_set_errno(EEXIST);
	    		goto __return;
	    	}
	    }
	    sem = (sem_t*) rt_malloc (sizeof(struct semdes));
	    if (sem == RT_NULL)
	    {
	    	rt_set_errno(ENFILE);
	    	goto __return;
	    }

	    sem->flags = oflag;
	    sem->sem = (posix_sem_t*) rt_malloc (sizeof(posix_sem_t));
	    if (sem->sem == RT_NULL)
	    {
	    	rt_set_errno(ENFILE);
	    	goto __return;
	    }

	    /* create RT-Thread semaphore */
	    sem->sem->sem = rt_sem_create(name, value, RT_IPC_FLAG_FIFO);
		if (sem->sem->sem == RT_NULL) /* create failed */
		{
			rt_set_errno(ENFILE);
			goto __return;
		}
		/* initialize reference count */
		sem->sem->refcount = 1;
		sem->sem->unlinked = 0;
		sem->sem->unamed = 0;

		/* insert semaphore to posix semaphore list */
		posix_sem_insert(sem->sem);
	}
	else
	{
		posix_sem_t *psem;

		/* find semaphore */
		psem = posix_sem_find(name);
		if (psem != RT_NULL)
		{
			sem = (sem_t*) rt_malloc (sizeof(struct semdes));
			sem->sem = psem;
			sem->flags = oflag;
			psem->refcount ++; /* increase reference count */
		}
		else
		{
			rt_set_errno(ENOENT);
			goto __return;
		}
	}
	rt_sem_release(&posix_sem_lock);
	return sem;

__return:
	/* release lock */
	rt_sem_release(&posix_sem_lock);

	/* release allocated memory */
	if (sem != RT_NULL)
	{
		if (sem->sem != RT_NULL)
		{
			/* delete RT-Thread semaphore */
			if (sem->sem->sem != RT_NULL)
				rt_sem_delete(sem->sem->sem);
			rt_free(sem->sem);
		}
		rt_free(sem);
	}
	return RT_NULL;
}

int sem_post(sem_t *sem)
{
	rt_err_t result;

	if (!sem)
	{
		rt_set_errno(EINVAL);
		return -1;
	}

	result = rt_sem_release(sem->sem->sem);
	if (result == RT_EOK) return 0;

	rt_set_errno(EINVAL);
	return -1;
}

int sem_timedwait(sem_t *sem, const struct timespec *abs_timeout)
{
	rt_err_t result;
	rt_int32_t tick;

	if (!sem || !abs_timeout) return EINVAL;

	/* calculate os tick */
	tick = libc_time_to_tick(abs_timeout);
	
	result = rt_sem_take(sem->sem->sem, tick);
	if (result == -RT_ETIMEOUT)
	{
		rt_set_errno(ETIMEDOUT);
		return -1;
	}
	if (result == RT_EOK) return 0;

	rt_set_errno(EINTR);
	return -1;
}

int sem_trywait(sem_t *sem)
{
	rt_err_t result;

	if (!sem)
	{
		rt_set_errno(EINVAL);
		return -1;
	}

	result = rt_sem_take(sem->sem->sem, RT_WAITING_FOREVER);
	if (result == -RT_ETIMEOUT)
	{
		rt_set_errno(EAGAIN);
		return -1;
	}
	if (result == RT_EOK) return 0;

	rt_set_errno(EINTR);
	return -1;
}

int sem_wait(sem_t *sem)
{
	rt_err_t result;

	if (!sem)
	{
		rt_set_errno(EINVAL);
		return -1;
	}

	result = rt_sem_take(sem->sem->sem, RT_WAITING_FOREVER);
	if (result == RT_EOK) return 0;

	rt_set_errno(EINTR);
	return -1;
}