bits.c

rtai-3.1-test3的源代码(Real-Time Application Interface )

/*
 * Copyright (C) 1999-2003 Paolo Mantegazza <[e-mail]>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/errno.h>
#include <linux/config.h>
#include <linux/version.h>
#include <asm/uaccess.h>
#include <rtai_sched.h>
#include <rtai_lxrt.h>
#include <rtai_bits.h>
#include <rtai_schedcore.h>

MODULE_LICENSE("GPL");

#define RT_BITS_MAGIC 0xaabcdeff  // same as SEM_MAGIC to ease it in user space

#define MASK0(x) ((unsigned long *)&(x))[0]
#define MASK1(x) ((unsigned long *)&(x))[1]

static int all_set(BITS *bits, unsigned long mask)
{
	return (bits->mask & mask) == mask;
}

static int any_set(BITS *bits, unsigned long mask)
{
	return (bits->mask & mask);
}

static int all_clr(BITS *bits, unsigned long mask)
{
	return (~bits->mask & mask) == mask;
}

static int any_clr(BITS *bits, unsigned long mask)
{
	return (~bits->mask & mask);
}

static int all_set_and_any_set(BITS *bits, unsigned long masks)
{
	return (bits->mask & MASK1(masks)) && (bits->mask & MASK0(masks)) == MASK0(masks);
}

static int all_set_and_all_clr(BITS *bits, unsigned long masks)
{
	return (bits->mask & MASK0(masks)) == MASK0(masks) && (~bits->mask & MASK1(masks)) == MASK1(masks);
}

static int all_set_and_any_clr(BITS *bits, unsigned long masks)
{
	return (bits->mask & MASK0(masks)) == MASK0(masks) && (~bits->mask & MASK1(masks));
}

static int any_set_and_all_clr(BITS *bits, unsigned long masks)
{
	return (bits->mask & MASK0(masks)) && (~bits->mask & MASK1(masks)) == MASK1(masks);
}

static int any_set_and_any_clr(BITS *bits, unsigned long masks)
{
	return (bits->mask & MASK0(masks)) && (~bits->mask & MASK1(masks));
}

static int all_clr_and_any_clr(BITS *bits, unsigned long masks)
{
	return (~bits->mask & MASK1(masks)) && (~bits->mask & MASK0(masks)) == MASK0(masks);
}

static int all_set_or_any_set(BITS *bits, unsigned long masks)
{
	return (bits->mask & MASK1(masks)) || (bits->mask & MASK0(masks)) == MASK0(masks);
}

static int all_set_or_all_clr(BITS *bits, unsigned long masks)
{
	return (bits->mask & MASK0(masks)) == MASK0(masks) || (~bits->mask & MASK1(masks)) == MASK1(masks);
}

static int all_set_or_any_clr(BITS *bits, unsigned long masks)
{
	return (bits->mask & MASK0(masks)) == MASK0(masks) || (~bits->mask & MASK1(masks));
}

static int any_set_or_all_clr(BITS *bits, unsigned long masks)
{
	return (bits->mask & MASK0(masks)) || (~bits->mask & MASK1(masks)) == MASK1(masks);
}

static int any_set_or_any_clr(BITS *bits, unsigned long masks)
{
	return (bits->mask & MASK0(masks)) || (~bits->mask & MASK1(masks));
}

static int all_clr_or_any_clr(BITS *bits, unsigned long masks)
{
	return (~bits->mask & MASK1(masks)) || (~bits->mask & MASK0(masks)) == MASK0(masks);
}

static void set_bits(BITS *bits, unsigned long mask)
{
	bits->mask |= mask;
}

static void clr_bits(BITS *bits, unsigned long mask)
{
	bits->mask &= ~mask;
}

static void set_clr_bits(BITS *bits, unsigned long masks)
{
	bits->mask =  (bits->mask | MASK0(masks)) & ~MASK1(masks);
}

static void nop_fun(BITS *bits, unsigned long mask)
{
}

static int (*test_fun[])(BITS *, unsigned long) = {
	all_set, any_set,             all_clr,             any_clr, 
	         all_set_and_any_set, all_set_and_all_clr, all_set_and_any_clr,
	                              any_set_and_all_clr, any_set_and_any_clr,
	                                                   all_clr_and_any_clr,
	         all_set_or_any_set,  all_set_or_all_clr,  all_set_or_any_clr,
	                              any_set_or_all_clr,  any_set_or_any_clr,
	                                                   all_clr_or_any_clr
};

static void (*exec_fun[])(BITS *, unsigned long) = {
	set_bits, clr_bits,
	          set_clr_bits,
	nop_fun
};

void rt_bits_init(BITS *bits, unsigned long mask)
{
	bits->magic      = RT_BITS_MAGIC;
	bits->queue.prev = &(bits->queue);
	bits->queue.next = &(bits->queue);
	bits->queue.task = 0;
	bits->mask       = mask;
}

int rt_bits_delete(BITS *bits)
{
	unsigned long flags, schedmap;
	RT_TASK *task;
	QUEUE *q;

	if (bits->magic != RT_BITS_MAGIC) {
		return BITS_ERR;
	}

	schedmap = 0;
	q = &bits->queue;
	flags = rt_global_save_flags_and_cli();
	bits->magic = 0;
	while ((q = q->next) != &bits->queue && (task = q->task)) {
		rem_timed_task(task);
		if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
			enq_ready_task(task);	
#ifdef CONFIG_SMP
			set_bit(task->runnable_on_cpus & 0x1F, &schedmap);
#endif
		}
	}
	RT_SCHEDULE_MAP(schedmap);
	rt_global_restore_flags(flags);
	return 0;
}

#define TEST_FUN(x)   ((int *)&(x)->retval)[0]
#define TEST_MASK(x)  ((unsigned long *)&(x)->retval)[1]

unsigned long rt_get_bits(BITS *bits)
{
	return bits->mask;
}

int rt_bits_reset(BITS *bits, unsigned long mask)
{
	unsigned long flags, schedmap, oldmask;
	RT_TASK *task;
	QUEUE *q;

	if (bits->magic != RT_BITS_MAGIC) {
		return BITS_ERR;
	}

	schedmap = 0;
	q = &bits->queue;
	flags = rt_global_save_flags_and_cli();
	oldmask = bits->mask;
	bits->mask = mask;
	while ((q = q->next) != &bits->queue) {
		dequeue_blocked(task = q->task);
		rem_timed_task(task);
		if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
			enq_ready_task(task);
#ifdef CONFIG_SMP
			set_bit(task->runnable_on_cpus & 0x1F, &schedmap);
#endif
		}
	}
	bits->queue.prev = bits->queue.next = &bits->queue;
	RT_SCHEDULE_MAP(schedmap);
	rt_global_restore_flags(flags);
	return oldmask;
}

unsigned long rt_bits_signal(BITS *bits, int setfun, unsigned long masks)
{
	unsigned long flags, schedmap;
	RT_TASK *task;
	QUEUE *q;

	if (bits->magic != RT_BITS_MAGIC) {
		return BITS_ERR;
	}

	schedmap = 0;
	q = &bits->queue;
	flags = rt_global_save_flags_and_cli();
	exec_fun[setfun](bits, masks);
	masks = bits->mask;
	while ((q = q->next) != &bits->queue) {
		task = q->task;
		if (test_fun[TEST_FUN(task)](bits, TEST_MASK(task))) {
			dequeue_blocked(task);
			rem_timed_task(task);
			if (task->state != RT_SCHED_READY && (task->state &= ~(RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED)) == RT_SCHED_READY) {
				enq_ready_task(task);
#ifdef CONFIG_SMP
				set_bit(task->runnable_on_cpus & 0x1F, &schedmap);
#endif
			}
		}
	}
	RT_SCHEDULE_MAP(schedmap);
	rt_global_restore_flags(flags);
	return masks;
}

int _rt_bits_wait(BITS *bits, int testfun, unsigned long testmasks, int exitfun, unsigned long exitmasks, unsigned long *resulting_mask, int space)
{
	RT_TASK *rt_current;
	unsigned long flags, mask;

	if (bits->magic != RT_BITS_MAGIC) {
		return BITS_ERR;
	}

	flags = rt_global_save_flags_and_cli();
	if (!test_fun[testfun](bits, testmasks)) {
		rt_current = RT_CURRENT;
		TEST_FUN(rt_current)  = testfun;
		TEST_MASK(rt_current) = testmasks;
		rt_current->state |= RT_SCHED_SEMAPHORE;
		rem_ready_current(rt_current);
		enqueue_blocked(rt_current, &bits->queue, 1);
		rt_schedule();
		mask = bits->mask;
		if (rt_current->blocked_on || bits->magic != RT_BITS_MAGIC) {
			rt_current->prio_passed_to = NOTHING;
			rt_global_restore_flags(flags);
			return BITS_ERR;
		}
	} else {
		mask = bits->mask;
	}
	exec_fun[exitfun](bits, exitmasks);
	rt_global_restore_flags(flags);
	if (resulting_mask) {
		if (space) {
			*resulting_mask = mask;
		} else {
			copy_to_user(resulting_mask, &mask, sizeof(mask));
		}
	}
	return 0;
}

int _rt_bits_wait_if(BITS *bits, int testfun, unsigned long testmasks, int exitfun, unsigned long exitmasks, unsigned long *resulting_mask, int space)
{
	unsigned long flags, mask;

	if (bits->magic != RT_BITS_MAGIC) {
		return BITS_ERR;
	}

	flags = rt_global_save_flags_and_cli();
	mask = bits->mask;
	if (test_fun[testfun](bits, testmasks)) {
		exec_fun[exitfun](bits, exitmasks);
		rt_global_restore_flags(flags);
		return 1;
	} 
	rt_global_restore_flags(flags);
	if (resulting_mask) {
		if (space) {
			*resulting_mask = mask;
		} else {
			copy_to_user(resulting_mask, &mask, sizeof(mask));
		}
	}
	return 0;
}

int _rt_bits_wait_until(BITS *bits, int testfun, unsigned long testmasks, int exitfun, unsigned long exitmasks, RTIME time, unsigned long *resulting_mask, int space)
{
	RT_TASK *rt_current;
	unsigned long flags, mask;

	if (bits->magic != RT_BITS_MAGIC) {
		return BITS_ERR;
	}

	flags = rt_global_save_flags_and_cli();
	if (!test_fun[testfun](bits, testmasks)) {
		rt_current = RT_CURRENT;
		TEST_FUN(rt_current)  = testfun;
		TEST_MASK(rt_current) = testmasks;
		rt_current->blocked_on = &bits->queue;
		if ((rt_current->resume_time = time) > get_time()) {
			rt_current->state |= (RT_SCHED_SEMAPHORE | RT_SCHED_DELAYED);
			rem_ready_current(rt_current);
			enqueue_blocked(rt_current, &bits->queue, 1);
			enq_timed_task(rt_current);
			rt_schedule();
		} else {
			rt_current->queue.prev = rt_current->queue.next = &rt_current->queue;
		}
		mask = bits->mask;
		if (bits->magic != RT_BITS_MAGIC) {
			rt_current->prio_passed_to = NOTHING;
			rt_global_restore_flags(flags);
			return BITS_ERR;
		} else if (rt_current->blocked_on) {
			dequeue_blocked(rt_current);
			rt_global_restore_flags(flags);
			return BITS_TIMOUT;
		}
	} else {
		mask = bits->mask;
	}
	exec_fun[exitfun](bits, exitmasks);
	rt_global_restore_flags(flags);
	if (resulting_mask) {
		if (space) {
			*resulting_mask = mask;
		} else {
			copy_to_user(resulting_mask, &mask, sizeof(mask));
		}
	}
	return 0;
}

int _rt_bits_wait_timed(BITS *bits, int testfun, unsigned long testmasks, int exitfun, unsigned long exitmasks, RTIME delay, unsigned long *resulting_mask, int space)
{
	return _rt_bits_wait_until(bits, testfun, testmasks, exitfun, exitmasks, get_time() + delay, resulting_mask, space);
}

/* +++++++++++++++++++++++++++++ NAMED BITS +++++++++++++++++++++++++++++++++ */

#include <rtai_registry.h>

BITS *rt_named_bits_init(const char *bits_name, unsigned long mask)
{
	BITS *bits;
	unsigned long name;

	if ((bits = rt_get_adr(name = nam2num(bits_name)))) {
		return bits;
	}
	if ((bits = rt_malloc(sizeof(SEM)))) {
		rt_bits_init(bits, mask);
		if (rt_register(name, bits, IS_BIT, 0)) {
			return bits;
		}
		rt_bits_delete(bits);
	}
	rt_free(bits);
	return (BITS *)0;
}

int rt_named_bits_delete(BITS *bits)
{
	if (!rt_bits_delete(bits)) {
		rt_free(bits);
	}
	return rt_drg_on_adr(bits);
}

int rt_bits_init_u(unsigned long name, unsigned long mask)
{
	BITS *bits;
	if (rt_get_adr(name)) {
		return 0;
	}
	if ((bits = rt_malloc(sizeof(BITS)))) {
		rt_bits_init(bits, mask);
		if (rt_register(name, bits, IS_BIT, current)) {
			return (int)bits;
		} else {
			rt_free(bits);
		}
	}
	return 0;
}

int rt_bits_delete_u(BITS *bits)
{
	if (rt_bits_delete(bits)) {
		return -EFAULT;
	}
	rt_free(bits);
	return rt_drg_on_adr(bits);
}

/* ++++++++++++++++++++++++++++ BITS ENTRIES ++++++++++++++++++++++++++++++++ */

struct rt_native_fun_entry rt_bits_entries[] = {
	{ { 0, rt_bits_init_u },          	BITS_INIT },
	{ { 0, rt_bits_delete_u },        	BITS_DELETE },
	{ { 0, rt_named_bits_init },    	NAMED_BITS_INIT },
	{ { 0, rt_named_bits_delete },  	NAMED_BITS_DELETE },
	{ { 1, rt_get_bits },           	BITS_GET },
	{ { 1, rt_bits_reset },         	BITS_RESET },
	{ { 1, rt_bits_signal },        	BITS_SIGNAL },
	{ { 1, _rt_bits_wait },          	BITS_WAIT },
	{ { 1, _rt_bits_wait_if },       	BITS_WAIT_IF },
	{ { 1, _rt_bits_wait_until },    	BITS_WAIT_UNTIL },
	{ { 1, _rt_bits_wait_timed },		BITS_WAIT_TIMED },
	{ { 0, 0 },                            	000 }
};

extern int set_rt_fun_entries(struct rt_native_fun_entry *entry);
extern void reset_rt_fun_entries(struct rt_native_fun_entry *entry);

int __rtai_bits_init(void)
{
	return set_rt_fun_entries(rt_bits_entries);
}

void __rtai_bits_exit(void)
{
	reset_rt_fun_entries(rt_bits_entries);
}

#ifndef CONFIG_RTAI_BITS_BUILTIN
module_init(__rtai_bits_init);
module_exit(__rtai_bits_exit);
#endif /* !CONFIG_RTAI_BITS_BUILTIN */

#ifdef CONFIG_KBUILD
EXPORT_SYMBOL(rt_bits_init);
EXPORT_SYMBOL(rt_bits_delete);
EXPORT_SYMBOL(rt_get_bits);
EXPORT_SYMBOL(rt_bits_reset);
EXPORT_SYMBOL(rt_bits_signal);
EXPORT_SYMBOL(_rt_bits_wait);
EXPORT_SYMBOL(_rt_bits_wait_if);
EXPORT_SYMBOL(_rt_bits_wait_until);
EXPORT_SYMBOL(_rt_bits_wait_timed);
EXPORT_SYMBOL(rt_named_bits_init);
EXPORT_SYMBOL(rt_named_bits_delete);
EXPORT_SYMBOL(rt_bits_init_u);
EXPORT_SYMBOL(rt_bits_delete_u);
#endif /* CONFIG_KBUILD */