usr.sbin/pcvt/set2061/ICD2061Aalt.c

   1 /*
   2  * This code is derived from code available from the STB bulletin board
   3  *
   4  * $FreeBSD: src/usr.sbin/pcvt/set2061/ICD2061Aalt.c,v 1.5 1999/12/29 05:07:54 peter Exp $
   5  */
   6
   7 /* $XFree86: mit/server/ddx/x386/common_hw/ICD2061Aalt.c,v 2.6 1994/04/15 05:10:30 dawes Exp $ */
   8
   9 #ifndef _KERNEL
  10 #include "compiler.h"
  11 #else
  12 #define GCCUSESGAS
  13 #define PCVT_STANDALONE 1
  14 #endif
  15
  16 #define SEQREG   0x03C4
  17 #define MISCREG  0x03C2
  18 #define MISCREAD 0x03CC
  19
  20 double fref = 14.31818 * 2.0;
  21 char ascclk[] = "VIDEO CLOCK ?";
  22
  23 unsigned short clknum;
  24 unsigned short vlbus_flag;
  25 unsigned short card;
  26 unsigned short crtcaddr;
  27 unsigned short clockreg;
  28
  29 static double range[15] = {50.0, 51.0, 53.2, 58.5, 60.7, 64.4, 66.8, 73.5,
  30                            75.6, 80.9, 83.2, 91.5, 100.0, 120.0, 120.0};
  31
  32 #ifdef __STDC__
  33 static double genratio(unsigned int *p, unsigned int *q, double tgt);
  34 static double f(unsigned int p, unsigned int q, double basefreq);
  35 #if 0
  36 static void prtbinary(unsigned int size, unsigned int val);
  37 #endif
  38 static void wait_vb();
  39 static void wrt_clk_bit(unsigned int value);
  40 static void init_clock(unsigned long setup, unsigned short crtcport);
  41 #else
  42 static double genratio();
  43 static double f();
  44 #if 0
  45 static void prtbinary();
  46 #endif
  47 static void wait_vb();
  48 static void wrt_clk_bit();
  49 static void init_clock();
  50 #endif
  51
  52 void AltICD2061SetClock(frequency, select)
  53 register long   frequency;               /* in Hz */
  54 int select;
  55 {
  56    unsigned int m, mval, ival;
  57    int i;
  58    long dwv;
  59    double realval;
  60    double freq, fvco;
  61    double dev, devx;
  62    double delta, deltax;
  63    unsigned int p, q;
  64    unsigned int bestp, bestq;
  65    unsigned char tmp;
  66
  67    crtcaddr=(inb(0x3CC) & 0x01) ? 0x3D4 : 0x3B4;
  68
  69
  70    outb(crtcaddr, 0x11);        /* Unlock CRTC registers */
  71    tmp = inb(crtcaddr + 1);
  72    outb(crtcaddr + 1, tmp & ~0x80);
  73
  74    outw(crtcaddr, 0x4838);      /* Unlock S3 register set */
  75    outw(crtcaddr, 0xA039);
  76
  77    clknum = select;
  78
  79    freq = ((double)frequency)/1000000.0;
  80    if (freq > range[14])
  81         freq =range[14];
  82    else if (freq <= 6.99)
  83       freq = 7.0;
  84
  85 /*
  86  *  Calculate values to load into ICD 2061A clock chip to set frequency
  87  */
  88    delta = 999.0;
  89    dev = 999.0;
  90    ival = 99;
  91    mval = 99;
  92
  93    fvco = freq / 2;
  94    for (m = 0; m < 8; m++) {
  95       fvco *= 2.0;
  96       for (i = 14; i >= 0; i--)
  97          if (fvco >= range[i])
  98             break;
  99       if (i < 0)
 100          continue;
 101       if (i == 14)
 102          break;
 103       devx = (fvco - (range[i] + range[i+1])/2)/fvco;
 104       if (devx < 0)
 105          devx = -devx;
 106       deltax = genratio(&p, &q, fvco);
 107       if (delta < deltax)
 108          continue;
 109       if (deltax < delta || devx < dev) {
 110          bestp = p;
 111          bestq = q;
 112          delta = deltax;
 113          dev = devx;
 114          ival = i;
 115          mval = m;
 116          }
 117       }
 118    fvco = fref;
 119    for (m=0; m<mval; m++)
 120       fvco /= 2.0;
 121    realval = f(bestp, bestq, fvco);
 122    dwv = ((((((long)ival << 7) | bestp) << 3) | mval) << 7) | bestq;
 123
 124 /*
 125  * Write ICD 2061A clock chip
 126  */
 127    init_clock(((unsigned long)dwv) | (((long)clknum) << 21), crtcaddr);
 128
 129    wait_vb();
 130    wait_vb();
 131    wait_vb();
 132    wait_vb();
 133    wait_vb();
 134    wait_vb();
 135    wait_vb();           /* 0.10 second delay... */
 136 }
 137
 138 static double f(p, q, base)
 139    unsigned int p;
 140    unsigned int q;
 141    double base;
 142    {
 143    return(base * (p + 3)/(q + 2));
 144    }
 145
 146 static double genratio(p, q, tgt)
 147    unsigned int *p;
 148    unsigned int *q;
 149    double tgt;
 150    {
 151    int k, m;
 152    double test, mindiff;
 153    unsigned int mmax;
 154
 155    mindiff = 999999999.0;
 156    for (k = 13; k < 69; k++) {         /* q={15..71}:Constraint 2 on page 14 */
 157       m = 50.0*k/fref - 3;
 158       if (m < 0)
 159          m = 0;
 160       mmax = 120*k/fref - 3;           /* m..mmax is constraint 3 on page 14 */
 161       if (mmax > 128)
 162          mmax = 128;
 163       while (m < mmax) {
 164          test = f(m, k, fref) - tgt;
 165          if (test < 0) test = -test;
 166          if (mindiff > test) {
 167             mindiff = test;
 168             *p = m;
 169             *q = k;
 170             }
 171          m++;
 172          }
 173       }
 174    return (mindiff);
 175    }
 176
 177 #if 0
 178 static void prtbinary(size, val)
 179    unsigned int size;
 180    unsigned int val;
 181    {
 182    unsigned int mask;
 183    int k;
 184
 185    mask = 1;
 186
 187    for (k=size; --k > 0 || mask <= val/2;)
 188       mask <<= 1;
 189
 190    while (mask) {
 191       fputc((mask&val)? '1': '0' , stderr);
 192       mask >>= 1;
 193       }
 194    }
 195 #endif
 196
 197 static void wait_vb()
 198    {
 199    while ((inb(crtcaddr+6) & 0x08) == 0)
 200       ;
 201    while (inb(crtcaddr+6) & 0x08)
 202       ;
 203    }
 204
 205
 206 #ifdef __STDC__
 207 static void init_clock(unsigned long setup, unsigned short crtcport)
 208 #else
 209 static void init_clock(setup, crtcport)
 210    unsigned long setup;
 211    unsigned short crtcport;
 212 #endif
 213    {
 214    unsigned char nclk[2], clk[2];
 215    unsigned short restore42;
 216    unsigned short oldclk;
 217    unsigned short bitval;
 218    int i;
 219    unsigned char c;
 220
 221 #ifndef PCVT_STANDALONE
 222    (void)xf86DisableInterrupts();
 223 #endif
 224
 225    oldclk = inb(0x3CC);
 226
 227    outb(crtcport, 0x42);
 228    restore42 = inb(crtcport+1);
 229
 230    outw(0x3C4, 0x0100);
 231
 232    outb(0x3C4, 1);
 233    c = inb(0x3C5);
 234    outb(0x3C5, 0x20 | c);
 235
 236    outb(crtcport, 0x42);
 237    outb(crtcport+1, 0x03);
 238
 239    outw(0x3C4, 0x0300);
 240
 241    nclk[0] = oldclk & 0xF3;
 242    nclk[1] = nclk[0] | 0x08;
 243    clk[0] = nclk[0] | 0x04;
 244    clk[1] = nclk[0] | 0x0C;
 245
 246    outb(crtcport, 0x42);
 247    i = inw(crtcport);
 248
 249    outw(0x3C4, 0x0100);
 250
 251    wrt_clk_bit(oldclk | 0x08);
 252    wrt_clk_bit(oldclk | 0x0C);
 253    for (i=0; i<5; i++) {
 254       wrt_clk_bit(nclk[1]);
 255       wrt_clk_bit(clk[1]);
 256       }
 257    wrt_clk_bit(nclk[1]);
 258    wrt_clk_bit(nclk[0]);
 259    wrt_clk_bit(clk[0]);
 260    wrt_clk_bit(nclk[0]);
 261    wrt_clk_bit(clk[0]);
 262    for (i=0; i<24; i++) {
 263       bitval = setup & 0x01;
 264       setup >>= 1;
 265       wrt_clk_bit(clk[1-bitval]);
 266       wrt_clk_bit(nclk[1-bitval]);
 267       wrt_clk_bit(nclk[bitval]);
 268       wrt_clk_bit(clk[bitval]);
 269       }
 270    wrt_clk_bit(clk[1]);
 271    wrt_clk_bit(nclk[1]);
 272    wrt_clk_bit(clk[1]);
 273
 274    outb(0x3C4, 1);
 275    c = inb(0x3C5);
 276    outb(0x3C5, 0xDF & c);
 277
 278    outb(crtcport, 0x42);
 279    outb(crtcport+1, restore42);
 280
 281    outb(0x3C2, oldclk);
 282
 283    outw(0x3C4, 0x0300);
 284
 285 #ifndef PCVT_STANDALONE
 286    xf86EnableInterrupts();
 287 #endif
 288
 289    }
 290
 291 static void wrt_clk_bit(value)
 292    unsigned int value;
 293    {
 294    int j;
 295
 296    outb(0x3C2, value);
 297    for (j=2; --j; )
 298       inb(0x200);
 299    }