hal.c

xenomai 很好的linux实时补丁

/**
 *   @ingroup hal
 *   @file
 *
 *   Generic Real-Time HAL.
 *   Copyright © 2005 Philippe Gerum.
 *
 *   This program 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.
 *
 *   This program 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.
 */

/**
 * @defgroup hal HAL.
 *
 * Generic Adeos-based hardware abstraction layer.
 *
 *@{*/

#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 <linux/kallsyms.h>
#include <asm/system.h>
#include <asm/hardirq.h>
#include <asm/irq.h>
#include <asm/uaccess.h>
#include <asm/unistd.h>
#include <asm/xenomai/hal.h>
#ifdef CONFIG_PROC_FS
#include <linux/proc_fs.h>
#endif /* CONFIG_PROC_FS */
#include <stdarg.h>

MODULE_LICENSE("GPL");

unsigned long rthal_cpufreq_arg;
module_param_named(cpufreq,rthal_cpufreq_arg,ulong,0444);

unsigned long rthal_timerfreq_arg;
module_param_named(timerfreq,rthal_timerfreq_arg,ulong,0444);

static struct {

    void (*handler)(void *cookie);
    void *cookie;
    const char *name;
    unsigned long hits[RTHAL_NR_CPUS];

} rthal_apc_table[RTHAL_NR_APCS];

static int rthal_init_done;

static unsigned rthal_apc_virq;

static unsigned long rthal_apc_map;

static unsigned long rthal_apc_pending[RTHAL_NR_CPUS];

static rthal_spinlock_t rthal_apc_lock = RTHAL_SPIN_LOCK_UNLOCKED;

static atomic_t rthal_sync_count = ATOMIC_INIT(1);

rthal_pipeline_stage_t rthal_domain;

struct rthal_calibration_data rthal_tunables;

rthal_trap_handler_t rthal_trap_handler;

int rthal_realtime_faults[RTHAL_NR_CPUS][RTHAL_NR_FAULTS];

volatile int rthal_sync_op;

volatile unsigned long rthal_cpu_realtime;

unsigned long rthal_critical_enter (void (*synch)(void))

{
    unsigned long flags = rthal_grab_superlock(synch);

    if (atomic_dec_and_test(&rthal_sync_count))
	rthal_sync_op = 0;
    else if (synch != NULL)
	printk(KERN_WARNING "Xenomai: Nested critical sync will fail.\n");

    return flags;
}

void rthal_critical_exit (unsigned long flags)

{
    atomic_inc(&rthal_sync_count);
    rthal_release_superlock(flags);
}

/**
 * @fn int rthal_irq_request(unsigned irq, rthal_irq_handler_t handler, rthal_irq_ackfn_t ackfn, void *cookie)
 *
 * @brief Install a real-time interrupt handler.
 *
 * Installs an interrupt handler for the specified IRQ line by
 * requesting the appropriate Adeos virtualization service. The
 * handler is invoked by Adeos on behalf of the Xenomai domain
 * context.  Once installed, the HAL interrupt handler will be called
 * prior to the regular Linux handler for the same interrupt source.
 *
 * @param irq The hardware interrupt channel to install a handler on.
 * This value is architecture-dependent.
 *
 * @param handler The address of a valid interrupt service routine.
 * This handler will be called each time the corresponding IRQ is
 * delivered, and will be passed the @a cookie value unmodified.
 *
 * @param ackfn The address of an optional interrupt acknowledge
 * routine, aimed at replacing the one provided by Adeos. Only very
 * specific situations actually require to override the default Adeos
 * setting for this parameter, like having to acknowledge non-standard
 * PIC hardware. @a ackfn should return a non-zero value to indicate
 * that the interrupt has been properly acknowledged. If @a ackfn is
 * NULL, the default Adeos routine will be used instead.
 *
 * @param cookie A user-defined opaque cookie the HAL will pass to the
 * interrupt handler as its sole argument.
 *
 * @return 0 is returned upon success. Otherwise:
 *
 * - -EBUSY is returned if an interrupt handler is already installed.
 * rthal_irq_release() must be issued first before a handler is
 * installed anew.
 *
 * - -EINVAL is returned if @a irq is invalid or @a handler is NULL.
 *
 * - Other error codes might be returned in case some internal error
 * happens at the Adeos level. Such error might caused by conflicting
 * Adeos requests made by third-party code.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Any domain context.
 */

int rthal_irq_request (unsigned irq,
		       rthal_irq_handler_t handler,
		       rthal_irq_ackfn_t ackfn,
		       void *cookie)
{
    unsigned long flags;
    int err = 0;

    if (handler == NULL || irq >= IPIPE_NR_IRQS)
	return -EINVAL;

    flags = rthal_critical_enter(NULL);

    if (rthal_irq_handler(&rthal_domain, irq) != NULL)
	{
	err = -EBUSY;
	goto unlock_and_exit;
	}

    err = rthal_virtualize_irq(&rthal_domain,
			       irq,
			       handler,
			       cookie,
			       ackfn,
			       IPIPE_DYNAMIC_MASK);

 unlock_and_exit:

    rthal_critical_exit(flags);

    return err;
}

/**
 * @fn int rthal_irq_release(unsigned irq)
 *                           
 * @brief Uninstall a real-time interrupt handler.
 *
 * Uninstalls an interrupt handler previously attached using the
 * rthal_irq_request() service.
 *
 * @param irq The hardware interrupt channel to uninstall a handler
 * from.  This value is architecture-dependent.
 *
 * @return 0 is returned upon success. Otherwise:
 *
 * - -EINVAL is returned if @a irq is invalid.
 *
 * - Other error codes might be returned in case some internal error
 * happens at the Adeos level. Such error might caused by conflicting
 * Adeos requests made by third-party code.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Any domain context.
 */

int rthal_irq_release (unsigned irq)

{
    int err = 0;
	
    if (irq >= IPIPE_NR_IRQS)
	return -EINVAL;

    err = rthal_virtualize_irq(&rthal_domain,
			       irq,
			       NULL,
			       NULL,
			       NULL,
			       IPIPE_PASS_MASK);

    return err;
}

/**
 * @fn int rthal_irq_host_request (unsigned irq,irqreturn_t (*handler)(int irq,void *dev_id,struct pt_regs *regs),char *name,void *dev_id)
 *                           
 * @brief Install a shared Linux interrupt handler.
 *
 * Installs a shared interrupt handler in the Linux domain for the
 * given interrupt source.  The handler is appended to the existing
 * list of Linux handlers for this interrupt source.
 *
 * @param irq The interrupt source to attach the shared handler to.
 * This value is architecture-dependent.
 *
 * @param handler The address of a valid interrupt service routine.
 * This handler will be called each time the corresponding IRQ is
 * delivered, as part of the chain of existing regular Linux handlers
 * for this interrupt source. The handler prototype is the same as the
 * one required by the request_irq() service provided by the Linux
 * kernel.
 *
 * @param name is a symbolic name identifying the handler which will
 * get reported through the /proc/interrupts interface.
 *
 * @param dev_id is a unique device id, identical in essence to the
 * one requested by the request_irq() service.
 *
 * @return 0 is returned upon success. Otherwise:
 *
 * - -EINVAL is returned if @a irq is invalid or @a handler is NULL.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Linux domain context.
 */

/**
 * @fn int rthal_irq_host_release (unsigned irq,void *dev_id)
 *                           
 * @brief Uninstall a shared Linux interrupt handler.
 *
 * Uninstalls a shared interrupt handler from the Linux domain for the
 * given interrupt source.  The handler is removed from the existing
 * list of Linux handlers for this interrupt source.
 *
 * @param irq The interrupt source to detach the shared handler from.
 * This value is architecture-dependent.
 *
 * @param dev_id is a valid device id, identical in essence to the one
 * requested by the free_irq() service provided by the Linux
 * kernel. This value will be used to locate the handler to remove
 * from the chain of existing Linux handlers for the given interrupt
 * source. This parameter must match the device id. passed to
 * rthal_irq_host_request() for the same handler instance.
 *
 * @return 0 is returned upon success. Otherwise:
 *
 * - -EINVAL is returned if @a irq is invalid.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Linux domain context.
 */

/**
 * @fn int rthal_irq_host_pend (unsigned irq)
 *                           
 * @brief Propagate an IRQ event to Linux.
 *
 * Causes the given IRQ to be propagated down to the Adeos pipeline to
 * the Linux kernel. This operation is typically used after the given
 * IRQ has been processed into the Xenomai domain by a real-time
 * interrupt handler (see rthal_irq_request()), in case such interrupt
 * must also be handled by the Linux kernel.
 *
 * @param irq The interrupt source to detach the shared handler from.
 * This value is architecture-dependent.
 *
 * @return 0 is returned upon success. Otherwise:
 *
 * - -EINVAL is returned if @a irq is invalid.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Xenomai domain context.
 */

int rthal_irq_host_pend (unsigned irq)

{
    return rthal_propagate_irq(irq);
}

/**
 * @fn int rthal_irq_affinity (unsigned irq,cpumask_t cpumask,cpumask_t *oldmask)
 *                           
 * @brief Set/Get processor affinity for external interrupt.
 *
 * On SMP systems, this service ensures that the given interrupt is
 * preferably dispatched to the specified set of processors. The
 * previous affinity mask is returned by this service.
 * 
 * @param irq The interrupt source whose processor affinity is
 * affected by the operation. Only external interrupts can have their
 * affinity changed/queried, thus virtual interrupt numbers allocated
 * by rthal_alloc_virq() are invalid values for this parameter.
 *
 * @param cpumask A list of CPU identifiers passed as a bitmask
 * representing the new affinity for this interrupt. A zero value
 * cause this service to return the current affinity mask without
 * changing it.
 * 
 * @param oldmask If non-NULL, a pointer to a memory area which will
 * bve overwritten by the previous affinity mask used for this
 * interrupt source, or a zeroed mask if an error occurred.  This
 * service always returns a zeroed mask on uniprocessor systems.
 *
 * @return 0 is returned upon success. Otherwise:
 *
 * - -EINVAL is returned if @a irq is invalid.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Linux domain context.
 */

#ifdef CONFIG_SMP

int rthal_irq_affinity (unsigned irq, cpumask_t cpumask, cpumask_t *oldmask)

{
    cpumask_t _oldmask;

    if (irq >= IPIPE_NR_XIRQS)
	return -EINVAL;

    _oldmask = rthal_set_irq_affinity(irq,cpumask);

    if (oldmask)
	*oldmask = _oldmask;

    return cpus_empty(_oldmask) ? -EINVAL : 0;
}

#else /* !CONFIG_SMP */

int rthal_irq_affinity (unsigned irq, cpumask_t cpumask, cpumask_t *oldmask)
{
    return 0;
}

#endif /* CONFIG_SMP */

/**
 * @fn int rthal_trap_catch (rthal_trap_handler_t handler)
 *                           
 * @brief Installs a fault handler.
 *
 * The HAL attempts to invoke a fault handler whenever an uncontrolled
 * exception or fault is caught at machine level. This service allows
 * to install a user-defined handler for such events.
 *
 * @param handler The address of the fault handler to call upon
 * exception condition. The handler is passed the address of the
 * low-level information block describing the fault as passed by
 * Adeos. Its layout is implementation-dependent.
 *
 * @return The address of the fault handler previously installed.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Any domain context.
 */

rthal_trap_handler_t rthal_trap_catch (rthal_trap_handler_t handler)
{
    return (rthal_trap_handler_t)xchg(&rthal_trap_handler,handler);
}

static void rthal_apc_handler (unsigned virq, void *arg)
{
    void (*handler)(void *), *cookie;
    rthal_declare_cpuid;

    rthal_load_cpuid();

    rthal_spin_lock(&rthal_apc_lock);

    /* <!> This loop is not protected against a handler becoming
       unavailable while processing the pending queue; the software
       must make sure to uninstall all apcs before eventually
       unloading any module that may contain apc handlers. We keep the
       handler affinity with the poster's CPU, so that the handler is
       invoked on the same CPU than the code which called
       rthal_apc_schedule(). */

    while (rthal_apc_pending[cpuid] != 0)
	{
	int apc = ffnz(rthal_apc_pending[cpuid]);
	clear_bit(apc,&rthal_apc_pending[cpuid]);
	handler = rthal_apc_table[apc].handler;
	cookie = rthal_apc_table[apc].cookie;
	rthal_apc_table[apc].hits[cpuid]++;
	rthal_spin_unlock(&rthal_apc_lock);
	handler(cookie);
	rthal_spin_lock(&rthal_apc_lock);
	}

    rthal_spin_unlock(&rthal_apc_lock);
}

#ifdef CONFIG_PREEMPT_RT

/* On PREEMPT_RT, we need to invoke the apc handlers over a process
   context, so that the latter can access non-atomic kernel services
   properly. So the Adeos virq is only used to kick a per-CPU apc
   server process which in turns runs the apc dispatcher. A bit
   twisted, but indeed consistent with the threaded IRQ model of
   PREEMPT_RT. */

#include <linux/kthread.h>

static struct task_struct *rthal_apc_servers[RTHAL_NR_CPUS];

static int rthal_apc_thread (void *data)

{
    unsigned cpu = (unsigned)(unsigned long)data;

    set_cpus_allowed(current, cpumask_of_cpu(cpu));
    sigfillset(&current->blocked);
    current->flags |= PF_NOFREEZE;
    /* Use highest priority here, since some apc handlers might
       require to run as soon as possible after the request has been
       pended. */
    rthal_setsched_root(current,SCHED_FIFO,MAX_RT_PRIO-1);

    while (!kthread_should_stop()) {
	set_current_state(TASK_INTERRUPTIBLE);
	schedule();
	rthal_apc_handler(0);
    }

    __set_current_state(TASK_RUNNING);

    return 0;
}

void rthal_apc_kicker (unsigned virq, void *cookie)

{
    wake_up_process(rthal_apc_servers[smp_processor_id()]);
}

#define rthal_apc_trampoline rthal_apc_kicker

#else /* !CONFIG_PREEMPT_RT */

#define rthal_apc_trampoline rthal_apc_handler

#endif /* CONFIG_PREEMPT_RT */

/**
 * @fn int rthal_apc_alloc (const char *name,void (*handler)(void *cookie),void *cookie)
 *
 * @brief Allocate an APC slot.
 *
 * APC is the acronym for Asynchronous Procedure Call, a mean by which
 * activities from the Xenomai domain can schedule deferred
 * invocations of handlers to be run into the Linux domain, as soon as
 * possible when the Linux kernel gets back in control. Up to
 * BITS_PER_LONG APC slots can be active at any point in time. APC
 * support is built upon Adeos's virtual interrupt support.
 *
 * The HAL guarantees that any Linux kernel service which would be
 * callable from a regular Linux interrupt handler is also available
 * to APC handlers, including over PREEMPT_RT kernels exhibiting a
 * threaded IRQ model.
 *
 * @param name is a symbolic name identifying the APC which will get
 * reported through the /proc/xenomai/apc interface.
 *
 * @param handler The address of the fault handler to call upon
 * exception condition. The handle will be passed the @a cookie value
 * unmodified.
 *
 * @param cookie A user-defined opaque cookie the HAL will pass to the
 * APC handler as its sole argument.
 *
 * @return an valid APC id. is returned upon success, or a negative
 * error code otherwise:
 *
 * - -EINVAL is returned if @a handler is invalid.
 *
 * - -EBUSY is returned if no more APC slots are available.
 *
 * Environments:
 *
 * This service can be called from:
 *
 * - Linux domain context.
 */

int rthal_apc_alloc (const char *name,
		     void (*handler)(void *cookie),
		     void *cookie)
{
    unsigned long flags;
    int apc;

    if (handler == NULL)
	return -EINVAL;

    rthal_spin_lock_irqsave(&rthal_apc_lock,flags);

    if (rthal_apc_map != ~0)
	{
	apc = ffz(rthal_apc_map);
	set_bit(apc,&rthal_apc_map);
	rthal_apc_table[apc].handler = handler;
	rthal_apc_table[apc].cookie = cookie;
	rthal_apc_table[apc].name = name;
	}
    else
	apc = -EBUSY;

    rthal_spin_unlock_irqrestore(&rthal_apc_lock,flags);