rtai.c

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

		TRACE_RTAI_GLOBAL_IRQ_ENTRY(irq, !user_mode(regs));

		if ((rirq = rtai_irq[irq]) < 0) { 
			rirq = map_global_ppc_irq(irq); /* not yet mapped */
		}

		if (global_irq[rirq].handler) {
			global_irq[rirq].irq_count++;
			if (global_irq[rirq].ext) {
				if (((int (*)(int, unsigned long))global_irq[rirq].handler)(irq, global_irq[rirq].data)) {
					return 0;
				}
			} else {
				((void (*)(int))global_irq[rirq].handler)(irq);
			}
			rt_spin_lock(&(global.data_lock));
		} else {
			rt_spin_lock(&(global.data_lock));
#ifdef CONFIG_SMP
			if (rirq => LAST_GLOBAL_RTAI_IRQ) {
				set_bit(rirq, &processor[hard_cpu_id()].pending_irqs);
			} else {
				set_bit(rirq, &(global.pending_irqs));
			}
#else
			set_bit(rirq, &(global.pending_irqs));
#endif
		}
		if ((global.used_by_linux & processor[hard_cpu_id()].intr_flag)) {
			rt_spin_unlock_irq(&(global.data_lock));
			run_pending_irqs();

			TRACE_RTAI_GLOBAL_IRQ_EXIT();

			return 1;
		} else {
			rt_spin_unlock(&(global.data_lock));

			TRACE_RTAI_GLOBAL_IRQ_EXIT();

			return 0;
		}

		TRACE_RTAI_GLOBAL_IRQ_EXIT(); /* PARANOIA */

	} else {
		rt_spin_unlock(&global.ic_lock);
		return 0;
	
	}
}

static int dispatch_timer_irq(struct pt_regs *regs)
{
	int cpuid;
	
	/* 
	 * TRACE_RTAI_TRAP_ENTRY is not yet handled correctly by LTT.
	 * Therefore we treat the decrementer trap like an IRQ 255
	 */
	TRACE_RTAI_GLOBAL_IRQ_ENTRY(255, !user_mode(regs));
	
	if (processor[cpuid = hard_cpu_id()].rt_timer_handler) {
		(processor[cpuid].rt_timer_handler)();
		rt_spin_lock(&(global.data_lock));
	} else {
		rt_spin_lock(&(global.data_lock));
		set_bit(TIMER_IRQ, &processor[cpuid].pending_irqs);
	}
	if ((global.used_by_linux & processor[cpuid].intr_flag)) {
		rt_spin_unlock_irq(&(global.data_lock));
		run_pending_irqs();
		TRACE_RTAI_GLOBAL_IRQ_EXIT();
		return 1;
	} else {
		rt_spin_unlock(&(global.data_lock));
		TRACE_RTAI_GLOBAL_IRQ_EXIT();
		return 0;
	}
}

#define MIN_IDT_VEC 0xF0
#define MAX_IDT_VEC 0xFF

static unsigned long long (*idt_table[MAX_IDT_VEC - MIN_IDT_VEC + 1])(int srq, unsigned long name);

static int dispatch_srq(struct pt_regs *regs)
{
	unsigned long vec, srq, whatever; 
	long long retval;

	if (regs->gpr[0] && regs->gpr[0] == ((srq = regs->gpr[3]) + (whatever = regs->gpr[4]))) {

	        TRACE_RTAI_SRQ_ENTRY(srq, !user_mode(regs));

		if (!(vec = srq >> 24)) {
			if (srq > 1 && srq < NR_SYSRQS && sysrq[srq].user_handler) {
				retval = sysrq[srq].user_handler(whatever);
			} else {
				for (srq = 2; srq < NR_SYSRQS; srq++) {
					if (sysrq[srq].label == whatever) {
						retval = srq;
					}
				}
			}
		} else {
			retval = idt_table[vec - MIN_IDT_VEC](srq & 0xFFFFFF, whatever);
		}
		regs->gpr[0] = 0;
		regs->gpr[3] = ((unsigned long *)&retval)[0];
		regs->gpr[4] = ((unsigned long *)&retval)[1];
		regs->nip += 4;

		TRACE_RTAI_SRQ_EXIT();

		return 0;
	} else {
		return 1;
	}
}

struct desc_struct rt_set_full_intr_vect(unsigned int vector, int type, int dpl, void *handler)
{
	struct desc_struct fun = { 0 };
	if (vector >= MIN_IDT_VEC && vector <= MAX_IDT_VEC) {
		fun.fun = idt_table[vector - MIN_IDT_VEC];
		idt_table[vector - MIN_IDT_VEC] = handler;
		if (!rtai_srq_bckdr) {
			rtai_srq_bckdr = dispatch_srq;
		}
	}
	return fun;
}

void rt_reset_full_intr_vect(unsigned int vector, struct desc_struct idt_element)
{
	if (vector >= MIN_IDT_VEC && vector <= MAX_IDT_VEC) {
		idt_table[vector - MIN_IDT_VEC] = idt_element.fun;
	}
}

/* Here are the trapped irq actions for Linux interrupt handlers. */

static void trpd_enable_irq(unsigned int irq)
{
	rt_spin_lock_irq(&global.ic_lock);
	if(linux_irq_desc_handler[irq]->enable)
		linux_irq_desc_handler[irq]->enable(irq);
	rt_spin_unlock_irq(&global.ic_lock);
}

static void trpd_disable_irq(unsigned int irq)
{
	rt_spin_lock_irq(&global.ic_lock);
	if(linux_irq_desc_handler[irq]->disable)
		linux_irq_desc_handler[irq]->disable(irq);
	rt_spin_unlock_irq(&global.ic_lock);
}

static int trpd_get_irq(struct pt_regs *regs)
{
	printk("TRAPPED GET_IRQ SHOULD NEVER BE CALLED\n");
	return -1;
}

static void trpd_ack_irq(unsigned int irq)
{
#if 0
        rt_spin_lock_irq(&global.ic_lock);
	if(linux_irq_desc_handler[irq]->ack)
		linux_irq_desc_handler[irq]->ack(irq);
	rt_spin_unlock_irq(&global.ic_lock);
#endif
}

static void trpd_end_irq(unsigned int irq)
{
        rt_spin_lock_irq(&global.ic_lock);
	if(linux_irq_desc_handler[irq]->end)
		linux_irq_desc_handler[irq]->end(irq);
	else if(linux_irq_desc_handler[irq]->enable)
		linux_irq_desc_handler[irq]->enable(irq);
	rt_spin_unlock_irq(&global.ic_lock);
}

static struct hw_interrupt_type trapped_linux_irq_type = { 
	typename:	"RT SPVISD",
	startup:	rt_startup_irq,
	shutdown:	rt_shutdown_irq,
	enable:		trpd_enable_irq,
	disable:	trpd_disable_irq,
	ack:		trpd_ack_irq,
	end:		trpd_end_irq,
	set_affinity:	NULL,
};

#ifndef GLOBAL_PEND_LINUX_IRQ
static struct hw_interrupt_type real_time_irq_type = { 
	typename:	"REAL TIME",
	startup:	(unsigned int (*)(unsigned int))do_nothing_picfun,
	shutdown:	do_nothing_picfun,
	enable:		do_nothing_picfun,
	disable:	do_nothing_picfun,
	ack:		do_nothing_picfun,
	end:		do_nothing_picfun,
	set_affinity:	NULL,
};
#endif

/* Request and free interrupts, system requests and interprocessors messages */
/* Request for regular Linux irqs also included. They are nicely chained to  */
/* Linux, forcing sharing with any already installed handler, so that we can */
/* have an echo from Linux for global handlers. We found that usefull during */
/* debug, but can be nice for a lot of other things, e.g. see the jiffies    */
/* recovery in rtai_sched.c, and the global broadcast to local apic timers.  */

static unsigned long irq_action_flags[NR_IRQS];
static int chained_to_linux[NR_IRQS];

int rt_request_global_irq(unsigned int irq, void (*handler)(unsigned int irq))
{
	unsigned long flags;
	int rirq;

	if (irq >= NR_IRQS || !handler) {
		return -EINVAL;
	}
	rirq = rtai_irq[irq];
	if (rirq >= 0 && global_irq[rirq].handler) {
		return -EBUSY;
	}
	
	flags = hard_lock_all();
	if (rirq < 0) {
#ifdef CONFIG_SMP
		if (irq < OPENPIC_VEC_IPI) {
			rirq = map_global_ppc_irq(irq);
		} else {
			rirq = map_cpu_own_ppc_irq(irq);
		}
		global_irq[rirq].dest_status = 0;
#else
		rirq = map_global_ppc_irq(irq);
#endif
		global_irq[rirq].mapped = RTAI_IRQ_MAPPED_TEMP;
	}
	global_irq[rirq].handler = handler;
#ifndef GLOBAL_PEND_LINUX_IRQ
	/* 
	 * If you use rt_pend_linux_irq(), this might not be what 
	 * you expect or want especially with level sensitive IRQ.
	 * We keep this part mainly for compatibility with i386.
	 */
	IRQ_DESC[irq].handler = &real_time_irq_type;
#endif
	hard_unlock_all(flags);

	return 0;
}

int rt_request_global_irq_ext(unsigned int irq, 
			      int (*handler)(unsigned int irq, unsigned long data), 
			      unsigned long data)
{
	int rirq, ret;
	if (!(ret = rt_request_global_irq(irq, (void (*)(unsigned int irq))handler))) {
		rirq = rtai_irq[irq];
		global_irq[rirq].ext = 1;
		global_irq[rirq].data = data;
		return 0;
	}
	return ret;
}

void rt_set_global_irq_ext(unsigned int irq, int ext, unsigned long data)
{
	int rirq;
	if (irq < NR_IRQS && (rirq = rtai_irq[irq]) >= 0) {
		global_irq[rirq].ext = ext ? 1 : 0;
		global_irq[rirq].data = data;
	} else {
		printk("rt_set_global_irq_ext: irq=%d is invalid\n", irq);
	}
}

int rt_free_global_irq(unsigned int irq)
{
	unsigned long flags;
	int rirq;

	if (irq < 0 || irq >= NR_IRQS) {
		return -EINVAL;
	}
	rirq = rtai_irq[irq];
	if (rirq < 0 || !global_irq[rirq].handler) {
		return -EINVAL;
	}

	flags = hard_lock_all();
	IRQ_DESC[irq].handler = &trapped_linux_irq_type;
	if (global_irq[rirq].mapped == RTAI_IRQ_MAPPED_TEMP)
		unmap_ppc_irq(irq);
#ifdef CONFIG_SMP
	global_irq[rirq].dest_status = 0;
#endif
	global_irq[rirq].handler = 0;
	global_irq[rirq].ext = 0; 
	global_irq[rirq].irq_count = 0; 
	hard_unlock_all(flags);

	return 0;
}

int rt_request_linux_irq(unsigned int irq,
	void (*linux_handler)(int irq, void *dev_id, struct pt_regs *regs), 
	char *linux_handler_id, void *dev_id)
{
	unsigned long flags;

	if (irq == TIMER_8254_IRQ) {
		processor[0].trailing_irq_handler = linux_handler;
		return 0;
	}

	if (irq >= NR_IRQS || !linux_handler) {
		return -EINVAL;
	}

	save_flags(flags);
	cli();
	if (!chained_to_linux[irq]++) {
		if (IRQ_DESC[irq].action) {
			irq_action_flags[irq] = IRQ_DESC[irq].action->flags;
			IRQ_DESC[irq].action->flags |= SA_SHIRQ;
		}
	}
	restore_flags(flags);
#if defined(CONFIG_8xx) || defined(CONFIG_8260)
	request_8xxirq(irq, linux_handler, SA_SHIRQ, linux_handler_id, dev_id);
#else
	request_irq(irq, linux_handler, SA_SHIRQ, linux_handler_id, dev_id);
#endif
	return 0;
}

int rt_free_linux_irq(unsigned int irq, void *dev_id)
{
	unsigned long flags;

	if (irq == TIMER_8254_IRQ) {
		processor[0].trailing_irq_handler = 0;
		return 0;
	}

	if (irq >= NR_IRQS || !chained_to_linux[irq]) {
		return -EINVAL;
	}

	free_irq(irq, dev_id);
	save_flags(flags);
	cli();
	if (!(--chained_to_linux[irq]) && IRQ_DESC[irq].action) {
		IRQ_DESC[irq].action->flags = irq_action_flags[irq];
	}
	restore_flags(flags);

	return 0;
}

void rt_pend_linux_irq(unsigned int irq)
{
	if (irq == TIMER_8254_IRQ) {
		set_bit(TIMER_IRQ, &processor[hard_cpu_id()].pending_irqs);
		return;
	}
	if ((irq = rtai_irq[irq]) <= LAST_GLOBAL_RTAI_IRQ) {
		set_bit(irq, &global.pending_irqs);
	}
}

void rt_tick_linux_timer(void)
{
	set_bit(TIMER_IRQ, &processor[hard_cpu_id()].pending_irqs);
}

int rt_request_srq(unsigned int label, void (*rtai_handler)(void), long long (*user_handler)(unsigned int whatever))
{
	unsigned long flags;
	int srq;

	flags = rt_spin_lock_irqsave(&global.data_lock);
	if (!rtai_handler) {
		rt_spin_unlock_irqrestore(flags, &global.data_lock);
		return -EINVAL;
	}
	for (srq = 2; srq < NR_SYSRQS; srq++) {
		if (!(sysrq[srq].rtai_handler)) {
			sysrq[srq].rtai_handler = rtai_handler;
			sysrq[srq].label = label;
			if (user_handler) {
				sysrq[srq].user_handler = user_handler;
				if (!rtai_srq_bckdr) {
					rtai_srq_bckdr = dispatch_srq;
				}
			}
			rt_spin_unlock_irqrestore(flags, &global.data_lock);
			return srq;
		}
	}
	rt_spin_unlock_irqrestore(flags, &global.data_lock);

	return -EBUSY;
}

int rt_free_srq(unsigned int srq)
{
	unsigned long flags;

	flags = rt_spin_lock_irqsave(&global.data_lock);
	if (srq < 2 || srq >= NR_SYSRQS || !sysrq[srq].rtai_handler) {
		rt_spin_unlock_irqrestore(flags, &global.data_lock);
		return -EINVAL;
	}
	sysrq[srq].rtai_handler = 0; 
	sysrq[srq].user_handler = 0; 
	sysrq[srq].label = 0;
	for (srq = 2; srq < NR_SYSRQS; srq++) {
		if (sysrq[srq].user_handler) {
			rt_spin_unlock_irqrestore(flags, &global.data_lock);
			return 0;
		}
	}
	if (rtai_srq_bckdr) {
		rtai_srq_bckdr = 0;
	}
	rt_spin_unlock_irqrestore(flags, &global.data_lock);

	return 0;
}

void rt_pend_linux_srq(unsigned int srq)
{
	set_bit(srq, &global.pending_srqs);
}

void rt_switch_to_linux(int cpuid)
{

	TRACE_RTAI_SWITCHTO_LINUX(cpuid);

	set_bit(cpuid, &global.used_by_linux);
	set_intr_flag(processor[cpuid].intr_flag,processor[cpuid].linux_intr_flag);
}

void rt_switch_to_real_time(int cpuid)
{

	TRACE_RTAI_SWITCHTO_RT(cpuid);

	processor[cpuid].linux_intr_flag = processor[cpuid].intr_flag;
	set_intr_flag(processor[cpuid].intr_flag,0);
	clear_bit(cpuid, &global.used_by_linux);
}

/* RTAI mount-unmount functions to be called from the application to       */
/* initialise the real time application interface, i.e. this module, only  */
/* when it is required; so that it can stay asleep when it is not needed   */

#ifdef CONFIG_RTAI_MOUNT_ON_LOAD
#define rtai_mounted 1
#else
static spinlock_t rtai_mount_lock = SPIN_LOCK_UNLOCKED;
static int rtai_mounted;
#endif

// Trivial, but we do things carefully, the blocking part is relatively short,
// should cause no troubles in the transition phase. 
// All the zeroings are strictly not required as mostly related to static data. 
// Done esplicitly for emphasis. Simple, just block other processors and grab 
// everything from Linux.  To this aim first block all the other cpus by using
// a dedicated HARD_LOCK_IPI and its vector without any protection.

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,2)
/*
 * This is the binary patch we're applying to the Linux kernel.
 *
00000000 <patch>:
   0:	94 21 ff f0 	stwu	r1,-16(r1)
   4:	7c 08 02 a6 	mflr	r0
   8:	90 01 00 14 	stw	r0,20(r1)
   c:	3c 00 00 00 	lis	r0,0
			e: R_PPC_ADDR16_HI	func
  10:	60 00 00 00 	nop
			12: R_PPC_ADDR16_LO	func
  14:	7c 08 03 a6 	mtlr	r0
  18:	4e 80 00 21 	blrl
  1c:	80 01 00 14 	lwz	r0,20(r1)
  20:	7c 08 03 a6 	mtlr	r0
  24:	38 21 00 10 	addi	r1,r1,16
  28:	4e 80 00 20 	blr
*/

#if 0
/* This function creates essentially the same code as the above
 * fragment, if you want to know where it comes from. */
void patch_prototype(void)
{
	(*(void (*)(void))0xc001dafe)();
}
#endif

static u32 patch_insns[]={
	0x9421fff0, 0x7c0802a6, 0x90010014, 0x3c000000,
	0x60000000, 0x7c0803a6, 0x4e800021, 0x80010014,
	0x7c0803a6, 0x38210010, 0x4e800020,
};
#define N_INSNS 11
static u32 saved_insns_cli[N_INSNS];
static u32 saved_insns_sti[N_INSNS];
static u32 saved_insns_save_flags[N_INSNS];
static u32 saved_insns_restore_flags[N_INSNS];


static void patch_function(void *dest,void *func,void *save)
{
	memcpy(save,dest,sizeof(patch_insns));
	memcpy(dest,patch_insns,sizeof(patch_insns));
	((u32 *)dest)[3]|=(((unsigned int)(func))>>16)&0xffff;
	((u32 *)dest)[4]|=((unsigned int)(func))&0xffff;
	flush_icache_range((int)dest,(int)dest+sizeof(patch_insns));
}

static void unpatch_function(void *dest,void *save)
{
	memcpy(dest,save,sizeof(patch_insns));
	flush_icache_range((int)dest,(int)dest+sizeof(patch_insns));
}

static void install_patch(void)
{
	patch_function(&__cli,&linux_cli,saved_insns_cli);
	patch_function(&__sti,&linux_sti,saved_insns_sti);
	patch_function(&__save_flags_ptr,&linux_save_flags,saved_insns_save_flags);
	patch_function(&__restore_flags,&linux_restore_flags,saved_insns_restore_flags);
}

static void uninstall_patch(void)