rtai.c

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

{
	unpatch_function(&__cli,saved_insns_cli);
	unpatch_function(&__sti,saved_insns_sti);
	unpatch_function(&__save_flags_ptr,saved_insns_save_flags);
	unpatch_function(&__restore_flags,saved_insns_restore_flags);
}
#endif

void __rt_mount_rtai(void)
{
#define MSR(x) ((struct pt_regs *)((x)->thread.ksp + STACK_FRAME_OVERHEAD))->msr
	struct task_struct *task;
     	unsigned long flags, i;

	printk("rtai: mounting\n");
#ifdef CONFIG_SMP
	global_irq[HARD_LOCK_IPI].handler = hard_lock_all_handler;
#endif
	flags = hard_lock_all();

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,2) // approximate
	install_patch();
#else
	int_control.int_cli           = linux_cli;
	int_control.int_sti           = linux_sti;
	int_control.int_save_flags    = linux_save_flags;
	int_control.int_restore_flags = linux_restore_flags;
#endif
#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,4,3) // approximate
	int_control.int_set_lost      = (void (*)(unsigned long))rt_pend_linux_irq;
#endif
	ppc_md.get_irq = trpd_get_irq;
	do_IRQ_intercept = (unsigned long)dispatch_irq;
	timer_interrupt_intercept = (unsigned long)dispatch_timer_irq;
	rtai_soft_sti = linux_sti; /* and not linux_soft_sti */
	atomic_set(&ppc_n_lost_interrupts, 0);
	for (i = 0; i < NR_MASK_WORDS; i++) {
		ppc_lost_interrupts[i] = 0;
	}

	for (i = 0; i < NR_IRQS; i++) {
		if (IRQ_DESC[i].handler) {
			IRQ_DESC[i].handler = &trapped_linux_irq_type;
		}
	}

	task = &init_task;
	MSR(task) |= MSR_EE;
	if (task->thread.regs) {
		(task->thread.regs)->msr |= MSR_EE;
	}
	for_each_task(task) {
		MSR(task) |= MSR_EE;
		if (task->thread.regs) {
			(task->thread.regs)->msr |= MSR_EE;
		}
	}

	hard_unlock_all(flags);
	//printk("\n***** RTAI NEWLY MOUNTED (MOUNT COUNT %d) ******\n\n", rtai_mounted);
	printk("rtai: mount done\n");
}

// Simple, now we can simply block other processors and copy original data back
// to Linux. The HARD_LOCK_IPI is the last one to be reset.
void __rt_umount_rtai(void)
{
	int i;
	unsigned long flags;

	flags = hard_lock_all();
	rtai_srq_bckdr			= 0;
	rtai_soft_sti			= 0;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,2)
	uninstall_patch();
	for(i=0;i<NR_CPUS;i++){
		disarm_decr[i]=0;
	}
#endif
#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,4,3)
	int_control               = ppc_int_control;
#endif
	timer_interrupt_intercept = ppc_timer_handler;
	do_IRQ_intercept          = ppc_irq_dispatcher;
	ppc_md.get_irq            = ppc_get_irq;
	for (i = 0; i < NR_IRQS; i++) {
		IRQ_DESC[i].handler = linux_irq_desc_handler[i];
	}
#ifdef CONFIG_SMP
	global_irq[HARD_LOCK_IPI].handler = 0;
	global_irq[HARD_LOCK_IPI].dest_status = 0;
#endif
#ifdef CONFIG_4xx
        /* Reenable auto-reload mode used by Linux */
        if ((mfspr(SPRN_TCR) & TCR_ARE) == 0) {
		while (get_dec_4xx() > 0);
		mtspr(SPRN_TCR,  mfspr(SPRN_TCR) | TCR_ARE);
		set_dec_4xx(tb_ticks_per_jiffy);
        }
#else
	while (get_dec() <= tb_ticks_per_jiffy);
	set_dec(tb_ticks_per_jiffy);
#endif
	/* XXX We probably should change the per-process soft
	 * interrupt flags back to MSR_EE here. */
	hard_unlock_all(flags);
	printk("\n***** RTAI UNMOUNTED (MOUNT COUNT %d) ******\n\n", rtai_mounted);
}

#ifdef CONFIG_RTAI_MOUNT_ON_LOAD
void rt_mount_rtai(void) { }
void rt_umount_rtai(void) { }
#else
void rt_mount_rtai(void)
{
	rt_spin_lock(&rtai_mount_lock);
	rtai_mounted++;
	MOD_INC_USE_COUNT;

	TRACE_RTAI_MOUNT();

	if(rtai_mounted==1)__rtai_mount_rtai();
	rt_spin_unlock(&rtai_mount_lock);
}

void rt_umount_rtai(void)
{
	rt_spin_lock(&rtai_mount_lock);
	rtai_mounted--;
	MOD_DEC_USE_COUNT;

	TRACE_RTAI_UMOUNT();

	if(!rtai_mounted)__rtai_umount_rtai();
	rt_spin_unlock(&rtai_mount_lock);

}
#endif

// Module parameters to allow frequencies to be overriden via insmod
static int CpuFreq = 0;
MODULE_PARM(CpuFreq, "i");

/* module init-cleanup */

static void rt_printk_sysreq_handler(void);

// Let's prepare our side without any problem, so that there remain just a few
// things to be done when mounting RTAI. All the zeroings are strictly not 
// required as mostly related to static data. Done esplicitly for emphasis.
int init_module(void)
{
     	unsigned int i;

	// Passed in CPU frequency overides auto detected Linux value
	if (CpuFreq == 0) {
	    	extern unsigned tb_ticks_per_jiffy;

		CpuFreq = HZ * tb_ticks_per_jiffy;
	}
	tuned.cpu_freq = CpuFreq;
	printk("rtai: decrementer frequency %d Hz\n",tuned.cpu_freq);

#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,4,3)
	ppc_int_control    = int_control;
#endif
	ppc_get_irq        = ppc_md.get_irq;
	ppc_irq_dispatcher = do_IRQ_intercept;
	ppc_timer_handler  = timer_interrupt_intercept;

       	global.pending_irqs    = 0;
       	global.activ_irqs      = 0;
       	global.pending_srqs    = 0;
       	global.activ_srqs      = 0;
       	global.cpu_in_sti      = 0;
	global.used_by_linux   = ~(0xFFFFFFFF << smp_num_cpus);
#ifdef CONFIG_SMP
	global.locked_cpus     = 0;
	global.hard_nesting    = 0;
      	spin_lock_init(&(global.hard_lock));
#endif
      	spin_lock_init(&(global.data_lock));
      	spin_lock_init(&global.global.ic_lock);

	for (i = 0; i < NR_RT_CPUS; i++) {
		processor[i].intr_flag         = (1 << IFLAG) | (1 << i);
		processor[i].linux_intr_flag   = (1 << IFLAG) | (1 << i);
		processor[i].pending_irqs      = 0;
		processor[i].activ_irqs        = 0;
		processor[i].rt_timer_handler  = 0;
		processor[i].trailing_irq_handler      = 0;
	}
        for (i = 0; i < NR_SYSRQS; i++) {
		sysrq[i].rtai_handler = 0;
		sysrq[i].user_handler = 0;
		sysrq[i].label        = 0;
        }
	for (i = 0; i < NR_RTAI_IRQS; i++) {
		global_irq[i].ppc_irq = 0;
		global_irq[i].mapped  = RTAI_IRQ_UNMAPPED;
#ifdef CONFIG_SMP
		global_irq[i].dest_status = 0;
#endif
		global_irq[i].handler = 0;
		global_irq[i].ext = 0;
		global_irq[i].irq_count = 0;
	}
	for (i = 0; i < NR_IRQS; i++) {
		ic_ack_irq[i] = do_nothing_picfun;
		if ((linux_irq_desc_handler[i] = IRQ_DESC[i].handler)) {
			ic_ack_irq[i] = (IRQ_DESC[i].handler)->ack;
		}

		if (IRQ_DESC[i].handler && IRQ_DESC[i].action) {
#ifdef CONFIG_SMP
			if (i < OPENPIC_VEC_IPI) {
				map_global_ppc_irq(i);
			} else {
				map_cpu_own_ppc_irq(i);
			}
#else
			map_global_ppc_irq(i);
#endif
		} else {
			rtai_irq[i] = -1;
		}
	}

	sysrq[1].rtai_handler = rt_printk_sysreq_handler;
	sysrq[1].label = 0x1F0000F1;
#ifdef CONFIG_RTAI_MOUNT_ON_LOAD
	__rt_mount_rtai();
#endif

#ifdef CONFIG_PROC_FS
	rtai_proc_register();
#endif

	return 0;
}

void cleanup_module(void)
{
#ifdef CONFIG_RTAI_MOUNT_ON_LOAD
	__rt_umount_rtai();
#endif

#ifdef CONFIG_PROC_FS
	rtai_proc_unregister();
#endif

	return;
}

/* ----------------------< proc filesystem section >----------------------*/
#ifdef CONFIG_PROC_FS

struct proc_dir_entry *rtai_proc_root = NULL;

static int rtai_read_rtai(char *page, char **start, off_t off, int count,
                          int *eof, void *data)
{
	PROC_PRINT_VARS;
        int i;

        PROC_PRINT("\nRTAI Real Time Kernel, Version: %s\n\n", RTAI_RELEASE);
        PROC_PRINT("    Mount count: %d\n", rtai_mounted);
        PROC_PRINT("    Frequency  : %d\n", FREQ_DECR);
        PROC_PRINT("    Latency    : %d ns\n", LATENCY_DECR);
        PROC_PRINT("    Setup time : %d ns\n", SETUP_TIME_DECR);
        PROC_PRINT("\nGlobal irqs used by RTAI:\n");
        for (i = 0; i <= LAST_GLOBAL_RTAI_IRQ; i++) {
          if (global_irq[i].handler) {
            PROC_PRINT("%3d: %10i\n", 
		       global_irq[i].ppc_irq, global_irq[i].irq_count);
          }
        }
#ifdef CONFIG_SMP
        PROC_PRINT("\nCpu_Own irqs used by RTAI: \n");
        for (i = LAST_GLOBAL_RTAI_IRQ + 1; i < NR_RTAI_IRQS; i++) {
          if (global_irq[i].handler) {
            PROC_PRINT("%d ", global_irq[i].ppc_irq);
          }
        }
#endif
        PROC_PRINT("\nRTAI sysreqs in use: \n");
        for (i = 0; i < NR_SYSRQS; i++) {
          if (sysrq[i].rtai_handler || sysrq[i].user_handler) {
            PROC_PRINT("%d ", i);
          }
        }
        PROC_PRINT("\n\n");

	PROC_PRINT_DONE;
}       /* End function - rtai_read_rtai */

static int rtai_proc_register(void)
{

	struct proc_dir_entry *ent;

        rtai_proc_root = create_proc_entry("rtai", S_IFDIR, 0);
        if (!rtai_proc_root) {
		printk("Unable to initialize /proc/rtai\n");
                return(-1);
        }
	rtai_proc_root->owner = THIS_MODULE;
        ent = create_proc_entry("rtai", S_IFREG|S_IRUGO|S_IWUSR, rtai_proc_root);
        if (!ent) {
		printk("Unable to initialize /proc/rtai/rtai\n");
                return(-1);
        }
	ent->read_proc = rtai_read_rtai;
        return(0);
}       /* End function - rtai_proc_register */


static void rtai_proc_unregister(void)
{
        remove_proc_entry("rtai", rtai_proc_root);
        remove_proc_entry("rtai", 0);
}       /* End function - rtai_proc_unregister */

#endif /* CONFIG_PROC_FS */
/* ------------------< end of proc filesystem section >------------------*/


/********** SOME TIMER FUNCTIONS TO BE LIKELY NEVER PUT ELSWHERE *************/

/* Real time timers. No oneshot, and related timer programming, calibration. */
/* Use the utility module. It is also to be decided if this stuff has to     */
/* stay here.                                                                */

struct calibration_data tuned;
struct rt_times rt_times;
struct rt_times rt_smp_times[NR_RT_CPUS];

void rt_request_timer(void (*handler)(void), unsigned int tick, int unused)
{
	unsigned int cpuid;
	RTIME t;
	unsigned long flags;

	TRACE_RTAI_TIMER(TRACE_RTAI_EV_TIMER_REQUEST, handler, tick);

	if (processor[cpuid = hard_cpu_id()].rt_timer_handler) {
		return;
	}
	flags = hard_lock_all();

#ifdef CONFIG_4xx
	/* Disable auto-reload mode used by Linux */
	mtspr(SPRN_TCR,  mfspr(SPRN_TCR) & ~TCR_ARE);
	do {
		t = rdtsc();
	} while (get_dec_4xx() > 0);
#else
	do {
		t = rdtsc();
	} while (get_dec() <= tb_ticks_per_jiffy);
#endif

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,2)
	disarm_decr[cpuid]=1;
#else
	rtai_regs.trap = 1;
#endif
	rt_times.linux_tick = tb_ticks_per_jiffy;
	rt_times.periodic_tick = tick > 0 && tick < tb_ticks_per_jiffy ? tick : rt_times.linux_tick;
	rt_times.tick_time  = t;
	rt_times.intr_time  = t + rt_times.periodic_tick;
	rt_times.linux_time = t + rt_times.linux_tick;
	processor[cpuid].rt_timer_handler = handler;
	rt_set_decrementer_count(rt_times.periodic_tick);
	hard_unlock_all(flags);
	return;
}

void rt_free_timer(void)
{
	unsigned long flags;

	TRACE_RTAI_TIMER(TRACE_RTAI_EV_TIMER_FREE, 0, 0);

	flags = hard_lock_all();
#ifdef CONFIG_4xx
	/* Restore auto-reload mode for Linux */
	mtspr(SPRN_TCR, mfspr(SPRN_TCR) | TCR_ARE);

	/* Set the PIT reload value and just let it run. */
	mtspr(SPRN_PIT, tb_ticks_per_jiffy);
#endif

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,2)
	disarm_decr[hard_cpu_id()]=0;
#else
	rtai_regs.trap = 0;
#endif
	processor[hard_cpu_id()].rt_timer_handler = 0;
	hard_unlock_all(flags);
}

void rt_request_apic_timers(void (*handler)(void), struct apic_timer_setup_data *apic_timer_data)
{
	RTIME t;
	int cpuid;
	unsigned long flags;
	struct apic_timer_setup_data *p;
	struct rt_times *rt_times;

	TRACE_RTAI_TIMER(TRACE_RTAI_EV_TIMER_REQUEST_APIC, handler, 0);

	flags = hard_lock_all();
	do {
		t = rdtsc();
	} while (get_dec());
	for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
		*(p = apic_timer_data + cpuid) = apic_timer_data[cpuid];
		rt_times = rt_smp_times + cpuid;
		p->mode = 1;
		rt_times->linux_tick = tb_ticks_per_jiffy;
		rt_times->tick_time = llimd(t + tb_ticks_per_jiffy, FREQ_DECR, tuned.cpu_freq);
		rt_times->periodic_tick = 
		p->count = p->mode > 0 ? imuldiv(p->count, FREQ_DECR, 1000000000) : tb_ticks_per_jiffy;
		rt_times->intr_time = rt_times->tick_time + rt_times->periodic_tick;
		rt_times->linux_time = rt_times->tick_time + rt_times->linux_tick;
		processor[cpuid = hard_cpu_id()].rt_timer_handler = handler;
	}
	hard_unlock_all(flags);
	for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
		if ((p = apic_timer_data + cpuid)->mode > 0) {
			p->mode = 1;
			p->count = imuldiv(p->count, FREQ_DECR, 1000000000);
		} else {
			p->mode = 0;
			p->count = imuldiv(p->count, tuned.cpu_freq, 1000000000);
		}
	}
}

void rt_free_apic_timers(void)
{
	unsigned long flags, cpuid;

	TRACE_RTAI_TIMER(TRACE_RTAI_EV_TIMER_APIC_FREE, 0, 0);

	flags = hard_lock_all();
	for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
		processor[cpuid = hard_cpu_id()].rt_timer_handler = 0;
	}
	hard_unlock_all(flags);
}

/******** END OF SOME TIMER FUNCTIONS TO BE LIKELY NEVER PUT ELSWHERE *********/

// Our printk function, its use should be safe everywhere.
#include <linux/console.h>

int rtai_print_to_screen(const char *format, ...)
{
        static spinlock_t display_lock = SPIN_LOCK_UNLOCKED;
        static char display[25*80];
        unsigned long flags;
        struct console *c;
        va_list args;
        int len;

        flags = rt_spin_lock_irqsave(&display_lock);
        va_start(args, format);
        len = vsprintf(display, format, args);
        va_end(args);
        c = console_drivers;
        while(c) {
                if ((c->flags & CON_ENABLED) && c->write)
                        c->write(c, display, len);
                c = c->next;
	}
        rt_spin_unlock_irqrestore(flags, &display_lock);

	return len;
}

/*
 *  rt_printk.c, hacked from linux/kernel/printk.c.
 *
 * Modified for RT support, David Schleef.
 *
 * Adapted to RTAI, and restyled his way by Paolo Mantegazza. Now it has been
 * taken away from the fifos module and has become an integral part of the basic
 * RTAI module.
 */

#define PRINTK_BUF_LEN	(4096)
#define TEMP_BUF_LEN	(256)

static char rt_printk_buf[PRINTK_BUF_LEN];
static int buf_front, buf_back;

static char buf[TEMP_BUF_LEN];

int rt_printk(const char *fmt, ...)
{
        static spinlock_t display_lock = SPIN_LOCK_UNLOCKED;
	va_list args;
	int len, i;
	unsigned long flags;

        flags = rt_spin_lock_irqsave(&display_lock);
	va_start(args, fmt);
	len = vsprintf(buf, fmt, args);
	va_end(args);
	if (buf_front + len >= PRINTK_BUF_LEN) {
		i = PRINTK_BUF_LEN - buf_front;
		memcpy(rt_printk_buf + buf_front, buf, i);
		memcpy(rt_printk_buf, buf + i, len - i);
		buf_front = len - i;
	} else {
		memcpy(rt_printk_buf + buf_front, buf, len);
		buf_front += len;
	}
        rt_spin_unlock_irqrestore(flags, &display_lock);
	rt_pend_linux_srq(1);

	return len;
}

static void rt_printk_sysreq_handler(void)
{
	int tmp;

	while(1) {
		tmp = buf_front;
		if (buf_back  > tmp) {
			printk("%.*s", PRINTK_BUF_LEN - buf_back, rt_printk_buf + buf_back);
			buf_back = 0;
		}
		if (buf_back == tmp) {
			break;
		}
		printk("%.*s", tmp - buf_back, rt_printk_buf + buf_back);
		buf_back = tmp;
	}
}