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