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