• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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