rtl_time.c

来自「fsmlabs的real time linux的内核」· C语言 代码 · 共 1,029 行 · 第 1/2 页

	}

	rtl_restore_interrupts(flags);
}

static unsigned int _8254_irq(unsigned int irq, struct pt_regs *regs)
{
	int flags;
	rtl_spin_lock_irqsave (&lock8254, flags);
	if (_i8254_clock.mode == RTL_CLOCK_MODE_PERIODIC) {
		if (test_and_set_bit(0, &_i8254_clock.arch.count_irqs)) {
			_i8254_clock.value += _i8254_clock.resolution;
		}
	} else {
		_i8254_clock.arch.istimerset = 0;
	}
	rtl_hard_enable_irq(0);
	rtl_spin_unlock_irqrestore (&lock8254, flags);
	_i8254_clock.handler(regs);
	if (rtl_rt_system_is_idle()) {
		rtl_allow_interrupts();
	}
	_8254_checklinuxirq();
	rtl_stop_interrupts();
	return 0;
}


static int _8254_setperiodic (clockid_t c, hrtime_t interval)
{
	int flags;
	long t;

	rtl_spin_lock_irqsave (&lock8254, flags);
	t = RTIME_to_8254_ticks (interval) + 1;
	if (t < 10) {
		t = LATCH;
		rtl_printf("RTLinux 8254 periodic settimer set too low!\n");
	}
	if (t > LATCH) {
		t = LATCH;
		rtl_printf("RTLinux 8254 periodic settimer set too high!\n");
	}
	WRITE_COUNTER_ZERO16 (t);

	_i8254_clock.value = gethrtime();
	_i8254_clock.resolution = interval;
	_i8254_clock.arch.istimerset = 1;
	rtl_spin_unlock_irqrestore(&lock8254, flags);
	return 0;
}
	
static int _8254_setoneshot (clockid_t c, hrtime_t interval)
{
	rtl_irqstate_t flags;
	long t;
	rtl_spin_lock_irqsave (&lock8254, flags);
	if (interval > MAX_LATCH_ONESHOT) {
		interval = MAX_LATCH_ONESHOT;
	}

	t = RTIME_to_8254_ticks (interval); /* - _8254_latency); */
	if (t < 1) {
		t = 1;
	}
	WRITE_COUNTER_ZERO_ONESHOT(t);
	_i8254_clock.arch.istimerset = 1;

	rtl_spin_unlock_irqrestore(&lock8254, flags);
	return 0;
}



int _8254_settimermode (struct rtl_clock *c, int mode)
{
	if (mode == _i8254_clock.mode) {
		return 0;
	}
	if (mode == RTL_CLOCK_MODE_PERIODIC) {
		outb(0x30, 0x43);/* 8254, channel 0, mode 0, lsb+msb */
		outb(0x34, 0x43); /* binary, mode 2, LSB/MSB, ch 0 */
		_i8254_clock.mode = mode;
		_i8254_clock.gethrtime = periodic_gethrtime;
		_i8254_clock.settimer = _8254_setperiodic;
		_i8254_clock.arch.count_irqs = 0;
	} else if (mode == RTL_CLOCK_MODE_ONESHOT) {
		outb(0x30, 0x43);    /* 8254, channel 0, mode 0, lsb+msb */
		_i8254_clock.mode = mode;
		_i8254_clock.gethrtime = oneshot_gethrtime;
		_i8254_clock.settimer = _8254_setoneshot;
		_i8254_clock.resolution = HRTICKS_PER_SEC / CLOCK_TICK_RATE;
/*		{
			int i;
			hrtime_t begin = gethrtime();
			hrtime_t end;
			for (i=0; i < 100; i++) {
				_i8254_clock.settimer(&_i8254_clock, MAX_LATCH_ONESHOT);
			}
			end = gethrtime();
			_8254_latency = (int)(end - begin) / 100;
			rtl_printf("8254 oneshot settimer takes %d\n", _8254_latency);
		} */
	} else {
		return -EINVAL;
	}
	return 0;
}


extern int use_tsc;
int save_use_tsc;
static int _8254_init (clockid_t clock)
{
	int flags;
	rtl_no_interrupts (flags);

/* #ifndef CONFIG_X86_TSC */
	save_do_gettimeoffset = do_gettimeoffset;
	do_gettimeoffset = do_rt_gettimeoffset;
	save_use_tsc = use_tsc;
	use_tsc = 0;
	rtl_save_jiffies = jiffies;
/* #endif */
	_i8254_clock.arch.linux_time = gethrtime() + LATCH_NS;
	rtl_request_global_irq(0, _8254_irq);
	_8254_settimermode (clock, RTL_CLOCK_MODE_ONESHOT);
	_i8254_clock.settimer (clock, HRTIME_INFINITY);
	rtl_restore_interrupts (flags);
	return 0;
}

static void _8254_uninit (clockid_t clock)
{
	int flags;
	if (clock -> mode == RTL_CLOCK_MODE_UNINITIALIZED) {
		return;
	}
	clock->handler = RTL_CLOCK_DEFAULTS.handler;
	rtl_spin_lock_irqsave (&lock8254, flags);
/* #ifndef CONFIG_X88_TSC */
	do_gettimeoffset = save_do_gettimeoffset;
	use_tsc = save_use_tsc;
/* #endif */
	outb(0x34,0x43);		/* binary, mode 2, LSB/MSB, ch 0 */
	WRITE_COUNTER_ZERO16(LATCH);
	rtl_free_global_irq(0);
	clock -> mode = RTL_CLOCK_MODE_UNINITIALIZED;
	rtl_spin_unlock_irqrestore (&lock8254, flags);
}


/* sort of a constructor */
int rtl_create_clock_8254(void)
{
	_i8254_clock = RTL_CLOCK_DEFAULTS;
	_i8254_clock.init = _8254_init;
	_i8254_clock.uninit = _8254_uninit;
	_i8254_clock.settimermode = _8254_settimermode;
	return 0;
}


#ifdef CONFIG_X86_LOCAL_APIC
/* APIC clocks */


static long RTIME_to_apic_ticks(long t)
{
	int dummy;
#if HRTICKS_PER_SEC != NSECS_PER_SEC
	t <<= 10;
#endif
	__asm__("mull %2"
		:"=a" (dummy), "=d" (t)
		:"g" (scaler_hrtime_to_apic), "0" (t)
		);
		
	return (t);
}


static inline int rtl_apic_write_initial_count (long count)
{
	unsigned int tmp_value;
	tmp_value = apic_read(APIC_TMICT);
	apic_write (APIC_TMICT, count);
	return 0;
}

static int apic_setoneshot (clockid_t apic, hrtime_t interval)
{
	long t;
	if (apic != &_apic_clock[rtl_getcpuid()]) {
		rtl_printf("apic_setoneshot crosses CPUs!\n");
		return -1;
	}
	if (interval > MAX_LATCH_ONESHOT) {
		interval = MAX_LATCH_ONESHOT;
	}

	t = RTIME_to_apic_ticks (interval);
	if (t < 1) {
		t = 1;
	}
	rtl_apic_write_initial_count (t);
	apic->arch.istimerset = 1;
	return 0;
}

static int apic_setperiodic (clockid_t apic, hrtime_t interval)
{
	long t;
	t = RTIME_to_apic_ticks (interval);
	rtl_apic_write_initial_count (t);
	apic->value = gethrtime();
	apic->resolution = interval;
	apic->arch.istimerset = 1;
	return 0;
}

int apic_settimermode (struct rtl_clock *apic, int mode)
{
	unsigned long lvtt1_value;
	unsigned int tmp_value;

	if (apic != &_apic_clock[rtl_getcpuid()]) {
		rtl_printf("apic_settimermode crosses CPUs!\n");
		return -EINVAL;
	}

	if (mode == apic->mode) {
		return 0;
	}

	if (mode == RTL_CLOCK_MODE_PERIODIC) {
		apic -> mode = mode;
		apic -> gethrtime = periodic_gethrtime;
		apic -> settimer = apic_setperiodic;
		tmp_value = apic_read(APIC_LVTT);
		lvtt1_value = APIC_LVT_TIMER_PERIODIC | LOCAL_TIMER_VECTOR;
		apic_write(APIC_LVTT , lvtt1_value);
	} else if (mode == RTL_CLOCK_MODE_ONESHOT) {
		apic -> mode = mode;
		apic -> gethrtime = oneshot_gethrtime;
		apic -> settimer = apic_setoneshot;
		apic -> resolution = hrtime_resolution;

		tmp_value = apic_read(APIC_LVTT);
		lvtt1_value = LOCAL_TIMER_VECTOR;
		apic_write(APIC_LVTT , lvtt1_value);

	} else {
		return -EINVAL;
	}
	return 0;
}


static void apic_checklinuxirq (void)
{
	unsigned int cpu_id = rtl_getcpuid();
	clockid_t apic = &_apic_clock[cpu_id];
	hrtime_t t;
#ifdef __RTL_LOCALIRQS__
	if ((1 << cpu_id) & rtl_reserved_cpumask) return;
#endif
	t = apic->gethrtime(apic);
	if  (t > apic->arch.linux_time) {
		rtl_local_pend_vec (LOCAL_TIMER_VECTOR,cpu_id);
		apic->arch.linux_time += LATCH_NS;
		if (t > apic->arch.linux_time) {
			int njiffies = 0;
			do {
				if (++njiffies >= 2 * HZ) {
					apic->arch.linux_time = t;
/* 					rtl_printf("RTL: lost %d linux apic ticks on CPU %d\n", njiffies, cpu_id); */
					break;
				}
				apic->arch.linux_time += LATCH_NS;
			} while (t > apic->arch.linux_time);
		}
	}
}

static unsigned int apic_timer_irq(struct pt_regs *r)
{
	unsigned int cpu_id = rtl_getcpuid();
	clockid_t apic = &_apic_clock[cpu_id];
	if (apic->mode == RTL_CLOCK_MODE_PERIODIC) {
		apic->value += apic->resolution;
	} else {
		apic->arch.istimerset = 0;
	}
	apic -> handler (r);
	apic_checklinuxirq();
	if (!apic -> arch.istimerset) { // TODO move into the default handler
		apic->settimer (apic, MAX_LATCH_ONESHOT);
	}
	return 0;
}


int volatile apic_init_flag;

static unsigned int apic_init_irq(struct pt_regs *r)
{
	unsigned int cpu_id = rtl_getcpuid();
	CLOCK_APIC->settimermode (CLOCK_APIC, RTL_CLOCK_MODE_ONESHOT);
	CLOCK_APIC->settimer (CLOCK_APIC, HRTIME_INFINITY);
	rtl_free_local_irq (LOCAL_TIMER_VECTOR, cpu_id);
	rtl_request_local_irq (LOCAL_TIMER_VECTOR, apic_timer_irq, cpu_id);
	clear_bit (0, &apic_init_flag);
	return 0;
}

/* apics are initialized to the oneshot mode by default */
static int apic_clock_init (clockid_t clk)
{
	int flags;
	unsigned int cpu_id = rtl_getcpuid();

	rtl_no_interrupts (flags);
	clk->arch.linux_time = gethrtime() + LATCH_NS;

	if (clk->arch.apic_cpu == cpu_id) {
		clk->settimermode (clk, RTL_CLOCK_MODE_ONESHOT);
		clk->settimer (clk, HRTIME_INFINITY);
		rtl_request_local_irq (LOCAL_TIMER_VECTOR, apic_timer_irq, cpu_id);
	} else {
		set_bit (0, &apic_init_flag);
		rtl_request_local_irq (LOCAL_TIMER_VECTOR, apic_init_irq, clk->arch.apic_cpu);
		while (test_bit(0, &apic_init_flag));
	}
	rtl_restore_interrupts (flags);
	return 0;
}


static void apic_uninit_handler( struct pt_regs *regs)
{
	unsigned int cpu_id = rtl_getcpuid();
	CLOCK_APIC->settimermode (CLOCK_APIC, RTL_CLOCK_MODE_PERIODIC);
	CLOCK_APIC->settimer (CLOCK_APIC, LATCH_NS);
	CLOCK_APIC -> mode = RTL_CLOCK_MODE_UNINITIALIZED;
	rtl_unsetclockhandler (CLOCK_APIC);
	rtl_free_local_irq (LOCAL_TIMER_VECTOR, cpu_id);
	clear_bit (0, &apic_init_flag);
}

static void apic_clock_uninit (clockid_t clock)
{
	int flags;
	unsigned int cpu_id = rtl_getcpuid();
	rtl_no_interrupts (flags);

	if (clock -> mode == RTL_CLOCK_MODE_UNINITIALIZED) {
		rtl_restore_interrupts (flags);
		return;
	}

	if (clock->arch.apic_cpu == cpu_id) {
		apic_uninit_handler(NULL);
	} else {
		set_bit (0, &apic_init_flag);
		rtl_unsetclockhandler (clock);
		rtl_setclockhandler (clock, apic_uninit_handler);
		while (test_bit(0, &apic_init_flag));
	}
	rtl_restore_interrupts (flags);
}

int rtl_create_clock_apic(int cpu)
{
	_apic_clock[cpu] = RTL_CLOCK_DEFAULTS;
	_apic_clock[cpu].init = apic_clock_init;
	_apic_clock[cpu].uninit = apic_clock_uninit;
	_apic_clock[cpu].settimermode = apic_settimermode;
	_apic_clock[cpu].arch.apic_cpu = cpu;
	return 0;
}

#endif



/* returns a pointer to the clock structure of the best controlling hw clock 
 * for this CPU */
clockid_t rtl_getbestclock (unsigned int cpu)
{
#ifdef CONFIG_X86_LOCAL_APIC
	if (smp_found_config) {
		return &_apic_clock[cpu];
	} else {
		return &_i8254_clock;
	}
#else
	return &_i8254_clock;
#endif
}

#ifdef CONFIG_RTL_CLOCK_GPOS

static struct rtl_clock clock_gpos;
clockid_t CLOCK_GPOS = &clock_gpos;

static hrtime_t gpos_gethrtime(struct rtl_clock *c)
{
	hrtime_t t;
	pthread_spin_lock(&c->lock);
	t = gethrtime() + c->delta;
	pthread_spin_unlock(&c->lock);
	return t;
}

int rtl_clockadjust (clockid_t clock_id, hrtime_t delta)
{
	hrtime_t interdelta = delta - clock_id->delta;
	pthread_spin_lock(&clock_id->lock);
	clock_id->delta += (interdelta >> 3);
	pthread_spin_unlock(&clock_id->lock);

	return 0;
}

static int rtl_clock_gpos_irq = -1;

static void clock_irq_handler (int irq, void *dev_id, struct pt_regs *p)
{
	hrtime_t delta = gpos_get_delta();

	rtl_clockadjust (&clock_gpos, delta);

/*	do_every(300)
       	{
		rtl_printf("%d\n ", (int) clock_id->delta);
	} */
}

static void rtl_clock_gpos_init(void)
{
	rtl_clock_gpos_irq = rtl_get_soft_irq (clock_irq_handler, "RTLinux CLOCK_GPOS");
	clock_gpos = RTL_CLOCK_DEFAULTS;
	clock_gpos.gethrtime = &gpos_gethrtime;
	clock_gpos.resolution = gethrtimeres();
	clock_gpos.delta = gpos_get_delta();
}

static void rtl_clock_gpos_cleanup(void)
{
	rtl_free_soft_irq (rtl_clock_gpos_irq);
}


void rtl_clock_gpos_update(void)
{
	rtl_global_pend_irq (rtl_clock_gpos_irq);
}
#endif

int init_module (void)
{
	rtl_spin_lock_init (&lock8254);
	rtl_spin_lock_init (&lock_linuxtime);

	init_hrtime();
	rtl_create_clock_8254();
#ifdef CONFIG_X86_LOCAL_APIC
	{
	int i;	
		for (i = 0; i < rtl_num_cpus(); i++) {
			int cpu = cpu_logical_map (i);
			rtl_create_clock_apic(cpu);
		}
	}
#ifdef CONFIG_RTL_CLOCK_GPOS
	if (smp_found_config) {
		rtl_irqstate_t flags;
		rtl_no_interrupts(flags);
		_8254_init(&_i8254_clock);
		_i8254_clock.settimermode (&_i8254_clock, RTL_CLOCK_MODE_PERIODIC);
		_i8254_clock.settimer (&_i8254_clock, LATCH_NS);
		rtl_restore_interrupts(flags);
	}
#endif
#endif
	rtl_init_standard_clocks();
#ifdef CONFIG_RTL_CLOCK_GPOS
	rtl_clock_gpos_init();
#endif
	return 0;
}

void cleanup_module(void)
{
#ifdef CONFIG_RTL_CLOCK_GPOS
	rtl_clock_gpos_cleanup();
#endif
	rtl_cleanup_standard_clocks();
#ifdef CONFIG_X86_LOCAL_APIC
	if (smp_found_config) {
		int i;
		for (i = 0; i < rtl_num_cpus(); i++) {
			int cpu = cpu_logical_map(i);
			apic_clock_uninit (&_apic_clock[cpu]);
		}
	}
#endif
	_8254_uninit(&_i8254_clock);
	uninit_hrtime();
}