arch.c

fsmlabs的real time linux的内核

/*
 * (C) Finite State Machine Labs Inc. 1999-2001 <[e-mail]>
 *
 * Released under the terms of GPL 2.
 * Open RTLinux makes use of a patented process described in
 * US Patent 5,995,745. Use of this process is governed
 * by the Open RTLinux Patent License which can be obtained from
 * www.fsmlabs.com/PATENT or by sending email to
 * [e-mail]
 */

#include <asm/uaccess.h>
#include <asm/system.h>
#include <linux/irq.h>
#include <asm/irq.h>

#include "arch.h"
#include <rtl_core.h>
#include <rtl_sync.h>

#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,4,1)
struct int_control_struct rtl_hard_int_control;
#endif /* LINUX_VERSION_CODE <= KERNEL_VERSION(2,4,1) */

irq_desc_t rtl_hard_irq_desc[NR_IRQS];
unsigned long hard_timer_interrupt, hard_do_IRQ, hard_do_IRQ2, hard_do_IRQ3;
int (*hard_get_irq)(struct pt_regs *);
int bogus_get_irq(struct pt_regs *);
unsigned int (*timer_handler)(struct pt_regs *r);
extern ulong ret_to_user_hook;
extern void flush_icache_range(ulong, ulong);

#if LINUX_VERSION_CODE > KERNEL_VERSION(2,4,1)
ulong ppc_cli_block[11], ppc_sti_block[11], ppc_save_flags_ptr_block[11],
	ppc_restore_flags_block[11];
#endif

void rtl_local_intercept(struct pt_regs *regs);

int rtl_irq_set_affinity (unsigned int irq, const unsigned long *mask, unsigned long *oldmask)
{
	return -1;
}

void rtl_hard_pic_end(unsigned int irq_nr)
{
	rtl_irqstate_t flags;

	rtl_no_interrupts(flags);
	if ( rtl_hard_irq_desc[irq_nr].handler && rtl_hard_irq_desc[irq_nr].handler->end )
		rtl_hard_irq_desc[irq_nr].handler->end(irq_nr);
	rtl_restore_interrupts(flags);
}

hw_irq_controller rtl_fake_pic =
{ " RTLinux PIC  ", NULL, NULL, rtl_virt_enable, rtl_virt_disable, NULL, /*rtl_hard_pic_end*/0 };

void ppc_rtl_soft_restore_flags(unsigned long x)
{
	rtl_soft_restore_flags(x&MSR_EE);
}

void rtl_set_lost(unsigned long irq)
{
	rtl_global_pend_irq(irq);
}

struct {
	atomic_t waiting;
	atomic_t done;
} sync_data = {{0},{0}};

/*
 * Make a copy of the hard version of these functions in the kernel, then write over them
 * with a jump to our soft version.
 * -- Cort <[e-mail]>
 */
void write_func( ulong *hard_func, ulong *hard_func_end, ulong *copy_to, ulong *copy_to_end, ulong call_addr )
{
	unsigned long block[11] = {
		/* r3-r13 are caller save - so we use them */
		0x3821fff8, /* subi r1,r1,8 */
		0x7da802a6, /* mflr r13 */
		0x91a10000, /* stw r13,0(r1) */
		0x3d80dead, /* lis r12,0xdeadbeef@h */
		0x618cbeef, /* ori r12,r12,0xdeadbeef@l */
		0x7d8803a6, /* mtlr r12 */
		0x4e800021, /* blrl */
		0x81a10000, /* lwz r13,0(r1) */
		0x38210008, /* addi r1,r1,8 */
		0x7da803a6, /* mtlr r13 */
		0x4e800020  /* blr */
	};

	/* check to make sure the hard_func size is big enough to hold our patch-up */
	if ( (((ulong)hard_func_end) - ((ulong)hard_func)) < (sizeof(ulong)*11) )
	{
		printk( "write_func(): Cannot patch function at %p, too small\n",
			hard_func );
		return;
	}

	/*
	 * These blocks are allocated statically now, so we don't need to
	 * do checks at run-time.
	 * -- Cort <[e-mail]>
	 */
#if 0	
	/*
	 * Check to make sure the copy_to func size is big enough to hold
	 * the hard version of the function - but only the amount that
	 * we need to copy (11 longs).
	 */
	if ( (((ulong)copy_to_end) - ((ulong)copy_to)) <
	     (((ulong)hard_func_end) - ((ulong)hard_func)) )
	{
		printk("write_func(): Cannot copy hard function at %p, "
		       "too large %lu <= %lu\n",
		       hard_func,
		       (((ulong)copy_to_end) - ((ulong)copy_to)),
		       (((ulong)hard_func_end) - ((ulong)hard_func)));
		return;
	}
#endif	
	
	/* make a copy of the function that we're going to over-write */
	memcpy( copy_to, hard_func, sizeof(long)*11 );
	flush_icache_range( (ulong)copy_to, (ulong)copy_to + (sizeof(ulong)*11) );

	/* over-write that function with a call to our version */
	block[3] = (block[3]&0xffff0000) | (((ulong)call_addr>>16)&0xffff);
	block[4] = (block[4]&0xffff0000) | ((ulong)call_addr&0xffff);
	memcpy( (ulong *)hard_func, &block[0], sizeof(ulong)*11 );
	flush_icache_range( (ulong)hard_func, ((ulong)hard_func)+(sizeof(ulong)*11));
}

/*
 * Architecture specific function to take over the handling of interrupts.
 * -- Cort
 */
int arch_takeover(void)
{
	int i;
	struct task_struct *p;
	void rtl_soft_sti_no_emulation(void);
#ifdef CONFIG_SMP	
	int timeout, cpus = smp_num_cpus - 1;
#endif /* CONFIG_SMP */	

	if ( timer_interrupt_intercept != (unsigned long)timer_interrupt )
	{
		printk("RTL: Timer interrupt already being intercepted!\n");
		return -1;
	}

#ifdef CONFIG_SMP	
	smp_call_function( sync_takeover, 0, 0 /*atomic */,0 /*don't wait*/ );
	/* everyone else is now starting to exec sync_function */
	timeout = jiffies + HZ;
	while ((atomic_read(&sync_data.waiting) != cpus)
			&& time_before(jiffies, timeout));
	if(atomic_read(&sync_data.waiting) != cpus)
	{
	       printk("rtl_smp_synchronize timed out\n");
       	       return -1;
	}
#endif /* CONFIG_SMP */
	
	__cli();

	/*
	 * Put our trampoline code, for the ret_from_int call to rtl_soft_sti(), at
	 * KERNELBASE. -- Cort
	 */
	{
		long trash[50];
		write_func( (ulong *)KERNELBASE, (ulong *)KERNELBASE+0x30, trash, &trash[50],
			    (ulong)rtl_soft_sti );
	}
	ret_to_user_hook = 0x48000000 | 1 |
		(((KERNELBASE+0x0) - (ulong)&ret_to_user_hook)&0x03fffffc);
	flush_icache_range((ulong)&ret_to_user_hook, (ulong)&ret_to_user_hook);
	
	/* copy the current (hard) versions from the kernel... */
	hard_timer_interrupt = timer_interrupt_intercept;
	hard_do_IRQ = do_IRQ_intercept;
	hard_get_irq = ppc_md.get_irq;

	memcpy(rtl_hard_irq_desc, irq_desc, sizeof(irq_desc_t)*NR_IRQS);
	
	/*
	 * Take over cli/sti and restore/save flags, then replace
	 * them with our own versions.
	 *
	 * __sti/__cli and friends changed with v2.4.2 of Linux/PPC to
	 * outlined functions instead of function pointers.
	 *   -- Cort Dougan <[e-mail]>
	 */
#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,4,1)
	rtl_hard_int_control = int_control;
	int_control.int_cli = rtl_soft_cli;
	int_control.int_sti = rtl_soft_sti;
	int_control.int_save_flags = rtl_soft_save_flags;
	int_control.int_restore_flags = ppc_rtl_soft_restore_flags;
	int_control.int_set_lost = rtl_set_lost;
#else
	write_func( (ulong *)__cli, (ulong *)&__cli_end, &ppc_cli_block[0], &ppc_cli_block[11],
		    (ulong)rtl_soft_cli );
	write_func( (ulong *)__sti, (ulong *)&__sti_end, &ppc_sti_block[0], &ppc_sti_block[11],
		    (ulong)rtl_soft_sti );
	write_func( (ulong *)__save_flags_ptr, (ulong *)&__save_flags_ptr_end,
		    &ppc_save_flags_ptr_block[0], &ppc_save_flags_ptr_block[11],
		    (ulong)rtl_soft_save_flags );
	write_func( (ulong *)__restore_flags, (ulong *)&__restore_flags_end,
		    &ppc_restore_flags_block[0], &ppc_restore_flags_block[11],
		    (ulong)ppc_rtl_soft_restore_flags );
#endif	
	
	atomic_set(&ppc_n_lost_interrupts,0);
	for ( i = 0 ; i < NR_MASK_WORDS; i++ )
		ppc_lost_interrupts[i] = 0;
	do_IRQ_intercept = (unsigned long)rtl_intercept;
	timer_interrupt_intercept = (unsigned long)rtl_local_intercept;

	/*
	 * This should never be called since it's only used in the
	 * linux version of do_IRQ() but just in case...
	 * -- Cort
	 */
	ppc_md.get_irq = bogus_get_irq;

	/* replace all the interrupt controller pointers with our fake one */
	for ( i = 0 ; i < NR_IRQS; i++ )
		if ( irq_desc[i].handler )
			irq_desc[i].handler = &rtl_fake_pic;

	/*
	 * Now we need to correct for any tasks that may have
	 * MSR_EE cleared (interrupts disabled) in their task
	 * struct so they don't disable when switching back to
	 * them.
	 *  -- Cort
	 */
	/* have to do init_task separately */
	((struct pt_regs *)(init_task.thread.ksp+STACK_FRAME_OVERHEAD))->msr |= MSR_EE;
	for_each_task(p)
	{
		((struct pt_regs *)(p->thread.ksp+STACK_FRAME_OVERHEAD))->msr |= MSR_EE;
		if ( p->thread.regs )
			p->thread.regs->msr |= MSR_EE;
	}
	/*
	 * This hard sti matches the above cli() since the cli()
	 * was hard (RTLinux wasn't loaded yet).
	 *  -- Cort
	 */
	rtl_hard_sti();
	__sti();
#ifdef CONFIG_SMP
	atomic_set( &sync_data.done, 1);
#endif /* CONFIG_SMP */

	return 0;
}

void arch_giveup(void)
{
	int i;
	
	rtl_hard_cli();

	timer_interrupt_intercept = hard_timer_interrupt;

	do_IRQ_intercept = hard_do_IRQ;
	ppc_md.get_irq = hard_get_irq;

	/* clear out our call to to the rtl_soft_sti trampoline code in the ret from int path */
	ret_to_user_hook = 0x60000000; /* nop */
	flush_icache_range((ulong)&ret_to_user_hook, (ulong)&ret_to_user_hook);

#if LINUX_VERSION_CODE > KERNEL_VERSION(2,4,1)
	/* put the old copies of __cli/__sti and such back */
	memcpy( (void *)__cli, &ppc_cli_block[0], sizeof(ulong)*11 );
	flush_icache_range( (ulong)__cli, ((ulong)__cli)+(sizeof(ulong)*11));
	
	memcpy( (void *)__sti, &ppc_sti_block[0], sizeof(ulong)*11 );
	flush_icache_range( (ulong)__sti, ((ulong)__sti)+(sizeof(ulong)*11));
	
	memcpy( (void *)__save_flags_ptr, &ppc_save_flags_ptr_block[0], sizeof(ulong)*11 );
	flush_icache_range( (ulong)__save_flags_ptr, ((ulong)__save_flags_ptr)+(sizeof(ulong)*11));
	
	memcpy( (void *)__restore_flags, &ppc_restore_flags_block[0], sizeof(ulong)*11 );
	flush_icache_range( (ulong)__restore_flags, ((ulong)__restore_flags)+(sizeof(ulong)*11));
#else
	int_control = rtl_hard_int_control;
#endif	

	/* give Linux back the hard irq handlers */
	for ( i = 0; i < NR_IRQS; i++ )
		irq_desc[i].handler = rtl_hard_irq_desc[i].handler;

	/*
	 * This is safe since we have turned off RTLinux and
	 * the RTLinux __sti() will behave just like the normal
	 * linux __sti().  -- Cort
	 */
	__sti();
}

int bogus_get_irq(struct pt_regs *regs)
{
	printk("BOGUS_GET_IRQ! from %p\n", __builtin_return_address(0));
	*(unsigned long *)(0) = 1;
	return -1;
}

void dispatch_rtl_local_irq(int irq)
{
	struct pt_regs r;
	if ( !(unsigned long)timer_handler )
		printk("No timer handler in dispatch_rtl_local_irq!\n");
	else
		timer_handler(&r);
}

int rtl_free_local_irq(int i, unsigned int cpu)
{
	if ( (unsigned long)timer_handler )
	{
		(unsigned long)timer_handler = 0;
		clear_bit(0,&rtl_local[smp_processor_id()].rtirq);
	}
	else
		return -EINVAL;
	return 0;
}

int rtl_request_local_irq(int i, unsigned int (*handler)(struct pt_regs *r),
			  unsigned int cpu)
{
	if ( !(unsigned long)timer_handler )
	{
		timer_handler = handler;
		set_bit(0,&rtl_local[smp_processor_id()].rtirq);
		return 0;
	}
	else
		return -EINVAL;
}