17706df26Smrg#ifdef HAVE_CONFIG_H 27706df26Smrg#include "config.h" 37706df26Smrg#endif 47706df26Smrg 57706df26Smrg#define BASE_FREQ 14.31818 67706df26Smrg#define MAX_FREQ 230000 77706df26Smrg 87706df26Smrg#define reorder(a) ( \ 97706df26Smrg (a & 0x80) >> 7 | \ 107706df26Smrg (a & 0x40) >> 5 | \ 117706df26Smrg (a & 0x20) >> 3 | \ 127706df26Smrg (a & 0x10) >> 1 | \ 137706df26Smrg (a & 0x08) << 1 | \ 147706df26Smrg (a & 0x04) << 3 | \ 157706df26Smrg (a & 0x02) << 5 | \ 167706df26Smrg (a & 0x01) << 7 ) 177706df26Smrg 187706df26Smrg#define CHECK_MIN(m,a,r,n) do { \ 197706df26Smrg diff = f - (BASE_FREQ * 1e3 * (n)) / (r << p); \ 207706df26Smrg if (diff < 0) diff = -diff; \ 217706df26Smrg if (diff < min_diff) { \ 227706df26Smrg min_diff = diff; \ 237706df26Smrg best_m = m; \ 247706df26Smrg best_a = a; \ 257706df26Smrg best_r = r; \ 267706df26Smrg } \ 277706df26Smrg } while(0) 287706df26Smrg 297706df26Smrg 307706df26Smrgstatic void Set_1562_PLL(int f, int p, int m, int a, int r, unsigned char *bits) 317706df26Smrg{ 327706df26Smrg bits[0] = 0x80; /* N1 = 4, not used for PLL */ 337706df26Smrg bits[1] = reorder(p) >> 4; 347706df26Smrg bits[2] = 0x00; /* N2 = 1 */ 357706df26Smrg if (f <= 120000) 367706df26Smrg bits[3] = 0x20; /* V = 100 */ 377706df26Smrg else if (f <= 200000) 387706df26Smrg bits[3] = 0xa0; /* V = 101 */ 397706df26Smrg else 407706df26Smrg bits[3] = 0x60; /* V = 110 */ 417706df26Smrg bits[3] |= 0x05; /* P = 10, phase detector on */ 427706df26Smrg bits[4] = reorder(m); 437706df26Smrg bits[5] = reorder(a); 447706df26Smrg bits[6] = reorder(r); 457706df26Smrg 467706df26Smrg#ifdef DEBUG 477706df26Smrg { 487706df26Smrg int i; 497706df26Smrg for(i=0; i<7; i++) 507706df26Smrg ErrorF("%02x ", bits[i]); 517706df26Smrg ErrorF("\n"); 527706df26Smrg } 537706df26Smrg#endif 547706df26Smrg} 557706df26Smrg 567706df26Smrgvoid ICS1562_CalcClockBits(long f, unsigned char *bits); 577706df26Smrg 587706df26Smrgvoid ICS1562_CalcClockBits(long f, unsigned char *bits) 597706df26Smrg{ 607706df26Smrg int n,r, a,m, p, r0,r1,n0,n1; 617706df26Smrg int best_m=34, best_a=1, best_r=30; 627706df26Smrg double diff, min_diff; 637706df26Smrg double ff, ffp; 647706df26Smrg 657706df26Smrg if (f > MAX_FREQ) 667706df26Smrg f = MAX_FREQ; 677706df26Smrg 687706df26Smrg if (f >= MAX_FREQ/2) 697706df26Smrg p=0; 707706df26Smrg else if (f >= MAX_FREQ/4) 717706df26Smrg p=1; 727706df26Smrg else 737706df26Smrg p=2; 747706df26Smrg 757706df26Smrg ff = f / 1e3 / BASE_FREQ; 767706df26Smrg ffp = ff * (1 << p); 777706df26Smrg min_diff = 999999999; 787706df26Smrg 797706df26Smrg r0 = (int)(7/ffp); 807706df26Smrg if (r0 < 1) r0 = 1; 817706df26Smrg r1 = (int)(449/ffp); 827706df26Smrg if (r1 > 0x7f+1) r1 = 0x7f+1; 837706df26Smrg if (r1 < r0) r1 = r0; 847706df26Smrg 857706df26Smrg for (r=r0; r<r1; r++) { 867706df26Smrg n0 = (int)(ffp * r); 877706df26Smrg if (n0 < 7) n0 = 7; 887706df26Smrg n1 = (int)(ffp * (r+1)); 897706df26Smrg if (n1 > 448) n1 = 448; 907706df26Smrg for (n=n0; n<n1; n++) { 917706df26Smrg m = ((n+3)/7) - 1; 927706df26Smrg if (m <= 0x3f) CHECK_MIN(m, 0, r, (m+1)*7); 937706df26Smrg if (++m <= 0x3f) CHECK_MIN(m, 0, r, (m+1)*7); 947706df26Smrg m = (n/6) - 1; 957706df26Smrg a = n - (m+1)*6; 967706df26Smrg if (m <= 0x3f && a > 0 && a<=7) CHECK_MIN(m, a, r, (m+1)*6+a); 977706df26Smrg m++; 987706df26Smrg a = n - (m+1)*6; 997706df26Smrg if (m <= 0x3f && a > 0 && a<=7) CHECK_MIN(m, a, r, (m+1)*6+a); 1007706df26Smrg } 1017706df26Smrg } 1027706df26Smrg 1037706df26Smrg if (best_a) n = (best_m+1)*6+best_a; 1047706df26Smrg else n = (best_m+1)*7; 1057706df26Smrg 1067706df26Smrg#ifdef DEBUG 1077706df26Smrg ErrorF("%8.3f %f %f p=%d m=%d a=%d r=%d %d/%d\n" 1087706df26Smrg ,f/1e3 1097706df26Smrg ,min_diff,(BASE_FREQ * (n)) / (best_r << p) 1107706df26Smrg ,p ,best_m, best_a, best_r-1 1117706df26Smrg ,n,best_r 1127706df26Smrg ); 1137706df26Smrg#endif 1147706df26Smrg 1157706df26Smrg Set_1562_PLL(f, p, best_m, best_a, best_r-1, bits); 1167706df26Smrg} 117