1 // SPDX-License-Identifier: GPL-2.0
2 // Copyright (c) 2010-2011 EIA Electronics,
3 // Pieter Beyens <pieter.beyens@eia.be>
4 // Copyright (c) 2010-2011 EIA Electronics,
5 // Kurt Van Dijck <kurt.van.dijck@eia.be>
6 // Copyright (c) 2018 Protonic,
7 // Robin van der Gracht <robin@protonic.nl>
8 // Copyright (c) 2017-2019 Pengutronix,
9 // Marc Kleine-Budde <kernel@pengutronix.de>
10 // Copyright (c) 2017-2019 Pengutronix,
11 // Oleksij Rempel <kernel@pengutronix.de>
12
13 /* Core of can-j1939 that links j1939 to CAN. */
14
15 #include <linux/can/can-ml.h>
16 #include <linux/can/core.h>
17 #include <linux/can/skb.h>
18 #include <linux/if_arp.h>
19 #include <linux/module.h>
20
21 #include "j1939-priv.h"
22
23 MODULE_DESCRIPTION("PF_CAN SAE J1939");
24 MODULE_LICENSE("GPL v2");
25 MODULE_AUTHOR("EIA Electronics (Kurt Van Dijck & Pieter Beyens)");
26 MODULE_ALIAS("can-proto-" __stringify(CAN_J1939));
27
28 /* LOWLEVEL CAN interface */
29
30 /* CAN_HDR: #bytes before can_frame data part */
31 #define J1939_CAN_HDR (offsetof(struct can_frame, data))
32
33 /* CAN_FTR: #bytes beyond data part */
34 #define J1939_CAN_FTR (sizeof(struct can_frame) - J1939_CAN_HDR - \
35 sizeof(((struct can_frame *)0)->data))
36
37 /* lowest layer */
j1939_can_recv(struct sk_buff * iskb,void * data)38 static void j1939_can_recv(struct sk_buff *iskb, void *data)
39 {
40 struct j1939_priv *priv = data;
41 struct sk_buff *skb;
42 struct j1939_sk_buff_cb *skcb, *iskcb;
43 struct can_frame *cf;
44
45 /* create a copy of the skb
46 * j1939 only delivers the real data bytes,
47 * the header goes into sockaddr.
48 * j1939 may not touch the incoming skb in such way
49 */
50 skb = skb_clone(iskb, GFP_ATOMIC);
51 if (!skb)
52 return;
53
54 j1939_priv_get(priv);
55 can_skb_set_owner(skb, iskb->sk);
56
57 /* get a pointer to the header of the skb
58 * the skb payload (pointer) is moved, so that the next skb_data
59 * returns the actual payload
60 */
61 cf = (void *)skb->data;
62 skb_pull(skb, J1939_CAN_HDR);
63
64 /* fix length, set to dlc, with 8 maximum */
65 skb_trim(skb, min_t(uint8_t, cf->can_dlc, 8));
66
67 /* set addr */
68 skcb = j1939_skb_to_cb(skb);
69 memset(skcb, 0, sizeof(*skcb));
70
71 iskcb = j1939_skb_to_cb(iskb);
72 skcb->tskey = iskcb->tskey;
73 skcb->priority = (cf->can_id >> 26) & 0x7;
74 skcb->addr.sa = cf->can_id;
75 skcb->addr.pgn = (cf->can_id >> 8) & J1939_PGN_MAX;
76 /* set default message type */
77 skcb->addr.type = J1939_TP;
78 if (j1939_pgn_is_pdu1(skcb->addr.pgn)) {
79 /* Type 1: with destination address */
80 skcb->addr.da = skcb->addr.pgn;
81 /* normalize pgn: strip dst address */
82 skcb->addr.pgn &= 0x3ff00;
83 } else {
84 /* set broadcast address */
85 skcb->addr.da = J1939_NO_ADDR;
86 }
87
88 /* update localflags */
89 read_lock_bh(&priv->lock);
90 if (j1939_address_is_unicast(skcb->addr.sa) &&
91 priv->ents[skcb->addr.sa].nusers)
92 skcb->flags |= J1939_ECU_LOCAL_SRC;
93 if (j1939_address_is_unicast(skcb->addr.da) &&
94 priv->ents[skcb->addr.da].nusers)
95 skcb->flags |= J1939_ECU_LOCAL_DST;
96 read_unlock_bh(&priv->lock);
97
98 /* deliver into the j1939 stack ... */
99 j1939_ac_recv(priv, skb);
100
101 if (j1939_tp_recv(priv, skb))
102 /* this means the transport layer processed the message */
103 goto done;
104
105 j1939_simple_recv(priv, skb);
106 j1939_sk_recv(priv, skb);
107 done:
108 j1939_priv_put(priv);
109 kfree_skb(skb);
110 }
111
112 /* NETDEV MANAGEMENT */
113
114 /* values for can_rx_(un)register */
115 #define J1939_CAN_ID CAN_EFF_FLAG
116 #define J1939_CAN_MASK (CAN_EFF_FLAG | CAN_RTR_FLAG)
117
118 static DEFINE_SPINLOCK(j1939_netdev_lock);
119
j1939_priv_create(struct net_device * ndev)120 static struct j1939_priv *j1939_priv_create(struct net_device *ndev)
121 {
122 struct j1939_priv *priv;
123
124 priv = kzalloc(sizeof(*priv), GFP_KERNEL);
125 if (!priv)
126 return NULL;
127
128 rwlock_init(&priv->lock);
129 INIT_LIST_HEAD(&priv->ecus);
130 priv->ndev = ndev;
131 kref_init(&priv->kref);
132 kref_init(&priv->rx_kref);
133 dev_hold(ndev);
134
135 netdev_dbg(priv->ndev, "%s : 0x%p\n", __func__, priv);
136
137 return priv;
138 }
139
j1939_priv_set(struct net_device * ndev,struct j1939_priv * priv)140 static inline void j1939_priv_set(struct net_device *ndev,
141 struct j1939_priv *priv)
142 {
143 struct can_ml_priv *can_ml_priv = ndev->ml_priv;
144
145 can_ml_priv->j1939_priv = priv;
146 }
147
__j1939_priv_release(struct kref * kref)148 static void __j1939_priv_release(struct kref *kref)
149 {
150 struct j1939_priv *priv = container_of(kref, struct j1939_priv, kref);
151 struct net_device *ndev = priv->ndev;
152
153 netdev_dbg(priv->ndev, "%s: 0x%p\n", __func__, priv);
154
155 WARN_ON_ONCE(!list_empty(&priv->active_session_list));
156 WARN_ON_ONCE(!list_empty(&priv->ecus));
157 WARN_ON_ONCE(!list_empty(&priv->j1939_socks));
158
159 dev_put(ndev);
160 kfree(priv);
161 }
162
j1939_priv_put(struct j1939_priv * priv)163 void j1939_priv_put(struct j1939_priv *priv)
164 {
165 kref_put(&priv->kref, __j1939_priv_release);
166 }
167
j1939_priv_get(struct j1939_priv * priv)168 void j1939_priv_get(struct j1939_priv *priv)
169 {
170 kref_get(&priv->kref);
171 }
172
j1939_can_rx_register(struct j1939_priv * priv)173 static int j1939_can_rx_register(struct j1939_priv *priv)
174 {
175 struct net_device *ndev = priv->ndev;
176 int ret;
177
178 j1939_priv_get(priv);
179 ret = can_rx_register(dev_net(ndev), ndev, J1939_CAN_ID, J1939_CAN_MASK,
180 j1939_can_recv, priv, "j1939", NULL);
181 if (ret < 0) {
182 j1939_priv_put(priv);
183 return ret;
184 }
185
186 return 0;
187 }
188
j1939_can_rx_unregister(struct j1939_priv * priv)189 static void j1939_can_rx_unregister(struct j1939_priv *priv)
190 {
191 struct net_device *ndev = priv->ndev;
192
193 can_rx_unregister(dev_net(ndev), ndev, J1939_CAN_ID, J1939_CAN_MASK,
194 j1939_can_recv, priv);
195
196 j1939_priv_put(priv);
197 }
198
__j1939_rx_release(struct kref * kref)199 static void __j1939_rx_release(struct kref *kref)
200 __releases(&j1939_netdev_lock)
201 {
202 struct j1939_priv *priv = container_of(kref, struct j1939_priv,
203 rx_kref);
204
205 j1939_can_rx_unregister(priv);
206 j1939_ecu_unmap_all(priv);
207 j1939_priv_set(priv->ndev, NULL);
208 spin_unlock(&j1939_netdev_lock);
209 }
210
211 /* get pointer to priv without increasing ref counter */
j1939_ndev_to_priv(struct net_device * ndev)212 static inline struct j1939_priv *j1939_ndev_to_priv(struct net_device *ndev)
213 {
214 struct can_ml_priv *can_ml_priv = ndev->ml_priv;
215
216 if (!can_ml_priv)
217 return NULL;
218
219 return can_ml_priv->j1939_priv;
220 }
221
j1939_priv_get_by_ndev_locked(struct net_device * ndev)222 static struct j1939_priv *j1939_priv_get_by_ndev_locked(struct net_device *ndev)
223 {
224 struct j1939_priv *priv;
225
226 lockdep_assert_held(&j1939_netdev_lock);
227
228 if (ndev->type != ARPHRD_CAN)
229 return NULL;
230
231 priv = j1939_ndev_to_priv(ndev);
232 if (priv)
233 j1939_priv_get(priv);
234
235 return priv;
236 }
237
j1939_priv_get_by_ndev(struct net_device * ndev)238 static struct j1939_priv *j1939_priv_get_by_ndev(struct net_device *ndev)
239 {
240 struct j1939_priv *priv;
241
242 spin_lock(&j1939_netdev_lock);
243 priv = j1939_priv_get_by_ndev_locked(ndev);
244 spin_unlock(&j1939_netdev_lock);
245
246 return priv;
247 }
248
j1939_netdev_start(struct net_device * ndev)249 struct j1939_priv *j1939_netdev_start(struct net_device *ndev)
250 {
251 struct j1939_priv *priv, *priv_new;
252 int ret;
253
254 priv = j1939_priv_get_by_ndev(ndev);
255 if (priv) {
256 kref_get(&priv->rx_kref);
257 return priv;
258 }
259
260 priv = j1939_priv_create(ndev);
261 if (!priv)
262 return ERR_PTR(-ENOMEM);
263
264 j1939_tp_init(priv);
265 spin_lock_init(&priv->j1939_socks_lock);
266 INIT_LIST_HEAD(&priv->j1939_socks);
267
268 spin_lock(&j1939_netdev_lock);
269 priv_new = j1939_priv_get_by_ndev_locked(ndev);
270 if (priv_new) {
271 /* Someone was faster than us, use their priv and roll
272 * back our's.
273 */
274 spin_unlock(&j1939_netdev_lock);
275 dev_put(ndev);
276 kfree(priv);
277 kref_get(&priv_new->rx_kref);
278 return priv_new;
279 }
280 j1939_priv_set(ndev, priv);
281 spin_unlock(&j1939_netdev_lock);
282
283 ret = j1939_can_rx_register(priv);
284 if (ret < 0)
285 goto out_priv_put;
286
287 return priv;
288
289 out_priv_put:
290 j1939_priv_set(ndev, NULL);
291 dev_put(ndev);
292 kfree(priv);
293
294 return ERR_PTR(ret);
295 }
296
j1939_netdev_stop(struct j1939_priv * priv)297 void j1939_netdev_stop(struct j1939_priv *priv)
298 {
299 kref_put_lock(&priv->rx_kref, __j1939_rx_release, &j1939_netdev_lock);
300 j1939_priv_put(priv);
301 }
302
j1939_send_one(struct j1939_priv * priv,struct sk_buff * skb)303 int j1939_send_one(struct j1939_priv *priv, struct sk_buff *skb)
304 {
305 int ret, dlc;
306 canid_t canid;
307 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
308 struct can_frame *cf;
309
310 /* apply sanity checks */
311 if (j1939_pgn_is_pdu1(skcb->addr.pgn))
312 skcb->addr.pgn &= J1939_PGN_PDU1_MAX;
313 else
314 skcb->addr.pgn &= J1939_PGN_MAX;
315
316 if (skcb->priority > 7)
317 skcb->priority = 6;
318
319 ret = j1939_ac_fixup(priv, skb);
320 if (unlikely(ret))
321 goto failed;
322 dlc = skb->len;
323
324 /* re-claim the CAN_HDR from the SKB */
325 cf = skb_push(skb, J1939_CAN_HDR);
326
327 /* make it a full can frame again */
328 skb_put(skb, J1939_CAN_FTR + (8 - dlc));
329
330 canid = CAN_EFF_FLAG |
331 (skcb->priority << 26) |
332 (skcb->addr.pgn << 8) |
333 skcb->addr.sa;
334 if (j1939_pgn_is_pdu1(skcb->addr.pgn))
335 canid |= skcb->addr.da << 8;
336
337 cf->can_id = canid;
338 cf->can_dlc = dlc;
339
340 return can_send(skb, 1);
341
342 failed:
343 kfree_skb(skb);
344 return ret;
345 }
346
j1939_netdev_notify(struct notifier_block * nb,unsigned long msg,void * data)347 static int j1939_netdev_notify(struct notifier_block *nb,
348 unsigned long msg, void *data)
349 {
350 struct net_device *ndev = netdev_notifier_info_to_dev(data);
351 struct j1939_priv *priv;
352
353 priv = j1939_priv_get_by_ndev(ndev);
354 if (!priv)
355 goto notify_done;
356
357 if (ndev->type != ARPHRD_CAN)
358 goto notify_put;
359
360 switch (msg) {
361 case NETDEV_DOWN:
362 j1939_cancel_active_session(priv, NULL);
363 j1939_sk_netdev_event_netdown(priv);
364 j1939_ecu_unmap_all(priv);
365 break;
366 }
367
368 notify_put:
369 j1939_priv_put(priv);
370
371 notify_done:
372 return NOTIFY_DONE;
373 }
374
375 static struct notifier_block j1939_netdev_notifier = {
376 .notifier_call = j1939_netdev_notify,
377 };
378
379 /* MODULE interface */
j1939_module_init(void)380 static __init int j1939_module_init(void)
381 {
382 int ret;
383
384 pr_info("can: SAE J1939\n");
385
386 ret = register_netdevice_notifier(&j1939_netdev_notifier);
387 if (ret)
388 goto fail_notifier;
389
390 ret = can_proto_register(&j1939_can_proto);
391 if (ret < 0) {
392 pr_err("can: registration of j1939 protocol failed\n");
393 goto fail_sk;
394 }
395
396 return 0;
397
398 fail_sk:
399 unregister_netdevice_notifier(&j1939_netdev_notifier);
400 fail_notifier:
401 return ret;
402 }
403
j1939_module_exit(void)404 static __exit void j1939_module_exit(void)
405 {
406 can_proto_unregister(&j1939_can_proto);
407
408 unregister_netdevice_notifier(&j1939_netdev_notifier);
409 }
410
411 module_init(j1939_module_init);
412 module_exit(j1939_module_exit);
413