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