
#include "stbdefs.h"   /* RLM */

void wrt_clk_bit(unsigned int value);

init_clock(setup, crtcport)
   unsigned long setup;
   unsigned short crtcport;
   {
   unsigned char nclk[2], clk[2];
   unsigned short restore42;
   unsigned short oldclk;
   unsigned short bitval;
   int i;
   unsigned char c;

   oldclk = inp(0x3CC);

   outb(crtcport, 0x42);
   restore42 = (inb(crtcport+1) << 8) & 0x3F00;
   restore42 |= 0x42;

   outb(0x3C4, 0x00);
   outb(0x3C4, 0x01);

   outb(0x3C4, 1);
   c= inb(0x3C5);
   outb(0x3C5, 0x20 | c);

   outb(crtcport, 0x42);
   outb(crtcport+1, 0x03);

   outb(0x3C4, 0x00);
   outb(0x3C4, 0x03);

   nclk[0] = oldclk & 0xF3;
   nclk[1] = nclk[0] | 0x08;
   clk[0] = nclk[0] | 0x04;
   clk[1] = nclk[0] | 0x0C;

   outb(crtcport, 0x42);
   i = inpw(crtcport);

   outb(0x3C4, 0x00);
   outb(0x3C4, 0x01);

   wrt_clk_bit(oldclk | 0x08);
   wrt_clk_bit(oldclk | 0x0C);
   for (i=0; i<5; i++) {
      wrt_clk_bit(nclk[1]);
      wrt_clk_bit(clk[1]);
      }
   wrt_clk_bit(nclk[1]);
   wrt_clk_bit(nclk[0]);
   wrt_clk_bit(clk[0]);
   wrt_clk_bit(nclk[0]);
   wrt_clk_bit(clk[0]);
   for (i=0; i<24; i++) {
      bitval = setup & 0x01;
      setup >>= 1;
      wrt_clk_bit(clk[1-bitval]);
      wrt_clk_bit(nclk[1-bitval]);
      wrt_clk_bit(nclk[bitval]);
      wrt_clk_bit(clk[bitval]);
      }
   wrt_clk_bit(clk[1]);
   wrt_clk_bit(nclk[1]);
   wrt_clk_bit(clk[1]);

   outb(0x3C4, 1);
   outb(0x3C5, 0xDF & inp(0x3C5));

   outb(crtcport, 0x42);
   outb(crtcport+1, restore42);

   outb(0x3C2, oldclk);

   outb(0x3C4, 0x00);
   outb(0x3C4, 0x03);
   }

void wrt_clk_bit(value)
   unsigned int value;
   {
   int j;

   outb(0x3C2, value);
   for (j=2; --j; )
      inb(0x200);
   }
