/* ---------------------------------------------------------------------------
 * can.c                                                (c) 2008 Micro-key bv
 * ---------------------------------------------------------------------------
 * Micro-key bv
 * Industrieweg 28, 9804 TG Noordhorn
 * Postbus 92, 9800 AB Zuidhorn
 * The Netherlands
 * Tel: +31 594 503020
 * Fax: +31 594 505825
 * Email: support@microkey.nl  
 * Web: www.microkey.nl
 * ---------------------------------------------------------------------------
 * Description: 
 * ---------------------------------------------------------------------------
 * Version(s):  0.1, Feb 11, 2008, MMi
 *                   Creation.
 * ---------------------------------------------------------------------------
 */

/* ---------------------------------------------------------------------------
 * System include files
 * ---------------------------------------------------------------------------
 */
/* Hardware Includes */
#include "LPC23xx.h"
#include "types.h"

/* FreeRTOS includes */
#include "FreeRTOS.h"
#include "Task.h"
#include "queue.h"
/* ---------------------------------------------------------------------------
 * Application include files
 * ---------------------------------------------------------------------------
 */ 
#include "can.h"
#include "SerOut.h"
#include "armVIC.h"
/* ---------------------------------------------------------------------------
 * Local constant and macro definitions
 * ---------------------------------------------------------------------------
 */
#define CAN_PORT            2
/* ---------------------------------------------------------------------------
 * Global variable definitions
 * ---------------------------------------------------------------------------
 */
static UINT32 debugVar = 0;
static UINT32 debugCnt = 0;
/* ---------------------------------------------------------------------------
 * Local variable definitions
 * ---------------------------------------------------------------------------
 */
UINT16 volatile gCANFilter = 0;             /* Number of global set filters */

CAN_MSG volatile gCANList[MAX_FILTERS];     /* CAN message list             */
/* ---------------------------------------------------------------------------
 * Local function definitions
 * ---------------------------------------------------------------------------
 */


UINT16 CANInit (UINT32 can_btr)
{
    
    PCONP |= (1 << 14);                     /* power on CAN2                */


    /* Enable Pins for CAN interface                                        */
    /* Set P0.4 to function 10 (RD2)                                        */
    PINSEL0 &=~(1 << 8);                    /* Clear Bit 8 in PINSEL0       */
    PINSEL0 |= (1 << 9);                    /* set bit 9 in PINSEL0         */
    /* Set P0.5 to function 10 (TD2)                                        */
    PINSEL0 &=~(1 << 10);                   /* Clear bit 10 in PINSEL0      */
    PINSEL0 |= (1 << 11);                   /* Set bit 11 in PINSEL0        */

    gCANFilter = 0;                         /* Reset previous Filters       */
//    CAN_AFMR = 0x00000001L;                 /* Switch off Aceptance Filter  */
    
    CAN2MOD = 0x00000001;                   /* Set Mode to Reset (Bit 0)    */
    CAN2IER = 0x00000000;                   /* Disable all CAN2 Interrupts  */
    CAN2GSR = 0x00000000;                   /* Clear all status bits        */
    CAN2BTR = can_btr;                      /* Set bus timing               */

    /* Set Address of CAN ISR to Vectored Interrupt Address 23              */
    VICVectAddr23 = (unsigned long) CAN_IRS_Handler;
    VICVectCntl23 = 1;                      /* Set a low interrupt priority */
    VICIntEnable |= (1 << 23);              /* Enable CAN interrupt         */

//    CAN2IER = 0x00000081;                   /* Enable all RX and Err IRQs   */
    CAN2IER = 0x00000001;                   /* Enable all RX         IRQs   */
    CAN2MOD = 0x00000000;                   /* Set Mode to Normal (Bit 0)   */

    return (TRUE);
}


UINT16 CANSetFilter (UINT32 CANID)
{
    UINT32 loopcnt = 0;
    UINT32 regbuffer = 0;
    UINT32 buf0;
    UINT32 buf1;
    UINT32 ID_lower;
    UINT32 ID_upper;
    UINT32 *pAddr;

    CAN_AFMR = 0x00000001L;                 /* Switch off Acceptance filter */

    if (gCANFilter == 0)
        {
        /* First filter set - Init Entry of position Zero                   */
        /* Make first array entry for CAN device 1
         * This is necessary due to the following sort algorithm
         */
        gCANList[0].Dat1 = 0x000037FFL;
        }
    
    if (gCANFilter >= MAX_FILTERS)
        {
        /* If Array index exceeds the limit, return with error              */
        return (FALSE);
        }

    CANID &= 0x000007FFL;                   /* Mask the 11-bit ID           */
    CANID |= ((CAN_PORT - 1) << 13);        /* Add PORT# to ID              */

    /* Filters must be sorted by interface, then by priority                */
    
    /* Algorithm to sort new entry into excisting array                     */
    while (loopcnt < gCANFilter)            /* Look through all entries     */
        {
        if ((gCANList[loopcnt].Dat1 & 0x0000FFFFL)> CANID)
            {
            /* Found position to insert new entry                           */
            break;
            }
        loopcnt++;
        }

    buf0 = gCANList[loopcnt].Dat1;      /* Buffer old array entry       */
    gCANList[loopcnt].Dat1 = CANID;     /* Insert the new entry         */

    /* move all remaining entries one position up                           */
    gCANFilter++;                           /* Increase Filter counter      */
    while (loopcnt < gCANFilter)
        {
        loopcnt++;
        buf1 = gCANList[loopcnt].Dat1;
        gCANList[loopcnt].Dat1 = buf0;
        buf0 = buf1;
        }


    CAN_SFF_SA = regbuffer;                 /* Set Std Frame Start Address  */
    /* Set pointer to the Filter RAM base Address                           */
    pAddr = ( UINT32 *) ACCEPTANCE_FILTER_RAM_BASE;
    
    for (loopcnt = 0; loopcnt < ((gCANFilter + 1) / 2); loopcnt++)
        {
        ID_lower = gCANList[loopcnt * 2].Dat1 & 0x0000FFFFL;
        ID_upper = gCANList[loopcnt * 2 + 1].Dat1 & 0x0000FFFFL;
//        candata = (ID_lower << 16) + ID_upper;                            // \TODO IS THIS WORKING?
//        *pAddr = candata;
        *pAddr = (ID_lower << 16) + ID_upper;
        regbuffer += 4;
        pAddr++;
        }

    /* regbuffer points to the End of the Table                             */
    
    CAN_SFF_GRP_SA = regbuffer;             /* Set Std Group Start Address  */

    // Set pointer for Extended Frame Individual
    // Extended Frame Start Address Register
    CAN_EFF_SA = regbuffer;                 /* Set Extd. Frame Start Address*/

    CAN_EFF_GRP_SA = regbuffer;             /* Set Extd. Group Start Address*/

    CAN_EOT = regbuffer;                    /* Set End of Table             */

    CAN_AFMR = 0;                           /* enable Acception Filter      */

    return (TRUE);
}



UINT16 CANPushMessage (CAN_MSG *pTransmitBuf)
    {
    UINT32 *pAddr;
    UINT32 *pCandata;
    UINT8 TXBufOffset;

    pAddr = (UINT32 *) &CAN2SR; // CANSR
    
    /* Check if one of the three transmit buffers are available, use first  */
    if (!(*pAddr & 0x00000004L))
        {
        /* First Buffer is not available                                    */
        if (!(*pAddr & 0x00000400L))
            {
            /* Second Buffer is not available                               */
            if (!(*pAddr & 0x00040000L))
                {
                /* Third Buffer is not available -> No Buffer available     */
                return (FALSE);             /* Abort transmission, Error    */
                }
            else
                {
                /* Third Buffer is available, set Buffer offset             */
                TXBufOffset = 0x08;
                }
            }
        else
            {
            /* Second Buffer is available, set Buffer offset                */
            TXBufOffset = 0x04;
            }
        }
    else
        {
        /* First Buffer is available, set Buffer offset                     */
        TXBufOffset = 0x00;
        }

    /* Write Data to Transmit Frame Information Register                    */
    pAddr = (UINT32 *) &CAN2TFI1 + TXBufOffset; /* Set pointer to CAN2TFIx  */ // \TODO WATCH HERE (had offset)!!!
    *pAddr = (pTransmitBuf->Dat1 & 0x000F0000L);

    /* Write CAN ID to Transmit Identifier Register                         */
    pAddr++;                                /* Increase pointer to CAN2TIDx */
    *pAddr = pTransmitBuf->Dat1 & 0x000007FFL;

    /* Write first four Bytes to Transmit Data Register A                   */
    pCandata = (UINT32 *) &(pTransmitBuf->DatA); /* Get first 4 bytes       */
    pAddr++;                                /* Set pointer to DataA register*/
    *pAddr = *pCandata;                     /* Write first 4 bytes to DataA */

    /* Write second four Bytes to Transmit Data Register B                  */
    pCandata++;                             /* Get second four bytes        */
    pAddr++;                                /* Point to DataB register      */
    *pAddr = *pCandata;                     /* Write second 4 Bytes to DataB*/
    

    /* Write Transmission Request to CAN2 Command Register
     * This Action is depending on the chosen transmission buffer (1-3)
     * Two bits must be written -   Bit (0): (TR) Transmission Request
     *                              Bit (5|6|7): (STBx) Select Buffer (1|2|3)
     */
    if (TXBufOffset == 0x00)
        {
        /* First buffer was chosen. Write TR and STB1                       */
        CAN2CMR = 0x21;
        }
    else if (TXBufOffset == 0x04)
        {
        /* Second buffer was chosen. Write TR and STB2                      */
        CAN2CMR = 0x41;
        }
    else if (TXBufOffset == 0x08)
        {
        /* Third buffer was chosen. Write TR and STB3                       */
        CAN2CMR = 0x81;
        }
    else
        {
        /* No Buffer was chosen, return with error (should not come here)   */
        return (FALSE);
        }

    return (TRUE);
}


UINT16 CANPullMessage (CAN_MSG *pReceiveBuf)
{
    UINT32 loopcnt = 0;
    UINT32 *pSrc;
    UINT32 *pDst;
    UINT32 match; 


    /* Initialise Source and Destination Pointer                            */
    pSrc = (UINT32 *) &(gCANList[0].Dat1);
    pDst = (UINT32 *) &(pReceiveBuf->Dat1);

    /* Prepare match value for CAN interface                                */
    match = CAN_PORT << 13;
    match |= 0x03000000L;                   /* Semaphore bits are 11b       */

    while (loopcnt < gCANFilter)
        {
        /* Scan for every set Filter                                        */
        if ((*pSrc & 0x0300E000L) == match)
            {
            /* A new Message is detected in Source Array                    */
            *pSrc &= 0xFCFFFFFFL;           /* clear Semaphore              */

            *pDst = *pSrc;                  /* Copy Dat1 from SRC to DST    */

            pSrc++;                         /* Set SRC-Pointer to SRC.DataA */
            pDst++;                         /* Set DST-Pointer to DST.DataA */

            *pDst = *pSrc;                  /* Copy DatA from SRC to DST    */

            pSrc++;                         /* Set SRC-Pointer to SRC.DataB */
            pDst++;                         /* Set DST-Pointer to DST.DataB */

            *pDst = *pSrc;                  /* Copy DatB from SRC to DST    */

            pSrc -= 2;                      // Reset SRC-Pointer to Dat1    */
            pDst -= 2;                      // Reset DST-Pointer to Dat1    */

            if ((*pSrc & 0x03000000L) == 0)
                { 
                /* If actual SRC was updated while reading, return to caller
                 * This is to prevent of ignoring the message that came in while
                 * reading, which would not be noticed if the message counter
                 * will be increased like the loop will do in the next step
                 */
                return (TRUE);
                }
            }
        loopcnt++;                          /* Go to next Message in Buffer */
        pSrc += 3;                          /* Increase SRC-Pointer to next */
        }
    return (FALSE);                         /* Return False if no Message in*/ 
}


void CAN_IRS_Handler()
{
    UINT32  CANStatus;
    
    ISR_ENTRY();
    CANStatus = CAN_RX_SR;
    if (CANStatus & (1 << 8))
        {
        /* A received Message is available in CAN1 controller               */
        CAN_CANISR_Rx1();
        }
    if (CANStatus & (1 << 9))
        {
        /* A received Message is available in CAN2 controller               */
        CAN_CANISR_Rx2();
        }

/* Error Interrupts are currently disabled due to purpose of CAN driver     */    
    if (CAN_MSR & (1 << 1))
        {
        /* At least one Error-Counter of CAN2 has reached the limit         */
        CAN_CANISR_Err();
        }    
    if (CAN1GSR & (1 << 6 ))
        {        
        /* The error count includes both TX and RX */
        }
    if (CAN2GSR & (1 << 6 ))
        {
        /* The error count includes both TX and RX */
        }
    if (CAN2ICR & (1 << 7))
        {
        /* Error-on-Bus Interrupt detected                                  */
        CAN_ERRORBUS();
        }
    
    VICVectAddr = 0; /* Acknowledge Interrupt */

    ISR_EXIT();
}


void CAN_CANISR_Rx1 (void) 
{
UINT32 buf;
UINT32 *pDest;

  if (!(CAN1RFS & 0xC0000400L))
  { // 11-bit ID, no RTR, matched a filter

    // initialize destination pointer
    // filter number is in lower 10 bits of C1RFS
    pDest = (UINT32 *) &(gCANList[(CAN1RFS & 0x000003FFL)].Dat1);
    
    // calculate contents for first entry into CAN list
    buf = CAN1RFS & 0xC00F0000L; // mask FF, RTR and DLC
    buf |= 0x01002000L; // set semaphore to 01b and CAN port to 1
    buf |= CAN1RID & 0x000007FFL; // get CAN message ID

    // now copy entire message to CAN list
    *pDest = buf; 
    pDest++; // set to gCANList[(C1RFS & 0x000003FFL)].DatA
    *pDest = CAN1RDA; 
    pDest++; // set to gCANList[(C1RFS & 0x000003FFL)].DatB
    *pDest = CAN1RDB; 

    // now set the sempahore to complete
    buf |= 0x03000000L; // set semaphore to 11b
    pDest -= 2; // set to gCANList[(C1RFS & 0x000003FFL)].Dat1
    *pDest = buf; 
  }

  CAN1CMR = 0x04; // release receive buffer
}


void CAN_CANISR_Rx2(void)
{
    UINT32 buf;
    UINT32 *pDest;
    
    debugVar = 0x00000001;

    if (!(CAN2RFS & 0xC0000400L))
        { 
        /* 11-bit ID, no RTR                                                */             
        debugVar = 0x00000011;
        /* initialize Destination Pointer                                   */
        /* Filter Nmber is in bit 0-9 of CAN2RFS                            */
        pDest = (UINT32 *) &(gCANList[(CAN2RFS & 0x000003FFL)].Dat1);
//        pDest = (UINT32 *) &(gCANList[(1)].Dat1);

        /* Calculate contents for first entry into CAN list                 */
        buf = CAN2RFS & 0xC00F0000L;        /* Mask FF, RTR and DLC         */
        buf |= 0x01004000L;                 /* set sema to 01b and port to 2*/
        buf |= CAN2RID & 0x000007FFL;       /* get CAN message ID           */

        /* Copy entire message to CAN list                                  */
        *pDest = buf;                       /* Copy Dat1 to DST             */
        pDest++;                            /* Set Pointer to DataA         */
        *pDest = CAN2RDA;                   /* copy DataA                   */
        pDest++;                            /* Set Pointer to DataB         */
        *pDest = CAN2RDB;                   /* Copy DataB                   */

        
        buf |= 0x03000000L;                 /* semaphore to 11b (complete)  */
        pDest -= 2;                         /* Set back to Dat1             */
        *pDest = buf;                       /* update semaphore in DST      */
        }
    
    debugCnt++;
    CAN2CMR = 0x04;                         /* Release Receive Buffer       */
}


void CAN_CANISR_Err (void) 
{

}

void CAN_ERRORBUS (void)
{
    volatile UINT32 regRead;
    
    regRead = CAN2ICR;
    
    debugPrint("released");
}
