#include <u.h>
#include <libc.h>
#include <bio.h>

#include "pci.h"
#include "vga.h"

#define SCALE(f)        ((f)/10)                /* could be /10 */

static void
init(Vga* vga, Ctlr* ctlr)
{
       int f, k;
       ulong fmin, fvco, m, n, p, q;
       double z;

       if(ctlr->flag & Finit)
               return;

       if(vga->f[0] == 0)
               vga->f[0] = vga->mode->frequency;
       vga->misc &= ~0x0C;
       if(vga->f[0] == VgaFreq0){
               /* nothing to do */;
       }
       else if(vga->f[0] == VgaFreq1)
               vga->misc |= 0x04;
       else
               vga->misc |= 0x0C;

       /*
        * Look for values of n, d and p that give
        * the least error for
        *      Fvco = 8*RefFreq*(65-m)/(65-n)
        *      Fpll = Fvco/2**p
        * N and m are 6 bits, p is 2 bits. Constraints:
        *      110MHz <= Fvco <= 250MHz
        *      40 <= n <= 62
        *       1 <= m <= 62
        *       0 <= p <=  3
        * Should try to minimise n, m.
        *
        * There's nothing like brute force and ignorance.
        */
       fmin = vga->f[0];
       vga->m[0] = 0x15;
       vga->n[0] = 0x18;
       vga->p[0] = 3;
       for(m = 62; m > 0; m--){
               for(n = 62; n >= 40; n--){
                       fvco = 8*SCALE(RefFreq)*(65-m)/(65-n);
                       if(fvco < SCALE(110000000) || fvco > SCALE(250000000))
                               continue;
                       for(p = 0; p < 4; p++){
                               f = SCALE(vga->f[0]) - (fvco>>p);
                               if(f < 0)
                                       f = -f;
                               if(f < fmin){
                                       fmin = f;
                                       vga->m[0] = m;
                                       vga->n[0] = n;
                                       vga->p[0] = p;
                               }
                       }
               }
       }

       /*
        * Now the loop clock:
        *      m is fixed;
        *      calculate n;
        *      set z to the lower bound (110MHz) and calculate p and q.
        */
       vga->m[1] = 61;
       if(ctlr->flag & Uenhanced)
               k = 64/8;
       else
               k = 8/8;
       n = 65 - 4*k;
       fvco = (8*RefFreq*(65-vga->m[0]))/(65-vga->n[0]);
       vga->f[1] = fvco;
       z = 110.0*(65-n)/(4*(fvco/1000000.0)*k);
       if(z <= 16){
               for(p = 0; p < 4; p++){
                       if(1<<(p+1) > z)
                               break;
               }
               q = 0;
       }
       else{
               p = 3;
               q = (z - 16)/16 + 1;
       }
       vga->n[1] = n;
       vga->p[1] = p;
       vga->q[1] = q;

       ctlr->flag |= Finit;
}

Ctlr tvp3026clock = {
       "tvp3026clock",                 /* name */
       0,                              /* snarf */
       0,                              /* options */
       init,                           /* init */
       0,                              /* load */
       0,                              /* dump */
};