You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

521 lines
12 KiB

/*
* Copyright (C) 2016 OTA keys S.A.
*
* This file is subject to the terms and conditions of the GNU Lesser General
* Public License v2.1. See the file LICENSE in the top level directory for more
* details.
*/
/**
* @file
* @brief CAN Data Link Layer module
*
* This module contains the DLL interfaces for upper layer (raw_can_*)
* and devices (can_dll_*).
* It manages the connection between an device number and its candev thread.
*
*
* @author Toon Stegen <toon.stegen@altran.com>
* @author Vincent Dupont <vincent@otakeys.com>
* @author Aurelien Gonce <aurelien.gonce@altran.com>
*/
#include <errno.h>
#include <string.h>
#include "thread.h"
#include "can/dll.h"
#include "can/raw.h"
#include "can/device.h"
#include "can/pkt.h"
#include "can/common.h"
#include "can/router.h"
#include "utlist.h"
#define ENABLE_DEBUG (0)
#include "debug.h"
static candev_dev_t *candev_list[CAN_DLL_NUMOF];
static int candev_nb = 0;
static can_reg_entry_t *tx_list[CAN_DLL_NUMOF];
static mutex_t tx_lock = MUTEX_INIT;
static int _get_ifnum(kernel_pid_t pid)
{
for (int i = 0; i < candev_nb; i++) {
if (candev_list[i]->pid == pid) {
return i;
}
}
return -ENODEV;
}
int _send_pkt(can_pkt_t *pkt)
{
if (!pkt) {
return -ENOMEM;
}
msg_t msg;
int handle = pkt->handle;
mutex_lock(&tx_lock);
LL_APPEND(tx_list[pkt->entry.ifnum], &pkt->entry);
mutex_unlock(&tx_lock);
msg.type = CAN_MSG_SEND_FRAME;
msg.content.ptr = (void*) pkt;
if (msg_send(&msg, candev_list[pkt->entry.ifnum]->pid) <= 0) {
return -EOVERFLOW;
}
return handle;
}
int raw_can_send(int ifnum, const struct can_frame *frame, kernel_pid_t pid)
{
can_pkt_t *pkt;
assert(frame);
assert(ifnum < candev_nb);
pkt = can_pkt_alloc_tx(ifnum, frame, pid);
DEBUG("raw_can_send: ifnum=%d, id=0x%" PRIx32 " from pid=%" PRIkernel_pid ", handle=%d\n",
ifnum, frame->can_id, pid, pkt->handle);
return _send_pkt(pkt);
}
#ifdef MODULE_CAN_MBOX
int raw_can_send_mbox(int ifnum, const struct can_frame *frame, mbox_t *mbox)
{
can_pkt_t *pkt;
assert(frame);
assert(ifnum < candev_nb);
pkt = can_pkt_alloc_mbox_tx(ifnum, frame, mbox);
DEBUG("raw_can_send: ifnum=%d, id=0x%" PRIx32 ", handle=%d\n", ifnum, frame->can_id, pkt->handle);
return _send_pkt(pkt);
}
#endif
int raw_can_abort(int ifnum, int handle)
{
msg_t msg, reply;
can_pkt_t *pkt = NULL;
can_reg_entry_t *entry;
assert(ifnum < candev_nb);
DEBUG("raw_can_abort: ifnum=%u, handle=%d\n", ifnum, handle);
mutex_lock(&tx_lock);
LL_FOREACH(tx_list[ifnum], entry) {
pkt = container_of(entry, can_pkt_t, entry);
if (pkt->handle == handle) {
break;
}
}
LL_DELETE(tx_list[ifnum], entry);
mutex_unlock(&tx_lock);
if (pkt == NULL) {
DEBUG("raw_can_abort: no pkt\n");
return -ENODEV;
}
msg.type = CAN_MSG_ABORT_FRAME;
msg.content.ptr = pkt;
msg_send_receive(&msg, &reply, candev_list[ifnum]->pid);
can_pkt_free(pkt);
return 0;
}
static int register_filter_entry(can_reg_entry_t *entry, struct can_filter *filter, void *param)
{
msg_t msg, reply;
int ret;
DEBUG("register_filter_entry: ifnum=%d, filter=0x%" PRIx32 ", mask=0x%" PRIx32 ", param=%p\n",
entry->ifnum, filter->can_id, filter->can_mask, param);
ret = can_router_register(entry, filter->can_id, filter->can_mask, param);
if (ret < 0) {
return -ENOMEM;
}
else if (ret == 1) {
DEBUG("raw_can_subscribe_rx: filter=0x%" PRIx32 " already in use\n", filter->can_id);
return 0;
}
msg.type = CAN_MSG_SET_FILTER;
msg.content.ptr = filter;
msg_send_receive(&msg, &reply, candev_list[entry->ifnum]->pid);
if ((int) reply.content.value < 0) {
can_router_unregister(entry, filter->can_id, filter->can_mask, param);
return -ENOMEM;
}
return 0;
}
static int unregister_filter_entry(can_reg_entry_t *entry, struct can_filter *filter, void *param)
{
msg_t msg, reply;
int ret;
DEBUG("unregister_filter_entry: ifnum=%d, filter=0x%" PRIx32 ", mask=0x%" PRIx32 ", param=%p\n",
entry->ifnum, filter->can_id, filter->can_mask, param);
ret = can_router_unregister(entry, filter->can_id, filter->can_mask, param);
if (ret < 0) {
return -ENOMEM;
}
else if (ret == 1) {
DEBUG("raw_can_unsubscribe_rx: filter=0x%" PRIx32 " still in use\n", filter->can_id);
return 0;
}
msg.type = CAN_MSG_REMOVE_FILTER;
msg.content.ptr = filter;
msg_send_receive(&msg, &reply, candev_list[entry->ifnum]->pid);
if ((int) reply.content.value < 0) {
return -ENOMEM;
}
return 0;
}
int raw_can_subscribe_rx(int ifnum, struct can_filter *filter, kernel_pid_t pid, void *param)
{
assert(ifnum < candev_nb);
assert(filter);
can_reg_entry_t entry;
entry.ifnum = ifnum;
entry.target.pid = pid;
#ifdef MODULE_CAN_MBOX
entry.type = CAN_TYPE_DEFAULT;
#endif
return register_filter_entry(&entry, filter, param);
}
#ifdef MODULE_CAN_MBOX
int raw_can_subscribe_rx_mbox(int ifnum, struct can_filter *filter, mbox_t *mbox, void *param)
{
assert(ifnum < candev_nb);
assert(filter);
can_reg_entry_t entry;
entry.ifnum = ifnum;
entry.target.mbox = mbox;
entry.type = CAN_TYPE_MBOX;
return register_filter_entry(&entry, filter, param);
}
#endif
int raw_can_unsubscribe_rx(int ifnum, struct can_filter *filter, kernel_pid_t pid, void *param)
{
assert(ifnum < candev_nb);
assert(filter);
can_reg_entry_t entry;
entry.ifnum = ifnum;
entry.target.pid = pid;
#ifdef MODULE_CAN_MBOX
entry.type = CAN_TYPE_DEFAULT;
#endif
return unregister_filter_entry(&entry, filter, param);
}
#ifdef MODULE_CAN_MBOX
int raw_can_unsubscribe_rx_mbox(int ifnum, struct can_filter *filter, mbox_t *mbox, void *param)
{
assert(ifnum < candev_nb);
assert(filter);
can_reg_entry_t entry;
entry.ifnum = ifnum;
entry.target.mbox = mbox;
entry.type = CAN_TYPE_MBOX;
return unregister_filter_entry(&entry, filter, param);
}
#endif
int raw_can_free_frame(can_rx_data_t *frame)
{
int ret = can_router_free_frame((struct can_frame *)frame->data.iov_base);
can_pkt_free_rx_data(frame);
return ret;
}
int raw_can_get_can_opt(int ifnum, can_opt_t *opt)
{
msg_t msg, reply;
assert(ifnum < CAN_DLL_NUMOF);
if (!opt) {
return -ENOMEM;
}
opt->context = (uint16_t)candev_list[ifnum]->pid;
msg.type = CAN_MSG_GET;
msg.content.ptr = (void *)opt;
if (msg_send_receive(&msg, &reply, opt->context) != 1) {
return -EBUSY;
}
return (int) reply.content.value;
}
int raw_can_set_can_opt(int ifnum, can_opt_t *opt)
{
msg_t msg, reply;
assert(ifnum < CAN_DLL_NUMOF);
if (!opt) {
return -ENOMEM;
}
opt->context = (uint16_t)candev_list[ifnum]->pid;
msg.type = CAN_MSG_SET;
msg.content.ptr = (void *)opt;
if (msg_send_receive(&msg, &reply, opt->context) != 1) {
return -EBUSY;
}
return (int) reply.content.value;
}
int can_dll_register_candev(candev_dev_t *candev)
{
if (candev_nb >= CAN_DLL_NUMOF) {
return -ENODEV;
}
DEBUG("can_dll_register_candev: candev=%p, ifnum=%d, pid=%" PRIkernel_pid "\n",
(void *)candev, candev_nb, candev->pid);
candev_list[candev_nb] = candev;
return candev_nb++;
}
int can_dll_dispatch_rx_frame(struct can_frame *frame, kernel_pid_t pid)
{
can_pkt_t *pkt = can_pkt_alloc_rx(_get_ifnum(pid), frame);
return can_router_dispatch_rx_indic(pkt);
}
int can_dll_dispatch_tx_conf(can_pkt_t *pkt)
{
DEBUG("can_dll_dispatch_tx_conf: pkt=0x%p\n", (void*)pkt);
can_router_dispatch_tx_conf(pkt);
mutex_lock(&tx_lock);
LL_DELETE(tx_list[pkt->entry.ifnum], &pkt->entry);
mutex_unlock(&tx_lock);
can_pkt_free(pkt);
return 0;
}
int can_dll_dispatch_tx_error(can_pkt_t *pkt)
{
DEBUG("can_dll_dispatch_tx_error: pkt=0x%p\n", (void*)pkt);
can_router_dispatch_tx_error(pkt);
mutex_lock(&tx_lock);
LL_DELETE(tx_list[pkt->entry.ifnum], &pkt->entry);
mutex_unlock(&tx_lock);
can_pkt_free(pkt);
return 0;
}
int can_dll_dispatch_bus_off(kernel_pid_t pid)
{
int ifnum = _get_ifnum(pid);
can_reg_entry_t *entry = tx_list[ifnum];
DEBUG("can_dll_dispatch_bus_off: ifnum=%d, pid=%" PRIkernel_pid "\n", ifnum, pid);
mutex_lock(&tx_lock);
while (entry) {
can_pkt_t *pkt = container_of(entry, can_pkt_t, entry);
can_router_dispatch_tx_error(pkt);
LL_DELETE(tx_list[ifnum], entry);
entry = tx_list[ifnum];
}
mutex_unlock(&tx_lock);
return 0;
}
int can_dll_init(void)
{
can_pkt_init();
return 0;
}
int raw_can_power_down(int ifnum)
{
msg_t msg, reply;
assert(ifnum < candev_nb);
msg.type = CAN_MSG_POWER_DOWN;
if (msg_send_receive(&msg, &reply, candev_list[ifnum]->pid) != 1) {
return -EBUSY;
}
return (int) reply.content.value;
}
int raw_can_power_up(int ifnum)
{
msg_t msg, reply;
assert(ifnum < candev_nb);
msg.type = CAN_MSG_POWER_UP;
if (msg_send_receive(&msg, &reply, candev_list[ifnum]->pid) != 1) {
return -EBUSY;
}
return (int) reply.content.value;
}
int raw_can_set_bitrate(int ifnum, uint32_t bitrate, uint32_t sample_point)
{
assert(ifnum < candev_nb);
int res = 0;
int ret;
uint32_t clock;
struct can_bittiming_const btc;
struct can_bittiming bittiming;
bittiming.bitrate = bitrate;
bittiming.sample_point = sample_point;
can_opt_t opt;
opt.opt = CANOPT_CLOCK;
opt.data = &clock;
opt.data_len = sizeof(clock);
ret = raw_can_get_can_opt(ifnum, &opt);
if (ret < 0) {
DEBUG("raw_can_set_bitrate: error when getting clock (%d)\n", ret);
return -1;
}
DEBUG("raw_can_set_bitrate: clock=%" PRIu32 " Hz\n", clock);
opt.opt = CANOPT_BITTIMING_CONST;
opt.data = &btc;
opt.data_len = sizeof(btc);
ret = raw_can_get_can_opt(ifnum, &opt);
if (ret < 0) {
DEBUG("raw_can_set_bitrate: error when getting const (%d)\n", ret);
return -1;
}
ret = can_device_calc_bittiming(clock, &btc, &bittiming);
if (ret < 0) {
DEBUG("raw_can_set_bitrate: bittiming might be wrong, ret=%d\n", ret);
res = 1;
}
opt.data = &bittiming;
opt.data_len = sizeof(bittiming);
opt.opt = CANOPT_BITTIMING;
ret = raw_can_set_can_opt(ifnum, &opt);
if (ret < 0) {
DEBUG("raw_can_set_bitrate: error when setting bitrate (%d)\n", ret);
return -1;
}
DEBUG("raw_can_set_bitrate: success bitrate=%" PRIu32 ", spt=%" PRIu32 "\n",
bittiming.bitrate, bittiming.sample_point);
return res;
}
#ifdef MODULE_CAN_TRX
int raw_can_set_trx(int ifnum, can_trx_t *trx)
{
msg_t msg, reply;
assert(ifnum < candev_nb);
msg.type = CAN_MSG_SET_TRX;
msg.content.ptr = trx;
if (msg_send_receive(&msg, &reply, candev_list[ifnum]->pid) != 1) {
return -EBUSY;
}
return (int) reply.content.value;
}
#endif
int raw_can_get_ifnum_by_name(const char *name)
{
for (int i = 0; i < candev_nb; i++) {
if ((strcmp(name, candev_list[i]->name) == 0) &&
(strlen(name) == strlen(candev_list[i]->name))) {
return i;
}
}
return RAW_CAN_DEV_UNDEF;
}
const char *raw_can_get_name_by_ifnum(int ifnum)
{
assert(ifnum >= 0);
if (ifnum >= candev_nb) {
return NULL;
}
return candev_list[ifnum]->name;
}
candev_dev_t *raw_can_get_dev_by_ifnum(int ifnum)
{
assert(ifnum >= 0);
if (ifnum >= candev_nb) {
return NULL;
}
return candev_list[ifnum];
}