#include <defBF533.h>
#include "startup.h"

/************************************************************************************************/
/*			   GLOBAL & EXTERNAL DECLARATIONS													*/	
/************************************************************************************************/    
.SECTION	data1;

.GLOBAL		PFA_HANDLER;
.GLOBAL		PFB_HANDLER;

.SECTION	program;
/************************************************************************
 * PROGRAMMABLE FLAG A INTERRUPT HANDLER								*
 *																		*
 * Acknowledges the PFA (SW4) interrupt and modifies core regulator		*
 * voltage by decrementing the VLEV field of VR_CTL.  If VLEV is at the	*
 * minimum, it will wrap to the high level of 1.20 V.					*
 ************************************************************************/
PFA_HANDLER:
	[--SP] = (r7:0,p5:0);
	[--SP] = ASTAT;

	p5.h = hi(FIO_FLAG_C);	p5.l = lo(FIO_FLAG_C);
	r0 = 0x0100 (z);			// PF8
	w[p5] = r0.l;				// Acknowledge PFA Interrupt

	p0.h = hi(VR_CTL);   	p0.l = lo(VR_CTL);
	r1 = w[p0](z);				// Read VR_CTL into scene_reg for EXTRACT

	r2 = 0x0404;				// pattern_reg for EXTRACT, start bit is bit 4
	r7 = EXTRACT(r1, r2.l)(z);	// length of field is 4 bits, R7 = VLEV field
	
	r2 = 0x3(z);				// Minimum VLEV rating
	
	CC = r7 == r2;
	IF !CC JUMP Not_Minimal;
	
	r7 = 0xE(z);				// Set VLEV to MAX Value of 0xD + 1
	
Not_Minimal:
	r7 += -1;				// Decrement VLEV field
							// If Min, setting to 0xE above wraps to max VLEV of 0xD

	r2 = 0x8(z);			// VLEV = 8 == ~1V, change processor speed
	
	cc = r7 == r2;			// CC contains check for crossing 1V threshold
	
	if !cc jump post_freqA;	// if not crossing 1V, no need to call freq_change

		[--SP] = rets;
			call freq_change;
		rets = [SP++];
		
post_freqA:
	// R1 contains current VR_CTL setting and will be the backgnd_reg for the DEPOSIT
	r0 = r7;				// backup VLEV setting for compare later
	r7 <<= 16;				// shift r7.l into r7.h
							// r7 is foregnd_reg for DEPOSIT, r7.h is bit-field
	r2 = 0x0404(z);			// Position = Bit 4, Length = 4 Bits
	
	r7 = r7|r2;				// r7.l now contains position and length for DEPOSIT
	
	r2 = DEPOSIT(r1, r7);
	
	w[p0] =r2; ssync;

	cli r4;
	idle;
	sti r4;

	r2 = 0xD;					// check for max VLEV of 0xD
	cc = r2 == r0;				// compare to current VLEV (backed up in r0)
	if !cc jump light_led_pfA;	// if NOT at max VLEV, we're done
		[--SP] = rets;			// otherwise, we wrapped from min to max...
			call freq_change;	// ...which means it is now safe to adjust...
		rets = [SP++];			// ...the CCLK back to VCO/1
		
//  Toggle LED after write to VR_CTL
//  flash A csio reg (portA,B data reg)
light_led_pfA:
	P3.H = hi(flashA_csio);
	P3.L = lo(flashA_csio) + portB_data_out;
	R0 = B[P3] (z); 		// Read LEDs
	BITTGL (R0, 3);			// Toggle LED7
	B[P3] = R0;

 	ASTAT       = [SP++];
	(r7:0,p5:0) = [SP++];
	RTI;
PFA_HANDLER.end: nop;

/************************************************************************
 * PROGRAMMABLE FLAG B INTERRUPT HANDLER								*
 *																		*
 * Acknowledges the PFB (SW5) interrupt and modifies core regulator		*
 * voltage by incrementing the VLEV field of VR_CTL.  If VLEV is at the	*
 * maximum, it will wrap to the lowest level of 0.70 V.					*
 ************************************************************************/
PFB_HANDLER:
	[--SP] = (r7:0,p5:0);
	[--SP] = ASTAT;

	p5.h = hi(FIO_FLAG_C);	p5.l = lo(FIO_FLAG_C);
	r0 = 0x0200 (z);			// PF9
	w[p5] = r0.l;				// Acknowledge PFB Interrupt

	p0.h = hi(VR_CTL);	p0.l = lo(VR_CTL);
	r1 = w[p0](z);				// Read VR_CTL into scene_reg for EXTRACT

	r2 = 0x0404;				// pattern_reg for EXTRACT, start bit is bit 4
	r7 = EXTRACT(r1, r2.l)(z);	// length of field is 4 bits, R7 = VLEV field
	
	r2 = 0xD(z);				// Maximum VLEV rating
	
	CC = r7 == r2;
	IF !CC JUMP Not_Maximal;	// if we are at the max, we wrap to the min
	
	r7 = 0x2(z);				// Set VLEV to MIN Value of (0x3 - 1)
	
Not_Maximal:
	r7 += 1;				// Increment VLEV field
							// If Max, setting to 0x2 above wraps to min VLEV of 0x3

	r2 = 0x8(z);			// VLEV = 8 == ~1V, change processor speed
	
	cc = r7 == r2;			// CC contains check for crossing 1V threshold
	
	if !cc jump check_wrap;	// if not crossing 1V, no need to call freq_change

		[--SP] = rets;
			call freq_change;
		rets = [SP++];
		
check_wrap:
	r2 = 0x3(z);			// check for wrap to min voltage
	cc = r7 == r2;			// if we wrapped to low voltage
	if !cc jump post_freqB;	// if we didn't wrap, no need to call freq_change

		[--SP] = rets;			// otherwise, we must go to VCO/2 before writing...
			call freq_change;	// ...the min VLEV = 3 to VR_CTL
		rets = [SP++];

post_freqB:
	// R1 contains current VR_CTL setting and will be the backgnd_reg for the DEPOSIT
	r7 <<= 16;				// shift r7.l into r7.h
							// r7 is foregnd_reg for DEPOSIT, r7.h is bit-field
	r2 = 0x0404(z);			// Position = Bit 4, Length = 4 Bits
	
	r7 = r7|r2;				// r7.l now contains position and length for DEPOSIT
	
	r2 = DEPOSIT(r1, r7);
	
	w[p0] =r2; ssync;

	cli r4;
	idle;
	sti r4;
	
//  Toggle LED after write to VR_CTL
//  flash A csio reg (portA,B data reg)
	P3.H = hi(flashA_csio);
	P3.L = lo(flashA_csio) + portB_data_out;
	R0 = B[P3] (z); 		// Read LEDs
	BITTGL (R0, 3);			// Toggle LED7
	B[P3] = R0;

 	ASTAT       = [SP++];
	(r7:0,p5:0) = [SP++];
	RTI;
PFB_HANDLER.end: nop;
	
/************************************************************************
 * FREQUENCY CHANGING													*
 *																		*
 * This subroutine is called by each of the above ISRs if the change to	*
 * the VLEV field results in the voltage becoming too high or too low	*
 * for the current core clock frequency.  This simply switches between	*
 * CCLK = VCO/1 and CCLK = VCO/2.										*
 ************************************************************************/
freq_change:
	p4.h = hi(PLL_DIV);	p4.l = lo(PLL_DIV);
	r2 = w[p4] (z);
	BITTGL (r2, 4);			// if CSEL = 00, CCLK = VCO/1
	w[p4] = r2;				// if CSEL = 01, CCLK = VCO/2
	ssync;
	
	RTS;