/*
  SFBHWPinsHost.cpp - Board-specific pin definitions
  Copyright (C) 2008 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 "SFBHWPins.h"
#include "SFBReflex.h"        
#include "SFBAssert.h"        
#include <stdio.h>           /* For printf, etc */
#include <stdlib.h>          /* For exit */
#include <sys/time.h> /* For gettimeofday() */

#include <map>

typedef std::map<u32,SFBHWHostRegister> RegMap;

SFBHWHostRegisterMap hostRegisters;
SFBHWHostRegisterMap::SFBHWHostRegisterMap() : vmap(0) { }

void SFBHWHostRegisterMap::setMap(void * vmap) {
  API_ASSERT_ZERO(this->vmap);
  this->vmap = vmap;
}

void SFBHWHostRegisterMap::addRegister(u32 address, u32 initVal, u32 (*toRead)(u32,u32), u32 (*toWrite)(u32,u32)) {
  API_ASSERT_NONNULL(this->vmap);
  RegMap & map = *(RegMap*) this->vmap;
  if (map.find(address)==map.end())
    map.insert(std::make_pair(address,SFBHWHostRegister(address,initVal,toRead,toWrite)));
  else API_BUG(E_BUG_INCONSISTENT_STATE);
}

SFBHWHostRegisterMap::~SFBHWHostRegisterMap() {
  delete (RegMap*) vmap;
  vmap = 0;
}
SFBHWHostRegisterAccess SFBHWHostRegisterMap::registerAt(u32 address) const { return SFBHWHostRegisterAccess(address,4); }
SFBHWHostRegisterAccess SFBHWHostRegisterMap::registerAtU16(u32 address) const { return SFBHWHostRegisterAccess(address,2); }
SFBHWHostRegisterAccess SFBHWHostRegisterMap::registerAtU8(u32 address) const { return SFBHWHostRegisterAccess(address,1); }

SFBHWHostRegister & SFBHWHostRegisterMap::getRegister(u32 address) {
  API_ASSERT_NONNULL(this->vmap);
  RegMap & map = *(RegMap*) this->vmap;
  RegMap::iterator i = map.find(address);
  if (i==map.end()) {
    addRegister(address);
    i = map.find(address);
    API_ASSERT_TRUE(i!=map.end());
  }
  return i->second;
}

void setPinSelFunction(u32 port,u32 pinNumber,u32 function) {
  u32 bits = SFB_PINSEL_REGISTER(port,pinNumber);
  BIT_PAIR_AT_POSITION(bits,function,(pinNumber&0xf)<<1); /* Select appropriate function at position */
  SFB_PINSEL_REGISTER(port,pinNumber) = bits;
}


static u32 fioSetPinR(u32 addr, u32 oldval) {
  u32 base = addr&~0x1f;
  return hostRegisters.registerAt(base+0x14); // Reading FIOSET returns "the current contents of the port output register"
}
static u32 fioSetPinW(u32 addr, u32 val) {
  u32 base = addr&~0x1f;
  hostRegisters.registerAt(base+0x14) |= val&~hostRegisters.registerAt(base+0x10);   // FIOPIN |= val&~FIOMASK
  return 0;
}
static u32 fioClrPinR(u32 addr, u32 oldval) {
  API_BUG(E_HW_DABT);           // Reading a write-only register?  What actually happens?
}
static u32 fioClrPinW(u32 addr, u32 val) {
  u32 base = addr&~0x1f;
  hostRegisters.registerAt(base+0x14) &= (~val)|hostRegisters.registerAt(base+0x10); // FIOPIN &= (~val)|FIOMASK
  return 0;
}
static void loadRegs() {

  for (u32 port = 0; port < 4; ++port) {

    // (Partial) Fast GPIO pin support
    {
      u32 addr = FIO_BASE_ADDR+0x00+port*0x20;
      hostRegisters.addRegister(addr+0x00, 0, 0, 0); // SFB_PORT_DIR_REGISTER
      hostRegisters.addRegister(addr+0x10, 0, 0, 0); // SFB_PORT_MASK_REGISTER
      hostRegisters.addRegister(addr+0x14, 0, 0, 0); // SFB_PORT_PIN_REGISTER
      hostRegisters.addRegister(addr+0x18, 0, fioSetPinR, fioSetPinW); // SFB_PORT_SET_REGISTER
      hostRegisters.addRegister(addr+0x1C, 0, fioClrPinR, fioClrPinW); // SFB_PORT_CLR_REGISTER
    }
    // Pinsel registers
    {
      hostRegisters.addRegister(PINSEL_BASE_ADDR+port*8+0, 0, 0, 0);
      hostRegisters.addRegister(PINSEL_BASE_ADDR+port*8+4, 0, 0, 0);
    }
    // Pinmode registers
    {
      hostRegisters.addRegister(PINMODE_BASE_ADDR+port*8+0, 0, 0, 0);
      hostRegisters.addRegister(PINMODE_BASE_ADDR+port*8+4, 0, 0, 0);
    }
    
  }
}
static void initMap() {
  RegMap * vmap = new RegMap();
  hostRegisters.setMap(vmap);
  loadRegs();
}


void hwpins_startup_initialization() {
  initMap();
}

void reenterBootloader() __attribute__ ((noreturn));

void reenterBootloader() {
  fprintf(stderr,"REENTER BOOT LOADER\n");
  exit(10);
}

void reenterBrainstem() {
  fprintf(stderr,"REENTER BRAINSTEAM\n");
  exit(11);
}

void debugRGB(int rgbBits, int wait) {
  volatile int i, j;

  if (rgbBits&(1<<2))
    REFLEX_RGB_RED_ON;
  else
    REFLEX_RGB_RED_OFF;

  if (rgbBits&(1<<1))
    REFLEX_RGB_GREEN_ON;
  else
    REFLEX_RGB_GREEN_OFF;

  if (rgbBits&(1<<0))
    REFLEX_RGB_BLUE_ON;
  else
    REFLEX_RGB_BLUE_OFF;

  for (i = 0; i < wait; ++i) {
    for (j = 0; j < 1000000; ++j) { }
  }
}

u32 millisFunction() {
  static u32 lastTime = 0;
  struct timeval buf;
  gettimeofday(&buf,0);
  u32 now = buf.tv_sec*1000+(buf.tv_usec/1000);
  if (lastTime==0)
    lastTime = now;
  return now-lastTime;
}

u32 trueMillis() {
  return millis();
}

u32 microsFunction() {
  static u32 lastTime = 0;
  struct timeval buf;
  gettimeofday(&buf,0);
  u32 now = buf.tv_sec*1000000+buf.tv_usec;
  if (lastTime==0)
    lastTime = now;
  return now-lastTime;
}

void _dieOnBoard_(u32 blinkCode, const char * file, int lineno) {
  fprintf(stderr,"%s:%d: DIED -- BLINK_MAP(%d,%d,%d)\n",
          file,lineno,
          GET_BLINK_MAP_COLOR(blinkCode,0),
          GET_BLINK_MAP_COLOR(blinkCode,1),
          GET_BLINK_MAP_COLOR(blinkCode,2)
         );
  exit(1);
}

void _apiError_(u32 blinkCode, const char * file, int lineno) {
  _dieOnBoard_(blinkCode,file,lineno);

}

