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