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
23MODULE_DESCRIPTION("PF_CAN SAE J1939");
24MODULE_LICENSE("GPL v2");
25MODULE_AUTHOR("EIA Electronics (Kurt Van Dijck & Pieter Beyens)");
26MODULE_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 */
38static 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
129static DEFINE_MUTEX(j1939_netdev_lock);
130
131static 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
151static 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
159static 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
174void j1939_priv_put(struct j1939_priv *priv)
175{
176	kref_put(&priv->kref, __j1939_priv_release);
177}
178
179void j1939_priv_get(struct j1939_priv *priv)
180{
181	kref_get(&priv->kref);
182}
183
184static 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
200static 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
214static 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	mutex_unlock(&j1939_netdev_lock);
224}
225
226/* get pointer to priv without increasing ref counter */
227static 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
234static 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
247static struct j1939_priv *j1939_priv_get_by_ndev(struct net_device *ndev)
248{
249	struct j1939_priv *priv;
250
251	mutex_lock(&j1939_netdev_lock);
252	priv = j1939_priv_get_by_ndev_locked(ndev);
253	mutex_unlock(&j1939_netdev_lock);
254
255	return priv;
256}
257
258struct j1939_priv *j1939_netdev_start(struct net_device *ndev)
259{
260	struct j1939_priv *priv, *priv_new;
261	int ret;
262
263	mutex_lock(&j1939_netdev_lock);
264	priv = j1939_priv_get_by_ndev_locked(ndev);
265	if (priv) {
266		kref_get(&priv->rx_kref);
267		mutex_unlock(&j1939_netdev_lock);
268		return priv;
269	}
270	mutex_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	rwlock_init(&priv->j1939_socks_lock);
278	INIT_LIST_HEAD(&priv->j1939_socks);
279
280	mutex_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		mutex_unlock(&j1939_netdev_lock);
288		dev_put(ndev);
289		kfree(priv);
290		return priv_new;
291	}
292	j1939_priv_set(ndev, priv);
293
294	ret = j1939_can_rx_register(priv);
295	if (ret < 0)
296		goto out_priv_put;
297
298	mutex_unlock(&j1939_netdev_lock);
299	return priv;
300
301 out_priv_put:
302	j1939_priv_set(ndev, NULL);
303	mutex_unlock(&j1939_netdev_lock);
304
305	dev_put(ndev);
306	kfree(priv);
307
308	return ERR_PTR(ret);
309}
310
311void j1939_netdev_stop(struct j1939_priv *priv)
312{
313	kref_put_mutex(&priv->rx_kref, __j1939_rx_release, &j1939_netdev_lock);
314	j1939_priv_put(priv);
315}
316
317int j1939_send_one(struct j1939_priv *priv, struct sk_buff *skb)
318{
319	int ret, dlc;
320	canid_t canid;
321	struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
322	struct can_frame *cf;
323
324	/* apply sanity checks */
325	if (j1939_pgn_is_pdu1(skcb->addr.pgn))
326		skcb->addr.pgn &= J1939_PGN_PDU1_MAX;
327	else
328		skcb->addr.pgn &= J1939_PGN_MAX;
329
330	if (skcb->priority > 7)
331		skcb->priority = 6;
332
333	ret = j1939_ac_fixup(priv, skb);
334	if (unlikely(ret))
335		goto failed;
336	dlc = skb->len;
337
338	/* re-claim the CAN_HDR from the SKB */
339	cf = skb_push(skb, J1939_CAN_HDR);
340
341	/* initialize header structure */
342	memset(cf, 0, J1939_CAN_HDR);
343
344	/* make it a full can frame again */
345	skb_put(skb, J1939_CAN_FTR + (8 - dlc));
346
347	canid = CAN_EFF_FLAG |
348		(skcb->priority << 26) |
349		(skcb->addr.pgn << 8) |
350		skcb->addr.sa;
351	if (j1939_pgn_is_pdu1(skcb->addr.pgn))
352		canid |= skcb->addr.da << 8;
353
354	cf->can_id = canid;
355	cf->len = dlc;
356
357	return can_send(skb, 1);
358
359 failed:
360	kfree_skb(skb);
361	return ret;
362}
363
364static int j1939_netdev_notify(struct notifier_block *nb,
365			       unsigned long msg, void *data)
366{
367	struct net_device *ndev = netdev_notifier_info_to_dev(data);
368	struct can_ml_priv *can_ml = can_get_ml_priv(ndev);
369	struct j1939_priv *priv;
370
371	if (!can_ml)
372		goto notify_done;
373
374	priv = j1939_priv_get_by_ndev(ndev);
375	if (!priv)
376		goto notify_done;
377
378	switch (msg) {
379	case NETDEV_DOWN:
380		j1939_cancel_active_session(priv, NULL);
381		j1939_sk_netdev_event_netdown(priv);
382		j1939_ecu_unmap_all(priv);
383		break;
384	}
385
386	j1939_priv_put(priv);
387
388notify_done:
389	return NOTIFY_DONE;
390}
391
392static struct notifier_block j1939_netdev_notifier = {
393	.notifier_call = j1939_netdev_notify,
394};
395
396/* MODULE interface */
397static __init int j1939_module_init(void)
398{
399	int ret;
400
401	pr_info("can: SAE J1939\n");
402
403	ret = register_netdevice_notifier(&j1939_netdev_notifier);
404	if (ret)
405		goto fail_notifier;
406
407	ret = can_proto_register(&j1939_can_proto);
408	if (ret < 0) {
409		pr_err("can: registration of j1939 protocol failed\n");
410		goto fail_sk;
411	}
412
413	return 0;
414
415 fail_sk:
416	unregister_netdevice_notifier(&j1939_netdev_notifier);
417 fail_notifier:
418	return ret;
419}
420
421static __exit void j1939_module_exit(void)
422{
423	can_proto_unregister(&j1939_can_proto);
424
425	unregister_netdevice_notifier(&j1939_netdev_notifier);
426}
427
428module_init(j1939_module_init);
429module_exit(j1939_module_exit);
430