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