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