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