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