Subversion Repositories shark

Rev

Rev 194 | Rev 238 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

/* 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; //Warp around 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;

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();
   
   CMOS_READ(0x0C,set);

   __asm__("rdtsc\n\t"
           "pushl %%eax\n\t"
           "pushl %%edx\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"
           "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" (ptr_clk_per_msec),
              "c" (0), "m" (Mconst), "a" (0), "d" (0));
       
   //Offset
   init_nsec += dn;

   if (init_step < 5) {
           init_step++;
           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;
   
   sti();
   
}

#define HZ 100

#ifdef CONFIG_MELAN
#  define CLOCK_TICK_RATE 1189200 /* AMD Elan has different frequency! */
#else
#  define CLOCK_TICK_RATE 1193180 /* Underlying HZ */
#endif

#define LATCH  ((CLOCK_TICK_RATE + HZ/2) / HZ)

#define CALIBRATE_LATCH (5 * LATCH)
#define CALIBRATE_TIME  (5 * 1000020/HZ)

#define COUNTER_END 50

//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();
       
        /* Set the Gate high, disable speaker */
        outp(0x61, (inp(0x61) & ~0x02) | 0x01);

        outp(0x43,0xB0);                        /* binary, mode 0, LSB/MSB, Ch 2 */
        outp(0x42,CALIBRATE_LATCH & 0xff);      /* LSB of count */
        outp(0x42,CALIBRATE_LATCH >> 8);        /* MSB of count */

        rdtscll(start);
        outp(0x43,0x00);
        start_8253 = inp(0x42);
        start_8253 |= inp(0x42) << 8;          

        do {
                               
            outp(0x43,0x00);
            end_8253 = inp(0x42);
            end_8253 |= inp(0x42) << 8;

        } while (end_8253 > COUNTER_END);
       
        rdtscll(end);
        outp(0x43,0x00);
        end_8253 = inp(0x42);
        end_8253 |= inp(0x42) << 8;

        //Delta TSC
        dtsc = end - start;

        //Delta PIT
        delta_8253 = start_8253 - end_8253 + 1;

        if (delta_8253 > 0xFFFF) {
                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 * CALIBRATE_LATCH * 1000 / delta_8253 / CALIBRATE_TIME;
       
        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__("rdtsc\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"
            "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" (ptr_clk_per_msec),
              "c" (0), "m" (Mconst), "m" (Gconst));
             
}

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