1#ifdef HAVE_CONFIG_H
2#include "config.h"
3#endif
4
5#define BASE_FREQ 14.31818
6#define MAX_FREQ 230000
7
8#define	reorder(a)	( \
9	(a & 0x80) >> 7 | \
10	(a & 0x40) >> 5 | \
11	(a & 0x20) >> 3 | \
12	(a & 0x10) >> 1 | \
13	(a & 0x08) << 1 | \
14	(a & 0x04) << 3 | \
15	(a & 0x02) << 5 | \
16	(a & 0x01) << 7 )
17
18#define CHECK_MIN(m,a,r,n)  do { \
19      diff = f - (BASE_FREQ * 1e3 * (n)) / (r << p); \
20      if (diff < 0) diff = -diff; \
21      if (diff < min_diff) {  \
22	min_diff = diff; \
23	best_m = m; \
24	best_a = a; \
25	best_r = r; \
26      } \
27    } while(0)
28
29
30static void Set_1562_PLL(int f, int p, int m, int a, int r, unsigned char *bits)
31{
32  bits[0] = 0x80;  /* N1 = 4, not used for PLL */
33  bits[1] = reorder(p) >> 4;
34  bits[2] = 0x00;  /* N2 = 1 */
35  if (f <= 120000)
36    bits[3] = 0x20;  /* V = 100 */
37  else if (f <= 200000)
38    bits[3] = 0xa0;  /* V = 101 */
39  else
40    bits[3] = 0x60;  /* V = 110 */
41  bits[3] |= 0x05;   /* P = 10, phase detector on */
42  bits[4] = reorder(m);
43  bits[5] = reorder(a);
44  bits[6] = reorder(r);
45
46#ifdef DEBUG
47  {
48    int i;
49    for(i=0; i<7; i++)
50      ErrorF("%02x ", bits[i]);
51    ErrorF("\n");
52  }
53#endif
54}
55
56void ICS1562_CalcClockBits(long f, unsigned char *bits);
57
58void ICS1562_CalcClockBits(long f, unsigned char *bits)
59{
60  int n,r, a,m, p, r0,r1,n0,n1;
61  int best_m=34, best_a=1, best_r=30;
62  double diff, min_diff;
63  double ff, ffp;
64
65  if (f > MAX_FREQ)
66    f = MAX_FREQ;
67
68  if (f >= MAX_FREQ/2)
69    p=0;
70  else if (f >= MAX_FREQ/4)
71    p=1;
72  else
73    p=2;
74
75  ff  = f / 1e3 / BASE_FREQ;
76  ffp = ff * (1 << p);
77  min_diff = 999999999;
78
79  r0 = (int)(7/ffp);
80  if (r0 < 1) r0 = 1;
81  r1 = (int)(449/ffp);
82  if (r1 > 0x7f+1) r1 = 0x7f+1;
83  if (r1 < r0) r1 = r0;
84
85  for (r=r0; r<r1; r++) {
86    n0 = (int)(ffp * r);
87    if (n0 < 7) n0 = 7;
88    n1 = (int)(ffp * (r+1));
89    if (n1 > 448) n1 = 448;
90    for (n=n0; n<n1; n++) {
91      m = ((n+3)/7) - 1;
92      if (m   <= 0x3f) CHECK_MIN(m, 0, r, (m+1)*7);
93      if (++m <= 0x3f) CHECK_MIN(m, 0, r, (m+1)*7);
94      m = (n/6) - 1;
95      a = n - (m+1)*6;
96      if (m <= 0x3f && a > 0 && a<=7) CHECK_MIN(m, a, r, (m+1)*6+a);
97      m++;
98      a = n - (m+1)*6;
99      if (m <= 0x3f && a > 0 && a<=7) CHECK_MIN(m, a, r, (m+1)*6+a);
100    }
101  }
102
103  if (best_a) n = (best_m+1)*6+best_a;
104  else n = (best_m+1)*7;
105
106#ifdef DEBUG
107  ErrorF("%8.3f %f %f  p=%d m=%d a=%d r=%d  %d/%d\n"
108	 ,f/1e3
109	 ,min_diff,(BASE_FREQ * (n)) / (best_r << p)
110	 ,p ,best_m, best_a, best_r-1
111	 ,n,best_r
112	 );
113#endif
114
115  Set_1562_PLL(f, p, best_m, best_a, best_r-1, bits);
116}
117