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