Loading...
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 | /*
* Copyright (c) 2022 Vestas Wind Systems A/S
* Copyright (c) 2022 Alexander Wachter
* Copyright (c) 2019 Intel Corporation.
*
* SPDX-License-Identifier: Apache-2.0
*
*/
#include <zephyr/net/net_pkt.h>
#include <zephyr/net/canbus.h>
#include <zephyr/net/socketcan.h>
#include <zephyr/drivers/can.h>
#include <zephyr/devicetree.h>
#include <zephyr/device.h>
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(net_canbus, CONFIG_NET_CANBUS_LOG_LEVEL);
#define SEND_TIMEOUT K_MSEC(100)
struct net_canbus_context {
struct net_if *iface;
};
struct net_canbus_config {
const struct device *can_dev;
};
static void net_canbus_recv(const struct device *dev, struct can_frame *frame, void *user_data)
{
struct net_canbus_context *ctx = user_data;
struct net_pkt *pkt;
int ret;
ARG_UNUSED(dev);
LOG_DBG("pkt on interface %p", ctx->iface);
pkt = net_pkt_rx_alloc_with_buffer(ctx->iface, sizeof(struct can_frame),
AF_CAN, 0, K_NO_WAIT);
if (pkt == NULL) {
LOG_ERR("Failed to obtain net_pkt");
return;
}
if (net_pkt_write(pkt, frame, sizeof(struct can_frame))) {
LOG_ERR("Failed to append RX data");
net_pkt_unref(pkt);
return;
}
ret = net_recv_data(ctx->iface, pkt);
if (ret < 0) {
LOG_DBG("net_recv_data failed [%d]", ret);
net_pkt_unref(pkt);
}
}
static int net_canbus_setsockopt(const struct device *dev, void *obj, int level,
int optname, const void *optval, socklen_t optlen)
{
const struct net_canbus_config *cfg = dev->config;
struct net_canbus_context *context = dev->data;
struct net_context *ctx = obj;
int ret;
if (level != SOL_CAN_RAW && optname != CAN_RAW_FILTER) {
errno = EINVAL;
return -1;
}
__ASSERT_NO_MSG(optlen == sizeof(struct can_filter));
ret = can_add_rx_filter(cfg->can_dev, net_canbus_recv, context, optval);
if (ret == -ENOSPC) {
errno = ENOSPC;
return -1;
}
net_context_set_can_filter_id(ctx, ret);
return 0;
}
static void net_canbus_close(const struct device *dev, int filter_id)
{
const struct net_canbus_config *cfg = dev->config;
can_remove_rx_filter(cfg->can_dev, filter_id);
}
static void net_canbus_send_tx_callback(const struct device *dev, int error, void *user_data)
{
ARG_UNUSED(dev);
ARG_UNUSED(user_data);
if (error != 0) {
LOG_DBG("CAN bus TX error [%d]", error);
}
}
static int net_canbus_send(const struct device *dev, struct net_pkt *pkt)
{
const struct net_canbus_config *cfg = dev->config;
int ret;
if (net_pkt_family(pkt) != AF_CAN) {
return -EPFNOSUPPORT;
}
ret = can_send(cfg->can_dev, (struct can_frame *)pkt->frags->data,
SEND_TIMEOUT, net_canbus_send_tx_callback, NULL);
if (ret == 0) {
net_pkt_unref(pkt);
} else {
LOG_DBG("Cannot send CAN msg (%d)", ret);
}
/* If something went wrong, then we need to return negative value to
* net_if.c:net_if_tx() so that the net_pkt will get released.
*/
return ret;
}
static void net_canbus_iface_init(struct net_if *iface)
{
const struct device *dev = net_if_get_device(iface);
struct net_canbus_context *context = dev->data;
context->iface = iface;
LOG_DBG("Init CAN interface %p dev %p", iface, dev);
}
static int net_canbus_init(const struct device *dev)
{
const struct net_canbus_config *cfg = dev->config;
if (!device_is_ready(cfg->can_dev)) {
LOG_ERR("CAN device not ready");
return -ENODEV;
}
return 0;
}
static struct canbus_api net_canbus_api = {
.iface_api.init = net_canbus_iface_init,
.send = net_canbus_send,
.close = net_canbus_close,
.setsockopt = net_canbus_setsockopt,
};
static struct net_canbus_context net_canbus_ctx;
static const struct net_canbus_config net_canbus_cfg = {
.can_dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_canbus))
};
NET_DEVICE_INIT(net_canbus, "NET_CANBUS", net_canbus_init, NULL, &net_canbus_ctx, &net_canbus_cfg,
CONFIG_NET_CANBUS_INIT_PRIORITY, &net_canbus_api, CANBUS_RAW_L2,
NET_L2_GET_CTX_TYPE(CANBUS_RAW_L2), CAN_MTU);
|