#include "color.h"
#include "display.h"
#include "display_syscalls.h"
#include "keyboard.hpp"
#include "string.h"
#include "keyboard_syscalls.h"
#include "system.h"
#include "stdlib.h"
#include "stdio.h"
#include "BFILE_syscalls.h"
#include "rtc.h"



void change_freq(int mult) { 
   __asm__( 
      "mov r4, r0\n\t" 
      "and #0x3F, r0\n\t"  
      "shll16 r0\n\t"  
      "shll8 r0\n\t"  
      "mov.l frqcr, r1\n\t"   
      "mov.l pll_mask, r3\n\t"   
      "mov.l @r1, r2\n\t"   
      "and r3, r2\n\t"   
      "or r0, r2\n\t"  
      "mov.l r2, @r1\n\t"  
      "mov.l frqcr_kick_bit, r0\n\t"  
      "mov.l @r1, r2\n\t" 
      "or r0, r2\n\t" 
      "rts\n\t" 
      "mov.l r2, @r1\n\t" 
      ".align 4\n\t" 
      "frqcr_kick_bit: .long 0x80000000\n\t" 
      "pll_mask: .long 0xC0FFFFFF\n\t"  
      "frqcr: .long 0xA4150000\n\t" 
   ); 
}

void change_iclk(int div) { 
   __asm__( 
      "mov r4, r0\n\t" 
      "and #0x0F, r0\n\t"  
      "shll16 r0\n\t"  
      "shll2 r0\n\t"  
	  "shll2 r0\n\t" 
      "mov.l frqcr_iclk, r1\n\t"   
      "mov.l iclk_mask, r3\n\t"   
      "mov.l @r1, r2\n\t"   
      "and r3, r2\n\t"   
      "or r0, r2\n\t"  
      "mov.l r2, @r1\n\t"  
      "mov.l frqcr_kick_bit_iclk, r0\n\t"  
      "mov.l @r1, r2\n\t" 
      "or r0, r2\n\t" 
      "rts\n\t" 
      "mov.l r2, @r1\n\t" 
      ".align 4\n\t" 
      "frqcr_kick_bit_iclk: .long 0x80000000\n\t" 
      "iclk_mask: .long 0xFF0FFFFF\n\t"  
      "frqcr_iclk: .long 0xA4150000\n\t" 
   ); 
}

void change_pclk(int div) { 
   __asm__( 
      "mov r4, r0\n\t" 
      "and #0x0F, r0\n\t"  
      "shll16 r0\n\t"  
      "shll2 r0\n\t"  
	  "shll2 r0\n\t" 
      "mov.l frqcr_pclk, r1\n\t"   
      "mov.l pclk_mask, r3\n\t"   
      "mov.l @r1, r2\n\t"   
      "and r3, r2\n\t"   
      "or r0, r2\n\t"  
      "mov.l r2, @r1\n\t"  
      "mov.l frqcr_kick_bit_pclk, r0\n\t"  
      "mov.l @r1, r2\n\t" 
      "or r0, r2\n\t" 
      "rts\n\t" 
      "mov.l r2, @r1\n\t" 
      ".align 4\n\t" 
      "frqcr_kick_bit_pclk: .long 0x80000000\n\t" 
      "pclk_mask: .long 0xFFFFFFF0\n\t"  
      "frqcr_pclk: .long 0xA4150000\n\t" 
   ); 
}

void change_bclk(int div) { 
   __asm__( 
      "mov r4, r0\n\t" 
      "and #0x0F, r0\n\t"  
      "shll16 r0\n\t"  
      "shll2 r0\n\t"  
	  "shll2 r0\n\t" 
      "mov.l frqcr_bclk, r1\n\t"   
      "mov.l bclk_mask, r3\n\t"   
      "mov.l @r1, r2\n\t"   
      "and r3, r2\n\t"   
      "or r0, r2\n\t"  
      "mov.l r2, @r1\n\t"  
      "mov.l frqcr_kick_bit_bclk, r0\n\t"  
      "mov.l @r1, r2\n\t" 
      "or r0, r2\n\t" 
      "rts\n\t" 
      "mov.l r2, @r1\n\t" 
      ".align 4\n\t" 
      "frqcr_kick_bit_bclk: .long 0x80000000\n\t" 
      "bclk_mask: .long 0xFFFFF0FF\n\t"  
      "frqcr_bclk: .long 0xA4150000\n\t" 
   ); 
}

void change_sclk(int div) { 
   __asm__( 
      "mov r4, r0\n\t" 
      "and #0x0F, r0\n\t"  
      "shll16 r0\n\t"  
      "shll2 r0\n\t"  
	  "shll2 r0\n\t" 
      "mov.l frqcr_sclk, r1\n\t"   
      "mov.l sclk_mask, r3\n\t"   
      "mov.l @r1, r2\n\t"   
      "and r3, r2\n\t"   
      "or r0, r2\n\t"  
      "mov.l r2, @r1\n\t"  
      "mov.l frqcr_kick_bit_sclk, r0\n\t"  
      "mov.l @r1, r2\n\t" 
      "or r0, r2\n\t" 
      "rts\n\t" 
      "mov.l r2, @r1\n\t" 
      ".align 4\n\t" 
      "frqcr_kick_bit_sclk: .long 0x80000000\n\t" 
      "sclk_mask: .long 0xFFFF0FFF\n\t"  
      "frqcr_sclk: .long 0xA4150000\n\t" 
   ); 
}


#define PLL_28x 0b011011 // 101.5 MHz
#define PLL_26x 0b011001 // 94.3 MHz 
#define PLL_24x 0b010111 // 87 MHz 
#define PLL_20x 0b010011 // 72.5 MHz 
#define PLL_18x 0b010001 // 65.25 MHz 
#define PLL_16x 0b001111 // 58 MHz (NORMAL SPEED) 
#define PLL_15x 0b001110 // 54.37 MHz 
#define PLL_12x 0b001011 // 43.5 MHz 
#define PLL_8x  0b000111 // 29 MHz 
#define PLL_6x  0b000101 // 21.75 MHz 
#define PLL_4x  0b000011 // 14.5 MHz 
#define PLL_3x  0b000010 // 10.8 MHz 
#define PLL_2x  0b000001 // 7.25 MHz 
#define PLL_1x  0b000000 // 3.6 MHz

#define ICLK_2 0b0000
#define ICLK_3 0b0001
#define ICLK_4 0b0010
#define ICLK_6 0b0011
#define ICLK_8 0b0100
#define ICLK_12 0b0101
#define ICLK_16 0b0110
#define ICLK_24 0b0111
#define ICLK_32 0b1000
#define ICLK_36 0b1001
#define ICLK_48 0b1010
#define ICLK_72 0b1011

const int PLLs[] = {PLL_1x, PLL_2x, PLL_3x, PLL_4x, PLL_6x, PLL_8x, PLL_12x, PLL_15x, PLL_16x, PLL_18x, PLL_20x, PLL_24x, PLL_26x, PLL_28x};
const int DIVs[] = {ICLK_2, ICLK_3, ICLK_4, ICLK_6, ICLK_8, ICLK_12, ICLK_16, ICLK_24, ICLK_32, ICLK_36, ICLK_48, ICLK_72};

int main(void) {
	int key = KEY_PRGM_NONE;
	char*cur = 0x00000000;
	volatile unsigned int*FRQCR = (unsigned int*) 0xA4150000;
	void update(void) {
		Bdisp_AllClr_VRAM();
		PrintXY(2, 1, "  Operating Frequency", TEXT_MODE_NORMAL, TEXT_COLOR_GREEN);
		PrintXY(3, 2, "  Freq:", TEXT_MODE_NORMAL, TEXT_COLOR_BLUE);
		PrintXY(3, 3, "  ICLK:", TEXT_MODE_NORMAL, TEXT_COLOR_BLUE);
		PrintXY(3, 4, "  PCLK:", TEXT_MODE_NORMAL, TEXT_COLOR_BLUE);
		PrintXY(3, 5, "  BCLK:", TEXT_MODE_NORMAL, TEXT_COLOR_BLUE);
		PrintXY(3, 6, "  SCLK:", TEXT_MODE_NORMAL, TEXT_COLOR_BLUE);
		PrintXY(1, 7, "  U/D - FRQ, L/R - ICLK", TEXT_MODE_NORMAL, TEXT_COLOR_GREEN);
		int warning = 0;
		switch((*FRQCR & 0x3F000000) >> 24) {
			case PLL_28x:
				cur = "  101.5 MHz";
				warning = 1;
				break;
			case PLL_26x:
				cur = "  94.3 MHz";
				break;
			case PLL_24x:
				cur = "  87 MHz";
				break;
			case PLL_20x:
				cur = "  72.5 MHz";
				break;
			case PLL_18x:
				cur = "  65.3 MHz";
				break;
			case PLL_16x:
				cur = "  58 MHz (NORM)";
				break;
			case PLL_15x:
				cur = "  54.4 MHz";
				break;
			case PLL_12x:
				cur = "  43.5 MHz";
				break;
			case PLL_8x:
				cur = "  29 MHz";
				break;
			case PLL_6x:
				cur = "  21.7 MHz";
				break;
			case PLL_4x:
				cur = "  14.5 MHz";
				break;
			case PLL_3x:
				cur = "  10.8 MHz";
				break;
			case PLL_2x:
				cur = "  7.25 MHz";
				break;
			case PLL_1x:
				cur = "  3.6 MHz";
				break;
			default:
				cur = "  INVALID";
				break;
		}
		PrintXY(9, 2, cur, TEXT_MODE_NORMAL, warning==1?TEXT_COLOR_RED:TEXT_COLOR_BLACK);
		switch((*FRQCR & 0x00F00000) >> 20) {
			case ICLK_72:
				cur = "  1/72";
				break;
			case ICLK_48:
				cur = "  1/48";
				break;
			case ICLK_36:
				cur = "  1/36";
				break;
			case ICLK_32:
				cur = "  1/32";
				break;
			case ICLK_24:
				cur = "  1/24";
				break;
			case ICLK_16:
				cur = "  1/16";
				break;
			case ICLK_12:
				cur = "  1/12";
				break;
			case ICLK_8:
				cur = "  1/8";
				break;
			case ICLK_6:
				cur = "  1/6";
				break;
			case ICLK_4:
				cur = "  1/4";
				break;
			case ICLK_3:
				cur = "  1/3 (NORM)";
				break;
			case ICLK_2:
				cur = "  1/2";
				warning += 2;
				break;
			default:
				cur = "  INVALID";
				break;
		}
		PrintXY(9, 3, cur, TEXT_MODE_NORMAL, warning>=2?TEXT_COLOR_YELLOW:TEXT_COLOR_BLACK);
		switch(*FRQCR & 0xF) {
			case ICLK_72:
				cur = "  1/72";
				break;
			case ICLK_48:
				cur = "  1/48";
				break;
			case ICLK_36:
				cur = "  1/36";
				break;
			case ICLK_32:
				cur = "  1/32";
				break;
			case ICLK_24:
				cur = "  1/24";
				break;
			case ICLK_16:
				cur = "  1/16";
				break;
			case ICLK_12:
				cur = "  1/12";
				break;
			case ICLK_8:
				cur = "  1/8";
				break;
			case ICLK_6:
				cur = "  1/6 (NORM)";
				break;
			case ICLK_4:
				cur = "  1/4";
				break;
			case ICLK_3:
				cur = "  1/3";
				break;
			case ICLK_2:
				cur = "  1/2";
				break;
			default:
				cur = "  INVALID";
				break;
		}
		PrintXY(9, 4, cur, TEXT_MODE_NORMAL, TEXT_COLOR_BLACK);
		switch((*FRQCR & 0xF00) >> 8) {
			case ICLK_72:
				cur = "  1/72";
				break;
			case ICLK_48:
				cur = "  1/48";
				break;
			case ICLK_36:
				cur = "  1/36";
				break;
			case ICLK_32:
				cur = "  1/32";
				break;
			case ICLK_24:
				cur = "  1/24";
				break;
			case ICLK_16:
				cur = "  1/16";
				break;
			case ICLK_12:
				cur = "  1/12";
				break;
			case ICLK_8:
				cur = "  1/8";
				break;
			case ICLK_6:
				cur = "  1/6";
				break;
			case ICLK_4:
				cur = "  1/4 (NORM)";
				break;
			case ICLK_3:
				cur = "  1/3";
				break;
			case ICLK_2:
				cur = "  1/2";
				break;
			default:
				cur = "  INVALID";
				break;
		}
		PrintXY(9, 5, cur, TEXT_MODE_NORMAL, TEXT_COLOR_BLACK);
		switch((*FRQCR & 0xF000) >> 12) {
			case ICLK_72:
				cur = "  1/72";
				break;
			case ICLK_48:
				cur = "  1/48";
				break;
			case ICLK_36:
				cur = "  1/36";
				break;
			case ICLK_32:
				cur = "  1/32";
				break;
			case ICLK_24:
				cur = "  1/24";
				break;
			case ICLK_16:
				cur = "  1/16";
				break;
			case ICLK_12:
				cur = "  1/12";
				break;
			case ICLK_8:
				cur = "  1/8";
				break;
			case ICLK_6:
				cur = "  1/6";
				break;
			case ICLK_4:
				cur = "  1/4 (NORM)";
				break;
			case ICLK_3:
				cur = "  1/3";
				break;
			case ICLK_2:
				cur = "  1/2";
				break;
			default:
				cur = "  INVALID";
				break;
		}
		PrintXY(9, 6, cur, TEXT_MODE_NORMAL, TEXT_COLOR_BLACK);
		if(warning == 1 || warning == 3) {
			PrintXY(1, 8, "  UNSAFE FREQ/DIV VALUE", TEXT_MODE_NORMAL, TEXT_COLOR_RED);
		} else if(warning == 2) {
			PrintXY(1, 8, "  TROUBLESOME DIV VALUE", TEXT_MODE_NORMAL, TEXT_COLOR_YELLOW);
		}
		Bdisp_PutDisp_DD();
	}
	
	while(1) {
		update();
		switch (key) {
			case KEY_CTRL_UP:
				for(int i = 0; i < 13; i++) {
					if(((*FRQCR & 0x3F000000) >> 24) == PLLs[i]) {
						change_freq(PLLs[i+1]);
						update();
						break;
					}
				}
				break;
			case KEY_CTRL_DOWN:
				for(int i = 1; i < 14; i++) {
					if(((*FRQCR & 0x3F000000) >> 24) == PLLs[i]) {
						change_freq(PLLs[i-1]);
						update();
						break;
					}
				}
				break;
			case KEY_CTRL_LEFT:
				for(int i = 0; i < 2; i++) {
					if(((*FRQCR & 0x00F00000) >> 20) == DIVs[i]) {
						change_iclk(DIVs[i+1]);
						update();
						break;
					}
				}
				break;
			case KEY_CTRL_RIGHT:
				for(int i = 1; i < 3; i++) {
					if(((*FRQCR & 0x00F00000) >> 20) == DIVs[i]) {
						change_iclk(DIVs[i-1]);
						update();
						break;
					}
				}
				break;
			default:
				break;
		}
		GetKey(&key);
		update();
	}
 
	return 0;
}