softpll_ng.c 17.3 KB
Newer Older
1 2 3 4
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

5
#include <wrc.h>
6 7 8 9 10 11 12 13 14 15 16
#include "board.h"
#include "trace.h"
#include "hw/softpll_regs.h"
#include "hw/pps_gen_regs.h"

#include "softpll_ng.h"

#include "irq.h"

volatile int irq_count = 0;

17 18 19 20
volatile struct SPLL_WB *SPLL;
volatile struct PPSG_WB *PPSG;

int spll_n_chan_ref, spll_n_chan_out;
21

22 23 24 25
#if defined(CONFIG_WR_SWITCH)
struct spll_stats *stats_ptr = 0x8000;
#endif

26 27 28 29 30
/*
 * The includes below contain code (not only declarations) to enable
 * the compiler to inline functions where necessary and save some CPU
 * cycles
 */
31 32 33 34 35 36 37 38 39

#include "spll_defs.h"
#include "spll_common.h"
#include "spll_debug.h"
#include "spll_helper.h"
#include "spll_main.h"
#include "spll_ptracker.h"
#include "spll_external.h"

40 41
#define MAIN_CHANNEL (spll_n_chan_ref)

42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58
#define SEQ_START_EXT 1
#define SEQ_WAIT_EXT 2
#define SEQ_START_HELPER 3
#define SEQ_WAIT_HELPER 4
#define SEQ_START_MAIN 5
#define SEQ_WAIT_MAIN 6
#define SEQ_DISABLED 7
#define SEQ_READY 8
#define SEQ_CLEAR_DACS 9
#define SEQ_WAIT_CLEAR_DACS 10

#define AUX_DISABLED 1
#define AUX_LOCK_PLL 2
#define AUX_ALIGN_PHASE 3
#define AUX_READY 4

struct spll_aux_state {
59
	int seq_state;
60
	int32_t phase_target;
61 62 63 64
	union {
		struct spll_main_state dmtd;
		/* spll_external_state ch_bb */
	} pll;
65 66 67 68 69 70 71 72 73 74 75 76 77
};

struct softpll_state {
	int mode;
	int seq_state;
	int dac_timeout;
	int default_dac_main;
	int delock_count;
	int32_t mpll_shift_ps;

	struct spll_helper_state helper;
	struct spll_external_state ext;
	struct spll_main_state mpll;
78
	struct spll_aux_state aux[MAX_CHAN_AUX];
79 80 81
	struct spll_ptracker_state ptrackers[MAX_PTRACKERS];
};

82 83 84 85 86 87 88 89 90 91 92 93 94 95 96
static const struct stringlist_entry seq_states [] =
{
	{ SEQ_START_EXT, "start-ext" },
	{ SEQ_WAIT_EXT, "wait-ext" },
	{ SEQ_START_HELPER, "start-helper" },
	{ SEQ_WAIT_HELPER, "wait-helper" },
	{ SEQ_START_MAIN, "start-main" },
	{ SEQ_WAIT_MAIN, "wait-main" },
	{ SEQ_DISABLED, "disabled" },
	{ SEQ_READY, "ready" },
	{ SEQ_CLEAR_DACS, "clear-dacs" },
	{ SEQ_WAIT_CLEAR_DACS, "wait-clear-dacs" },
	{ 0, NULL }
};

97 98
static volatile struct softpll_state softpll;

99 100 101 102
static volatile int ptracker_mask = 0;
/* fixme: should be done by spll_init() but spll_init is called to
 * switch modes (and we won't like messing around with ptrackers
 * there) */
103

104
static inline void start_ptrackers(struct softpll_state *s)
105
{
106 107 108 109 110
	int i;
	for (i = 0; i < spll_n_chan_ref; i++)
		if (ptracker_mask & (1 << i))
				ptracker_start(&s->ptrackers[i]);
}
111

112 113
static inline void update_ptrackers(struct softpll_state *s, int tag_value, int tag_source)
{
114 115 116 117
	if(tag_source > spll_n_chan_ref)
		return;
		
	ptrackers_update(s->ptrackers, tag_value, tag_source);
118
}
119

120 121 122 123 124
static inline void sequencing_fsm(struct softpll_state *s, int tag_value, int tag_source)
{
	switch (s->seq_state) {
		/* State "Clear DACs": initial SPLL sequnencer state. Brings both DACs (not the AUXs) to the default values
		   prior to starting the SPLL. */
125
		case SEQ_CLEAR_DACS:
126 127 128 129 130 131 132 133 134 135
		{
			/* Helper always starts at the maximum value (to make sure it locks on positive offset */
			SPLL->DAC_HPLL = s->helper.pi.y_max;

			/* Main starts at midscale */
			SPLL->DAC_MAIN = (s->mpll.pi.y_max + s->mpll.pi.y_min) / 2;
			
			/* we need tags from at least one channel, so that the IRQ that calls this function
			   gets called again */
			spll_enable_tagger(MAIN_CHANNEL, 1);
136 137 138

			softpll.dac_timeout = timer_get_tics()
				+ TICS_PER_SECOND / 20;
139 140
			softpll.seq_state = SEQ_WAIT_CLEAR_DACS;
			
141
			break;
142 143 144
		}
		
		/* State "Wait until DACs have been cleared". Makes sure the VCO control inputs have stabilized before starting the PLL. */
145
		case SEQ_WAIT_CLEAR_DACS:
146
		{
147
			if (time_after(timer_get_tics(), softpll.dac_timeout))
148 149 150 151 152 153
			{
				if(s->mode == SPLL_MODE_GRAND_MASTER)
					s->seq_state = SEQ_START_EXT;
				else
					s->seq_state = SEQ_START_HELPER;
			}
154
			break;
155
		}
156

157
		/* State "Disabled". Entered when the whole PLL is off */
158 159
		case SEQ_DISABLED:
			break;
160

161 162

		/* State "Start external reference PLL": starts up BB PLL for locking local reference to 10 MHz input */
163
		case SEQ_START_EXT:
164
		{
165
			spll_enable_tagger(MAIN_CHANNEL, 0);
166 167 168
			external_start(&s->ext);

			s->seq_state = SEQ_WAIT_EXT;
169
			break;
170
		}
171

172
		/* State "Wait until we are locked to external 10MHz clock" */
173
		case SEQ_WAIT_EXT:
174
		{
175 176 177 178
			if (external_locked(&s->ext)) {
				start_ptrackers(s);
				s->seq_state = SEQ_READY;
			}
179
			break;
180
		}
181 182

		case SEQ_START_HELPER:
183 184 185 186
		{
			helper_start(&s->helper);

			s->seq_state = SEQ_WAIT_HELPER;
187
			break;
188
		}
189

190
		case SEQ_WAIT_HELPER:
191 192 193 194 195 196 197 198 199
		{
			if (s->helper.ld.locked && s->helper.ld.lock_changed)
			{
				if (s->mode == SPLL_MODE_SLAVE)
				{
					s->seq_state = SEQ_START_MAIN;
				} else {
					start_ptrackers(s);
					s->seq_state = SEQ_READY;	
200
				}
201 202
			}
			break;
203
		}
204

205
		case SEQ_START_MAIN:
206 207 208
		{
			mpll_start(&s->mpll);
			s->seq_state = SEQ_WAIT_MAIN;
209
			break;
210
		}
211

212
		case SEQ_WAIT_MAIN:
213
		{
214
			if (s->mpll.ld.locked)
215 216
			{
				start_ptrackers(s);
217
				s->seq_state = SEQ_READY;
218
			}
219
			break;
220
		}
221

222
		case SEQ_READY:
223
		{
224
			if (s->mode == SPLL_MODE_GRAND_MASTER && !external_locked(&s->ext)) {
225 226
				s->delock_count++;
				s->seq_state = SEQ_CLEAR_DACS;
227
			} else if (!s->helper.ld.locked) {
228
				s->delock_count++;
229 230
				s->seq_state = SEQ_CLEAR_DACS;
			} else if (s->mode == SPLL_MODE_SLAVE && !s->mpll.ld.locked) {
231 232 233
				s->delock_count++;
				s->seq_state = SEQ_CLEAR_DACS;
			}
234
			break;
235 236 237
		}
	}
}
238

239 240 241
static inline void update_loops(struct softpll_state *s, int tag_value, int tag_source)
{
	
242
	helper_update(&s->helper, tag_value, tag_source);
243

244
	if(s->helper.ld.locked)
245
	{
246
		mpll_update(&s->mpll, tag_value, tag_source);
247

248 249 250 251 252 253
		if(s->seq_state == SEQ_READY) {
			if(s->mode == SPLL_MODE_SLAVE) {
				int i;
				for (i = 0; i < spll_n_chan_out - 1; i++)
					mpll_update(&s->aux[i].pll.dmtd, tag_value, tag_source);
			}
254

255
			update_ptrackers(s, tag_value, tag_source);
256 257
		}
	}
258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273
}

void _irq_entry()
{
	struct softpll_state *s = (struct softpll_state *)&softpll;

/* check if there are more tags in the FIFO */
	while (!(SPLL->TRR_CSR & SPLL_TRR_CSR_EMPTY)) {
	
		volatile uint32_t trr = SPLL->TRR_R0;
		int tag_source = SPLL_TRR_R0_CHAN_ID_R(trr);
		int tag_value  = SPLL_TRR_R0_VALUE_R(trr);

		sequencing_fsm(s, tag_value, tag_source);
		update_loops(s, tag_value, tag_source);
	}
274 275 276 277 278 279 280

	irq_count++;
	clear_irq();
}

void spll_init(int mode, int slave_ref_channel, int align_pps)
{
281
	static const char *modes[] = { "", "grandmaster", "freemaster", "slave", "disabled" };
282 283
	volatile int dummy;
	int i;
284

285 286 287 288
	struct softpll_state *s = (struct softpll_state *) &softpll;

	disable_irq();

289 290
	SPLL = (volatile struct SPLL_WB *)BASE_SOFTPLL;
	PPSG = (volatile struct PPSG_WB *)BASE_PPS_GEN;
291

292
	uint32_t csr = SPLL->CSR;
293

294 295
	spll_n_chan_ref = SPLL_CSR_N_REF_R(csr);
	spll_n_chan_out = SPLL_CSR_N_OUT_R(csr);
296 297 298
	
	s->mode = mode;
	s->delock_count = 0;
299 300 301 302

	SPLL->DAC_HPLL = 0;
	SPLL->DAC_MAIN = 0;

303
	SPLL->CSR = 0;
304 305 306 307 308 309 310
	SPLL->OCER = 0;
	SPLL->RCER = 0;
	SPLL->ECCR = 0;
	SPLL->OCCR = 0;
	SPLL->DEGLITCH_THR = 1000;

	PPSG->ESCR = 0;
311
	PPSG->CR = PPSG_CR_CNT_EN | PPSG_CR_CNT_RST | PPSG_CR_PWIDTH_W(PPS_WIDTH);
312

313 314 315 316
	if(mode == SPLL_MODE_DISABLED)
		s->seq_state = SEQ_DISABLED;
	else
		s->seq_state = SEQ_CLEAR_DACS;
317

318 319 320 321 322 323
	int helper_ref;
	
	if( mode == SPLL_MODE_SLAVE)
		helper_ref = slave_ref_channel; // Slave mode: lock the helper to an uplink port
	else
		helper_ref = spll_n_chan_ref; // Master/GM mode: lock the helper to the local ref clock
324

325
	helper_init(&s->helper, helper_ref);
326
	mpll_init(&s->mpll, slave_ref_channel, spll_n_chan_ref);
327

328 329 330
	for (i = 0; i < spll_n_chan_out - 1; i++) {
		mpll_init(&s->aux[i].pll.dmtd, slave_ref_channel, spll_n_chan_ref + i + 1);
		s->aux[i].seq_state = AUX_DISABLED;
331
	}
332 333 334 335
	
	if(mode == SPLL_MODE_FREE_RUNNING_MASTER)
		PPSG->ESCR = PPSG_ESCR_PPS_VALID | PPSG_ESCR_TM_VALID;
	
336
	for (i = 0; i < spll_n_chan_ref; i++)
337
		ptracker_init(&s->ptrackers[i], i, PTRACKER_AVERAGE_SAMPLES);
338

339 340 341 342 343 344 345 346 347 348 349
	if(mode == SPLL_MODE_GRAND_MASTER) {
		if(SPLL->ECCR & SPLL_ECCR_EXT_SUPPORTED) {
			s->ext.helper = &s->helper;
			s->ext.main = &s->mpll;
			external_init(&s->ext, spll_n_chan_ref + spll_n_chan_out, align_pps);
		} else {
			TRACE_DEV("softpll: attempting to enable GM mode on non-GM hardware.\n");
			return;
		}
	}

350
	TRACE_DEV
351 352
	    ("softpll: mode %s, %d ref channels, %d out channels\n",
	     modes[mode], spll_n_chan_ref, spll_n_chan_out);
353 354

	/* Purge tag buffer */
355 356
	while (!(SPLL->TRR_CSR & SPLL_TRR_CSR_EMPTY))
		dummy = SPLL->TRR_R0;
357
	
358 359
	SPLL->EIC_IER = 1;
	SPLL->OCER |= 1;
360
	
361 362 363 364 365
	enable_irq();
}

void spll_shutdown()
{
366 367
	disable_irq();

368 369 370 371 372 373 374 375
	SPLL->OCER = 0;
	SPLL->RCER = 0;
	SPLL->ECCR = 0;
	SPLL->EIC_IDR = 1;
}

void spll_start_channel(int channel)
{
376 377 378
	struct softpll_state *s = (struct softpll_state *) &softpll;

	if (s->seq_state != SEQ_READY || !channel) {
379 380
		TRACE_DEV("Can't start channel %d, the PLL is not ready\n",
			  channel);
381
		return;
382
	}
383
	mpll_start(&s->aux[channel - 1].pll.dmtd);
384 385 386 387
}

void spll_stop_channel(int channel)
{
388 389
	struct softpll_state *s = (struct softpll_state *) &softpll;
	
390
	if (!channel)
391 392
		return;

393
	mpll_stop(&s->aux[channel - 1].pll.dmtd);
394 395
}

396 397 398 399 400
int spll_ext_locked()
{
	return external_locked( (struct spll_external_state *) &softpll.ext);
}

401 402
int spll_check_lock(int channel)
{
403 404 405 406
	if (!channel)
		return (softpll.seq_state == SEQ_READY);
	else
		return (softpll.seq_state == SEQ_READY)
407
		    && softpll.aux[channel - 1].pll.dmtd.ld.locked;
408 409 410 411
}

static int32_t to_picos(int32_t units)
{
412 413
	return (int32_t) (((int64_t) units *
			   (int64_t) CLOCK_PERIOD_PICOSECONDS) >> HPLL_N);
414 415 416 417 418
}

/* Channel 0 = local PLL reference, 1...N = aux oscillators */
static void set_phase_shift(int channel, int32_t value_picoseconds)
{
419 420
	struct spll_main_state *st = (struct spll_main_state *)
	    (!channel ? &softpll.mpll : &softpll.aux[channel - 1].pll.dmtd);
421
	mpll_set_phase_shift(st, value_picoseconds);
422 423 424 425 426 427
	softpll.mpll_shift_ps = value_picoseconds;
}

void spll_set_phase_shift(int channel, int32_t value_picoseconds)
{
	int i;
428
	if (channel == SPLL_ALL_CHANNELS) {
429
		spll_set_phase_shift(0, value_picoseconds);
430
		for (i = 0; i < spll_n_chan_out - 1; i++)
431
			if (softpll.aux[i].seq_state == AUX_READY)
432
				set_phase_shift(i + 1, value_picoseconds);
433 434 435 436
	} else
		set_phase_shift(channel, value_picoseconds);
}

437
void spll_get_phase_shift(int channel, int32_t *current, int32_t *target)
438
{
439 440
	volatile struct spll_main_state *st = (struct spll_main_state *)
	    (!channel ? &softpll.mpll : &softpll.aux[channel - 1].pll.dmtd);
441 442 443 444 445
	int div = (DIVIDE_DMTD_CLOCKS_BY_2 ? 2 : 1);
	if (current)
		*current = to_picos(st->phase_shift_current * div);
	if (target)
		*target = to_picos(st->phase_shift_target * div);
446 447
}

448
int spll_read_ptracker(int channel, int32_t *phase_ps, int *enabled)
449 450
{
	volatile struct spll_ptracker_state *st = &softpll.ptrackers[channel];
451 452 453 454 455 456 457
	int phase = st->phase_val;
	if (phase < 0)
		phase += (1 << HPLL_N);
	else if (phase >= (1 << HPLL_N))
		phase -= (1 << HPLL_N);

	if (DIVIDE_DMTD_CLOCKS_BY_2) {
458
		phase <<= 1;
459
		phase &= (1 << HPLL_N) - 1;
460
	}
461

462
	*phase_ps = to_picos(phase);
463
	if (enabled)
464
		*enabled = ptracker_mask & (1 << st->id) ? 1 : 0;
465 466 467 468 469
	return st->ready;
}

void spll_get_num_channels(int *n_ref, int *n_out)
{
470
	if (n_ref)
471
		*n_ref = spll_n_chan_ref;
472
	if (n_out)
473
		*n_out = spll_n_chan_out;
474 475 476 477
}

void spll_show_stats()
{
478
	if (softpll.mode > 0)
479 480 481 482 483 484
		    TRACE_DEV("softpll: irqs %d seq %s mode %d "
		     "alignment_state %d HL%d ML%d HY=%d MY=%d DelCnt=%d\n",
		     irq_count, stringlist_lookup(seq_states, softpll.seq_state), softpll.mode,
		     softpll.ext.align_state, softpll.helper.ld.locked, softpll.mpll.ld.locked,
		     softpll.helper.pi.y, softpll.mpll.pi.y,
		     softpll.delock_count);
485 486 487 488
}

int spll_shifter_busy(int channel)
{
489
	if (!channel)
490
		return mpll_shifter_busy((struct spll_main_state *)&softpll.mpll);
491
	else
492
		return mpll_shifter_busy((struct spll_main_state *)&softpll.aux[channel - 1].pll.dmtd);
493 494 495 496
}

void spll_enable_ptracker(int ref_channel, int enable)
{
497
	if (enable) {
498
		spll_enable_tagger(ref_channel, 1);
499 500 501
		ptracker_start((struct spll_ptracker_state *)&softpll.
			       ptrackers[ref_channel]);
		ptracker_mask |= (1 << ref_channel);
502 503 504
		TRACE_DEV("Enabling ptracker channel: %d\n", ref_channel);

	} else {
505 506
		ptracker_mask &= ~(1 << ref_channel);
		if (ref_channel != softpll.mpll.id_ref)
507 508 509 510 511 512 513 514 515 516
			spll_enable_tagger(ref_channel, 0);
		TRACE_DEV("Disabling ptracker tagger: %d\n", ref_channel);
	}
}

int spll_get_delock_count()
{
	return softpll.delock_count;
}

517
static inline int aux_locking_enabled(int channel)
518 519
{
	uint32_t occr_aux_en = SPLL_OCCR_OUT_EN_R(SPLL->OCCR);
520 521 522
	
	return occr_aux_en & (1 << channel);
}
523

524 525 526 527 528 529 530
static inline void aux_set_channel_status(int channel, int locked)
{
	if(!locked)
		SPLL->OCCR &= ~(SPLL_OCCR_OUT_LOCK_W((1 << channel)));
	else
		SPLL->OCCR |= (SPLL_OCCR_OUT_LOCK_W((1 << channel)));
}
531

532 533 534
int spll_update_aux_clocks()
{
	int ch;
535

536 537 538
	for (ch = 1; ch < spll_n_chan_out; ch++) 
	{
		struct spll_aux_state *s = (struct spll_aux_state *) &softpll.aux[ch - 1];
539

540
		if(s->seq_state != AUX_DISABLED && !aux_locking_enabled(ch))
541 542 543 544 545 546
		{
			TRACE_DEV("softpll: disabled aux channel %d\n", ch);
			spll_stop_channel(ch);
			aux_set_channel_status(ch, 0);
			s->seq_state = AUX_DISABLED;
		}
547

548 549 550 551 552 553
		switch (s->seq_state) {
			case AUX_DISABLED:
				if (softpll.mpll.ld.locked && aux_locking_enabled(ch)) {
					TRACE_DEV("softpll: enabled aux channel %d\n", ch);
					spll_start_channel(ch);
					s->seq_state = AUX_LOCK_PLL;
554 555 556 557
				}
				break;

			case AUX_LOCK_PLL:
558 559 560 561
				if (s->pll.dmtd.ld.locked) {
					TRACE_DEV ("softpll: channel %d locked [aligning @ %d ps]\n", ch, softpll.mpll_shift_ps);
					set_phase_shift(ch, softpll.mpll_shift_ps);
					s->seq_state = AUX_ALIGN_PHASE;
562 563 564 565 566
				}

				break;

			case AUX_ALIGN_PHASE:
567 568 569 570
				if (!mpll_shifter_busy(&s->pll.dmtd)) {
					TRACE_DEV("softpll: channel %d phase aligned\n", ch);
					aux_set_channel_status(ch, 1);
					s->seq_state = AUX_READY;
571 572 573 574
				}
				break;

			case AUX_READY:
575 576 577 578
				if (!softpll.mpll.ld.locked || !s->pll.dmtd.ld.locked) {
					TRACE_DEV("softpll: aux channel %d or mpll lost lock\n", ch);
					aux_set_channel_status(ch, 0); 
					s->seq_state = AUX_DISABLED;
579 580 581
				}
				break;
			}
582 583 584 585 586 587 588 589
	}
	return 0;
}

int spll_get_aux_status(int channel)
{
	int rval = 0;

590
	if (softpll.aux[channel].seq_state != AUX_DISABLED)
591
		rval |= SPLL_AUX_ENABLED;
592

593
	if (softpll.aux[channel].seq_state == AUX_READY)
594
		rval |= SPLL_AUX_LOCKED;
595 596 597 598

	return rval;
}

599 600 601
const char *spll_get_aux_status_string(int channel)
{
	const char *aux_stat[] = {"disabled", "locking", "aligning", "locked"};
602
	struct spll_aux_state *s = (struct spll_aux_state* )&softpll.aux[channel];
603 604 605 606 607 608 609 610 611 612 613

	switch(s->seq_state)
	{
		case AUX_DISABLED: return aux_stat[0];
		case AUX_LOCK_PLL: return aux_stat[1];
		case AUX_ALIGN_PHASE: return aux_stat[2];
		case AUX_READY: return aux_stat[3];
	}
	return "";
}

614 615
int spll_get_dac(int index)
{
616 617
	if (index < 0)
		return softpll.helper.pi.y;
618
	else if (index == 0)
619
		return softpll.mpll.pi.y;
620
	else if (index > 0)
621
		return softpll.aux[index - 1].pll.dmtd.pi.y;
622
	return 0;
623 624 625 626
}

void spll_set_dac(int index, int value)
{
627 628 629 630 631 632 633
	if (index < 0) {
		softpll.helper.pi.y = value;
		SPLL->DAC_HPLL = value;
	} else {
		SPLL->DAC_MAIN =
		    SPLL_DAC_MAIN_DAC_SEL_W(index) | (value & 0xffff);

634 635 636
		if (index == 0)
			softpll.mpll.pi.y = value;
		else if (index > 0)
637
			softpll.aux[index - 1].pll.dmtd.pi.y = value;
638 639
	}
}
640 641 642

void spll_update()
{
643
	struct spll_stats temp_stats;
644 645 646 647 648 649
	switch(softpll.mode) {
		case SPLL_MODE_GRAND_MASTER:
			external_align_fsm(&softpll.ext);
			break;
	}
	spll_update_aux_clocks();
650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668

#if defined(CONFIG_WR_SWITCH)
	/* for WRS update .stat section in memory */
	temp_stats.magic = 0x5b1157a7;
	temp_stats.ver	 = 1;
	temp_stats.valid = 0;
	temp_stats.mode  = softpll.mode;
	temp_stats.irq_cnt = irq_count;
	temp_stats.seq_state = softpll.seq_state;
	temp_stats.align_state = softpll.ext.align_state;
	temp_stats.H_lock = softpll.helper.ld.locked;
	temp_stats.M_lock = softpll.mpll.ld.locked;
	temp_stats.H_y = softpll.helper.pi.y;
	temp_stats.M_y = softpll.mpll.pi.y;
	temp_stats.del_cnt = softpll.delock_count;
	stats_ptr->valid = 0;
	*stats_ptr = temp_stats;
	stats_ptr->valid = 1;
#endif
669 670
}

671
static int spll_measure_frequency(int osc)
672 673 674 675 676 677 678 679 680 681 682 683 684
{
	volatile uint32_t *reg;

	switch(osc) {
		case SPLL_OSC_REF:
			reg = &SPLL->F_REF;
			break;
		case SPLL_OSC_DMTD:
			reg = &SPLL->F_DMTD;
			break;
		case SPLL_OSC_EXT:
			reg = &SPLL->F_EXT;
			break;
685 686
		default:
			return 0;
687 688 689 690 691
	}

    timer_delay_ms(2000);
    return (*reg ) & (0xfffffff);
}
692 693 694 695 696 697 698

static int calc_apr(int meas_min, int meas_max, int f_center )
{
	// apr_min is in PPM

	int64_t delta_low =  meas_min - f_center;
	int64_t delta_hi = meas_max - f_center;
699
	uint64_t u_delta_low, u_delta_hi;
700
	int ppm_lo, ppm_hi;
701 702 703 704 705 706

	if(delta_low >= 0)
		return -1;
	if(delta_hi <= 0)
		return -1;

707
	/* __div64_32 divides 64 by 32; result is in the 64 argument. */
708 709 710
	u_delta_low = -delta_low * 1000000LL;
	__div64_32(&u_delta_low, f_center);
	ppm_lo = (int)u_delta_low;
711

712 713 714
	u_delta_hi = delta_hi * 1000000LL;
	__div64_32(&u_delta_hi, f_center);
	ppm_hi = (int)u_delta_hi;
715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740

	return ppm_lo < ppm_hi ? ppm_lo : ppm_hi;
}

void check_vco_frequencies()
{
	disable_irq();

	int f_min, f_max;
	TRACE_DEV("SoftPLL VCO Frequency/APR test:\n");

	spll_set_dac(-1, 0);
	f_min = spll_measure_frequency(SPLL_OSC_DMTD);
	spll_set_dac(-1, 65535);
	f_max = spll_measure_frequency(SPLL_OSC_DMTD);
	TRACE_DEV("DMTD VCO:  Low=%d Hz Hi=%d Hz, APR = %d ppm.\n", f_min, f_max, calc_apr(f_min, f_max, 62500000));

	spll_set_dac(0, 0);
	f_min = spll_measure_frequency(SPLL_OSC_REF);
	spll_set_dac(0, 65535);
	f_max = spll_measure_frequency(SPLL_OSC_REF);
	TRACE_DEV("REF VCO:   Low=%d Hz Hi=%d Hz, APR = %d ppm.\n", f_min, f_max, calc_apr(f_min, f_max, REF_CLOCK_FREQ_HZ));

	f_min = spll_measure_frequency(SPLL_OSC_EXT);
	TRACE_DEV("EXT clock: Freq=%d Hz\n", f_min);
}