1
2
3
4
5
6
7
8
9
10
11
12
13
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
29
30
31#define J1939_CAN_HDR (offsetof(struct can_frame, data))
32
33
34#define J1939_CAN_FTR (sizeof(struct can_frame) - J1939_CAN_HDR - \
35 sizeof(((struct can_frame *)0)->data))
36
37
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
46
47
48
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
58
59
60
61 cf = (void *)skb->data;
62 skb_pull(skb, J1939_CAN_HDR);
63
64
65 skb_trim(skb, min_t(uint8_t, cf->len, 8));
66
67
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
77 skcb->addr.type = J1939_TP;
78 if (j1939_pgn_is_pdu1(skcb->addr.pgn)) {
79
80 skcb->addr.da = skcb->addr.pgn;
81
82 skcb->addr.pgn &= 0x3ff00;
83 } else {
84
85 skcb->addr.da = J1939_NO_ADDR;
86 }
87
88
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
99 j1939_ac_recv(priv, skb);
100
101 if (j1939_tp_recv(priv, skb))
102
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
113
114
115#define J1939_CAN_ID CAN_EFF_FLAG
116#define J1939_CAN_MASK (CAN_EFF_FLAG | CAN_RTR_FLAG)
117
118static DEFINE_SPINLOCK(j1939_netdev_lock);
119
120static 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
140static inline void j1939_priv_set(struct net_device *ndev,
141 struct j1939_priv *priv)
142{
143 struct can_ml_priv *can_ml = can_get_ml_priv(ndev);
144
145 can_ml->j1939_priv = priv;
146}
147
148static 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
163void j1939_priv_put(struct j1939_priv *priv)
164{
165 kref_put(&priv->kref, __j1939_priv_release);
166}
167
168void j1939_priv_get(struct j1939_priv *priv)
169{
170 kref_get(&priv->kref);
171}
172
173static 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
189static 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
197
198
199
200 j1939_priv_put(priv);
201}
202
203static void __j1939_rx_release(struct kref *kref)
204 __releases(&j1939_netdev_lock)
205{
206 struct j1939_priv *priv = container_of(kref, struct j1939_priv,
207 rx_kref);
208
209 j1939_can_rx_unregister(priv);
210 j1939_ecu_unmap_all(priv);
211 j1939_priv_set(priv->ndev, NULL);
212 spin_unlock(&j1939_netdev_lock);
213}
214
215
216static inline struct j1939_priv *j1939_ndev_to_priv(struct net_device *ndev)
217{
218 struct can_ml_priv *can_ml = can_get_ml_priv(ndev);
219
220 return can_ml->j1939_priv;
221}
222
223static struct j1939_priv *j1939_priv_get_by_ndev_locked(struct net_device *ndev)
224{
225 struct j1939_priv *priv;
226
227 lockdep_assert_held(&j1939_netdev_lock);
228
229 priv = j1939_ndev_to_priv(ndev);
230 if (priv)
231 j1939_priv_get(priv);
232
233 return priv;
234}
235
236static struct j1939_priv *j1939_priv_get_by_ndev(struct net_device *ndev)
237{
238 struct j1939_priv *priv;
239
240 spin_lock(&j1939_netdev_lock);
241 priv = j1939_priv_get_by_ndev_locked(ndev);
242 spin_unlock(&j1939_netdev_lock);
243
244 return priv;
245}
246
247struct j1939_priv *j1939_netdev_start(struct net_device *ndev)
248{
249 struct j1939_priv *priv, *priv_new;
250 int ret;
251
252 spin_lock(&j1939_netdev_lock);
253 priv = j1939_priv_get_by_ndev_locked(ndev);
254 if (priv) {
255 kref_get(&priv->rx_kref);
256 spin_unlock(&j1939_netdev_lock);
257 return priv;
258 }
259 spin_unlock(&j1939_netdev_lock);
260
261 priv = j1939_priv_create(ndev);
262 if (!priv)
263 return ERR_PTR(-ENOMEM);
264
265 j1939_tp_init(priv);
266 spin_lock_init(&priv->j1939_socks_lock);
267 INIT_LIST_HEAD(&priv->j1939_socks);
268
269 spin_lock(&j1939_netdev_lock);
270 priv_new = j1939_priv_get_by_ndev_locked(ndev);
271 if (priv_new) {
272
273
274
275 kref_get(&priv_new->rx_kref);
276 spin_unlock(&j1939_netdev_lock);
277 dev_put(ndev);
278 kfree(priv);
279 return priv_new;
280 }
281 j1939_priv_set(ndev, priv);
282 spin_unlock(&j1939_netdev_lock);
283
284 ret = j1939_can_rx_register(priv);
285 if (ret < 0)
286 goto out_priv_put;
287
288 return priv;
289
290 out_priv_put:
291 j1939_priv_set(ndev, NULL);
292 dev_put(ndev);
293 kfree(priv);
294
295 return ERR_PTR(ret);
296}
297
298void j1939_netdev_stop(struct j1939_priv *priv)
299{
300 kref_put_lock(&priv->rx_kref, __j1939_rx_release, &j1939_netdev_lock);
301 j1939_priv_put(priv);
302}
303
304int j1939_send_one(struct j1939_priv *priv, struct sk_buff *skb)
305{
306 int ret, dlc;
307 canid_t canid;
308 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
309 struct can_frame *cf;
310
311
312 if (j1939_pgn_is_pdu1(skcb->addr.pgn))
313 skcb->addr.pgn &= J1939_PGN_PDU1_MAX;
314 else
315 skcb->addr.pgn &= J1939_PGN_MAX;
316
317 if (skcb->priority > 7)
318 skcb->priority = 6;
319
320 ret = j1939_ac_fixup(priv, skb);
321 if (unlikely(ret))
322 goto failed;
323 dlc = skb->len;
324
325
326 cf = skb_push(skb, J1939_CAN_HDR);
327
328
329 skb_put(skb, J1939_CAN_FTR + (8 - dlc));
330
331 canid = CAN_EFF_FLAG |
332 (skcb->priority << 26) |
333 (skcb->addr.pgn << 8) |
334 skcb->addr.sa;
335 if (j1939_pgn_is_pdu1(skcb->addr.pgn))
336 canid |= skcb->addr.da << 8;
337
338 cf->can_id = canid;
339 cf->len = dlc;
340
341 return can_send(skb, 1);
342
343 failed:
344 kfree_skb(skb);
345 return ret;
346}
347
348static int j1939_netdev_notify(struct notifier_block *nb,
349 unsigned long msg, void *data)
350{
351 struct net_device *ndev = netdev_notifier_info_to_dev(data);
352 struct can_ml_priv *can_ml = can_get_ml_priv(ndev);
353 struct j1939_priv *priv;
354
355 if (!can_ml)
356 goto notify_done;
357
358 priv = j1939_priv_get_by_ndev(ndev);
359 if (!priv)
360 goto notify_done;
361
362 switch (msg) {
363 case NETDEV_DOWN:
364 j1939_cancel_active_session(priv, NULL);
365 j1939_sk_netdev_event_netdown(priv);
366 j1939_ecu_unmap_all(priv);
367 break;
368 }
369
370 j1939_priv_put(priv);
371
372notify_done:
373 return NOTIFY_DONE;
374}
375
376static struct notifier_block j1939_netdev_notifier = {
377 .notifier_call = j1939_netdev_notify,
378};
379
380
381static __init int j1939_module_init(void)
382{
383 int ret;
384
385 pr_info("can: SAE J1939\n");
386
387 ret = register_netdevice_notifier(&j1939_netdev_notifier);
388 if (ret)
389 goto fail_notifier;
390
391 ret = can_proto_register(&j1939_can_proto);
392 if (ret < 0) {
393 pr_err("can: registration of j1939 protocol failed\n");
394 goto fail_sk;
395 }
396
397 return 0;
398
399 fail_sk:
400 unregister_netdevice_notifier(&j1939_netdev_notifier);
401 fail_notifier:
402 return ret;
403}
404
405static __exit void j1939_module_exit(void)
406{
407 can_proto_unregister(&j1939_can_proto);
408
409 unregister_netdevice_notifier(&j1939_netdev_notifier);
410}
411
412module_init(j1939_module_init);
413module_exit(j1939_module_exit);
414