/*
  SFBPower.cpp - The power handling routines for the SFB.
  Copyright (C) 2009 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

  $Id$
*/

#include "SFBPower.h"
#include "SFBAssert.h"          /* For BUG, etc */
#include "SFBWiring.h"          /* For pinMode */
#include "SFBPins.h"            /* For FACE_VOUT_ENABLE_PIN */
#include "SFBBootBlock.h"       /* For getBootBlockPowerIn, etc */

#include "LPC23xx.h"		/* LPC23xx/24xx Peripheral Registers	*/

xcalib adcCalibs[VSENSE_IDX_COUNT] IN_BODYRAM_BSS;

u16 adcLatestSample[ADC_IDX_COUNT] IN_BODYRAM_BSS;

u16 adcSmooth[ADC_IDX_COUNT] IN_BODYRAM_BSS;

void setPowerSensingEnabled(bool enable) {
  for (u32 i = NORTH_ISENSE_ADC_IDX; i < ADC_IDX_COUNT; ++i)
    adcLatestSample[i] = enable?0:POWER_SENSING_FLAG_CODE;
}

void calibData::initXcalib(xcalib & x) {
  x.init(getCutoffCt(), getSlopeScaledMvPerCt(0), getSlopeScaledMvPerCt(1), getInterceptMv());
}
s32 calibData::getSlopeScaledMvPerCt(u32 which) { 
  API_ASSERT_MAX(which,2);
  if (which==0)
    return getValue(SLOPE0_INDEX,SLOPE0_BITS,SLOPE0_CENTER,SLOPE0_SCALED_MV_PER_CT_PER_VALUE,SLOPE0_OFFSET);
  else 
    return getValue(SLOPE1_INDEX,SLOPE1_BITS,SLOPE1_CENTER,SLOPE1_SCALED_MV_PER_CT_PER_VALUE,SLOPE1_OFFSET);
}

void initxcalibsFromGene(u32 gene[VSENSE_IDX_COUNT]) {
  for (u32 i = 0; i < VSENSE_IDX_COUNT; ++i) {
    calibData cdata;
    cdata.data = gene[i];
    cdata.initXcalib(adcCalibs[i]);
  }
}

void unpackCalibrationData(u32 gene[VSENSE_IDX_COUNT], u8 bytes[VOLTAGE_CALIBRATION_SIZE_BYTES]) {
  u32 byteIdx = 0;
  for (u32 i = 0; i < VSENSE_IDX_COUNT; ++i) {
    u32 n = 0;
    n |= bytes[byteIdx++]<<16;
    n |= bytes[byteIdx++]<< 8;
    n |= bytes[byteIdx++]<< 0;
    gene[i] = n;
  }
}

// We load calibration data on first use, rather than during
// initialization, primarily to save space in boot3, which doesn't
// call faceVoltage or railVoltage, and so (this way) doesn't have to
// link any of the calibration stuff.
static u8 calibCheck IN_BODYRAM_BSS; 

void invalidateCalibrationData() {
  calibCheck = 0;
}

void loadCalibrationData() {
  u8 cdata[VOLTAGE_CALIBRATION_SIZE_BYTES];
  u32 gene[VSENSE_IDX_COUNT];

  getBootBlockVoltageCalibrationData(cdata);
  unpackCalibrationData(gene, cdata);

  bool useDefault = true;
  for (u32 i = 0; i < VSENSE_IDX_COUNT; ++i) {
    adcCalibs[i].init();
    if (gene[i]) useDefault = false;
  }
  if (useDefault) return;
  for (u32 i = 0; i < VSENSE_IDX_COUNT; ++i) {
    calibData cal;
    cal.data = gene[i];
    cal.initXcalib(adcCalibs[i]);
  }
  calibCheck = 1;
}



/**
 * Setup the ADC converters; check for voltage calibration data.
 */
void initADC(void)
{ 
  u32 val;
  PCONP |= PCAD;     /* heat 'em up */

  /*              PCLK_ADC */
  val = PCLKSEL0&~(3<<24);
  PCLKSEL0 = val| (3<<24); /* PCLK == CCLK/8 == 9MHz */

  /*              AD0.0   AD0.1   AD0.2   AD0.3 */
  val = PINSEL1&~((3<<14)|(3<<16)|(3<<18)|(3<<20));
  PINSEL1 = val |((1<<14)|(1<<16)|(1<<18)|(1<<20));
  
  /*              AD0.4   AD0.5 */
  val = PINSEL3&~((3<<28)|(3<<30));
  PINSEL3 = val |((3<<28)|(3<<30));
  
  AD0CR =                       /* Set up ADC control register */
    /* All channels */  (0x3F <<  0) |
    /* CLKDIV==2 */     (   1 <<  8) |
    /* Burst mode */    (   1 << 16) |
    /* CLKS 11 */       (   0 << 17) |
    /* Power on */      (   1 << 21) | 
    /* No start */      (   0 << 24)
    ;
}

int powerDraw() {
  int res = 0;
  for (u32 f = NORTH; f <= WEST; ++f) 
    res += powerDraw(f);
  return res;
}

int powerDraw(u32 face) {
  API_ASSERT_VALID_FACE(face);
  return -1800*(((long) adcSmooth[NORTH_ISENSE_ADC_IDX+face]>>ADC_SCALE_BITS)-556)/467;
}

int rawPower(u32 face) {
  API_ASSERT_VALID_FACE(face);
  return adcSmooth[NORTH_ISENSE_ADC_IDX+face]>>ADC_SCALE_BITS;
}


s32 faceVoltage(u32 face, bool smoothed) {
  API_ASSERT_VALID_FACE(face);
  if (!calibCheck) loadCalibrationData();
  xcalib & xc = adcCalibs[face];
  u16 sample = getADCSample(face,smoothed);
  u32 piece = (sample <= (xc.cutoff<<ADC_SCALE_BITS))?0:1;
  return mapADCCounts(sample,xc.slope[piece],xc.intercept[piece]);
}

s32 railVoltage(bool smoothed) {
  if (!calibCheck) loadCalibrationData();
  u16 sample = getADCSample(VEE_VSENSE_ADC_IDX,smoothed);
  xcalib & xc = adcCalibs[VEE_VSENSE_ADC_IDX];
  u32 piece = (sample <= (xc.cutoff<<ADC_SCALE_BITS))?0:1;
  return mapADCCounts(sample,xc.slope[piece],xc.intercept[piece]);
}

int rawVoltage() {              // DEPRECATED
  return railVoltage();
}

void powerOut(u32 face, int on) {

  int pin = pinInFace(FACE_VOUT_ENABLE_PIN,face);

  pinMode(pin, REFLEX_MODE);        /* Take back control of the power pin */ 

  if (on) {

    pinMode(pin,INPUT);       /* To activate a Vout: Make an input..*/
    reflexSetPinResistor(pin, 0x2); /* ..and disable the pull up */

  } else {

    //    reflexSetPinResistor(pin, 0x0);   /* (Reestablish pull up resistor?) */
    pinMode(pin,OUTPUT);      /* To deactivate a Vout: Make it an output..*/
    reflexDigitalWrite(pin, LOW);   /* ..and ground it */

  }
}

/** Control inbound power -- NOTE THIS CODE IS UNTESTED! */
void powerIn(u32 face, int on) {

  int pin = pinInFace(FACE_VIN_ENABLE_PIN,face);

  pinMode(pin, REFLEX_MODE);        /* Take back control of the power pin */ 

  if (on) {

    pinMode(pin,INPUT);       /* To activate a Vin: Make an input..*/
    reflexSetPinResistor(pin, 0x2); /* ..and disable the pull up */

  } else {

    //    reflexSetPinResistor(pin, 0x0);   /* (Reestablish pull up resistor?) */
    pinMode(pin,OUTPUT);      /* To deactivate a Vin: Make it an output..*/
    reflexDigitalWrite(pin, LOW);   /* ..and ground it */

  }
}

void initPowerOuts() {
  for (u32 f = NORTH; f <= WEST; ++f) 
    powerOut(f,getBootBlockPowerOut(f));
  for (u32 f = NORTH; f <= WEST; ++f) 
    powerIn(f,getBootBlockPowerIn(f));
}
