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