can_accfilt_lpc2xxx.c

开放源码实时操作系统源码.

    } // if (pLine < pExtGrpStart)
          
    if (CAN_ACCFILT_EFF_GRP_SA == Table)
    {
        //
        // If we are in the area of extended groups then we need to insert
        // 2 lines because lower and upper identifier need 1 line each
        //
        entry_size += sizeof(cyg_uint32); // one entry is 2 dword long
    }
    
    entry_address = end_of_table - sizeof(cyg_uint32);
    end_of_table  += entry_size;  // add one additional entry
        
    //
    // Move alle entries one or two dwords upwards - that means we insert a new empty line
    //
    while (entry_address >= copy_start)
    {
        HAL_READ_UINT32(CAN_ACCFILT_RAM_BASE + (cyg_uint32)entry_address, lsc_val.dword);
        HAL_WRITE_UINT32(CAN_ACCFILT_RAM_BASE + (cyg_uint32)entry_address + entry_size, lsc_val.dword);
        entry_address -= sizeof(cyg_uint32);  
    }
    
    //
    // For the std ID area we need a special pocedure
    //
    if (CAN_ACCFILT_SFF_SA == Table)
    {
        lsc_buf_t preval;
        //
        // Start copy with last entry of std id table
        //
        entry_address = sff_grp_sa - sizeof(cyg_uint32);    
                
        while (entry_address > insert_address)
        {
            HAL_READ_UINT32(CAN_ACCFILT_RAM_BASE + (cyg_uint32)entry_address, lsc_val.dword);
            HAL_READ_UINT32(CAN_ACCFILT_RAM_BASE + (cyg_uint32)entry_address - sizeof(cyg_uint32), preval.dword);
            lsc_val.column.upper = lsc_val.column.lower;
            lsc_val.column.lower = preval.column.upper;
            HAL_WRITE_UINT32(CAN_ACCFILT_RAM_BASE + (cyg_uint32)entry_address, lsc_val.dword);
            entry_address -= sizeof(cyg_uint32);
        }
        
        //
        // If we insert an entry into the lower colum, then we need to move the
        // content of the lower column into the upper column
        //
        if (!(EntryNo % 2))
        {
            HAL_READ_UINT32(CAN_ACCFILT_RAM_BASE + (cyg_uint32)insert_address, lsc_val.dword);
            lsc_val.column.upper = lsc_val.column.lower;
            HAL_WRITE_UINT32(CAN_ACCFILT_RAM_BASE + (cyg_uint32)insert_address, lsc_val.dword);
        }
        
        //
        // If we inserted a new line, then we have an odd number of identifiers now
        // and need to disable the last (the upper) entry
        //
        if (entry_size)
        {
            HAL_READ_UINT32(CAN_ACCFILT_RAM_BASE + sff_grp_sa - sizeof(cyg_uint32) , lsc_val.dword);
            lsc_val.column.upper = 0xFFFF;  // disable the entry
            HAL_WRITE_UINT32(CAN_ACCFILT_RAM_BASE + sff_grp_sa - sizeof(cyg_uint32) , lsc_val.dword);
        }
    }
    
    HAL_WRITE_UINT32(CAN_ACCFILT_SFF_SA, sff_sa);
    HAL_WRITE_UINT32(CAN_ACCFILT_SFF_GRP_SA, sff_grp_sa);
    HAL_WRITE_UINT32(CAN_ACCFILT_EFF_SA, eff_sa);      
    HAL_WRITE_UINT32(CAN_ACCFILT_EFF_GRP_SA, eff_grp_sa);
    HAL_WRITE_UINT32(CAN_ACCFILT_ENDOFTABLE, end_of_table);
}


//===========================================================================
// Query number of entries in a certain table
//===========================================================================
static cyg_uint16 lpc2xxx_can_accfilt_get_table_entries(cyg_uint32 TableStartAddress)
{
    cyg_uint32  start;
    cyg_uint32  end;
    
    switch (TableStartAddress)
    {
#ifdef CYGOPT_IO_CAN_STD_CAN_ID
    case CAN_ACCFILT_SFF_SA:
         HAL_READ_UINT32(CAN_ACCFILT_SFF_SA,     start);
         HAL_READ_UINT32(CAN_ACCFILT_SFF_GRP_SA, end);
         if (end - start)
         {
             lsc_buf_t data;
             HAL_READ_UINT32(CAN_ACCFILT_RAM_BASE + end - sizeof(cyg_uint32), data.dword);
             if (data.column.upper & ACCFILT_STD_DIS)
             {
                 return (((end - start) >> 1) - 1);
             }
         }
         return (end - start) >> 1;
         
    case CAN_ACCFILT_SFF_GRP_SA:
         HAL_READ_UINT32(CAN_ACCFILT_SFF_GRP_SA, start);
         HAL_READ_UINT32(CAN_ACCFILT_EFF_SA,     end);
         return (end - start) >> 2;
#endif // CYGOPT_IO_CAN_STD_CAN_ID

#ifdef CYGOPT_IO_CAN_EXT_CAN_ID        
    case CAN_ACCFILT_EFF_SA:
         HAL_READ_UINT32(CAN_ACCFILT_EFF_SA,     start);
         HAL_READ_UINT32(CAN_ACCFILT_EFF_GRP_SA, end);
         return (end - start) >> 2;
                 
    case CAN_ACCFILT_EFF_GRP_SA:
         HAL_READ_UINT32(CAN_ACCFILT_EFF_GRP_SA, start);
         HAL_READ_UINT32(CAN_ACCFILT_ENDOFTABLE, end);
         return (end - start) >> 3;
#endif // CYGOPT_IO_CAN_EXT_CAN_ID
    default:
        CYG_FAIL("Invalid identifier table address");
        return 0;         
    } // switch (TableStartAddress)
}

//===========================================================================
// Query certain entry from table
//===========================================================================
static void lpc2xxx_can_accfilt_get_entry(cyg_uint32 TableStartAddress, cyg_uint16 EntryNo, lpc2xxx_accfilt_entry_t *pEntry)
{
    cyg_uint32 EntryAddress = lpc2xxx_can_accfilt_calc_entry_address(TableStartAddress, EntryNo);
    lsc_buf_t  Data;

    HAL_READ_UINT32(CAN_ACCFILT_RAM_BASE + EntryAddress, Data.dword);
    pEntry->data = Data.dword;
    switch (TableStartAddress)
    {
#ifdef CYGOPT_IO_CAN_STD_CAN_ID
    case CAN_ACCFILT_SFF_SA:
    {
         cyg_uint16 column; 
         if (EntryNo % 2)
         {
             column = Data.column.upper;
         }
         else
         {
             column = Data.column.lower; 
         }
         pEntry->id         = ACCFILT_STD_GET_ID(column);
         pEntry->channel_no = ACCFILT_STD_GET_CTRL(column);
    }
    break;
                 
    case CAN_ACCFILT_SFF_GRP_SA:
         pEntry->lower_id_bound = ACCFILT_STD_GET_ID(Data.column.lower);
         pEntry->upper_id_bound = ACCFILT_STD_GET_ID(Data.column.upper);
         pEntry->channel_no     = ACCFILT_STD_GET_CTRL(Data.column.lower);
         break;
#endif // #ifdef CYGOPT_IO_CAN_STD_CAN_ID
    
#ifdef CYGOPT_IO_CAN_EXT_CAN_ID
    case CAN_ACCFILT_EFF_SA:
         pEntry->id         = ACCFILT_EXT_GET_ID(Data.dword);
         pEntry->channel_no = ACCFILT_EXT_GET_CTRL(Data.dword);
         break;
                        
    case CAN_ACCFILT_EFF_GRP_SA:
         pEntry->lower_id_bound = ACCFILT_EXT_GET_ID(Data.dword);
         pEntry->channel_no     = ACCFILT_EXT_GET_CTRL(Data.dword);
         HAL_READ_UINT32(CAN_ACCFILT_RAM_BASE+ EntryAddress + sizeof(cyg_uint32), Data.dword);
         pEntry->upper_id_bound = ACCFILT_EXT_GET_ID(Data.dword);
         break;
#endif // #ifedf CYGOPT_IO_CAN_EXT_CAN_ID
    default:
        CYG_FAIL("Invalid identifier table address");
    } // switch ()
}


//===========================================================================
// Set certain entry in table
//===========================================================================
static void lpc2xxx_can_accfilt_set_entry(cyg_uint32 TableStartAddress, cyg_uint16 EntryNo, lpc2xxx_accfilt_entry_t *pEntry)
{
    cyg_uint32 EntryAddress = lpc2xxx_can_accfilt_calc_entry_address(TableStartAddress, EntryNo);
    lsc_buf_t Data;

    switch (TableStartAddress)
    {
#ifdef CYGOPT_IO_CAN_STD_CAN_ID
    case CAN_ACCFILT_SFF_SA:
         {
             HAL_READ_UINT32(CAN_ACCFILT_RAM_BASE + EntryAddress, Data.dword);            
             if (EntryNo % 2)
             {
                 Data.column.upper = (pEntry->channel_no << 13) | (pEntry->id & ACCFILT_STD_ID_MASK);
             }
             else
             {
                 Data.column.lower = (pEntry->channel_no << 13) | (pEntry->id & ACCFILT_STD_ID_MASK);
             }
         }
         break;
                 
    case CAN_ACCFILT_SFF_GRP_SA:
         Data.column.lower = (pEntry->channel_no << 13) | (pEntry->lower_id_bound & ACCFILT_STD_ID_MASK);
         Data.column.upper = (pEntry->channel_no << 13) | (pEntry->upper_id_bound & ACCFILT_STD_ID_MASK);
         break;
#endif // #ifdef CYGOPT_IO_CAN_STD_CAN_ID

#ifdef CYGOPT_IO_CAN_EXT_CAN_ID
    case CAN_ACCFILT_EFF_SA:
         Data.dword = (pEntry->channel_no << 29) | (pEntry->id & ACCFILT_EXT_ID_MASK); 
         break;
                        
    case CAN_ACCFILT_EFF_GRP_SA:
         {
             lsc_buf_t Data2;
             
             Data.dword  = (pEntry->channel_no << 29) | (pEntry->lower_id_bound & ACCFILT_EXT_ID_MASK);
             Data2.dword = (pEntry->channel_no << 29) | (pEntry->upper_id_bound & ACCFILT_EXT_ID_MASK);
             HAL_WRITE_UINT32(CAN_ACCFILT_RAM_BASE + EntryAddress + sizeof(cyg_uint32), Data2.dword);
         }
         break;
#endif // #ifdef CYGOPT_IO_CAN_EXT_CAN_ID
         
    default:
        CYG_FAIL("Invalid identifier table address");
    } // switch ()
    
    HAL_WRITE_UINT32(CAN_ACCFILT_RAM_BASE + EntryAddress, Data.dword);
}


//===========================================================================
// Add one entry to acceptance filter RAM
// If upper ID is > lower ID then we have to add a group filter - else we
// have to add a single message filter here
//===========================================================================
static bool lpc2xxx_can_accfilt_add(lpc2xxx_can_info_t *info, 
                                    cyg_uint32          lower_id, 
                                    cyg_uint32          upper_id, 
                                    cyg_can_id_type     ext)
{
    cyg_uint32              accfilt_bck; // acceptance filter backup
    cyg_uint32              end_of_table;
    cyg_uint32              table;
    lpc2xxx_accfilt_entry_t entry;
    lpc2xxx_accfilt_entry_t new_entry;
    
    
    //
    // first step: disable acceptance filter and prepare it for modification
    //
    HAL_READ_UINT32(CAN_ACCFILT_AFMR, accfilt_bck);
    HAL_WRITE_UINT32(CAN_ACCFILT_AFMR, AFMR_OFF | AFMR_BYPASS);
    
    //
    // Check if table is full
    //
    HAL_READ_UINT32(CAN_ACCFILT_ENDOFTABLE, end_of_table);
    if (end_of_table >= ACCFILT_RAM_SIZE)
    {
        return false;   
    }
    
    new_entry.id             = lower_id;
    new_entry.lower_id_bound = lower_id;
    new_entry.upper_id_bound = upper_id;
    
    //
    // Here we rely on the ISR vector ordering for calculaion of of channel number
    // Maybe this is not the right way for newer LPC parts
    //
    new_entry.channel_no = (CAN_ISRVEC(info) - CYGNUM_HAL_INTERRUPT_CAN1_TX) + 1; 
    
    //
    // If lower_id == upper_id then we know that we have to setup a single message filter 
    // here. If it is not equal the it is group of identifiers to receive
    //
    if ((lower_id == upper_id) || (lower_id > upper_id))
    {
        //
        // setup single message filter (standard or extended) here
        //
#ifdef CYGOPT_IO_CAN_EXT_CAN_ID
        if (ext)
        {
            table = CAN_ACCFILT_EFF_SA;
        }
        else
#endif // #ifdef CYGOPT_IO_CAN_EXT_CAN_ID
        {
#ifdef CYGOPT_IO_CAN_STD_CAN_ID
            table = CAN_ACCFILT_SFF_SA;
#endif // CYGOPT_IO_CAN_STD_CAN_ID
        }
    }
    else
    {
        //
        // setup single message filter (standard or extended) here
        //
#ifdef CYGOPT_IO_CAN_EXT_CAN_ID
        if (ext)
        {
            table = CAN_ACCFILT_EFF_GRP_SA;
        }
        else
#endif // #ifdef CYGOPT_IO_CAN_EXT_CAN_ID
        {
#ifdef CYGOPT_IO_CAN_STD_CAN_ID
            table = CAN_ACCFILT_SFF_GRP_SA;
#endif // #ifdef CYGOPT_IO_CAN_STD_CAN_ID
        }
    }
        
    cyg_uint16 entries = lpc2xxx_can_accfilt_get_table_entries(table);
    cyg_uint16 i;
    
           
    for (i = 0; i < entries; ++i)
    {
        lpc2xxx_can_accfilt_get_entry(table, i, &entry);

        if (entry.channel_no > new_entry.channel_no)
        {
            break;
        }