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 | /* make sure we only get Classical CAN frames */ |
46 | if (!can_is_can_skb(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(skb: iskb, GFP_ATOMIC); |
55 | if (!skb) |
56 | return; |
57 | |
58 | j1939_priv_get(priv); |
59 | can_skb_set_owner(skb, sk: 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(skb: 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(addr: 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(pgn: 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(addr: skcb->addr.sa) && |
102 | priv->ents[skcb->addr.sa].nusers) |
103 | skcb->flags |= J1939_ECU_LOCAL_SRC; |
104 | if (j1939_address_is_unicast(addr: 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 | |
129 | static DEFINE_MUTEX(j1939_netdev_lock); |
130 | |
131 | static struct j1939_priv *j1939_priv_create(struct net_device *ndev) |
132 | { |
133 | struct j1939_priv *priv; |
134 | |
135 | priv = kzalloc(size: sizeof(*priv), GFP_KERNEL); |
136 | if (!priv) |
137 | return NULL; |
138 | |
139 | rwlock_init(&priv->lock); |
140 | INIT_LIST_HEAD(list: &priv->ecus); |
141 | priv->ndev = ndev; |
142 | kref_init(kref: &priv->kref); |
143 | kref_init(kref: &priv->rx_kref); |
144 | dev_hold(dev: ndev); |
145 | |
146 | netdev_dbg(priv->ndev, "%s : 0x%p\n" , __func__, priv); |
147 | |
148 | return priv; |
149 | } |
150 | |
151 | static 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(dev: ndev); |
155 | |
156 | can_ml->j1939_priv = priv; |
157 | } |
158 | |
159 | static 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(dev: ndev); |
171 | kfree(objp: priv); |
172 | } |
173 | |
174 | void j1939_priv_put(struct j1939_priv *priv) |
175 | { |
176 | kref_put(kref: &priv->kref, release: __j1939_priv_release); |
177 | } |
178 | |
179 | void j1939_priv_get(struct j1939_priv *priv) |
180 | { |
181 | kref_get(kref: &priv->kref); |
182 | } |
183 | |
184 | static 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(net: dev_net(dev: ndev), dev: ndev, J1939_CAN_ID, J1939_CAN_MASK, |
191 | func: j1939_can_recv, data: priv, ident: "j1939" , NULL); |
192 | if (ret < 0) { |
193 | j1939_priv_put(priv); |
194 | return ret; |
195 | } |
196 | |
197 | return 0; |
198 | } |
199 | |
200 | static void j1939_can_rx_unregister(struct j1939_priv *priv) |
201 | { |
202 | struct net_device *ndev = priv->ndev; |
203 | |
204 | can_rx_unregister(net: dev_net(dev: ndev), dev: ndev, J1939_CAN_ID, J1939_CAN_MASK, |
205 | func: j1939_can_recv, data: 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 | |
214 | static 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(ndev: priv->ndev, NULL); |
223 | mutex_unlock(lock: &j1939_netdev_lock); |
224 | } |
225 | |
226 | /* get pointer to priv without increasing ref counter */ |
227 | static inline struct j1939_priv *j1939_ndev_to_priv(struct net_device *ndev) |
228 | { |
229 | struct can_ml_priv *can_ml = can_get_ml_priv(dev: ndev); |
230 | |
231 | return can_ml->j1939_priv; |
232 | } |
233 | |
234 | static 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 | |
247 | static 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(lock: &j1939_netdev_lock); |
254 | |
255 | return priv; |
256 | } |
257 | |
258 | struct 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(kref: &priv->rx_kref); |
267 | mutex_unlock(lock: &j1939_netdev_lock); |
268 | return priv; |
269 | } |
270 | mutex_unlock(lock: &j1939_netdev_lock); |
271 | |
272 | priv = j1939_priv_create(ndev); |
273 | if (!priv) |
274 | return ERR_PTR(error: -ENOMEM); |
275 | |
276 | j1939_tp_init(priv); |
277 | rwlock_init(&priv->j1939_socks_lock); |
278 | INIT_LIST_HEAD(list: &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(kref: &priv_new->rx_kref); |
287 | mutex_unlock(lock: &j1939_netdev_lock); |
288 | dev_put(dev: ndev); |
289 | kfree(objp: 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(lock: &j1939_netdev_lock); |
299 | return priv; |
300 | |
301 | out_priv_put: |
302 | j1939_priv_set(ndev, NULL); |
303 | mutex_unlock(lock: &j1939_netdev_lock); |
304 | |
305 | dev_put(dev: ndev); |
306 | kfree(objp: priv); |
307 | |
308 | return ERR_PTR(error: ret); |
309 | } |
310 | |
311 | void j1939_netdev_stop(struct j1939_priv *priv) |
312 | { |
313 | kref_put_mutex(kref: &priv->rx_kref, release: __j1939_rx_release, lock: &j1939_netdev_lock); |
314 | j1939_priv_put(priv); |
315 | } |
316 | |
317 | int 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(pgn: 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(pgn: 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, loop: 1); |
358 | |
359 | failed: |
360 | kfree_skb(skb); |
361 | return ret; |
362 | } |
363 | |
364 | static 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(info: data); |
368 | struct can_ml_priv *can_ml = can_get_ml_priv(dev: 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 | |
388 | notify_done: |
389 | return NOTIFY_DONE; |
390 | } |
391 | |
392 | static struct notifier_block j1939_netdev_notifier = { |
393 | .notifier_call = j1939_netdev_notify, |
394 | }; |
395 | |
396 | /* MODULE interface */ |
397 | static __init int j1939_module_init(void) |
398 | { |
399 | int ret; |
400 | |
401 | pr_info("can: SAE J1939\n" ); |
402 | |
403 | ret = register_netdevice_notifier(nb: &j1939_netdev_notifier); |
404 | if (ret) |
405 | goto fail_notifier; |
406 | |
407 | ret = can_proto_register(cp: &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(nb: &j1939_netdev_notifier); |
417 | fail_notifier: |
418 | return ret; |
419 | } |
420 | |
421 | static __exit void j1939_module_exit(void) |
422 | { |
423 | can_proto_unregister(cp: &j1939_can_proto); |
424 | |
425 | unregister_netdevice_notifier(nb: &j1939_netdev_notifier); |
426 | } |
427 | |
428 | module_init(j1939_module_init); |
429 | module_exit(j1939_module_exit); |
430 | |