/*
  SFBHWPWM.cpp - Support for h/w-based Pulse Width Modulation
  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 "SFBHWPWM.h"
#include "lpc/LPC23xx.h" // For hardware register definitions 
#include "SFBPins.h"     // For BODY_RGB_RED_PIN, etc
#include "SFBAssert.h"   // For API_ASSERT_MAX, etc
#include "SFBWiring.h"   // For pinMode

/**
   The per-PWM-capable SFB pin information needed to configure PWM on
   that pin.
 */
struct PWMInfo {
  u8 sfbPin;    ///< The SFB pin number for this pin
  u8 func;      ///< The function to store in the appropriate PINSEL, for PWM
  u8 chnl;      ///< The PWM channel this pin operates on
};

const PWMInfo pwmInfo[] = {
  { BODY_RGB_RED_PIN,   1, 2 },        //PWM_BODY_RGB_RED_PIN    
  { BODY_RGB_GREEN_PIN, 1, 1 },        //PWM_BODY_RGB_GREEN_PIN  
  { BODY_RGB_BLUE_PIN,  1, 3 },        //PWM_BODY_RGB_BLUE_PIN   
  { NORTH_LED_PIN,      1, 4 },        //PWM_NORTH_LED_PIN
  { SOUTH_LED_PIN,      1, 5 },        //PWM_SOUTH_LED_PIN
  { EAST_LED_PIN,       1, 6 },        //PWM_EAST_LED_PIN
  { WEST_D0_PIN,        2, 2 },        //PWM_WEST_D0_PIN
  { WEST_D2_PIN,        2, 4 },        //PWM_WEST_D2_PIN
  { WEST_D3_PIN,        2, 3 },        //PWM_WEST_D3_PIN
  { EAST_D1_PIN,        2, 5 },        //PWM_EAST_D1_PIN
  { EAST_D3_PIN,        2, 6 }         //PWM_EAST_D3_PIN          
};
#define MAX_PWMS (sizeof(pwmInfo)/sizeof(pwmInfo[0]))

int PWMFindIndex(int sfbPin) {
  API_ASSERT_VALID_PIN(sfbPin);
  for (u32 i = 0; i < MAX_PWMS; ++i) {
    const PWMInfo & pwmi = pwmInfo[i];
    if (pwmi.sfbPin == sfbPin)
      return i;
  }
  return -1;
}

u32 PWMGetPin(u32 pwmIndex) {
  API_ASSERT_MAX(pwmIndex,MAX_PWMS);
  const PWMInfo & pwmi = pwmInfo[pwmIndex];
  return pwmi.sfbPin;
}

u32 PWMGetChannel(u32 pwmIndex) {
  API_ASSERT_MAX(pwmIndex,MAX_PWMS);
  const PWMInfo & pwmi = pwmInfo[pwmIndex];
  return pwmi.chnl;
}

void PWMSetMode(u32 pwmIndex, u32 mode) {
  API_ASSERT_MAX(pwmIndex,MAX_PWMS);
  API_ASSERT_MAX(mode,PWM_CHANNEL_MODES_MAX);

  bool isActive = mode!=PWM_INACTIVE;

  const PWMInfo & pwmi = pwmInfo[pwmIndex];
  u32 sfbPin = pwmi.sfbPin;

  pinMode(pwmi.sfbPin,isActive?HW_MODE:REFLEX_MODE); // Declare pin off-limit if active else normal

  struct PinInfo_s pinInfo;
  u32 port, mask;
  SFB_GET_PIN_PORT_AND_MASK(sfbPin, pinInfo, port, mask); // Get port details

  setPinSelFunction(port, pinInfo.bitNumber, isActive?pwmi.func:0); // Set PWM if active else GPIO

  u32 enableMask = (1<<8)<<pwmi.chnl; // Channel enable bit in PCR
  u32 doubleMask = 1<<pwmi.chnl;      // Double edge bit in PCR (chnl 2..6)
  
  u32 pcr = PWM1PCR;            // Read PWM control register
  if (isActive) 
    pcr |= enableMask;          // Enable appropriate channel output
  else
    pcr &= ~enableMask;         // Disable appropriate channel output

  if (mode==PWM_DOUBLE_EDGE) {
    API_ASSERT(pwmi.chnl > 1,E_API_ARGUMENT); // Channel 1 can't be in double edge mode
    pcr |= doubleMask;
  } else
    pcr &= ~doubleMask;         // (May clear bits 0 or 1, harmless)

  PWM1PCR = pcr;                // Update control register
}

void PWMSetWidth(u32 pwmIndex, u32 width, bool commit) {
  static u32 pendingLers IN_BODYRAM = 0;
  API_ASSERT_MAX(pwmIndex,MAX_PWMS);
  const PWMInfo & pwmi = pwmInfo[pwmIndex];
  u32 baseOffset = (pwmi.chnl<4)?0x18:0x30;
  REGISTER_OFFSET(PWM1_BASE_ADDR,baseOffset+4*pwmi.chnl) = width;
  pendingLers |= 1<<pwmi.chnl;

  if (commit) {
    PWM1LER = pendingLers;
    pendingLers = 0;
  }
}

u32 PWMGetWidth(u32 pwmIndex) {
  API_ASSERT_MAX(pwmIndex,MAX_PWMS);
  const PWMInfo & pwmi = pwmInfo[pwmIndex];
  u32 baseOffset = (pwmi.chnl<4)?0x18:0x30;
  return REGISTER_OFFSET(PWM1_BASE_ADDR,baseOffset+4*pwmi.chnl);
}


void PWMStop() {

  /////
  // Disable PWM
  PWM1TCR = 0;

  /////
  // Power down PWM
  PCONP &= ~PCPWM1;

}

void PWMStart() {
  u32 bits;                     // Scratch for bit ops

  PWMStop();

  /////
  // Power up PWM, 

  PCONP |= PCPWM1;

  /////
  // Set repetition rate to 50Hz == 20ms, suitable for servos, and for
  // LEDs, at least in the fovea, yes, it will blend.
  //
  // Within each 20ms period, let's have 10,000 counts, so we can set
  // each duty cycle to an arbitrary 0.01% or 2usecs, if we care, or
  // can shift just a byte value in 1..250 by 2, if we don't care.
  // It's a bit of a drag that we can't use 255 as the max value, as
  // is common in color representations..  May have to revisit that,
  // but it's a little hard since we're using a non-power-of-2 CCLK.
  //
  // And, unlike LEDs, servos will usually need scaling anyway since
  // their full range of motion is typically represented only by
  // periods in about 1ms-2ms, so this way we have ~500 potential
  // servo settings, which should be pa-lenty.
  //
  // So 50 cycles/sec * 10000 counts/cycle == 500000 counts/sec ==
  // 500KHz PWM counting rate, and we'll reset every 10000 counts.
  //
  // CCLK == 72MHz, PCLK == CCLK/8 == 9MHz, and we want to prescale
  // that down to 500KHz, so we prescale PCLK by 18.
  //

  bits = PCLKSEL0;                 // Get PCLK select bits
  BIT_PAIR_AT_POSITION(bits,3,12); // PCLK 3 == CCLK/8 in bit position 12
  PCLKSEL0 = bits;                 // Set our selection
  PWM1PR = 18-1;                   // prescale: divide by 18's

  /////
  // Set up the rate on MR0, before enabling PWM mode
  PWM1MR0 = 10000;                 // 10K counts per 20ms == 2usec/count
  
  /////
  // Set the match control register to reset TC on MR0 match (needed?)
  PWM1MCR = 1<<1;

  /////
  // Set timer mode
  PWM1CTCR = 0<<1; 

  /////
  // Set single-edge mode for all matches, and all outputs disabled
  PWM1PCR = 0;

  /////
  // Enable PWM
  PWM1TCR = (1<<0)|(1<<1)|(1<<3);  // Enable counters, reset counters, enable PWM
  PWM1TCR = (1<<0)|(1<<3);         // Let the counting begin

}

