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/i386/apic.h>

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

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

#define CALIBRATE_USING_CMOS

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

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

unsigned char use_apic = 1; //Enable the APIC

//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 long apic_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++;

           #ifdef IRQ8_DEBUG

             message(")");

           #endif

           sti();

           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 1193182 /* Underlying HZ */

#endif

#define COUNTER_END 100

#define barrier() __asm__ __volatile__("" ::: "memory");

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

void ll_calibrate_tsc(void)

{

        signed long long start;

        signed long long end;

        signed long long dtsc;

        signed long start_8253, end_8253, delta_8253;

        cli();

        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 */

        barrier();

        rdtscll(start);

        barrier();

        outp(0x43,0x00);

        start_8253 = inp(0x42);

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

        barrier();

        rdtscll(start);

        barrier();

        do {

            outp(0x43,0x00);

            end_8253 = inp(0x42);

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

        } while (end_8253 > COUNTER_END);

        barrier();

        rdtscll(end);

        barrier();

        outp(0x43,0x00);

        end_8253 = inp(0x42);

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

        barrier();

        rdtscll(end);

        barrier();

        //Delta TSC

        dtsc = end - start;

        //Delta PIT

        delta_8253 = start_8253 - end_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();

}

#define CMOS_INIT  0

#define CMOS_BEGIN 1

#define CMOS_START 2

#define CMOS_END   3

int cmos_calibrate_status = CMOS_INIT;

signed long long irq8_start;

signed long long irq8_end;

void calibrate_tsc_IRQ8(void *p)

{

  unsigned char set;

  cli();

  CMOS_READ(0x0C,set);

  barrier();

  rdtscll(irq8_end);

  barrier();

  if (cmos_calibrate_status == CMOS_START) {

    cmos_calibrate_status = CMOS_END;

  }

  if (cmos_calibrate_status == CMOS_BEGIN) {

    irq8_start = irq8_end;

    cmos_calibrate_status = CMOS_START;

  }

  if (cmos_calibrate_status == CMOS_INIT) {

    cmos_calibrate_status = CMOS_BEGIN;

  }

  sti();

}

//TSC Calibration using RTC

void ll_calibrate_tsc_cmos(void)

{

  signed long long dtsc;

  cli();           

  irq_bind(8, calibrate_tsc_IRQ8, 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();

  while (cmos_calibrate_status != CMOS_END) {

    barrier();

  }

  dtsc = irq8_end - irq8_start;

  clk_per_msec = dtsc / 500;

  message("Calibrated CPU Clk/msec  = %10ld\n",(long)clk_per_msec);

  cli();

  irq_mask(8);

  CMOS_WRITE(0x0A,save_CMOS_regA);

  CMOS_WRITE(0x0B,save_CMOS_regB);

  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));

}

int apic_get_maxlvt(void)

{

        unsigned int v, ver, maxlvt;

        v = apic_read(APIC_LVR);

        ver = GET_APIC_VERSION(v);

        /* 82489DXs do not report # of LVT entries. */

        maxlvt = APIC_INTEGRATED(ver) ? GET_APIC_MAXLVT(v) : 2;

        return maxlvt;

}

void clear_local_APIC(void)

{

        int maxlvt;

        unsigned long v;

        maxlvt = apic_get_maxlvt();

        /*

         * Masking an LVT entry on a P6 can trigger a local APIC error

         * if the vector is zero. Mask LVTERR first to prevent this.

         */

        if (maxlvt >= 3) {

                v = 0xFF; /* any non-zero vector will do */

                apic_write_around(APIC_LVTERR, v | APIC_LVT_MASKED);

        }

        /*

         * Careful: we have to set masks only first to deassert

         * any level-triggered sources.

         */

        v = apic_read(APIC_LVTT);

        apic_write_around(APIC_LVTT, v | APIC_LVT_MASKED);

        v = apic_read(APIC_LVT0);

        apic_write_around(APIC_LVT0, v | APIC_LVT_MASKED);

        v = apic_read(APIC_LVT1);

        apic_write_around(APIC_LVT1, v | APIC_LVT_MASKED);

        if (maxlvt >= 4) {

                v = apic_read(APIC_LVTPC);

                apic_write_around(APIC_LVTPC, v | APIC_LVT_MASKED);

        }

        /*

         * Clean APIC state for other OSs:

         */

        apic_write_around(APIC_LVTT, APIC_LVT_MASKED);

        apic_write_around(APIC_LVT0, APIC_LVT_MASKED);

        apic_write_around(APIC_LVT1, APIC_LVT_MASKED);

        if (maxlvt >= 3)

                apic_write_around(APIC_LVTERR, APIC_LVT_MASKED);

        if (maxlvt >= 4)

                apic_write_around(APIC_LVTPC, APIC_LVT_MASKED);

        v = GET_APIC_VERSION(apic_read(APIC_LVR));

        if (APIC_INTEGRATED(v)) {       /* !82489DX */

                if (maxlvt > 3)

                        apic_write(APIC_ESR, 0);

                apic_read(APIC_ESR);

        }

}

void disable_local_APIC(void)

{

        unsigned long value;

        clear_local_APIC();

        /*

         * Disable APIC (implies clearing of registers

         * for 82489DX!).

         */

        value = apic_read(APIC_SPIV);

        value &= ~APIC_SPIV_APIC_ENABLED;

        apic_write_around(APIC_SPIV, value);

}

#define SPURIOUS_APIC_VECTOR 0xFF

/*

 * An initial setup of the virtual wire mode.

 */

void setup_local_APIC (void)

{

        unsigned long value;

        /* Pound the ESR really hard over the head with a big hammer - mbligh */

        apic_write(APIC_ESR, 0);

        apic_write(APIC_ESR, 0);

        apic_write(APIC_ESR, 0);

        apic_write(APIC_ESR, 0);

        value = APIC_SPIV_FOCUS_DISABLED | APIC_SPIV_APIC_ENABLED | SPURIOUS_APIC_VECTOR;

        apic_write_around(APIC_SPIV, value);

        value = APIC_DM_EXTINT | APIC_LVT_LEVEL_TRIGGER;

        apic_write_around(APIC_LVT0, value);

        value = APIC_DM_NMI;

        apic_write_around(APIC_LVT1, value);

        apic_write(APIC_ESR, 0);

}

void disable_APIC_timer(void)

{

        unsigned long v;

        v = apic_read(APIC_LVTT);

        apic_write_around(APIC_LVTT, v | APIC_LVT_MASKED);

}

void enable_APIC_timer(void)

{

        unsigned long v;

        v = apic_read(APIC_LVTT);

        apic_write_around(APIC_LVTT, v & ~APIC_LVT_MASKED);

}

#define LOCAL_TIMER_VECTOR 0x66

void setup_APIC_LVTT(unsigned int clocks)

{

        unsigned int lvtt1_value, tmp_value;

        lvtt1_value = SET_APIC_TIMER_BASE(APIC_TIMER_BASE_DIV) |

                        APIC_LVT_TIMER_PERIODIC | LOCAL_TIMER_VECTOR;

        apic_write_around(APIC_LVTT, lvtt1_value);

        /*

         * Divide PICLK by 1

         */

        tmp_value = apic_read(APIC_TDCR);

        apic_write_around(APIC_TDCR, (tmp_value

                                & ~(APIC_TDR_DIV_1 | APIC_TDR_DIV_TMBASE))

                                | APIC_TDR_DIV_1);

        apic_write_around(APIC_TMICT, clocks);

}

#define APIC_LIMIT 0xFF000000

void ll_calibrate_apic(void)

{

  unsigned int apic_start = 0, apic_end = 0, dapic;

  signed long long tsc_start = 0, tsc_end = 0, dtsc;

  unsigned int tmp_value;

  cli();

  tmp_value = apic_read(APIC_TDCR);

  apic_write_around(APIC_TDCR, (tmp_value

                                 & ~(APIC_TDR_DIV_1 | APIC_TDR_DIV_TMBASE))

                                 | APIC_TDR_DIV_1);

  apic_write(APIC_TMICT, MAX_DWORD);

  barrier();

  rdtscll(tsc_start);

  barrier();

  apic_start = apic_read(APIC_TMCCT);

  barrier();            

  while (apic_read(APIC_TMCCT) > APIC_LIMIT) {

    barrier();

    rdtscll(tsc_end);

  }

  barrier();

  rdtscll(tsc_end);

  barrier();

  apic_end = apic_read(APIC_TMCCT);

  barrier();    

  sti();

  dtsc = tsc_end - tsc_start;

  dapic = apic_start - apic_end;

  apic_clk_per_msec = clk_per_msec * (signed long long)(dapic) / dtsc;

  message("Calibrated APIC Clk/msec = %10ld\n",(long)apic_clk_per_msec);

}

void ll_init_advtimer()

{

    if (use_tsc) {

        #ifdef CALIBRATE_USING_CMOS

          ll_calibrate_tsc_cmos();

        #else

          ll_calibrate_tsc();

        #endif  

        last_delta_clk_per_msec = 0;

        total_delta_clk_per_msec = 0;

        rdtscll(init_tsc); // Read start TSC

        init_nsec = 0;

        if (use_apic) {

          unsigned long msr_low_orig, tmp;

          rdmsr(APIC_BASE_MSR, msr_low_orig, tmp);

          wrmsr(APIC_BASE_MSR, msr_low_orig|(1<<11), 0);

          clear_local_APIC();

          ll_calibrate_apic();

          setup_local_APIC();

        }

        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_adv()

{

        if (use_cmos) {

                cli();

                irq_mask(8);

                CMOS_WRITE(0x0A,save_CMOS_regA);

                CMOS_WRITE(0x0B,save_CMOS_regB);

                sti();         

        }

        if (use_apic) {

                unsigned int msr_low_orig, tmp;

                cli();

                disable_APIC_timer();

                clear_local_APIC();

                rdmsr(APIC_BASE_MSR, msr_low_orig, tmp);

                wrmsr(APIC_BASE_MSR, msr_low_orig&~(1<<11), 0);

                sti();

        }

}