hal.c

xenomai 很好的linux实时补丁

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

    flags = rthal_critical_enter(NULL);

    outb(0x34,PIT_MODE);

    t = rthal_rdtsc();

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

    dt = rthal_rdtsc() - t;

    rthal_critical_exit(flags);

#ifdef CONFIG_IPIPE_TRACE_IRQSOFF
    /* reset the max trace, it contains the excessive calibration now */
    ipipe_trace_max_reset();
#endif /* CONFIG_IPIPE_TRACE_IRQSOFF */

    return rthal_imuldiv(dt,100000,RTHAL_CPU_FREQ);
}

int rthal_timer_request (void (*handler)(void),
                         unsigned long nstick)
{
    unsigned long flags;
    int err;

    flags = rthal_critical_enter(NULL);

    if (nstick > 0)
        {
        /* Periodic setup for 8254 channel #0. */
        unsigned period;
        period = (unsigned)rthal_llimd(nstick,RTHAL_TIMER_FREQ,1000000000);
        if (period > LATCH) period = LATCH;
        outb(0x34,PIT_MODE);
        outb(period & 0xff,PIT_CH0);
        outb(period >> 8,PIT_CH0);
        }
    else
        {
        /* Oneshot setup for 8254 channel #0. */
        outb(0x30,PIT_MODE);
        outb(LATCH & 0xff,PIT_CH0);
        outb(LATCH >> 8,PIT_CH0);
        }

    rthal_irq_release(RTHAL_8254_IRQ);

    err = rthal_irq_request(RTHAL_8254_IRQ,
                            (rthal_irq_handler_t)handler,
                            NULL,
                            NULL);

    rthal_critical_exit(flags);

    return err;
}

void rthal_timer_release (void)

{
    unsigned long flags;

    flags = rthal_critical_enter(NULL);
    outb(0x34,PIT_MODE);
    outb(LATCH & 0xff,PIT_CH0);
    outb(LATCH >> 8,PIT_CH0);
    rthal_irq_release(RTHAL_8254_IRQ);

    rthal_critical_exit(flags);
}

#endif /* CONFIG_X86_LOCAL_APIC */

#ifndef CONFIG_X86_TSC

static rthal_time_t rthal_tsc_8254;

static int rthal_last_8254_counter2;

/* TSC emulation using PIT channel #2. */

void rthal_setup_8254_tsc (void)

{
    unsigned long flags;
    int count;

    rthal_local_irq_save_hw(flags);

    outb_p(0x0,PIT_MODE);
    count = inb_p(PIT_CH0);
    count |= inb_p(PIT_CH0) << 8;
    outb_p(0xb4,PIT_MODE);
    outb_p(RTHAL_8254_COUNT2LATCH & 0xff,PIT_CH2);
    outb_p(RTHAL_8254_COUNT2LATCH >> 8,PIT_CH2);
    rthal_tsc_8254 = count + LATCH * jiffies;
    rthal_last_8254_counter2 = 0; 
    /* Gate high, disable speaker */
    outb_p((inb_p(0x61)&~0x2)|1,0x61);

    rthal_local_irq_restore_hw(flags);
}

rthal_time_t rthal_get_8254_tsc (void)

{
    unsigned long flags;
    int delta, count;
    rthal_time_t t;

    rthal_local_irq_save_hw(flags);

    outb(0xd8,PIT_MODE);
    count = inb(PIT_CH2);
    delta = rthal_last_8254_counter2 - (count |= (inb(PIT_CH2) << 8));
    rthal_last_8254_counter2 = count;
    rthal_tsc_8254 += (delta > 0 ? delta : delta + RTHAL_8254_COUNT2LATCH);
    t = rthal_tsc_8254;

    rthal_local_irq_restore_hw(flags);

    return t;
}

#endif /* !CONFIG_X86_TSC */

int rthal_irq_host_request (unsigned irq,
			    irqreturn_t (*handler)(int irq,
						   void *dev_id,
						   struct pt_regs *regs), 
			    char *name,
			    void *dev_id)
{
    unsigned long flags;

    if (irq >= IPIPE_NR_XIRQS || !handler)
	return -EINVAL;

    spin_lock_irqsave(&rthal_irq_descp(irq)->lock,flags);

    if (rthal_linux_irq[irq].count++ == 0 && rthal_irq_descp(irq)->action)
	{
	rthal_linux_irq[irq].flags = rthal_irq_descp(irq)->action->flags;
	rthal_irq_descp(irq)->action->flags |= SA_SHIRQ;
	}

    spin_unlock_irqrestore(&rthal_irq_descp(irq)->lock,flags);

    return request_irq(irq,handler,SA_SHIRQ,name,dev_id);
}

int rthal_irq_host_release (unsigned irq, void *dev_id)

{
    unsigned long flags;

    if (irq >= IPIPE_NR_XIRQS || rthal_linux_irq[irq].count == 0)
	return -EINVAL;

    free_irq(irq,dev_id);

    spin_lock_irqsave(&rthal_irq_descp(irq)->lock,flags);

    if (--rthal_linux_irq[irq].count == 0 && rthal_irq_descp(irq)->action)
	rthal_irq_descp(irq)->action->flags = rthal_linux_irq[irq].flags;

    spin_unlock_irqrestore(&rthal_irq_descp(irq)->lock,flags);

    return 0;
}

int rthal_irq_enable (unsigned irq)

{
    if (irq >= IPIPE_NR_XIRQS)
	return -EINVAL;

    if (rthal_irq_descp(irq)->handler == NULL ||
	rthal_irq_descp(irq)->handler->enable == NULL)
	return -ENODEV;

    rthal_irq_descp(irq)->handler->enable(irq);

    return 0;
}

int rthal_irq_disable (unsigned irq)
{

    if (irq >= IPIPE_NR_XIRQS)
	return -EINVAL;

    if (rthal_irq_descp(irq)->handler == NULL ||
	rthal_irq_descp(irq)->handler->disable == NULL)
	return -ENODEV;

    rthal_irq_descp(irq)->handler->disable(irq);

    return 0;
}

int rthal_irq_end (unsigned irq)

{
    if (irq >= IPIPE_NR_XIRQS)
	return -EINVAL;

    if (rthal_irq_descp(irq)->handler == NULL ||
	rthal_irq_descp(irq)->handler->enable == NULL)
	return -ENODEV;

    rthal_irq_descp(irq)->handler->enable(irq);

    return 0;
}

static inline int do_exception_event (unsigned event, unsigned domid, void *data)

{
    rthal_declare_cpuid;

    /* Notes:

    1) GPF needs to be propagated downstream whichever domain caused
    it. This is required so that we don't spuriously raise a fatal
    error when some fixup code is available to solve the error
    condition. For instance, Linux always attempts to reload the %gs
    segment register when switching a process in (__switch_to()),
    regardless of its value. It is then up to Linux's GPF handling
    code to search for a possible fixup whenever some exception
    occurs. In the particular case of the %gs register, such an
    exception could be raised for an exiting process if a preemption
    occurs inside a short time window, after the process's LDT has
    been dropped, but before the kernel lock is taken.  The same goes
    for Xenomai switching back a Linux thread in non-RT mode which
    happens to have been preempted inside do_exit() after the MM
    context has been dropped (thus the LDT too). In such a case, %gs
    could be reloaded with what used to be the TLS descriptor of the
    exiting thread, but unfortunately after the LDT itself has been
    dropped. Since the default LDT is only 5 entries long, any attempt
    to refer to an LDT-indexed descriptor above this value would cause
    a GPF.  2) NMI is not pipelined. */

    rthal_load_cpuid();

    if (domid == RTHAL_DOMAIN_ID)
        {
        rthal_realtime_faults[cpuid][event]++;

        if (rthal_trap_handler != NULL &&
            test_bit(cpuid,&rthal_cpu_realtime) &&
            rthal_trap_handler(event,domid,data) != 0)
            return RTHAL_EVENT_STOP;
        }

    return RTHAL_EVENT_PROPAGATE;
}

RTHAL_DECLARE_EVENT(exception_event);

static inline void do_rthal_domain_entry (void)

{
    unsigned trapnr;

    /* Trap all faults. */
    for (trapnr = 0; trapnr < RTHAL_NR_FAULTS; trapnr++)
        rthal_catch_exception(trapnr,&exception_event);

    printk(KERN_INFO "Xenomai: hal/x86 started.\n");
}

RTHAL_DECLARE_DOMAIN(rthal_domain_entry);

int rthal_arch_init (void)

{
#ifdef CONFIG_X86_LOCAL_APIC
    if (!test_bit(X86_FEATURE_APIC,boot_cpu_data.x86_capability))
    {
        printk("Xenomai: Local APIC absent or disabled!\n"
               "         Disable APIC support or pass \"lapic=1\" as bootparam.\n");
        rthal_smi_restore();
        return -ENODEV;
    }
#endif /* CONFIG_X86_LOCAL_APIC */

    if (rthal_cpufreq_arg == 0)
#ifdef CONFIG_X86_TSC
        /* FIXME: 4Ghz barrier is close... */
        rthal_cpufreq_arg = rthal_get_cpufreq();
#else /* ! CONFIG_X86_TSC */
        rthal_cpufreq_arg = CLOCK_TICK_RATE;

    rthal_setup_8254_tsc();
#endif /* CONFIG_X86_TSC */

    if (rthal_timerfreq_arg == 0)
#ifdef CONFIG_X86_LOCAL_APIC
        rthal_timerfreq_arg = apic_read(APIC_TMICT) * HZ;
#else /* !CONFIG_X86_LOCAL_APIC */
        rthal_timerfreq_arg = CLOCK_TICK_RATE;
#endif /* CONFIG_X86_LOCAL_APIC */

    return 0;
}

void rthal_arch_cleanup (void)

{
    printk(KERN_INFO "Xenomai: hal/x86 stopped.\n");
}

/*@}*/

EXPORT_SYMBOL(rthal_arch_init);
EXPORT_SYMBOL(rthal_arch_cleanup);
#ifndef CONFIG_X86_TSC
EXPORT_SYMBOL(rthal_get_8254_tsc);
#endif /* !CONFIG_X86_TSC */