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