#include <stdlib.h>

double fref = 14.31818 * 2.0;

#ifdef ICD2061
double range[15] = {40, 42.5, 47.5, 53.5, 58.5, 62.5, 68.5, 76, 82, 87, 92,
         97, 105, 115, 125};
#else /* 2061A */
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};
#endif

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);

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;

   if (argc != 2) {
usage_msg:
      printf("Usage: GENCODE freq\n");
      printf("       where \"freq\" is the desired frequency in Megahertz\n");
      printf("      %6.3f < freq <%7.2f MHz\n", range[0]/128, range[14]);
      exit(1);
      }
   freq = atof(argv[1]);
   if (freq > range[14])
      goto usage_msg;
   printf ("To generate a clock of %8.2f MHz, use the following code words:\n",
         freq);

   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);
/*
      printf("Possibility: i=%u, m=%u, center dev=%f, freq delta=%f, ratio=%u/%u\n",
            i, m, devx, deltax, p+3, q+2);
*/
      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);
   printf("Target frequency =%f MHz, target ratio =%f (freq dev=%f)\n",
         freq, freq/fvco, delta);
   printf("Real frequency will be %f MHz, ", realval);
   printf("real ratio=%f (%f off center)\n", realval/fvco, dev);
   printf("21-bit code word = ");
   prtbinary(4, ival);
   putchar(' ');
   prtbinary(7, bestp);
   putchar(' ');
   prtbinary(3, mval);
   putchar(' ');
   prtbinary(7, bestq);
   printf("      (i=%d p=%d m=%d q=%d)\n", ival, bestp+3, mval, bestq+2);
   dwv = ((((((long)ival << 7) | bestp) << 3) | mval) << 7) | bestq;
   for(i=0; i<3; i++) {
      printf("For clocksel[%i] load %06lX\n", i, dwv | (((long)i) << 21));
      }
   printf("For master clk  load %06lX\n", dwv | 0x600000L);
   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;
      }
   }
