
#include "util.h"
#define _SUPPRESS_PLIB_WARNING
#define _DISABLE_OPENADC10_CONFIGPORT_WARNING
#include <plib.h>

__attribute__((mips16)) int get_SYSCLK() {
    int rate;
    // calculate fundamental rate
    switch(OSCCONbits.COSC) {
        case 0: // FRC
        case 1: // FRCPLL
        case 6: // FRC/16
        case 7: // FRC/OSCCON.FRCDIV
            if( OSCTUNbits.TUN < 32 )
                rate = 8000000 * (OSCTUNbits.TUN+248) / 248;
            else
                rate = 8000000 * (OSCTUNbits.TUN+192) / 256;
            break;
        case 2: // POSC
        case 3: // POSCPLL
            rate = EXT_CLOCK_XTAL_FREQ;
            break;
        case 4: // SOSC
            rate = 32768;
            break;
        case 5: // LPRC
            rate = 31250;
            break;
    }
    // divide if necessary
    switch(OSCCONbits.COSC) {
        case 6: // FRC/16
            rate >>= 4;
            break;
        case 7: // FRC/OSCCON.FRCDIV
            rate >>= (OSCCONbits.FRCDIV < 7 ? OSCCONbits.FRCDIV : 8);
            break;
    }
    // apply PLL if necessary
    switch(OSCCONbits.COSC) {
        case 1: // FRCPLL
        case 3: // POSCPLL
            rate = (rate / (DEVCFG2bits.FPLLIDIV < 6 ? DEVCFG2bits.FPLLIDIV+1 : (DEVCFG2bits.FPLLIDIV-1)*2) * (OSCCONbits.PLLMULT < 7 ? OSCCONbits.PLLMULT+15 : 24)) >> (DEVCFG2bits.FPLLODIV < 7 ? DEVCFG2bits.FPLLODIV : 8);
            break;
    }
    return rate;
}
__attribute__((mips16)) int get_PBCLK() {
    return get_SYSCLK() >> OSCCONbits.PBDIV;
}
__attribute__((mips16)) char hexdig(int value) {
    return value < 10 ? '0' + value : 'A' + value - 10;
}
