hal.c

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

    spin_lock(&rtai_ssrq_lock);

    while ((pending = rtai_sysreq_pending & ~rtai_sysreq_running) != 0)
	{
	unsigned srq = ffnz(pending);
	set_bit(srq,&rtai_sysreq_running);
	clear_bit(srq,&rtai_sysreq_pending);
	spin_unlock(&rtai_ssrq_lock);

	if (test_bit(srq,&rtai_sysreq_map))
	    rtai_sysreq_table[srq].k_handler();

	clear_bit(srq,&rtai_sysreq_running);
	spin_lock(&rtai_ssrq_lock);
	}

    spin_unlock(&rtai_ssrq_lock);
}

long long rtai_usrq_trampoline (unsigned srq, unsigned label)

{
    long long r = 0;

    TRACE_RTAI_SRQ_ENTRY(srq);

    if (srq > 1 && srq < RTAI_NR_SRQS &&
	test_bit(srq,&rtai_sysreq_map) &&
	rtai_sysreq_table[srq].u_handler != NULL)
	r = rtai_sysreq_table[srq].u_handler(label);
    else
	for (srq = 2; srq < RTAI_NR_SRQS; srq++)
	    if (test_bit(srq,&rtai_sysreq_map) &&
		rtai_sysreq_table[srq].label == label)
		r = (long long)srq;

    TRACE_RTAI_SRQ_EXIT();

    return r;
}

static void rtai_uvec_handler (void)

{
    __asm__ __volatile__ ( \
	"cld\n\t" \
        "pushl %es\n\t" \
        "pushl %ds\n\t" \
        "pushl %ebp\n\t" \
	"pushl %edi\n\t" \
        "pushl %esi\n\t" \
        "pushl %ecx\n\t" \
	"pushl %ebx\n\t" \
        "pushl %edx\n\t" \
        "pushl %eax\n\t" \
	"movl $" STR(__KERNEL_DS) ",%ebx\n\t" \
        "mov %bx,%ds\n\t" \
        "mov %bx,%es\n\t" \
        "call "SYMBOL_NAME_STR(rtai_usrq_trampoline)"\n\t" \
	"addl $8,%esp\n\t" \
        "popl %ebx\n\t" \
        "popl %ecx\n\t" \
        "popl %esi\n\t" \
	"popl %edi\n\t" \
        "popl %ebp\n\t" \
        "popl %ds\n\t" \
        "popl %es\n\t" \
        "iret");
}

#define rtai_set_gate(gate_addr,type,dpl,addr) \
do { \
  int __d0, __d1; \
  __asm__ __volatile__ ("movw %%dx,%%ax\n\t" \
	"movw %4,%%dx\n\t" \
	"movl %%eax,%0\n\t" \
	"movl %%edx,%1" \
	:"=m" (*((long *) (gate_addr))), \
	 "=m" (*(1+(long *) (gate_addr))), "=&a" (__d0), "=&d" (__d1) \
	:"i" ((short) (0x8000+(dpl<<13)+(type<<8))), \
	 "3" ((char *) (addr)),"2" (__KERNEL_CS << 16)); \
} while (0)

static void rtai_install_archdep (void)

{
    unsigned long flags;

    flags = rtai_critical_enter(NULL);

    /* Backup and replace the sysreq vector. */
    rtai_sysvec = idt_table[RTAI_SYS_VECTOR];
    rtai_set_gate(idt_table+RTAI_SYS_VECTOR,15,3,&rtai_uvec_handler);

    rtai_critical_exit(flags);

    if (rtai_cpufreq_arg == 0)
	{
	adsysinfo_t sysinfo;
	adeos_get_sysinfo(&sysinfo);
	rtai_cpufreq_arg = (unsigned long)sysinfo.cpufreq; /* FIXME: 4Ghz barrier is close... */
	}

    rtai_tunables.cpu_freq = rtai_cpufreq_arg;

#ifdef CONFIG_X86_LOCAL_APIC
    if (rtai_apicfreq_arg == 0)
	rtai_apicfreq_arg = apic_read(APIC_TMICT) * HZ;

    rtai_tunables.apic_freq = rtai_apicfreq_arg;
#endif /* CONFIG_X86_LOCAL_APIC */
}

static void rtai_uninstall_archdep (void) {

    unsigned long flags;

    flags = rtai_critical_enter(NULL);
    idt_table[RTAI_SYS_VECTOR] = rtai_sysvec;
    rtai_critical_exit(flags);
}

int rtai_calibrate_8254 (void)

{
    unsigned long flags;
    RTIME t, dt;
    int i;

    flags = rtai_critical_enter(NULL);

    outb(0x34,0x43);

    t = rtai_rdtsc();

    for (i = 0; i < 10000; i++)
	{ 
	outb(LATCH & 0xff,0x40);
	outb(LATCH >> 8,0x40);
	}

    dt = rtai_rdtsc() - t;

    rtai_critical_exit(flags);

    return rtai_imuldiv(dt,100000,RTAI_CPU_FREQ);
}

void (*rt_set_ihook (void (*hookfn)(int)))(int) {

#ifdef CONFIG_RTAI_SCHED_ISR_LOCK
    return (void (*)(int))xchg(&rtai_isr_hook,hookfn); /* This is atomic */
#else  /* !CONFIG_RTAI_SCHED_ISR_LOCK */
    return NULL;
#endif /* CONFIG_RTAI_SCHED_ISR_LOCK */
}

void rtai_exit_trampoline (adevinfo_t *evinfo)

{
    struct task_struct *task = (struct task_struct *)evinfo->evdata;

    if (task->ptd[0])
	rtai_exit_callback();

    adeos_propagate_event(evinfo);
}

int set_rtai_callback (void (*fun)(void))

{
    rtai_exit_callback = fun;
    return adeos_catch_event(ADEOS_EXIT_PROCESS,&rtai_exit_trampoline);
}

void remove_rtai_callback (void (*fun)(void))
{
    adeos_catch_event(ADEOS_EXIT_PROCESS,0);
}

static int errno;

static inline _syscall3(int,
			sched_setscheduler,
			pid_t,pid,
			int,policy,
			struct sched_param *,param)

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
void rtai_set_linux_task_priority (struct task_struct *task, int policy, int prio)

{
    task->policy = policy;
    task->rt_priority = prio;
    set_tsk_need_resched(current);
}
#else /* KERNEL_VERSION >= 2.6.0 */
void rtai_set_linux_task_priority (struct task_struct *task, int policy, int prio)

{
    struct sched_param __user param;
    mm_segment_t old_fs;
    int rc;

    param.sched_priority = prio;
    old_fs = get_fs();
    set_fs(KERNEL_DS);
    rc = sched_setscheduler(task->pid,policy,&param);
    set_fs(old_fs);

    if (rc)
	printk("RTAI: sched_setscheduler(policy=%d,prio=%d) failed, code %d (%s -- pid=%d)\n",
	       policy,
	       prio,
	       rc,
	       task->comm,
	       task->pid);
}
#endif  /* KERNEL_VERSION < 2.6.0 */

#ifdef CONFIG_PROC_FS

struct proc_dir_entry *rtai_proc_root = NULL;

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

    PROC_PRINT("\n** RTAI/x86:\n\n");
#ifdef CONFIG_X86_LOCAL_APIC
    PROC_PRINT("    APIC Frequency: %lu\n",rtai_tunables.apic_freq);
    PROC_PRINT("    APIC Latency: %d ns\n",RTAI_LATENCY_APIC);
    PROC_PRINT("    APIC Setup: %d ns\n",RTAI_SETUP_TIME_APIC);
#endif /* CONFIG_X86_LOCAL_APIC */
    
    none = 1;

    PROC_PRINT("\n** Real-time IRQs used by RTAI: ");

    for (i = 0; i < NR_IRQS; i++)
	{
	if (rtai_realtime_irq[i].handler)
	    {
	    if (none)
		{
		PROC_PRINT("\n");
		none = 0;
		}

	    PROC_PRINT("\n    #%d at %p", i, rtai_realtime_irq[i].handler);
	    }
        }

    if (none)
	PROC_PRINT("none");

    PROC_PRINT("\n\n");

    PROC_PRINT("** RTAI extension traps: \n\n");
    PROC_PRINT("    SYSREQ=0x%x\n",RTAI_SYS_VECTOR);
    PROC_PRINT("      LXRT=0x%x\n",RTAI_LXRT_VECTOR);
    PROC_PRINT("       SHM=0x%x\n\n",RTAI_SHM_VECTOR);

    none = 1;
    PROC_PRINT("** RTAI SYSREQs in use: ");

    for (i = 0; i < RTAI_NR_SRQS; i++)
	{
	if (rtai_sysreq_table[i].k_handler ||
	    rtai_sysreq_table[i].u_handler)
	    {
	    PROC_PRINT("#%d ", i);
	    none = 0;
	    }
        }

    if (none)
	PROC_PRINT("none");

    PROC_PRINT("\n\n");

    PROC_PRINT_DONE;
}

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_proc;

    return 0;
}

static void rtai_proc_unregister (void)

{
    remove_proc_entry("rtai",rtai_proc_root);
    remove_proc_entry("rtai",0);
}

#endif /* CONFIG_PROC_FS */

static void rtai_domain_entry (int iflag)

{
    unsigned irq, trapnr;

    if (iflag)
	{
	for (irq = 0; irq < NR_IRQS; irq++)
	    adeos_virtualize_irq(irq,
				 &rtai_irq_trampoline,
				 NULL,
				 IPIPE_DYNAMIC_MASK);
	/* Trap all faults. */
	for (trapnr = 0; trapnr < ADEOS_NR_FAULTS; trapnr++)
	    adeos_catch_event(trapnr,&rtai_trap_fault);

	printk("RTAI %s mounted over Adeos %s.\n",PACKAGE_VERSION,ADEOS_VERSION_STRING);
	}

    for (;;)
	adeos_suspend_domain();
}

int __rtai_hal_init (void)

{
    unsigned long flags;
    int key0, key1;
    adattr_t attr;

    /* Allocate a virtual interrupt to handle sysreqs within the Linux
       domain. */
    rtai_sysreq_virq = adeos_alloc_irq();

    if (!rtai_sysreq_virq)
	{
	printk("RTAI: no virtual interrupt available.\n");
	return 1;
	}

    /* Reserve the first two _consecutive_ per-thread data key in the
       Linux domain. This is rather crappy, since we depend on
       statically defined PTD key values, which is exactly what the
       PTD scheme is here to prevent. Unfortunately, reserving these
       specific keys is the only way to remain source compatible with
       the current LXRT implementation. */
    flags = rtai_critical_enter(NULL);
    rtai_adeos_ptdbase = key0 = adeos_alloc_ptdkey();
    key1 = adeos_alloc_ptdkey();
    rtai_critical_exit(flags);

    if (key0 != 0 && key1 != 1)
	{
	printk("RTAI: per-thread keys #0 and/or #1 are busy.\n");
	return 1;
	}

    adeos_virtualize_irq(rtai_sysreq_virq,
			 &rtai_ssrq_trampoline,
			 NULL,
			 IPIPE_HANDLE_MASK);

    rtai_install_archdep();

#ifdef CONFIG_PROC_FS
    rtai_proc_register();
#endif

    /* Let Adeos do its magic for our real-time domain. */
    adeos_init_attr(&attr);
    attr.name = "RTAI";
    attr.domid = RTAI_DOMAIN_ID;
    attr.entry = &rtai_domain_entry;
    attr.priority = ADEOS_ROOT_PRI + 100; /* Precede Linux in the pipeline */

    return adeos_register_domain(&rtai_domain,&attr);
}

void __rtai_hal_exit (void)

{
#ifdef CONFIG_PROC_FS
    rtai_proc_unregister();
#endif

    adeos_virtualize_irq(rtai_sysreq_virq,NULL,NULL,0);
    adeos_free_irq(rtai_sysreq_virq);
    rtai_uninstall_archdep();
    adeos_free_ptdkey(rtai_adeos_ptdbase); /* #0 and #1 actually */
    adeos_free_ptdkey(rtai_adeos_ptdbase + 1);
    adeos_unregister_domain(&rtai_domain);
    printk("RTAI unmounted.\n");
}

module_init(__rtai_hal_init);
module_exit(__rtai_hal_exit);

EXPORT_SYMBOL(rt_request_irq);
EXPORT_SYMBOL(rt_release_irq);
EXPORT_SYMBOL(rt_set_irq_cookie);
EXPORT_SYMBOL(rt_startup_irq);
EXPORT_SYMBOL(rt_shutdown_irq);
EXPORT_SYMBOL(rt_enable_irq);
EXPORT_SYMBOL(rt_disable_irq);
EXPORT_SYMBOL(rt_mask_and_ack_irq);
EXPORT_SYMBOL(rt_unmask_irq);
EXPORT_SYMBOL(rt_ack_irq);
EXPORT_SYMBOL(rt_do_irq);
EXPORT_SYMBOL(rt_request_linux_irq);
EXPORT_SYMBOL(rt_free_linux_irq);
EXPORT_SYMBOL(rt_pend_linux_irq);
EXPORT_SYMBOL(rt_request_srq);
EXPORT_SYMBOL(rt_free_srq);
EXPORT_SYMBOL(rt_pend_linux_srq);
EXPORT_SYMBOL(rt_assign_irq_to_cpu);
EXPORT_SYMBOL(rt_reset_irq_to_sym_mode);
EXPORT_SYMBOL(rt_request_timer_cpuid);
EXPORT_SYMBOL(rt_request_apic_timers);
EXPORT_SYMBOL(rt_free_apic_timers);
EXPORT_SYMBOL(rt_request_timer);
EXPORT_SYMBOL(rt_free_timer);
EXPORT_SYMBOL(rt_set_trap_handler);
EXPORT_SYMBOL(rd_8254_ts);
EXPORT_SYMBOL(rt_setup_8254_tsc);
EXPORT_SYMBOL(rt_set_ihook);
EXPORT_SYMBOL(rt_mount);
EXPORT_SYMBOL(rt_umount);

EXPORT_SYMBOL(rtai_calibrate_8254);
EXPORT_SYMBOL(rtai_broadcast_to_local_timers);
EXPORT_SYMBOL(rtai_critical_enter);
EXPORT_SYMBOL(rtai_critical_exit);
EXPORT_SYMBOL(rtai_set_linux_task_priority);

EXPORT_SYMBOL(rtai_linux_context);
EXPORT_SYMBOL(rtai_domain);
EXPORT_SYMBOL(rtai_proc_root);
EXPORT_SYMBOL(rtai_tunables);
EXPORT_SYMBOL(rtai_cpu_lock);
EXPORT_SYMBOL(rtai_cpu_realtime);
EXPORT_SYMBOL(rtai_cpu_lxrt);
EXPORT_SYMBOL(set_rtai_callback);
EXPORT_SYMBOL(remove_rtai_callback);
EXPORT_SYMBOL(rt_times);
EXPORT_SYMBOL(rt_smp_times);

/*@}*/