softpll_ng.c 16.9 KB
Newer Older
1 2 3 4 5 6 7 8 9
/*
 * This work is part of the White Rabbit project
 *
 * Copyright (C) 2010 - 2015 CERN (www.cern.ch)
 * Author: Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
 * Author: Grzegorz Daniluk <grzegorz.daniluk@cern.ch>
 *
 * Released according to the GNU GPL, version 2 or any later version.
 */
10 11 12 13
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

14
#include <wrc.h>
15 16 17 18 19 20 21 22 23
#include "board.h"
#include "trace.h"
#include "hw/softpll_regs.h"
#include "hw/pps_gen_regs.h"

#include "softpll_ng.h"

#include "irq.h"

24 25 26 27
volatile struct SPLL_WB *SPLL;
volatile struct PPSG_WB *PPSG;

int spll_n_chan_ref, spll_n_chan_out;
28 29 30



31 32
#define MAIN_CHANNEL (spll_n_chan_ref)

33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48
#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

49 50 51 52 53 54 55 56 57 58 59 60 61 62 63
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 }
};

64
volatile struct softpll_state softpll;
65

66 67 68 69
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) */
70

71
static inline void start_ptrackers(struct softpll_state *s)
72
{
73 74 75 76 77
	int i;
	for (i = 0; i < spll_n_chan_ref; i++)
		if (ptracker_mask & (1 << i))
				ptracker_start(&s->ptrackers[i]);
}
78

79 80
static inline void update_ptrackers(struct softpll_state *s, int tag_value, int tag_source)
{
81 82 83 84
	if(tag_source > spll_n_chan_ref)
		return;
		
	ptrackers_update(s->ptrackers, tag_value, tag_source);
85
}
86

87 88 89 90 91
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. */
92
		case SEQ_CLEAR_DACS:
93 94 95 96 97 98 99 100 101 102
		{
			/* 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);
103 104 105

			softpll.dac_timeout = timer_get_tics()
				+ TICS_PER_SECOND / 20;
106 107
			softpll.seq_state = SEQ_WAIT_CLEAR_DACS;
			
108
			break;
109 110 111
		}
		
		/* State "Wait until DACs have been cleared". Makes sure the VCO control inputs have stabilized before starting the PLL. */
112
		case SEQ_WAIT_CLEAR_DACS:
113
		{
114
			if (time_after(timer_get_tics(), softpll.dac_timeout))
115 116 117 118 119 120
			{
				if(s->mode == SPLL_MODE_GRAND_MASTER)
					s->seq_state = SEQ_START_EXT;
				else
					s->seq_state = SEQ_START_HELPER;
			}
121
			break;
122
		}
123

124
		/* State "Disabled". Entered when the whole PLL is off */
125 126
		case SEQ_DISABLED:
			break;
127

128 129

		/* State "Start external reference PLL": starts up BB PLL for locking local reference to 10 MHz input */
130
		case SEQ_START_EXT:
131
		{
132
			spll_enable_tagger(MAIN_CHANNEL, 0);
133 134 135
			external_start(&s->ext);

			s->seq_state = SEQ_WAIT_EXT;
136
			break;
137
		}
138

139
		/* State "Wait until we are locked to external 10MHz clock" */
140
		case SEQ_WAIT_EXT:
141
		{
142 143 144 145
			if (external_locked(&s->ext)) {
				start_ptrackers(s);
				s->seq_state = SEQ_READY;
			}
146
			break;
147
		}
148 149

		case SEQ_START_HELPER:
150 151 152 153
		{
			helper_start(&s->helper);

			s->seq_state = SEQ_WAIT_HELPER;
154
			break;
155
		}
156

157
		case SEQ_WAIT_HELPER:
158 159 160 161 162 163 164 165 166
		{
			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;	
167
				}
168 169
			}
			break;
170
		}
171

172
		case SEQ_START_MAIN:
173 174 175
		{
			mpll_start(&s->mpll);
			s->seq_state = SEQ_WAIT_MAIN;
176
			break;
177
		}
178

179
		case SEQ_WAIT_MAIN:
180
		{
181
			if (s->mpll.ld.locked)
182 183
			{
				start_ptrackers(s);
184
				s->seq_state = SEQ_READY;
185
			}
186
			break;
187
		}
188

189
		case SEQ_READY:
190
		{
191
			if (s->mode == SPLL_MODE_GRAND_MASTER && !external_locked(&s->ext)) {
192 193
				s->delock_count++;
				s->seq_state = SEQ_CLEAR_DACS;
194
			} else if (!s->helper.ld.locked) {
195
				s->delock_count++;
196 197
				s->seq_state = SEQ_CLEAR_DACS;
			} else if (s->mode == SPLL_MODE_SLAVE && !s->mpll.ld.locked) {
198 199 200
				s->delock_count++;
				s->seq_state = SEQ_CLEAR_DACS;
			}
201
			break;
202 203 204
		}
	}
}
205

206 207 208
static inline void update_loops(struct softpll_state *s, int tag_value, int tag_source)
{
	
209
	helper_update(&s->helper, tag_value, tag_source);
210

211
	if(s->helper.ld.locked)
212
	{
213
		mpll_update(&s->mpll, tag_value, tag_source);
214

215 216 217 218 219 220
		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);
			}
221

222
			update_ptrackers(s, tag_value, tag_source);
223 224
		}
	}
225 226
}

227 228
struct spll_fifo_log fifo_log[FIFO_LOG_LEN];

229
void _irq_entry(void)
230 231
{
	struct softpll_state *s = (struct softpll_state *)&softpll;
232 233 234 235
	uint32_t trr;
	int i, tag_source, tag_value;
	static uint16_t tag_count;
	struct spll_fifo_log *l;
236

237
	/* check if there are more tags in the FIFO, and log them */
238
	while (!(SPLL->TRR_CSR & SPLL_TRR_CSR_EMPTY)) {
239 240 241 242 243 244 245 246 247 248 249 250
		trr = SPLL->TRR_R0;

		/* save this to a circular buffer */
		i = tag_count % FIFO_LOG_LEN;
		l = fifo_log + i;
		l->trr = trr;
		l->irq_count = s->irq_count & 0xffff;
		l->tag_count = tag_count++;

		/* And process the values */
		tag_source = SPLL_TRR_R0_CHAN_ID_R(trr);
		tag_value  = SPLL_TRR_R0_VALUE_R(trr);
251 252 253 254

		sequencing_fsm(s, tag_value, tag_source);
		update_loops(s, tag_value, tag_source);
	}
255

256
	s->irq_count++;
257 258 259
	clear_irq();
}

260 261 262 263 264 265
void spll_very_init()
{
	PPSG = (volatile struct PPSG_WB *)BASE_PPS_GEN;
	PPSG->CR = PPSG_CR_CNT_EN | PPSG_CR_CNT_RST | PPSG_CR_PWIDTH_W(PPS_WIDTH);
}

266 267
void spll_init(int mode, int slave_ref_channel, int align_pps)
{
268
	static const char *modes[] = { "", "grandmaster", "freemaster", "slave", "disabled" };
269 270
	volatile int dummy;
	int i;
271

272 273 274 275
	struct softpll_state *s = (struct softpll_state *) &softpll;

	disable_irq();

276 277
	SPLL = (volatile struct SPLL_WB *)BASE_SOFTPLL;
	PPSG = (volatile struct PPSG_WB *)BASE_PPS_GEN;
278

279
	uint32_t csr = SPLL->CSR;
280

281 282
	spll_n_chan_ref = SPLL_CSR_N_REF_R(csr);
	spll_n_chan_out = SPLL_CSR_N_OUT_R(csr);
283 284 285
	
	s->mode = mode;
	s->delock_count = 0;
286 287 288 289

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

290
	SPLL->CSR = 0;
291 292 293 294 295 296 297
	SPLL->OCER = 0;
	SPLL->RCER = 0;
	SPLL->ECCR = 0;
	SPLL->OCCR = 0;
	SPLL->DEGLITCH_THR = 1000;

	PPSG->ESCR = 0;
298
	PPSG->CR = PPSG_CR_CNT_EN | PPSG_CR_PWIDTH_W(PPS_WIDTH);
299

300 301 302 303
	if(mode == SPLL_MODE_DISABLED)
		s->seq_state = SEQ_DISABLED;
	else
		s->seq_state = SEQ_CLEAR_DACS;
304

305 306 307 308 309 310
	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
311

312
	helper_init(&s->helper, helper_ref);
313
	mpll_init(&s->mpll, slave_ref_channel, spll_n_chan_ref);
314

315 316 317
	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;
318
	}
319 320 321 322
	
	if(mode == SPLL_MODE_FREE_RUNNING_MASTER)
		PPSG->ESCR = PPSG_ESCR_PPS_VALID | PPSG_ESCR_TM_VALID;
	
323
	for (i = 0; i < spll_n_chan_ref; i++)
324
		ptracker_init(&s->ptrackers[i], i, PTRACKER_AVERAGE_SAMPLES);
325

326 327 328 329 330 331 332 333 334 335 336
	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;
		}
	}

337
	TRACE_DEV
338 339
	    ("softpll: mode %s, %d ref channels, %d out channels\n",
	     modes[mode], spll_n_chan_ref, spll_n_chan_out);
340 341

	/* Purge tag buffer */
342 343
	while (!(SPLL->TRR_CSR & SPLL_TRR_CSR_EMPTY))
		dummy = SPLL->TRR_R0;
344
	
345 346
	SPLL->EIC_IER = 1;
	SPLL->OCER |= 1;
347
	
348 349 350 351 352
	enable_irq();
}

void spll_shutdown()
{
353 354
	disable_irq();

355 356 357 358 359 360 361 362
	SPLL->OCER = 0;
	SPLL->RCER = 0;
	SPLL->ECCR = 0;
	SPLL->EIC_IDR = 1;
}

void spll_start_channel(int channel)
{
363 364 365
	struct softpll_state *s = (struct softpll_state *) &softpll;

	if (s->seq_state != SEQ_READY || !channel) {
366 367
		TRACE_DEV("Can't start channel %d, the PLL is not ready\n",
			  channel);
368
		return;
369
	}
370
	mpll_start(&s->aux[channel - 1].pll.dmtd);
371 372 373 374
}

void spll_stop_channel(int channel)
{
375 376
	struct softpll_state *s = (struct softpll_state *) &softpll;
	
377
	if (!channel)
378 379
		return;

380
	mpll_stop(&s->aux[channel - 1].pll.dmtd);
381 382 383 384
}

int spll_check_lock(int channel)
{
385 386 387 388
	if (!channel)
		return (softpll.seq_state == SEQ_READY);
	else
		return (softpll.seq_state == SEQ_READY)
389
		    && softpll.aux[channel - 1].pll.dmtd.ld.locked;
390 391 392 393
}

static int32_t to_picos(int32_t units)
{
394 395
	return (int32_t) (((int64_t) units *
			   (int64_t) CLOCK_PERIOD_PICOSECONDS) >> HPLL_N);
396 397 398 399 400
}

/* Channel 0 = local PLL reference, 1...N = aux oscillators */
static void set_phase_shift(int channel, int32_t value_picoseconds)
{
401 402
	struct spll_main_state *st = (struct spll_main_state *)
	    (!channel ? &softpll.mpll : &softpll.aux[channel - 1].pll.dmtd);
403
	mpll_set_phase_shift(st, value_picoseconds);
404 405 406 407 408 409
	softpll.mpll_shift_ps = value_picoseconds;
}

void spll_set_phase_shift(int channel, int32_t value_picoseconds)
{
	int i;
410
	if (channel == SPLL_ALL_CHANNELS) {
411
		spll_set_phase_shift(0, value_picoseconds);
412
		for (i = 0; i < spll_n_chan_out - 1; i++)
413
			if (softpll.aux[i].seq_state == AUX_READY)
414
				set_phase_shift(i + 1, value_picoseconds);
415 416 417 418
	} else
		set_phase_shift(channel, value_picoseconds);
}

419
void spll_get_phase_shift(int channel, int32_t *current, int32_t *target)
420
{
421 422
	volatile struct spll_main_state *st = (struct spll_main_state *)
	    (!channel ? &softpll.mpll : &softpll.aux[channel - 1].pll.dmtd);
423 424 425 426 427
	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);
428 429
}

430
int spll_read_ptracker(int channel, int32_t *phase_ps, int *enabled)
431 432
{
	volatile struct spll_ptracker_state *st = &softpll.ptrackers[channel];
433 434 435 436 437 438 439
	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) {
440
		phase <<= 1;
441
		phase &= (1 << HPLL_N) - 1;
442
	}
443

444
	*phase_ps = to_picos(phase);
445
	if (enabled)
446
		*enabled = ptracker_mask & (1 << st->id) ? 1 : 0;
447 448 449 450 451
	return st->ready;
}

void spll_get_num_channels(int *n_ref, int *n_out)
{
452
	if (n_ref)
453
		*n_ref = spll_n_chan_ref;
454
	if (n_out)
455
		*n_out = spll_n_chan_out;
456 457 458 459
}

void spll_show_stats()
{
460 461
	struct softpll_state *s = (struct softpll_state *)&softpll;

462
	if (softpll.mode > 0)
463
		    pp_printf("softpll: irqs %d seq %s mode %d "
464
		     "alignment_state %d HL%d ML%d HY=%d MY=%d DelCnt=%d\n",
465 466 467 468 469
		      s->irq_count, stringlist_lookup(seq_states, s->seq_state),
			      s->mode, s->ext.align_state,
			      s->helper.ld.locked, s->mpll.ld.locked,
			      s->helper.pi.y, s->mpll.pi.y,
			      s->delock_count);
470 471 472 473
}

int spll_shifter_busy(int channel)
{
474
	if (!channel)
475
		return mpll_shifter_busy((struct spll_main_state *)&softpll.mpll);
476
	else
477
		return mpll_shifter_busy((struct spll_main_state *)&softpll.aux[channel - 1].pll.dmtd);
478 479 480 481
}

void spll_enable_ptracker(int ref_channel, int enable)
{
482
	if (enable) {
483
		spll_enable_tagger(ref_channel, 1);
484 485 486
		ptracker_start((struct spll_ptracker_state *)&softpll.
			       ptrackers[ref_channel]);
		ptracker_mask |= (1 << ref_channel);
487 488 489
		TRACE_DEV("Enabling ptracker channel: %d\n", ref_channel);

	} else {
490 491
		ptracker_mask &= ~(1 << ref_channel);
		if (ref_channel != softpll.mpll.id_ref)
492 493 494 495 496 497 498 499 500 501
			spll_enable_tagger(ref_channel, 0);
		TRACE_DEV("Disabling ptracker tagger: %d\n", ref_channel);
	}
}

int spll_get_delock_count()
{
	return softpll.delock_count;
}

502
static inline int aux_locking_enabled(int channel)
503 504
{
	uint32_t occr_aux_en = SPLL_OCCR_OUT_EN_R(SPLL->OCCR);
505 506 507
	
	return occr_aux_en & (1 << channel);
}
508

509 510 511 512 513 514 515
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)));
}
516

517
static int spll_update_aux_clocks(void)
518 519
{
	int ch;
520

521 522 523
	for (ch = 1; ch < spll_n_chan_out; ch++) 
	{
		struct spll_aux_state *s = (struct spll_aux_state *) &softpll.aux[ch - 1];
524

525
		if(s->seq_state != AUX_DISABLED && !aux_locking_enabled(ch))
526 527 528 529 530 531
		{
			TRACE_DEV("softpll: disabled aux channel %d\n", ch);
			spll_stop_channel(ch);
			aux_set_channel_status(ch, 0);
			s->seq_state = AUX_DISABLED;
		}
532

533 534 535 536 537 538
		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;
539 540 541 542
				}
				break;

			case AUX_LOCK_PLL:
543 544 545 546
				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;
547 548 549 550 551
				}

				break;

			case AUX_ALIGN_PHASE:
552 553 554 555
				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;
556 557 558 559
				}
				break;

			case AUX_READY:
560 561 562 563
				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;
564 565 566
				}
				break;
			}
567 568 569 570 571 572 573 574
	}
	return 0;
}

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

575
	if (softpll.aux[channel].seq_state != AUX_DISABLED)
576
		rval |= SPLL_AUX_ENABLED;
577

578
	if (softpll.aux[channel].seq_state == AUX_READY)
579
		rval |= SPLL_AUX_LOCKED;
580 581 582 583

	return rval;
}

584 585 586
const char *spll_get_aux_status_string(int channel)
{
	const char *aux_stat[] = {"disabled", "locking", "aligning", "locked"};
587
	struct spll_aux_state *s = (struct spll_aux_state* )&softpll.aux[channel];
588 589 590 591 592 593 594 595 596 597 598

	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 "";
}

599 600
int spll_get_dac(int index)
{
601 602
	if (index < 0)
		return softpll.helper.pi.y;
603
	else if (index == 0)
604
		return softpll.mpll.pi.y;
605
	else if (index > 0)
606
		return softpll.aux[index - 1].pll.dmtd.pi.y;
607
	return 0;
608 609 610 611
}

void spll_set_dac(int index, int value)
{
612 613 614 615 616 617 618
	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);

619 620 621
		if (index == 0)
			softpll.mpll.pi.y = value;
		else if (index > 0)
622
			softpll.aux[index - 1].pll.dmtd.pi.y = value;
623 624
	}
}
625 626 627 628 629 630 631 632 633

void spll_update()
{
	switch(softpll.mode) {
		case SPLL_MODE_GRAND_MASTER:
			external_align_fsm(&softpll.ext);
			break;
	}
	spll_update_aux_clocks();
634

635
	/* currently we have statistics only in the switch */
636
	if (is_wr_switch) {
637
		stats.sequence++;
638
		stats.mode  = softpll.mode;
639
		stats.irq_cnt = softpll.irq_count;
640 641 642 643 644 645 646
		stats.seq_state = softpll.seq_state;
		stats.align_state = softpll.ext.align_state;
		stats.H_lock = softpll.helper.ld.locked;
		stats.M_lock = softpll.mpll.ld.locked;
		stats.H_y = softpll.helper.pi.y;
		stats.M_y = softpll.mpll.pi.y;
		stats.del_cnt = softpll.delock_count;
647
		stats.sequence++;
648
	}
649 650
}

651
static int spll_measure_frequency(int osc)
652 653 654 655 656 657 658 659 660 661 662 663 664
{
	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;
665 666
		default:
			return 0;
667 668 669 670 671
	}

    timer_delay_ms(2000);
    return (*reg ) & (0xfffffff);
}
672 673 674 675 676 677 678

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;
679
	uint64_t u_delta_low, u_delta_hi;
680
	int ppm_lo, ppm_hi;
681 682 683 684 685 686

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

687
	/* __div64_32 divides 64 by 32; result is in the 64 argument. */
688 689 690
	u_delta_low = -delta_low * 1000000LL;
	__div64_32(&u_delta_low, f_center);
	ppm_lo = (int)u_delta_low;
691

692 693 694
	u_delta_hi = delta_hi * 1000000LL;
	__div64_32(&u_delta_hi, f_center);
	ppm_hi = (int)u_delta_hi;
695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720

	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);
}