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