#include "stbdefs.h"
#include <stdio.h>

#define SEQREG   0x03C4
#define MISCREG  0x03C2
#define MISCREAD 0x03CC

double fref = 14.31818 * 2.0;
char ascclk[] = "VIDEO CLOCK ?";

unsigned short clknum;
unsigned short vlbus_flag;
unsigned short card;
unsigned short crtcaddr;
unsigned short clockreg;

double range[15] = {50.0, 51.0, 53.2, 58.5, 60.7, 64.4, 66.8, 73.5, 75.6, 80.9,
         83.2, 91.5, 100.0, 120.0, 120.0};

double genratio(unsigned int *p, unsigned int *q, double tgt);
double f(unsigned int p, unsigned int q, double basefreq);
void prtbinary(unsigned int size, unsigned int val);
void wait_vb();

main(argc, argv)
   int argc;
   char *argv[];
   {
   unsigned int m, mval, ival;
   int i;
   long dwv;
   double realval;
   double freq, fvco;
   double dev, devx;
   double delta, deltax;
   unsigned int p, q;
   unsigned int bestp, bestq;
   unsigned short clockval;

   if (argc < 2 || argc > 3) {
usage_msg:
      printf("Usage: SETCLK [clock number] freq\n\n");
      printf("    where \"freq\" is a floating point frequency to be apporximated\n");
      printf("    and \"clock number\" is the 2061A clock to be set to that frequency\n");
      printf("    \"clock number\" defaults to 3 (the system or memory clock for the\n");
      printf("    video card (high resolution video modes all use clock 2)\n");
      exit(1);
      }
   iopl(3);                     /* we need io access here! */
   crtcaddr=0x3d4;

    outb(0x3d4, 0x11);    /*       for register CR11, (Vertical Retrace End) */
   outb(0x3d5, 0x00);      /*      set to 0 */
   outb(0x3d4, 0x38);           /* for register CR38, (REG_LOCK1) */
   outb(0x3d5, 0x48);           /* unlock S3 register set for read/write */


   outb(0x3d4, 0x39);
   outb(0x3d5, 0xa5);

   outb(0x3d4, 0x30);
   m = inb(0x3d5);         /* get chip id */

   fprintf(stderr,"%x\n",m);

   if (m == 0x81) {
      printf("Video card is an S3 86C911 based WIND/X!\n");
      card = 924;
      }
   else if (m == 0x82) {
      printf("Video card is an S3 86C924 based WIND/X!\n");
      card = 924;
      }
   else if (m == 0xA0) {
      outb(crtcaddr, 0x36);
      if ((inb(crtcaddr+1) & 0x03) == 1) {
         printf("Video card is an S3 86C805 based Powergraph VL24!\n");
         vlbus_flag = 1;
         }
      else {
         printf("Video card is an S3 86C801 based Powergraph X24!\n");
         vlbus_flag = 0;
         }
      card = 801;
      }
   else if (m == 0x90) {
      printf("Video card is an S3 86C928 based card!\n");
      card = 928;
      }
   else {
      printf("Video card is not S3 86C9xx or 86C801/5 based, id %x!\n",m);
      exit(1);
      }

   clknum = 3;
   if (argc == 3) {
      clknum = atoi(argv[1]);
      argc--;
      argv++;
      }

   if (argv[1][0] > '9' || argv[1][0] < '0')
      goto usage_msg;

   freq = atof(argv[1]);
   if (freq > range[14])
      goto usage_msg;
   else if (freq <= 6.99)
      goto usage_msg;

/*
 *  Calculate values to load into ICD 2061A clock chip to set frequency
 */
   delta = 999.0;
   dev = 999.0;
   ival = 99;
   mval = 99;

   fvco = freq / 2;
   for (m = 0; m < 8; m++) {
      fvco *= 2.0;
      for (i = 14; i >= 0; i--)
         if (fvco >= range[i])
            break;
      if (i < 0)
         continue;
      if (i == 14)
         break;
      devx = (fvco - (range[i] + range[i+1])/2)/fvco;
      if (devx < 0)
         devx = -devx;
      deltax = genratio(&p, &q, fvco);
      if (delta < deltax)
         continue;
      if (deltax < delta || devx < dev) {
         bestp = p;
         bestq = q;
         delta = deltax;
         dev = devx;
         ival = i;
         mval = m;
         }
      }
   fvco = fref;
   for (m=0; m<mval; m++)
      fvco /= 2.0;
   realval = f(bestp, bestq, fvco);
   dwv = ((((((long)ival << 7) | bestp) << 3) | mval) << 7) | bestq;

/*
 * Write ICD 2061A clock chip
 */
   init_clock(((unsigned long)dwv) | (((long)clknum) << 21), crtcaddr);

   wait_vb();
   wait_vb();
   wait_vb();
   wait_vb();
   wait_vb();
   wait_vb();
   wait_vb();		/* 0.10 second delay... */

   ascclk[12] = clknum | '0';
   printf("Programmed %s = %f MHz (21-bit code word = ",
         clknum == 3? "MEMCLK": ascclk, realval);
   prtbinary(4, ival);
   putchar(' ');
   prtbinary(7, bestp);
   putchar(' ');
   prtbinary(3, mval);
   putchar(' ');
   prtbinary(7, bestq);
   putchar(')');
   putchar('\n');
   exit(0);
   }

double f(p, q, base)
   unsigned int p;
   unsigned int q;
   double base;
   {
   return(base * (p + 3)/(q + 2));
   }

double genratio(p, q, tgt)
   unsigned int *p;
   unsigned int *q;
   double tgt;
   {
   int k, m;
   double test, mindiff;
   unsigned int mmax;

   mindiff = 999999999.0;
   for (k = 13; k < 69; k++) {	       /* q={15..71}:Constraint 2 on page 14 */
      m = 50.0*k/fref - 3;
      if (m < 0)
         m = 0;
      mmax = 120*k/fref - 3;	       /* m..mmax is constraint 3 on page 14 */
      if (mmax > 128)
         mmax = 128;
      while (m < mmax) {
         test = f(m, k, fref) - tgt;
         if (test < 0) test = -test;
         if (mindiff > test) {
            mindiff = test;
            *p = m;
            *q = k;
            }
         m++;
         }
      }
   return (mindiff);
   }

void prtbinary(size, val)
   unsigned int size;
   unsigned int val;
   {
   unsigned int mask;
   int k;

   mask = 1;

   for (k=size; --k > 0 || mask <= val/2;)
      mask <<= 1;

   while (mask) {
      putchar((mask&val)? '1': '0');
      mask >>= 1;
      }
   }

void wait_vb()
   {
   while ((inp(crtcaddr+6) & 0x08) == 0)
      ;
   while (inp(crtcaddr+6) & 0x08)
      ;
   }
