reserve_cpu.c

fsmlabs的real time linux的内核

/*
 * (C) Finite State Machine Labs Inc. 1999 <[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 <linux/version.h>
#include <linux/config.h>
#include <rtl_conf.h>

#ifdef CONFIG_RTL_SUSPEND_LINUX

#ifndef CONFIG_SMP
#error CONFIG_RTL_SUSPEND_LINUX is not supported for non-SMP configurations. Please disable it.
#endif

#if (LINUX_VERSION_CODE < 0x020300)
#error CONFIG_RTL_SUSPEND_LINUX is not supported for 2.2.X kernels. Please disable it.
#endif


#include <reserve_cpu.h>
#include <rtl_core.h>
#include <linux/sched.h>
#include <asm/irq.h>
#include <asm/smplock.h>
#include <linux/smp.h>
#include <rtl_signal.h>
#include <linux/malloc.h>
#define __KERNEL_SYSCALLS__
#include <linux/sched.h>
#include <linux/unistd.h>
#include <asm/uaccess.h>
static int errno;


static int reserve_thread(void * arg);
static void reserve_irq(int irq, void *dev_id, struct pt_regs *p);
static void restore_linux_irq(int irq, void *dev_id, struct pt_regs *p);
static int idle_or_fork(void *);
#define NOLINUX_CLONE	(CLONE_FS | CLONE_FILES | CLONE_SIGHAND | SIGCHLD)

/* global data: better not rmmod until all users of this
 * are gone
 * */

struct reserve_thread_data {
	atomic_t telescoping;
       	atomic_t highwater;
       	atomic_t smp_num_cpus;
	pid_t root;
	int active;
       	int rtlinux_reserve_irq;
       	int rtlinux_restore_irq;
	atomic_t exit;
	wait_queue_head_t wait_reserve;
	wait_queue_head_t wait_telescope;
	wait_queue_head_t wait_root;
	}RD;

/*TODO make these static */
int rtlinux_suspend_linux_signal(void){
	return (RD.rtlinux_reserve_irq?\
		       	RD.rtlinux_reserve_irq+ RTL_LINUX_MIN_SIGNAL:0);
}
int rtlinux_suspend_linux_irq(void) { return RD.rtlinux_reserve_irq;}
int rtlinux_restore_linux_signal(void){
	return (RD.rtlinux_restore_irq?\
		       	RD.rtlinux_restore_irq+ RTL_LINUX_MIN_SIGNAL:0);
}
int rtlinux_restore_linux_irq(void) { return RD.rtlinux_restore_irq;}

/* interrupt handlers */
static void reserve_irq(int irq, void *dev_id, struct pt_regs *p){
	printk("RTLinux got RESERVE irq\n");
	wake_up(&RD.wait_reserve);
	return;
}

/* there can't be more than one of these running at one time can
 * there?
 */
static void restore_linux_irq(int irq, void *dev_id, struct pt_regs *p){
	if(atomic_read(&RD.smp_num_cpus) < atomic_read(&RD.highwater)){
		smp_num_cpus ++;
		atomic_set(&RD.smp_num_cpus,smp_num_cpus);
	}
	return;
}


/* module init */
void rtlinux_suspend_linux_init(void){
	int irq;
	atomic_set(&RD.highwater,smp_num_cpus);
	atomic_set(&RD.smp_num_cpus,smp_num_cpus);
	init_waitqueue_head(&RD.wait_reserve);
	init_waitqueue_head(&RD.wait_telescope);
	init_waitqueue_head(&RD.wait_root);

	irq = rtl_get_soft_irq (reserve_irq, "RTLinux disable linux");
	if (irq < 0) {
		printk("RTLinux RESERVE can't get first interrupt handler\n");
		return;
	}
	RD.rtlinux_reserve_irq = irq;

	irq = rtl_get_soft_irq (restore_linux_irq, "RTLinux restore linux");
	if (irq < 0) {
		printk("RTLinux RESERVE can't get second interrupt handler\n");
		rtl_free_soft_irq(RD.rtlinux_reserve_irq);
		return;
	}

	RD.rtlinux_restore_irq = irq;

	/* start the root kernel thread */
	if ((RD.root= kernel_thread(reserve_thread,0, NOLINUX_CLONE)) < 0){
		rtl_free_irq(RD.rtlinux_reserve_irq);
		rtl_free_irq(RD.rtlinux_restore_irq);
		printk("RTLinux RESERVE fails: can't make initial Kthread\n");
		RD.rtlinux_reserve_irq =0;
		RD.rtlinux_restore_irq =0;
	}
	RD.active = 1;
	return;
}


/* kernel threads */

#define ABSURDLY_HIGH_PRIORITY 1000;
int reserve_thread(void * arg){
       	int target;
       	lock_kernel();
       	exit_files(current);
       	daemonize();
	atomic_set(&RD.telescoping,0);
       	atomic_set(&RD.highwater,smp_num_cpus);
	atomic_set(&RD.exit,0);
       	unlock_kernel();

	/* until the exit flag becomes true
	 * sleep
	 * wakeup to steal a cpu
	 * if still telescoping (stealing the last one) return
	 * else spawn threads until one is on the target CPU and
	 * can choke it.
	 * */
	 
	while(atomic_read(&RD.exit) == 0){
		       	sleep_on(&RD.wait_reserve);
//printk("RTLinux woke up reserve\n");
			if(atomic_read(&RD.exit)){
//printk("RTLinux exit\n");
				if(atomic_read(&RD.telescoping)){
//printk("RTLinux wait telescoping\n");
					sleep_on(&RD.wait_telescope);
//printk("RTLinux woke up telescope exit\n");
				}
				RD.active = 0;
//printk("RTLinux about to wake up root\n");
				wake_up(&RD.wait_root);
				return 0; /* cleanup time */
			}
			if(atomic_read(&RD.telescoping)){
				/* still working on the last one
				 * ignore this*/
				printk("RTLinux RESERVE ignore double call\n");
				continue;
			}

			/* ok now we need to act */
#if LINUX_2_4_0_FINAL_OR_LATER
		       	current->nice =  ABSURDLY_HIGH_PRIORITY;
#else
		       	current->priority =  ABSURDLY_HIGH_PRIORITY;
#endif
		       	current->policy =  SCHED_RR; /* take that */
		       	if(smp_num_cpus > atomic_read(&RD.highwater))
		       	{ /* bad things */
		       	printk("RTLinux RESERVE major sync foulup!\n");
		       	return -1;
		       	}
		       	target = smp_num_cpus -1;
		       	if(target >= 0){
			       	smp_num_cpus --;
				atomic_set(&RD.smp_num_cpus,smp_num_cpus);
			       	atomic_set(&RD.telescoping,1);
				if(kernel_thread(idle_or_fork,(void *)target, NOLINUX_CLONE)< 0){
					printk("RTLinux RESERVE root can't fork");
					smp_num_cpus++;
					atomic_set(&RD.smp_num_cpus,smp_num_cpus);
					return 0;
				}
				else continue;
			}
			else{
				rtl_printf("RTLinux RESERVE no free processors\n");
				continue;
			}
	}
	return 0;
}

extern int rtl_reserved_cpumask;
extern void flush_tlb_all(void);
static int idle_or_fork(void *kv){
	int target = (int) kv;
#if LINUX_2_4_0_FINAL_OR_LATER
       	current->nice =  ABSURDLY_HIGH_PRIORITY;
#else
       	current->priority =  ABSURDLY_HIGH_PRIORITY;
#endif
       	current->policy =  SCHED_RR; /* take that */
       	if(hw_smp_processor_id() == target){
		unsigned long mask[NR_IRQS],oldmask[NR_IRQS],i;
	       	printk("RTLinux RESERVE thread has cpu %d\n",target);
		atomic_set(&RD.telescoping,0);
		local_irq_disable();
		for(i=0; i< NR_IRQS;i++){
		rtl_irq_set_affinity (i, 0, &oldmask[i]);
		mask[i] = oldmask[i];
		clear_bit(hw_smp_processor_id(),&mask[i]);
		rtl_irq_set_affinity (i, &mask[i],0 );
		}
		set_bit (hw_smp_processor_id(), &rtl_reserved_cpumask);
		set_bit (0,&mask);
	       	while((atomic_read(&RD.smp_num_cpus)  <= target)){
			if(atomic_read(&RD.exit)){
					printk("RTLinux RESERVE exits loop\n");
					break;
					}
		}
		for(i=0; i< NR_IRQS;i++){
			rtl_irq_set_affinity (i, &oldmask[i],0);
		}
		local_irq_enable();
		flush_tlb_all();
		printk("RTLinux RESERVE unreserve %d\n",hw_smp_processor_id());
		clear_bit (hw_smp_processor_id(), &rtl_reserved_cpumask);
       	}
       	else{
	       	if(kernel_thread (idle_or_fork,kv, NOLINUX_CLONE)< 0){
		       	printk("RTLinux RESERVE nonroot can't fork\n");
			atomic_set(&RD.telescoping,0);
			smp_num_cpus++;
			atomic_set(&RD.smp_num_cpus,smp_num_cpus);
			return -1;
		}
	       	else{

		       	while(atomic_read(&RD.telescoping)){
				if(atomic_read(&RD.exit)){
					printk("RTLinux RESERVE other exits loop\n");
					break;
				}
			}
			printk("RTLinux RESERVE release %d\n",hw_smp_processor_id());
	       	}
       	}
	return 0;
}

void rtlinux_suspend_linux_cleanup(void){
	lock_kernel();
	if((volatile int)RD.active){
		if(RD.rtlinux_reserve_irq)
			rtl_free_soft_irq(RD.rtlinux_reserve_irq);
		if(RD.rtlinux_restore_irq)
			rtl_free_soft_irq(RD.rtlinux_restore_irq);
		RD.rtlinux_reserve_irq = 0;
		RD.rtlinux_restore_irq = 0;
		atomic_set(&RD.exit,1);
//printk("RTLinux debug reserve about to wake %d\n",RD.root);
		if(RD.root){ wake_up(&RD.wait_reserve); }
		unlock_kernel();
		while((volatile int)RD.active){
			sleep_on_timeout(&RD.wait_root,1000);
//	printk("RTLinux RESERVE cleanup %s\n",(RD.active?"not done":"done"));
		}

	}else  unlock_kernel();
}

#endif