xref: /linux/net/can/j1939/main.c (revision 6c8c1406) 
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 */
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 
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 
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 
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 
174 void j1939_priv_put(struct j1939_priv *priv)
175 {
176 	kref_put(&priv->kref, __j1939_priv_release);
177 }
178 
179 void j1939_priv_get(struct j1939_priv *priv)
180 {
181 	kref_get(&priv->kref);
182 }
183 
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 
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 
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 */
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 
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 
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 
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 
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 
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 
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 */
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 
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