/*****************************************************************************

 * Низкоуровневый драйвер стека CANopen (портировано на ADSP-BF50x

 * с кода ECAN чипа PIC18F Microchip из апноута AN945).

 *****************************************************************************

 * Company:       Microchip Technology Incorporated

 * FileName:      CO_CANDRV.C

 * Author               Date        Comment

 *~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

 * Ross Fosler      11/13/03 ...

 *****************************************************************************/

//#include "CO_DEFS.DEF"        // Глобальные определения

#include < services_types.h >

#include < cdefBF504F.h >

#include < builtins.h >

#include < sys\exception.h >

#include "CO_TYPES.H"

#include "CO_CANDRV.H"        // Службы драйвера

#include "CO_NMTE.H"

#include "CO_COMM.H"

#include "CO_SDO1.H"

#include "CO_PDO.H"

#include "CO_NMT.H"

#include "CO_SYNC.h"

#include "pins.h"

////////////////////////////////////////////////////////////////

// Обработчик прерывания приема CAN

//#pragma optimize_off

//EX_INTERRUPT_HANDLER(CAN_RX_HANDLER)
EX_REENTRANT_HANDLER(CAN_RX_HANDLER)
{
   u16 bit_pos = 0;
   char mbID;
   CANmessage m;  // содержит сообщение CAN

   //Внимание: эта переменная обязательно должна быть volatile,
   // иначе код ISR будет работать только при отключенной
   // оптимизации.
   volatile u16 mbim_status = *pCAN_MBRIF1;
   
   while (!(mbim_status & 0x8000))
   {
      mbim_status << = 1;
      bit_pos++;
   }
   mbID = (15 - bit_pos);
   m.cob_id = (*(u16*)CAN_MB_ID1(mbID) & BASEID) >> 2;       //11-битный ID
   m.rtr = (*(u16*)CAN_MB_ID1(mbID) & RTR)?true:false;
   m.len = *(u16*)CAN_MB_LENGTH(mbID) & DLC;
   *(u16*)(&m.data[6]) = SWAPBYTES(*(u16*)CAN_MB_DATA0(mbID));
   *(u16*)(&m.data[4]) = SWAPBYTES(*(u16*)CAN_MB_DATA1(mbID));
   *(u16*)(&m.data[2]) = SWAPBYTES(*(u16*)CAN_MB_DATA2(mbID));
   *(u16*)(&m.data[0]) = SWAPBYTES(*(u16*)CAN_MB_DATA3(mbID));
   // Если как минимум состояние pre-operational, и если хендл допустимый,
   // то декодируем сообщение и генерируем соответствующее событие.
   if (COMM_STATE_PREOP)
   {
      switch(m.cob_id >> 7)
      {
      case NMT:
         //proceedNMTstateChange(d,m);
         if (COMM_NETCTL_NMT_EN)
         {
            _CO_COMM_NMT_RXEvent(&m);
         }
         break;
      case SYNCcode:
         if (COMM_NETCTL_SYNC_EN)
         {
            if (!COMM_STATE_STOP)
               _CO_COMM_SYNC_RXEvent(&m);
         }
         break;
      case TIME_STAMP:
         //if (COMM_NETCTL_TMSTP_EN)
         //{
         //   if (!COMM_STATE_STOP)
         //      _CO_COMM_TIME_RXEvent();
         //}
         break;
      case PDO1tx:
         break;
      case PDO1rx:
         _CO_COMM_PDO1_RXEvent(&m);
         break;
      case PDO2tx:
      case PDO2rx:
      case PDO3tx:
      case PDO3rx:
      case PDO4tx:
      case PDO4rx:
         break;
      case SDOtx:
         break;
      case SDOrx:
         if (!COMM_STATE_STOP)
         {
            if (COMM_SDO_1_EN)
            {
               _CO_COMM_SDO1_RXEvent(&m);
            }
         }
         break;
      case NODE_GUARD:
         if (COMM_NETCTL_NMTE_EN)
         {
            //proceedNODE_GUARD(d,m);
            _CO_COMM_NMTE_RXEvent(&m);
         }
         break;
      default:
         break;
      }
   }
   *pCAN_MBRIF1 = (1 << mbID);
   ssync();
}

//#pragma optimize_as_cmd_line

void canInit (u16 baudrate)
{
   //Инициализация ножек CAN:
   *pPORTG_FER |= (PG1 | PG2);
   *pPORTG_MUX |= PG3;
   *pPORTG_MUX &= ~PG2;
   *pPORTG_MUX |= PG5;
   *pPORTG_MUX &= ~PG4;
   ssync();
   //Установка скорости:
   *pCAN_TIMING = _TIMING;
   *pCAN_CLOCK  = CANBPR;
   ssync();
   //Настройка и разрешение работы ящиков:
   *pCAN_MD1 = 0xFFFF;     // ящики 0-15 на прием
   *pCAN_MD2 = 0x0000;     // ящики 16-31 на передачу
   *pCAN_MC1 = 0xFFFF;     // разрешить ящики 0-15
   *pCAN_MC2 = 0xFFFF;     // разрешить ящики 16-31
   ssync();
   u8 mbID;
   //Настройка передающих ящиков:
   for (mbID = 16; mbID < 32; mbID++)
   {
      //Очистка регистра CAN_MBxx_ID0 (расширенный ID):
      //*(pCAN_Ptr + 12) = 0;
      *(u16*)CAN_MB_ID0(mbID) = 0;
   }
   //Настройка идентификаторов ящиков приема:
   for (mbID = 0; mbID < 16; mbID++)
   {
      //Очистка регистра CAN_MBxx_ID0 (расширенный ID):
      *(u16*)CAN_MB_ID0(mbID) = 0;
      //Установка регистра CAN_MBxx_ID1 (базовый ID, AME):
      *(u16*)CAN_MB_ID1(mbID) = AME;
   }
   // установка масок для приема:
   volatile u16* pCAN_Ptr = pCAN_AM00L;
   int i;
   for(i=0; i < 16; ++i)
   {
      //Настройка регистра маски CAN_AMxxL:
      *pCAN_Ptr = 0;
      //Настройка регистра маски CAN_AMxxH:
      //*(pCAN_Ptr+2) = BASEID;     //всеразрешающая маска по базовому ID
      // Биты ID узла должны совпадать:
      *(pCAN_Ptr+2) = (~(0x007F << 2) | (_uCO_nodeID << 2)) & BASEID;
      pCAN_Ptr+=4;
   }
   //Инициализация прерывания, привязка CAN к приоритету IVG:
   *pSIC_IAR3 |= PX_IVG(CAN_RX_Peripheral, CAN_RX_IVG);
   register_handler(CAN_RX_IVG, CAN_RX_HANDLER);
   *pSIC_IMASK0 |= IRQ_CAN_RX;
   // Запрет режима конфигурации CAN (очистка бита CCR)
   *pCAN_CONTROL = 0;
   ssync();
   //Ожидание завершения режима конфигурирования (ожидание выхода из него):
   while(*pCAN_STATUS & CCA);
   *pCAN_MBIM1 = 0xFFFF;   // разрешить прерывания ящиков 0-15 (прием)
   ssync();
}

/*********************************************************************

 * Overview:      Эта функция используется, чтобы показать драйверу,

 *                что данные, длина и идентификатор CAN были загружены

 *                и готовы к отправке.

 * Портированная версия: передает сразу через первый свободный ящик.

 *

 * 1. Ищется первый свободный передающий ящик.

 * 2. В зависимости от event создается пакет и отправляется

 ********************************************************************/

void mCANSendMessage (TEventTX event)
{
   //Поиск свободного ящика для отправки:
   u16 mask=1;
   u16 mbID;
   TUnion32 data;
   
   for(mbID=16; mbID < 32; ++mbID)
   {
      if((mask & *pCAN_TRS2)==0)
         break;
      mask << = 1;
   }
   if (0 == mask)
   {
      // Нет свободных ящиков для отправки
      return;
   }
   
   switch(event)
   {
   case NMTE_BootTXEvent:
      // Установка COB-ID:
      setCAN_MBxx_ID1(mbID, 0x700L + _uCO_nodeID);
      // Установка длины 1:
      setCAN_MBxx_LENGTH(mbID, 1);
      // Загрузка данных (byte0=0):
      setCAN_MBxx_DATA3(mbID, ((u16)0) << 8);
      // Запуск отправки:
      *pCAN_TRS2 |= mask;
      ssync();
      break;
   case NMTE_NetCtlTXEvent:
      // Установка COB-ID:
      setCAN_MBxx_ID1(mbID, 0x700L + _uCO_nodeID);
      // Установка длины 1:
      setCAN_MBxx_LENGTH(mbID, 1);
      //Подготовка данных:
      _uNMTELocalState.byte = _uNMTELocalState.byte & 0x80;
      if (NMTE_NODE_GUARD_EN)
      {
         _uNMTELocalState.bits.b7 = (~_uNMTELocalState.bits.b7);
      }
      else
      {
         _uNMTELocalState.bits.b7 = 0;
      }
      if (COMM_STATE_STOP)
         _uNMTELocalState.byte = _uNMTELocalState.byte | 4;
      else if (COMM_STATE_OPER)
         _uNMTELocalState.byte = _uNMTELocalState.byte | 5;
      else if (COMM_STATE_PREOP)
         _uNMTELocalState.byte = _uNMTELocalState.byte | 127;
      // Загрузка данных (byte0, младший байт данных ящика):
      setCAN_MBxx_DATA3(mbID, ((u16)_uNMTELocalState.byte) << 8);
      // Запуск отправки:
      *pCAN_TRS2 |= mask;
      ssync();
      break;
   case SDO1_TXEvent:
      // Установка COB-ID
      setCAN_MBxx_ID1(mbID, 0x580L + _uCO_nodeID);
      // Установка длины 8:
      setCAN_MBxx_LENGTH(mbID, 8);
      // Загрузка данных
      if (_uSDO1ACode == E_SUCCESS)
      {
         // Перенос данных в буфер передачи:
         setCAN_MBxx_DATA0(mbID, SWAPBYTES(*(u16*)(_uSDO1TxBuf+6))); //Байты 6, 7
         setCAN_MBxx_DATA1(mbID, SWAPBYTES(*(u16*)(_uSDO1TxBuf+4))); //Байты 4, 5
         setCAN_MBxx_DATA2(mbID, SWAPBYTES(*(u16*)(_uSDO1TxBuf+2))); //Байты 2, 3
         setCAN_MBxx_DATA3(mbID, SWAPBYTES(*(u16*)(_uSDO1TxBuf+0))); //Байты 0, 1
      }
      else 
      {
         // Установка кода abort
         data.bytes[0] = (u8)0x80;
         // Установка мультиплексора
         data.bytes[1] = _uSDO1Dict.index.bytes[0];
         setCAN_MBxx_DATA3(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 0, 1
         data.bytes[2] = _uSDO1Dict.index.bytes[1];
         data.bytes[3] = _uSDO1Dict.subindex;
         setCAN_MBxx_DATA2(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 2, 3
         switch (_uSDO1ACode)
         {
         case E_TOGGLE:
            // Бит Toggle не переключился
            data.word = 0x05030000L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_SDO_TIME:
            // Таймаут протокола SDO
            data.word = 0x05040000L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_CS_CMD:
            // Спецификатор команды клиент/сервер недопустим или неизвестен
            data.word = 0x05040001L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_MEMORY_OUT:
            // Не хватает памяти
            data.word = 0x05040005L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_UNSUPP_ACCESS:
            // Не поддерживаемый доступ к объекту
            data.word = 0x06010000L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_CANNOT_READ:
            // Попытка прочитать объект, у которого доступ только на запись
            data.word = 0x06010001L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_CANNOT_WRITE:
            // Попытка записать объект, у которого доступ только на чтение
            data.word = 0x06010002L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_OBJ_NOT_FOUND:
            // Объект не существует в OD
            data.word = 0x06020000L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_OBJ_CANNOT_MAP:
            // Объект не может быть отображен (привязан к) на PDO
            data.word = 0x06040041L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_OBJ_MAP_LEN:
            // Количество и длина привязанных объектов превысили длину PDO
            data.word = 0x06040042L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_GEN_PARAM_COMP:
            // Причина в общей несовместимости параметра
            data.word = 0x06040043L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_GEN_INTERNAL_COMP:
            // Общая внутренняя несовместимость в устройстве
            data.word = 0x06040047L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_HARDWARE:
            // Ошибочный доступ из-за аппаратной ошибки
            data.word = 0x06060000L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_LEN_SERVICE:
            // Не соответствует тип данных, не соответствует длина параметра службы
            data.word = 0x06070010L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_LEN_SERVICE_HIGH:
            // Не соответствует тип данных, длина параметра службы слишком велика
            data.word = 0x06070012L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_LEN_SERVICE_LOW:
            // Не соответствует тип данных, длина параметра службы слишком мала
            data.word = 0x06070013L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_SUBINDEX_NOT_FOUND:
            // Sub-индекс не существует
            data.word = 0x06090011L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_PARAM_RANGE:
            // Превышен диапазон значения параметра
            data.word = 0x06090030L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_PARAM_HIGH:
            // Значение записываемого параметра слишком велико
            data.word = 0x06090031L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_PARAM_LOW:
            // Значение записываемого параметра слишком мало
            data.word = 0x06090032L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_MAX_LT_MIN:
            // Максимальное значение меньше, чем минимальное
            data.word = 0x06090036L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_GENERAL:
            // Общая ошибка
            data.word = 0x08000000L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_TRANSFER:
            // Данные не могут быть переданы или сохранены в приложение
            data.word = 0x08000020L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_LOCAL_CONTROL:
            // Данные не могут быть переданы или сохранены в приложение из-за локального управления
            data.word = 0x08000021L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         case E_DEV_STATE:
            // Данные не могут быть переданы или сохранены в приложение из-за текущего состояния устройства
            data.word = 0x08000022L;
            setCAN_MBxx_DATA0(mbID, (((u16)data.bytes[2]) << 8)|data.bytes[3]); //Байты 6, 7
            setCAN_MBxx_DATA1(mbID, (((u16)data.bytes[0]) << 8)|data.bytes[1]); //Байты 4, 5
            break;
         }
      }
      // Запуск отправки:
      *pCAN_TRS2 |= mask;
      ssync();
      break;
   case PDO1_TXEvent:
      // Установка COB-ID:
      setCAN_MBxx_ID1(mbID, uTPDOComm1.word);
      //Установка длины:
      setCAN_MBxx_LENGTH(mbID, _uPDO1.TPDO.len);
      // Загрузка данных для передачи
      setCAN_MBxx_DATA0(mbID, SWAPBYTES(*(u16*)(_uPDO1.TPDO.buf+6)));  //Байты 6, 7
      setCAN_MBxx_DATA1(mbID, SWAPBYTES(*(u16*)(_uPDO1.TPDO.buf+4)));  //Байты 4, 5
      setCAN_MBxx_DATA2(mbID, SWAPBYTES(*(u16*)(_uPDO1.TPDO.buf+2)));  //Байты 2, 3
      setCAN_MBxx_DATA3(mbID, SWAPBYTES(*(u16*)(_uPDO1.TPDO.buf+0)));  //Байты 0, 1
      // Запуск отправки:
      *pCAN_TRS2 |= mask;
      ssync();
      break;

#if CO_NUM_OF_PDO > 1
   case PDO2_TXEvent:
      break;

#endif

#if CO_NUM_OF_PDO > 2
   case PDO3_TXEvent:
      break;

#endif

#if CO_NUM_OF_PDO > 3
   case PDO4_TXEvent:
      break;

#endif
   }
}