/** Find Oldest MOB
* Searches the CAN timestamp and returns the
* can-mob with the oldest timestamp.
*/
static inline uint8_t can_find_oldest_mob(void) {
static uint8_t mob, mob_winner;
static uint16_t mob_time;
static uint32_t time;
static uint16_t diff, diff_highest;
diff_highest = 0;
for (mob = 0; mob <= 14; mob++) {
CAN_SET_MOB(mob);
if (CANSTMOB) {
time = CANTIMH << 8 | CANTIML;
mob_time = CANSTMH << 8 | CANSTML;
/* Check for overflow */
if (mob_time > time) {
diff = (time + 0xFFFF) - mob_time;
} else {
diff = time - mob_time;
}
if (diff >= diff_highest) {
mob_winner = mob;
diff_highest = diff;
}
}
}
return mob_winner;
}
int can_send(can_id_t id, uint8_t data[], uint8_t dlc, CSP_BASE_TYPE * task_woken) {
int i, m = -1;
/* Disable CAN interrupt while looping MOBs */
CAN_CLEAR_INTERRUPT();
/* Disable interrupts while looping mailboxes */
if (task_woken == NULL) {
portENTER_CRITICAL();
}
/* Search free MOB from 0 -> CAN_TX_MOBs */
for(i = 0; i < CAN_TX_MOBS; i++) {
if (mbox[i] == MBOX_FREE && !(CANEN2 & (1 << i))) {
mbox[i] = MBOX_USED;
m = i;
break;
}
}
/* Enable interrupts */
if (task_woken == NULL) {
portEXIT_CRITICAL();
}
/* Enable CAN interrupt */
CAN_SET_INTERRUPT();
/* Return if no available MOB was found */
if (m < 0) {
csp_log_warn("TX overflow, no available MOB\r\n");
return -1;
}
/* Select and clear mob */
CAN_SET_MOB(m);
CAN_MOB_ABORT();
CAN_CLEAR_STATUS_MOB();
/* Set identifier */
CAN_SET_EXT_ID(id);
/* Set data - CANMSG is auto incrementing */
for (i = 0; i < dlc; i++)
CANMSG = data[i];
/* Set DLC */
CAN_CLEAR_DLC();
CAN_SET_DLC(dlc);
/* Start TX */
CAN_CONFIG_TX();
return 0;
}
PROCESS_THREAD(can_drv_process, ev, data) {
static can_cmd_t * cmd;
static uint8_t mob_handle;
uint8_t do_create_recv_buf = 0;
PROCESS_BEGIN();
for(;;) {
// wait on polling or timer event
PROCESS_WAIT_EVENT();
// Post event about completed commands and free the mob
for(mob_handle = 0; mob_handle < NB_MOB; ++mob_handle) {
cmd = can_mob[mob_handle];
do_create_recv_buf = 0;
if(cmd != NULL) {
if(cmd->status != MOB_PENDING) {
if(cmd->status & MOB_TX_COMPLETED) {
ftimer_unregister_func(cmd->ftimer_id);
can_send_callback(CAN_DRV_RC_NORM, cmd->frame, cmd->context);
} else if(cmd->status & MOB_RX_COMPLETED) {
can_recv_callback(cmd->frame);
do_create_recv_buf = 1;
}
can_mob[mob_handle] = NULL;
free(cmd);
CAN_SET_MOB(mob_handle);
CAN_MOB_ABORT();
}
}
if(do_create_recv_buf) {
create_recv_buf();
}
}
// If waiting queue not empty, try to schedule waiting commands
while( list_head(can_cmd_list) != 0 ) {
if(can_get_mob_free() != NO_MOB) {
cmd = list_pop(can_cmd_list);
post_cmd(cmd);
} else {
break;
}
}
}
PROCESS_END();
}
static void abort_cmd(can_cmd_t * cmd) {
uint8_t cpt;
if(cmd == NULL) return;
if(cmd->next != NULL) {
// remove command from waiting queue
list_remove(can_cmd_list, cmd);
} else {
// remove from pending table
for(cpt = 0; cpt < NB_MOB; ++cpt) {
if(cmd == can_mob[cpt]) {
can_mob[cpt] = NULL;
CAN_SET_MOB(cpt);
CAN_MOB_ABORT();
break;
}
}
}
}
static void int_handler(uint16_t source) {
uint8_t mob_handle;
can_cmd_t * cmd;
mob_handle = 0;
while(source != 0) {
if(source & 1) {
cmd = can_mob[mob_handle];
if(cmd != NULL) {
CAN_SET_MOB(mob_handle);
cmd->status = CAN_GET_STATUS_MOB() ;
// RX operation finished
if(cmd->status & MOB_RX_COMPLETED_DLCW) {
cmd->frame->data.len = CAN_GET_DLC();
can_get_data(cmd->frame->data.itself);
cmd->frame->inf.rtr = CAN_GET_RTR();
if (CAN_GET_IDE()) {
cmd->frame->inf.ide = 1;
CAN_GET_EXT_ID(cmd->frame->inf.id);
} else {
cmd->frame->inf.ide = 0;
CAN_GET_STD_ID(cmd->frame->inf.id);
}
}
}
// reset MOb status
CAN_CLEAR_STATUS_MOB();
}
++mob_handle;
source >>= 1;
}
process_poll(&can_drv_process);
}
void can_configure_mobs(void) {
int mob;
/* Initialize MOBs */
for (mob = 0; mob < CAN_MOBS; mob++) {
/* Set MOB */
CAN_SET_MOB(mob);
CAN_CLEAR_MOB();
/* Only RX mobs require special initialization */
if (mob >= CAN_TX_MOBS) {
/* Set id and mask */
CAN_SET_EXT_ID(can_id);
CAN_SET_EXT_MSK(can_mask);
/* Accept extended frames */
CAN_CLEAR_IDEMSK();
CAN_CONFIG_RX();
}
}
}
static void post_cmd(can_cmd_t * cmd) {
uint8_t mob_handle;
uint8_t i;
if(cmd == NULL) return;
mob_handle = can_get_mob_free();
// add command to wait queue if there are no free mobs
if(mob_handle == NO_MOB) {
cmd->status = MOB_NOT_COMPLETED;
list_add(can_cmd_list, cmd);
}
// add command to table of pending command otherwise
else {
can_mob[mob_handle] = cmd;
cmd->status = MOB_PENDING;
CAN_SET_MOB(mob_handle);
CAN_CLEAR_MOB();
switch(cmd->type) {
case TX:
if(cmd->frame->inf.ide) {
CAN_SET_EXT_ID(cmd->frame->inf.id);
} else {
CAN_SET_STD_ID(cmd->frame->inf.id);
}
for(i = 0; i < cmd->frame->data.len; ++i) {
CANMSG = cmd->frame->data.itself[i];
}
if(cmd->frame->inf.rtr) {
CAN_SET_RTR();
} else {
CAN_CLEAR_RTR();
}
CAN_SET_DLC(cmd->frame->data.len);
CAN_CONFIG_TX();
break;
case RX:
if (cmd->frame->inf.ide) {
CAN_SET_EXT_ID(can_masked_id);
CAN_SET_IDEMSK();
CAN_SET_EXT_MSK(can_mask);
} else {
CAN_SET_STD_ID(can_masked_id);
CAN_CLEAR_IDEMSK();
CAN_SET_STD_MSK(can_mask);
}
CAN_SET_DLC(cmd->frame->data.len);
CAN_CLEAR_RTRMSK();
CAN_CONFIG_RX();
break;
default:
cmd->status = STATUS_CLEARED;
break;
}
}
}
