xref: /openbmc/linux/net/can/j1939/main.c (revision 8dce88fe) 
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 	/* 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->len, 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 
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 
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 
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 
170 void j1939_priv_put(struct j1939_priv *priv)
171 {
172 	kref_put(&priv->kref, __j1939_priv_release);
173 }
174 
175 void j1939_priv_get(struct j1939_priv *priv)
176 {
177 	kref_get(&priv->kref);
178 }
179 
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 
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 
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 */
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 
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 
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 
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 
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 
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 	/* make it a full can frame again */
336 	skb_put(skb, J1939_CAN_FTR + (8 - dlc));
337 
338 	canid = CAN_EFF_FLAG |
339 		(skcb->priority << 26) |
340 		(skcb->addr.pgn << 8) |
341 		skcb->addr.sa;
342 	if (j1939_pgn_is_pdu1(skcb->addr.pgn))
343 		canid |= skcb->addr.da << 8;
344 
345 	cf->can_id = canid;
346 	cf->len = dlc;
347 
348 	return can_send(skb, 1);
349 
350  failed:
351 	kfree_skb(skb);
352 	return ret;
353 }
354 
355 static int j1939_netdev_notify(struct notifier_block *nb,
356 			       unsigned long msg, void *data)
357 {
358 	struct net_device *ndev = netdev_notifier_info_to_dev(data);
359 	struct can_ml_priv *can_ml = can_get_ml_priv(ndev);
360 	struct j1939_priv *priv;
361 
362 	if (!can_ml)
363 		goto notify_done;
364 
365 	priv = j1939_priv_get_by_ndev(ndev);
366 	if (!priv)
367 		goto notify_done;
368 
369 	switch (msg) {
370 	case NETDEV_DOWN:
371 		j1939_cancel_active_session(priv, NULL);
372 		j1939_sk_netdev_event_netdown(priv);
373 		j1939_ecu_unmap_all(priv);
374 		break;
375 	}
376 
377 	j1939_priv_put(priv);
378 
379 notify_done:
380 	return NOTIFY_DONE;
381 }
382 
383 static struct notifier_block j1939_netdev_notifier = {
384 	.notifier_call = j1939_netdev_notify,
385 };
386 
387 /* MODULE interface */
388 static __init int j1939_module_init(void)
389 {
390 	int ret;
391 
392 	pr_info("can: SAE J1939\n");
393 
394 	ret = register_netdevice_notifier(&j1939_netdev_notifier);
395 	if (ret)
396 		goto fail_notifier;
397 
398 	ret = can_proto_register(&j1939_can_proto);
399 	if (ret < 0) {
400 		pr_err("can: registration of j1939 protocol failed\n");
401 		goto fail_sk;
402 	}
403 
404 	return 0;
405 
406  fail_sk:
407 	unregister_netdevice_notifier(&j1939_netdev_notifier);
408  fail_notifier:
409 	return ret;
410 }
411 
412 static __exit void j1939_module_exit(void)
413 {
414 	can_proto_unregister(&j1939_can_proto);
415 
416 	unregister_netdevice_notifier(&j1939_netdev_notifier);
417 }
418 
419 module_init(j1939_module_init);
420 module_exit(j1939_module_exit);
421