main.c

  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/* lowest layer */
 34static void j1939_can_recv(struct sk_buff *iskb, void *data)
 35{
 36	struct j1939_priv *priv = data;
 37	struct sk_buff *skb;
 38	struct j1939_sk_buff_cb *skcb, *iskcb;
 39	struct can_frame *cf;
 40
 41	/* make sure we only get Classical CAN frames */
 42	if (!can_is_can_skb(iskb))
 43		return;
 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
125static DEFINE_MUTEX(j1939_netdev_lock);
126
127static 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
147static 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
155static 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
170void j1939_priv_put(struct j1939_priv *priv)
171{
172	kref_put(&priv->kref, __j1939_priv_release);
173}
174
175void j1939_priv_get(struct j1939_priv *priv)
176{
177	kref_get(&priv->kref);
178}
179
180static 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
196static 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
210static 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	mutex_unlock(&j1939_netdev_lock);
220}
221
222/* get pointer to priv without increasing ref counter */
223static 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
230static 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
243static struct j1939_priv *j1939_priv_get_by_ndev(struct net_device *ndev)
244{
245	struct j1939_priv *priv;
246
247	mutex_lock(&j1939_netdev_lock);
248	priv = j1939_priv_get_by_ndev_locked(ndev);
249	mutex_unlock(&j1939_netdev_lock);
250
251	return priv;
252}
253
254struct j1939_priv *j1939_netdev_start(struct net_device *ndev)
255{
256	struct j1939_priv *priv, *priv_new;
257	int ret;
258
259	mutex_lock(&j1939_netdev_lock);
260	priv = j1939_priv_get_by_ndev_locked(ndev);
261	if (priv) {
262		kref_get(&priv->rx_kref);
263		mutex_unlock(&j1939_netdev_lock);
264		return priv;
265	}
266	mutex_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	rwlock_init(&priv->j1939_socks_lock);
274	INIT_LIST_HEAD(&priv->j1939_socks);
275
276	mutex_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		mutex_unlock(&j1939_netdev_lock);
284		dev_put(ndev);
285		kfree(priv);
286		return priv_new;
287	}
288	j1939_priv_set(ndev, priv);
289
290	ret = j1939_can_rx_register(priv);
291	if (ret < 0)
292		goto out_priv_put;
293
294	mutex_unlock(&j1939_netdev_lock);
295	return priv;
296
297 out_priv_put:
298	j1939_priv_set(ndev, NULL);
299	mutex_unlock(&j1939_netdev_lock);
300
301	dev_put(ndev);
302	kfree(priv);
303
304	return ERR_PTR(ret);
305}
306
307void j1939_netdev_stop(struct j1939_priv *priv)
308{
309	kref_put_mutex(&priv->rx_kref, __j1939_rx_release, &j1939_netdev_lock);
310	j1939_priv_put(priv);
311}
312
313int j1939_send_one(struct j1939_priv *priv, struct sk_buff *skb)
314{
315	int ret, dlc;
316	canid_t canid;
317	struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
318	struct can_frame *cf;
319
320	/* apply sanity checks */
321	if (j1939_pgn_is_pdu1(skcb->addr.pgn))
322		skcb->addr.pgn &= J1939_PGN_PDU1_MAX;
323	else
324		skcb->addr.pgn &= J1939_PGN_MAX;
325
326	if (skcb->priority > 7)
327		skcb->priority = 6;
328
329	ret = j1939_ac_fixup(priv, skb);
330	if (unlikely(ret))
331		goto failed;
332	dlc = skb->len;
333
334	/* re-claim the CAN_HDR from the SKB */
335	cf = skb_push(skb, J1939_CAN_HDR);
336
337	/* initialize header structure */
338	memset(cf, 0, J1939_CAN_HDR);
339
340	/* make it a full can frame again */
341	skb_put_zero(skb, 8 - dlc);
342
343	canid = CAN_EFF_FLAG |
344		(skcb->priority << 26) |
345		(skcb->addr.pgn << 8) |
346		skcb->addr.sa;
347	if (j1939_pgn_is_pdu1(skcb->addr.pgn))
348		canid |= skcb->addr.da << 8;
349
350	cf->can_id = canid;
351	cf->len = dlc;
352
353	return can_send(skb, 1);
354
355 failed:
356	kfree_skb(skb);
357	return ret;
358}
359
360static int j1939_netdev_notify(struct notifier_block *nb,
361			       unsigned long msg, void *data)
362{
363	struct net_device *ndev = netdev_notifier_info_to_dev(data);
364	struct can_ml_priv *can_ml = can_get_ml_priv(ndev);
365	struct j1939_priv *priv;
366
367	if (!can_ml)
368		goto notify_done;
369
370	priv = j1939_priv_get_by_ndev(ndev);
371	if (!priv)
372		goto notify_done;
373
374	switch (msg) {
375	case NETDEV_DOWN:
376		j1939_cancel_active_session(priv, NULL);
377		j1939_sk_netdev_event_netdown(priv);
378		j1939_ecu_unmap_all(priv);
379		break;
380	case NETDEV_UNREGISTER:
381		j1939_cancel_active_session(priv, NULL);
382		j1939_sk_netdev_event_netdown(priv);
383		j1939_sk_netdev_event_unregister(priv);
384		break;
385	}
386
387	j1939_priv_put(priv);
388
389notify_done:
390	return NOTIFY_DONE;
391}
392
393static struct notifier_block j1939_netdev_notifier = {
394	.notifier_call = j1939_netdev_notify,
395};
396
397/* MODULE interface */
398static __init int j1939_module_init(void)
399{
400	int ret;
401
402	pr_info("can: SAE J1939\n");
403
404	ret = register_netdevice_notifier(&j1939_netdev_notifier);
405	if (ret)
406		goto fail_notifier;
407
408	ret = can_proto_register(&j1939_can_proto);
409	if (ret < 0) {
410		pr_err("can: registration of j1939 protocol failed\n");
411		goto fail_sk;
412	}
413
414	return 0;
415
416 fail_sk:
417	unregister_netdevice_notifier(&j1939_netdev_notifier);
418 fail_notifier:
419	return ret;
420}
421
422static __exit void j1939_module_exit(void)
423{
424	can_proto_unregister(&j1939_can_proto);
425
426	unregister_netdevice_notifier(&j1939_netdev_notifier);
427}
428
429module_init(j1939_module_init);
430module_exit(j1939_module_exit);