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