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