/*
 * Copyright (c) 2007 Marco Glietsch (http://www.mikrocontroller.net/topic/98697)
 * Original author: Marco Glietsch
 * Modified for octopus by Michael Hartmann <ich@speicherleck.de>
 * All rights reserved.
 *
 * Short descripton of file:
 *
 *
 * Redistribution and use in source and binary forms, with or without 
 * modification, are permitted provided that the following conditions 
 * are met:
 *
 *   * Redistributions of source code must retain the above copyright 
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above 
 *     copyright notice, this list of conditions and the following 
 *     disclaimer in the documentation and/or other materials provided 
 *     with the distribution.
 *   * Neither the name of the FH Augsburg nor the names of its 
 *     contributors may be used to endorse or promote products derived 
 *     from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES{} LOSS OF USE, 
 * DATA, OR PROFITS{} OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifdef __AVR_AT90CAN128__

#include "common.h"
#include "protocol.h"
#include "can.h"

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>  
#include <inttypes.h>
#include <stdbool.h>

struct {
  uint32_t id;
  int8_t data[8];
} message;

int8_t iemob[15] = {
  IEMOB0, IEMOB1, IEMOB2,  IEMOB3,  IEMOB4,  IEMOB5,  IEMOB6, IEMOB7,
  IEMOB8, IEMOB9, IEMOB10, IEMOB11, IEMOB12, IEMOB13, IEMOB14 };

// extended id
int8_t extended_id;

void can_parser(uint8_t *buf)
{
  switch(buf[0])
  {
    case CMD_CAN_INIT:
      can_init_usb((uint8_t)buf[2], (uint8_t)buf[3]);
      break;
    case CMD_CAN_DEINIT:
      can_deinit_usb();
      break;
    case CMD_CAN_ENABLE_MOB:
      asm("nop");
      uint32_t idm = 0;
      uint32_t id  = 0;
     
      id  = ((uint32_t)buf[4])        +
            ((uint32_t)buf[5]  <<  8) +
            ((uint32_t)buf[6]  << 16) +
            ((uint32_t)buf[7]  << 24);
     
      idm = ((uint32_t)buf[8])        +
            ((uint32_t)buf[9]  <<  8) +
            ((uint32_t)buf[10] << 16) +
            ((uint32_t)buf[11] << 24);

      // mob, mode, id, idm
      can_enable_mob_usb(buf[2], buf[3], id, idm);
      break;
    case CMD_CAN_DISABLE_MOB:
      can_disable_mob_usb((uint8_t)buf[2]);
      break;
    case CMD_CAN_SEND_DATA:
      can_send_data_usb(buf[2], buf[3], &buf[4]);
      break;
    case CMD_CAN_SEND_REMOTE:
      can_send_remote_usb(buf[2]);
      break;
    case CMD_CAN_RECEIVE_DATA:
      can_receive_data_usb(buf[2]);
      break;
    case CMD_CAN_SET_AUTOREPLY:
      can_set_autoreply_usb(buf[2], buf[3], &buf[4]);
      break;
    default:
      answer[0] = buf[0];
      answer[1] = RSP_UNKOWN_CMD;
      answer[2] = 0;
      CommandAnswer(3);
   }
}

void can_init_usb(uint8_t baudrate, uint8_t eid)
{
  answer[0] = CMD_CAN_INIT;
  answer[1] = (uint8_t)can_init(baudrate, CAN_INTERRUPTS_RX, eid);
  answer[2] = 0;

  CommandAnswer(3);
}

// Message Objects zuruecksetzen, CAN-Controller aktivieren
// Parameter:
//   uint8_t baud: 0,1,2...5 (fuer 100, 125, 200, 250, 500, 1000)
//   uint8_t intmode: Ereignis, bei dem ein Interrupt ausgelöst werden soll
//     - NONE  - Deaktiviert
//     - TX    - Daten gesendet
//     - RX    - Daten empfangen
//     - TXRX  - Daten gesendet und/oder empfangen
int can_init(uint8_t baudrate, uint8_t intmode, uint8_t eid)
{
  uint8_t i;

  octopus.ports[17] = PIN_CAN;
  octopus.ports[18] = PIN_CAN;

  extended_id = eid ? 1 : 0;

  message.id = 0;
  for(i = 0; i < 8; i++)
    message.data[i] = 0;

  // Status- und Steuerregister aller Message Objects initialisieren
  for(i = 0; i < NO_MOBS; i++)
  {
    can_get_mob(i);
    CANSTMOB = 0;
    CANCDMOB = 0;
  }

  // set baudrate
  if(can_set_baudrate(baudrate) == 0)
    return RSP_ERROR;

  if(can_set_interrupt(intmode) == 0)
    return RSP_ERROR;

  // CAN-Controller in Enabled Mode setzen
  setbit(CANGCON, ENASTB);

  // Warten bis der CAN-Controller das Enabled-Bit gesetzt hat und
  // einsatzbereit ist
  while (!getbit(CANGSTA, ENFG));

  return RSP_OK; 
}

void can_deinit_usb(void)
{
  answer[0] = CMD_CAN_DEINIT;
  answer[1] = (int8_t)can_deinit();
  answer[2] = 0;

  CommandAnswer(3);
}

int can_deinit(void)
{
  uint8_t i;

  octopus.ports[17] = PIN_NONE;
  octopus.ports[18] = PIN_NONE;

  for(i = 0; i < 15; i++)
    can_disable_mob(i);

  // CAN-Controller ausschalten
  clearbit(CANGCON, ENASTB);

  // warten bis wirklich ausgechaltet
  while (getbit(CANGSTA, ENFG));

  return RSP_OK;
}

void can_set_autoreply(uint8_t mob, uint8_t length, uint8_t *buf)
{
  can_get_mob(mob);
  can_set_mode(CAN_MODE_AUTO_REPLY);
  can_set_data(length, buf);
}

void can_set_autoreply_usb(uint8_t mob, uint8_t length, uint8_t *buf)
{
  can_set_autoreply(mob, length, buf);

  answer[0] = CMD_CAN_SET_AUTOREPLY;
  answer[1] = RSP_OK;
  answer[2] = 0;

  CommandAnswer(3);
}

int can_set_baudrate(uint8_t baudrate)
{
  // @16 MHz
  switch(baudrate)
  {
    case CAN_BAUDRATE_100K:
      CANBT1 = 0x12;
      CANBT2 = 0x0c;
      CANBT3 = 0x37;
      break;

    case CAN_BAUDRATE_125K:
      CANBT1 = 0x0e;
      CANBT2 = 0x0c;
      CANBT3 = 0x37;
      break;

    case CAN_BAUDRATE_200K:
      CANBT1 = 0x08;
      CANBT2 = 0x0c;
      CANBT3 = 0x37;
      break;

    case CAN_BAUDRATE_250K:
      CANBT1 = 0x06;
      CANBT2 = 0x0c;
      CANBT3 = 0x37;
      break;

    case CAN_BAUDRATE_500K:
      CANBT1 = 0x02;
      CANBT2 = 0x0c;
      CANBT3 = 0x37;
      break;

    case CAN_BAUDRATE_1000K:
      CANBT1 = 0x00;
      CANBT2 = 0x0c;
      CANBT3 = 0x37;
      break;

    default:
      return 0;
  }

  return 1;
}

void can_enable_mob_usb(uint8_t mob, uint8_t mode, uint32_t id, uint32_t idm)
{
  answer[0] = CMD_CAN_ENABLE_MOB;
  answer[1] = (int8_t)can_enable_mob(mob, mode, id, idm);
  answer[2] = 0;

  CommandAnswer(3);
}


// Parameter:
//   uint8_t mob: Nummer des zu wählenden Objekts (0-14)
//
//   uint8_t mode
//     - Betriebsart des Message Objekts:
//     - DISABLED         Deaktiviert
//     - TRANSMIT_DATA    Daten senden
//     - TRANSMIT_REMOTE  Anfrage senden
//     - RECEIVE_DATA     Empfangsmodus
//     - AUTO_REPLY       automatischer Antwortmodus
//
// Funktion setzt die Betriebsart des vorher gewählten Objekts
int can_enable_mob(uint8_t mob, uint8_t mode, uint32_t id, uint32_t idm)
{
  // Objekt wählen
  can_get_mob(mob);

  // Interrupt für dieses Objekt aktivieren
  can_set_mob_interrupt(mob);

  // ID-Maske setzen
  can_set_id_mask(idm);

  // ID setzen
  can_set_id(id);

  // Betriebsmodus setzen
  if(can_set_mode(mode) == 0)
    return RSP_ERROR;

  return RSP_OK;
}


void can_send_data_usb(uint8_t mob, uint8_t length, uint8_t *data)
{
  answer[0] = CMD_CAN_SEND_DATA;
  answer[1] = (int8_t)can_send_data(mob, length, data);
  answer[2] = 0;

  CommandAnswer(3);
}

int can_send_data(uint8_t mob, uint8_t length, uint8_t * data)
{
  uint8_t mode;

  // Objekt wählen
  can_get_mob(mob);
  
  // Aktuelle Betriebsart sichern
  mode = can_get_mode();

  // Nutzdaten in Register schreiben
  can_set_data(length, data);

  // Datenübertragung starten
  can_set_mode(CAN_MODE_TRANSMIT_DATA);

  // Warten bis die Datenübertragung beendet ist (TXOK-Flag von CAN-Controller
  // gesetzt)
  while (!getbit(CANSTMOB, TXOK));      

  // TXOK-Flag von Hand löschen
  clearbit(CANSTMOB, TXOK);

  // Alte Betriebsart wiederherstellen
  can_set_mode(mode);

  return RSP_OK;
}


// Parameter: uint8_t mode: Ereignis, bei dem Interrupt ausgelöst werden soll
//   - NONE  - Deaktiviert
//   - TX    - Daten gesendet
//   - RX    - Daten empfangen
//   - TXRX  - Daten gesendet und/oder empfangen
int can_set_interrupt(uint8_t mode)
{
  switch(mode)
  {
    case CAN_INTERRUPTS_NONE:
      clearbit(CANGIE, ENIT);
      clearbit(CANGIE, ENRX);
      clearbit(CANGIE, ENTX);
      break;

    case CAN_INTERRUPTS_TX:
      setbit(CANGIE, ENIT);
      clearbit(CANGIE, ENRX);
      setbit(CANGIE, ENTX);
      break;

    case CAN_INTERRUPTS_RX:
      setbit(CANGIE, ENIT);
      setbit(CANGIE, ENRX);
      clearbit(CANGIE, ENTX);
      break;

    case CAN_INTERRUPTS_TXRX:
      setbit(CANGIE, ENIT);
      setbit(CANGIE, ENRX);
      setbit(CANGIE, ENTX);
      break;

    default:
      return 0;
  }

  return 1;
}


// Funktion wählt CANPAGE des betreffenden Objekts aus und stellt Zugang zu
// Registern des Objekts her
void can_get_mob(uint8_t mob)
{
  CANPAGE = (mob << 4);
}

  
// Parameter: uint32_t idm: ID-Maske in Dezimalschreibweise
// Funktion setzt ID-Maske eines Objekts auf einen neuen Wert. In CANIDM4
// bleiben dabei die Werte der unteren 3 Bit (RTRTAG, Reserved und IDEMSK)
// erhalten.
void can_set_id_mask(uint32_t idm)
{
  //Standart identifier (11 bit)
  if(!extended_id)
  {
    CANIDM2 = (uint8_t)(idm << 5); 
    CANIDM1 = (uint8_t)(idm >> 3);
  }
  //extended identifier
  else
  {
    idm <<= 3;
    idm  |= 7;

    CANIDM4 = (int8_t) (idm);
    CANIDM3 = (int8_t) (idm>>8);
    CANIDM2 = (int8_t) (idm>>16);
    CANIDM1 = (int8_t) (idm>>24);
  }
}

// Funktion holt ID der empfangenen Nachricht
uint32_t can_get_id(void)
{
  uint32_t id = 0;

  //Standart identifier (11 bit)
  if(!extended_id)
  {
    id  = (uint8_t)  CANIDT2 >> 5;
    id |= (uint16_t) CANIDT1 << 3;
  }
  //extended identifier
  else
  {
    id |= ((uint32_t) CANIDT1 << 24);
    id |= ((uint32_t) CANIDT2 << 16);
    id |= ((uint32_t) CANIDT3 << 8);
    id |= (CANIDT4&0xF8);
    id >>= 3;
  }

  return id;
}

// Funktion setzt ID eines Objekts auf einen neuen Wert. In CANIDM4 bleiben
// dabei die Werte der unteren 3 Bit (RTRTAG, RB1TAG und RB0TAG) erhalten.
void can_set_id(uint32_t id)
{
  //Standart identifier (11 bit)
  if(!extended_id)
  {
    CANIDT2 = (uint8_t)(id << 5); 
    CANIDT1 = (uint8_t)(id >> 3);
  }
  //extended identifier
  else
  {
    id <<= 3;
    id &= 0xfffffff8;
    id |= (CANIDT4 & 0x07);

    CANIDT4 = (int8_t) (id);
    CANIDT3 = (int8_t) (id>>8);
    CANIDT2 = (int8_t) (id>>16);
    CANIDT1 = (int8_t) (id>>24);
  }
}


// Funktion setzt die Betriebsart des vorher gewählten Objekts.
// Parameter: uint8_t mode:
//   - Betriebsart des Message Objekts:
//   - DISABLED        - Deaktiviert
//   - TRANSMIT_DATA   - Daten senden
//   - TRANSMIT_REMOTE - Anfrage senden
//   - RECEIVE_DATA    - Empfangsmodus
//   - AUTO_REPLY      - automatischer Antwortmodus
int can_set_mode(uint8_t mode)
{
  if(extended_id)
    setbit(CANCDMOB, IDE);
  else
    clearbit(CANCDMOB, IDE);

  switch(mode)
  {
    case CAN_MODE_DISABLED:
      clearbit(CANCDMOB, CONMOB0);
      clearbit(CANCDMOB, CONMOB1);
      clearbit(CANCDMOB, RPLV);
      clearbit(CANIDT4, RTRTAG);
      clearbit(CANIDM4, RTRMSK);
      break;

    case CAN_MODE_TRANSMIT_DATA:
      CANCDMOB &= ~(1 << CONMOB1 || 1 << CONMOB0 || 1 << RPLV);
      CANCDMOB |=  (1 << CONMOB0 || 0 << CONMOB1);
      clearbit(CANIDT4, RTRTAG);
      break;

    case CAN_MODE_TRANSMIT_REMOTE:
      clearbit(CANCDMOB, CONMOB1);
      setbit(CANCDMOB, CONMOB0);
      clearbit(CANCDMOB, RPLV);
      setbit(CANIDT4, RTRTAG);
      break;

    case CAN_MODE_RECEIVE_DATA:
      clearbit(CANCDMOB, CONMOB0);
      setbit(CANCDMOB, CONMOB1);
      clearbit(CANCDMOB, RPLV);
      clearbit(CANIDT4, RTRTAG);
      break;

    case CAN_MODE_AUTO_REPLY:
      clearbit(CANCDMOB, CONMOB0);
      setbit(CANCDMOB, CONMOB1);
      setbit(CANCDMOB, RPLV);
      setbit(CANIDT4, RTRTAG);
      break;

    default:
      return 0;
  }

  return 1;
}


// Funktion holt die Betriebsart des vorher gewaehlten Objekts
uint8_t can_get_mode(void)
{
  uint8_t mode = 0;

  if(!getbit(CANCDMOB, CONMOB1) && !getbit(CANCDMOB, CONMOB0))
  {
    mode = CAN_MODE_DISABLED;
  }
  else if(!getbit(CANCDMOB, CONMOB1) && getbit(CANCDMOB, CONMOB0) &&
    !getbit(CANIDT4, RTRTAG))
  {
    mode = CAN_MODE_TRANSMIT_DATA;
  }
  else if(!getbit(CANCDMOB, CONMOB1) && getbit(CANCDMOB, CONMOB0) &&
    getbit(CANIDT4, RTRTAG))
  {
    mode = CAN_MODE_TRANSMIT_REMOTE;
  }
  else if(getbit(CANCDMOB, CONMOB1) && !getbit(CANCDMOB, CONMOB0) &&
    !getbit(CANIDT4, RTRTAG))
  {
    mode = CAN_MODE_RECEIVE_DATA;
  }
  else if(getbit(CANCDMOB, CONMOB1) && !getbit(CANCDMOB, CONMOB0) &&
    getbit(CANCDMOB,RPLV) && getbit(CANIDT4, RTRTAG))
  {
    mode = CAN_MODE_AUTO_REPLY;
  }

  return mode;
}


// Funktion schreibt in das Objekt die zu uebermittelnden Daten
void can_set_data(uint8_t length, uint8_t * data)
{
  uint8_t i;
  uint8_t cancdmob;

  if(length > 8)
    length = 8;

  // Anzahl der Datenbytes in der Nachricht
  // scheinbar darf man das CANCDMOB register nicht beliebig oft
  // schreiben/lesen, daher speichern wir den wert dazwischen, loeschen die
  // entsprechenden bits fuer die laenge und schreiben dann unsere laenge rein
  // wie dem auch sei: so funktionierts zumindest, also vorsicht beim aufraeumen ;-)
  cancdmob = CANCDMOB;
  clearbit(cancdmob, DLC0);
  clearbit(cancdmob, DLC1);
  clearbit(cancdmob, DLC2);
  clearbit(cancdmob, DLC3);
  CANCDMOB = (cancdmob | (length << DLC0));
  for(i = 0; i < length; i++)
    CANMSG = data[i];
}

void can_get_data(int8_t *msg)
{
  uint8_t i;

  for(i = 0; i < 8; i++)
    msg[i] = CANMSG;
}

// Parameter: uint8_t mob: Nummer des Objekts (0-14)
// Funktion setzt den Interrupt für das jeweilige Objekt
void can_set_mob_interrupt(uint8_t mob)
{
  // bugfix von C.H.
  // http://www.mikrocontroller.net/topic/98697#1232848
  if (mob < 8) { setbit(CANIE2, iemob[mob]); }
  else         { setbit(CANIE1, iemob[mob]); }
}

// Parameter: uint8_t mob: Nummer des Objekts (0-14)
// Funktion löscht den Interrupt für das jeweilige Objekt
void can_clear_mob_interrupt(uint8_t mob)
{
  clearbit(CANIE2, iemob[mob]);
}

// Rückgabe: uint8_t mob: Nummer des Objekts
// Funktion ermittelt, welches Objekt Interrupt ausgeloest hat
uint8_t can_get_mob_interrupt(void)
{
  uint8_t  mob;
  uint16_t maske;
  maske = CANSIT2 | (CANSIT1 << 8);

  // Wenn alle 32 Bit der Bitmaske 0 sind dann ist ein Fehler aufgetreten
  if(maske == 0)
    return NOMOB;

  // Die Bitmaske wird so lange nach rechts geschoben, bis Bit0 eine 1 hat.
  // Die Anzahl der Schiebeoperatoren gibt somit die Nummer des MOBs zurück
  for(mob=0; (maske & 0x01)==0; maske >>= 1, ++mob);

  // Kontrolle: Wenn mob größer als die Anzahl der verfügbaren
  // Message Objects ist das Ergebnis falsch
  if(mob > 14)
    return NOMOB;

  return mob;
}

void can_disable_mob_usb(uint8_t mob)
{
   answer[0] = CMD_CAN_DISABLE_MOB;
   answer[1] = (int8_t)can_disable_mob(mob);
   answer[2] = 0;

   CommandAnswer(3);
}

// Funktion deaktiviert das gewählte Objekt
int can_disable_mob(uint8_t mob)
{
  // Objekt wählen
  can_get_mob(mob);

  // Interrupt für dieses Objekt aktivieren
  can_clear_mob_interrupt(mob);

  // Betriebsmodus setzen
  can_set_mode(CAN_MODE_DISABLED);

  return RSP_OK;
}


void can_send_remote_usb(uint8_t mob)
{
  answer[0] = CMD_CAN_SEND_REMOTE;
  answer[1] = (int8_t)can_send_remote(mob);
  answer[2] = 0;

  CommandAnswer(3);
}

// Funktion sendet eine Anfrage (Remote Frame)
int can_send_remote(uint8_t mob)
{
  uint8_t mode;

  // Objekt wählen
  can_get_mob(mob);
  
  // Aktuelle Betriebsart sichern
  mode = can_get_mode();

  // Datenübertragung starten
  can_set_mode(CAN_MODE_TRANSMIT_REMOTE);

  // Warten bis die Datenübertragung beendet ist (TXOK-Flag von CAN-Controller
  // gesetzt)
  while (!getbit(CANSTMOB, TXOK));      

  // TXOK-Flag von Hand löschen
  clearbit(CANSTMOB, TXOK);

  // Alte Betriebsart wiederherstellen
  can_set_mode(mode);

  return RSP_OK;
}

void can_receive_data_usb(int mob)
{
  uint8_t i;
  answer[0] = CMD_CAN_RECEIVE_DATA;
  answer[1] = RSP_OK;

  // id
  answer[2] = (int8_t) (message.id);
  answer[3] = (int8_t) (message.id >> 8);
  answer[4] = (int8_t) (message.id >> 16);
  answer[5] = (int8_t) (message.id >> 24);

  message.id = 0;
  for(i = 0; i < 8; i++)
  {
    answer[6+i] = message.data[i];
    message.data[i] = 0;
  }
  answer[14] = 0;
  
  CommandAnswer(15);
}

// Interrupt fuer Empfang einer Nachricht
SIGNAL(SIG_CAN_INTERRUPT1)
{
  uint8_t save_canpage;
  uint8_t mob;

  // Aktuelle CANPAGE sichern
  save_canpage = CANPAGE;

  // Index des MOB ermitteln, der den Interrupt ausgelöst hat
  mob = can_get_mob_interrupt();
    
  // Falls es kein gültiges MOB war abbrechen
  if(mob == NOMOB)
    return;

  // Objekt das den Interrupt ausgelöst hat holen
  can_get_mob(mob);
  
  // Id der Nachricht holen
  message.id = can_get_id();
    
  // Daten des MOBs aus CANMSG auslesen
  can_get_data(message.data);

  // Id der Nachricht holen
  message.id = can_get_id();

  // RXOK-Flag löschen
  clearbit(CANSTMOB, RXOK);
    
  // MOB auf Empfang setzen
  can_set_mode(CAN_MODE_RECEIVE_DATA);
    
  // CANPAGE wiederherstellen
  CANPAGE = save_canpage;
}

#endif