1 // SPDX-License-Identifier: GPL-2.0 2 // Copyright (c) 2010-2011 EIA Electronics, 3 // Kurt Van Dijck <kurt.van.dijck@eia.be> 4 // Copyright (c) 2018 Protonic, 5 // Robin van der Gracht <robin@protonic.nl> 6 // Copyright (c) 2017-2019 Pengutronix, 7 // Marc Kleine-Budde <kernel@pengutronix.de> 8 // Copyright (c) 2017-2019 Pengutronix, 9 // Oleksij Rempel <kernel@pengutronix.de> 10 11 #include <linux/can/skb.h> 12 13 #include "j1939-priv.h" 14 15 #define J1939_XTP_TX_RETRY_LIMIT 100 16 17 #define J1939_ETP_PGN_CTL 0xc800 18 #define J1939_ETP_PGN_DAT 0xc700 19 #define J1939_TP_PGN_CTL 0xec00 20 #define J1939_TP_PGN_DAT 0xeb00 21 22 #define J1939_TP_CMD_RTS 0x10 23 #define J1939_TP_CMD_CTS 0x11 24 #define J1939_TP_CMD_EOMA 0x13 25 #define J1939_TP_CMD_BAM 0x20 26 #define J1939_TP_CMD_ABORT 0xff 27 28 #define J1939_ETP_CMD_RTS 0x14 29 #define J1939_ETP_CMD_CTS 0x15 30 #define J1939_ETP_CMD_DPO 0x16 31 #define J1939_ETP_CMD_EOMA 0x17 32 #define J1939_ETP_CMD_ABORT 0xff 33 34 enum j1939_xtp_abort { 35 J1939_XTP_NO_ABORT = 0, 36 J1939_XTP_ABORT_BUSY = 1, 37 /* Already in one or more connection managed sessions and 38 * cannot support another. 39 * 40 * EALREADY: 41 * Operation already in progress 42 */ 43 44 J1939_XTP_ABORT_RESOURCE = 2, 45 /* System resources were needed for another task so this 46 * connection managed session was terminated. 47 * 48 * EMSGSIZE: 49 * The socket type requires that message be sent atomically, 50 * and the size of the message to be sent made this 51 * impossible. 52 */ 53 54 J1939_XTP_ABORT_TIMEOUT = 3, 55 /* A timeout occurred and this is the connection abort to 56 * close the session. 57 * 58 * EHOSTUNREACH: 59 * The destination host cannot be reached (probably because 60 * the host is down or a remote router cannot reach it). 61 */ 62 63 J1939_XTP_ABORT_GENERIC = 4, 64 /* CTS messages received when data transfer is in progress 65 * 66 * EBADMSG: 67 * Not a data message 68 */ 69 70 J1939_XTP_ABORT_FAULT = 5, 71 /* Maximal retransmit request limit reached 72 * 73 * ENOTRECOVERABLE: 74 * State not recoverable 75 */ 76 77 J1939_XTP_ABORT_UNEXPECTED_DATA = 6, 78 /* Unexpected data transfer packet 79 * 80 * ENOTCONN: 81 * Transport endpoint is not connected 82 */ 83 84 J1939_XTP_ABORT_BAD_SEQ = 7, 85 /* Bad sequence number (and software is not able to recover) 86 * 87 * EILSEQ: 88 * Illegal byte sequence 89 */ 90 91 J1939_XTP_ABORT_DUP_SEQ = 8, 92 /* Duplicate sequence number (and software is not able to 93 * recover) 94 */ 95 96 J1939_XTP_ABORT_EDPO_UNEXPECTED = 9, 97 /* Unexpected EDPO packet (ETP) or Message size > 1785 bytes 98 * (TP) 99 */ 100 101 J1939_XTP_ABORT_BAD_EDPO_PGN = 10, 102 /* Unexpected EDPO PGN (PGN in EDPO is bad) */ 103 104 J1939_XTP_ABORT_EDPO_OUTOF_CTS = 11, 105 /* EDPO number of packets is greater than CTS */ 106 107 J1939_XTP_ABORT_BAD_EDPO_OFFSET = 12, 108 /* Bad EDPO offset */ 109 110 J1939_XTP_ABORT_OTHER_DEPRECATED = 13, 111 /* Deprecated. Use 250 instead (Any other reason) */ 112 113 J1939_XTP_ABORT_ECTS_UNXPECTED_PGN = 14, 114 /* Unexpected ECTS PGN (PGN in ECTS is bad) */ 115 116 J1939_XTP_ABORT_ECTS_TOO_BIG = 15, 117 /* ECTS requested packets exceeds message size */ 118 119 J1939_XTP_ABORT_OTHER = 250, 120 /* Any other reason (if a Connection Abort reason is 121 * identified that is not listed in the table use code 250) 122 */ 123 }; 124 125 static unsigned int j1939_tp_block = 255; 126 static unsigned int j1939_tp_packet_delay; 127 static unsigned int j1939_tp_padding = 1; 128 129 /* helpers */ 130 static const char *j1939_xtp_abort_to_str(enum j1939_xtp_abort abort) 131 { 132 switch (abort) { 133 case J1939_XTP_ABORT_BUSY: 134 return "Already in one or more connection managed sessions and cannot support another."; 135 case J1939_XTP_ABORT_RESOURCE: 136 return "System resources were needed for another task so this connection managed session was terminated."; 137 case J1939_XTP_ABORT_TIMEOUT: 138 return "A timeout occurred and this is the connection abort to close the session."; 139 case J1939_XTP_ABORT_GENERIC: 140 return "CTS messages received when data transfer is in progress"; 141 case J1939_XTP_ABORT_FAULT: 142 return "Maximal retransmit request limit reached"; 143 case J1939_XTP_ABORT_UNEXPECTED_DATA: 144 return "Unexpected data transfer packet"; 145 case J1939_XTP_ABORT_BAD_SEQ: 146 return "Bad sequence number (and software is not able to recover)"; 147 case J1939_XTP_ABORT_DUP_SEQ: 148 return "Duplicate sequence number (and software is not able to recover)"; 149 case J1939_XTP_ABORT_EDPO_UNEXPECTED: 150 return "Unexpected EDPO packet (ETP) or Message size > 1785 bytes (TP)"; 151 case J1939_XTP_ABORT_BAD_EDPO_PGN: 152 return "Unexpected EDPO PGN (PGN in EDPO is bad)"; 153 case J1939_XTP_ABORT_EDPO_OUTOF_CTS: 154 return "EDPO number of packets is greater than CTS"; 155 case J1939_XTP_ABORT_BAD_EDPO_OFFSET: 156 return "Bad EDPO offset"; 157 case J1939_XTP_ABORT_OTHER_DEPRECATED: 158 return "Deprecated. Use 250 instead (Any other reason)"; 159 case J1939_XTP_ABORT_ECTS_UNXPECTED_PGN: 160 return "Unexpected ECTS PGN (PGN in ECTS is bad)"; 161 case J1939_XTP_ABORT_ECTS_TOO_BIG: 162 return "ECTS requested packets exceeds message size"; 163 case J1939_XTP_ABORT_OTHER: 164 return "Any other reason (if a Connection Abort reason is identified that is not listed in the table use code 250)"; 165 default: 166 return "<unknown>"; 167 } 168 } 169 170 static int j1939_xtp_abort_to_errno(struct j1939_priv *priv, 171 enum j1939_xtp_abort abort) 172 { 173 int err; 174 175 switch (abort) { 176 case J1939_XTP_NO_ABORT: 177 WARN_ON_ONCE(abort == J1939_XTP_NO_ABORT); 178 err = 0; 179 break; 180 case J1939_XTP_ABORT_BUSY: 181 err = EALREADY; 182 break; 183 case J1939_XTP_ABORT_RESOURCE: 184 err = EMSGSIZE; 185 break; 186 case J1939_XTP_ABORT_TIMEOUT: 187 err = EHOSTUNREACH; 188 break; 189 case J1939_XTP_ABORT_GENERIC: 190 err = EBADMSG; 191 break; 192 case J1939_XTP_ABORT_FAULT: 193 err = ENOTRECOVERABLE; 194 break; 195 case J1939_XTP_ABORT_UNEXPECTED_DATA: 196 err = ENOTCONN; 197 break; 198 case J1939_XTP_ABORT_BAD_SEQ: 199 err = EILSEQ; 200 break; 201 case J1939_XTP_ABORT_DUP_SEQ: 202 err = EPROTO; 203 break; 204 case J1939_XTP_ABORT_EDPO_UNEXPECTED: 205 err = EPROTO; 206 break; 207 case J1939_XTP_ABORT_BAD_EDPO_PGN: 208 err = EPROTO; 209 break; 210 case J1939_XTP_ABORT_EDPO_OUTOF_CTS: 211 err = EPROTO; 212 break; 213 case J1939_XTP_ABORT_BAD_EDPO_OFFSET: 214 err = EPROTO; 215 break; 216 case J1939_XTP_ABORT_OTHER_DEPRECATED: 217 err = EPROTO; 218 break; 219 case J1939_XTP_ABORT_ECTS_UNXPECTED_PGN: 220 err = EPROTO; 221 break; 222 case J1939_XTP_ABORT_ECTS_TOO_BIG: 223 err = EPROTO; 224 break; 225 case J1939_XTP_ABORT_OTHER: 226 err = EPROTO; 227 break; 228 default: 229 netdev_warn(priv->ndev, "Unknown abort code %i", abort); 230 err = EPROTO; 231 } 232 233 return err; 234 } 235 236 static inline void j1939_session_list_lock(struct j1939_priv *priv) 237 { 238 spin_lock_bh(&priv->active_session_list_lock); 239 } 240 241 static inline void j1939_session_list_unlock(struct j1939_priv *priv) 242 { 243 spin_unlock_bh(&priv->active_session_list_lock); 244 } 245 246 void j1939_session_get(struct j1939_session *session) 247 { 248 kref_get(&session->kref); 249 } 250 251 /* session completion functions */ 252 static void __j1939_session_drop(struct j1939_session *session) 253 { 254 if (!session->transmission) 255 return; 256 257 j1939_sock_pending_del(session->sk); 258 sock_put(session->sk); 259 } 260 261 static void j1939_session_destroy(struct j1939_session *session) 262 { 263 struct sk_buff *skb; 264 265 if (session->transmission) { 266 if (session->err) 267 j1939_sk_errqueue(session, J1939_ERRQUEUE_TX_ABORT); 268 else 269 j1939_sk_errqueue(session, J1939_ERRQUEUE_TX_ACK); 270 } else if (session->err) { 271 j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_ABORT); 272 } 273 274 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session); 275 276 WARN_ON_ONCE(!list_empty(&session->sk_session_queue_entry)); 277 WARN_ON_ONCE(!list_empty(&session->active_session_list_entry)); 278 279 while ((skb = skb_dequeue(&session->skb_queue)) != NULL) { 280 /* drop ref taken in j1939_session_skb_queue() */ 281 skb_unref(skb); 282 kfree_skb(skb); 283 } 284 __j1939_session_drop(session); 285 j1939_priv_put(session->priv); 286 kfree(session); 287 } 288 289 static void __j1939_session_release(struct kref *kref) 290 { 291 struct j1939_session *session = container_of(kref, struct j1939_session, 292 kref); 293 294 j1939_session_destroy(session); 295 } 296 297 void j1939_session_put(struct j1939_session *session) 298 { 299 kref_put(&session->kref, __j1939_session_release); 300 } 301 302 static void j1939_session_txtimer_cancel(struct j1939_session *session) 303 { 304 if (hrtimer_cancel(&session->txtimer)) 305 j1939_session_put(session); 306 } 307 308 static void j1939_session_rxtimer_cancel(struct j1939_session *session) 309 { 310 if (hrtimer_cancel(&session->rxtimer)) 311 j1939_session_put(session); 312 } 313 314 void j1939_session_timers_cancel(struct j1939_session *session) 315 { 316 j1939_session_txtimer_cancel(session); 317 j1939_session_rxtimer_cancel(session); 318 } 319 320 static inline bool j1939_cb_is_broadcast(const struct j1939_sk_buff_cb *skcb) 321 { 322 return (!skcb->addr.dst_name && (skcb->addr.da == 0xff)); 323 } 324 325 static void j1939_session_skb_drop_old(struct j1939_session *session) 326 { 327 struct sk_buff *do_skb; 328 struct j1939_sk_buff_cb *do_skcb; 329 unsigned int offset_start; 330 unsigned long flags; 331 332 if (skb_queue_len(&session->skb_queue) < 2) 333 return; 334 335 offset_start = session->pkt.tx_acked * 7; 336 337 spin_lock_irqsave(&session->skb_queue.lock, flags); 338 do_skb = skb_peek(&session->skb_queue); 339 do_skcb = j1939_skb_to_cb(do_skb); 340 341 if ((do_skcb->offset + do_skb->len) < offset_start) { 342 __skb_unlink(do_skb, &session->skb_queue); 343 /* drop ref taken in j1939_session_skb_queue() */ 344 skb_unref(do_skb); 345 spin_unlock_irqrestore(&session->skb_queue.lock, flags); 346 347 kfree_skb(do_skb); 348 } else { 349 spin_unlock_irqrestore(&session->skb_queue.lock, flags); 350 } 351 } 352 353 void j1939_session_skb_queue(struct j1939_session *session, 354 struct sk_buff *skb) 355 { 356 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 357 struct j1939_priv *priv = session->priv; 358 359 j1939_ac_fixup(priv, skb); 360 361 if (j1939_address_is_unicast(skcb->addr.da) && 362 priv->ents[skcb->addr.da].nusers) 363 skcb->flags |= J1939_ECU_LOCAL_DST; 364 365 skcb->flags |= J1939_ECU_LOCAL_SRC; 366 367 skb_get(skb); 368 skb_queue_tail(&session->skb_queue, skb); 369 } 370 371 static struct 372 sk_buff *j1939_session_skb_get_by_offset(struct j1939_session *session, 373 unsigned int offset_start) 374 { 375 struct j1939_priv *priv = session->priv; 376 struct j1939_sk_buff_cb *do_skcb; 377 struct sk_buff *skb = NULL; 378 struct sk_buff *do_skb; 379 unsigned long flags; 380 381 spin_lock_irqsave(&session->skb_queue.lock, flags); 382 skb_queue_walk(&session->skb_queue, do_skb) { 383 do_skcb = j1939_skb_to_cb(do_skb); 384 385 if (offset_start >= do_skcb->offset && 386 offset_start < (do_skcb->offset + do_skb->len)) { 387 skb = do_skb; 388 } 389 } 390 391 if (skb) 392 skb_get(skb); 393 394 spin_unlock_irqrestore(&session->skb_queue.lock, flags); 395 396 if (!skb) 397 netdev_dbg(priv->ndev, "%s: 0x%p: no skb found for start: %i, queue size: %i\n", 398 __func__, session, offset_start, 399 skb_queue_len(&session->skb_queue)); 400 401 return skb; 402 } 403 404 static struct sk_buff *j1939_session_skb_get(struct j1939_session *session) 405 { 406 unsigned int offset_start; 407 408 offset_start = session->pkt.dpo * 7; 409 return j1939_session_skb_get_by_offset(session, offset_start); 410 } 411 412 /* see if we are receiver 413 * returns 0 for broadcasts, although we will receive them 414 */ 415 static inline int j1939_tp_im_receiver(const struct j1939_sk_buff_cb *skcb) 416 { 417 return skcb->flags & J1939_ECU_LOCAL_DST; 418 } 419 420 /* see if we are sender */ 421 static inline int j1939_tp_im_transmitter(const struct j1939_sk_buff_cb *skcb) 422 { 423 return skcb->flags & J1939_ECU_LOCAL_SRC; 424 } 425 426 /* see if we are involved as either receiver or transmitter */ 427 static int j1939_tp_im_involved(const struct j1939_sk_buff_cb *skcb, bool swap) 428 { 429 if (swap) 430 return j1939_tp_im_receiver(skcb); 431 else 432 return j1939_tp_im_transmitter(skcb); 433 } 434 435 static int j1939_tp_im_involved_anydir(struct j1939_sk_buff_cb *skcb) 436 { 437 return skcb->flags & (J1939_ECU_LOCAL_SRC | J1939_ECU_LOCAL_DST); 438 } 439 440 /* extract pgn from flow-ctl message */ 441 static inline pgn_t j1939_xtp_ctl_to_pgn(const u8 *dat) 442 { 443 pgn_t pgn; 444 445 pgn = (dat[7] << 16) | (dat[6] << 8) | (dat[5] << 0); 446 if (j1939_pgn_is_pdu1(pgn)) 447 pgn &= 0xffff00; 448 return pgn; 449 } 450 451 static inline unsigned int j1939_tp_ctl_to_size(const u8 *dat) 452 { 453 return (dat[2] << 8) + (dat[1] << 0); 454 } 455 456 static inline unsigned int j1939_etp_ctl_to_packet(const u8 *dat) 457 { 458 return (dat[4] << 16) | (dat[3] << 8) | (dat[2] << 0); 459 } 460 461 static inline unsigned int j1939_etp_ctl_to_size(const u8 *dat) 462 { 463 return (dat[4] << 24) | (dat[3] << 16) | 464 (dat[2] << 8) | (dat[1] << 0); 465 } 466 467 /* find existing session: 468 * reverse: swap cb's src & dst 469 * there is no problem with matching broadcasts, since 470 * broadcasts (no dst, no da) would never call this 471 * with reverse == true 472 */ 473 static bool j1939_session_match(struct j1939_addr *se_addr, 474 struct j1939_addr *sk_addr, bool reverse) 475 { 476 if (se_addr->type != sk_addr->type) 477 return false; 478 479 if (reverse) { 480 if (se_addr->src_name) { 481 if (se_addr->src_name != sk_addr->dst_name) 482 return false; 483 } else if (se_addr->sa != sk_addr->da) { 484 return false; 485 } 486 487 if (se_addr->dst_name) { 488 if (se_addr->dst_name != sk_addr->src_name) 489 return false; 490 } else if (se_addr->da != sk_addr->sa) { 491 return false; 492 } 493 } else { 494 if (se_addr->src_name) { 495 if (se_addr->src_name != sk_addr->src_name) 496 return false; 497 } else if (se_addr->sa != sk_addr->sa) { 498 return false; 499 } 500 501 if (se_addr->dst_name) { 502 if (se_addr->dst_name != sk_addr->dst_name) 503 return false; 504 } else if (se_addr->da != sk_addr->da) { 505 return false; 506 } 507 } 508 509 return true; 510 } 511 512 static struct 513 j1939_session *j1939_session_get_by_addr_locked(struct j1939_priv *priv, 514 struct list_head *root, 515 struct j1939_addr *addr, 516 bool reverse, bool transmitter) 517 { 518 struct j1939_session *session; 519 520 lockdep_assert_held(&priv->active_session_list_lock); 521 522 list_for_each_entry(session, root, active_session_list_entry) { 523 j1939_session_get(session); 524 if (j1939_session_match(&session->skcb.addr, addr, reverse) && 525 session->transmission == transmitter) 526 return session; 527 j1939_session_put(session); 528 } 529 530 return NULL; 531 } 532 533 static struct 534 j1939_session *j1939_session_get_simple(struct j1939_priv *priv, 535 struct sk_buff *skb) 536 { 537 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 538 struct j1939_session *session; 539 540 lockdep_assert_held(&priv->active_session_list_lock); 541 542 list_for_each_entry(session, &priv->active_session_list, 543 active_session_list_entry) { 544 j1939_session_get(session); 545 if (session->skcb.addr.type == J1939_SIMPLE && 546 session->tskey == skcb->tskey && session->sk == skb->sk) 547 return session; 548 j1939_session_put(session); 549 } 550 551 return NULL; 552 } 553 554 static struct 555 j1939_session *j1939_session_get_by_addr(struct j1939_priv *priv, 556 struct j1939_addr *addr, 557 bool reverse, bool transmitter) 558 { 559 struct j1939_session *session; 560 561 j1939_session_list_lock(priv); 562 session = j1939_session_get_by_addr_locked(priv, 563 &priv->active_session_list, 564 addr, reverse, transmitter); 565 j1939_session_list_unlock(priv); 566 567 return session; 568 } 569 570 static void j1939_skbcb_swap(struct j1939_sk_buff_cb *skcb) 571 { 572 u8 tmp = 0; 573 574 swap(skcb->addr.dst_name, skcb->addr.src_name); 575 swap(skcb->addr.da, skcb->addr.sa); 576 577 /* swap SRC and DST flags, leave other untouched */ 578 if (skcb->flags & J1939_ECU_LOCAL_SRC) 579 tmp |= J1939_ECU_LOCAL_DST; 580 if (skcb->flags & J1939_ECU_LOCAL_DST) 581 tmp |= J1939_ECU_LOCAL_SRC; 582 skcb->flags &= ~(J1939_ECU_LOCAL_SRC | J1939_ECU_LOCAL_DST); 583 skcb->flags |= tmp; 584 } 585 586 static struct 587 sk_buff *j1939_tp_tx_dat_new(struct j1939_priv *priv, 588 const struct j1939_sk_buff_cb *re_skcb, 589 bool ctl, 590 bool swap_src_dst) 591 { 592 struct sk_buff *skb; 593 struct j1939_sk_buff_cb *skcb; 594 595 skb = alloc_skb(sizeof(struct can_frame) + sizeof(struct can_skb_priv), 596 GFP_ATOMIC); 597 if (unlikely(!skb)) 598 return ERR_PTR(-ENOMEM); 599 600 skb->dev = priv->ndev; 601 can_skb_reserve(skb); 602 can_skb_prv(skb)->ifindex = priv->ndev->ifindex; 603 can_skb_prv(skb)->skbcnt = 0; 604 /* reserve CAN header */ 605 skb_reserve(skb, offsetof(struct can_frame, data)); 606 607 memcpy(skb->cb, re_skcb, sizeof(skb->cb)); 608 skcb = j1939_skb_to_cb(skb); 609 if (swap_src_dst) 610 j1939_skbcb_swap(skcb); 611 612 if (ctl) { 613 if (skcb->addr.type == J1939_ETP) 614 skcb->addr.pgn = J1939_ETP_PGN_CTL; 615 else 616 skcb->addr.pgn = J1939_TP_PGN_CTL; 617 } else { 618 if (skcb->addr.type == J1939_ETP) 619 skcb->addr.pgn = J1939_ETP_PGN_DAT; 620 else 621 skcb->addr.pgn = J1939_TP_PGN_DAT; 622 } 623 624 return skb; 625 } 626 627 /* TP transmit packet functions */ 628 static int j1939_tp_tx_dat(struct j1939_session *session, 629 const u8 *dat, int len) 630 { 631 struct j1939_priv *priv = session->priv; 632 struct sk_buff *skb; 633 634 skb = j1939_tp_tx_dat_new(priv, &session->skcb, 635 false, false); 636 if (IS_ERR(skb)) 637 return PTR_ERR(skb); 638 639 skb_put_data(skb, dat, len); 640 if (j1939_tp_padding && len < 8) 641 memset(skb_put(skb, 8 - len), 0xff, 8 - len); 642 643 return j1939_send_one(priv, skb); 644 } 645 646 static int j1939_xtp_do_tx_ctl(struct j1939_priv *priv, 647 const struct j1939_sk_buff_cb *re_skcb, 648 bool swap_src_dst, pgn_t pgn, const u8 *dat) 649 { 650 struct sk_buff *skb; 651 u8 *skdat; 652 653 if (!j1939_tp_im_involved(re_skcb, swap_src_dst)) 654 return 0; 655 656 skb = j1939_tp_tx_dat_new(priv, re_skcb, true, swap_src_dst); 657 if (IS_ERR(skb)) 658 return PTR_ERR(skb); 659 660 skdat = skb_put(skb, 8); 661 memcpy(skdat, dat, 5); 662 skdat[5] = (pgn >> 0); 663 skdat[6] = (pgn >> 8); 664 skdat[7] = (pgn >> 16); 665 666 return j1939_send_one(priv, skb); 667 } 668 669 static inline int j1939_tp_tx_ctl(struct j1939_session *session, 670 bool swap_src_dst, const u8 *dat) 671 { 672 struct j1939_priv *priv = session->priv; 673 674 return j1939_xtp_do_tx_ctl(priv, &session->skcb, 675 swap_src_dst, 676 session->skcb.addr.pgn, dat); 677 } 678 679 static int j1939_xtp_tx_abort(struct j1939_priv *priv, 680 const struct j1939_sk_buff_cb *re_skcb, 681 bool swap_src_dst, 682 enum j1939_xtp_abort err, 683 pgn_t pgn) 684 { 685 u8 dat[5]; 686 687 if (!j1939_tp_im_involved(re_skcb, swap_src_dst)) 688 return 0; 689 690 memset(dat, 0xff, sizeof(dat)); 691 dat[0] = J1939_TP_CMD_ABORT; 692 dat[1] = err; 693 return j1939_xtp_do_tx_ctl(priv, re_skcb, swap_src_dst, pgn, dat); 694 } 695 696 void j1939_tp_schedule_txtimer(struct j1939_session *session, int msec) 697 { 698 j1939_session_get(session); 699 hrtimer_start(&session->txtimer, ms_to_ktime(msec), 700 HRTIMER_MODE_REL_SOFT); 701 } 702 703 static inline void j1939_tp_set_rxtimeout(struct j1939_session *session, 704 int msec) 705 { 706 j1939_session_rxtimer_cancel(session); 707 j1939_session_get(session); 708 hrtimer_start(&session->rxtimer, ms_to_ktime(msec), 709 HRTIMER_MODE_REL_SOFT); 710 } 711 712 static int j1939_session_tx_rts(struct j1939_session *session) 713 { 714 u8 dat[8]; 715 int ret; 716 717 memset(dat, 0xff, sizeof(dat)); 718 719 dat[1] = (session->total_message_size >> 0); 720 dat[2] = (session->total_message_size >> 8); 721 dat[3] = session->pkt.total; 722 723 if (session->skcb.addr.type == J1939_ETP) { 724 dat[0] = J1939_ETP_CMD_RTS; 725 dat[1] = (session->total_message_size >> 0); 726 dat[2] = (session->total_message_size >> 8); 727 dat[3] = (session->total_message_size >> 16); 728 dat[4] = (session->total_message_size >> 24); 729 } else if (j1939_cb_is_broadcast(&session->skcb)) { 730 dat[0] = J1939_TP_CMD_BAM; 731 /* fake cts for broadcast */ 732 session->pkt.tx = 0; 733 } else { 734 dat[0] = J1939_TP_CMD_RTS; 735 dat[4] = dat[3]; 736 } 737 738 if (dat[0] == session->last_txcmd) 739 /* done already */ 740 return 0; 741 742 ret = j1939_tp_tx_ctl(session, false, dat); 743 if (ret < 0) 744 return ret; 745 746 session->last_txcmd = dat[0]; 747 if (dat[0] == J1939_TP_CMD_BAM) { 748 j1939_tp_schedule_txtimer(session, 50); 749 j1939_tp_set_rxtimeout(session, 250); 750 } else { 751 j1939_tp_set_rxtimeout(session, 1250); 752 } 753 754 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session); 755 756 return 0; 757 } 758 759 static int j1939_session_tx_dpo(struct j1939_session *session) 760 { 761 unsigned int pkt; 762 u8 dat[8]; 763 int ret; 764 765 memset(dat, 0xff, sizeof(dat)); 766 767 dat[0] = J1939_ETP_CMD_DPO; 768 session->pkt.dpo = session->pkt.tx_acked; 769 pkt = session->pkt.dpo; 770 dat[1] = session->pkt.last - session->pkt.tx_acked; 771 dat[2] = (pkt >> 0); 772 dat[3] = (pkt >> 8); 773 dat[4] = (pkt >> 16); 774 775 ret = j1939_tp_tx_ctl(session, false, dat); 776 if (ret < 0) 777 return ret; 778 779 session->last_txcmd = dat[0]; 780 j1939_tp_set_rxtimeout(session, 1250); 781 session->pkt.tx = session->pkt.tx_acked; 782 783 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session); 784 785 return 0; 786 } 787 788 static int j1939_session_tx_dat(struct j1939_session *session) 789 { 790 struct j1939_priv *priv = session->priv; 791 struct j1939_sk_buff_cb *se_skcb; 792 int offset, pkt_done, pkt_end; 793 unsigned int len, pdelay; 794 struct sk_buff *se_skb; 795 const u8 *tpdat; 796 int ret = 0; 797 u8 dat[8]; 798 799 se_skb = j1939_session_skb_get_by_offset(session, session->pkt.tx * 7); 800 if (!se_skb) 801 return -ENOBUFS; 802 803 se_skcb = j1939_skb_to_cb(se_skb); 804 tpdat = se_skb->data; 805 ret = 0; 806 pkt_done = 0; 807 if (session->skcb.addr.type != J1939_ETP && 808 j1939_cb_is_broadcast(&session->skcb)) 809 pkt_end = session->pkt.total; 810 else 811 pkt_end = session->pkt.last; 812 813 while (session->pkt.tx < pkt_end) { 814 dat[0] = session->pkt.tx - session->pkt.dpo + 1; 815 offset = (session->pkt.tx * 7) - se_skcb->offset; 816 len = se_skb->len - offset; 817 if (len > 7) 818 len = 7; 819 820 if (offset + len > se_skb->len) { 821 netdev_err_once(priv->ndev, 822 "%s: 0x%p: requested data outside of queued buffer: offset %i, len %i, pkt.tx: %i\n", 823 __func__, session, se_skcb->offset, 824 se_skb->len , session->pkt.tx); 825 ret = -EOVERFLOW; 826 goto out_free; 827 } 828 829 if (!len) { 830 ret = -ENOBUFS; 831 break; 832 } 833 834 memcpy(&dat[1], &tpdat[offset], len); 835 ret = j1939_tp_tx_dat(session, dat, len + 1); 836 if (ret < 0) { 837 /* ENOBUFS == CAN interface TX queue is full */ 838 if (ret != -ENOBUFS) 839 netdev_alert(priv->ndev, 840 "%s: 0x%p: queue data error: %i\n", 841 __func__, session, ret); 842 break; 843 } 844 845 session->last_txcmd = 0xff; 846 pkt_done++; 847 session->pkt.tx++; 848 pdelay = j1939_cb_is_broadcast(&session->skcb) ? 50 : 849 j1939_tp_packet_delay; 850 851 if (session->pkt.tx < session->pkt.total && pdelay) { 852 j1939_tp_schedule_txtimer(session, pdelay); 853 break; 854 } 855 } 856 857 if (pkt_done) 858 j1939_tp_set_rxtimeout(session, 250); 859 860 out_free: 861 if (ret) 862 kfree_skb(se_skb); 863 else 864 consume_skb(se_skb); 865 866 return ret; 867 } 868 869 static int j1939_xtp_txnext_transmiter(struct j1939_session *session) 870 { 871 struct j1939_priv *priv = session->priv; 872 int ret = 0; 873 874 if (!j1939_tp_im_transmitter(&session->skcb)) { 875 netdev_alert(priv->ndev, "%s: 0x%p: called by not transmitter!\n", 876 __func__, session); 877 return -EINVAL; 878 } 879 880 switch (session->last_cmd) { 881 case 0: 882 ret = j1939_session_tx_rts(session); 883 break; 884 885 case J1939_ETP_CMD_CTS: 886 if (session->last_txcmd != J1939_ETP_CMD_DPO) { 887 ret = j1939_session_tx_dpo(session); 888 if (ret) 889 return ret; 890 } 891 892 fallthrough; 893 case J1939_TP_CMD_CTS: 894 case 0xff: /* did some data */ 895 case J1939_ETP_CMD_DPO: 896 case J1939_TP_CMD_BAM: 897 ret = j1939_session_tx_dat(session); 898 899 break; 900 default: 901 netdev_alert(priv->ndev, "%s: 0x%p: unexpected last_cmd: %x\n", 902 __func__, session, session->last_cmd); 903 } 904 905 return ret; 906 } 907 908 static int j1939_session_tx_cts(struct j1939_session *session) 909 { 910 struct j1939_priv *priv = session->priv; 911 unsigned int pkt, len; 912 int ret; 913 u8 dat[8]; 914 915 if (!j1939_sk_recv_match(priv, &session->skcb)) 916 return -ENOENT; 917 918 len = session->pkt.total - session->pkt.rx; 919 len = min3(len, session->pkt.block, j1939_tp_block ?: 255); 920 memset(dat, 0xff, sizeof(dat)); 921 922 if (session->skcb.addr.type == J1939_ETP) { 923 pkt = session->pkt.rx + 1; 924 dat[0] = J1939_ETP_CMD_CTS; 925 dat[1] = len; 926 dat[2] = (pkt >> 0); 927 dat[3] = (pkt >> 8); 928 dat[4] = (pkt >> 16); 929 } else { 930 dat[0] = J1939_TP_CMD_CTS; 931 dat[1] = len; 932 dat[2] = session->pkt.rx + 1; 933 } 934 935 if (dat[0] == session->last_txcmd) 936 /* done already */ 937 return 0; 938 939 ret = j1939_tp_tx_ctl(session, true, dat); 940 if (ret < 0) 941 return ret; 942 943 if (len) 944 /* only mark cts done when len is set */ 945 session->last_txcmd = dat[0]; 946 j1939_tp_set_rxtimeout(session, 1250); 947 948 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session); 949 950 return 0; 951 } 952 953 static int j1939_session_tx_eoma(struct j1939_session *session) 954 { 955 struct j1939_priv *priv = session->priv; 956 u8 dat[8]; 957 int ret; 958 959 if (!j1939_sk_recv_match(priv, &session->skcb)) 960 return -ENOENT; 961 962 memset(dat, 0xff, sizeof(dat)); 963 964 if (session->skcb.addr.type == J1939_ETP) { 965 dat[0] = J1939_ETP_CMD_EOMA; 966 dat[1] = session->total_message_size >> 0; 967 dat[2] = session->total_message_size >> 8; 968 dat[3] = session->total_message_size >> 16; 969 dat[4] = session->total_message_size >> 24; 970 } else { 971 dat[0] = J1939_TP_CMD_EOMA; 972 dat[1] = session->total_message_size; 973 dat[2] = session->total_message_size >> 8; 974 dat[3] = session->pkt.total; 975 } 976 977 if (dat[0] == session->last_txcmd) 978 /* done already */ 979 return 0; 980 981 ret = j1939_tp_tx_ctl(session, true, dat); 982 if (ret < 0) 983 return ret; 984 985 session->last_txcmd = dat[0]; 986 987 /* wait for the EOMA packet to come in */ 988 j1939_tp_set_rxtimeout(session, 1250); 989 990 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session); 991 992 return 0; 993 } 994 995 static int j1939_xtp_txnext_receiver(struct j1939_session *session) 996 { 997 struct j1939_priv *priv = session->priv; 998 int ret = 0; 999 1000 if (!j1939_tp_im_receiver(&session->skcb)) { 1001 netdev_alert(priv->ndev, "%s: 0x%p: called by not receiver!\n", 1002 __func__, session); 1003 return -EINVAL; 1004 } 1005 1006 switch (session->last_cmd) { 1007 case J1939_TP_CMD_RTS: 1008 case J1939_ETP_CMD_RTS: 1009 ret = j1939_session_tx_cts(session); 1010 break; 1011 1012 case J1939_ETP_CMD_CTS: 1013 case J1939_TP_CMD_CTS: 1014 case 0xff: /* did some data */ 1015 case J1939_ETP_CMD_DPO: 1016 if ((session->skcb.addr.type == J1939_TP && 1017 j1939_cb_is_broadcast(&session->skcb))) 1018 break; 1019 1020 if (session->pkt.rx >= session->pkt.total) { 1021 ret = j1939_session_tx_eoma(session); 1022 } else if (session->pkt.rx >= session->pkt.last) { 1023 session->last_txcmd = 0; 1024 ret = j1939_session_tx_cts(session); 1025 } 1026 break; 1027 default: 1028 netdev_alert(priv->ndev, "%s: 0x%p: unexpected last_cmd: %x\n", 1029 __func__, session, session->last_cmd); 1030 } 1031 1032 return ret; 1033 } 1034 1035 static int j1939_simple_txnext(struct j1939_session *session) 1036 { 1037 struct j1939_priv *priv = session->priv; 1038 struct sk_buff *se_skb = j1939_session_skb_get(session); 1039 struct sk_buff *skb; 1040 int ret; 1041 1042 if (!se_skb) 1043 return 0; 1044 1045 skb = skb_clone(se_skb, GFP_ATOMIC); 1046 if (!skb) { 1047 ret = -ENOMEM; 1048 goto out_free; 1049 } 1050 1051 can_skb_set_owner(skb, se_skb->sk); 1052 1053 j1939_tp_set_rxtimeout(session, J1939_SIMPLE_ECHO_TIMEOUT_MS); 1054 1055 ret = j1939_send_one(priv, skb); 1056 if (ret) 1057 goto out_free; 1058 1059 j1939_sk_errqueue(session, J1939_ERRQUEUE_TX_SCHED); 1060 j1939_sk_queue_activate_next(session); 1061 1062 out_free: 1063 if (ret) 1064 kfree_skb(se_skb); 1065 else 1066 consume_skb(se_skb); 1067 1068 return ret; 1069 } 1070 1071 static bool j1939_session_deactivate_locked(struct j1939_session *session) 1072 { 1073 bool active = false; 1074 1075 lockdep_assert_held(&session->priv->active_session_list_lock); 1076 1077 if (session->state >= J1939_SESSION_ACTIVE && 1078 session->state < J1939_SESSION_ACTIVE_MAX) { 1079 active = true; 1080 1081 list_del_init(&session->active_session_list_entry); 1082 session->state = J1939_SESSION_DONE; 1083 j1939_session_put(session); 1084 } 1085 1086 return active; 1087 } 1088 1089 static bool j1939_session_deactivate(struct j1939_session *session) 1090 { 1091 struct j1939_priv *priv = session->priv; 1092 bool active; 1093 1094 j1939_session_list_lock(priv); 1095 active = j1939_session_deactivate_locked(session); 1096 j1939_session_list_unlock(priv); 1097 1098 return active; 1099 } 1100 1101 static void 1102 j1939_session_deactivate_activate_next(struct j1939_session *session) 1103 { 1104 if (j1939_session_deactivate(session)) 1105 j1939_sk_queue_activate_next(session); 1106 } 1107 1108 static void __j1939_session_cancel(struct j1939_session *session, 1109 enum j1939_xtp_abort err) 1110 { 1111 struct j1939_priv *priv = session->priv; 1112 1113 WARN_ON_ONCE(!err); 1114 lockdep_assert_held(&session->priv->active_session_list_lock); 1115 1116 session->err = j1939_xtp_abort_to_errno(priv, err); 1117 session->state = J1939_SESSION_WAITING_ABORT; 1118 /* do not send aborts on incoming broadcasts */ 1119 if (!j1939_cb_is_broadcast(&session->skcb)) { 1120 j1939_xtp_tx_abort(priv, &session->skcb, 1121 !session->transmission, 1122 err, session->skcb.addr.pgn); 1123 } 1124 1125 if (session->sk) 1126 j1939_sk_send_loop_abort(session->sk, session->err); 1127 else 1128 j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_ABORT); 1129 } 1130 1131 static void j1939_session_cancel(struct j1939_session *session, 1132 enum j1939_xtp_abort err) 1133 { 1134 j1939_session_list_lock(session->priv); 1135 1136 if (session->state >= J1939_SESSION_ACTIVE && 1137 session->state < J1939_SESSION_WAITING_ABORT) { 1138 j1939_tp_set_rxtimeout(session, J1939_XTP_ABORT_TIMEOUT_MS); 1139 __j1939_session_cancel(session, err); 1140 } 1141 1142 j1939_session_list_unlock(session->priv); 1143 } 1144 1145 static enum hrtimer_restart j1939_tp_txtimer(struct hrtimer *hrtimer) 1146 { 1147 struct j1939_session *session = 1148 container_of(hrtimer, struct j1939_session, txtimer); 1149 struct j1939_priv *priv = session->priv; 1150 int ret = 0; 1151 1152 if (session->skcb.addr.type == J1939_SIMPLE) { 1153 ret = j1939_simple_txnext(session); 1154 } else { 1155 if (session->transmission) 1156 ret = j1939_xtp_txnext_transmiter(session); 1157 else 1158 ret = j1939_xtp_txnext_receiver(session); 1159 } 1160 1161 switch (ret) { 1162 case -ENOBUFS: 1163 /* Retry limit is currently arbitrary chosen */ 1164 if (session->tx_retry < J1939_XTP_TX_RETRY_LIMIT) { 1165 session->tx_retry++; 1166 j1939_tp_schedule_txtimer(session, 1167 10 + get_random_u32_below(16)); 1168 } else { 1169 netdev_alert(priv->ndev, "%s: 0x%p: tx retry count reached\n", 1170 __func__, session); 1171 session->err = -ENETUNREACH; 1172 j1939_session_rxtimer_cancel(session); 1173 j1939_session_deactivate_activate_next(session); 1174 } 1175 break; 1176 case -ENETDOWN: 1177 /* In this case we should get a netdev_event(), all active 1178 * sessions will be cleared by 1179 * j1939_cancel_all_active_sessions(). So handle this as an 1180 * error, but let j1939_cancel_all_active_sessions() do the 1181 * cleanup including propagation of the error to user space. 1182 */ 1183 break; 1184 case -EOVERFLOW: 1185 j1939_session_cancel(session, J1939_XTP_ABORT_ECTS_TOO_BIG); 1186 break; 1187 case 0: 1188 session->tx_retry = 0; 1189 break; 1190 default: 1191 netdev_alert(priv->ndev, "%s: 0x%p: tx aborted with unknown reason: %i\n", 1192 __func__, session, ret); 1193 if (session->skcb.addr.type != J1939_SIMPLE) { 1194 j1939_session_cancel(session, J1939_XTP_ABORT_OTHER); 1195 } else { 1196 session->err = ret; 1197 j1939_session_rxtimer_cancel(session); 1198 j1939_session_deactivate_activate_next(session); 1199 } 1200 } 1201 1202 j1939_session_put(session); 1203 1204 return HRTIMER_NORESTART; 1205 } 1206 1207 static void j1939_session_completed(struct j1939_session *session) 1208 { 1209 struct sk_buff *se_skb; 1210 1211 if (!session->transmission) { 1212 se_skb = j1939_session_skb_get(session); 1213 /* distribute among j1939 receivers */ 1214 j1939_sk_recv(session->priv, se_skb); 1215 consume_skb(se_skb); 1216 } 1217 1218 j1939_session_deactivate_activate_next(session); 1219 } 1220 1221 static enum hrtimer_restart j1939_tp_rxtimer(struct hrtimer *hrtimer) 1222 { 1223 struct j1939_session *session = container_of(hrtimer, 1224 struct j1939_session, 1225 rxtimer); 1226 struct j1939_priv *priv = session->priv; 1227 1228 if (session->state == J1939_SESSION_WAITING_ABORT) { 1229 netdev_alert(priv->ndev, "%s: 0x%p: abort rx timeout. Force session deactivation\n", 1230 __func__, session); 1231 1232 j1939_session_deactivate_activate_next(session); 1233 1234 } else if (session->skcb.addr.type == J1939_SIMPLE) { 1235 netdev_alert(priv->ndev, "%s: 0x%p: Timeout. Failed to send simple message.\n", 1236 __func__, session); 1237 1238 /* The message is probably stuck in the CAN controller and can 1239 * be send as soon as CAN bus is in working state again. 1240 */ 1241 session->err = -ETIME; 1242 j1939_session_deactivate(session); 1243 } else { 1244 j1939_session_list_lock(session->priv); 1245 if (session->state >= J1939_SESSION_ACTIVE && 1246 session->state < J1939_SESSION_ACTIVE_MAX) { 1247 netdev_alert(priv->ndev, "%s: 0x%p: rx timeout, send abort\n", 1248 __func__, session); 1249 j1939_session_get(session); 1250 hrtimer_start(&session->rxtimer, 1251 ms_to_ktime(J1939_XTP_ABORT_TIMEOUT_MS), 1252 HRTIMER_MODE_REL_SOFT); 1253 __j1939_session_cancel(session, J1939_XTP_ABORT_TIMEOUT); 1254 } 1255 j1939_session_list_unlock(session->priv); 1256 } 1257 1258 j1939_session_put(session); 1259 1260 return HRTIMER_NORESTART; 1261 } 1262 1263 static bool j1939_xtp_rx_cmd_bad_pgn(struct j1939_session *session, 1264 const struct sk_buff *skb) 1265 { 1266 const struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 1267 pgn_t pgn = j1939_xtp_ctl_to_pgn(skb->data); 1268 struct j1939_priv *priv = session->priv; 1269 enum j1939_xtp_abort abort = J1939_XTP_NO_ABORT; 1270 u8 cmd = skb->data[0]; 1271 1272 if (session->skcb.addr.pgn == pgn) 1273 return false; 1274 1275 switch (cmd) { 1276 case J1939_TP_CMD_BAM: 1277 abort = J1939_XTP_NO_ABORT; 1278 break; 1279 1280 case J1939_ETP_CMD_RTS: 1281 fallthrough; 1282 case J1939_TP_CMD_RTS: 1283 abort = J1939_XTP_ABORT_BUSY; 1284 break; 1285 1286 case J1939_ETP_CMD_CTS: 1287 fallthrough; 1288 case J1939_TP_CMD_CTS: 1289 abort = J1939_XTP_ABORT_ECTS_UNXPECTED_PGN; 1290 break; 1291 1292 case J1939_ETP_CMD_DPO: 1293 abort = J1939_XTP_ABORT_BAD_EDPO_PGN; 1294 break; 1295 1296 case J1939_ETP_CMD_EOMA: 1297 fallthrough; 1298 case J1939_TP_CMD_EOMA: 1299 abort = J1939_XTP_ABORT_OTHER; 1300 break; 1301 1302 case J1939_ETP_CMD_ABORT: /* && J1939_TP_CMD_ABORT */ 1303 abort = J1939_XTP_NO_ABORT; 1304 break; 1305 1306 default: 1307 WARN_ON_ONCE(1); 1308 break; 1309 } 1310 1311 netdev_warn(priv->ndev, "%s: 0x%p: CMD 0x%02x with PGN 0x%05x for running session with different PGN 0x%05x.\n", 1312 __func__, session, cmd, pgn, session->skcb.addr.pgn); 1313 if (abort != J1939_XTP_NO_ABORT) 1314 j1939_xtp_tx_abort(priv, skcb, true, abort, pgn); 1315 1316 return true; 1317 } 1318 1319 static void j1939_xtp_rx_abort_one(struct j1939_priv *priv, struct sk_buff *skb, 1320 bool reverse, bool transmitter) 1321 { 1322 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 1323 struct j1939_session *session; 1324 u8 abort = skb->data[1]; 1325 1326 session = j1939_session_get_by_addr(priv, &skcb->addr, reverse, 1327 transmitter); 1328 if (!session) 1329 return; 1330 1331 if (j1939_xtp_rx_cmd_bad_pgn(session, skb)) 1332 goto abort_put; 1333 1334 netdev_info(priv->ndev, "%s: 0x%p: 0x%05x: (%u) %s\n", __func__, 1335 session, j1939_xtp_ctl_to_pgn(skb->data), abort, 1336 j1939_xtp_abort_to_str(abort)); 1337 1338 j1939_session_timers_cancel(session); 1339 session->err = j1939_xtp_abort_to_errno(priv, abort); 1340 if (session->sk) 1341 j1939_sk_send_loop_abort(session->sk, session->err); 1342 else 1343 j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_ABORT); 1344 j1939_session_deactivate_activate_next(session); 1345 1346 abort_put: 1347 j1939_session_put(session); 1348 } 1349 1350 /* abort packets may come in 2 directions */ 1351 static void 1352 j1939_xtp_rx_abort(struct j1939_priv *priv, struct sk_buff *skb, 1353 bool transmitter) 1354 { 1355 j1939_xtp_rx_abort_one(priv, skb, false, transmitter); 1356 j1939_xtp_rx_abort_one(priv, skb, true, transmitter); 1357 } 1358 1359 static void 1360 j1939_xtp_rx_eoma_one(struct j1939_session *session, struct sk_buff *skb) 1361 { 1362 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 1363 const u8 *dat; 1364 int len; 1365 1366 if (j1939_xtp_rx_cmd_bad_pgn(session, skb)) 1367 return; 1368 1369 dat = skb->data; 1370 1371 if (skcb->addr.type == J1939_ETP) 1372 len = j1939_etp_ctl_to_size(dat); 1373 else 1374 len = j1939_tp_ctl_to_size(dat); 1375 1376 if (session->total_message_size != len) { 1377 netdev_warn_once(session->priv->ndev, 1378 "%s: 0x%p: Incorrect size. Expected: %i; got: %i.\n", 1379 __func__, session, session->total_message_size, 1380 len); 1381 } 1382 1383 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session); 1384 1385 session->pkt.tx_acked = session->pkt.total; 1386 j1939_session_timers_cancel(session); 1387 /* transmitted without problems */ 1388 j1939_session_completed(session); 1389 } 1390 1391 static void 1392 j1939_xtp_rx_eoma(struct j1939_priv *priv, struct sk_buff *skb, 1393 bool transmitter) 1394 { 1395 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 1396 struct j1939_session *session; 1397 1398 session = j1939_session_get_by_addr(priv, &skcb->addr, true, 1399 transmitter); 1400 if (!session) 1401 return; 1402 1403 j1939_xtp_rx_eoma_one(session, skb); 1404 j1939_session_put(session); 1405 } 1406 1407 static void 1408 j1939_xtp_rx_cts_one(struct j1939_session *session, struct sk_buff *skb) 1409 { 1410 enum j1939_xtp_abort err = J1939_XTP_ABORT_FAULT; 1411 unsigned int pkt; 1412 const u8 *dat; 1413 1414 dat = skb->data; 1415 1416 if (j1939_xtp_rx_cmd_bad_pgn(session, skb)) 1417 return; 1418 1419 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session); 1420 1421 if (session->last_cmd == dat[0]) { 1422 err = J1939_XTP_ABORT_DUP_SEQ; 1423 goto out_session_cancel; 1424 } 1425 1426 if (session->skcb.addr.type == J1939_ETP) 1427 pkt = j1939_etp_ctl_to_packet(dat); 1428 else 1429 pkt = dat[2]; 1430 1431 if (!pkt) 1432 goto out_session_cancel; 1433 else if (dat[1] > session->pkt.block /* 0xff for etp */) 1434 goto out_session_cancel; 1435 1436 /* set packet counters only when not CTS(0) */ 1437 session->pkt.tx_acked = pkt - 1; 1438 j1939_session_skb_drop_old(session); 1439 session->pkt.last = session->pkt.tx_acked + dat[1]; 1440 if (session->pkt.last > session->pkt.total) 1441 /* safety measure */ 1442 session->pkt.last = session->pkt.total; 1443 /* TODO: do not set tx here, do it in txtimer */ 1444 session->pkt.tx = session->pkt.tx_acked; 1445 1446 session->last_cmd = dat[0]; 1447 if (dat[1]) { 1448 j1939_tp_set_rxtimeout(session, 1250); 1449 if (session->transmission) { 1450 if (session->pkt.tx_acked) 1451 j1939_sk_errqueue(session, 1452 J1939_ERRQUEUE_TX_SCHED); 1453 j1939_session_txtimer_cancel(session); 1454 j1939_tp_schedule_txtimer(session, 0); 1455 } 1456 } else { 1457 /* CTS(0) */ 1458 j1939_tp_set_rxtimeout(session, 550); 1459 } 1460 return; 1461 1462 out_session_cancel: 1463 j1939_session_timers_cancel(session); 1464 j1939_session_cancel(session, err); 1465 } 1466 1467 static void 1468 j1939_xtp_rx_cts(struct j1939_priv *priv, struct sk_buff *skb, bool transmitter) 1469 { 1470 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 1471 struct j1939_session *session; 1472 1473 session = j1939_session_get_by_addr(priv, &skcb->addr, true, 1474 transmitter); 1475 if (!session) 1476 return; 1477 j1939_xtp_rx_cts_one(session, skb); 1478 j1939_session_put(session); 1479 } 1480 1481 static struct j1939_session *j1939_session_new(struct j1939_priv *priv, 1482 struct sk_buff *skb, size_t size) 1483 { 1484 struct j1939_session *session; 1485 struct j1939_sk_buff_cb *skcb; 1486 1487 session = kzalloc(sizeof(*session), gfp_any()); 1488 if (!session) 1489 return NULL; 1490 1491 INIT_LIST_HEAD(&session->active_session_list_entry); 1492 INIT_LIST_HEAD(&session->sk_session_queue_entry); 1493 kref_init(&session->kref); 1494 1495 j1939_priv_get(priv); 1496 session->priv = priv; 1497 session->total_message_size = size; 1498 session->state = J1939_SESSION_NEW; 1499 1500 skb_queue_head_init(&session->skb_queue); 1501 skb_queue_tail(&session->skb_queue, skb); 1502 1503 skcb = j1939_skb_to_cb(skb); 1504 memcpy(&session->skcb, skcb, sizeof(session->skcb)); 1505 1506 hrtimer_init(&session->txtimer, CLOCK_MONOTONIC, 1507 HRTIMER_MODE_REL_SOFT); 1508 session->txtimer.function = j1939_tp_txtimer; 1509 hrtimer_init(&session->rxtimer, CLOCK_MONOTONIC, 1510 HRTIMER_MODE_REL_SOFT); 1511 session->rxtimer.function = j1939_tp_rxtimer; 1512 1513 netdev_dbg(priv->ndev, "%s: 0x%p: sa: %02x, da: %02x\n", 1514 __func__, session, skcb->addr.sa, skcb->addr.da); 1515 1516 return session; 1517 } 1518 1519 static struct 1520 j1939_session *j1939_session_fresh_new(struct j1939_priv *priv, 1521 int size, 1522 const struct j1939_sk_buff_cb *rel_skcb) 1523 { 1524 struct sk_buff *skb; 1525 struct j1939_sk_buff_cb *skcb; 1526 struct j1939_session *session; 1527 1528 skb = alloc_skb(size + sizeof(struct can_skb_priv), GFP_ATOMIC); 1529 if (unlikely(!skb)) 1530 return NULL; 1531 1532 skb->dev = priv->ndev; 1533 can_skb_reserve(skb); 1534 can_skb_prv(skb)->ifindex = priv->ndev->ifindex; 1535 can_skb_prv(skb)->skbcnt = 0; 1536 skcb = j1939_skb_to_cb(skb); 1537 memcpy(skcb, rel_skcb, sizeof(*skcb)); 1538 1539 session = j1939_session_new(priv, skb, size); 1540 if (!session) { 1541 kfree_skb(skb); 1542 return NULL; 1543 } 1544 1545 /* alloc data area */ 1546 skb_put(skb, size); 1547 /* skb is recounted in j1939_session_new() */ 1548 return session; 1549 } 1550 1551 int j1939_session_activate(struct j1939_session *session) 1552 { 1553 struct j1939_priv *priv = session->priv; 1554 struct j1939_session *active = NULL; 1555 int ret = 0; 1556 1557 j1939_session_list_lock(priv); 1558 if (session->skcb.addr.type != J1939_SIMPLE) 1559 active = j1939_session_get_by_addr_locked(priv, 1560 &priv->active_session_list, 1561 &session->skcb.addr, false, 1562 session->transmission); 1563 if (active) { 1564 j1939_session_put(active); 1565 ret = -EAGAIN; 1566 } else { 1567 WARN_ON_ONCE(session->state != J1939_SESSION_NEW); 1568 list_add_tail(&session->active_session_list_entry, 1569 &priv->active_session_list); 1570 j1939_session_get(session); 1571 session->state = J1939_SESSION_ACTIVE; 1572 1573 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", 1574 __func__, session); 1575 } 1576 j1939_session_list_unlock(priv); 1577 1578 return ret; 1579 } 1580 1581 static struct 1582 j1939_session *j1939_xtp_rx_rts_session_new(struct j1939_priv *priv, 1583 struct sk_buff *skb) 1584 { 1585 enum j1939_xtp_abort abort = J1939_XTP_NO_ABORT; 1586 struct j1939_sk_buff_cb skcb = *j1939_skb_to_cb(skb); 1587 struct j1939_session *session; 1588 const u8 *dat; 1589 pgn_t pgn; 1590 int len; 1591 1592 netdev_dbg(priv->ndev, "%s\n", __func__); 1593 1594 dat = skb->data; 1595 pgn = j1939_xtp_ctl_to_pgn(dat); 1596 skcb.addr.pgn = pgn; 1597 1598 if (!j1939_sk_recv_match(priv, &skcb)) 1599 return NULL; 1600 1601 if (skcb.addr.type == J1939_ETP) { 1602 len = j1939_etp_ctl_to_size(dat); 1603 if (len > J1939_MAX_ETP_PACKET_SIZE) 1604 abort = J1939_XTP_ABORT_FAULT; 1605 else if (len > priv->tp_max_packet_size) 1606 abort = J1939_XTP_ABORT_RESOURCE; 1607 else if (len <= J1939_MAX_TP_PACKET_SIZE) 1608 abort = J1939_XTP_ABORT_FAULT; 1609 } else { 1610 len = j1939_tp_ctl_to_size(dat); 1611 if (len > J1939_MAX_TP_PACKET_SIZE) 1612 abort = J1939_XTP_ABORT_FAULT; 1613 else if (len > priv->tp_max_packet_size) 1614 abort = J1939_XTP_ABORT_RESOURCE; 1615 else if (len < J1939_MIN_TP_PACKET_SIZE) 1616 abort = J1939_XTP_ABORT_FAULT; 1617 } 1618 1619 if (abort != J1939_XTP_NO_ABORT) { 1620 j1939_xtp_tx_abort(priv, &skcb, true, abort, pgn); 1621 return NULL; 1622 } 1623 1624 session = j1939_session_fresh_new(priv, len, &skcb); 1625 if (!session) { 1626 j1939_xtp_tx_abort(priv, &skcb, true, 1627 J1939_XTP_ABORT_RESOURCE, pgn); 1628 return NULL; 1629 } 1630 1631 /* initialize the control buffer: plain copy */ 1632 session->pkt.total = (len + 6) / 7; 1633 session->pkt.block = 0xff; 1634 if (skcb.addr.type != J1939_ETP) { 1635 if (dat[3] != session->pkt.total) 1636 netdev_alert(priv->ndev, "%s: 0x%p: strange total, %u != %u\n", 1637 __func__, session, session->pkt.total, 1638 dat[3]); 1639 session->pkt.total = dat[3]; 1640 session->pkt.block = min(dat[3], dat[4]); 1641 } 1642 1643 session->pkt.rx = 0; 1644 session->pkt.tx = 0; 1645 1646 session->tskey = priv->rx_tskey++; 1647 j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_RTS); 1648 1649 WARN_ON_ONCE(j1939_session_activate(session)); 1650 1651 return session; 1652 } 1653 1654 static int j1939_xtp_rx_rts_session_active(struct j1939_session *session, 1655 struct sk_buff *skb) 1656 { 1657 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 1658 struct j1939_priv *priv = session->priv; 1659 1660 if (!session->transmission) { 1661 if (j1939_xtp_rx_cmd_bad_pgn(session, skb)) 1662 return -EBUSY; 1663 1664 /* RTS on active session */ 1665 j1939_session_timers_cancel(session); 1666 j1939_session_cancel(session, J1939_XTP_ABORT_BUSY); 1667 } 1668 1669 if (session->last_cmd != 0) { 1670 /* we received a second rts on the same connection */ 1671 netdev_alert(priv->ndev, "%s: 0x%p: connection exists (%02x %02x). last cmd: %x\n", 1672 __func__, session, skcb->addr.sa, skcb->addr.da, 1673 session->last_cmd); 1674 1675 j1939_session_timers_cancel(session); 1676 j1939_session_cancel(session, J1939_XTP_ABORT_BUSY); 1677 1678 return -EBUSY; 1679 } 1680 1681 if (session->skcb.addr.sa != skcb->addr.sa || 1682 session->skcb.addr.da != skcb->addr.da) 1683 netdev_warn(priv->ndev, "%s: 0x%p: session->skcb.addr.sa=0x%02x skcb->addr.sa=0x%02x session->skcb.addr.da=0x%02x skcb->addr.da=0x%02x\n", 1684 __func__, session, 1685 session->skcb.addr.sa, skcb->addr.sa, 1686 session->skcb.addr.da, skcb->addr.da); 1687 /* make sure 'sa' & 'da' are correct ! 1688 * They may be 'not filled in yet' for sending 1689 * skb's, since they did not pass the Address Claim ever. 1690 */ 1691 session->skcb.addr.sa = skcb->addr.sa; 1692 session->skcb.addr.da = skcb->addr.da; 1693 1694 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session); 1695 1696 return 0; 1697 } 1698 1699 static void j1939_xtp_rx_rts(struct j1939_priv *priv, struct sk_buff *skb, 1700 bool transmitter) 1701 { 1702 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 1703 struct j1939_session *session; 1704 u8 cmd = skb->data[0]; 1705 1706 session = j1939_session_get_by_addr(priv, &skcb->addr, false, 1707 transmitter); 1708 1709 if (!session) { 1710 if (transmitter) { 1711 /* If we're the transmitter and this function is called, 1712 * we received our own RTS. A session has already been 1713 * created. 1714 * 1715 * For some reasons however it might have been destroyed 1716 * already. So don't create a new one here (using 1717 * "j1939_xtp_rx_rts_session_new()") as this will be a 1718 * receiver session. 1719 * 1720 * The reasons the session is already destroyed might 1721 * be: 1722 * - user space closed socket was and the session was 1723 * aborted 1724 * - session was aborted due to external abort message 1725 */ 1726 return; 1727 } 1728 session = j1939_xtp_rx_rts_session_new(priv, skb); 1729 if (!session) { 1730 if (cmd == J1939_TP_CMD_BAM && j1939_sk_recv_match(priv, skcb)) 1731 netdev_info(priv->ndev, "%s: failed to create TP BAM session\n", 1732 __func__); 1733 return; 1734 } 1735 } else { 1736 if (j1939_xtp_rx_rts_session_active(session, skb)) { 1737 j1939_session_put(session); 1738 return; 1739 } 1740 } 1741 session->last_cmd = cmd; 1742 1743 if (cmd == J1939_TP_CMD_BAM) { 1744 if (!session->transmission) 1745 j1939_tp_set_rxtimeout(session, 750); 1746 } else { 1747 if (!session->transmission) { 1748 j1939_session_txtimer_cancel(session); 1749 j1939_tp_schedule_txtimer(session, 0); 1750 } 1751 j1939_tp_set_rxtimeout(session, 1250); 1752 } 1753 1754 j1939_session_put(session); 1755 } 1756 1757 static void j1939_xtp_rx_dpo_one(struct j1939_session *session, 1758 struct sk_buff *skb) 1759 { 1760 const u8 *dat = skb->data; 1761 1762 if (j1939_xtp_rx_cmd_bad_pgn(session, skb)) 1763 return; 1764 1765 netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session); 1766 1767 /* transmitted without problems */ 1768 session->pkt.dpo = j1939_etp_ctl_to_packet(skb->data); 1769 session->last_cmd = dat[0]; 1770 j1939_tp_set_rxtimeout(session, 750); 1771 1772 if (!session->transmission) 1773 j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_DPO); 1774 } 1775 1776 static void j1939_xtp_rx_dpo(struct j1939_priv *priv, struct sk_buff *skb, 1777 bool transmitter) 1778 { 1779 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 1780 struct j1939_session *session; 1781 1782 session = j1939_session_get_by_addr(priv, &skcb->addr, false, 1783 transmitter); 1784 if (!session) { 1785 netdev_info(priv->ndev, 1786 "%s: no connection found\n", __func__); 1787 return; 1788 } 1789 1790 j1939_xtp_rx_dpo_one(session, skb); 1791 j1939_session_put(session); 1792 } 1793 1794 static void j1939_xtp_rx_dat_one(struct j1939_session *session, 1795 struct sk_buff *skb) 1796 { 1797 enum j1939_xtp_abort abort = J1939_XTP_ABORT_FAULT; 1798 struct j1939_priv *priv = session->priv; 1799 struct j1939_sk_buff_cb *skcb, *se_skcb; 1800 struct sk_buff *se_skb = NULL; 1801 const u8 *dat; 1802 u8 *tpdat; 1803 int offset; 1804 int nbytes; 1805 bool final = false; 1806 bool remain = false; 1807 bool do_cts_eoma = false; 1808 int packet; 1809 1810 skcb = j1939_skb_to_cb(skb); 1811 dat = skb->data; 1812 if (skb->len != 8) { 1813 /* makes no sense */ 1814 abort = J1939_XTP_ABORT_UNEXPECTED_DATA; 1815 goto out_session_cancel; 1816 } 1817 1818 switch (session->last_cmd) { 1819 case 0xff: 1820 break; 1821 case J1939_ETP_CMD_DPO: 1822 if (skcb->addr.type == J1939_ETP) 1823 break; 1824 fallthrough; 1825 case J1939_TP_CMD_BAM: 1826 fallthrough; 1827 case J1939_TP_CMD_CTS: 1828 if (skcb->addr.type != J1939_ETP) 1829 break; 1830 fallthrough; 1831 default: 1832 netdev_info(priv->ndev, "%s: 0x%p: last %02x\n", __func__, 1833 session, session->last_cmd); 1834 goto out_session_cancel; 1835 } 1836 1837 packet = (dat[0] - 1 + session->pkt.dpo); 1838 if (packet > session->pkt.total || 1839 (session->pkt.rx + 1) > session->pkt.total) { 1840 netdev_info(priv->ndev, "%s: 0x%p: should have been completed\n", 1841 __func__, session); 1842 goto out_session_cancel; 1843 } 1844 1845 se_skb = j1939_session_skb_get_by_offset(session, packet * 7); 1846 if (!se_skb) { 1847 netdev_warn(priv->ndev, "%s: 0x%p: no skb found\n", __func__, 1848 session); 1849 goto out_session_cancel; 1850 } 1851 1852 se_skcb = j1939_skb_to_cb(se_skb); 1853 offset = packet * 7 - se_skcb->offset; 1854 nbytes = se_skb->len - offset; 1855 if (nbytes > 7) 1856 nbytes = 7; 1857 if (nbytes <= 0 || (nbytes + 1) > skb->len) { 1858 netdev_info(priv->ndev, "%s: 0x%p: nbytes %i, len %i\n", 1859 __func__, session, nbytes, skb->len); 1860 goto out_session_cancel; 1861 } 1862 1863 tpdat = se_skb->data; 1864 if (!session->transmission) { 1865 memcpy(&tpdat[offset], &dat[1], nbytes); 1866 } else { 1867 int err; 1868 1869 err = memcmp(&tpdat[offset], &dat[1], nbytes); 1870 if (err) 1871 netdev_err_once(priv->ndev, 1872 "%s: 0x%p: Data of RX-looped back packet (%*ph) doesn't match TX data (%*ph)!\n", 1873 __func__, session, 1874 nbytes, &dat[1], 1875 nbytes, &tpdat[offset]); 1876 } 1877 1878 if (packet == session->pkt.rx) 1879 session->pkt.rx++; 1880 1881 if (se_skcb->addr.type != J1939_ETP && 1882 j1939_cb_is_broadcast(&session->skcb)) { 1883 if (session->pkt.rx >= session->pkt.total) 1884 final = true; 1885 else 1886 remain = true; 1887 } else { 1888 /* never final, an EOMA must follow */ 1889 if (session->pkt.rx >= session->pkt.last) 1890 do_cts_eoma = true; 1891 } 1892 1893 if (final) { 1894 j1939_session_timers_cancel(session); 1895 j1939_session_completed(session); 1896 } else if (remain) { 1897 if (!session->transmission) 1898 j1939_tp_set_rxtimeout(session, 750); 1899 } else if (do_cts_eoma) { 1900 j1939_tp_set_rxtimeout(session, 1250); 1901 if (!session->transmission) 1902 j1939_tp_schedule_txtimer(session, 0); 1903 } else { 1904 j1939_tp_set_rxtimeout(session, 750); 1905 } 1906 session->last_cmd = 0xff; 1907 consume_skb(se_skb); 1908 j1939_session_put(session); 1909 1910 return; 1911 1912 out_session_cancel: 1913 kfree_skb(se_skb); 1914 j1939_session_timers_cancel(session); 1915 j1939_session_cancel(session, abort); 1916 j1939_session_put(session); 1917 } 1918 1919 static void j1939_xtp_rx_dat(struct j1939_priv *priv, struct sk_buff *skb) 1920 { 1921 struct j1939_sk_buff_cb *skcb; 1922 struct j1939_session *session; 1923 1924 skcb = j1939_skb_to_cb(skb); 1925 1926 if (j1939_tp_im_transmitter(skcb)) { 1927 session = j1939_session_get_by_addr(priv, &skcb->addr, false, 1928 true); 1929 if (!session) 1930 netdev_info(priv->ndev, "%s: no tx connection found\n", 1931 __func__); 1932 else 1933 j1939_xtp_rx_dat_one(session, skb); 1934 } 1935 1936 if (j1939_tp_im_receiver(skcb)) { 1937 session = j1939_session_get_by_addr(priv, &skcb->addr, false, 1938 false); 1939 if (!session) 1940 netdev_info(priv->ndev, "%s: no rx connection found\n", 1941 __func__); 1942 else 1943 j1939_xtp_rx_dat_one(session, skb); 1944 } 1945 1946 if (j1939_cb_is_broadcast(skcb)) { 1947 session = j1939_session_get_by_addr(priv, &skcb->addr, false, 1948 false); 1949 if (session) 1950 j1939_xtp_rx_dat_one(session, skb); 1951 } 1952 } 1953 1954 /* j1939 main intf */ 1955 struct j1939_session *j1939_tp_send(struct j1939_priv *priv, 1956 struct sk_buff *skb, size_t size) 1957 { 1958 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 1959 struct j1939_session *session; 1960 int ret; 1961 1962 if (skcb->addr.pgn == J1939_TP_PGN_DAT || 1963 skcb->addr.pgn == J1939_TP_PGN_CTL || 1964 skcb->addr.pgn == J1939_ETP_PGN_DAT || 1965 skcb->addr.pgn == J1939_ETP_PGN_CTL) 1966 /* avoid conflict */ 1967 return ERR_PTR(-EDOM); 1968 1969 if (size > priv->tp_max_packet_size) 1970 return ERR_PTR(-EMSGSIZE); 1971 1972 if (size <= 8) 1973 skcb->addr.type = J1939_SIMPLE; 1974 else if (size > J1939_MAX_TP_PACKET_SIZE) 1975 skcb->addr.type = J1939_ETP; 1976 else 1977 skcb->addr.type = J1939_TP; 1978 1979 if (skcb->addr.type == J1939_ETP && 1980 j1939_cb_is_broadcast(skcb)) 1981 return ERR_PTR(-EDESTADDRREQ); 1982 1983 /* fill in addresses from names */ 1984 ret = j1939_ac_fixup(priv, skb); 1985 if (unlikely(ret)) 1986 return ERR_PTR(ret); 1987 1988 /* fix DST flags, it may be used there soon */ 1989 if (j1939_address_is_unicast(skcb->addr.da) && 1990 priv->ents[skcb->addr.da].nusers) 1991 skcb->flags |= J1939_ECU_LOCAL_DST; 1992 1993 /* src is always local, I'm sending ... */ 1994 skcb->flags |= J1939_ECU_LOCAL_SRC; 1995 1996 /* prepare new session */ 1997 session = j1939_session_new(priv, skb, size); 1998 if (!session) 1999 return ERR_PTR(-ENOMEM); 2000 2001 /* skb is recounted in j1939_session_new() */ 2002 sock_hold(skb->sk); 2003 session->sk = skb->sk; 2004 session->transmission = true; 2005 session->pkt.total = (size + 6) / 7; 2006 session->pkt.block = skcb->addr.type == J1939_ETP ? 255 : 2007 min(j1939_tp_block ?: 255, session->pkt.total); 2008 2009 if (j1939_cb_is_broadcast(&session->skcb)) 2010 /* set the end-packet for broadcast */ 2011 session->pkt.last = session->pkt.total; 2012 2013 skcb->tskey = atomic_inc_return(&session->sk->sk_tskey) - 1; 2014 session->tskey = skcb->tskey; 2015 2016 return session; 2017 } 2018 2019 static void j1939_tp_cmd_recv(struct j1939_priv *priv, struct sk_buff *skb) 2020 { 2021 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 2022 int extd = J1939_TP; 2023 u8 cmd = skb->data[0]; 2024 2025 switch (cmd) { 2026 case J1939_ETP_CMD_RTS: 2027 extd = J1939_ETP; 2028 fallthrough; 2029 case J1939_TP_CMD_BAM: 2030 if (cmd == J1939_TP_CMD_BAM && !j1939_cb_is_broadcast(skcb)) { 2031 netdev_err_once(priv->ndev, "%s: BAM to unicast (%02x), ignoring!\n", 2032 __func__, skcb->addr.sa); 2033 return; 2034 } 2035 fallthrough; 2036 case J1939_TP_CMD_RTS: 2037 if (skcb->addr.type != extd) 2038 return; 2039 2040 if (cmd == J1939_TP_CMD_RTS && j1939_cb_is_broadcast(skcb)) { 2041 netdev_alert(priv->ndev, "%s: rts without destination (%02x)\n", 2042 __func__, skcb->addr.sa); 2043 return; 2044 } 2045 2046 if (j1939_tp_im_transmitter(skcb)) 2047 j1939_xtp_rx_rts(priv, skb, true); 2048 2049 if (j1939_tp_im_receiver(skcb) || j1939_cb_is_broadcast(skcb)) 2050 j1939_xtp_rx_rts(priv, skb, false); 2051 2052 break; 2053 2054 case J1939_ETP_CMD_CTS: 2055 extd = J1939_ETP; 2056 fallthrough; 2057 case J1939_TP_CMD_CTS: 2058 if (skcb->addr.type != extd) 2059 return; 2060 2061 if (j1939_tp_im_transmitter(skcb)) 2062 j1939_xtp_rx_cts(priv, skb, false); 2063 2064 if (j1939_tp_im_receiver(skcb)) 2065 j1939_xtp_rx_cts(priv, skb, true); 2066 2067 break; 2068 2069 case J1939_ETP_CMD_DPO: 2070 if (skcb->addr.type != J1939_ETP) 2071 return; 2072 2073 if (j1939_tp_im_transmitter(skcb)) 2074 j1939_xtp_rx_dpo(priv, skb, true); 2075 2076 if (j1939_tp_im_receiver(skcb)) 2077 j1939_xtp_rx_dpo(priv, skb, false); 2078 2079 break; 2080 2081 case J1939_ETP_CMD_EOMA: 2082 extd = J1939_ETP; 2083 fallthrough; 2084 case J1939_TP_CMD_EOMA: 2085 if (skcb->addr.type != extd) 2086 return; 2087 2088 if (j1939_tp_im_transmitter(skcb)) 2089 j1939_xtp_rx_eoma(priv, skb, false); 2090 2091 if (j1939_tp_im_receiver(skcb)) 2092 j1939_xtp_rx_eoma(priv, skb, true); 2093 2094 break; 2095 2096 case J1939_ETP_CMD_ABORT: /* && J1939_TP_CMD_ABORT */ 2097 if (j1939_cb_is_broadcast(skcb)) { 2098 netdev_err_once(priv->ndev, "%s: abort to broadcast (%02x), ignoring!\n", 2099 __func__, skcb->addr.sa); 2100 return; 2101 } 2102 2103 if (j1939_tp_im_transmitter(skcb)) 2104 j1939_xtp_rx_abort(priv, skb, true); 2105 2106 if (j1939_tp_im_receiver(skcb)) 2107 j1939_xtp_rx_abort(priv, skb, false); 2108 2109 break; 2110 default: 2111 return; 2112 } 2113 } 2114 2115 int j1939_tp_recv(struct j1939_priv *priv, struct sk_buff *skb) 2116 { 2117 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 2118 2119 if (!j1939_tp_im_involved_anydir(skcb) && !j1939_cb_is_broadcast(skcb)) 2120 return 0; 2121 2122 switch (skcb->addr.pgn) { 2123 case J1939_ETP_PGN_DAT: 2124 skcb->addr.type = J1939_ETP; 2125 fallthrough; 2126 case J1939_TP_PGN_DAT: 2127 j1939_xtp_rx_dat(priv, skb); 2128 break; 2129 2130 case J1939_ETP_PGN_CTL: 2131 skcb->addr.type = J1939_ETP; 2132 fallthrough; 2133 case J1939_TP_PGN_CTL: 2134 if (skb->len < 8) 2135 return 0; /* Don't care. Nothing to extract here */ 2136 2137 j1939_tp_cmd_recv(priv, skb); 2138 break; 2139 default: 2140 return 0; /* no problem */ 2141 } 2142 return 1; /* "I processed the message" */ 2143 } 2144 2145 void j1939_simple_recv(struct j1939_priv *priv, struct sk_buff *skb) 2146 { 2147 struct j1939_session *session; 2148 2149 if (!skb->sk) 2150 return; 2151 2152 if (skb->sk->sk_family != AF_CAN || 2153 skb->sk->sk_protocol != CAN_J1939) 2154 return; 2155 2156 j1939_session_list_lock(priv); 2157 session = j1939_session_get_simple(priv, skb); 2158 j1939_session_list_unlock(priv); 2159 if (!session) { 2160 netdev_warn(priv->ndev, 2161 "%s: Received already invalidated message\n", 2162 __func__); 2163 return; 2164 } 2165 2166 j1939_session_timers_cancel(session); 2167 j1939_session_deactivate(session); 2168 j1939_session_put(session); 2169 } 2170 2171 int j1939_cancel_active_session(struct j1939_priv *priv, struct sock *sk) 2172 { 2173 struct j1939_session *session, *saved; 2174 2175 netdev_dbg(priv->ndev, "%s, sk: %p\n", __func__, sk); 2176 j1939_session_list_lock(priv); 2177 list_for_each_entry_safe(session, saved, 2178 &priv->active_session_list, 2179 active_session_list_entry) { 2180 if (!sk || sk == session->sk) { 2181 if (hrtimer_try_to_cancel(&session->txtimer) == 1) 2182 j1939_session_put(session); 2183 if (hrtimer_try_to_cancel(&session->rxtimer) == 1) 2184 j1939_session_put(session); 2185 2186 session->err = ESHUTDOWN; 2187 j1939_session_deactivate_locked(session); 2188 } 2189 } 2190 j1939_session_list_unlock(priv); 2191 return NOTIFY_DONE; 2192 } 2193 2194 void j1939_tp_init(struct j1939_priv *priv) 2195 { 2196 spin_lock_init(&priv->active_session_list_lock); 2197 INIT_LIST_HEAD(&priv->active_session_list); 2198 priv->tp_max_packet_size = J1939_MAX_ETP_PACKET_SIZE; 2199 } 2200