arch.h

fsmlabs的real time linux的内核

BUILD_SMP_INTERRUPT(invalidate_interrupt,INVALIDATE_TLB_VECTOR)
BUILD_SMP_INTERRUPT(call_function_interrupt,CALL_FUNCTION_VECTOR)
BUILD_SMP_INTERRUPT(spurious_interrupt,SPURIOUS_APIC_VECTOR)
BUILD_SMP_INTERRUPT(error_interrupt,ERROR_APIC_VECTOR)
BUILD_SMP_TIMER_INTERRUPT(apic_timer_interrupt,LOCAL_TIMER_VECTOR)
*/

void init_local_code(const struct func_table *pf)
{
#ifdef CONFIG_SMP
	local_code.smp_reschedule_interrupt =\
	       	(void *)pf[pf_smp_reschedule_interrupt].address;
	local_code.smp_invalidate_interrupt = \
	       	(void *)pf[pf_smp_invalidate_interrupt].address;
	local_code.smp_call_function_interrupt = \
	       	(void *)pf[pf_smp_call_function_interrupt].address;
	local_code.rtl_reschedule = \
	       	(void *)pf[pf_rtl_reschedule].address;
#endif
	local_code.smp_spurious_interrupt=\
	       	(void *)pf[pf_smp_spurious_interrupt].address;
	local_code.smp_error_interrupt=\
	       	(void *)pf[pf_smp_error_interrupt].address;
        local_code.smp_apic_timer_interrupt = \
	       	(void *)pf[pf_smp_apic_timer_interrupt].address;
}

#endif /* CONFIG_X86_LOCAL_APIC */

static char * find_patch(unsigned long v )
{
       	const struct func_table *pfunc = (struct func_table *)&__start_rtlinux_funcs;
       	struct patch_table *p = (struct patch_table *)pfunc[pf___start_rtlinux_patch].address;
       	for(; (ulong)p < (ulong)pfunc[pf___stop_rtlinux_patch].address;p++) {
	       	if(((unsigned long)p->magic) == v) {
		       	return p->address;
		}
	}
       	return 0;
}


int rtl_request_traps(int (*rtl_exception_intercept)(int vector, struct pt_regs *regs, int error_code))
{
	const struct func_table *pfunc = (struct func_table *)&__start_rtlinux_funcs;
	*((long *)(pfunc[pf_rtl_exception_intercept].address)) = (long)rtl_exception_intercept;
	return 0;
}

static rtl_local_handler_t rtl_reschedule_handlers[NR_CPUS];

unsigned int default_reschedule_handler(struct pt_regs *r)
{
	return 0;
}


static int patch_kernel (unsigned int cpu_id){
	enum pfunctions i;
	char *p;
	irq_desc_t *h;
	const struct func_table *pfunc = (struct func_table *)&__start_rtlinux_funcs;
	/* make my func the pfunc */
	/* rtl_intercept */

	xdo_IRQ = (void *)pfunc[pf_do_IRQ].address; /*do_IRQ */
        local_ret_from_intr = (void *)pfunc[pf_ret_from_intr].address;
 	if( !(p = find_patch((ulong)pfunc[pf_do_IRQ].address)))
	{
		printk("RTLinux cannot patch intercept routine\n");
		return -1;
	}
	else {
#ifdef DEBUG_RTLINUX
		printk("RTLinux patch intercept=%x\n",(int)rtl_intercept);
#endif
		save_jump(p,pf_do_IRQ);
		patch_jump(p,(char *)rtl_intercept);
	}

	
	/* insert call to sti */
#ifdef DEBUG_RTLINUX
	printk("RTLinux patch kernel  skip emulate IRET DEBUG\n");
#endif
	*((long *)(pfunc[pf_rtl_emulate_iret].address)) = (long)rtl_soft_sti;

#ifdef CONFIG_X86_LOCAL_APIC 
	/* patch the calls to the smp handlers and zap their apic calls */
	for(i=PF_LOCAL_START; i <= PF_LOCAL_END;i++){
		p = find_patch((ulong)pfunc[i].address);
		if(!p){
			printk("RTLinux can't smp patch %d\n",i);
			return -1;
		}
		else{
			save_jump(p,i);
#ifdef DEBUG_RTLINUX
			printk("patching %x, jmp at %#x\n", (unsigned) (pfunc[i].address), (unsigned) p);
#endif
			patch_jump(p,(char *)rtl_local_intercept);
			zap_ack_apic(pfunc[i].address);
		}
	}
	init_local_code(pfunc);

#endif

	/* cli/sti etc */
	pre_patch_control = irq_control; /* save old irq_control */
	irq_control.do_save_flags = rtl_soft_save_flags;
	irq_control.do_restore_flags = rtl_soft_restore_flags;
	irq_control.do_cli = rtl_soft_cli;
	irq_control.do_sti = rtl_soft_sti;
	irq_control.do_local_irq_save = rtl_soft_local_irq_save;
	irq_control.do_local_irq_restore = rtl_soft_restore_flags;

	/* take over the descriptor table */
	h = (irq_desc_t *)pfunc[pf_irq_desc].address;
	for(i=0; i < NR_IRQS; i++){
		save_linux_irq_desc[i] = h[i] . handler;
#ifdef CONFIG_X86_IO_APIC
#if LINUX_VERSION_CODE >= 0x020300
		if (h[i].handler && !strcmp(h[i].handler->typename, "IO-APIC-level")) {
			if (!linux_ioapic_level_irq_type_ptr) {
				linux_ioapic_level_irq_type_ptr = h[i].handler;
				rtl_ioapic_level_irq_type = *linux_ioapic_level_irq_type_ptr;
				rtl_ioapic_level_irq_type . ack = rtl_mask_and_ack_level_ioapic_irq;
				rtl_ioapic_level_irq_type . end = rtl_end_level_io_apic_irq;
			}
			rtl_irq_desc[i]= &rtl_ioapic_level_irq_type;
		} else
#endif
#ifdef CONFIG_SMP
		if (i == RTL_RESCHEDULE_VECTOR - 0x20) {
			rtl_irq_desc[i] = &rtl_resched_irq_type;
		} else 
#endif
#endif
			rtl_irq_desc[i]= h[i].handler;

		h[i].handler = &rtl_generic_type;
	}

#ifdef DEBUG_RTLINUX
	printk("RTLinux  patch kernel done \n");
#endif
	return 0;


}

inline static int unpatch_kernel(void){ 
	irq_desc_t *h;
	int i;
       	const struct func_table *pfunc = (struct func_table *)&__start_rtlinux_funcs;
	char *p;

	*((long *)(pfunc[pf_rtl_exception_intercept].address)) = (long)0;

	irq_control=	pre_patch_control;
	*((long *)(pfunc[pf_rtl_emulate_iret].address)) = 0;
	/* replace the descriptor table */
	h = (irq_desc_t *)pfunc[pf_irq_desc].address;
	for(i=0; i < NR_IRQS; i++){
		h[i].handler = save_linux_irq_desc[i];
	}
/* 	rtl_hard_sti();		   */
	/*unpatch jump do_IRQ */
 	if( !(p = find_patch((ulong)pfunc[pf_do_IRQ].address)))
	{
		printk("RTLinux cannot unpatch patch intercept routine\n");
		return -1;
	}
	else { unpatch_jump(p,pf_do_IRQ); }

#ifdef CONFIG_X86_LOCAL_APIC
	for(i=PF_LOCAL_START; i <= PF_LOCAL_END;i++){
		p = find_patch((ulong)pfunc[i].address);
		if(!p){
			printk("RTLinux can't smp unpatch %d\n",i);
			return -1;
		}
		else{
#ifdef DEBUG_RTLINUX
			printk("unpatching %x, jmp at %#x\n", (unsigned) (pfunc[i].address), (unsigned) p);
#endif
			unpatch_jump(p,i);
			unzap_ack_apic(pfunc[i].address);
		}
	}
#endif

	return 0;
	
}

/* WARNING this fails on more than bits-per-word cpus machines
   Stephen, don't try this on your desktop Galaxy
 */
struct { atomic_t  waiting_with_cli;
	atomic_t done_patch;
	atomic_t waiting_for_unpatch;
	atomic_t done_unpatch;} sync_data = {{0},{0},{0},{0}};
void sync_on(void *unused)
{
	int i;
	rtl_hard_cli();
	atomic_inc(&sync_data.waiting_with_cli);
	i = 0;
	while (!atomic_read(&sync_data.done_patch) && i < 1000000000) {
		i++;
	}
	if (i == 1000000000) {
		printk("timed out on sync_data.done_patch\n");
	}
	rtl_hard_sti();/* and never hard cli again in linux mode */
}

void sync_off(void *unused){
	unsigned long f;
	int i;
	save_flags(f);
	rtl_hard_cli();
	atomic_inc(&sync_data.waiting_for_unpatch);
	i = 0;
	while(!atomic_read(&sync_data.done_unpatch) && i < 1000000000) {
		i++;
	}
	if (i == 1000000000) {
		printk("timed out on sync_data.done_unpatch\n");
	}
	mb();
	restore_flags(f); /* this is hard restore now! */
#ifdef DEBUG_RTLINUX
	printk("sync off returned\n");
#endif
}


#ifdef CONFIG_SMP
int rtl_smp_synchronize( void (*sync_func)(void *),atomic_t  *waitpt)
{
	int error;
	int cpus = smp_num_cpus - 1;
	int timeout;

	error= smp_call_function (sync_func, 0, 0 /*atomic */,0 /*don't wait*/);
#ifdef DEBUG_RTLINUX
	printk("smp_call_function returned\n");
#endif
	if(error) {
		printk("Cannot install due to smp call error %x\n",error);
		return -1;
	}
	/* everyone else is now starting to exec sync_function */
	timeout = jiffies + HZ;
	while ((atomic_read(waitpt) != cpus)
			&& time_before(jiffies, timeout));
	if(atomic_read(waitpt) != cpus){
	       printk("rtl_smp_synchronize timed out\n");
       	       return -1;
	}
	/* now all cpus have disabled interrupts and are waiting completion */
	return 0;
}
#else 
#define rtl_smp_synchronize(a,b) 0
#endif

unsigned int rtl_reschedule_interrupt(unsigned int irq, struct pt_regs *r);
/* Start the RTLinux operation */
/*TODO Must test smp synchronization!!! */
inline int arch_takeover(void)
{
int i;
DeclareAndInit(cpu_id);

rtl_hard_cli(); 
if(G_TEST_AND_SET(g_initialized)){ 
	printk("Can't lock to install RTL. Already installed?\n");
	rtl_hard_sti();
	return -1;
}
#ifdef DEBUG_RTLINUX
	printk("RTLinux takeover ok1\n");
#endif
	if( rtl_smp_synchronize(sync_on,&sync_data.waiting_with_cli)) return -1;

#ifdef DEBUG_RTLINUX
	printk("RTLinux takeover ok2\n");
#endif
/* initialize the main RTLinux structures */
rtl_global.flags = (1<<g_initialized);

for(i = 0; i < NR_CPUS; i++){
	rtl_local[i].flags = POS_TO_BIT(l_ienable) | POS_TO_BIT(l_idle);
	rtl_reschedule_handlers[i] = &default_reschedule_handler;
}

#ifdef DEBUG_RTLINUX
	printk("RTLinux takeover ok3\n");
#endif
patch_kernel(cpu_id); 
#ifdef CONFIG_SMP
barrier();
atomic_inc(&sync_data.done_patch);
mb();
#endif
rtl_hard_sti();
rtl_soft_sti();
#ifdef CONFIG_SMP
rtl_request_global_irq (RTL_RESCHEDULE_VECTOR - 0x20, rtl_reschedule_interrupt);
#endif

return 0;
}


/*TODO on a failure, we should refuse to remove the module! */
inline void arch_giveup(void)
{
	/* TODO test sync*/
unsigned long flags;

#ifdef CONFIG_SMP
rtl_free_global_irq (RTL_RESCHEDULE_VECTOR - 0x20);
#endif
save_flags(flags);
if(!G_TEST_AND_CLEAR(g_initialized)){ 
	printk("Can't uninstall RTL. Not installed?\n");
	return ;
}
#ifdef DEBUG_RTLINUX
	printk("RTLinux giveup ok1\n");
#endif
if(rtl_smp_synchronize(sync_off,&sync_data.waiting_for_unpatch) ){
       printk("Cannot synchronize RTLinux so cannot unpatch!\n\
	       Save all files, give to the poor, reboot\n");
	       return;
}
#ifdef DEBUG_RTLINUX
	printk("RTLinux giveup ok2\n");
#endif
rtl_hard_cli(); 
unpatch_kernel(); /* machine dependent */
#ifdef CONFIG_SMP
barrier();
atomic_inc(&sync_data.done_unpatch);
mb();
#endif
restore_flags(flags); /* hard restore now */
}

#ifdef __LOCAL_IRQS__
int rtl_request_local_irq(int i, unsigned int (*handler)(struct pt_regs *r),
			  unsigned int cpu_id)
{
	if (i == LOCAL_TIMER_VECTOR || i == RTL_RESCHEDULE_VECTOR) {
		rtl_local[cpu_id].rt_handlers[VECTOR_TO_LOCAL_PND(i)] = handler;
		L_SET_RTH(VECTOR_TO_LOCAL_PND(i));
		return 0;
	}
	return -EINVAL;
}

int rtl_free_local_irq(int i, unsigned int cpu_id)
{
	if (i == LOCAL_TIMER_VECTOR || i == RTL_RESCHEDULE_VECTOR) {
		L_CLEAR_RTH(VECTOR_TO_LOCAL_PND(i));
		return 0;
	}
	return -EINVAL;
}
#endif

#ifdef CONFIG_SMP

unsigned int rtl_reschedule_interrupt(unsigned int irq, struct pt_regs *r)
{
	int cpu_id = rtl_getcpuid();

/* 	rtl_printf("resched on %x\n", cpu_id); */
	rtl_reschedule_handlers[cpu_id](r);
	return 0;
}

void rtl_reschedule(unsigned int cpu)
{
	local_code.rtl_reschedule(cpu);
}

int rtl_request_ipi (unsigned int (*f)(struct pt_regs *r), int cpu)
{
	rtl_reschedule_handlers[cpu] = f;
/* 	return rtl_request_local_irq (RTL_RESCHEDULE_VECTOR, rtl_reschedule_interrupt, cpu); */
	return 0;
}

int rtl_free_ipi (int cpu)
{
	rtl_reschedule_handlers[cpu] = &default_reschedule_handler;
/* 	return rtl_free_local_irq (RTL_RESCHEDULE_VECTOR, cpu); */
	return 0;
}
#endif