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