1#include <unistd.h>
2#include <stdio.h>
3#include <stdlib.h>
4
5#include <fnmatch.h>
6
7#include "iopl.h"
8
9#define tolerance 0.01 /* +/- 1% */
10
11
12#define CT65520 0x1
13#define CT65525 0x2
14#define CT65530 0x3
15#define CT64200 0x4
16
17#define CT65535 0x11
18#define CT65540 0x12
19#define CT65545 0x13
20#define CT65546 0x14
21#define CT65548 0x15
22#define CT64300 0x16
23
24#define CT65550 0x31
25#define CT65554 0x32
26#define CT65555 0x33
27#define CT68554 0x34
28#define CT69000 0x35
29#define CT69030 0x36
30
31#define IS_Programmable(X) X&0x10
32#define IS_HiQV(X) X&0x20
33
34#define DotClk 0
35#define MemClk 1
36#define IS_MemClk(X) X&0x1
37
38static int compute_clock (
39		   unsigned int ChipType,
40		   double target,
41		   double Fref,
42		   unsigned int ClkMaxN,
43		   unsigned int ClkMaxM,
44		   unsigned int *bestM,
45		   unsigned int *bestN,
46		   unsigned int *bestP,
47		   unsigned int *bestPSN) {
48
49  unsigned int M, N, P, PSN, PSNx;
50
51  double bestError = 0, abest = 42, bestFout = 0;
52
53  double Fvco, Fout;
54  double error, aerror;
55
56  unsigned int M_min = 3;
57  unsigned int M_max = ClkMaxM;
58
59  if (target < 1e6){
60    fprintf (stderr, "MHz assumed, changed to %g MHz\n", target);
61    target *= 1e6;
62  }
63
64  if (target > 220.0e6) {
65    fprintf (stderr, "too large\n");
66    return 1;
67  }
68
69  /* Other parameters available on the 65548 but not the 65545, and
70     not documented in the Clock Synthesizer doc in rev 1.0 of the
71     65548 datasheet:
72
73     + XR30[4] = 0, VCO divider loop uses divide by 4 (same as 65545)
74		 1, VCO divider loop uses divide by 16
75
76     + XR30[5] = 1, reference clock is divided by 5
77
78     I haven't put in any support for those here.  For simplicity,
79     they should be set to 0 on the 65548, and left untouched on
80     earlier chips.  */
81
82  for (PSNx = ((ChipType == CT69000) || (ChipType == CT69030)) ? 1 : 0;
83       PSNx <= 1; PSNx++) {
84    unsigned int low_N, high_N;
85    double Fref4PSN;
86
87    PSN = PSNx ? 1 : 4;
88
89    low_N = 3;
90    high_N = ClkMaxN;
91
92    while (Fref / (PSN * low_N) > (((ChipType == CT69000) ||
93				    (ChipType == CT69030)) ? 5.0e6 : 2.0e6))
94      low_N++;
95    while (Fref / (PSN * high_N) < 150.0e3)
96      high_N--;
97
98    Fref4PSN = Fref * 4 / PSN;
99    for (N = low_N; N <= high_N; N++) {
100      double tmp = Fref4PSN / N;
101
102      for (P = (IS_HiQV(ChipType) && (ChipType != CT69000) &&
103		(ChipType != CT69030)) ? 1 : 0; P <= 5; P++) {
104	double Fvco_desired = target * (1 << P);
105	double M_desired = Fvco_desired / tmp;
106	/* Which way will M_desired be rounded?  Do all three just to
107	   be safe.  */
108	unsigned int M_low = M_desired - 1;
109	unsigned int M_hi = M_desired + 1;
110
111	if (M_hi < M_min || M_low > M_max)
112	  continue;
113
114	if (M_low < M_min)
115	  M_low = M_min;
116	if (M_hi > M_max)
117	  M_hi = M_max;
118
119	for (M = M_low; M <= M_hi; M++) {
120	  Fvco = tmp * M;
121	  if (Fvco <= ((ChipType == CT69000) || (ChipType == CT69030) ?
122		       100e6 : 48.0e6))
123	    continue;
124	  if (Fvco > 220.0e6)
125	    break;
126
127	  Fout = Fvco / (1 << P);
128
129	  error = (target - Fout) / target;
130
131	  aerror = (error < 0) ? -error : error;
132	  if (aerror < abest) {
133	    abest = aerror;
134	    bestError = error;
135	    *bestM = M;
136	    *bestN = N;
137	    *bestP = P;
138	    *bestPSN = PSN;
139	    bestFout = Fout;
140	  }
141	}
142      }
143    }
144  }
145
146  if (abest < tolerance) {
147    printf ("best: M=%d N=%d P=%d PSN=%d\n", *bestM, *bestN, *bestP, *bestPSN);
148
149    if (bestFout > 1.0e6)
150      printf ("Fout = %g MHz", bestFout / 1.0e6);
151    else if (bestFout > 1.0e3)
152      printf ("Fout = %g kHz", bestFout / 1.0e3);
153    else
154      printf ("Fout = %g Hz", bestFout);
155    printf (", error = %g\n", bestError);
156    return 0;
157  }
158  printf ("can't do it with less than %g error\n", bestError);
159  return 1;
160}
161
162static int set_clock(
163	      unsigned int ChipType,
164	      unsigned int ClockType,
165	      unsigned int ProgClock,
166	      unsigned int M,
167	      unsigned int N,
168	      unsigned int P,
169	      unsigned int PSN) {
170
171  unsigned int tmp, idx;
172
173  SET_IOPL();
174
175  idx = inb(0x3D6);
176  if (IS_HiQV(ChipType)) {
177    if (IS_MemClk(ClockType)) {
178      printf ("XRCC = 0x%02X\n", M - 2);
179      printf ("XRCD = 0x%02X\n", N - 2);
180      printf ("XRCE = 0x%02X\n", (0x80 | (P * 16 + (PSN == 1))));
181
182      outb(0x3D6, 0xCE); /* Select Fix MClk before */
183      tmp = inb(0x3D7);
184      outb(0x3D7, tmp & 0x7F);
185      outb(0x3D6, 0xCC);
186      outb(0x3D7, (M - 2));
187      outb(0x3D6, 0xCD);
188      outb(0x3D7, (N - 2));
189      outb(0x3D6, 0xCE);
190      outb(0x3D7, (0x80 | (P * 16 + (PSN == 1))));
191    } else {
192      printf ("XR%X = 0x%02X\n", 0xC0 + 4 * ProgClock, M - 2);
193      printf ("XR%X = 0x%02X\n",  0xC1 + 4 * ProgClock, N - 2);
194      printf ("XR%X = 0x%02X\n",  0xC2 + 4 * ProgClock, 0);
195      printf ("XR%X = 0x%02X\n",  0xC3 + 4 * ProgClock, P * 16 + (PSN == 1));
196
197      outb(0x3D6, 0xC0 + 4 * ProgClock);
198      outb(0x3D7, (M - 2));
199      outb(0x3D6, 0xC1 + 4 * ProgClock);
200      outb(0x3D7, (N - 2));
201      outb(0x3D6, 0xC2 + 4 * ProgClock);
202      outb(0x3D7, 0x0);
203      outb(0x3D6, 0xC3 + 4 * ProgClock);
204      outb(0x3D7, (P * 16 + (PSN == 1)));
205    }
206   } else {
207    printf ("XR30 = 0x%02X\n", P * 2 + (PSN == 1));
208    printf ("XR31 = 0x%02X\n", M - 2);
209    printf ("XR32 = 0x%02X\n", N - 2);
210    outb(0x3D6, 0x33);
211    tmp = inb(0x3D7);
212    if (IS_MemClk(ClockType)) {
213      outb(0x3D7, tmp | 0x20);
214    } else {
215      outb(0x3D7, tmp & ~0x20);
216    }
217    outb(0x3D6, 0x30);
218    outb(0x3D7, (P * 2 + (PSN == 1)));
219    outb(0x3D6, 0x31);
220    outb(0x3D7, (M - 2));
221    outb(0x3D6, 0x32);
222    outb(0x3D7, (N - 2));
223    outb(0x3D6, 0x33);
224    outb(0x3D7, tmp);
225  }
226  outb(0x3D6, idx);
227  RESET_IOPL();
228  return 0;
229}
230
231static unsigned int probe_chip(void) {
232
233  unsigned int ChipType, temp;
234
235  SET_IOPL();
236
237  outb(0x3D6, 0x00);
238  temp = inb(0x3D7);
239  ChipType = 0;
240  if (temp != 0xA5) {
241    if ((temp & 0xF0) == 0x70) {
242      ChipType = CT65520;
243    }
244    if ((temp & 0xF0) == 0x80) {  /* could also be a 65525 */
245      ChipType = CT65530;
246    }
247    if ((temp & 0xF0) == 0xA0) {
248      ChipType = CT64200;
249    }
250    if ((temp & 0xF0) == 0xB0) {
251      ChipType = CT64300;
252    }
253    if ((temp & 0xF0) == 0xC0) {
254      ChipType = CT65535;
255    }
256    if ((temp & 0xF8) == 0xD0) {
257      ChipType = CT65540;
258    }
259    if ((temp & 0xF8) == 0xD8) {
260      switch (temp & 0x07) {
261      case 3:
262	ChipType = CT65546;
263	break;
264      case 4:
265	ChipType = CT65548;
266	break;
267      default:
268	ChipType = CT65545;
269      }
270    }
271  }
272  /* At this point the chip could still be a HiQV, so check for
273   * that. This test needs some looking at */
274  if ((temp != 0) && (ChipType == 0)) {
275    outb(0x3D6, 0x02);
276    temp = inb(0x03D7);
277    if (temp == 0xE0) {
278      ChipType = CT65550;
279    }
280    if (temp == 0xE4) {
281      ChipType = CT65554;
282    }
283    if (temp == 0xE5) {
284      ChipType = CT65555;
285    }
286    if (temp == 0xF4) {
287      ChipType = CT68554;
288    }
289    if (temp == 0xC0) {
290      ChipType = CT69000;
291    }
292    if (temp == 0x30) {
293      outb(0x3D6, 0x03);
294      temp = inb(0x03D7);
295      if (temp == 0x0C) ChipType = CT69030;
296    }
297  }
298
299  RESET_IOPL();
300
301  if (ChipType == 0) {      /* failure */
302    fprintf(stderr, "Not a Chips and Technologies Chipset\n");
303  }
304
305  return ChipType;
306}
307
308
309int main (int argc, char *argv[]) {
310  double target;
311  double Fref = 14318180;
312  unsigned int M, N, P, PSN, ChipType, ClockType, progclock;
313
314  switch (argc) {
315  case 2:
316    progclock = 2;
317    target = atof (argv[1]);
318    break;
319  case 3:
320    progclock = abs(atof (argv[1]));
321    target = atof (argv[2]);
322    break;
323  default:
324    fprintf (stderr, "usage: %s [-0|-1|-2] freq\n", argv[0]);
325    return 1;
326  }
327
328  ClockType = DotClk;
329  if (! fnmatch("*memClock",argv[0],FNM_PATHNAME)) {
330    ClockType = MemClk;
331  }
332
333  ChipType = probe_chip();
334  if (!ChipType) {
335    return 1;
336  }
337
338  if (!(IS_Programmable(ChipType))) {
339    fprintf(stderr, "No programmable Clock!\n");
340    return 1;
341  }
342
343  if (IS_HiQV(ChipType)) {
344    if (! compute_clock(ChipType, target, Fref, 63, 127, &M, &N, &P, &PSN)) {
345      return set_clock(ChipType, ClockType, progclock, M, N, P, PSN);
346    } else {
347      return 1;
348    }
349  } else {
350    if (! compute_clock(ChipType, target, Fref, 127, 127, &M, &N, &P, &PSN)) {
351      return set_clock(ChipType, ClockType, progclock, M, N, P, PSN);
352    } else {
353      return 1;
354    }
355  }
356}
357