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 | #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt |
14 | |
15 | #include <linux/can/can-ml.h> |
16 | #include <linux/can/core.h> |
17 | #include <linux/can/skb.h> |
18 | #include <linux/errqueue.h> |
19 | #include <linux/if_arp.h> |
20 | |
21 | #include "j1939-priv.h" |
22 | |
23 | #define J1939_MIN_NAMELEN CAN_REQUIRED_SIZE(struct sockaddr_can, can_addr.j1939) |
24 | |
25 | /* conversion function between struct sock::sk_priority from linux and |
26 | * j1939 priority field |
27 | */ |
28 | static inline priority_t j1939_prio(u32 sk_priority) |
29 | { |
30 | sk_priority = min(sk_priority, 7U); |
31 | |
32 | return 7 - sk_priority; |
33 | } |
34 | |
35 | static inline u32 j1939_to_sk_priority(priority_t prio) |
36 | { |
37 | return 7 - prio; |
38 | } |
39 | |
40 | /* function to see if pgn is to be evaluated */ |
41 | static inline bool j1939_pgn_is_valid(pgn_t pgn) |
42 | { |
43 | return pgn <= J1939_PGN_MAX; |
44 | } |
45 | |
46 | /* test function to avoid non-zero DA placeholder for pdu1 pgn's */ |
47 | static inline bool j1939_pgn_is_clean_pdu(pgn_t pgn) |
48 | { |
49 | if (j1939_pgn_is_pdu1(pgn)) |
50 | return !(pgn & 0xff); |
51 | else |
52 | return true; |
53 | } |
54 | |
55 | static inline void j1939_sock_pending_add(struct sock *sk) |
56 | { |
57 | struct j1939_sock *jsk = j1939_sk(sk); |
58 | |
59 | atomic_inc(v: &jsk->skb_pending); |
60 | } |
61 | |
62 | static int j1939_sock_pending_get(struct sock *sk) |
63 | { |
64 | struct j1939_sock *jsk = j1939_sk(sk); |
65 | |
66 | return atomic_read(v: &jsk->skb_pending); |
67 | } |
68 | |
69 | void j1939_sock_pending_del(struct sock *sk) |
70 | { |
71 | struct j1939_sock *jsk = j1939_sk(sk); |
72 | |
73 | /* atomic_dec_return returns the new value */ |
74 | if (!atomic_dec_return(v: &jsk->skb_pending)) |
75 | wake_up(&jsk->waitq); /* no pending SKB's */ |
76 | } |
77 | |
78 | static void j1939_jsk_add(struct j1939_priv *priv, struct j1939_sock *jsk) |
79 | { |
80 | jsk->state |= J1939_SOCK_BOUND; |
81 | j1939_priv_get(priv); |
82 | |
83 | write_lock_bh(&priv->j1939_socks_lock); |
84 | list_add_tail(new: &jsk->list, head: &priv->j1939_socks); |
85 | write_unlock_bh(&priv->j1939_socks_lock); |
86 | } |
87 | |
88 | static void j1939_jsk_del(struct j1939_priv *priv, struct j1939_sock *jsk) |
89 | { |
90 | write_lock_bh(&priv->j1939_socks_lock); |
91 | list_del_init(entry: &jsk->list); |
92 | write_unlock_bh(&priv->j1939_socks_lock); |
93 | |
94 | j1939_priv_put(priv); |
95 | jsk->state &= ~J1939_SOCK_BOUND; |
96 | } |
97 | |
98 | static bool j1939_sk_queue_session(struct j1939_session *session) |
99 | { |
100 | struct j1939_sock *jsk = j1939_sk(sk: session->sk); |
101 | bool empty; |
102 | |
103 | spin_lock_bh(lock: &jsk->sk_session_queue_lock); |
104 | empty = list_empty(head: &jsk->sk_session_queue); |
105 | j1939_session_get(session); |
106 | list_add_tail(new: &session->sk_session_queue_entry, head: &jsk->sk_session_queue); |
107 | spin_unlock_bh(lock: &jsk->sk_session_queue_lock); |
108 | j1939_sock_pending_add(sk: &jsk->sk); |
109 | |
110 | return empty; |
111 | } |
112 | |
113 | static struct |
114 | j1939_session *j1939_sk_get_incomplete_session(struct j1939_sock *jsk) |
115 | { |
116 | struct j1939_session *session = NULL; |
117 | |
118 | spin_lock_bh(lock: &jsk->sk_session_queue_lock); |
119 | if (!list_empty(head: &jsk->sk_session_queue)) { |
120 | session = list_last_entry(&jsk->sk_session_queue, |
121 | struct j1939_session, |
122 | sk_session_queue_entry); |
123 | if (session->total_queued_size == session->total_message_size) |
124 | session = NULL; |
125 | else |
126 | j1939_session_get(session); |
127 | } |
128 | spin_unlock_bh(lock: &jsk->sk_session_queue_lock); |
129 | |
130 | return session; |
131 | } |
132 | |
133 | static void j1939_sk_queue_drop_all(struct j1939_priv *priv, |
134 | struct j1939_sock *jsk, int err) |
135 | { |
136 | struct j1939_session *session, *tmp; |
137 | |
138 | netdev_dbg(priv->ndev, "%s: err: %i\n" , __func__, err); |
139 | spin_lock_bh(lock: &jsk->sk_session_queue_lock); |
140 | list_for_each_entry_safe(session, tmp, &jsk->sk_session_queue, |
141 | sk_session_queue_entry) { |
142 | list_del_init(entry: &session->sk_session_queue_entry); |
143 | session->err = err; |
144 | j1939_session_put(session); |
145 | } |
146 | spin_unlock_bh(lock: &jsk->sk_session_queue_lock); |
147 | } |
148 | |
149 | static void j1939_sk_queue_activate_next_locked(struct j1939_session *session) |
150 | { |
151 | struct j1939_sock *jsk; |
152 | struct j1939_session *first; |
153 | int err; |
154 | |
155 | /* RX-Session don't have a socket (yet) */ |
156 | if (!session->sk) |
157 | return; |
158 | |
159 | jsk = j1939_sk(sk: session->sk); |
160 | lockdep_assert_held(&jsk->sk_session_queue_lock); |
161 | |
162 | err = session->err; |
163 | |
164 | first = list_first_entry_or_null(&jsk->sk_session_queue, |
165 | struct j1939_session, |
166 | sk_session_queue_entry); |
167 | |
168 | /* Some else has already activated the next session */ |
169 | if (first != session) |
170 | return; |
171 | |
172 | activate_next: |
173 | list_del_init(entry: &first->sk_session_queue_entry); |
174 | j1939_session_put(session: first); |
175 | first = list_first_entry_or_null(&jsk->sk_session_queue, |
176 | struct j1939_session, |
177 | sk_session_queue_entry); |
178 | if (!first) |
179 | return; |
180 | |
181 | if (j1939_session_activate(session: first)) { |
182 | netdev_warn_once(first->priv->ndev, |
183 | "%s: 0x%p: Identical session is already activated.\n" , |
184 | __func__, first); |
185 | first->err = -EBUSY; |
186 | goto activate_next; |
187 | } else { |
188 | /* Give receiver some time (arbitrary chosen) to recover */ |
189 | int time_ms = 0; |
190 | |
191 | if (err) |
192 | time_ms = 10 + get_random_u32_below(ceil: 16); |
193 | |
194 | j1939_tp_schedule_txtimer(session: first, msec: time_ms); |
195 | } |
196 | } |
197 | |
198 | void j1939_sk_queue_activate_next(struct j1939_session *session) |
199 | { |
200 | struct j1939_sock *jsk; |
201 | |
202 | if (!session->sk) |
203 | return; |
204 | |
205 | jsk = j1939_sk(sk: session->sk); |
206 | |
207 | spin_lock_bh(lock: &jsk->sk_session_queue_lock); |
208 | j1939_sk_queue_activate_next_locked(session); |
209 | spin_unlock_bh(lock: &jsk->sk_session_queue_lock); |
210 | } |
211 | |
212 | static bool j1939_sk_match_dst(struct j1939_sock *jsk, |
213 | const struct j1939_sk_buff_cb *skcb) |
214 | { |
215 | if ((jsk->state & J1939_SOCK_PROMISC)) |
216 | return true; |
217 | |
218 | /* Destination address filter */ |
219 | if (jsk->addr.src_name && skcb->addr.dst_name) { |
220 | if (jsk->addr.src_name != skcb->addr.dst_name) |
221 | return false; |
222 | } else { |
223 | /* receive (all sockets) if |
224 | * - all packages that match our bind() address |
225 | * - all broadcast on a socket if SO_BROADCAST |
226 | * is set |
227 | */ |
228 | if (j1939_address_is_unicast(addr: skcb->addr.da)) { |
229 | if (jsk->addr.sa != skcb->addr.da) |
230 | return false; |
231 | } else if (!sock_flag(sk: &jsk->sk, flag: SOCK_BROADCAST)) { |
232 | /* receiving broadcast without SO_BROADCAST |
233 | * flag is not allowed |
234 | */ |
235 | return false; |
236 | } |
237 | } |
238 | |
239 | /* Source address filter */ |
240 | if (jsk->state & J1939_SOCK_CONNECTED) { |
241 | /* receive (all sockets) if |
242 | * - all packages that match our connect() name or address |
243 | */ |
244 | if (jsk->addr.dst_name && skcb->addr.src_name) { |
245 | if (jsk->addr.dst_name != skcb->addr.src_name) |
246 | return false; |
247 | } else { |
248 | if (jsk->addr.da != skcb->addr.sa) |
249 | return false; |
250 | } |
251 | } |
252 | |
253 | /* PGN filter */ |
254 | if (j1939_pgn_is_valid(pgn: jsk->pgn_rx_filter) && |
255 | jsk->pgn_rx_filter != skcb->addr.pgn) |
256 | return false; |
257 | |
258 | return true; |
259 | } |
260 | |
261 | /* matches skb control buffer (addr) with a j1939 filter */ |
262 | static bool j1939_sk_match_filter(struct j1939_sock *jsk, |
263 | const struct j1939_sk_buff_cb *skcb) |
264 | { |
265 | const struct j1939_filter *f; |
266 | int nfilter; |
267 | |
268 | spin_lock_bh(lock: &jsk->filters_lock); |
269 | |
270 | f = jsk->filters; |
271 | nfilter = jsk->nfilters; |
272 | |
273 | if (!nfilter) |
274 | /* receive all when no filters are assigned */ |
275 | goto filter_match_found; |
276 | |
277 | for (; nfilter; ++f, --nfilter) { |
278 | if ((skcb->addr.pgn & f->pgn_mask) != f->pgn) |
279 | continue; |
280 | if ((skcb->addr.sa & f->addr_mask) != f->addr) |
281 | continue; |
282 | if ((skcb->addr.src_name & f->name_mask) != f->name) |
283 | continue; |
284 | goto filter_match_found; |
285 | } |
286 | |
287 | spin_unlock_bh(lock: &jsk->filters_lock); |
288 | return false; |
289 | |
290 | filter_match_found: |
291 | spin_unlock_bh(lock: &jsk->filters_lock); |
292 | return true; |
293 | } |
294 | |
295 | static bool j1939_sk_recv_match_one(struct j1939_sock *jsk, |
296 | const struct j1939_sk_buff_cb *skcb) |
297 | { |
298 | if (!(jsk->state & J1939_SOCK_BOUND)) |
299 | return false; |
300 | |
301 | if (!j1939_sk_match_dst(jsk, skcb)) |
302 | return false; |
303 | |
304 | if (!j1939_sk_match_filter(jsk, skcb)) |
305 | return false; |
306 | |
307 | return true; |
308 | } |
309 | |
310 | static void j1939_sk_recv_one(struct j1939_sock *jsk, struct sk_buff *oskb) |
311 | { |
312 | const struct j1939_sk_buff_cb *oskcb = j1939_skb_to_cb(skb: oskb); |
313 | struct j1939_sk_buff_cb *skcb; |
314 | struct sk_buff *skb; |
315 | |
316 | if (oskb->sk == &jsk->sk) |
317 | return; |
318 | |
319 | if (!j1939_sk_recv_match_one(jsk, skcb: oskcb)) |
320 | return; |
321 | |
322 | skb = skb_clone(skb: oskb, GFP_ATOMIC); |
323 | if (!skb) { |
324 | pr_warn("skb clone failed\n" ); |
325 | return; |
326 | } |
327 | can_skb_set_owner(skb, sk: oskb->sk); |
328 | |
329 | skcb = j1939_skb_to_cb(skb); |
330 | skcb->msg_flags &= ~(MSG_DONTROUTE); |
331 | if (skb->sk) |
332 | skcb->msg_flags |= MSG_DONTROUTE; |
333 | |
334 | if (sock_queue_rcv_skb(sk: &jsk->sk, skb) < 0) |
335 | kfree_skb(skb); |
336 | } |
337 | |
338 | bool j1939_sk_recv_match(struct j1939_priv *priv, struct j1939_sk_buff_cb *skcb) |
339 | { |
340 | struct j1939_sock *jsk; |
341 | bool match = false; |
342 | |
343 | read_lock_bh(&priv->j1939_socks_lock); |
344 | list_for_each_entry(jsk, &priv->j1939_socks, list) { |
345 | match = j1939_sk_recv_match_one(jsk, skcb); |
346 | if (match) |
347 | break; |
348 | } |
349 | read_unlock_bh(&priv->j1939_socks_lock); |
350 | |
351 | return match; |
352 | } |
353 | |
354 | void j1939_sk_recv(struct j1939_priv *priv, struct sk_buff *skb) |
355 | { |
356 | struct j1939_sock *jsk; |
357 | |
358 | read_lock_bh(&priv->j1939_socks_lock); |
359 | list_for_each_entry(jsk, &priv->j1939_socks, list) { |
360 | j1939_sk_recv_one(jsk, oskb: skb); |
361 | } |
362 | read_unlock_bh(&priv->j1939_socks_lock); |
363 | } |
364 | |
365 | static void j1939_sk_sock_destruct(struct sock *sk) |
366 | { |
367 | struct j1939_sock *jsk = j1939_sk(sk); |
368 | |
369 | /* This function will be called by the generic networking code, when |
370 | * the socket is ultimately closed (sk->sk_destruct). |
371 | * |
372 | * The race between |
373 | * - processing a received CAN frame |
374 | * (can_receive -> j1939_can_recv) |
375 | * and accessing j1939_priv |
376 | * ... and ... |
377 | * - closing a socket |
378 | * (j1939_can_rx_unregister -> can_rx_unregister) |
379 | * and calling the final j1939_priv_put() |
380 | * |
381 | * is avoided by calling the final j1939_priv_put() from this |
382 | * RCU deferred cleanup call. |
383 | */ |
384 | if (jsk->priv) { |
385 | j1939_priv_put(priv: jsk->priv); |
386 | jsk->priv = NULL; |
387 | } |
388 | |
389 | /* call generic CAN sock destruct */ |
390 | can_sock_destruct(sk); |
391 | } |
392 | |
393 | static int j1939_sk_init(struct sock *sk) |
394 | { |
395 | struct j1939_sock *jsk = j1939_sk(sk); |
396 | |
397 | /* Ensure that "sk" is first member in "struct j1939_sock", so that we |
398 | * can skip it during memset(). |
399 | */ |
400 | BUILD_BUG_ON(offsetof(struct j1939_sock, sk) != 0); |
401 | memset((void *)jsk + sizeof(jsk->sk), 0x0, |
402 | sizeof(*jsk) - sizeof(jsk->sk)); |
403 | |
404 | INIT_LIST_HEAD(list: &jsk->list); |
405 | init_waitqueue_head(&jsk->waitq); |
406 | jsk->sk.sk_priority = j1939_to_sk_priority(prio: 6); |
407 | jsk->sk.sk_reuse = 1; /* per default */ |
408 | jsk->addr.sa = J1939_NO_ADDR; |
409 | jsk->addr.da = J1939_NO_ADDR; |
410 | jsk->addr.pgn = J1939_NO_PGN; |
411 | jsk->pgn_rx_filter = J1939_NO_PGN; |
412 | atomic_set(v: &jsk->skb_pending, i: 0); |
413 | spin_lock_init(&jsk->sk_session_queue_lock); |
414 | INIT_LIST_HEAD(list: &jsk->sk_session_queue); |
415 | spin_lock_init(&jsk->filters_lock); |
416 | |
417 | /* j1939_sk_sock_destruct() depends on SOCK_RCU_FREE flag */ |
418 | sock_set_flag(sk, flag: SOCK_RCU_FREE); |
419 | sk->sk_destruct = j1939_sk_sock_destruct; |
420 | sk->sk_protocol = CAN_J1939; |
421 | |
422 | return 0; |
423 | } |
424 | |
425 | static int j1939_sk_sanity_check(struct sockaddr_can *addr, int len) |
426 | { |
427 | if (!addr) |
428 | return -EDESTADDRREQ; |
429 | if (len < J1939_MIN_NAMELEN) |
430 | return -EINVAL; |
431 | if (addr->can_family != AF_CAN) |
432 | return -EINVAL; |
433 | if (!addr->can_ifindex) |
434 | return -ENODEV; |
435 | if (j1939_pgn_is_valid(pgn: addr->can_addr.j1939.pgn) && |
436 | !j1939_pgn_is_clean_pdu(pgn: addr->can_addr.j1939.pgn)) |
437 | return -EINVAL; |
438 | |
439 | return 0; |
440 | } |
441 | |
442 | static int j1939_sk_bind(struct socket *sock, struct sockaddr *uaddr, int len) |
443 | { |
444 | struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; |
445 | struct j1939_sock *jsk = j1939_sk(sk: sock->sk); |
446 | struct j1939_priv *priv; |
447 | struct sock *sk; |
448 | struct net *net; |
449 | int ret = 0; |
450 | |
451 | ret = j1939_sk_sanity_check(addr, len); |
452 | if (ret) |
453 | return ret; |
454 | |
455 | lock_sock(sk: sock->sk); |
456 | |
457 | priv = jsk->priv; |
458 | sk = sock->sk; |
459 | net = sock_net(sk); |
460 | |
461 | /* Already bound to an interface? */ |
462 | if (jsk->state & J1939_SOCK_BOUND) { |
463 | /* A re-bind() to a different interface is not |
464 | * supported. |
465 | */ |
466 | if (jsk->ifindex != addr->can_ifindex) { |
467 | ret = -EINVAL; |
468 | goto out_release_sock; |
469 | } |
470 | |
471 | /* drop old references */ |
472 | j1939_jsk_del(priv, jsk); |
473 | j1939_local_ecu_put(priv, name: jsk->addr.src_name, sa: jsk->addr.sa); |
474 | } else { |
475 | struct can_ml_priv *can_ml; |
476 | struct net_device *ndev; |
477 | |
478 | ndev = dev_get_by_index(net, ifindex: addr->can_ifindex); |
479 | if (!ndev) { |
480 | ret = -ENODEV; |
481 | goto out_release_sock; |
482 | } |
483 | |
484 | can_ml = can_get_ml_priv(dev: ndev); |
485 | if (!can_ml) { |
486 | dev_put(dev: ndev); |
487 | ret = -ENODEV; |
488 | goto out_release_sock; |
489 | } |
490 | |
491 | if (!(ndev->flags & IFF_UP)) { |
492 | dev_put(dev: ndev); |
493 | ret = -ENETDOWN; |
494 | goto out_release_sock; |
495 | } |
496 | |
497 | priv = j1939_netdev_start(ndev); |
498 | dev_put(dev: ndev); |
499 | if (IS_ERR(ptr: priv)) { |
500 | ret = PTR_ERR(ptr: priv); |
501 | goto out_release_sock; |
502 | } |
503 | |
504 | jsk->ifindex = addr->can_ifindex; |
505 | |
506 | /* the corresponding j1939_priv_put() is called via |
507 | * sk->sk_destruct, which points to j1939_sk_sock_destruct() |
508 | */ |
509 | j1939_priv_get(priv); |
510 | jsk->priv = priv; |
511 | } |
512 | |
513 | /* set default transmit pgn */ |
514 | if (j1939_pgn_is_valid(pgn: addr->can_addr.j1939.pgn)) |
515 | jsk->pgn_rx_filter = addr->can_addr.j1939.pgn; |
516 | jsk->addr.src_name = addr->can_addr.j1939.name; |
517 | jsk->addr.sa = addr->can_addr.j1939.addr; |
518 | |
519 | /* get new references */ |
520 | ret = j1939_local_ecu_get(priv, name: jsk->addr.src_name, sa: jsk->addr.sa); |
521 | if (ret) { |
522 | j1939_netdev_stop(priv); |
523 | goto out_release_sock; |
524 | } |
525 | |
526 | j1939_jsk_add(priv, jsk); |
527 | |
528 | out_release_sock: /* fall through */ |
529 | release_sock(sk: sock->sk); |
530 | |
531 | return ret; |
532 | } |
533 | |
534 | static int j1939_sk_connect(struct socket *sock, struct sockaddr *uaddr, |
535 | int len, int flags) |
536 | { |
537 | struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; |
538 | struct j1939_sock *jsk = j1939_sk(sk: sock->sk); |
539 | int ret = 0; |
540 | |
541 | ret = j1939_sk_sanity_check(addr, len); |
542 | if (ret) |
543 | return ret; |
544 | |
545 | lock_sock(sk: sock->sk); |
546 | |
547 | /* bind() before connect() is mandatory */ |
548 | if (!(jsk->state & J1939_SOCK_BOUND)) { |
549 | ret = -EINVAL; |
550 | goto out_release_sock; |
551 | } |
552 | |
553 | /* A connect() to a different interface is not supported. */ |
554 | if (jsk->ifindex != addr->can_ifindex) { |
555 | ret = -EINVAL; |
556 | goto out_release_sock; |
557 | } |
558 | |
559 | if (!addr->can_addr.j1939.name && |
560 | addr->can_addr.j1939.addr == J1939_NO_ADDR && |
561 | !sock_flag(sk: &jsk->sk, flag: SOCK_BROADCAST)) { |
562 | /* broadcast, but SO_BROADCAST not set */ |
563 | ret = -EACCES; |
564 | goto out_release_sock; |
565 | } |
566 | |
567 | jsk->addr.dst_name = addr->can_addr.j1939.name; |
568 | jsk->addr.da = addr->can_addr.j1939.addr; |
569 | |
570 | if (j1939_pgn_is_valid(pgn: addr->can_addr.j1939.pgn)) |
571 | jsk->addr.pgn = addr->can_addr.j1939.pgn; |
572 | |
573 | jsk->state |= J1939_SOCK_CONNECTED; |
574 | |
575 | out_release_sock: /* fall through */ |
576 | release_sock(sk: sock->sk); |
577 | |
578 | return ret; |
579 | } |
580 | |
581 | static void j1939_sk_sock2sockaddr_can(struct sockaddr_can *addr, |
582 | const struct j1939_sock *jsk, int peer) |
583 | { |
584 | /* There are two holes (2 bytes and 3 bytes) to clear to avoid |
585 | * leaking kernel information to user space. |
586 | */ |
587 | memset(addr, 0, J1939_MIN_NAMELEN); |
588 | |
589 | addr->can_family = AF_CAN; |
590 | addr->can_ifindex = jsk->ifindex; |
591 | addr->can_addr.j1939.pgn = jsk->addr.pgn; |
592 | if (peer) { |
593 | addr->can_addr.j1939.name = jsk->addr.dst_name; |
594 | addr->can_addr.j1939.addr = jsk->addr.da; |
595 | } else { |
596 | addr->can_addr.j1939.name = jsk->addr.src_name; |
597 | addr->can_addr.j1939.addr = jsk->addr.sa; |
598 | } |
599 | } |
600 | |
601 | static int j1939_sk_getname(struct socket *sock, struct sockaddr *uaddr, |
602 | int peer) |
603 | { |
604 | struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; |
605 | struct sock *sk = sock->sk; |
606 | struct j1939_sock *jsk = j1939_sk(sk); |
607 | int ret = 0; |
608 | |
609 | lock_sock(sk); |
610 | |
611 | if (peer && !(jsk->state & J1939_SOCK_CONNECTED)) { |
612 | ret = -EADDRNOTAVAIL; |
613 | goto failure; |
614 | } |
615 | |
616 | j1939_sk_sock2sockaddr_can(addr, jsk, peer); |
617 | ret = J1939_MIN_NAMELEN; |
618 | |
619 | failure: |
620 | release_sock(sk); |
621 | |
622 | return ret; |
623 | } |
624 | |
625 | static int j1939_sk_release(struct socket *sock) |
626 | { |
627 | struct sock *sk = sock->sk; |
628 | struct j1939_sock *jsk; |
629 | |
630 | if (!sk) |
631 | return 0; |
632 | |
633 | lock_sock(sk); |
634 | jsk = j1939_sk(sk); |
635 | |
636 | if (jsk->state & J1939_SOCK_BOUND) { |
637 | struct j1939_priv *priv = jsk->priv; |
638 | |
639 | if (wait_event_interruptible(jsk->waitq, |
640 | !j1939_sock_pending_get(&jsk->sk))) { |
641 | j1939_cancel_active_session(priv, sk); |
642 | j1939_sk_queue_drop_all(priv, jsk, ESHUTDOWN); |
643 | } |
644 | |
645 | j1939_jsk_del(priv, jsk); |
646 | |
647 | j1939_local_ecu_put(priv, name: jsk->addr.src_name, |
648 | sa: jsk->addr.sa); |
649 | |
650 | j1939_netdev_stop(priv); |
651 | } |
652 | |
653 | kfree(objp: jsk->filters); |
654 | sock_orphan(sk); |
655 | sock->sk = NULL; |
656 | |
657 | release_sock(sk); |
658 | sock_put(sk); |
659 | |
660 | return 0; |
661 | } |
662 | |
663 | static int j1939_sk_setsockopt_flag(struct j1939_sock *jsk, sockptr_t optval, |
664 | unsigned int optlen, int flag) |
665 | { |
666 | int tmp; |
667 | |
668 | if (optlen != sizeof(tmp)) |
669 | return -EINVAL; |
670 | if (copy_from_sockptr(dst: &tmp, src: optval, size: optlen)) |
671 | return -EFAULT; |
672 | lock_sock(sk: &jsk->sk); |
673 | if (tmp) |
674 | jsk->state |= flag; |
675 | else |
676 | jsk->state &= ~flag; |
677 | release_sock(sk: &jsk->sk); |
678 | return tmp; |
679 | } |
680 | |
681 | static int j1939_sk_setsockopt(struct socket *sock, int level, int optname, |
682 | sockptr_t optval, unsigned int optlen) |
683 | { |
684 | struct sock *sk = sock->sk; |
685 | struct j1939_sock *jsk = j1939_sk(sk); |
686 | int tmp, count = 0, ret = 0; |
687 | struct j1939_filter *filters = NULL, *ofilters; |
688 | |
689 | if (level != SOL_CAN_J1939) |
690 | return -EINVAL; |
691 | |
692 | switch (optname) { |
693 | case SO_J1939_FILTER: |
694 | if (!sockptr_is_null(sockptr: optval) && optlen != 0) { |
695 | struct j1939_filter *f; |
696 | int c; |
697 | |
698 | if (optlen % sizeof(*filters) != 0) |
699 | return -EINVAL; |
700 | |
701 | if (optlen > J1939_FILTER_MAX * |
702 | sizeof(struct j1939_filter)) |
703 | return -EINVAL; |
704 | |
705 | count = optlen / sizeof(*filters); |
706 | filters = memdup_sockptr(src: optval, len: optlen); |
707 | if (IS_ERR(ptr: filters)) |
708 | return PTR_ERR(ptr: filters); |
709 | |
710 | for (f = filters, c = count; c; f++, c--) { |
711 | f->name &= f->name_mask; |
712 | f->pgn &= f->pgn_mask; |
713 | f->addr &= f->addr_mask; |
714 | } |
715 | } |
716 | |
717 | lock_sock(sk: &jsk->sk); |
718 | spin_lock_bh(lock: &jsk->filters_lock); |
719 | ofilters = jsk->filters; |
720 | jsk->filters = filters; |
721 | jsk->nfilters = count; |
722 | spin_unlock_bh(lock: &jsk->filters_lock); |
723 | release_sock(sk: &jsk->sk); |
724 | kfree(objp: ofilters); |
725 | return 0; |
726 | case SO_J1939_PROMISC: |
727 | return j1939_sk_setsockopt_flag(jsk, optval, optlen, |
728 | J1939_SOCK_PROMISC); |
729 | case SO_J1939_ERRQUEUE: |
730 | ret = j1939_sk_setsockopt_flag(jsk, optval, optlen, |
731 | J1939_SOCK_ERRQUEUE); |
732 | if (ret < 0) |
733 | return ret; |
734 | |
735 | if (!(jsk->state & J1939_SOCK_ERRQUEUE)) |
736 | skb_queue_purge(list: &sk->sk_error_queue); |
737 | return ret; |
738 | case SO_J1939_SEND_PRIO: |
739 | if (optlen != sizeof(tmp)) |
740 | return -EINVAL; |
741 | if (copy_from_sockptr(dst: &tmp, src: optval, size: optlen)) |
742 | return -EFAULT; |
743 | if (tmp < 0 || tmp > 7) |
744 | return -EDOM; |
745 | if (tmp < 2 && !capable(CAP_NET_ADMIN)) |
746 | return -EPERM; |
747 | lock_sock(sk: &jsk->sk); |
748 | jsk->sk.sk_priority = j1939_to_sk_priority(prio: tmp); |
749 | release_sock(sk: &jsk->sk); |
750 | return 0; |
751 | default: |
752 | return -ENOPROTOOPT; |
753 | } |
754 | } |
755 | |
756 | static int j1939_sk_getsockopt(struct socket *sock, int level, int optname, |
757 | char __user *optval, int __user *optlen) |
758 | { |
759 | struct sock *sk = sock->sk; |
760 | struct j1939_sock *jsk = j1939_sk(sk); |
761 | int ret, ulen; |
762 | /* set defaults for using 'int' properties */ |
763 | int tmp = 0; |
764 | int len = sizeof(tmp); |
765 | void *val = &tmp; |
766 | |
767 | if (level != SOL_CAN_J1939) |
768 | return -EINVAL; |
769 | if (get_user(ulen, optlen)) |
770 | return -EFAULT; |
771 | if (ulen < 0) |
772 | return -EINVAL; |
773 | |
774 | lock_sock(sk: &jsk->sk); |
775 | switch (optname) { |
776 | case SO_J1939_PROMISC: |
777 | tmp = (jsk->state & J1939_SOCK_PROMISC) ? 1 : 0; |
778 | break; |
779 | case SO_J1939_ERRQUEUE: |
780 | tmp = (jsk->state & J1939_SOCK_ERRQUEUE) ? 1 : 0; |
781 | break; |
782 | case SO_J1939_SEND_PRIO: |
783 | tmp = j1939_prio(sk_priority: jsk->sk.sk_priority); |
784 | break; |
785 | default: |
786 | ret = -ENOPROTOOPT; |
787 | goto no_copy; |
788 | } |
789 | |
790 | /* copy to user, based on 'len' & 'val' |
791 | * but most sockopt's are 'int' properties, and have 'len' & 'val' |
792 | * left unchanged, but instead modified 'tmp' |
793 | */ |
794 | if (len > ulen) |
795 | ret = -EFAULT; |
796 | else if (put_user(len, optlen)) |
797 | ret = -EFAULT; |
798 | else if (copy_to_user(to: optval, from: val, n: len)) |
799 | ret = -EFAULT; |
800 | else |
801 | ret = 0; |
802 | no_copy: |
803 | release_sock(sk: &jsk->sk); |
804 | return ret; |
805 | } |
806 | |
807 | static int j1939_sk_recvmsg(struct socket *sock, struct msghdr *msg, |
808 | size_t size, int flags) |
809 | { |
810 | struct sock *sk = sock->sk; |
811 | struct sk_buff *skb; |
812 | struct j1939_sk_buff_cb *skcb; |
813 | int ret = 0; |
814 | |
815 | if (flags & ~(MSG_DONTWAIT | MSG_ERRQUEUE | MSG_CMSG_COMPAT)) |
816 | return -EINVAL; |
817 | |
818 | if (flags & MSG_ERRQUEUE) |
819 | return sock_recv_errqueue(sk: sock->sk, msg, len: size, SOL_CAN_J1939, |
820 | type: SCM_J1939_ERRQUEUE); |
821 | |
822 | skb = skb_recv_datagram(sk, flags, err: &ret); |
823 | if (!skb) |
824 | return ret; |
825 | |
826 | if (size < skb->len) |
827 | msg->msg_flags |= MSG_TRUNC; |
828 | else |
829 | size = skb->len; |
830 | |
831 | ret = memcpy_to_msg(msg, data: skb->data, len: size); |
832 | if (ret < 0) { |
833 | skb_free_datagram(sk, skb); |
834 | return ret; |
835 | } |
836 | |
837 | skcb = j1939_skb_to_cb(skb); |
838 | if (j1939_address_is_valid(addr: skcb->addr.da)) |
839 | put_cmsg(msg, SOL_CAN_J1939, type: SCM_J1939_DEST_ADDR, |
840 | len: sizeof(skcb->addr.da), data: &skcb->addr.da); |
841 | |
842 | if (skcb->addr.dst_name) |
843 | put_cmsg(msg, SOL_CAN_J1939, type: SCM_J1939_DEST_NAME, |
844 | len: sizeof(skcb->addr.dst_name), data: &skcb->addr.dst_name); |
845 | |
846 | put_cmsg(msg, SOL_CAN_J1939, type: SCM_J1939_PRIO, |
847 | len: sizeof(skcb->priority), data: &skcb->priority); |
848 | |
849 | if (msg->msg_name) { |
850 | struct sockaddr_can *paddr = msg->msg_name; |
851 | |
852 | msg->msg_namelen = J1939_MIN_NAMELEN; |
853 | memset(msg->msg_name, 0, msg->msg_namelen); |
854 | paddr->can_family = AF_CAN; |
855 | paddr->can_ifindex = skb->skb_iif; |
856 | paddr->can_addr.j1939.name = skcb->addr.src_name; |
857 | paddr->can_addr.j1939.addr = skcb->addr.sa; |
858 | paddr->can_addr.j1939.pgn = skcb->addr.pgn; |
859 | } |
860 | |
861 | sock_recv_cmsgs(msg, sk, skb); |
862 | msg->msg_flags |= skcb->msg_flags; |
863 | skb_free_datagram(sk, skb); |
864 | |
865 | return size; |
866 | } |
867 | |
868 | static struct sk_buff *j1939_sk_alloc_skb(struct net_device *ndev, |
869 | struct sock *sk, |
870 | struct msghdr *msg, size_t size, |
871 | int *errcode) |
872 | { |
873 | struct j1939_sock *jsk = j1939_sk(sk); |
874 | struct j1939_sk_buff_cb *skcb; |
875 | struct sk_buff *skb; |
876 | int ret; |
877 | |
878 | skb = sock_alloc_send_skb(sk, |
879 | size: size + |
880 | sizeof(struct can_frame) - |
881 | sizeof(((struct can_frame *)NULL)->data) + |
882 | sizeof(struct can_skb_priv), |
883 | noblock: msg->msg_flags & MSG_DONTWAIT, errcode: &ret); |
884 | if (!skb) |
885 | goto failure; |
886 | |
887 | can_skb_reserve(skb); |
888 | can_skb_prv(skb)->ifindex = ndev->ifindex; |
889 | can_skb_prv(skb)->skbcnt = 0; |
890 | skb_reserve(skb, offsetof(struct can_frame, data)); |
891 | |
892 | ret = memcpy_from_msg(data: skb_put(skb, len: size), msg, len: size); |
893 | if (ret < 0) |
894 | goto free_skb; |
895 | |
896 | skb->dev = ndev; |
897 | |
898 | skcb = j1939_skb_to_cb(skb); |
899 | memset(skcb, 0, sizeof(*skcb)); |
900 | skcb->addr = jsk->addr; |
901 | skcb->priority = j1939_prio(READ_ONCE(sk->sk_priority)); |
902 | |
903 | if (msg->msg_name) { |
904 | struct sockaddr_can *addr = msg->msg_name; |
905 | |
906 | if (addr->can_addr.j1939.name || |
907 | addr->can_addr.j1939.addr != J1939_NO_ADDR) { |
908 | skcb->addr.dst_name = addr->can_addr.j1939.name; |
909 | skcb->addr.da = addr->can_addr.j1939.addr; |
910 | } |
911 | if (j1939_pgn_is_valid(pgn: addr->can_addr.j1939.pgn)) |
912 | skcb->addr.pgn = addr->can_addr.j1939.pgn; |
913 | } |
914 | |
915 | *errcode = ret; |
916 | return skb; |
917 | |
918 | free_skb: |
919 | kfree_skb(skb); |
920 | failure: |
921 | *errcode = ret; |
922 | return NULL; |
923 | } |
924 | |
925 | static size_t j1939_sk_opt_stats_get_size(enum j1939_sk_errqueue_type type) |
926 | { |
927 | switch (type) { |
928 | case J1939_ERRQUEUE_RX_RTS: |
929 | return |
930 | nla_total_size(payload: sizeof(u32)) + /* J1939_NLA_TOTAL_SIZE */ |
931 | nla_total_size(payload: sizeof(u32)) + /* J1939_NLA_PGN */ |
932 | nla_total_size(payload: sizeof(u64)) + /* J1939_NLA_SRC_NAME */ |
933 | nla_total_size(payload: sizeof(u64)) + /* J1939_NLA_DEST_NAME */ |
934 | nla_total_size(payload: sizeof(u8)) + /* J1939_NLA_SRC_ADDR */ |
935 | nla_total_size(payload: sizeof(u8)) + /* J1939_NLA_DEST_ADDR */ |
936 | 0; |
937 | default: |
938 | return |
939 | nla_total_size(payload: sizeof(u32)) + /* J1939_NLA_BYTES_ACKED */ |
940 | 0; |
941 | } |
942 | } |
943 | |
944 | static struct sk_buff * |
945 | j1939_sk_get_timestamping_opt_stats(struct j1939_session *session, |
946 | enum j1939_sk_errqueue_type type) |
947 | { |
948 | struct sk_buff *stats; |
949 | u32 size; |
950 | |
951 | stats = alloc_skb(size: j1939_sk_opt_stats_get_size(type), GFP_ATOMIC); |
952 | if (!stats) |
953 | return NULL; |
954 | |
955 | if (session->skcb.addr.type == J1939_SIMPLE) |
956 | size = session->total_message_size; |
957 | else |
958 | size = min(session->pkt.tx_acked * 7, |
959 | session->total_message_size); |
960 | |
961 | switch (type) { |
962 | case J1939_ERRQUEUE_RX_RTS: |
963 | nla_put_u32(skb: stats, attrtype: J1939_NLA_TOTAL_SIZE, |
964 | value: session->total_message_size); |
965 | nla_put_u32(skb: stats, attrtype: J1939_NLA_PGN, |
966 | value: session->skcb.addr.pgn); |
967 | nla_put_u64_64bit(skb: stats, attrtype: J1939_NLA_SRC_NAME, |
968 | value: session->skcb.addr.src_name, padattr: J1939_NLA_PAD); |
969 | nla_put_u64_64bit(skb: stats, attrtype: J1939_NLA_DEST_NAME, |
970 | value: session->skcb.addr.dst_name, padattr: J1939_NLA_PAD); |
971 | nla_put_u8(skb: stats, attrtype: J1939_NLA_SRC_ADDR, |
972 | value: session->skcb.addr.sa); |
973 | nla_put_u8(skb: stats, attrtype: J1939_NLA_DEST_ADDR, |
974 | value: session->skcb.addr.da); |
975 | break; |
976 | default: |
977 | nla_put_u32(skb: stats, attrtype: J1939_NLA_BYTES_ACKED, value: size); |
978 | } |
979 | |
980 | return stats; |
981 | } |
982 | |
983 | static void __j1939_sk_errqueue(struct j1939_session *session, struct sock *sk, |
984 | enum j1939_sk_errqueue_type type) |
985 | { |
986 | struct j1939_priv *priv = session->priv; |
987 | struct j1939_sock *jsk; |
988 | struct sock_exterr_skb *serr; |
989 | struct sk_buff *skb; |
990 | char *state = "UNK" ; |
991 | u32 tsflags; |
992 | int err; |
993 | |
994 | jsk = j1939_sk(sk); |
995 | |
996 | if (!(jsk->state & J1939_SOCK_ERRQUEUE)) |
997 | return; |
998 | |
999 | tsflags = READ_ONCE(sk->sk_tsflags); |
1000 | switch (type) { |
1001 | case J1939_ERRQUEUE_TX_ACK: |
1002 | if (!(tsflags & SOF_TIMESTAMPING_TX_ACK)) |
1003 | return; |
1004 | break; |
1005 | case J1939_ERRQUEUE_TX_SCHED: |
1006 | if (!(tsflags & SOF_TIMESTAMPING_TX_SCHED)) |
1007 | return; |
1008 | break; |
1009 | case J1939_ERRQUEUE_TX_ABORT: |
1010 | break; |
1011 | case J1939_ERRQUEUE_RX_RTS: |
1012 | fallthrough; |
1013 | case J1939_ERRQUEUE_RX_DPO: |
1014 | fallthrough; |
1015 | case J1939_ERRQUEUE_RX_ABORT: |
1016 | if (!(tsflags & SOF_TIMESTAMPING_RX_SOFTWARE)) |
1017 | return; |
1018 | break; |
1019 | default: |
1020 | netdev_err(dev: priv->ndev, format: "Unknown errqueue type %i\n" , type); |
1021 | } |
1022 | |
1023 | skb = j1939_sk_get_timestamping_opt_stats(session, type); |
1024 | if (!skb) |
1025 | return; |
1026 | |
1027 | skb->tstamp = ktime_get_real(); |
1028 | |
1029 | BUILD_BUG_ON(sizeof(struct sock_exterr_skb) > sizeof(skb->cb)); |
1030 | |
1031 | serr = SKB_EXT_ERR(skb); |
1032 | memset(serr, 0, sizeof(*serr)); |
1033 | switch (type) { |
1034 | case J1939_ERRQUEUE_TX_ACK: |
1035 | serr->ee.ee_errno = ENOMSG; |
1036 | serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING; |
1037 | serr->ee.ee_info = SCM_TSTAMP_ACK; |
1038 | state = "TX ACK" ; |
1039 | break; |
1040 | case J1939_ERRQUEUE_TX_SCHED: |
1041 | serr->ee.ee_errno = ENOMSG; |
1042 | serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING; |
1043 | serr->ee.ee_info = SCM_TSTAMP_SCHED; |
1044 | state = "TX SCH" ; |
1045 | break; |
1046 | case J1939_ERRQUEUE_TX_ABORT: |
1047 | serr->ee.ee_errno = session->err; |
1048 | serr->ee.ee_origin = SO_EE_ORIGIN_LOCAL; |
1049 | serr->ee.ee_info = J1939_EE_INFO_TX_ABORT; |
1050 | state = "TX ABT" ; |
1051 | break; |
1052 | case J1939_ERRQUEUE_RX_RTS: |
1053 | serr->ee.ee_errno = ENOMSG; |
1054 | serr->ee.ee_origin = SO_EE_ORIGIN_LOCAL; |
1055 | serr->ee.ee_info = J1939_EE_INFO_RX_RTS; |
1056 | state = "RX RTS" ; |
1057 | break; |
1058 | case J1939_ERRQUEUE_RX_DPO: |
1059 | serr->ee.ee_errno = ENOMSG; |
1060 | serr->ee.ee_origin = SO_EE_ORIGIN_LOCAL; |
1061 | serr->ee.ee_info = J1939_EE_INFO_RX_DPO; |
1062 | state = "RX DPO" ; |
1063 | break; |
1064 | case J1939_ERRQUEUE_RX_ABORT: |
1065 | serr->ee.ee_errno = session->err; |
1066 | serr->ee.ee_origin = SO_EE_ORIGIN_LOCAL; |
1067 | serr->ee.ee_info = J1939_EE_INFO_RX_ABORT; |
1068 | state = "RX ABT" ; |
1069 | break; |
1070 | } |
1071 | |
1072 | serr->opt_stats = true; |
1073 | if (tsflags & SOF_TIMESTAMPING_OPT_ID) |
1074 | serr->ee.ee_data = session->tskey; |
1075 | |
1076 | netdev_dbg(session->priv->ndev, "%s: 0x%p tskey: %i, state: %s\n" , |
1077 | __func__, session, session->tskey, state); |
1078 | err = sock_queue_err_skb(sk, skb); |
1079 | |
1080 | if (err) |
1081 | kfree_skb(skb); |
1082 | }; |
1083 | |
1084 | void j1939_sk_errqueue(struct j1939_session *session, |
1085 | enum j1939_sk_errqueue_type type) |
1086 | { |
1087 | struct j1939_priv *priv = session->priv; |
1088 | struct j1939_sock *jsk; |
1089 | |
1090 | if (session->sk) { |
1091 | /* send TX notifications to the socket of origin */ |
1092 | __j1939_sk_errqueue(session, sk: session->sk, type); |
1093 | return; |
1094 | } |
1095 | |
1096 | /* spread RX notifications to all sockets subscribed to this session */ |
1097 | read_lock_bh(&priv->j1939_socks_lock); |
1098 | list_for_each_entry(jsk, &priv->j1939_socks, list) { |
1099 | if (j1939_sk_recv_match_one(jsk, skcb: &session->skcb)) |
1100 | __j1939_sk_errqueue(session, sk: &jsk->sk, type); |
1101 | } |
1102 | read_unlock_bh(&priv->j1939_socks_lock); |
1103 | }; |
1104 | |
1105 | void j1939_sk_send_loop_abort(struct sock *sk, int err) |
1106 | { |
1107 | struct j1939_sock *jsk = j1939_sk(sk); |
1108 | |
1109 | if (jsk->state & J1939_SOCK_ERRQUEUE) |
1110 | return; |
1111 | |
1112 | sk->sk_err = err; |
1113 | |
1114 | sk_error_report(sk); |
1115 | } |
1116 | |
1117 | static int j1939_sk_send_loop(struct j1939_priv *priv, struct sock *sk, |
1118 | struct msghdr *msg, size_t size) |
1119 | |
1120 | { |
1121 | struct j1939_sock *jsk = j1939_sk(sk); |
1122 | struct j1939_session *session = j1939_sk_get_incomplete_session(jsk); |
1123 | struct sk_buff *skb; |
1124 | size_t segment_size, todo_size; |
1125 | int ret = 0; |
1126 | |
1127 | if (session && |
1128 | session->total_message_size != session->total_queued_size + size) { |
1129 | j1939_session_put(session); |
1130 | return -EIO; |
1131 | } |
1132 | |
1133 | todo_size = size; |
1134 | |
1135 | while (todo_size) { |
1136 | struct j1939_sk_buff_cb *skcb; |
1137 | |
1138 | segment_size = min_t(size_t, J1939_MAX_TP_PACKET_SIZE, |
1139 | todo_size); |
1140 | |
1141 | /* Allocate skb for one segment */ |
1142 | skb = j1939_sk_alloc_skb(ndev: priv->ndev, sk, msg, size: segment_size, |
1143 | errcode: &ret); |
1144 | if (ret) |
1145 | break; |
1146 | |
1147 | skcb = j1939_skb_to_cb(skb); |
1148 | |
1149 | if (!session) { |
1150 | /* at this point the size should be full size |
1151 | * of the session |
1152 | */ |
1153 | skcb->offset = 0; |
1154 | session = j1939_tp_send(priv, skb, size); |
1155 | if (IS_ERR(ptr: session)) { |
1156 | ret = PTR_ERR(ptr: session); |
1157 | goto kfree_skb; |
1158 | } |
1159 | if (j1939_sk_queue_session(session)) { |
1160 | /* try to activate session if we a |
1161 | * fist in the queue |
1162 | */ |
1163 | if (!j1939_session_activate(session)) { |
1164 | j1939_tp_schedule_txtimer(session, msec: 0); |
1165 | } else { |
1166 | ret = -EBUSY; |
1167 | session->err = ret; |
1168 | j1939_sk_queue_drop_all(priv, jsk, |
1169 | EBUSY); |
1170 | break; |
1171 | } |
1172 | } |
1173 | } else { |
1174 | skcb->offset = session->total_queued_size; |
1175 | j1939_session_skb_queue(session, skb); |
1176 | } |
1177 | |
1178 | todo_size -= segment_size; |
1179 | session->total_queued_size += segment_size; |
1180 | } |
1181 | |
1182 | switch (ret) { |
1183 | case 0: /* OK */ |
1184 | if (todo_size) |
1185 | netdev_warn(dev: priv->ndev, |
1186 | format: "no error found and not completely queued?! %zu\n" , |
1187 | todo_size); |
1188 | ret = size; |
1189 | break; |
1190 | case -ERESTARTSYS: |
1191 | ret = -EINTR; |
1192 | fallthrough; |
1193 | case -EAGAIN: /* OK */ |
1194 | if (todo_size != size) |
1195 | ret = size - todo_size; |
1196 | break; |
1197 | default: /* ERROR */ |
1198 | break; |
1199 | } |
1200 | |
1201 | if (session) |
1202 | j1939_session_put(session); |
1203 | |
1204 | return ret; |
1205 | |
1206 | kfree_skb: |
1207 | kfree_skb(skb); |
1208 | return ret; |
1209 | } |
1210 | |
1211 | static int j1939_sk_sendmsg(struct socket *sock, struct msghdr *msg, |
1212 | size_t size) |
1213 | { |
1214 | struct sock *sk = sock->sk; |
1215 | struct j1939_sock *jsk = j1939_sk(sk); |
1216 | struct j1939_priv *priv; |
1217 | int ifindex; |
1218 | int ret; |
1219 | |
1220 | lock_sock(sk: sock->sk); |
1221 | /* various socket state tests */ |
1222 | if (!(jsk->state & J1939_SOCK_BOUND)) { |
1223 | ret = -EBADFD; |
1224 | goto sendmsg_done; |
1225 | } |
1226 | |
1227 | priv = jsk->priv; |
1228 | ifindex = jsk->ifindex; |
1229 | |
1230 | if (!jsk->addr.src_name && jsk->addr.sa == J1939_NO_ADDR) { |
1231 | /* no source address assigned yet */ |
1232 | ret = -EBADFD; |
1233 | goto sendmsg_done; |
1234 | } |
1235 | |
1236 | /* deal with provided destination address info */ |
1237 | if (msg->msg_name) { |
1238 | struct sockaddr_can *addr = msg->msg_name; |
1239 | |
1240 | if (msg->msg_namelen < J1939_MIN_NAMELEN) { |
1241 | ret = -EINVAL; |
1242 | goto sendmsg_done; |
1243 | } |
1244 | |
1245 | if (addr->can_family != AF_CAN) { |
1246 | ret = -EINVAL; |
1247 | goto sendmsg_done; |
1248 | } |
1249 | |
1250 | if (addr->can_ifindex && addr->can_ifindex != ifindex) { |
1251 | ret = -EBADFD; |
1252 | goto sendmsg_done; |
1253 | } |
1254 | |
1255 | if (j1939_pgn_is_valid(pgn: addr->can_addr.j1939.pgn) && |
1256 | !j1939_pgn_is_clean_pdu(pgn: addr->can_addr.j1939.pgn)) { |
1257 | ret = -EINVAL; |
1258 | goto sendmsg_done; |
1259 | } |
1260 | |
1261 | if (!addr->can_addr.j1939.name && |
1262 | addr->can_addr.j1939.addr == J1939_NO_ADDR && |
1263 | !sock_flag(sk, flag: SOCK_BROADCAST)) { |
1264 | /* broadcast, but SO_BROADCAST not set */ |
1265 | ret = -EACCES; |
1266 | goto sendmsg_done; |
1267 | } |
1268 | } else { |
1269 | if (!jsk->addr.dst_name && jsk->addr.da == J1939_NO_ADDR && |
1270 | !sock_flag(sk, flag: SOCK_BROADCAST)) { |
1271 | /* broadcast, but SO_BROADCAST not set */ |
1272 | ret = -EACCES; |
1273 | goto sendmsg_done; |
1274 | } |
1275 | } |
1276 | |
1277 | ret = j1939_sk_send_loop(priv, sk, msg, size); |
1278 | |
1279 | sendmsg_done: |
1280 | release_sock(sk: sock->sk); |
1281 | |
1282 | return ret; |
1283 | } |
1284 | |
1285 | void j1939_sk_netdev_event_netdown(struct j1939_priv *priv) |
1286 | { |
1287 | struct j1939_sock *jsk; |
1288 | int error_code = ENETDOWN; |
1289 | |
1290 | read_lock_bh(&priv->j1939_socks_lock); |
1291 | list_for_each_entry(jsk, &priv->j1939_socks, list) { |
1292 | jsk->sk.sk_err = error_code; |
1293 | if (!sock_flag(sk: &jsk->sk, flag: SOCK_DEAD)) |
1294 | sk_error_report(sk: &jsk->sk); |
1295 | |
1296 | j1939_sk_queue_drop_all(priv, jsk, err: error_code); |
1297 | } |
1298 | read_unlock_bh(&priv->j1939_socks_lock); |
1299 | } |
1300 | |
1301 | static int j1939_sk_no_ioctlcmd(struct socket *sock, unsigned int cmd, |
1302 | unsigned long arg) |
1303 | { |
1304 | /* no ioctls for socket layer -> hand it down to NIC layer */ |
1305 | return -ENOIOCTLCMD; |
1306 | } |
1307 | |
1308 | static const struct proto_ops j1939_ops = { |
1309 | .family = PF_CAN, |
1310 | .release = j1939_sk_release, |
1311 | .bind = j1939_sk_bind, |
1312 | .connect = j1939_sk_connect, |
1313 | .socketpair = sock_no_socketpair, |
1314 | .accept = sock_no_accept, |
1315 | .getname = j1939_sk_getname, |
1316 | .poll = datagram_poll, |
1317 | .ioctl = j1939_sk_no_ioctlcmd, |
1318 | .listen = sock_no_listen, |
1319 | .shutdown = sock_no_shutdown, |
1320 | .setsockopt = j1939_sk_setsockopt, |
1321 | .getsockopt = j1939_sk_getsockopt, |
1322 | .sendmsg = j1939_sk_sendmsg, |
1323 | .recvmsg = j1939_sk_recvmsg, |
1324 | .mmap = sock_no_mmap, |
1325 | }; |
1326 | |
1327 | static struct proto j1939_proto __read_mostly = { |
1328 | .name = "CAN_J1939" , |
1329 | .owner = THIS_MODULE, |
1330 | .obj_size = sizeof(struct j1939_sock), |
1331 | .init = j1939_sk_init, |
1332 | }; |
1333 | |
1334 | const struct can_proto j1939_can_proto = { |
1335 | .type = SOCK_DGRAM, |
1336 | .protocol = CAN_J1939, |
1337 | .ops = &j1939_ops, |
1338 | .prot = &j1939_proto, |
1339 | }; |
1340 | |