/*                                             -*- mode:C++; fill-column:100  -*-
  clock.c Support for changing/setting the system clock speed
  Copyright (C) 2010 The Regents of the University of New Mexico.  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA
*/

#include "clock.h"
#include "SFBMacros.h"          /* For IN_BODYRAM_BSS */
#include "SFBAssert.h"          /* For API_ASSERT_EQUAL, etc */
#include "SFBHWMisc.h"
#include "lpc/LPC23xx.h"

#define FOSC 12000000

const ClockConfiguration clockParms[CSC_MAX] = {
  {  0,  0,  1, 1, 1, 12 },     /* 12MHz: PLL bypassed, M and N not used, /1 PCLK 12MHz */
  { 12,  1, 12, 2, 2, 12 },     /* 24MHz: 12MHz*12/1==Fcco=288MHz/12 == 24MHz /2 PCLK 12MHz */
  { 12,  1,  8, 2, 2, 18 },     /* 36MHz: 12MHz*12/1==Fcco=288MHz/8 == 36MHz /2 PCLK 18MHz */
  { 12,  1,  6, 3, 4, 12 },     /* 48MHz: 12MHz*12/1==Fcco=288MHz/6 == 48MHz /4 PCLK 12MHz */
  { 15,  1,  6, 4, 4, 15 },     /* 60MHz: 12MHz*15/1==Fcco=360MHz/6 == 60MHz /4 PCLK 15MHz */
  { 12,  1,  4, 4, 4, 18 }      /* 72MHz: 12MHz*12/1==Fcco=288MHz/4 == 72MHz /4 PCLK 18MHz */
};

static u8 currentSpeedCode IN_BODYRAM_BSS;

u8 getCCLKCode() { return currentSpeedCode; }

u32 getCCLK() {
  return (currentSpeedCode-CSC_12MHZ+1)*FOSC;
}

const ClockConfiguration * getCurrentClockConfiguration() {
  return getClockConfigurationForCode(currentSpeedCode);
}

const ClockConfiguration * getClockConfigurationForCode(u32 code) {
  API_ASSERT(code >= CSC_MIN_MHZ && code <= CSC_MAX_MHZ, E_API_ARGUMENT);
  return & clockParms[code-CSC_12MHZ];
}

extern void debugRGB(int rgbBits, int wait);

/* unsafeSetClockSpeed is meant to be called only from setClockSpeed.
   It is 'unsafe' for at least two reasons: 

   (1) It changes the clock speed but doesn't compensate any of the
       peripherals, which therefore can end up all misconfigured, and

   (2) Because certain speed changes -- in particular, large
       accelerations -- seem to cause the processor to lock up during
       the feed sequence to connect the PLL.
 */
static void unsafeSetClockSpeed(u32 code) {
  const ClockConfiguration * cp;
  
  if (code > CSC_72MHZ)         /* CSC_MAX, CSC_AUTO, and bogus codes -> 72MHz.. */
    code = CSC_72MHZ;

  currentSpeedCode = code;      /* Stash new speed */
  cp = getCurrentClockConfiguration(); /* Get its associated config info */

  /* First, disconnect PLL if connected */
  if (PLLSTAT & PLLSTAT_PLLC) {
    PLLCON = PLLCON_PLLE;       /* Enabled PLL, disconnected ( PLLC = 0 )*/
    PLLFEED = 0xaa;
    PLLFEED = 0x55;
  }

  /* Second, disable PLL */
  PLLCON  = 0;        /* Disable PLL, disconnected */
  PLLFEED = 0xaa;
  PLLFEED = 0x55;

  /* Third, switch to 4MHz internal RC oscillator */
  CLKSRCSEL = CLKSRC_INT_RC;   

  /* Fourth, set clock divider to 1, to get 4MHz during setup */
  CCLKCFG = (1-1)<<0;

  /* Fifth, disable main oscillator, in the spirit of total restartness */
  SCS &= ~SCS_OSCEN;  

  /* Sixth, reenable main OSC */
  SCS |= SCS_OSCEN;  
  while ((SCS & SCS_OSCSTAT) == 0) { /* Wait until clock OSC is usable */ }

  /* Seventh, switch to 12MHz main oscillator */
  CLKSRCSEL = CLKSRC_MAIN_OSC;   

  /* Eighth, set up PLL if we're using it */
  if (code != CSC_12MHZ) {      /* Need the PLL for everything but 12MHz */
    u32 config, readback;

    config = 
      ((cp->pllMulM - 1) << 0) | /* Set up PLL multiplier */
      ((cp->pllDivN - 1) << 16); /* and PLL pre-divider */

    PLLCFG =  config;           /* Configure PLL */
    PLLFEED = 0xaa;             
    PLLFEED = 0x55;
	
    PLLCON = PLLCON_PLLE;       /* Enable PLL, disconnected ( PLLC = 0 ) */
    PLLFEED = 0xaa;
    PLLFEED = 0x55;
	
    CCLKCFG = (cp->clkDiv - 1) << 0; /* Set clock divider */
  
    while ((PLLSTAT & PLLSTAT_PLOCK)==0) { /* block until PLL lock */ }

    readback = PLLSTAT & (((32768 - 1) << 0) | ((256 - 1) << 16));

    while (readback != config) { /* hang if mismatch */ }
	
    PLLCON = (PLLCON_PLLE|PLLCON_PLLC);  /* enable and connect */

    /*debugRGB(4,0);  Believe it or not, we can die.. */
    PLLFEED = 0xaa;
    PLLFEED = 0x55;
    /*debugRGB(2,0);  ..between these two 'debugRGB' calls. */

    while ((PLLSTAT & PLLSTAT_PLLC) == 0) { /* block until PLL connected */ }
  }

  /* Ninth, setup & enable the MAM */
  MAMCR = MAMCR_OFF;
  MAMTIM = cp->mamtim;
  MAMCR = MAMCR_FULL;

  /* Tenth, default the PCLKs */
  {
    u32 bits;
    switch (cp->pclkDiv) {
    case 1: bits = 0x55555555; break; /* /1 */
    case 2: bits = 0xAAAAAAAA; break; /* /2 */
    case 4: bits = 0x00000000; break; /* /4 */
    case 8: bits = 0xFFFFFFFF; break; /* /8 */
    default: API_BUG(E_BUG_INCONSISTENT_STATE);
    }
    PCLKSEL0 = bits;
    PCLKSEL1 = bits;
  }
}

extern void reinitPeripherals(); /* In startup.cpp.. */

static ClockSpeedChangeCallback speedChangeCallback IN_BODYRAM_BSS;

ClockSpeedChangeCallback setClockSpeedChangeCallback(ClockSpeedChangeCallback callback) {
  ClockSpeedChangeCallback ret = speedChangeCallback;
  speedChangeCallback = callback;
  return ret;
}

void setClockSpeed(u32 code) {
  u32 status;
  if (code == CSC_UNSET || code > CSC_72MHZ)
    code =  CSC_72MHZ;

  if (currentSpeedCode == code) /* Anything to do? */
    return;

  suspendInterrupts(status);    /* Interrupts off */

  if (code < currentSpeedCode || currentSpeedCode == CSC_UNSET) /* Downward (and 1st) PLL speed changes seem safe */
    unsafeSetClockSpeed(code);
  else
    while (code > currentSpeedCode) /* But upward ones seem to lock up if we change it too fast  */
      unsafeSetClockSpeed(currentSpeedCode+1); /* So, go by 12MHz jumps */

  reinitPeripherals();          /* Let periperals deal with the possibly changed PCLK */

  if (speedChangeCallback)      /* Let sketch level deal with the new CCLK  */
    (*speedChangeCallback)(getCCLK()); /* however it wishes */

  restoreInterrupts(status);    /* Resume interrupts */

}

