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