can/j1939/transport.c

   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         /* skb->cb must be large enough to hold a j1939_sk_buff_cb structure */
 608         BUILD_BUG_ON(sizeof(skb->cb) < sizeof(*re_skcb));
 609
 610         memcpy(skb->cb, re_skcb, sizeof(*re_skcb));
 611         skcb = j1939_skb_to_cb(skb);
 612         if (swap_src_dst)
 613                 j1939_skbcb_swap(skcb);
 614
 615         if (ctl) {
 616                 if (skcb->addr.type == J1939_ETP)
 617                         skcb->addr.pgn = J1939_ETP_PGN_CTL;
 618                 else
 619                         skcb->addr.pgn = J1939_TP_PGN_CTL;
 620         } else {
 621                 if (skcb->addr.type == J1939_ETP)
 622                         skcb->addr.pgn = J1939_ETP_PGN_DAT;
 623                 else
 624                         skcb->addr.pgn = J1939_TP_PGN_DAT;
 625         }
 626
 627         return skb;
 628 }
 629
 630 /* TP transmit packet functions */
 631 static int j1939_tp_tx_dat(struct j1939_session *session,
 632                            const u8 *dat, int len)
 633 {
 634         struct j1939_priv *priv = session->priv;
 635         struct sk_buff *skb;
 636
 637         skb = j1939_tp_tx_dat_new(priv, &session->skcb,
 638                                   false, false);
 639         if (IS_ERR(skb))
 640                 return PTR_ERR(skb);
 641
 642         skb_put_data(skb, dat, len);
 643         if (j1939_tp_padding && len < 8)
 644                 memset(skb_put(skb, 8 - len), 0xff, 8 - len);
 645
 646         return j1939_send_one(priv, skb);
 647 }
 648
 649 static int j1939_xtp_do_tx_ctl(struct j1939_priv *priv,
 650                                const struct j1939_sk_buff_cb *re_skcb,
 651                                bool swap_src_dst, pgn_t pgn, const u8 *dat)
 652 {
 653         struct sk_buff *skb;
 654         u8 *skdat;
 655
 656         if (!j1939_tp_im_involved(re_skcb, swap_src_dst))
 657                 return 0;
 658
 659         skb = j1939_tp_tx_dat_new(priv, re_skcb, true, swap_src_dst);
 660         if (IS_ERR(skb))
 661                 return PTR_ERR(skb);
 662
 663         skdat = skb_put(skb, 8);
 664         memcpy(skdat, dat, 5);
 665         skdat[5] = (pgn >> 0);
 666         skdat[6] = (pgn >> 8);
 667         skdat[7] = (pgn >> 16);
 668
 669         return j1939_send_one(priv, skb);
 670 }
 671
 672 static inline int j1939_tp_tx_ctl(struct j1939_session *session,
 673                                   bool swap_src_dst, const u8 *dat)
 674 {
 675         struct j1939_priv *priv = session->priv;
 676
 677         return j1939_xtp_do_tx_ctl(priv, &session->skcb,
 678                                    swap_src_dst,
 679                                    session->skcb.addr.pgn, dat);
 680 }
 681
 682 static int j1939_xtp_tx_abort(struct j1939_priv *priv,
 683                               const struct j1939_sk_buff_cb *re_skcb,
 684                               bool swap_src_dst,
 685                               enum j1939_xtp_abort err,
 686                               pgn_t pgn)
 687 {
 688         u8 dat[5];
 689
 690         if (!j1939_tp_im_involved(re_skcb, swap_src_dst))
 691                 return 0;
 692
 693         memset(dat, 0xff, sizeof(dat));
 694         dat[0] = J1939_TP_CMD_ABORT;
 695         dat[1] = err;
 696         return j1939_xtp_do_tx_ctl(priv, re_skcb, swap_src_dst, pgn, dat);
 697 }
 698
 699 void j1939_tp_schedule_txtimer(struct j1939_session *session, int msec)
 700 {
 701         j1939_session_get(session);
 702         hrtimer_start(&session->txtimer, ms_to_ktime(msec),
 703                       HRTIMER_MODE_REL_SOFT);
 704 }
 705
 706 static inline void j1939_tp_set_rxtimeout(struct j1939_session *session,
 707                                           int msec)
 708 {
 709         j1939_session_rxtimer_cancel(session);
 710         j1939_session_get(session);
 711         hrtimer_start(&session->rxtimer, ms_to_ktime(msec),
 712                       HRTIMER_MODE_REL_SOFT);
 713 }
 714
 715 static int j1939_session_tx_rts(struct j1939_session *session)
 716 {
 717         u8 dat[8];
 718         int ret;
 719
 720         memset(dat, 0xff, sizeof(dat));
 721
 722         dat[1] = (session->total_message_size >> 0);
 723         dat[2] = (session->total_message_size >> 8);
 724         dat[3] = session->pkt.total;
 725
 726         if (session->skcb.addr.type == J1939_ETP) {
 727                 dat[0] = J1939_ETP_CMD_RTS;
 728                 dat[1] = (session->total_message_size >> 0);
 729                 dat[2] = (session->total_message_size >> 8);
 730                 dat[3] = (session->total_message_size >> 16);
 731                 dat[4] = (session->total_message_size >> 24);
 732         } else if (j1939_cb_is_broadcast(&session->skcb)) {
 733                 dat[0] = J1939_TP_CMD_BAM;
 734                 /* fake cts for broadcast */
 735                 session->pkt.tx = 0;
 736         } else {
 737                 dat[0] = J1939_TP_CMD_RTS;
 738                 dat[4] = dat[3];
 739         }
 740
 741         if (dat[0] == session->last_txcmd)
 742                 /* done already */
 743                 return 0;
 744
 745         ret = j1939_tp_tx_ctl(session, false, dat);
 746         if (ret < 0)
 747                 return ret;
 748
 749         session->last_txcmd = dat[0];
 750         if (dat[0] == J1939_TP_CMD_BAM) {
 751                 j1939_tp_schedule_txtimer(session, 50);
 752                 j1939_tp_set_rxtimeout(session, 250);
 753         } else {
 754                 j1939_tp_set_rxtimeout(session, 1250);
 755         }
 756
 757         netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
 758
 759         return 0;
 760 }
 761
 762 static int j1939_session_tx_dpo(struct j1939_session *session)
 763 {
 764         unsigned int pkt;
 765         u8 dat[8];
 766         int ret;
 767
 768         memset(dat, 0xff, sizeof(dat));
 769
 770         dat[0] = J1939_ETP_CMD_DPO;
 771         session->pkt.dpo = session->pkt.tx_acked;
 772         pkt = session->pkt.dpo;
 773         dat[1] = session->pkt.last - session->pkt.tx_acked;
 774         dat[2] = (pkt >> 0);
 775         dat[3] = (pkt >> 8);
 776         dat[4] = (pkt >> 16);
 777
 778         ret = j1939_tp_tx_ctl(session, false, dat);
 779         if (ret < 0)
 780                 return ret;
 781
 782         session->last_txcmd = dat[0];
 783         j1939_tp_set_rxtimeout(session, 1250);
 784         session->pkt.tx = session->pkt.tx_acked;
 785
 786         netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
 787
 788         return 0;
 789 }
 790
 791 static int j1939_session_tx_dat(struct j1939_session *session)
 792 {
 793         struct j1939_priv *priv = session->priv;
 794         struct j1939_sk_buff_cb *se_skcb;
 795         int offset, pkt_done, pkt_end;
 796         unsigned int len, pdelay;
 797         struct sk_buff *se_skb;
 798         const u8 *tpdat;
 799         int ret = 0;
 800         u8 dat[8];
 801
 802         se_skb = j1939_session_skb_get_by_offset(session, session->pkt.tx * 7);
 803         if (!se_skb)
 804                 return -ENOBUFS;
 805
 806         se_skcb = j1939_skb_to_cb(se_skb);
 807         tpdat = se_skb->data;
 808         ret = 0;
 809         pkt_done = 0;
 810         if (session->skcb.addr.type != J1939_ETP &&
 811             j1939_cb_is_broadcast(&session->skcb))
 812                 pkt_end = session->pkt.total;
 813         else
 814                 pkt_end = session->pkt.last;
 815
 816         while (session->pkt.tx < pkt_end) {
 817                 dat[0] = session->pkt.tx - session->pkt.dpo + 1;
 818                 offset = (session->pkt.tx * 7) - se_skcb->offset;
 819                 len =  se_skb->len - offset;
 820                 if (len > 7)
 821                         len = 7;
 822
 823                 if (offset + len > se_skb->len) {
 824                         netdev_err_once(priv->ndev,
 825                                         "%s: 0x%p: requested data outside of queued buffer: offset %i, len %i, pkt.tx: %i\n",
 826                                         __func__, session, se_skcb->offset,
 827                                         se_skb->len , session->pkt.tx);
 828                         ret = -EOVERFLOW;
 829                         goto out_free;
 830                 }
 831
 832                 if (!len) {
 833                         ret = -ENOBUFS;
 834                         break;
 835                 }
 836
 837                 memcpy(&dat[1], &tpdat[offset], len);
 838                 ret = j1939_tp_tx_dat(session, dat, len + 1);
 839                 if (ret < 0) {
 840                         /* ENOBUFS == CAN interface TX queue is full */
 841                         if (ret != -ENOBUFS)
 842                                 netdev_alert(priv->ndev,
 843                                              "%s: 0x%p: queue data error: %i\n",
 844                                              __func__, session, ret);
 845                         break;
 846                 }
 847
 848                 session->last_txcmd = 0xff;
 849                 pkt_done++;
 850                 session->pkt.tx++;
 851                 pdelay = j1939_cb_is_broadcast(&session->skcb) ? 50 :
 852                         j1939_tp_packet_delay;
 853
 854                 if (session->pkt.tx < session->pkt.total && pdelay) {
 855                         j1939_tp_schedule_txtimer(session, pdelay);
 856                         break;
 857                 }
 858         }
 859
 860         if (pkt_done)
 861                 j1939_tp_set_rxtimeout(session, 250);
 862
 863  out_free:
 864         if (ret)
 865                 kfree_skb(se_skb);
 866         else
 867                 consume_skb(se_skb);
 868
 869         return ret;
 870 }
 871
 872 static int j1939_xtp_txnext_transmiter(struct j1939_session *session)
 873 {
 874         struct j1939_priv *priv = session->priv;
 875         int ret = 0;
 876
 877         if (!j1939_tp_im_transmitter(&session->skcb)) {
 878                 netdev_alert(priv->ndev, "%s: 0x%p: called by not transmitter!\n",
 879                              __func__, session);
 880                 return -EINVAL;
 881         }
 882
 883         switch (session->last_cmd) {
 884         case 0:
 885                 ret = j1939_session_tx_rts(session);
 886                 break;
 887
 888         case J1939_ETP_CMD_CTS:
 889                 if (session->last_txcmd != J1939_ETP_CMD_DPO) {
 890                         ret = j1939_session_tx_dpo(session);
 891                         if (ret)
 892                                 return ret;
 893                 }
 894
 895                 fallthrough;
 896         case J1939_TP_CMD_CTS:
 897         case 0xff: /* did some data */
 898         case J1939_ETP_CMD_DPO:
 899         case J1939_TP_CMD_BAM:
 900                 ret = j1939_session_tx_dat(session);
 901
 902                 break;
 903         default:
 904                 netdev_alert(priv->ndev, "%s: 0x%p: unexpected last_cmd: %x\n",
 905                              __func__, session, session->last_cmd);
 906         }
 907
 908         return ret;
 909 }
 910
 911 static int j1939_session_tx_cts(struct j1939_session *session)
 912 {
 913         struct j1939_priv *priv = session->priv;
 914         unsigned int pkt, len;
 915         int ret;
 916         u8 dat[8];
 917
 918         if (!j1939_sk_recv_match(priv, &session->skcb))
 919                 return -ENOENT;
 920
 921         len = session->pkt.total - session->pkt.rx;
 922         len = min3(len, session->pkt.block, j1939_tp_block ?: 255);
 923         memset(dat, 0xff, sizeof(dat));
 924
 925         if (session->skcb.addr.type == J1939_ETP) {
 926                 pkt = session->pkt.rx + 1;
 927                 dat[0] = J1939_ETP_CMD_CTS;
 928                 dat[1] = len;
 929                 dat[2] = (pkt >> 0);
 930                 dat[3] = (pkt >> 8);
 931                 dat[4] = (pkt >> 16);
 932         } else {
 933                 dat[0] = J1939_TP_CMD_CTS;
 934                 dat[1] = len;
 935                 dat[2] = session->pkt.rx + 1;
 936         }
 937
 938         if (dat[0] == session->last_txcmd)
 939                 /* done already */
 940                 return 0;
 941
 942         ret = j1939_tp_tx_ctl(session, true, dat);
 943         if (ret < 0)
 944                 return ret;
 945
 946         if (len)
 947                 /* only mark cts done when len is set */
 948                 session->last_txcmd = dat[0];
 949         j1939_tp_set_rxtimeout(session, 1250);
 950
 951         netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
 952
 953         return 0;
 954 }
 955
 956 static int j1939_session_tx_eoma(struct j1939_session *session)
 957 {
 958         struct j1939_priv *priv = session->priv;
 959         u8 dat[8];
 960         int ret;
 961
 962         if (!j1939_sk_recv_match(priv, &session->skcb))
 963                 return -ENOENT;
 964
 965         memset(dat, 0xff, sizeof(dat));
 966
 967         if (session->skcb.addr.type == J1939_ETP) {
 968                 dat[0] = J1939_ETP_CMD_EOMA;
 969                 dat[1] = session->total_message_size >> 0;
 970                 dat[2] = session->total_message_size >> 8;
 971                 dat[3] = session->total_message_size >> 16;
 972                 dat[4] = session->total_message_size >> 24;
 973         } else {
 974                 dat[0] = J1939_TP_CMD_EOMA;
 975                 dat[1] = session->total_message_size;
 976                 dat[2] = session->total_message_size >> 8;
 977                 dat[3] = session->pkt.total;
 978         }
 979
 980         if (dat[0] == session->last_txcmd)
 981                 /* done already */
 982                 return 0;
 983
 984         ret = j1939_tp_tx_ctl(session, true, dat);
 985         if (ret < 0)
 986                 return ret;
 987
 988         session->last_txcmd = dat[0];
 989
 990         /* wait for the EOMA packet to come in */
 991         j1939_tp_set_rxtimeout(session, 1250);
 992
 993         netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
 994
 995         return 0;
 996 }
 997
 998 static int j1939_xtp_txnext_receiver(struct j1939_session *session)
 999 {
1000         struct j1939_priv *priv = session->priv;
1001         int ret = 0;
1002
1003         if (!j1939_tp_im_receiver(&session->skcb)) {
1004                 netdev_alert(priv->ndev, "%s: 0x%p: called by not receiver!\n",
1005                              __func__, session);
1006                 return -EINVAL;
1007         }
1008
1009         switch (session->last_cmd) {
1010         case J1939_TP_CMD_RTS:
1011         case J1939_ETP_CMD_RTS:
1012                 ret = j1939_session_tx_cts(session);
1013                 break;
1014
1015         case J1939_ETP_CMD_CTS:
1016         case J1939_TP_CMD_CTS:
1017         case 0xff: /* did some data */
1018         case J1939_ETP_CMD_DPO:
1019                 if ((session->skcb.addr.type == J1939_TP &&
1020                      j1939_cb_is_broadcast(&session->skcb)))
1021                         break;
1022
1023                 if (session->pkt.rx >= session->pkt.total) {
1024                         ret = j1939_session_tx_eoma(session);
1025                 } else if (session->pkt.rx >= session->pkt.last) {
1026                         session->last_txcmd = 0;
1027                         ret = j1939_session_tx_cts(session);
1028                 }
1029                 break;
1030         default:
1031                 netdev_alert(priv->ndev, "%s: 0x%p: unexpected last_cmd: %x\n",
1032                              __func__, session, session->last_cmd);
1033         }
1034
1035         return ret;
1036 }
1037
1038 static int j1939_simple_txnext(struct j1939_session *session)
1039 {
1040         struct j1939_priv *priv = session->priv;
1041         struct sk_buff *se_skb = j1939_session_skb_get(session);
1042         struct sk_buff *skb;
1043         int ret;
1044
1045         if (!se_skb)
1046                 return 0;
1047
1048         skb = skb_clone(se_skb, GFP_ATOMIC);
1049         if (!skb) {
1050                 ret = -ENOMEM;
1051                 goto out_free;
1052         }
1053
1054         can_skb_set_owner(skb, se_skb->sk);
1055
1056         j1939_tp_set_rxtimeout(session, J1939_SIMPLE_ECHO_TIMEOUT_MS);
1057
1058         ret = j1939_send_one(priv, skb);
1059         if (ret)
1060                 goto out_free;
1061
1062         j1939_sk_errqueue(session, J1939_ERRQUEUE_TX_SCHED);
1063         j1939_sk_queue_activate_next(session);
1064
1065  out_free:
1066         if (ret)
1067                 kfree_skb(se_skb);
1068         else
1069                 consume_skb(se_skb);
1070
1071         return ret;
1072 }
1073
1074 static bool j1939_session_deactivate_locked(struct j1939_session *session)
1075 {
1076         bool active = false;
1077
1078         lockdep_assert_held(&session->priv->active_session_list_lock);
1079
1080         if (session->state >= J1939_SESSION_ACTIVE &&
1081             session->state < J1939_SESSION_ACTIVE_MAX) {
1082                 active = true;
1083
1084                 list_del_init(&session->active_session_list_entry);
1085                 session->state = J1939_SESSION_DONE;
1086                 j1939_session_put(session);
1087         }
1088
1089         return active;
1090 }
1091
1092 static bool j1939_session_deactivate(struct j1939_session *session)
1093 {
1094         struct j1939_priv *priv = session->priv;
1095         bool active;
1096
1097         j1939_session_list_lock(priv);
1098         active = j1939_session_deactivate_locked(session);
1099         j1939_session_list_unlock(priv);
1100
1101         return active;
1102 }
1103
1104 static void
1105 j1939_session_deactivate_activate_next(struct j1939_session *session)
1106 {
1107         if (j1939_session_deactivate(session))
1108                 j1939_sk_queue_activate_next(session);
1109 }
1110
1111 static void __j1939_session_cancel(struct j1939_session *session,
1112                                    enum j1939_xtp_abort err)
1113 {
1114         struct j1939_priv *priv = session->priv;
1115
1116         WARN_ON_ONCE(!err);
1117         lockdep_assert_held(&session->priv->active_session_list_lock);
1118
1119         session->err = j1939_xtp_abort_to_errno(priv, err);
1120         session->state = J1939_SESSION_WAITING_ABORT;
1121         /* do not send aborts on incoming broadcasts */
1122         if (!j1939_cb_is_broadcast(&session->skcb)) {
1123                 j1939_xtp_tx_abort(priv, &session->skcb,
1124                                    !session->transmission,
1125                                    err, session->skcb.addr.pgn);
1126         }
1127
1128         if (session->sk)
1129                 j1939_sk_send_loop_abort(session->sk, session->err);
1130 }
1131
1132 static void j1939_session_cancel(struct j1939_session *session,
1133                                  enum j1939_xtp_abort err)
1134 {
1135         j1939_session_list_lock(session->priv);
1136
1137         if (session->state >= J1939_SESSION_ACTIVE &&
1138             session->state < J1939_SESSION_WAITING_ABORT) {
1139                 j1939_tp_set_rxtimeout(session, J1939_XTP_ABORT_TIMEOUT_MS);
1140                 __j1939_session_cancel(session, err);
1141         }
1142
1143         j1939_session_list_unlock(session->priv);
1144
1145         if (!session->sk)
1146                 j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_ABORT);
1147 }
1148
1149 static enum hrtimer_restart j1939_tp_txtimer(struct hrtimer *hrtimer)
1150 {
1151         struct j1939_session *session =
1152                 container_of(hrtimer, struct j1939_session, txtimer);
1153         struct j1939_priv *priv = session->priv;
1154         int ret = 0;
1155
1156         if (session->skcb.addr.type == J1939_SIMPLE) {
1157                 ret = j1939_simple_txnext(session);
1158         } else {
1159                 if (session->transmission)
1160                         ret = j1939_xtp_txnext_transmiter(session);
1161                 else
1162                         ret = j1939_xtp_txnext_receiver(session);
1163         }
1164
1165         switch (ret) {
1166         case -ENOBUFS:
1167                 /* Retry limit is currently arbitrary chosen */
1168                 if (session->tx_retry < J1939_XTP_TX_RETRY_LIMIT) {
1169                         session->tx_retry++;
1170                         j1939_tp_schedule_txtimer(session,
1171                                                   10 + get_random_u32_below(16));
1172                 } else {
1173                         netdev_alert(priv->ndev, "%s: 0x%p: tx retry count reached\n",
1174                                      __func__, session);
1175                         session->err = -ENETUNREACH;
1176                         j1939_session_rxtimer_cancel(session);
1177                         j1939_session_deactivate_activate_next(session);
1178                 }
1179                 break;
1180         case -ENETDOWN:
1181                 /* In this case we should get a netdev_event(), all active
1182                  * sessions will be cleared by
1183                  * j1939_cancel_all_active_sessions(). So handle this as an
1184                  * error, but let j1939_cancel_all_active_sessions() do the
1185                  * cleanup including propagation of the error to user space.
1186                  */
1187                 break;
1188         case -EOVERFLOW:
1189                 j1939_session_cancel(session, J1939_XTP_ABORT_ECTS_TOO_BIG);
1190                 break;
1191         case 0:
1192                 session->tx_retry = 0;
1193                 break;
1194         default:
1195                 netdev_alert(priv->ndev, "%s: 0x%p: tx aborted with unknown reason: %i\n",
1196                              __func__, session, ret);
1197                 if (session->skcb.addr.type != J1939_SIMPLE) {
1198                         j1939_session_cancel(session, J1939_XTP_ABORT_OTHER);
1199                 } else {
1200                         session->err = ret;
1201                         j1939_session_rxtimer_cancel(session);
1202                         j1939_session_deactivate_activate_next(session);
1203                 }
1204         }
1205
1206         j1939_session_put(session);
1207
1208         return HRTIMER_NORESTART;
1209 }
1210
1211 static void j1939_session_completed(struct j1939_session *session)
1212 {
1213         struct sk_buff *se_skb;
1214
1215         if (!session->transmission) {
1216                 se_skb = j1939_session_skb_get(session);
1217                 /* distribute among j1939 receivers */
1218                 j1939_sk_recv(session->priv, se_skb);
1219                 consume_skb(se_skb);
1220         }
1221
1222         j1939_session_deactivate_activate_next(session);
1223 }
1224
1225 static enum hrtimer_restart j1939_tp_rxtimer(struct hrtimer *hrtimer)
1226 {
1227         struct j1939_session *session = container_of(hrtimer,
1228                                                      struct j1939_session,
1229                                                      rxtimer);
1230         struct j1939_priv *priv = session->priv;
1231
1232         if (session->state == J1939_SESSION_WAITING_ABORT) {
1233                 netdev_alert(priv->ndev, "%s: 0x%p: abort rx timeout. Force session deactivation\n",
1234                              __func__, session);
1235
1236                 j1939_session_deactivate_activate_next(session);
1237
1238         } else if (session->skcb.addr.type == J1939_SIMPLE) {
1239                 netdev_alert(priv->ndev, "%s: 0x%p: Timeout. Failed to send simple message.\n",
1240                              __func__, session);
1241
1242                 /* The message is probably stuck in the CAN controller and can
1243                  * be send as soon as CAN bus is in working state again.
1244                  */
1245                 session->err = -ETIME;
1246                 j1939_session_deactivate(session);
1247         } else {
1248                 j1939_session_list_lock(session->priv);
1249                 if (session->state >= J1939_SESSION_ACTIVE &&
1250                     session->state < J1939_SESSION_ACTIVE_MAX) {
1251                         netdev_alert(priv->ndev, "%s: 0x%p: rx timeout, send abort\n",
1252                                      __func__, session);
1253                         j1939_session_get(session);
1254                         hrtimer_start(&session->rxtimer,
1255                                       ms_to_ktime(J1939_XTP_ABORT_TIMEOUT_MS),
1256                                       HRTIMER_MODE_REL_SOFT);
1257                         __j1939_session_cancel(session, J1939_XTP_ABORT_TIMEOUT);
1258                 }
1259                 j1939_session_list_unlock(session->priv);
1260
1261                 if (!session->sk)
1262                         j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_ABORT);
1263         }
1264
1265         j1939_session_put(session);
1266
1267         return HRTIMER_NORESTART;
1268 }
1269
1270 static bool j1939_xtp_rx_cmd_bad_pgn(struct j1939_session *session,
1271                                      const struct sk_buff *skb)
1272 {
1273         const struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1274         pgn_t pgn = j1939_xtp_ctl_to_pgn(skb->data);
1275         struct j1939_priv *priv = session->priv;
1276         enum j1939_xtp_abort abort = J1939_XTP_NO_ABORT;
1277         u8 cmd = skb->data[0];
1278
1279         if (session->skcb.addr.pgn == pgn)
1280                 return false;
1281
1282         switch (cmd) {
1283         case J1939_TP_CMD_BAM:
1284                 abort = J1939_XTP_NO_ABORT;
1285                 break;
1286
1287         case J1939_ETP_CMD_RTS:
1288                 fallthrough;
1289         case J1939_TP_CMD_RTS:
1290                 abort = J1939_XTP_ABORT_BUSY;
1291                 break;
1292
1293         case J1939_ETP_CMD_CTS:
1294                 fallthrough;
1295         case J1939_TP_CMD_CTS:
1296                 abort = J1939_XTP_ABORT_ECTS_UNXPECTED_PGN;
1297                 break;
1298
1299         case J1939_ETP_CMD_DPO:
1300                 abort = J1939_XTP_ABORT_BAD_EDPO_PGN;
1301                 break;
1302
1303         case J1939_ETP_CMD_EOMA:
1304                 fallthrough;
1305         case J1939_TP_CMD_EOMA:
1306                 abort = J1939_XTP_ABORT_OTHER;
1307                 break;
1308
1309         case J1939_ETP_CMD_ABORT: /* && J1939_TP_CMD_ABORT */
1310                 abort = J1939_XTP_NO_ABORT;
1311                 break;
1312
1313         default:
1314                 WARN_ON_ONCE(1);
1315                 break;
1316         }
1317
1318         netdev_warn(priv->ndev, "%s: 0x%p: CMD 0x%02x with PGN 0x%05x for running session with different PGN 0x%05x.\n",
1319                     __func__, session, cmd, pgn, session->skcb.addr.pgn);
1320         if (abort != J1939_XTP_NO_ABORT)
1321                 j1939_xtp_tx_abort(priv, skcb, true, abort, pgn);
1322
1323         return true;
1324 }
1325
1326 static void j1939_xtp_rx_abort_one(struct j1939_priv *priv, struct sk_buff *skb,
1327                                    bool reverse, bool transmitter)
1328 {
1329         struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1330         struct j1939_session *session;
1331         u8 abort = skb->data[1];
1332
1333         session = j1939_session_get_by_addr(priv, &skcb->addr, reverse,
1334                                             transmitter);
1335         if (!session)
1336                 return;
1337
1338         if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
1339                 goto abort_put;
1340
1341         netdev_info(priv->ndev, "%s: 0x%p: 0x%05x: (%u) %s\n", __func__,
1342                     session, j1939_xtp_ctl_to_pgn(skb->data), abort,
1343                     j1939_xtp_abort_to_str(abort));
1344
1345         j1939_session_timers_cancel(session);
1346         session->err = j1939_xtp_abort_to_errno(priv, abort);
1347         if (session->sk)
1348                 j1939_sk_send_loop_abort(session->sk, session->err);
1349         else
1350                 j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_ABORT);
1351         j1939_session_deactivate_activate_next(session);
1352
1353 abort_put:
1354         j1939_session_put(session);
1355 }
1356
1357 /* abort packets may come in 2 directions */
1358 static void
1359 j1939_xtp_rx_abort(struct j1939_priv *priv, struct sk_buff *skb,
1360                    bool transmitter)
1361 {
1362         j1939_xtp_rx_abort_one(priv, skb, false, transmitter);
1363         j1939_xtp_rx_abort_one(priv, skb, true, transmitter);
1364 }
1365
1366 static void
1367 j1939_xtp_rx_eoma_one(struct j1939_session *session, struct sk_buff *skb)
1368 {
1369         struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1370         const u8 *dat;
1371         int len;
1372
1373         if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
1374                 return;
1375
1376         dat = skb->data;
1377
1378         if (skcb->addr.type == J1939_ETP)
1379                 len = j1939_etp_ctl_to_size(dat);
1380         else
1381                 len = j1939_tp_ctl_to_size(dat);
1382
1383         if (session->total_message_size != len) {
1384                 netdev_warn_once(session->priv->ndev,
1385                                  "%s: 0x%p: Incorrect size. Expected: %i; got: %i.\n",
1386                                  __func__, session, session->total_message_size,
1387                                  len);
1388         }
1389
1390         netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
1391
1392         session->pkt.tx_acked = session->pkt.total;
1393         j1939_session_timers_cancel(session);
1394         /* transmitted without problems */
1395         j1939_session_completed(session);
1396 }
1397
1398 static void
1399 j1939_xtp_rx_eoma(struct j1939_priv *priv, struct sk_buff *skb,
1400                   bool transmitter)
1401 {
1402         struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1403         struct j1939_session *session;
1404
1405         session = j1939_session_get_by_addr(priv, &skcb->addr, true,
1406                                             transmitter);
1407         if (!session)
1408                 return;
1409
1410         j1939_xtp_rx_eoma_one(session, skb);
1411         j1939_session_put(session);
1412 }
1413
1414 static void
1415 j1939_xtp_rx_cts_one(struct j1939_session *session, struct sk_buff *skb)
1416 {
1417         enum j1939_xtp_abort err = J1939_XTP_ABORT_FAULT;
1418         unsigned int pkt;
1419         const u8 *dat;
1420
1421         dat = skb->data;
1422
1423         if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
1424                 return;
1425
1426         netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
1427
1428         if (session->last_cmd == dat[0]) {
1429                 err = J1939_XTP_ABORT_DUP_SEQ;
1430                 goto out_session_cancel;
1431         }
1432
1433         if (session->skcb.addr.type == J1939_ETP)
1434                 pkt = j1939_etp_ctl_to_packet(dat);
1435         else
1436                 pkt = dat[2];
1437
1438         if (!pkt)
1439                 goto out_session_cancel;
1440         else if (dat[1] > session->pkt.block /* 0xff for etp */)
1441                 goto out_session_cancel;
1442
1443         /* set packet counters only when not CTS(0) */
1444         session->pkt.tx_acked = pkt - 1;
1445         j1939_session_skb_drop_old(session);
1446         session->pkt.last = session->pkt.tx_acked + dat[1];
1447         if (session->pkt.last > session->pkt.total)
1448                 /* safety measure */
1449                 session->pkt.last = session->pkt.total;
1450         /* TODO: do not set tx here, do it in txtimer */
1451         session->pkt.tx = session->pkt.tx_acked;
1452
1453         session->last_cmd = dat[0];
1454         if (dat[1]) {
1455                 j1939_tp_set_rxtimeout(session, 1250);
1456                 if (session->transmission) {
1457                         if (session->pkt.tx_acked)
1458                                 j1939_sk_errqueue(session,
1459                                                   J1939_ERRQUEUE_TX_SCHED);
1460                         j1939_session_txtimer_cancel(session);
1461                         j1939_tp_schedule_txtimer(session, 0);
1462                 }
1463         } else {
1464                 /* CTS(0) */
1465                 j1939_tp_set_rxtimeout(session, 550);
1466         }
1467         return;
1468
1469  out_session_cancel:
1470         j1939_session_timers_cancel(session);
1471         j1939_session_cancel(session, err);
1472 }
1473
1474 static void
1475 j1939_xtp_rx_cts(struct j1939_priv *priv, struct sk_buff *skb, bool transmitter)
1476 {
1477         struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1478         struct j1939_session *session;
1479
1480         session = j1939_session_get_by_addr(priv, &skcb->addr, true,
1481                                             transmitter);
1482         if (!session)
1483                 return;
1484         j1939_xtp_rx_cts_one(session, skb);
1485         j1939_session_put(session);
1486 }
1487
1488 static struct j1939_session *j1939_session_new(struct j1939_priv *priv,
1489                                                struct sk_buff *skb, size_t size)
1490 {
1491         struct j1939_session *session;
1492         struct j1939_sk_buff_cb *skcb;
1493
1494         session = kzalloc(sizeof(*session), gfp_any());
1495         if (!session)
1496                 return NULL;
1497
1498         INIT_LIST_HEAD(&session->active_session_list_entry);
1499         INIT_LIST_HEAD(&session->sk_session_queue_entry);
1500         kref_init(&session->kref);
1501
1502         j1939_priv_get(priv);
1503         session->priv = priv;
1504         session->total_message_size = size;
1505         session->state = J1939_SESSION_NEW;
1506
1507         skb_queue_head_init(&session->skb_queue);
1508         skb_queue_tail(&session->skb_queue, skb);
1509
1510         skcb = j1939_skb_to_cb(skb);
1511         memcpy(&session->skcb, skcb, sizeof(session->skcb));
1512
1513         hrtimer_init(&session->txtimer, CLOCK_MONOTONIC,
1514                      HRTIMER_MODE_REL_SOFT);
1515         session->txtimer.function = j1939_tp_txtimer;
1516         hrtimer_init(&session->rxtimer, CLOCK_MONOTONIC,
1517                      HRTIMER_MODE_REL_SOFT);
1518         session->rxtimer.function = j1939_tp_rxtimer;
1519
1520         netdev_dbg(priv->ndev, "%s: 0x%p: sa: %02x, da: %02x\n",
1521                    __func__, session, skcb->addr.sa, skcb->addr.da);
1522
1523         return session;
1524 }
1525
1526 static struct
1527 j1939_session *j1939_session_fresh_new(struct j1939_priv *priv,
1528                                        int size,
1529                                        const struct j1939_sk_buff_cb *rel_skcb)
1530 {
1531         struct sk_buff *skb;
1532         struct j1939_sk_buff_cb *skcb;
1533         struct j1939_session *session;
1534
1535         skb = alloc_skb(size + sizeof(struct can_skb_priv), GFP_ATOMIC);
1536         if (unlikely(!skb))
1537                 return NULL;
1538
1539         skb->dev = priv->ndev;
1540         can_skb_reserve(skb);
1541         can_skb_prv(skb)->ifindex = priv->ndev->ifindex;
1542         can_skb_prv(skb)->skbcnt = 0;
1543         skcb = j1939_skb_to_cb(skb);
1544         memcpy(skcb, rel_skcb, sizeof(*skcb));
1545
1546         session = j1939_session_new(priv, skb, size);
1547         if (!session) {
1548                 kfree_skb(skb);
1549                 return NULL;
1550         }
1551
1552         /* alloc data area */
1553         skb_put(skb, size);
1554         /* skb is recounted in j1939_session_new() */
1555         return session;
1556 }
1557
1558 int j1939_session_activate(struct j1939_session *session)
1559 {
1560         struct j1939_priv *priv = session->priv;
1561         struct j1939_session *active = NULL;
1562         int ret = 0;
1563
1564         j1939_session_list_lock(priv);
1565         if (session->skcb.addr.type != J1939_SIMPLE)
1566                 active = j1939_session_get_by_addr_locked(priv,
1567                                                           &priv->active_session_list,
1568                                                           &session->skcb.addr, false,
1569                                                           session->transmission);
1570         if (active) {
1571                 j1939_session_put(active);
1572                 ret = -EAGAIN;
1573         } else {
1574                 WARN_ON_ONCE(session->state != J1939_SESSION_NEW);
1575                 list_add_tail(&session->active_session_list_entry,
1576                               &priv->active_session_list);
1577                 j1939_session_get(session);
1578                 session->state = J1939_SESSION_ACTIVE;
1579
1580                 netdev_dbg(session->priv->ndev, "%s: 0x%p\n",
1581                            __func__, session);
1582         }
1583         j1939_session_list_unlock(priv);
1584
1585         return ret;
1586 }
1587
1588 static struct
1589 j1939_session *j1939_xtp_rx_rts_session_new(struct j1939_priv *priv,
1590                                             struct sk_buff *skb)
1591 {
1592         enum j1939_xtp_abort abort = J1939_XTP_NO_ABORT;
1593         struct j1939_sk_buff_cb skcb = *j1939_skb_to_cb(skb);
1594         struct j1939_session *session;
1595         const u8 *dat;
1596         pgn_t pgn;
1597         int len;
1598
1599         netdev_dbg(priv->ndev, "%s\n", __func__);
1600
1601         dat = skb->data;
1602         pgn = j1939_xtp_ctl_to_pgn(dat);
1603         skcb.addr.pgn = pgn;
1604
1605         if (!j1939_sk_recv_match(priv, &skcb))
1606                 return NULL;
1607
1608         if (skcb.addr.type == J1939_ETP) {
1609                 len = j1939_etp_ctl_to_size(dat);
1610                 if (len > J1939_MAX_ETP_PACKET_SIZE)
1611                         abort = J1939_XTP_ABORT_FAULT;
1612                 else if (len > priv->tp_max_packet_size)
1613                         abort = J1939_XTP_ABORT_RESOURCE;
1614                 else if (len <= J1939_MAX_TP_PACKET_SIZE)
1615                         abort = J1939_XTP_ABORT_FAULT;
1616         } else {
1617                 len = j1939_tp_ctl_to_size(dat);
1618                 if (len > J1939_MAX_TP_PACKET_SIZE)
1619                         abort = J1939_XTP_ABORT_FAULT;
1620                 else if (len > priv->tp_max_packet_size)
1621                         abort = J1939_XTP_ABORT_RESOURCE;
1622                 else if (len < J1939_MIN_TP_PACKET_SIZE)
1623                         abort = J1939_XTP_ABORT_FAULT;
1624         }
1625
1626         if (abort != J1939_XTP_NO_ABORT) {
1627                 j1939_xtp_tx_abort(priv, &skcb, true, abort, pgn);
1628                 return NULL;
1629         }
1630
1631         session = j1939_session_fresh_new(priv, len, &skcb);
1632         if (!session) {
1633                 j1939_xtp_tx_abort(priv, &skcb, true,
1634                                    J1939_XTP_ABORT_RESOURCE, pgn);
1635                 return NULL;
1636         }
1637
1638         /* initialize the control buffer: plain copy */
1639         session->pkt.total = (len + 6) / 7;
1640         session->pkt.block = 0xff;
1641         if (skcb.addr.type != J1939_ETP) {
1642                 if (dat[3] != session->pkt.total)
1643                         netdev_alert(priv->ndev, "%s: 0x%p: strange total, %u != %u\n",
1644                                      __func__, session, session->pkt.total,
1645                                      dat[3]);
1646                 session->pkt.total = dat[3];
1647                 session->pkt.block = min(dat[3], dat[4]);
1648         }
1649
1650         session->pkt.rx = 0;
1651         session->pkt.tx = 0;
1652
1653         session->tskey = priv->rx_tskey++;
1654         j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_RTS);
1655
1656         WARN_ON_ONCE(j1939_session_activate(session));
1657
1658         return session;
1659 }
1660
1661 static int j1939_xtp_rx_rts_session_active(struct j1939_session *session,
1662                                            struct sk_buff *skb)
1663 {
1664         struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1665         struct j1939_priv *priv = session->priv;
1666
1667         if (!session->transmission) {
1668                 if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
1669                         return -EBUSY;
1670
1671                 /* RTS on active session */
1672                 j1939_session_timers_cancel(session);
1673                 j1939_session_cancel(session, J1939_XTP_ABORT_BUSY);
1674         }
1675
1676         if (session->last_cmd != 0) {
1677                 /* we received a second rts on the same connection */
1678                 netdev_alert(priv->ndev, "%s: 0x%p: connection exists (%02x %02x). last cmd: %x\n",
1679                              __func__, session, skcb->addr.sa, skcb->addr.da,
1680                              session->last_cmd);
1681
1682                 j1939_session_timers_cancel(session);
1683                 j1939_session_cancel(session, J1939_XTP_ABORT_BUSY);
1684
1685                 return -EBUSY;
1686         }
1687
1688         if (session->skcb.addr.sa != skcb->addr.sa ||
1689             session->skcb.addr.da != skcb->addr.da)
1690                 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",
1691                             __func__, session,
1692                             session->skcb.addr.sa, skcb->addr.sa,
1693                             session->skcb.addr.da, skcb->addr.da);
1694         /* make sure 'sa' & 'da' are correct !
1695          * They may be 'not filled in yet' for sending
1696          * skb's, since they did not pass the Address Claim ever.
1697          */
1698         session->skcb.addr.sa = skcb->addr.sa;
1699         session->skcb.addr.da = skcb->addr.da;
1700
1701         netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
1702
1703         return 0;
1704 }
1705
1706 static void j1939_xtp_rx_rts(struct j1939_priv *priv, struct sk_buff *skb,
1707                              bool transmitter)
1708 {
1709         struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1710         struct j1939_session *session;
1711         u8 cmd = skb->data[0];
1712
1713         session = j1939_session_get_by_addr(priv, &skcb->addr, false,
1714                                             transmitter);
1715
1716         if (!session) {
1717                 if (transmitter) {
1718                         /* If we're the transmitter and this function is called,
1719                          * we received our own RTS. A session has already been
1720                          * created.
1721                          *
1722                          * For some reasons however it might have been destroyed
1723                          * already. So don't create a new one here (using
1724                          * "j1939_xtp_rx_rts_session_new()") as this will be a
1725                          * receiver session.
1726                          *
1727                          * The reasons the session is already destroyed might
1728                          * be:
1729                          * - user space closed socket was and the session was
1730                          *   aborted
1731                          * - session was aborted due to external abort message
1732                          */
1733                         return;
1734                 }
1735                 session = j1939_xtp_rx_rts_session_new(priv, skb);
1736                 if (!session) {
1737                         if (cmd == J1939_TP_CMD_BAM && j1939_sk_recv_match(priv, skcb))
1738                                 netdev_info(priv->ndev, "%s: failed to create TP BAM session\n",
1739                                             __func__);
1740                         return;
1741                 }
1742         } else {
1743                 if (j1939_xtp_rx_rts_session_active(session, skb)) {
1744                         j1939_session_put(session);
1745                         return;
1746                 }
1747         }
1748         session->last_cmd = cmd;
1749
1750         if (cmd == J1939_TP_CMD_BAM) {
1751                 if (!session->transmission)
1752                         j1939_tp_set_rxtimeout(session, 750);
1753         } else {
1754                 if (!session->transmission) {
1755                         j1939_session_txtimer_cancel(session);
1756                         j1939_tp_schedule_txtimer(session, 0);
1757                 }
1758                 j1939_tp_set_rxtimeout(session, 1250);
1759         }
1760
1761         j1939_session_put(session);
1762 }
1763
1764 static void j1939_xtp_rx_dpo_one(struct j1939_session *session,
1765                                  struct sk_buff *skb)
1766 {
1767         const u8 *dat = skb->data;
1768
1769         if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
1770                 return;
1771
1772         netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
1773
1774         /* transmitted without problems */
1775         session->pkt.dpo = j1939_etp_ctl_to_packet(skb->data);
1776         session->last_cmd = dat[0];
1777         j1939_tp_set_rxtimeout(session, 750);
1778
1779         if (!session->transmission)
1780                 j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_DPO);
1781 }
1782
1783 static void j1939_xtp_rx_dpo(struct j1939_priv *priv, struct sk_buff *skb,
1784                              bool transmitter)
1785 {
1786         struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1787         struct j1939_session *session;
1788
1789         session = j1939_session_get_by_addr(priv, &skcb->addr, false,
1790                                             transmitter);
1791         if (!session) {
1792                 netdev_info(priv->ndev,
1793                             "%s: no connection found\n", __func__);
1794                 return;
1795         }
1796
1797         j1939_xtp_rx_dpo_one(session, skb);
1798         j1939_session_put(session);
1799 }
1800
1801 static void j1939_xtp_rx_dat_one(struct j1939_session *session,
1802                                  struct sk_buff *skb)
1803 {
1804         enum j1939_xtp_abort abort = J1939_XTP_ABORT_FAULT;
1805         struct j1939_priv *priv = session->priv;
1806         struct j1939_sk_buff_cb *skcb, *se_skcb;
1807         struct sk_buff *se_skb = NULL;
1808         const u8 *dat;
1809         u8 *tpdat;
1810         int offset;
1811         int nbytes;
1812         bool final = false;
1813         bool remain = false;
1814         bool do_cts_eoma = false;
1815         int packet;
1816
1817         skcb = j1939_skb_to_cb(skb);
1818         dat = skb->data;
1819         if (skb->len != 8) {
1820                 /* makes no sense */
1821                 abort = J1939_XTP_ABORT_UNEXPECTED_DATA;
1822                 goto out_session_cancel;
1823         }
1824
1825         switch (session->last_cmd) {
1826         case 0xff:
1827                 break;
1828         case J1939_ETP_CMD_DPO:
1829                 if (skcb->addr.type == J1939_ETP)
1830                         break;
1831                 fallthrough;
1832         case J1939_TP_CMD_BAM:
1833                 fallthrough;
1834         case J1939_TP_CMD_CTS:
1835                 if (skcb->addr.type != J1939_ETP)
1836                         break;
1837                 fallthrough;
1838         default:
1839                 netdev_info(priv->ndev, "%s: 0x%p: last %02x\n", __func__,
1840                             session, session->last_cmd);
1841                 goto out_session_cancel;
1842         }
1843
1844         packet = (dat[0] - 1 + session->pkt.dpo);
1845         if (packet > session->pkt.total ||
1846             (session->pkt.rx + 1) > session->pkt.total) {
1847                 netdev_info(priv->ndev, "%s: 0x%p: should have been completed\n",
1848                             __func__, session);
1849                 goto out_session_cancel;
1850         }
1851
1852         se_skb = j1939_session_skb_get_by_offset(session, packet * 7);
1853         if (!se_skb) {
1854                 netdev_warn(priv->ndev, "%s: 0x%p: no skb found\n", __func__,
1855                             session);
1856                 goto out_session_cancel;
1857         }
1858
1859         se_skcb = j1939_skb_to_cb(se_skb);
1860         offset = packet * 7 - se_skcb->offset;
1861         nbytes = se_skb->len - offset;
1862         if (nbytes > 7)
1863                 nbytes = 7;
1864         if (nbytes <= 0 || (nbytes + 1) > skb->len) {
1865                 netdev_info(priv->ndev, "%s: 0x%p: nbytes %i, len %i\n",
1866                             __func__, session, nbytes, skb->len);
1867                 goto out_session_cancel;
1868         }
1869
1870         tpdat = se_skb->data;
1871         if (!session->transmission) {
1872                 memcpy(&tpdat[offset], &dat[1], nbytes);
1873         } else {
1874                 int err;
1875
1876                 err = memcmp(&tpdat[offset], &dat[1], nbytes);
1877                 if (err)
1878                         netdev_err_once(priv->ndev,
1879                                         "%s: 0x%p: Data of RX-looped back packet (%*ph) doesn't match TX data (%*ph)!\n",
1880                                         __func__, session,
1881                                         nbytes, &dat[1],
1882                                         nbytes, &tpdat[offset]);
1883         }
1884
1885         if (packet == session->pkt.rx)
1886                 session->pkt.rx++;
1887
1888         if (se_skcb->addr.type != J1939_ETP &&
1889             j1939_cb_is_broadcast(&session->skcb)) {
1890                 if (session->pkt.rx >= session->pkt.total)
1891                         final = true;
1892                 else
1893                         remain = true;
1894         } else {
1895                 /* never final, an EOMA must follow */
1896                 if (session->pkt.rx >= session->pkt.last)
1897                         do_cts_eoma = true;
1898         }
1899
1900         if (final) {
1901                 j1939_session_timers_cancel(session);
1902                 j1939_session_completed(session);
1903         } else if (remain) {
1904                 if (!session->transmission)
1905                         j1939_tp_set_rxtimeout(session, 750);
1906         } else if (do_cts_eoma) {
1907                 j1939_tp_set_rxtimeout(session, 1250);
1908                 if (!session->transmission)
1909                         j1939_tp_schedule_txtimer(session, 0);
1910         } else {
1911                 j1939_tp_set_rxtimeout(session, 750);
1912         }
1913         session->last_cmd = 0xff;
1914         consume_skb(se_skb);
1915         j1939_session_put(session);
1916
1917         return;
1918
1919  out_session_cancel:
1920         kfree_skb(se_skb);
1921         j1939_session_timers_cancel(session);
1922         j1939_session_cancel(session, abort);
1923         j1939_session_put(session);
1924 }
1925
1926 static void j1939_xtp_rx_dat(struct j1939_priv *priv, struct sk_buff *skb)
1927 {
1928         struct j1939_sk_buff_cb *skcb;
1929         struct j1939_session *session;
1930
1931         skcb = j1939_skb_to_cb(skb);
1932
1933         if (j1939_tp_im_transmitter(skcb)) {
1934                 session = j1939_session_get_by_addr(priv, &skcb->addr, false,
1935                                                     true);
1936                 if (!session)
1937                         netdev_info(priv->ndev, "%s: no tx connection found\n",
1938                                     __func__);
1939                 else
1940                         j1939_xtp_rx_dat_one(session, skb);
1941         }
1942
1943         if (j1939_tp_im_receiver(skcb)) {
1944                 session = j1939_session_get_by_addr(priv, &skcb->addr, false,
1945                                                     false);
1946                 if (!session)
1947                         netdev_info(priv->ndev, "%s: no rx connection found\n",
1948                                     __func__);
1949                 else
1950                         j1939_xtp_rx_dat_one(session, skb);
1951         }
1952
1953         if (j1939_cb_is_broadcast(skcb)) {
1954                 session = j1939_session_get_by_addr(priv, &skcb->addr, false,
1955                                                     false);
1956                 if (session)
1957                         j1939_xtp_rx_dat_one(session, skb);
1958         }
1959 }
1960
1961 /* j1939 main intf */
1962 struct j1939_session *j1939_tp_send(struct j1939_priv *priv,
1963                                     struct sk_buff *skb, size_t size)
1964 {
1965         struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
1966         struct j1939_session *session;
1967         int ret;
1968
1969         if (skcb->addr.pgn == J1939_TP_PGN_DAT ||
1970             skcb->addr.pgn == J1939_TP_PGN_CTL ||
1971             skcb->addr.pgn == J1939_ETP_PGN_DAT ||
1972             skcb->addr.pgn == J1939_ETP_PGN_CTL)
1973                 /* avoid conflict */
1974                 return ERR_PTR(-EDOM);
1975
1976         if (size > priv->tp_max_packet_size)
1977                 return ERR_PTR(-EMSGSIZE);
1978
1979         if (size <= 8)
1980                 skcb->addr.type = J1939_SIMPLE;
1981         else if (size > J1939_MAX_TP_PACKET_SIZE)
1982                 skcb->addr.type = J1939_ETP;
1983         else
1984                 skcb->addr.type = J1939_TP;
1985
1986         if (skcb->addr.type == J1939_ETP &&
1987             j1939_cb_is_broadcast(skcb))
1988                 return ERR_PTR(-EDESTADDRREQ);
1989
1990         /* fill in addresses from names */
1991         ret = j1939_ac_fixup(priv, skb);
1992         if (unlikely(ret))
1993                 return ERR_PTR(ret);
1994
1995         /* fix DST flags, it may be used there soon */
1996         if (j1939_address_is_unicast(skcb->addr.da) &&
1997             priv->ents[skcb->addr.da].nusers)
1998                 skcb->flags |= J1939_ECU_LOCAL_DST;
1999
2000         /* src is always local, I'm sending ... */
2001         skcb->flags |= J1939_ECU_LOCAL_SRC;
2002
2003         /* prepare new session */
2004         session = j1939_session_new(priv, skb, size);
2005         if (!session)
2006                 return ERR_PTR(-ENOMEM);
2007
2008         /* skb is recounted in j1939_session_new() */
2009         sock_hold(skb->sk);
2010         session->sk = skb->sk;
2011         session->transmission = true;
2012         session->pkt.total = (size + 6) / 7;
2013         session->pkt.block = skcb->addr.type == J1939_ETP ? 255 :
2014                 min(j1939_tp_block ?: 255, session->pkt.total);
2015
2016         if (j1939_cb_is_broadcast(&session->skcb))
2017                 /* set the end-packet for broadcast */
2018                 session->pkt.last = session->pkt.total;
2019
2020         skcb->tskey = atomic_inc_return(&session->sk->sk_tskey) - 1;
2021         session->tskey = skcb->tskey;
2022
2023         return session;
2024 }
2025
2026 static void j1939_tp_cmd_recv(struct j1939_priv *priv, struct sk_buff *skb)
2027 {
2028         struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
2029         int extd = J1939_TP;
2030         u8 cmd = skb->data[0];
2031
2032         switch (cmd) {
2033         case J1939_ETP_CMD_RTS:
2034                 extd = J1939_ETP;
2035                 fallthrough;
2036         case J1939_TP_CMD_BAM:
2037                 if (cmd == J1939_TP_CMD_BAM && !j1939_cb_is_broadcast(skcb)) {
2038                         netdev_err_once(priv->ndev, "%s: BAM to unicast (%02x), ignoring!\n",
2039                                         __func__, skcb->addr.sa);
2040                         return;
2041                 }
2042                 fallthrough;
2043         case J1939_TP_CMD_RTS:
2044                 if (skcb->addr.type != extd)
2045                         return;
2046
2047                 if (cmd == J1939_TP_CMD_RTS && j1939_cb_is_broadcast(skcb)) {
2048                         netdev_alert(priv->ndev, "%s: rts without destination (%02x)\n",
2049                                      __func__, skcb->addr.sa);
2050                         return;
2051                 }
2052
2053                 if (j1939_tp_im_transmitter(skcb))
2054                         j1939_xtp_rx_rts(priv, skb, true);
2055
2056                 if (j1939_tp_im_receiver(skcb) || j1939_cb_is_broadcast(skcb))
2057                         j1939_xtp_rx_rts(priv, skb, false);
2058
2059                 break;
2060
2061         case J1939_ETP_CMD_CTS:
2062                 extd = J1939_ETP;
2063                 fallthrough;
2064         case J1939_TP_CMD_CTS:
2065                 if (skcb->addr.type != extd)
2066                         return;
2067
2068                 if (j1939_tp_im_transmitter(skcb))
2069                         j1939_xtp_rx_cts(priv, skb, false);
2070
2071                 if (j1939_tp_im_receiver(skcb))
2072                         j1939_xtp_rx_cts(priv, skb, true);
2073
2074                 break;
2075
2076         case J1939_ETP_CMD_DPO:
2077                 if (skcb->addr.type != J1939_ETP)
2078                         return;
2079
2080                 if (j1939_tp_im_transmitter(skcb))
2081                         j1939_xtp_rx_dpo(priv, skb, true);
2082
2083                 if (j1939_tp_im_receiver(skcb))
2084                         j1939_xtp_rx_dpo(priv, skb, false);
2085
2086                 break;
2087
2088         case J1939_ETP_CMD_EOMA:
2089                 extd = J1939_ETP;
2090                 fallthrough;
2091         case J1939_TP_CMD_EOMA:
2092                 if (skcb->addr.type != extd)
2093                         return;
2094
2095                 if (j1939_tp_im_transmitter(skcb))
2096                         j1939_xtp_rx_eoma(priv, skb, false);
2097
2098                 if (j1939_tp_im_receiver(skcb))
2099                         j1939_xtp_rx_eoma(priv, skb, true);
2100
2101                 break;
2102
2103         case J1939_ETP_CMD_ABORT: /* && J1939_TP_CMD_ABORT */
2104                 if (j1939_cb_is_broadcast(skcb)) {
2105                         netdev_err_once(priv->ndev, "%s: abort to broadcast (%02x), ignoring!\n",
2106                                         __func__, skcb->addr.sa);
2107                         return;
2108                 }
2109
2110                 if (j1939_tp_im_transmitter(skcb))
2111                         j1939_xtp_rx_abort(priv, skb, true);
2112
2113                 if (j1939_tp_im_receiver(skcb))
2114                         j1939_xtp_rx_abort(priv, skb, false);
2115
2116                 break;
2117         default:
2118                 return;
2119         }
2120 }
2121
2122 int j1939_tp_recv(struct j1939_priv *priv, struct sk_buff *skb)
2123 {
2124         struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
2125
2126         if (!j1939_tp_im_involved_anydir(skcb) && !j1939_cb_is_broadcast(skcb))
2127                 return 0;
2128
2129         switch (skcb->addr.pgn) {
2130         case J1939_ETP_PGN_DAT:
2131                 skcb->addr.type = J1939_ETP;
2132                 fallthrough;
2133         case J1939_TP_PGN_DAT:
2134                 j1939_xtp_rx_dat(priv, skb);
2135                 break;
2136
2137         case J1939_ETP_PGN_CTL:
2138                 skcb->addr.type = J1939_ETP;
2139                 fallthrough;
2140         case J1939_TP_PGN_CTL:
2141                 if (skb->len < 8)
2142                         return 0; /* Don't care. Nothing to extract here */
2143
2144                 j1939_tp_cmd_recv(priv, skb);
2145                 break;
2146         default:
2147                 return 0; /* no problem */
2148         }
2149         return 1; /* "I processed the message" */
2150 }
2151
2152 void j1939_simple_recv(struct j1939_priv *priv, struct sk_buff *skb)
2153 {
2154         struct j1939_session *session;
2155
2156         if (!skb->sk)
2157                 return;
2158
2159         if (skb->sk->sk_family != AF_CAN ||
2160             skb->sk->sk_protocol != CAN_J1939)
2161                 return;
2162
2163         j1939_session_list_lock(priv);
2164         session = j1939_session_get_simple(priv, skb);
2165         j1939_session_list_unlock(priv);
2166         if (!session) {
2167                 netdev_warn(priv->ndev,
2168                             "%s: Received already invalidated message\n",
2169                             __func__);
2170                 return;
2171         }
2172
2173         j1939_session_timers_cancel(session);
2174         j1939_session_deactivate(session);
2175         j1939_session_put(session);
2176 }
2177
2178 int j1939_cancel_active_session(struct j1939_priv *priv, struct sock *sk)
2179 {
2180         struct j1939_session *session, *saved;
2181
2182         netdev_dbg(priv->ndev, "%s, sk: %p\n", __func__, sk);
2183         j1939_session_list_lock(priv);
2184         list_for_each_entry_safe(session, saved,
2185                                  &priv->active_session_list,
2186                                  active_session_list_entry) {
2187                 if (!sk || sk == session->sk) {
2188                         if (hrtimer_try_to_cancel(&session->txtimer) == 1)
2189                                 j1939_session_put(session);
2190                         if (hrtimer_try_to_cancel(&session->rxtimer) == 1)
2191                                 j1939_session_put(session);
2192
2193                         session->err = ESHUTDOWN;
2194                         j1939_session_deactivate_locked(session);
2195                 }
2196         }
2197         j1939_session_list_unlock(priv);
2198         return NOTIFY_DONE;
2199 }
2200
2201 void j1939_tp_init(struct j1939_priv *priv)
2202 {
2203         spin_lock_init(&priv->active_session_list_lock);
2204         INIT_LIST_HEAD(&priv->active_session_list);
2205         priv->tp_max_packet_size = J1939_MAX_ETP_PACKET_SIZE;
2206 }