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 402 return 0; 403 } 404 405 static int j1939_sk_sanity_check(struct sockaddr_can *addr, int len) 406 { 407 if (!addr) 408 return -EDESTADDRREQ; 409 if (len < J1939_MIN_NAMELEN) 410 return -EINVAL; 411 if (addr->can_family != AF_CAN) 412 return -EINVAL; 413 if (!addr->can_ifindex) 414 return -ENODEV; 415 if (j1939_pgn_is_valid(addr->can_addr.j1939.pgn) && 416 !j1939_pgn_is_clean_pdu(addr->can_addr.j1939.pgn)) 417 return -EINVAL; 418 419 return 0; 420 } 421 422 static int j1939_sk_bind(struct socket *sock, struct sockaddr *uaddr, int len) 423 { 424 struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; 425 struct j1939_sock *jsk = j1939_sk(sock->sk); 426 struct j1939_priv *priv; 427 struct sock *sk; 428 struct net *net; 429 int ret = 0; 430 431 ret = j1939_sk_sanity_check(addr, len); 432 if (ret) 433 return ret; 434 435 lock_sock(sock->sk); 436 437 priv = jsk->priv; 438 sk = sock->sk; 439 net = sock_net(sk); 440 441 /* Already bound to an interface? */ 442 if (jsk->state & J1939_SOCK_BOUND) { 443 /* A re-bind() to a different interface is not 444 * supported. 445 */ 446 if (jsk->ifindex != addr->can_ifindex) { 447 ret = -EINVAL; 448 goto out_release_sock; 449 } 450 451 /* drop old references */ 452 j1939_jsk_del(priv, jsk); 453 j1939_local_ecu_put(priv, jsk->addr.src_name, jsk->addr.sa); 454 } else { 455 struct net_device *ndev; 456 457 ndev = dev_get_by_index(net, addr->can_ifindex); 458 if (!ndev) { 459 ret = -ENODEV; 460 goto out_release_sock; 461 } 462 463 if (ndev->type != ARPHRD_CAN) { 464 dev_put(ndev); 465 ret = -ENODEV; 466 goto out_release_sock; 467 } 468 469 priv = j1939_netdev_start(ndev); 470 dev_put(ndev); 471 if (IS_ERR(priv)) { 472 ret = PTR_ERR(priv); 473 goto out_release_sock; 474 } 475 476 jsk->ifindex = addr->can_ifindex; 477 478 /* the corresponding j1939_priv_put() is called via 479 * sk->sk_destruct, which points to j1939_sk_sock_destruct() 480 */ 481 j1939_priv_get(priv); 482 jsk->priv = priv; 483 } 484 485 /* set default transmit pgn */ 486 if (j1939_pgn_is_valid(addr->can_addr.j1939.pgn)) 487 jsk->pgn_rx_filter = addr->can_addr.j1939.pgn; 488 jsk->addr.src_name = addr->can_addr.j1939.name; 489 jsk->addr.sa = addr->can_addr.j1939.addr; 490 491 /* get new references */ 492 ret = j1939_local_ecu_get(priv, jsk->addr.src_name, jsk->addr.sa); 493 if (ret) { 494 j1939_netdev_stop(priv); 495 goto out_release_sock; 496 } 497 498 j1939_jsk_add(priv, jsk); 499 500 out_release_sock: /* fall through */ 501 release_sock(sock->sk); 502 503 return ret; 504 } 505 506 static int j1939_sk_connect(struct socket *sock, struct sockaddr *uaddr, 507 int len, int flags) 508 { 509 struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; 510 struct j1939_sock *jsk = j1939_sk(sock->sk); 511 int ret = 0; 512 513 ret = j1939_sk_sanity_check(addr, len); 514 if (ret) 515 return ret; 516 517 lock_sock(sock->sk); 518 519 /* bind() before connect() is mandatory */ 520 if (!(jsk->state & J1939_SOCK_BOUND)) { 521 ret = -EINVAL; 522 goto out_release_sock; 523 } 524 525 /* A connect() to a different interface is not supported. */ 526 if (jsk->ifindex != addr->can_ifindex) { 527 ret = -EINVAL; 528 goto out_release_sock; 529 } 530 531 if (!addr->can_addr.j1939.name && 532 addr->can_addr.j1939.addr == J1939_NO_ADDR && 533 !sock_flag(&jsk->sk, SOCK_BROADCAST)) { 534 /* broadcast, but SO_BROADCAST not set */ 535 ret = -EACCES; 536 goto out_release_sock; 537 } 538 539 jsk->addr.dst_name = addr->can_addr.j1939.name; 540 jsk->addr.da = addr->can_addr.j1939.addr; 541 542 if (j1939_pgn_is_valid(addr->can_addr.j1939.pgn)) 543 jsk->addr.pgn = addr->can_addr.j1939.pgn; 544 545 jsk->state |= J1939_SOCK_CONNECTED; 546 547 out_release_sock: /* fall through */ 548 release_sock(sock->sk); 549 550 return ret; 551 } 552 553 static void j1939_sk_sock2sockaddr_can(struct sockaddr_can *addr, 554 const struct j1939_sock *jsk, int peer) 555 { 556 addr->can_family = AF_CAN; 557 addr->can_ifindex = jsk->ifindex; 558 addr->can_addr.j1939.pgn = jsk->addr.pgn; 559 if (peer) { 560 addr->can_addr.j1939.name = jsk->addr.dst_name; 561 addr->can_addr.j1939.addr = jsk->addr.da; 562 } else { 563 addr->can_addr.j1939.name = jsk->addr.src_name; 564 addr->can_addr.j1939.addr = jsk->addr.sa; 565 } 566 } 567 568 static int j1939_sk_getname(struct socket *sock, struct sockaddr *uaddr, 569 int peer) 570 { 571 struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; 572 struct sock *sk = sock->sk; 573 struct j1939_sock *jsk = j1939_sk(sk); 574 int ret = 0; 575 576 lock_sock(sk); 577 578 if (peer && !(jsk->state & J1939_SOCK_CONNECTED)) { 579 ret = -EADDRNOTAVAIL; 580 goto failure; 581 } 582 583 j1939_sk_sock2sockaddr_can(addr, jsk, peer); 584 ret = J1939_MIN_NAMELEN; 585 586 failure: 587 release_sock(sk); 588 589 return ret; 590 } 591 592 static int j1939_sk_release(struct socket *sock) 593 { 594 struct sock *sk = sock->sk; 595 struct j1939_sock *jsk; 596 597 if (!sk) 598 return 0; 599 600 lock_sock(sk); 601 jsk = j1939_sk(sk); 602 603 if (jsk->state & J1939_SOCK_BOUND) { 604 struct j1939_priv *priv = jsk->priv; 605 606 if (wait_event_interruptible(jsk->waitq, 607 !j1939_sock_pending_get(&jsk->sk))) { 608 j1939_cancel_active_session(priv, sk); 609 j1939_sk_queue_drop_all(priv, jsk, ESHUTDOWN); 610 } 611 612 j1939_jsk_del(priv, jsk); 613 614 j1939_local_ecu_put(priv, jsk->addr.src_name, 615 jsk->addr.sa); 616 617 j1939_netdev_stop(priv); 618 } 619 620 kfree(jsk->filters); 621 sock_orphan(sk); 622 sock->sk = NULL; 623 624 release_sock(sk); 625 sock_put(sk); 626 627 return 0; 628 } 629 630 static int j1939_sk_setsockopt_flag(struct j1939_sock *jsk, char __user *optval, 631 unsigned int optlen, int flag) 632 { 633 int tmp; 634 635 if (optlen != sizeof(tmp)) 636 return -EINVAL; 637 if (copy_from_user(&tmp, optval, optlen)) 638 return -EFAULT; 639 lock_sock(&jsk->sk); 640 if (tmp) 641 jsk->state |= flag; 642 else 643 jsk->state &= ~flag; 644 release_sock(&jsk->sk); 645 return tmp; 646 } 647 648 static int j1939_sk_setsockopt(struct socket *sock, int level, int optname, 649 char __user *optval, unsigned int optlen) 650 { 651 struct sock *sk = sock->sk; 652 struct j1939_sock *jsk = j1939_sk(sk); 653 int tmp, count = 0, ret = 0; 654 struct j1939_filter *filters = NULL, *ofilters; 655 656 if (level != SOL_CAN_J1939) 657 return -EINVAL; 658 659 switch (optname) { 660 case SO_J1939_FILTER: 661 if (optval) { 662 struct j1939_filter *f; 663 int c; 664 665 if (optlen % sizeof(*filters) != 0) 666 return -EINVAL; 667 668 if (optlen > J1939_FILTER_MAX * 669 sizeof(struct j1939_filter)) 670 return -EINVAL; 671 672 count = optlen / sizeof(*filters); 673 filters = memdup_user(optval, optlen); 674 if (IS_ERR(filters)) 675 return PTR_ERR(filters); 676 677 for (f = filters, c = count; c; f++, c--) { 678 f->name &= f->name_mask; 679 f->pgn &= f->pgn_mask; 680 f->addr &= f->addr_mask; 681 } 682 } 683 684 lock_sock(&jsk->sk); 685 ofilters = jsk->filters; 686 jsk->filters = filters; 687 jsk->nfilters = count; 688 release_sock(&jsk->sk); 689 kfree(ofilters); 690 return 0; 691 case SO_J1939_PROMISC: 692 return j1939_sk_setsockopt_flag(jsk, optval, optlen, 693 J1939_SOCK_PROMISC); 694 case SO_J1939_ERRQUEUE: 695 ret = j1939_sk_setsockopt_flag(jsk, optval, optlen, 696 J1939_SOCK_ERRQUEUE); 697 if (ret < 0) 698 return ret; 699 700 if (!(jsk->state & J1939_SOCK_ERRQUEUE)) 701 skb_queue_purge(&sk->sk_error_queue); 702 return ret; 703 case SO_J1939_SEND_PRIO: 704 if (optlen != sizeof(tmp)) 705 return -EINVAL; 706 if (copy_from_user(&tmp, optval, optlen)) 707 return -EFAULT; 708 if (tmp < 0 || tmp > 7) 709 return -EDOM; 710 if (tmp < 2 && !capable(CAP_NET_ADMIN)) 711 return -EPERM; 712 lock_sock(&jsk->sk); 713 jsk->sk.sk_priority = j1939_to_sk_priority(tmp); 714 release_sock(&jsk->sk); 715 return 0; 716 default: 717 return -ENOPROTOOPT; 718 } 719 } 720 721 static int j1939_sk_getsockopt(struct socket *sock, int level, int optname, 722 char __user *optval, int __user *optlen) 723 { 724 struct sock *sk = sock->sk; 725 struct j1939_sock *jsk = j1939_sk(sk); 726 int ret, ulen; 727 /* set defaults for using 'int' properties */ 728 int tmp = 0; 729 int len = sizeof(tmp); 730 void *val = &tmp; 731 732 if (level != SOL_CAN_J1939) 733 return -EINVAL; 734 if (get_user(ulen, optlen)) 735 return -EFAULT; 736 if (ulen < 0) 737 return -EINVAL; 738 739 lock_sock(&jsk->sk); 740 switch (optname) { 741 case SO_J1939_PROMISC: 742 tmp = (jsk->state & J1939_SOCK_PROMISC) ? 1 : 0; 743 break; 744 case SO_J1939_ERRQUEUE: 745 tmp = (jsk->state & J1939_SOCK_ERRQUEUE) ? 1 : 0; 746 break; 747 case SO_J1939_SEND_PRIO: 748 tmp = j1939_prio(jsk->sk.sk_priority); 749 break; 750 default: 751 ret = -ENOPROTOOPT; 752 goto no_copy; 753 } 754 755 /* copy to user, based on 'len' & 'val' 756 * but most sockopt's are 'int' properties, and have 'len' & 'val' 757 * left unchanged, but instead modified 'tmp' 758 */ 759 if (len > ulen) 760 ret = -EFAULT; 761 else if (put_user(len, optlen)) 762 ret = -EFAULT; 763 else if (copy_to_user(optval, val, len)) 764 ret = -EFAULT; 765 else 766 ret = 0; 767 no_copy: 768 release_sock(&jsk->sk); 769 return ret; 770 } 771 772 static int j1939_sk_recvmsg(struct socket *sock, struct msghdr *msg, 773 size_t size, int flags) 774 { 775 struct sock *sk = sock->sk; 776 struct sk_buff *skb; 777 struct j1939_sk_buff_cb *skcb; 778 int ret = 0; 779 780 if (flags & ~(MSG_DONTWAIT | MSG_ERRQUEUE)) 781 return -EINVAL; 782 783 if (flags & MSG_ERRQUEUE) 784 return sock_recv_errqueue(sock->sk, msg, size, SOL_CAN_J1939, 785 SCM_J1939_ERRQUEUE); 786 787 skb = skb_recv_datagram(sk, flags, 0, &ret); 788 if (!skb) 789 return ret; 790 791 if (size < skb->len) 792 msg->msg_flags |= MSG_TRUNC; 793 else 794 size = skb->len; 795 796 ret = memcpy_to_msg(msg, skb->data, size); 797 if (ret < 0) { 798 skb_free_datagram(sk, skb); 799 return ret; 800 } 801 802 skcb = j1939_skb_to_cb(skb); 803 if (j1939_address_is_valid(skcb->addr.da)) 804 put_cmsg(msg, SOL_CAN_J1939, SCM_J1939_DEST_ADDR, 805 sizeof(skcb->addr.da), &skcb->addr.da); 806 807 if (skcb->addr.dst_name) 808 put_cmsg(msg, SOL_CAN_J1939, SCM_J1939_DEST_NAME, 809 sizeof(skcb->addr.dst_name), &skcb->addr.dst_name); 810 811 put_cmsg(msg, SOL_CAN_J1939, SCM_J1939_PRIO, 812 sizeof(skcb->priority), &skcb->priority); 813 814 if (msg->msg_name) { 815 struct sockaddr_can *paddr = msg->msg_name; 816 817 msg->msg_namelen = J1939_MIN_NAMELEN; 818 memset(msg->msg_name, 0, msg->msg_namelen); 819 paddr->can_family = AF_CAN; 820 paddr->can_ifindex = skb->skb_iif; 821 paddr->can_addr.j1939.name = skcb->addr.src_name; 822 paddr->can_addr.j1939.addr = skcb->addr.sa; 823 paddr->can_addr.j1939.pgn = skcb->addr.pgn; 824 } 825 826 sock_recv_ts_and_drops(msg, sk, skb); 827 msg->msg_flags |= skcb->msg_flags; 828 skb_free_datagram(sk, skb); 829 830 return size; 831 } 832 833 static struct sk_buff *j1939_sk_alloc_skb(struct net_device *ndev, 834 struct sock *sk, 835 struct msghdr *msg, size_t size, 836 int *errcode) 837 { 838 struct j1939_sock *jsk = j1939_sk(sk); 839 struct j1939_sk_buff_cb *skcb; 840 struct sk_buff *skb; 841 int ret; 842 843 skb = sock_alloc_send_skb(sk, 844 size + 845 sizeof(struct can_frame) - 846 sizeof(((struct can_frame *)NULL)->data) + 847 sizeof(struct can_skb_priv), 848 msg->msg_flags & MSG_DONTWAIT, &ret); 849 if (!skb) 850 goto failure; 851 852 can_skb_reserve(skb); 853 can_skb_prv(skb)->ifindex = ndev->ifindex; 854 can_skb_prv(skb)->skbcnt = 0; 855 skb_reserve(skb, offsetof(struct can_frame, data)); 856 857 ret = memcpy_from_msg(skb_put(skb, size), msg, size); 858 if (ret < 0) 859 goto free_skb; 860 861 skb->dev = ndev; 862 863 skcb = j1939_skb_to_cb(skb); 864 memset(skcb, 0, sizeof(*skcb)); 865 skcb->addr = jsk->addr; 866 skcb->priority = j1939_prio(sk->sk_priority); 867 868 if (msg->msg_name) { 869 struct sockaddr_can *addr = msg->msg_name; 870 871 if (addr->can_addr.j1939.name || 872 addr->can_addr.j1939.addr != J1939_NO_ADDR) { 873 skcb->addr.dst_name = addr->can_addr.j1939.name; 874 skcb->addr.da = addr->can_addr.j1939.addr; 875 } 876 if (j1939_pgn_is_valid(addr->can_addr.j1939.pgn)) 877 skcb->addr.pgn = addr->can_addr.j1939.pgn; 878 } 879 880 *errcode = ret; 881 return skb; 882 883 free_skb: 884 kfree_skb(skb); 885 failure: 886 *errcode = ret; 887 return NULL; 888 } 889 890 static size_t j1939_sk_opt_stats_get_size(void) 891 { 892 return 893 nla_total_size(sizeof(u32)) + /* J1939_NLA_BYTES_ACKED */ 894 0; 895 } 896 897 static struct sk_buff * 898 j1939_sk_get_timestamping_opt_stats(struct j1939_session *session) 899 { 900 struct sk_buff *stats; 901 u32 size; 902 903 stats = alloc_skb(j1939_sk_opt_stats_get_size(), GFP_ATOMIC); 904 if (!stats) 905 return NULL; 906 907 if (session->skcb.addr.type == J1939_SIMPLE) 908 size = session->total_message_size; 909 else 910 size = min(session->pkt.tx_acked * 7, 911 session->total_message_size); 912 913 nla_put_u32(stats, J1939_NLA_BYTES_ACKED, size); 914 915 return stats; 916 } 917 918 void j1939_sk_errqueue(struct j1939_session *session, 919 enum j1939_sk_errqueue_type type) 920 { 921 struct j1939_priv *priv = session->priv; 922 struct sock *sk = session->sk; 923 struct j1939_sock *jsk; 924 struct sock_exterr_skb *serr; 925 struct sk_buff *skb; 926 char *state = "UNK"; 927 int err; 928 929 /* currently we have no sk for the RX session */ 930 if (!sk) 931 return; 932 933 jsk = j1939_sk(sk); 934 935 if (!(jsk->state & J1939_SOCK_ERRQUEUE)) 936 return; 937 938 skb = j1939_sk_get_timestamping_opt_stats(session); 939 if (!skb) 940 return; 941 942 skb->tstamp = ktime_get_real(); 943 944 BUILD_BUG_ON(sizeof(struct sock_exterr_skb) > sizeof(skb->cb)); 945 946 serr = SKB_EXT_ERR(skb); 947 memset(serr, 0, sizeof(*serr)); 948 switch (type) { 949 case J1939_ERRQUEUE_ACK: 950 if (!(sk->sk_tsflags & SOF_TIMESTAMPING_TX_ACK)) { 951 kfree_skb(skb); 952 return; 953 } 954 955 serr->ee.ee_errno = ENOMSG; 956 serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING; 957 serr->ee.ee_info = SCM_TSTAMP_ACK; 958 state = "ACK"; 959 break; 960 case J1939_ERRQUEUE_SCHED: 961 if (!(sk->sk_tsflags & SOF_TIMESTAMPING_TX_SCHED)) { 962 kfree_skb(skb); 963 return; 964 } 965 966 serr->ee.ee_errno = ENOMSG; 967 serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING; 968 serr->ee.ee_info = SCM_TSTAMP_SCHED; 969 state = "SCH"; 970 break; 971 case J1939_ERRQUEUE_ABORT: 972 serr->ee.ee_errno = session->err; 973 serr->ee.ee_origin = SO_EE_ORIGIN_LOCAL; 974 serr->ee.ee_info = J1939_EE_INFO_TX_ABORT; 975 state = "ABT"; 976 break; 977 default: 978 netdev_err(priv->ndev, "Unknown errqueue type %i\n", type); 979 } 980 981 serr->opt_stats = true; 982 if (sk->sk_tsflags & SOF_TIMESTAMPING_OPT_ID) 983 serr->ee.ee_data = session->tskey; 984 985 netdev_dbg(session->priv->ndev, "%s: 0x%p tskey: %i, state: %s\n", 986 __func__, session, session->tskey, state); 987 err = sock_queue_err_skb(sk, skb); 988 989 if (err) 990 kfree_skb(skb); 991 }; 992 993 void j1939_sk_send_loop_abort(struct sock *sk, int err) 994 { 995 sk->sk_err = err; 996 997 sk->sk_error_report(sk); 998 } 999 1000 static int j1939_sk_send_loop(struct j1939_priv *priv, struct sock *sk, 1001 struct msghdr *msg, size_t size) 1002 1003 { 1004 struct j1939_sock *jsk = j1939_sk(sk); 1005 struct j1939_session *session = j1939_sk_get_incomplete_session(jsk); 1006 struct sk_buff *skb; 1007 size_t segment_size, todo_size; 1008 int ret = 0; 1009 1010 if (session && 1011 session->total_message_size != session->total_queued_size + size) { 1012 j1939_session_put(session); 1013 return -EIO; 1014 } 1015 1016 todo_size = size; 1017 1018 while (todo_size) { 1019 struct j1939_sk_buff_cb *skcb; 1020 1021 segment_size = min_t(size_t, J1939_MAX_TP_PACKET_SIZE, 1022 todo_size); 1023 1024 /* Allocate skb for one segment */ 1025 skb = j1939_sk_alloc_skb(priv->ndev, sk, msg, segment_size, 1026 &ret); 1027 if (ret) 1028 break; 1029 1030 skcb = j1939_skb_to_cb(skb); 1031 1032 if (!session) { 1033 /* at this point the size should be full size 1034 * of the session 1035 */ 1036 skcb->offset = 0; 1037 session = j1939_tp_send(priv, skb, size); 1038 if (IS_ERR(session)) { 1039 ret = PTR_ERR(session); 1040 goto kfree_skb; 1041 } 1042 if (j1939_sk_queue_session(session)) { 1043 /* try to activate session if we a 1044 * fist in the queue 1045 */ 1046 if (!j1939_session_activate(session)) { 1047 j1939_tp_schedule_txtimer(session, 0); 1048 } else { 1049 ret = -EBUSY; 1050 session->err = ret; 1051 j1939_sk_queue_drop_all(priv, jsk, 1052 EBUSY); 1053 break; 1054 } 1055 } 1056 } else { 1057 skcb->offset = session->total_queued_size; 1058 j1939_session_skb_queue(session, skb); 1059 } 1060 1061 todo_size -= segment_size; 1062 session->total_queued_size += segment_size; 1063 } 1064 1065 switch (ret) { 1066 case 0: /* OK */ 1067 if (todo_size) 1068 netdev_warn(priv->ndev, 1069 "no error found and not completely queued?! %zu\n", 1070 todo_size); 1071 ret = size; 1072 break; 1073 case -ERESTARTSYS: 1074 ret = -EINTR; 1075 /* fall through */ 1076 case -EAGAIN: /* OK */ 1077 if (todo_size != size) 1078 ret = size - todo_size; 1079 break; 1080 default: /* ERROR */ 1081 break; 1082 } 1083 1084 if (session) 1085 j1939_session_put(session); 1086 1087 return ret; 1088 1089 kfree_skb: 1090 kfree_skb(skb); 1091 return ret; 1092 } 1093 1094 static int j1939_sk_sendmsg(struct socket *sock, struct msghdr *msg, 1095 size_t size) 1096 { 1097 struct sock *sk = sock->sk; 1098 struct j1939_sock *jsk = j1939_sk(sk); 1099 struct j1939_priv *priv; 1100 int ifindex; 1101 int ret; 1102 1103 lock_sock(sock->sk); 1104 /* various socket state tests */ 1105 if (!(jsk->state & J1939_SOCK_BOUND)) { 1106 ret = -EBADFD; 1107 goto sendmsg_done; 1108 } 1109 1110 priv = jsk->priv; 1111 ifindex = jsk->ifindex; 1112 1113 if (!jsk->addr.src_name && jsk->addr.sa == J1939_NO_ADDR) { 1114 /* no source address assigned yet */ 1115 ret = -EBADFD; 1116 goto sendmsg_done; 1117 } 1118 1119 /* deal with provided destination address info */ 1120 if (msg->msg_name) { 1121 struct sockaddr_can *addr = msg->msg_name; 1122 1123 if (msg->msg_namelen < J1939_MIN_NAMELEN) { 1124 ret = -EINVAL; 1125 goto sendmsg_done; 1126 } 1127 1128 if (addr->can_family != AF_CAN) { 1129 ret = -EINVAL; 1130 goto sendmsg_done; 1131 } 1132 1133 if (addr->can_ifindex && addr->can_ifindex != ifindex) { 1134 ret = -EBADFD; 1135 goto sendmsg_done; 1136 } 1137 1138 if (j1939_pgn_is_valid(addr->can_addr.j1939.pgn) && 1139 !j1939_pgn_is_clean_pdu(addr->can_addr.j1939.pgn)) { 1140 ret = -EINVAL; 1141 goto sendmsg_done; 1142 } 1143 1144 if (!addr->can_addr.j1939.name && 1145 addr->can_addr.j1939.addr == J1939_NO_ADDR && 1146 !sock_flag(sk, SOCK_BROADCAST)) { 1147 /* broadcast, but SO_BROADCAST not set */ 1148 ret = -EACCES; 1149 goto sendmsg_done; 1150 } 1151 } else { 1152 if (!jsk->addr.dst_name && jsk->addr.da == J1939_NO_ADDR && 1153 !sock_flag(sk, SOCK_BROADCAST)) { 1154 /* broadcast, but SO_BROADCAST not set */ 1155 ret = -EACCES; 1156 goto sendmsg_done; 1157 } 1158 } 1159 1160 ret = j1939_sk_send_loop(priv, sk, msg, size); 1161 1162 sendmsg_done: 1163 release_sock(sock->sk); 1164 1165 return ret; 1166 } 1167 1168 void j1939_sk_netdev_event_netdown(struct j1939_priv *priv) 1169 { 1170 struct j1939_sock *jsk; 1171 int error_code = ENETDOWN; 1172 1173 spin_lock_bh(&priv->j1939_socks_lock); 1174 list_for_each_entry(jsk, &priv->j1939_socks, list) { 1175 jsk->sk.sk_err = error_code; 1176 if (!sock_flag(&jsk->sk, SOCK_DEAD)) 1177 jsk->sk.sk_error_report(&jsk->sk); 1178 1179 j1939_sk_queue_drop_all(priv, jsk, error_code); 1180 } 1181 spin_unlock_bh(&priv->j1939_socks_lock); 1182 } 1183 1184 static int j1939_sk_no_ioctlcmd(struct socket *sock, unsigned int cmd, 1185 unsigned long arg) 1186 { 1187 /* no ioctls for socket layer -> hand it down to NIC layer */ 1188 return -ENOIOCTLCMD; 1189 } 1190 1191 static const struct proto_ops j1939_ops = { 1192 .family = PF_CAN, 1193 .release = j1939_sk_release, 1194 .bind = j1939_sk_bind, 1195 .connect = j1939_sk_connect, 1196 .socketpair = sock_no_socketpair, 1197 .accept = sock_no_accept, 1198 .getname = j1939_sk_getname, 1199 .poll = datagram_poll, 1200 .ioctl = j1939_sk_no_ioctlcmd, 1201 .listen = sock_no_listen, 1202 .shutdown = sock_no_shutdown, 1203 .setsockopt = j1939_sk_setsockopt, 1204 .getsockopt = j1939_sk_getsockopt, 1205 .sendmsg = j1939_sk_sendmsg, 1206 .recvmsg = j1939_sk_recvmsg, 1207 .mmap = sock_no_mmap, 1208 .sendpage = sock_no_sendpage, 1209 }; 1210 1211 static struct proto j1939_proto __read_mostly = { 1212 .name = "CAN_J1939", 1213 .owner = THIS_MODULE, 1214 .obj_size = sizeof(struct j1939_sock), 1215 .init = j1939_sk_init, 1216 }; 1217 1218 const struct can_proto j1939_can_proto = { 1219 .type = SOCK_DGRAM, 1220 .protocol = CAN_J1939, 1221 .ops = &j1939_ops, 1222 .prot = &j1939_proto, 1223 }; 1224