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