hal.c

xenomai 很好的linux实时补丁

/**
 *   @ingroup hal
 *   @file
 *
 *   Adeos-based Real-Time Abstraction Layer for x86.
 *
 *   Inspired from original RTAI/x86 HAL interface: \n
 *   Copyright © 2000 Paolo Mantegazza, \n
 *   Copyright © 2000 Steve Papacharalambous, \n
 *   Copyright © 2000 Stuart Hughes, \n
 *
 *   RTAI/x86 rewrite over Adeos: \n
 *   Copyright © 2002 Philippe Gerum.
 *   NMI watchdog, SMI workaround: \n
 *   Copyright © 2004 Gilles Chanteperdrix.
 *
 *   Xenomai 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, Inc., 675 Mass Ave,
 *   Cambridge MA 02139, USA; either version 2 of the License, or (at
 *   your option) any later version.
 *
 *   Xenomai 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.
 */

/**
 * @addtogroup hal
 *
 * x86-specific HAL services.
 *
 *@{*/

#include <linux/version.h>
#include <linux/slab.h>
#include <linux/errno.h>
#include <linux/module.h>
#include <linux/interrupt.h>
#include <linux/console.h>
#include <asm/system.h>
#include <asm/hardirq.h>
#include <asm/desc.h>
#include <asm/io.h>
#include <asm/uaccess.h>
#include <asm/unistd.h>
#ifdef CONFIG_X86_LOCAL_APIC
#include <asm/fixmap.h>
#include <asm/bitops.h>
#include <asm/mpspec.h>
#ifdef CONFIG_X86_IO_APIC
#include <asm/io_apic.h>
#endif /* CONFIG_X86_IO_APIC */
#include <asm/apic.h>
#endif /* CONFIG_X86_LOCAL_APIC */
#include <asm/xenomai/hal.h>
#include <stdarg.h>
#ifdef CONFIG_IPIPE_TRACE
#include <linux/ipipe_trace.h>
#endif /* CONFIG_IPIPE_TRACE */

extern struct desc_struct idt_table[];

static struct {

    unsigned long flags;
    int count;

} rthal_linux_irq[IPIPE_NR_XIRQS];

#ifdef CONFIG_X86_LOCAL_APIC

static long long rthal_timers_sync_time;

struct rthal_apic_data {

    int mode;
    unsigned long count;
};

static struct rthal_apic_data rthal_timer_mode[RTHAL_NR_CPUS];

static inline void rthal_setup_periodic_apic (unsigned count,
                                              unsigned vector)
{
    apic_read(APIC_LVTT);
    apic_write_around(APIC_LVTT,APIC_LVT_TIMER_PERIODIC|vector);
    apic_read(APIC_TMICT);
    apic_write_around(APIC_TMICT,count);
}

static inline void rthal_setup_oneshot_apic (unsigned count,
                                             unsigned vector)
{
    apic_read(APIC_LVTT);
    apic_write_around(APIC_LVTT,vector);
    apic_read(APIC_TMICT);
    apic_write_around(APIC_TMICT,count);
}

static void rthal_critical_sync (void)

{
    struct rthal_apic_data *p;
    long long sync_time;
    rthal_declare_cpuid;

    switch (rthal_sync_op)
        {
        case 1:
            rthal_load_cpuid();

            p = &rthal_timer_mode[cpuid];

            sync_time = rthal_timers_sync_time;

            /* Stagger local timers on SMP systems, to prevent the
               tick handler from stupidly spinning while running on
               other CPU. */
            if(p->mode)
                sync_time += rthal_imuldiv(p->count, cpuid, num_online_cpus());

            while (rthal_rdtsc() < sync_time)
                ;
            
            if (p->mode)
                rthal_setup_periodic_apic(p->count,RTHAL_APIC_TIMER_VECTOR);
            else
                rthal_setup_oneshot_apic(p->count,RTHAL_APIC_TIMER_VECTOR);

            break;

        case 2:

            rthal_setup_periodic_apic(RTHAL_APIC_ICOUNT,LOCAL_TIMER_VECTOR);
            break;
        }
}

irqreturn_t rthal_broadcast_to_local_timers (int irq,
                                             void *dev_id,
                                             struct pt_regs *regs)
{
    unsigned long flags;

    rthal_local_irq_save_hw(flags);
    apic_wait_icr_idle();
    apic_write_around(APIC_ICR,APIC_DM_FIXED|APIC_DEST_ALLINC|LOCAL_TIMER_VECTOR);
    rthal_local_irq_restore_hw(flags);

    return IRQ_HANDLED;
}

unsigned long rthal_timer_calibrate (void)

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

    flags = rthal_critical_enter(NULL);

    t = rthal_rdtsc();

    for (i = 0; i < 10000; i++)
        { 
        apic_read(APIC_LVTT);
        apic_write_around(APIC_LVTT,APIC_LVT_TIMER_PERIODIC|LOCAL_TIMER_VECTOR);
        apic_read(APIC_TMICT);
        apic_write_around(APIC_TMICT,RTHAL_APIC_ICOUNT);
        }

    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);
}

#ifdef CONFIG_XENO_HW_NMI_DEBUG_LATENCY

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
extern void show_registers(struct pt_regs *regs);

extern spinlock_t nmi_print_lock;

void die_nmi (struct pt_regs *regs, const char *msg)
{
    spin_lock(&nmi_print_lock);
    /*
     * We are in trouble anyway, lets at least try
     * to get a message out.
     */
    bust_spinlocks(1);
    printk(msg);
    show_registers(regs);
    printk("console shuts up ...\n");
    console_silent();
    spin_unlock(&nmi_print_lock);
    bust_spinlocks(0);
    do_exit(SIGSEGV);
}
#else /* Linux >= 2.6 */
#include <asm/nmi.h>
#endif /* Linux < 2.6 */

static void rthal_latency_above_max(struct pt_regs *regs)
{
#ifdef CONFIG_IPIPE_TRACE
    ipipe_trace_freeze(rthal_maxlat_us);
#else /* !CONFIG_IPIPE_TRACE */
    char buf[128];
    snprintf(buf,
             sizeof(buf),
             "NMI watchdog detected timer latency above %u us\n",
             rthal_maxlat_us);
    die_nmi(regs, buf);
#endif /* CONFIG_IPIPE_TRACE */
}

#endif /* CONFIG_XENO_HW_NMI_DEBUG_LATENCY */

int rthal_timer_request (void (*handler)(void),
                         unsigned long nstick)
{
    struct rthal_apic_data *p;
    long long sync_time;
    unsigned long flags;
    rthal_declare_cpuid;
    int cpu;

    /* This code works both for UP+LAPIC and SMP configurations. */

    /* Try releasing the LAPIC-bound IRQ now so that any attempt to
       run a LAPIC-enabled configuration over a plain 8254-only/UP
       kernel will beget an error immediately. */

    if (rthal_irq_release(RTHAL_APIC_TIMER_IPI) < 0)
        return -EINVAL;

    flags = rthal_critical_enter(rthal_critical_sync);

    rthal_sync_op = 1;

    rthal_timers_sync_time = rthal_rdtsc() + rthal_imuldiv(LATCH,
                                                           RTHAL_CPU_FREQ,
                                                           CLOCK_TICK_RATE);

    /* We keep the setup data array just to be able to expose it to
       the visible interface if it happens to be really needed at some
       point in time. */
    
    for_each_online_cpu(cpu) {
        p = &rthal_timer_mode[cpu];
        p->mode = !!nstick;     /* 0=oneshot, 1=periodic */
        p->count = nstick;

        if (p->mode)
            p->count = rthal_imuldiv(p->count,RTHAL_TIMER_FREQ,1000000000);
        else
            p->count = RTHAL_APIC_ICOUNT;
    }

    rthal_load_cpuid();

    p = &rthal_timer_mode[cpuid];

    sync_time = rthal_timers_sync_time;

    if(p->mode)
        sync_time += rthal_imuldiv(p->count, cpuid, num_online_cpus());
    
    while (rthal_rdtsc() < sync_time)
        ;

    if (p->mode)
        rthal_setup_periodic_apic(p->count,RTHAL_APIC_TIMER_VECTOR);
    else
        rthal_setup_oneshot_apic(p->count,RTHAL_APIC_TIMER_VECTOR);

    rthal_irq_request(RTHAL_APIC_TIMER_IPI,
                      (rthal_irq_handler_t)handler,
                      NULL,
                      NULL);

    rthal_critical_exit(flags);

    rthal_irq_host_request(RTHAL_8254_IRQ,
                           &rthal_broadcast_to_local_timers,
                           "rthal_broadcast_timer",
                           &rthal_broadcast_to_local_timers);

    if (!p->mode)	/* This only works in aperiodic mode. */
	rthal_nmi_init(&rthal_latency_above_max);

    return 0;
}

void rthal_timer_release (void)

{
    unsigned long flags;

#ifdef CONFIG_XENO_HW_NMI_DEBUG_LATENCY
    rthal_nmi_release();
#endif /* CONFIG_XENO_HW_NMI_DEBUG_LATENCY */

    rthal_irq_host_release(RTHAL_8254_IRQ,
                            &rthal_broadcast_to_local_timers);

    flags = rthal_critical_enter(&rthal_critical_sync);

    rthal_sync_op = 2;
    rthal_setup_periodic_apic(RTHAL_APIC_ICOUNT,LOCAL_TIMER_VECTOR);
    rthal_irq_release(RTHAL_APIC_TIMER_IPI);

    rthal_critical_exit(flags);
}

#else /* !CONFIG_X86_LOCAL_APIC */

unsigned long rthal_timer_calibrate (void)