411 lines
6.7 KiB
C
411 lines
6.7 KiB
C
/**
|
|
* candevice.c
|
|
* @param config
|
|
*/
|
|
|
|
#include <can/can.h>
|
|
#include <sys/errno.h>
|
|
#include <sys/cpu.h>
|
|
#include <sys/systick.h>
|
|
|
|
#include <hwo/bits.h>
|
|
|
|
#include <avr/io.h>
|
|
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
|
|
|
|
// How many mailboxes to use for RX
|
|
#define CAN_RX_MAILBOXES 12
|
|
|
|
|
|
static _can_device_t
|
|
device;
|
|
|
|
int can_prepare_mailbox_rx(int mailbox);
|
|
|
|
uint32_t can_get_device_config(void){
|
|
return device.setup;
|
|
};
|
|
|
|
int can_init_device(uint32_t config,int rxpool_length) {
|
|
uint8_t n,i;
|
|
|
|
memset( &device, 0x00, sizeof(device) );
|
|
|
|
list_init( &(device.tx_queue) );
|
|
|
|
for (n=0;n<16;n++)
|
|
{
|
|
CANPAGE = (n << 4);
|
|
|
|
CANCDMOB= 0x00;
|
|
CANSTMOB= 0x00;
|
|
CANIDT1 = 0x00;
|
|
CANIDT2 = 0x00;
|
|
CANIDT3 = 0x00;
|
|
CANIDT4 = 0x00;
|
|
CANIDM1 = 0x00;
|
|
CANIDM2 = 0x00;
|
|
CANIDM3 = 0x00;
|
|
CANIDM4 = 0x00;
|
|
CANSTMH = 0x00;
|
|
CANSTML = 0x00;
|
|
|
|
for (i=0;i<8;i++)
|
|
CANMSG = 0x00;
|
|
};
|
|
|
|
CANGCON = _BV(SWRES);
|
|
|
|
device.setup = config;
|
|
device.rxpool = malloc( sizeof(canframe_t) * rxpool_length );
|
|
if (device.rxpool == NULL){
|
|
return -ENOMEM;
|
|
};
|
|
memset( device.rxpool, 0x00, sizeof(canframe_t) * rxpool_length );
|
|
device.rxpool_length = rxpool_length;
|
|
|
|
switch (__freq_cpu)
|
|
{
|
|
case 16000000:
|
|
switch (config & CAN_SPEED_MASK)
|
|
{
|
|
case CAN_1MBIT:
|
|
CANBT1 = 0x02;
|
|
CANBT2 = 0x04;
|
|
CANBT3 = 0x13;
|
|
break;
|
|
case CAN_125KBIT:
|
|
CANBT1 = 0x0E;
|
|
CANBT2 = 0x0C;
|
|
CANBT3 = 0x37;
|
|
break;
|
|
};
|
|
break;
|
|
case 8000000:
|
|
switch (config & CAN_SPEED_MASK)
|
|
{
|
|
case CAN_1MBIT:
|
|
CANBT1 = 0x00;
|
|
CANBT2 = 0x04;// 0x04;
|
|
CANBT3 = 0x12; //0x12;
|
|
break;
|
|
case CAN_125KBIT:
|
|
CANBT1 = 0x0E; //0x06;
|
|
CANBT2 = 0x04; //0x0c;
|
|
CANBT3 = 0x13; //0x37;
|
|
break;
|
|
};
|
|
break;
|
|
};
|
|
|
|
CANTCON = CANBT1;
|
|
|
|
CANGIE = _BV(ENIT) | _BV(ENRX) | _BV(ENTX) | _BV(ENERR);
|
|
CANIE1 = 0x7F;
|
|
CANIE2 = 0xFF;
|
|
|
|
CANGCON |= _BV(ENASTB);
|
|
|
|
{
|
|
ATOMIC
|
|
|
|
for (n=0; n < CAN_RX_MAILBOXES; n++){
|
|
can_prepare_mailbox_rx(n);
|
|
}
|
|
|
|
}
|
|
|
|
CANPAGE = 0;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
|
|
|
|
static inline uint32_t canid_to_register(canid_t* cid){
|
|
bits32_t raw;
|
|
|
|
raw.ui32 = cid->addr;
|
|
|
|
if (cid->ide){
|
|
raw.ui32 <<= 3;
|
|
} else
|
|
{
|
|
raw.ui32 <<= 21;
|
|
};
|
|
|
|
if (cid->rtr) {
|
|
raw.ui8[0] |= _BV(RTRTAG);
|
|
};
|
|
|
|
return raw.ui32;
|
|
};
|
|
|
|
static inline void canid_from_register(canid_t* cid){
|
|
bits32_t raw;
|
|
|
|
raw.ui32 = CANIDT;
|
|
|
|
cid->ide = (CANCDMOB & _BV(IDE)) ? 1 : 0;
|
|
if (cid->ide){
|
|
cid->addr = raw.ui32 >> 3;
|
|
} else {
|
|
cid->addr = raw.ui16[1] >> 5;
|
|
};
|
|
cid->rtr = (raw.ui8[0] & _BV(RTRTAG)) ? 1 : 0;
|
|
cid->reserved = 0;
|
|
};
|
|
|
|
/**
|
|
* @brief test canframe for matching canfilter criteria
|
|
* @param filter canfilter to be used for test
|
|
* @param frame frame to be tested
|
|
* @return 1 if filter matches frame, 0 if no match
|
|
*/
|
|
static inline int can_filter_match(canfilter_t *filter,canframe_t *frame){
|
|
canid_t id1,id2;
|
|
|
|
id1.value = filter->id.value & filter->mask.value;
|
|
id2.value = frame->id.value & filter->mask.value;
|
|
|
|
return (id1.value == id2.value) ? 1 : 0;
|
|
};
|
|
|
|
int can_prepare_mailbox_rx(int mailbox) {
|
|
|
|
ATOMIC
|
|
|
|
if (mailbox < 15)
|
|
{
|
|
CANPAGE = mailbox << 4;
|
|
|
|
CANSTMOB = 0x00;
|
|
CANCDMOB = 0x00;
|
|
|
|
CANIDT = 0;
|
|
CANIDM = 0;
|
|
|
|
CANCDMOB = _BV(CONMOB1);
|
|
|
|
} else {
|
|
return -EPARAM;
|
|
};
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
/**
|
|
* @brief copy frame to frame_t* from hardware registers
|
|
* @param frame target frame_t
|
|
* @return 0 on success
|
|
*/
|
|
int can_frame_from_mailbox(canframe_t *frame){
|
|
uint8_t n;
|
|
|
|
frame->frame_id = device.frame_id_rx++;
|
|
frame->flags.len = (CANCDMOB & 0x0F);
|
|
canid_from_register( &(frame->id) );
|
|
|
|
|
|
for (n=0;n<8;n++){
|
|
frame->payload.bytes[n] = CANMSG;
|
|
};
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int can_next_frame_tx(void){
|
|
canframe_t *frame = list_first_entry( &(device.tx_queue), canframe_t, list );
|
|
|
|
uint8_t tmp = CANPAGE;
|
|
CANPAGE = (14 << 4);
|
|
|
|
CANSTMOB = 0x00;
|
|
CANCDMOB = 0x00;
|
|
|
|
if (frame){
|
|
uint8_t n;
|
|
|
|
CANCDMOB = (frame->flags.len & 0x0f) | (frame->id.ide ? _BV(IDE) : 0);
|
|
|
|
CANIDT = canid_to_register( &frame->id );
|
|
for (n=0;n<8;n++){
|
|
CANMSG = frame->payload.bytes[n];
|
|
};
|
|
|
|
CANCDMOB |= _BV(CONMOB0);
|
|
};
|
|
|
|
CANPAGE = tmp;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
|
|
void can_vect(void){
|
|
uint16_t sit = CANSIT2 | (CANSIT1 << 8);
|
|
uint8_t n;
|
|
|
|
uint8_t cptemp = CANPAGE;
|
|
|
|
if (sit & 0x7FFF)
|
|
{
|
|
for (n=0;n<15;n++)
|
|
{
|
|
if (sit & _BV(n))
|
|
{
|
|
CANPAGE = (n << 4);
|
|
CANCDMOB &= ~(_BV(CONMOB0) | _BV(CONMOB1));
|
|
|
|
if (n < CAN_RX_MAILBOXES){
|
|
if (CANSTMOB & 0x1F) {
|
|
// Ein Fehler trat auf
|
|
|
|
} else {
|
|
// Empfangen
|
|
can_frame_from_mailbox( &(device.rxpool[ device.rxpool_next++ ]) );
|
|
|
|
if (device.rxpool_next >= device.rxpool_length){
|
|
device.rxpool_next = 0;
|
|
};
|
|
};
|
|
can_prepare_mailbox_rx(n);
|
|
|
|
} else if (n == 14) {
|
|
|
|
canframe_t* tx = list_first_entry( &(device.tx_queue), canframe_t, list);
|
|
if (tx){
|
|
tx->frame_id = device.frame_id_tx++;
|
|
tx->flags.error = (CANSTMOB & 0x1F) ? 1 : 0;
|
|
tx->flags.tx = 0;
|
|
list_remove( &(tx->list) );
|
|
thread_wake( tx->notify );
|
|
};
|
|
can_next_frame_tx();
|
|
|
|
};
|
|
};
|
|
};
|
|
};
|
|
|
|
CANPAGE = cptemp;
|
|
};
|
|
|
|
|
|
VECT(CANIT_vect){
|
|
can_vect();
|
|
};
|
|
|
|
|
|
|
|
/* USER API */
|
|
|
|
int can_send(canframe_t *frame){
|
|
if (frame){
|
|
ATOMIC
|
|
|
|
int empty = list_is_empty( &(device.tx_queue) );
|
|
|
|
frame->notify = current_thread();
|
|
list_append( &(frame->list ), &(device.tx_queue) );
|
|
|
|
if (empty){
|
|
can_next_frame_tx();
|
|
};
|
|
|
|
wait_ms( 10 );
|
|
|
|
if (!list_is_empty( &(frame->list) ) ){
|
|
list_remove( &(frame->list) );
|
|
};
|
|
|
|
if (frame->flags.tx || frame->flags.error){
|
|
return -EFAIL;
|
|
};
|
|
|
|
return ESUCCESS;
|
|
};
|
|
return -EFAIL;
|
|
};
|
|
|
|
int can_send_simple(canid_t cid,uint8_t len,canpayload_t payload){
|
|
canframe_t *frame = malloc(sizeof(canframe_t));
|
|
int r;
|
|
|
|
if (frame){
|
|
memset( frame, 0x00, sizeof(canframe_t) );
|
|
frame->flags.tx = 1;
|
|
frame->flags.len = len;
|
|
frame->id = cid;
|
|
frame->payload = payload;
|
|
|
|
r = can_send( frame );
|
|
|
|
free(frame);
|
|
|
|
return r;
|
|
} else {
|
|
return -ENOMEM;
|
|
}
|
|
};
|
|
|
|
int can_recv(canfilter_t *filter,canframe_t *frame){
|
|
ATOMIC
|
|
|
|
int n;
|
|
|
|
if (!frame){
|
|
return -ENULLPTR;
|
|
}
|
|
|
|
n = device.rxpool_next;
|
|
do {
|
|
if ((device.rxpool[n].frame_id > filter->last_seen_frame_id) || (n < device.rxpool_next)){
|
|
|
|
if (can_filter_match( filter, &(device.rxpool[n]) )){
|
|
*frame = device.rxpool[n];
|
|
|
|
filter->last_seen_frame_id = frame->frame_id;
|
|
|
|
memset( &device.rxpool[n], 0x00, sizeof(canframe_t) );
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
};
|
|
|
|
filter->last_seen_frame_id = device.rxpool[n].frame_id;
|
|
|
|
n++;
|
|
if (n == device.rxpool_length){
|
|
n = 0;
|
|
};
|
|
|
|
} while (device.rxpool_next != n);
|
|
|
|
return -EFAIL;
|
|
};
|
|
|
|
int can_recv_timeout(canfilter_t *filter,canframe_t *frame,int ms){
|
|
int n,r;
|
|
|
|
for (n = 0; n < ms; n++){
|
|
r = can_recv( filter, frame );
|
|
if (r == ESUCCESS){
|
|
return ESUCCESS;
|
|
};
|
|
wait_ms(1);
|
|
};
|
|
|
|
return -ETIMEOUT;
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|