Subversion Repositories shark

/* Project:     OSLib

 * Description: The OS Construction Kit

 * Date:                1.6.2000

 * Idea by:             Luca Abeni & Gerardo Lamastra

 *

 * OSLib is an SO project aimed at developing a common, easy-to-use

 * low-level infrastructure for developing OS kernels and Embedded

 * Applications; it partially derives from the HARTIK project but it

 * currently is independently developed.

 *

 * OSLib is distributed under GPL License, and some of its code has

 * been derived from the Linux kernel source; also some important

 * ideas come from studying the DJGPP go32 extender.

 *

 * We acknowledge the Linux Community, Free Software Foundation,

 * D.J. Delorie and all the other developers who believe in the

 * freedom of software and ideas.

 *

 * For legalese, check out the included GPL license.

 */

/*	Advanced Timer Managment

 *	Author: Giacomo Guidi <giacomo@gandalf.sssup.it>

 */

#include <ll/i386/stdlib.h>

#include <ll/i386/error.h>

#include <ll/i386/advtimer.h>

#include <ll/sys/ll/ll-data.h>

#include <ll/sys/ll/ll-func.h>

#include <ll/i386/pic.h>

#include <ll/sys/ll/event.h>

#include <ll/sys/ll/time.h>

unsigned char use_tsc = 1; //Enable the TSC counter mode

unsigned char use_cmos = 0; //Enable the RTC correction

//Max single delta_clk_per_msec increment = clk_per_msec / MAX_DIV_INK;

#define MAX_DIV_INK 30000

signed long long init_tsc;

signed long long * ptr_init_tsc = &init_tsc;

signed long long init_nsec; //Wraparound 292 years

signed long long * ptr_init_nsec = &init_nsec;

signed long long clk_per_msec;

signed long long * ptr_clk_per_msec = &clk_per_msec;

signed long last_delta_clk_per_msec;

signed long total_delta_clk_per_msec;

unsigned char save_CMOS_regA;

unsigned char save_CMOS_regB;

//#define IRQ8_DEBUG

void HandlerIRQ8(void *p)

{

   unsigned char set;

   static unsigned long Mconst = 1000000;

   static unsigned long init_step = 0;

   signed long max_dcms = clk_per_msec / MAX_DIV_INK;

   static signed long long dn;

   static signed long long * ptr_dn = &dn;

   signed long delta_clk_per_msec;

   cli();

   #ifdef IRQ8_DEBUG

     message("(IRQ8");

   #endif

   CMOS_READ(0x0C,set);

   __asm__("xorl %%eax,%%eax\n\t"

           "cpuid\n\t"

           "rdtsc\n\t"

	   "pushl %%eax\n\t"

	   "pushl %%edx\n\t"

           "pushl %%eax\n\t"

           "pushl %%edx\n\t"

           "xorl %%eax,%%eax\n\t"

           "cpuid\n\t"

           "popl %%edx\n\t"

           "popl %%eax\n\t"

	   "subl (%%edi),%%eax\n\t"

	   "sbbl 4(%%edi),%%edx\n\t"

	   "popl 4(%%edi)\n\t"

	   "popl (%%edi)\n\t"

	   "movl %%edx,%%ecx\n\t"

	   "mull %4\n\t"

	   "pushl %%eax\n\t"

	   "movl %%ecx,%%eax\n\t"

	   "movl %%edx,%%ecx\n\t"

	   "mull %4\n\t"

	   "addl %%ecx,%%eax\n\t"

	   "adcl $0,%%edx\n\t"

           "movl %7,%%ebx\n\t"

	   "divl (%%ebx)\n\t"

	   "movl %%eax,4(%%esi)\n\t"

	   "popl %%eax\n\t"

	   "divl (%%ebx)\n\t"

	   "movl %%eax,(%%esi)\n\t"

	    :

	    : "D" (ptr_init_tsc), "S" (ptr_dn), "b" (0),

	      "c" (0), "m" (Mconst), "a" (0), "d" (0), "m" (ptr_clk_per_msec));

   //Offset

   init_nsec += dn;

   if (init_step < 5) {

	   init_step++;

           sti();

           #ifdef IRQ8_DEBUG

             message(")");

           #endif

	   return;

   }

   dn = dn % 1000000000 - 500000000;

   //Delta clk/msec

   delta_clk_per_msec = dn * clk_per_msec / (500000000 - dn);

   //clk_per_msec adjustment

   if (delta_clk_per_msec < 0) {

	if (delta_clk_per_msec > -max_dcms)

		clk_per_msec += delta_clk_per_msec;

	else

		clk_per_msec -= max_dcms;

   } else {

	if (delta_clk_per_msec < max_dcms)

		clk_per_msec += delta_clk_per_msec;

	else

		clk_per_msec += max_dcms;

   }

   last_delta_clk_per_msec = delta_clk_per_msec;

   total_delta_clk_per_msec += delta_clk_per_msec;

   #ifdef IRQ8_DEBUG

      message(")");

   #endif

   sti();

}

#ifdef CONFIG_MELAN

#  define CLOCK_TICK_RATE 1189200 /* AMD Elan has different frequency! */

#else

#  define CLOCK_TICK_RATE 1193180 /* Underlying HZ */

#endif

#define COUNTER_END 1000

//TSC Calibration (idea from the linux kernel code)

void ll_calibrate_tsc(void)

{

	signed long long start;

	signed long long end;

	signed long long dtsc1,dtsc2,dtsc;

	signed long start_8253, end_8253, delta1_8253, delta2_8253, delta_8253;

	cli();

        /* Set the Gate high, disable speaker */

	outp(0x61, (inp(0x61) & ~0x02) | 0x01);

	outp(0x43,0xB0);			/* binary, mode 0, LSB/MSB, Ch 2 */

	outp(0x42,0xFF);			/* LSB of count */

	outp(0x42,0xFF);			/* MSB of count */

        outp(0x43,0x00);

	start_8253 = inp(0x42);

	start_8253 |= inp(0x42) << 8;

        rdtscll(start);

	do {

	    outp(0x43,0x00);

	    end_8253 = inp(0x42);

	    end_8253 |= inp(0x42) << 8;

	} while (end_8253 > COUNTER_END);

        do {

            outp(0x43,0x00);

            end_8253 = inp(0x42);

            end_8253 |= inp(0x42) << 8;

        } while (end_8253 < COUNTER_END);

        do {

            outp(0x43,0x00);

            end_8253 = inp(0x42);

            end_8253 |= inp(0x42) << 8;

        } while (end_8253 > COUNTER_END);

	outp(0x43,0x00);

	end_8253 = inp(0x42);

	end_8253 |= inp(0x42) << 8;

        rdtscll(end);

	//Delta TSC

   	dtsc1 = end - start;

	//Delta PIT

	delta1_8253 = start_8253 - end_8253 + 0x10001;

        /* Set the Gate high, disable speaker */

	outp(0x61, (inp(0x61) & ~0x02) | 0x01);

	outp(0x43,0xB0);			/* binary, mode 0, LSB/MSB, Ch 2 */

	outp(0x42,0xFF);			/* LSB of count */

	outp(0x42,0xFF);			/* MSB of count */

        outp(0x43,0x00);

	start_8253 = inp(0x42);

	start_8253 |= inp(0x42) << 8;

        rdtscll(start);

	do {

	    outp(0x43,0x00);

	    end_8253 = inp(0x42);

	    end_8253 |= inp(0x42) << 8;

	} while (end_8253 > COUNTER_END);

        do {

            outp(0x43,0x00);

            end_8253 = inp(0x42);

            end_8253 |= inp(0x42) << 8;

        } while (end_8253 < COUNTER_END);

        do {

            outp(0x43,0x00);

            end_8253 = inp(0x42);

            end_8253 |= inp(0x42) << 8;

        } while (end_8253 > COUNTER_END);

	outp(0x43,0x00);

	end_8253 = inp(0x42);

	end_8253 |= inp(0x42) << 8;

        rdtscll(end);

	//Delta TSC

   	dtsc2 = end - start;

	//Delta PIT

	delta2_8253 = start_8253 - end_8253 + 0x10001;

        if (dtsc1 > dtsc2) {

          dtsc = dtsc1;

          delta_8253 = delta1_8253;

        } else {

          dtsc = dtsc2;

          delta_8253 = delta2_8253;

        }

	if (delta_8253 > 0x20000) {

		message("Error calculating Delta PIT\n");

		ll_abort(10);

	}

	message("Delta TSC               = %10ld\n",(long)dtsc);

	message("Delta PIT               = %10ld\n",(long)delta_8253);

	clk_per_msec = dtsc * CLOCK_TICK_RATE / delta_8253 / 1000;

	message("Calibrated Clk_per_msec = %10ld\n",(long)clk_per_msec);

	sti();

}

//Low level time read function

void ll_read_timespec(struct timespec *tspec)

{

    static unsigned long Gconst = 1000000000;

    static unsigned long Mconst = 1000000;

    if (clk_per_msec <= 0) {

	    NULL_TIMESPEC(tspec);

	    return;

    }

    __asm__("xorl %%eax,%%eax\n\t"

            "cpuid\n\t"

            "rdtsc\n\t"

            "pushl %%eax\n\t"

            "pushl %%edx\n\t"

	    "xorl %%eax,%%eax\n\t"

            "cpuid\n\t"

            "popl %%edx\n\t"

            "popl %%eax\n\t"

            "subl (%%edi),%%eax\n\t"

	    "sbbl 4(%%edi),%%edx\n\t"

	    "movl %%edx,%%ecx\n\t"

	    "mull %6\n\t"

	    "pushl %%eax\n\t"

	    "movl %%ecx,%%eax\n\t"

	    "movl %%edx,%%ecx\n\t"

	    "mull %6\n\t"

	    "addl %%ecx,%%eax\n\t"

	    "adcl $0,%%edx\n\t"

            "movl %8,%%ebx\n\t"

	    "divl (%%ebx)\n\t"

	    "movl %%eax,%%ecx\n\t"

	    "popl %%eax\n\t"

	    "divl (%%ebx)\n\t"

	    "movl %%ecx,%%edx\n\t"

	    "addl (%%esi),%%eax\n\t"

	    "adcl 4(%%esi),%%edx\n\t"

	    "divl %7\n\t"

	    : "=a" (tspec->tv_sec), "=d" (tspec->tv_nsec)

	    : "D" (ptr_init_tsc), "S" (ptr_init_nsec), "b" (0),

	      "c" (0), "m" (Mconst), "m" (Gconst), "m" (ptr_clk_per_msec));

}

void ll_init_advtimer()

{

    if (use_tsc) {

	ll_calibrate_tsc();

	last_delta_clk_per_msec = 0;

	total_delta_clk_per_msec = 0;

	rdtscll(init_tsc); // Read start TSC

	init_nsec = 0;

	if (use_cmos) {

	    message("CMOS adjustement enabled\n");

    	    cli();

	    irq_bind(8, HandlerIRQ8, INT_FORCE);

	    CMOS_READ(0x0A,save_CMOS_regA);

	    CMOS_READ(0x0B,save_CMOS_regB);

	    CMOS_WRITE(0x0A,0x2F); // Set 2 Hz Periodic Interrupt

	    CMOS_WRITE(0x0B,0x42); // Enable Interrupt

	    irq_unmask(8);

	    sti();

	}

    } else {

	use_cmos = 0;

   }

}

void ll_restore_CMOS()

{

	if (use_cmos) {

		cli();

		irq_mask(8);

		CMOS_WRITE(0x0A,save_CMOS_regA);

		CMOS_WRITE(0x0B,save_CMOS_regB);

		sti();

	}

}