transport.c 55 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206
  1. // SPDX-License-Identifier: GPL-2.0
  2. // Copyright (c) 2010-2011 EIA Electronics,
  3. // Kurt Van Dijck <[email protected]>
  4. // Copyright (c) 2018 Protonic,
  5. // Robin van der Gracht <[email protected]>
  6. // Copyright (c) 2017-2019 Pengutronix,
  7. // Marc Kleine-Budde <[email protected]>
  8. // Copyright (c) 2017-2019 Pengutronix,
  9. // Oleksij Rempel <[email protected]>
  10. #include <linux/can/skb.h>
  11. #include "j1939-priv.h"
  12. #define J1939_XTP_TX_RETRY_LIMIT 100
  13. #define J1939_ETP_PGN_CTL 0xc800
  14. #define J1939_ETP_PGN_DAT 0xc700
  15. #define J1939_TP_PGN_CTL 0xec00
  16. #define J1939_TP_PGN_DAT 0xeb00
  17. #define J1939_TP_CMD_RTS 0x10
  18. #define J1939_TP_CMD_CTS 0x11
  19. #define J1939_TP_CMD_EOMA 0x13
  20. #define J1939_TP_CMD_BAM 0x20
  21. #define J1939_TP_CMD_ABORT 0xff
  22. #define J1939_ETP_CMD_RTS 0x14
  23. #define J1939_ETP_CMD_CTS 0x15
  24. #define J1939_ETP_CMD_DPO 0x16
  25. #define J1939_ETP_CMD_EOMA 0x17
  26. #define J1939_ETP_CMD_ABORT 0xff
  27. enum j1939_xtp_abort {
  28. J1939_XTP_NO_ABORT = 0,
  29. J1939_XTP_ABORT_BUSY = 1,
  30. /* Already in one or more connection managed sessions and
  31. * cannot support another.
  32. *
  33. * EALREADY:
  34. * Operation already in progress
  35. */
  36. J1939_XTP_ABORT_RESOURCE = 2,
  37. /* System resources were needed for another task so this
  38. * connection managed session was terminated.
  39. *
  40. * EMSGSIZE:
  41. * The socket type requires that message be sent atomically,
  42. * and the size of the message to be sent made this
  43. * impossible.
  44. */
  45. J1939_XTP_ABORT_TIMEOUT = 3,
  46. /* A timeout occurred and this is the connection abort to
  47. * close the session.
  48. *
  49. * EHOSTUNREACH:
  50. * The destination host cannot be reached (probably because
  51. * the host is down or a remote router cannot reach it).
  52. */
  53. J1939_XTP_ABORT_GENERIC = 4,
  54. /* CTS messages received when data transfer is in progress
  55. *
  56. * EBADMSG:
  57. * Not a data message
  58. */
  59. J1939_XTP_ABORT_FAULT = 5,
  60. /* Maximal retransmit request limit reached
  61. *
  62. * ENOTRECOVERABLE:
  63. * State not recoverable
  64. */
  65. J1939_XTP_ABORT_UNEXPECTED_DATA = 6,
  66. /* Unexpected data transfer packet
  67. *
  68. * ENOTCONN:
  69. * Transport endpoint is not connected
  70. */
  71. J1939_XTP_ABORT_BAD_SEQ = 7,
  72. /* Bad sequence number (and software is not able to recover)
  73. *
  74. * EILSEQ:
  75. * Illegal byte sequence
  76. */
  77. J1939_XTP_ABORT_DUP_SEQ = 8,
  78. /* Duplicate sequence number (and software is not able to
  79. * recover)
  80. */
  81. J1939_XTP_ABORT_EDPO_UNEXPECTED = 9,
  82. /* Unexpected EDPO packet (ETP) or Message size > 1785 bytes
  83. * (TP)
  84. */
  85. J1939_XTP_ABORT_BAD_EDPO_PGN = 10,
  86. /* Unexpected EDPO PGN (PGN in EDPO is bad) */
  87. J1939_XTP_ABORT_EDPO_OUTOF_CTS = 11,
  88. /* EDPO number of packets is greater than CTS */
  89. J1939_XTP_ABORT_BAD_EDPO_OFFSET = 12,
  90. /* Bad EDPO offset */
  91. J1939_XTP_ABORT_OTHER_DEPRECATED = 13,
  92. /* Deprecated. Use 250 instead (Any other reason) */
  93. J1939_XTP_ABORT_ECTS_UNXPECTED_PGN = 14,
  94. /* Unexpected ECTS PGN (PGN in ECTS is bad) */
  95. J1939_XTP_ABORT_ECTS_TOO_BIG = 15,
  96. /* ECTS requested packets exceeds message size */
  97. J1939_XTP_ABORT_OTHER = 250,
  98. /* Any other reason (if a Connection Abort reason is
  99. * identified that is not listed in the table use code 250)
  100. */
  101. };
  102. static unsigned int j1939_tp_block = 255;
  103. static unsigned int j1939_tp_packet_delay;
  104. static unsigned int j1939_tp_padding = 1;
  105. /* helpers */
  106. static const char *j1939_xtp_abort_to_str(enum j1939_xtp_abort abort)
  107. {
  108. switch (abort) {
  109. case J1939_XTP_ABORT_BUSY:
  110. return "Already in one or more connection managed sessions and cannot support another.";
  111. case J1939_XTP_ABORT_RESOURCE:
  112. return "System resources were needed for another task so this connection managed session was terminated.";
  113. case J1939_XTP_ABORT_TIMEOUT:
  114. return "A timeout occurred and this is the connection abort to close the session.";
  115. case J1939_XTP_ABORT_GENERIC:
  116. return "CTS messages received when data transfer is in progress";
  117. case J1939_XTP_ABORT_FAULT:
  118. return "Maximal retransmit request limit reached";
  119. case J1939_XTP_ABORT_UNEXPECTED_DATA:
  120. return "Unexpected data transfer packet";
  121. case J1939_XTP_ABORT_BAD_SEQ:
  122. return "Bad sequence number (and software is not able to recover)";
  123. case J1939_XTP_ABORT_DUP_SEQ:
  124. return "Duplicate sequence number (and software is not able to recover)";
  125. case J1939_XTP_ABORT_EDPO_UNEXPECTED:
  126. return "Unexpected EDPO packet (ETP) or Message size > 1785 bytes (TP)";
  127. case J1939_XTP_ABORT_BAD_EDPO_PGN:
  128. return "Unexpected EDPO PGN (PGN in EDPO is bad)";
  129. case J1939_XTP_ABORT_EDPO_OUTOF_CTS:
  130. return "EDPO number of packets is greater than CTS";
  131. case J1939_XTP_ABORT_BAD_EDPO_OFFSET:
  132. return "Bad EDPO offset";
  133. case J1939_XTP_ABORT_OTHER_DEPRECATED:
  134. return "Deprecated. Use 250 instead (Any other reason)";
  135. case J1939_XTP_ABORT_ECTS_UNXPECTED_PGN:
  136. return "Unexpected ECTS PGN (PGN in ECTS is bad)";
  137. case J1939_XTP_ABORT_ECTS_TOO_BIG:
  138. return "ECTS requested packets exceeds message size";
  139. case J1939_XTP_ABORT_OTHER:
  140. return "Any other reason (if a Connection Abort reason is identified that is not listed in the table use code 250)";
  141. default:
  142. return "<unknown>";
  143. }
  144. }
  145. static int j1939_xtp_abort_to_errno(struct j1939_priv *priv,
  146. enum j1939_xtp_abort abort)
  147. {
  148. int err;
  149. switch (abort) {
  150. case J1939_XTP_NO_ABORT:
  151. WARN_ON_ONCE(abort == J1939_XTP_NO_ABORT);
  152. err = 0;
  153. break;
  154. case J1939_XTP_ABORT_BUSY:
  155. err = EALREADY;
  156. break;
  157. case J1939_XTP_ABORT_RESOURCE:
  158. err = EMSGSIZE;
  159. break;
  160. case J1939_XTP_ABORT_TIMEOUT:
  161. err = EHOSTUNREACH;
  162. break;
  163. case J1939_XTP_ABORT_GENERIC:
  164. err = EBADMSG;
  165. break;
  166. case J1939_XTP_ABORT_FAULT:
  167. err = ENOTRECOVERABLE;
  168. break;
  169. case J1939_XTP_ABORT_UNEXPECTED_DATA:
  170. err = ENOTCONN;
  171. break;
  172. case J1939_XTP_ABORT_BAD_SEQ:
  173. err = EILSEQ;
  174. break;
  175. case J1939_XTP_ABORT_DUP_SEQ:
  176. err = EPROTO;
  177. break;
  178. case J1939_XTP_ABORT_EDPO_UNEXPECTED:
  179. err = EPROTO;
  180. break;
  181. case J1939_XTP_ABORT_BAD_EDPO_PGN:
  182. err = EPROTO;
  183. break;
  184. case J1939_XTP_ABORT_EDPO_OUTOF_CTS:
  185. err = EPROTO;
  186. break;
  187. case J1939_XTP_ABORT_BAD_EDPO_OFFSET:
  188. err = EPROTO;
  189. break;
  190. case J1939_XTP_ABORT_OTHER_DEPRECATED:
  191. err = EPROTO;
  192. break;
  193. case J1939_XTP_ABORT_ECTS_UNXPECTED_PGN:
  194. err = EPROTO;
  195. break;
  196. case J1939_XTP_ABORT_ECTS_TOO_BIG:
  197. err = EPROTO;
  198. break;
  199. case J1939_XTP_ABORT_OTHER:
  200. err = EPROTO;
  201. break;
  202. default:
  203. netdev_warn(priv->ndev, "Unknown abort code %i", abort);
  204. err = EPROTO;
  205. }
  206. return err;
  207. }
  208. static inline void j1939_session_list_lock(struct j1939_priv *priv)
  209. {
  210. spin_lock_bh(&priv->active_session_list_lock);
  211. }
  212. static inline void j1939_session_list_unlock(struct j1939_priv *priv)
  213. {
  214. spin_unlock_bh(&priv->active_session_list_lock);
  215. }
  216. void j1939_session_get(struct j1939_session *session)
  217. {
  218. kref_get(&session->kref);
  219. }
  220. /* session completion functions */
  221. static void __j1939_session_drop(struct j1939_session *session)
  222. {
  223. if (!session->transmission)
  224. return;
  225. j1939_sock_pending_del(session->sk);
  226. sock_put(session->sk);
  227. }
  228. static void j1939_session_destroy(struct j1939_session *session)
  229. {
  230. struct sk_buff *skb;
  231. if (session->transmission) {
  232. if (session->err)
  233. j1939_sk_errqueue(session, J1939_ERRQUEUE_TX_ABORT);
  234. else
  235. j1939_sk_errqueue(session, J1939_ERRQUEUE_TX_ACK);
  236. } else if (session->err) {
  237. j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_ABORT);
  238. }
  239. netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
  240. WARN_ON_ONCE(!list_empty(&session->sk_session_queue_entry));
  241. WARN_ON_ONCE(!list_empty(&session->active_session_list_entry));
  242. while ((skb = skb_dequeue(&session->skb_queue)) != NULL) {
  243. /* drop ref taken in j1939_session_skb_queue() */
  244. skb_unref(skb);
  245. kfree_skb(skb);
  246. }
  247. __j1939_session_drop(session);
  248. j1939_priv_put(session->priv);
  249. kfree(session);
  250. }
  251. static void __j1939_session_release(struct kref *kref)
  252. {
  253. struct j1939_session *session = container_of(kref, struct j1939_session,
  254. kref);
  255. j1939_session_destroy(session);
  256. }
  257. void j1939_session_put(struct j1939_session *session)
  258. {
  259. kref_put(&session->kref, __j1939_session_release);
  260. }
  261. static void j1939_session_txtimer_cancel(struct j1939_session *session)
  262. {
  263. if (hrtimer_cancel(&session->txtimer))
  264. j1939_session_put(session);
  265. }
  266. static void j1939_session_rxtimer_cancel(struct j1939_session *session)
  267. {
  268. if (hrtimer_cancel(&session->rxtimer))
  269. j1939_session_put(session);
  270. }
  271. void j1939_session_timers_cancel(struct j1939_session *session)
  272. {
  273. j1939_session_txtimer_cancel(session);
  274. j1939_session_rxtimer_cancel(session);
  275. }
  276. static inline bool j1939_cb_is_broadcast(const struct j1939_sk_buff_cb *skcb)
  277. {
  278. return (!skcb->addr.dst_name && (skcb->addr.da == 0xff));
  279. }
  280. static void j1939_session_skb_drop_old(struct j1939_session *session)
  281. {
  282. struct sk_buff *do_skb;
  283. struct j1939_sk_buff_cb *do_skcb;
  284. unsigned int offset_start;
  285. unsigned long flags;
  286. if (skb_queue_len(&session->skb_queue) < 2)
  287. return;
  288. offset_start = session->pkt.tx_acked * 7;
  289. spin_lock_irqsave(&session->skb_queue.lock, flags);
  290. do_skb = skb_peek(&session->skb_queue);
  291. do_skcb = j1939_skb_to_cb(do_skb);
  292. if ((do_skcb->offset + do_skb->len) < offset_start) {
  293. __skb_unlink(do_skb, &session->skb_queue);
  294. /* drop ref taken in j1939_session_skb_queue() */
  295. skb_unref(do_skb);
  296. spin_unlock_irqrestore(&session->skb_queue.lock, flags);
  297. kfree_skb(do_skb);
  298. } else {
  299. spin_unlock_irqrestore(&session->skb_queue.lock, flags);
  300. }
  301. }
  302. void j1939_session_skb_queue(struct j1939_session *session,
  303. struct sk_buff *skb)
  304. {
  305. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  306. struct j1939_priv *priv = session->priv;
  307. j1939_ac_fixup(priv, skb);
  308. if (j1939_address_is_unicast(skcb->addr.da) &&
  309. priv->ents[skcb->addr.da].nusers)
  310. skcb->flags |= J1939_ECU_LOCAL_DST;
  311. skcb->flags |= J1939_ECU_LOCAL_SRC;
  312. skb_get(skb);
  313. skb_queue_tail(&session->skb_queue, skb);
  314. }
  315. static struct
  316. sk_buff *j1939_session_skb_get_by_offset(struct j1939_session *session,
  317. unsigned int offset_start)
  318. {
  319. struct j1939_priv *priv = session->priv;
  320. struct j1939_sk_buff_cb *do_skcb;
  321. struct sk_buff *skb = NULL;
  322. struct sk_buff *do_skb;
  323. unsigned long flags;
  324. spin_lock_irqsave(&session->skb_queue.lock, flags);
  325. skb_queue_walk(&session->skb_queue, do_skb) {
  326. do_skcb = j1939_skb_to_cb(do_skb);
  327. if (offset_start >= do_skcb->offset &&
  328. offset_start < (do_skcb->offset + do_skb->len)) {
  329. skb = do_skb;
  330. }
  331. }
  332. if (skb)
  333. skb_get(skb);
  334. spin_unlock_irqrestore(&session->skb_queue.lock, flags);
  335. if (!skb)
  336. netdev_dbg(priv->ndev, "%s: 0x%p: no skb found for start: %i, queue size: %i\n",
  337. __func__, session, offset_start,
  338. skb_queue_len(&session->skb_queue));
  339. return skb;
  340. }
  341. static struct sk_buff *j1939_session_skb_get(struct j1939_session *session)
  342. {
  343. unsigned int offset_start;
  344. offset_start = session->pkt.dpo * 7;
  345. return j1939_session_skb_get_by_offset(session, offset_start);
  346. }
  347. /* see if we are receiver
  348. * returns 0 for broadcasts, although we will receive them
  349. */
  350. static inline int j1939_tp_im_receiver(const struct j1939_sk_buff_cb *skcb)
  351. {
  352. return skcb->flags & J1939_ECU_LOCAL_DST;
  353. }
  354. /* see if we are sender */
  355. static inline int j1939_tp_im_transmitter(const struct j1939_sk_buff_cb *skcb)
  356. {
  357. return skcb->flags & J1939_ECU_LOCAL_SRC;
  358. }
  359. /* see if we are involved as either receiver or transmitter */
  360. static int j1939_tp_im_involved(const struct j1939_sk_buff_cb *skcb, bool swap)
  361. {
  362. if (swap)
  363. return j1939_tp_im_receiver(skcb);
  364. else
  365. return j1939_tp_im_transmitter(skcb);
  366. }
  367. static int j1939_tp_im_involved_anydir(struct j1939_sk_buff_cb *skcb)
  368. {
  369. return skcb->flags & (J1939_ECU_LOCAL_SRC | J1939_ECU_LOCAL_DST);
  370. }
  371. /* extract pgn from flow-ctl message */
  372. static inline pgn_t j1939_xtp_ctl_to_pgn(const u8 *dat)
  373. {
  374. pgn_t pgn;
  375. pgn = (dat[7] << 16) | (dat[6] << 8) | (dat[5] << 0);
  376. if (j1939_pgn_is_pdu1(pgn))
  377. pgn &= 0xffff00;
  378. return pgn;
  379. }
  380. static inline unsigned int j1939_tp_ctl_to_size(const u8 *dat)
  381. {
  382. return (dat[2] << 8) + (dat[1] << 0);
  383. }
  384. static inline unsigned int j1939_etp_ctl_to_packet(const u8 *dat)
  385. {
  386. return (dat[4] << 16) | (dat[3] << 8) | (dat[2] << 0);
  387. }
  388. static inline unsigned int j1939_etp_ctl_to_size(const u8 *dat)
  389. {
  390. return (dat[4] << 24) | (dat[3] << 16) |
  391. (dat[2] << 8) | (dat[1] << 0);
  392. }
  393. /* find existing session:
  394. * reverse: swap cb's src & dst
  395. * there is no problem with matching broadcasts, since
  396. * broadcasts (no dst, no da) would never call this
  397. * with reverse == true
  398. */
  399. static bool j1939_session_match(struct j1939_addr *se_addr,
  400. struct j1939_addr *sk_addr, bool reverse)
  401. {
  402. if (se_addr->type != sk_addr->type)
  403. return false;
  404. if (reverse) {
  405. if (se_addr->src_name) {
  406. if (se_addr->src_name != sk_addr->dst_name)
  407. return false;
  408. } else if (se_addr->sa != sk_addr->da) {
  409. return false;
  410. }
  411. if (se_addr->dst_name) {
  412. if (se_addr->dst_name != sk_addr->src_name)
  413. return false;
  414. } else if (se_addr->da != sk_addr->sa) {
  415. return false;
  416. }
  417. } else {
  418. if (se_addr->src_name) {
  419. if (se_addr->src_name != sk_addr->src_name)
  420. return false;
  421. } else if (se_addr->sa != sk_addr->sa) {
  422. return false;
  423. }
  424. if (se_addr->dst_name) {
  425. if (se_addr->dst_name != sk_addr->dst_name)
  426. return false;
  427. } else if (se_addr->da != sk_addr->da) {
  428. return false;
  429. }
  430. }
  431. return true;
  432. }
  433. static struct
  434. j1939_session *j1939_session_get_by_addr_locked(struct j1939_priv *priv,
  435. struct list_head *root,
  436. struct j1939_addr *addr,
  437. bool reverse, bool transmitter)
  438. {
  439. struct j1939_session *session;
  440. lockdep_assert_held(&priv->active_session_list_lock);
  441. list_for_each_entry(session, root, active_session_list_entry) {
  442. j1939_session_get(session);
  443. if (j1939_session_match(&session->skcb.addr, addr, reverse) &&
  444. session->transmission == transmitter)
  445. return session;
  446. j1939_session_put(session);
  447. }
  448. return NULL;
  449. }
  450. static struct
  451. j1939_session *j1939_session_get_simple(struct j1939_priv *priv,
  452. struct sk_buff *skb)
  453. {
  454. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  455. struct j1939_session *session;
  456. lockdep_assert_held(&priv->active_session_list_lock);
  457. list_for_each_entry(session, &priv->active_session_list,
  458. active_session_list_entry) {
  459. j1939_session_get(session);
  460. if (session->skcb.addr.type == J1939_SIMPLE &&
  461. session->tskey == skcb->tskey && session->sk == skb->sk)
  462. return session;
  463. j1939_session_put(session);
  464. }
  465. return NULL;
  466. }
  467. static struct
  468. j1939_session *j1939_session_get_by_addr(struct j1939_priv *priv,
  469. struct j1939_addr *addr,
  470. bool reverse, bool transmitter)
  471. {
  472. struct j1939_session *session;
  473. j1939_session_list_lock(priv);
  474. session = j1939_session_get_by_addr_locked(priv,
  475. &priv->active_session_list,
  476. addr, reverse, transmitter);
  477. j1939_session_list_unlock(priv);
  478. return session;
  479. }
  480. static void j1939_skbcb_swap(struct j1939_sk_buff_cb *skcb)
  481. {
  482. u8 tmp = 0;
  483. swap(skcb->addr.dst_name, skcb->addr.src_name);
  484. swap(skcb->addr.da, skcb->addr.sa);
  485. /* swap SRC and DST flags, leave other untouched */
  486. if (skcb->flags & J1939_ECU_LOCAL_SRC)
  487. tmp |= J1939_ECU_LOCAL_DST;
  488. if (skcb->flags & J1939_ECU_LOCAL_DST)
  489. tmp |= J1939_ECU_LOCAL_SRC;
  490. skcb->flags &= ~(J1939_ECU_LOCAL_SRC | J1939_ECU_LOCAL_DST);
  491. skcb->flags |= tmp;
  492. }
  493. static struct
  494. sk_buff *j1939_tp_tx_dat_new(struct j1939_priv *priv,
  495. const struct j1939_sk_buff_cb *re_skcb,
  496. bool ctl,
  497. bool swap_src_dst)
  498. {
  499. struct sk_buff *skb;
  500. struct j1939_sk_buff_cb *skcb;
  501. skb = alloc_skb(sizeof(struct can_frame) + sizeof(struct can_skb_priv),
  502. GFP_ATOMIC);
  503. if (unlikely(!skb))
  504. return ERR_PTR(-ENOMEM);
  505. skb->dev = priv->ndev;
  506. can_skb_reserve(skb);
  507. can_skb_prv(skb)->ifindex = priv->ndev->ifindex;
  508. can_skb_prv(skb)->skbcnt = 0;
  509. /* reserve CAN header */
  510. skb_reserve(skb, offsetof(struct can_frame, data));
  511. /* skb->cb must be large enough to hold a j1939_sk_buff_cb structure */
  512. BUILD_BUG_ON(sizeof(skb->cb) < sizeof(*re_skcb));
  513. memcpy(skb->cb, re_skcb, sizeof(*re_skcb));
  514. skcb = j1939_skb_to_cb(skb);
  515. if (swap_src_dst)
  516. j1939_skbcb_swap(skcb);
  517. if (ctl) {
  518. if (skcb->addr.type == J1939_ETP)
  519. skcb->addr.pgn = J1939_ETP_PGN_CTL;
  520. else
  521. skcb->addr.pgn = J1939_TP_PGN_CTL;
  522. } else {
  523. if (skcb->addr.type == J1939_ETP)
  524. skcb->addr.pgn = J1939_ETP_PGN_DAT;
  525. else
  526. skcb->addr.pgn = J1939_TP_PGN_DAT;
  527. }
  528. return skb;
  529. }
  530. /* TP transmit packet functions */
  531. static int j1939_tp_tx_dat(struct j1939_session *session,
  532. const u8 *dat, int len)
  533. {
  534. struct j1939_priv *priv = session->priv;
  535. struct sk_buff *skb;
  536. skb = j1939_tp_tx_dat_new(priv, &session->skcb,
  537. false, false);
  538. if (IS_ERR(skb))
  539. return PTR_ERR(skb);
  540. skb_put_data(skb, dat, len);
  541. if (j1939_tp_padding && len < 8)
  542. memset(skb_put(skb, 8 - len), 0xff, 8 - len);
  543. return j1939_send_one(priv, skb);
  544. }
  545. static int j1939_xtp_do_tx_ctl(struct j1939_priv *priv,
  546. const struct j1939_sk_buff_cb *re_skcb,
  547. bool swap_src_dst, pgn_t pgn, const u8 *dat)
  548. {
  549. struct sk_buff *skb;
  550. u8 *skdat;
  551. if (!j1939_tp_im_involved(re_skcb, swap_src_dst))
  552. return 0;
  553. skb = j1939_tp_tx_dat_new(priv, re_skcb, true, swap_src_dst);
  554. if (IS_ERR(skb))
  555. return PTR_ERR(skb);
  556. skdat = skb_put(skb, 8);
  557. memcpy(skdat, dat, 5);
  558. skdat[5] = (pgn >> 0);
  559. skdat[6] = (pgn >> 8);
  560. skdat[7] = (pgn >> 16);
  561. return j1939_send_one(priv, skb);
  562. }
  563. static inline int j1939_tp_tx_ctl(struct j1939_session *session,
  564. bool swap_src_dst, const u8 *dat)
  565. {
  566. struct j1939_priv *priv = session->priv;
  567. return j1939_xtp_do_tx_ctl(priv, &session->skcb,
  568. swap_src_dst,
  569. session->skcb.addr.pgn, dat);
  570. }
  571. static int j1939_xtp_tx_abort(struct j1939_priv *priv,
  572. const struct j1939_sk_buff_cb *re_skcb,
  573. bool swap_src_dst,
  574. enum j1939_xtp_abort err,
  575. pgn_t pgn)
  576. {
  577. u8 dat[5];
  578. if (!j1939_tp_im_involved(re_skcb, swap_src_dst))
  579. return 0;
  580. memset(dat, 0xff, sizeof(dat));
  581. dat[0] = J1939_TP_CMD_ABORT;
  582. dat[1] = err;
  583. return j1939_xtp_do_tx_ctl(priv, re_skcb, swap_src_dst, pgn, dat);
  584. }
  585. void j1939_tp_schedule_txtimer(struct j1939_session *session, int msec)
  586. {
  587. j1939_session_get(session);
  588. hrtimer_start(&session->txtimer, ms_to_ktime(msec),
  589. HRTIMER_MODE_REL_SOFT);
  590. }
  591. static inline void j1939_tp_set_rxtimeout(struct j1939_session *session,
  592. int msec)
  593. {
  594. j1939_session_rxtimer_cancel(session);
  595. j1939_session_get(session);
  596. hrtimer_start(&session->rxtimer, ms_to_ktime(msec),
  597. HRTIMER_MODE_REL_SOFT);
  598. }
  599. static int j1939_session_tx_rts(struct j1939_session *session)
  600. {
  601. u8 dat[8];
  602. int ret;
  603. memset(dat, 0xff, sizeof(dat));
  604. dat[1] = (session->total_message_size >> 0);
  605. dat[2] = (session->total_message_size >> 8);
  606. dat[3] = session->pkt.total;
  607. if (session->skcb.addr.type == J1939_ETP) {
  608. dat[0] = J1939_ETP_CMD_RTS;
  609. dat[1] = (session->total_message_size >> 0);
  610. dat[2] = (session->total_message_size >> 8);
  611. dat[3] = (session->total_message_size >> 16);
  612. dat[4] = (session->total_message_size >> 24);
  613. } else if (j1939_cb_is_broadcast(&session->skcb)) {
  614. dat[0] = J1939_TP_CMD_BAM;
  615. /* fake cts for broadcast */
  616. session->pkt.tx = 0;
  617. } else {
  618. dat[0] = J1939_TP_CMD_RTS;
  619. dat[4] = dat[3];
  620. }
  621. if (dat[0] == session->last_txcmd)
  622. /* done already */
  623. return 0;
  624. ret = j1939_tp_tx_ctl(session, false, dat);
  625. if (ret < 0)
  626. return ret;
  627. session->last_txcmd = dat[0];
  628. if (dat[0] == J1939_TP_CMD_BAM) {
  629. j1939_tp_schedule_txtimer(session, 50);
  630. j1939_tp_set_rxtimeout(session, 250);
  631. } else {
  632. j1939_tp_set_rxtimeout(session, 1250);
  633. }
  634. netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
  635. return 0;
  636. }
  637. static int j1939_session_tx_dpo(struct j1939_session *session)
  638. {
  639. unsigned int pkt;
  640. u8 dat[8];
  641. int ret;
  642. memset(dat, 0xff, sizeof(dat));
  643. dat[0] = J1939_ETP_CMD_DPO;
  644. session->pkt.dpo = session->pkt.tx_acked;
  645. pkt = session->pkt.dpo;
  646. dat[1] = session->pkt.last - session->pkt.tx_acked;
  647. dat[2] = (pkt >> 0);
  648. dat[3] = (pkt >> 8);
  649. dat[4] = (pkt >> 16);
  650. ret = j1939_tp_tx_ctl(session, false, dat);
  651. if (ret < 0)
  652. return ret;
  653. session->last_txcmd = dat[0];
  654. j1939_tp_set_rxtimeout(session, 1250);
  655. session->pkt.tx = session->pkt.tx_acked;
  656. netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
  657. return 0;
  658. }
  659. static int j1939_session_tx_dat(struct j1939_session *session)
  660. {
  661. struct j1939_priv *priv = session->priv;
  662. struct j1939_sk_buff_cb *se_skcb;
  663. int offset, pkt_done, pkt_end;
  664. unsigned int len, pdelay;
  665. struct sk_buff *se_skb;
  666. const u8 *tpdat;
  667. int ret = 0;
  668. u8 dat[8];
  669. se_skb = j1939_session_skb_get_by_offset(session, session->pkt.tx * 7);
  670. if (!se_skb)
  671. return -ENOBUFS;
  672. se_skcb = j1939_skb_to_cb(se_skb);
  673. tpdat = se_skb->data;
  674. ret = 0;
  675. pkt_done = 0;
  676. if (session->skcb.addr.type != J1939_ETP &&
  677. j1939_cb_is_broadcast(&session->skcb))
  678. pkt_end = session->pkt.total;
  679. else
  680. pkt_end = session->pkt.last;
  681. while (session->pkt.tx < pkt_end) {
  682. dat[0] = session->pkt.tx - session->pkt.dpo + 1;
  683. offset = (session->pkt.tx * 7) - se_skcb->offset;
  684. len = se_skb->len - offset;
  685. if (len > 7)
  686. len = 7;
  687. if (offset + len > se_skb->len) {
  688. netdev_err_once(priv->ndev,
  689. "%s: 0x%p: requested data outside of queued buffer: offset %i, len %i, pkt.tx: %i\n",
  690. __func__, session, se_skcb->offset,
  691. se_skb->len , session->pkt.tx);
  692. ret = -EOVERFLOW;
  693. goto out_free;
  694. }
  695. if (!len) {
  696. ret = -ENOBUFS;
  697. break;
  698. }
  699. memcpy(&dat[1], &tpdat[offset], len);
  700. ret = j1939_tp_tx_dat(session, dat, len + 1);
  701. if (ret < 0) {
  702. /* ENOBUFS == CAN interface TX queue is full */
  703. if (ret != -ENOBUFS)
  704. netdev_alert(priv->ndev,
  705. "%s: 0x%p: queue data error: %i\n",
  706. __func__, session, ret);
  707. break;
  708. }
  709. session->last_txcmd = 0xff;
  710. pkt_done++;
  711. session->pkt.tx++;
  712. pdelay = j1939_cb_is_broadcast(&session->skcb) ? 50 :
  713. j1939_tp_packet_delay;
  714. if (session->pkt.tx < session->pkt.total && pdelay) {
  715. j1939_tp_schedule_txtimer(session, pdelay);
  716. break;
  717. }
  718. }
  719. if (pkt_done)
  720. j1939_tp_set_rxtimeout(session, 250);
  721. out_free:
  722. if (ret)
  723. kfree_skb(se_skb);
  724. else
  725. consume_skb(se_skb);
  726. return ret;
  727. }
  728. static int j1939_xtp_txnext_transmiter(struct j1939_session *session)
  729. {
  730. struct j1939_priv *priv = session->priv;
  731. int ret = 0;
  732. if (!j1939_tp_im_transmitter(&session->skcb)) {
  733. netdev_alert(priv->ndev, "%s: 0x%p: called by not transmitter!\n",
  734. __func__, session);
  735. return -EINVAL;
  736. }
  737. switch (session->last_cmd) {
  738. case 0:
  739. ret = j1939_session_tx_rts(session);
  740. break;
  741. case J1939_ETP_CMD_CTS:
  742. if (session->last_txcmd != J1939_ETP_CMD_DPO) {
  743. ret = j1939_session_tx_dpo(session);
  744. if (ret)
  745. return ret;
  746. }
  747. fallthrough;
  748. case J1939_TP_CMD_CTS:
  749. case 0xff: /* did some data */
  750. case J1939_ETP_CMD_DPO:
  751. case J1939_TP_CMD_BAM:
  752. ret = j1939_session_tx_dat(session);
  753. break;
  754. default:
  755. netdev_alert(priv->ndev, "%s: 0x%p: unexpected last_cmd: %x\n",
  756. __func__, session, session->last_cmd);
  757. }
  758. return ret;
  759. }
  760. static int j1939_session_tx_cts(struct j1939_session *session)
  761. {
  762. struct j1939_priv *priv = session->priv;
  763. unsigned int pkt, len;
  764. int ret;
  765. u8 dat[8];
  766. if (!j1939_sk_recv_match(priv, &session->skcb))
  767. return -ENOENT;
  768. len = session->pkt.total - session->pkt.rx;
  769. len = min3(len, session->pkt.block, j1939_tp_block ?: 255);
  770. memset(dat, 0xff, sizeof(dat));
  771. if (session->skcb.addr.type == J1939_ETP) {
  772. pkt = session->pkt.rx + 1;
  773. dat[0] = J1939_ETP_CMD_CTS;
  774. dat[1] = len;
  775. dat[2] = (pkt >> 0);
  776. dat[3] = (pkt >> 8);
  777. dat[4] = (pkt >> 16);
  778. } else {
  779. dat[0] = J1939_TP_CMD_CTS;
  780. dat[1] = len;
  781. dat[2] = session->pkt.rx + 1;
  782. }
  783. if (dat[0] == session->last_txcmd)
  784. /* done already */
  785. return 0;
  786. ret = j1939_tp_tx_ctl(session, true, dat);
  787. if (ret < 0)
  788. return ret;
  789. if (len)
  790. /* only mark cts done when len is set */
  791. session->last_txcmd = dat[0];
  792. j1939_tp_set_rxtimeout(session, 1250);
  793. netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
  794. return 0;
  795. }
  796. static int j1939_session_tx_eoma(struct j1939_session *session)
  797. {
  798. struct j1939_priv *priv = session->priv;
  799. u8 dat[8];
  800. int ret;
  801. if (!j1939_sk_recv_match(priv, &session->skcb))
  802. return -ENOENT;
  803. memset(dat, 0xff, sizeof(dat));
  804. if (session->skcb.addr.type == J1939_ETP) {
  805. dat[0] = J1939_ETP_CMD_EOMA;
  806. dat[1] = session->total_message_size >> 0;
  807. dat[2] = session->total_message_size >> 8;
  808. dat[3] = session->total_message_size >> 16;
  809. dat[4] = session->total_message_size >> 24;
  810. } else {
  811. dat[0] = J1939_TP_CMD_EOMA;
  812. dat[1] = session->total_message_size;
  813. dat[2] = session->total_message_size >> 8;
  814. dat[3] = session->pkt.total;
  815. }
  816. if (dat[0] == session->last_txcmd)
  817. /* done already */
  818. return 0;
  819. ret = j1939_tp_tx_ctl(session, true, dat);
  820. if (ret < 0)
  821. return ret;
  822. session->last_txcmd = dat[0];
  823. /* wait for the EOMA packet to come in */
  824. j1939_tp_set_rxtimeout(session, 1250);
  825. netdev_dbg(session->priv->ndev, "%p: 0x%p\n", __func__, session);
  826. return 0;
  827. }
  828. static int j1939_xtp_txnext_receiver(struct j1939_session *session)
  829. {
  830. struct j1939_priv *priv = session->priv;
  831. int ret = 0;
  832. if (!j1939_tp_im_receiver(&session->skcb)) {
  833. netdev_alert(priv->ndev, "%s: 0x%p: called by not receiver!\n",
  834. __func__, session);
  835. return -EINVAL;
  836. }
  837. switch (session->last_cmd) {
  838. case J1939_TP_CMD_RTS:
  839. case J1939_ETP_CMD_RTS:
  840. ret = j1939_session_tx_cts(session);
  841. break;
  842. case J1939_ETP_CMD_CTS:
  843. case J1939_TP_CMD_CTS:
  844. case 0xff: /* did some data */
  845. case J1939_ETP_CMD_DPO:
  846. if ((session->skcb.addr.type == J1939_TP &&
  847. j1939_cb_is_broadcast(&session->skcb)))
  848. break;
  849. if (session->pkt.rx >= session->pkt.total) {
  850. ret = j1939_session_tx_eoma(session);
  851. } else if (session->pkt.rx >= session->pkt.last) {
  852. session->last_txcmd = 0;
  853. ret = j1939_session_tx_cts(session);
  854. }
  855. break;
  856. default:
  857. netdev_alert(priv->ndev, "%s: 0x%p: unexpected last_cmd: %x\n",
  858. __func__, session, session->last_cmd);
  859. }
  860. return ret;
  861. }
  862. static int j1939_simple_txnext(struct j1939_session *session)
  863. {
  864. struct j1939_priv *priv = session->priv;
  865. struct sk_buff *se_skb = j1939_session_skb_get(session);
  866. struct sk_buff *skb;
  867. int ret;
  868. if (!se_skb)
  869. return 0;
  870. skb = skb_clone(se_skb, GFP_ATOMIC);
  871. if (!skb) {
  872. ret = -ENOMEM;
  873. goto out_free;
  874. }
  875. can_skb_set_owner(skb, se_skb->sk);
  876. j1939_tp_set_rxtimeout(session, J1939_SIMPLE_ECHO_TIMEOUT_MS);
  877. ret = j1939_send_one(priv, skb);
  878. if (ret)
  879. goto out_free;
  880. j1939_sk_errqueue(session, J1939_ERRQUEUE_TX_SCHED);
  881. j1939_sk_queue_activate_next(session);
  882. out_free:
  883. if (ret)
  884. kfree_skb(se_skb);
  885. else
  886. consume_skb(se_skb);
  887. return ret;
  888. }
  889. static bool j1939_session_deactivate_locked(struct j1939_session *session)
  890. {
  891. bool active = false;
  892. lockdep_assert_held(&session->priv->active_session_list_lock);
  893. if (session->state >= J1939_SESSION_ACTIVE &&
  894. session->state < J1939_SESSION_ACTIVE_MAX) {
  895. active = true;
  896. list_del_init(&session->active_session_list_entry);
  897. session->state = J1939_SESSION_DONE;
  898. j1939_session_put(session);
  899. }
  900. return active;
  901. }
  902. static bool j1939_session_deactivate(struct j1939_session *session)
  903. {
  904. struct j1939_priv *priv = session->priv;
  905. bool active;
  906. j1939_session_list_lock(priv);
  907. active = j1939_session_deactivate_locked(session);
  908. j1939_session_list_unlock(priv);
  909. return active;
  910. }
  911. static void
  912. j1939_session_deactivate_activate_next(struct j1939_session *session)
  913. {
  914. if (j1939_session_deactivate(session))
  915. j1939_sk_queue_activate_next(session);
  916. }
  917. static void __j1939_session_cancel(struct j1939_session *session,
  918. enum j1939_xtp_abort err)
  919. {
  920. struct j1939_priv *priv = session->priv;
  921. WARN_ON_ONCE(!err);
  922. lockdep_assert_held(&session->priv->active_session_list_lock);
  923. session->err = j1939_xtp_abort_to_errno(priv, err);
  924. session->state = J1939_SESSION_WAITING_ABORT;
  925. /* do not send aborts on incoming broadcasts */
  926. if (!j1939_cb_is_broadcast(&session->skcb)) {
  927. j1939_xtp_tx_abort(priv, &session->skcb,
  928. !session->transmission,
  929. err, session->skcb.addr.pgn);
  930. }
  931. if (session->sk)
  932. j1939_sk_send_loop_abort(session->sk, session->err);
  933. }
  934. static void j1939_session_cancel(struct j1939_session *session,
  935. enum j1939_xtp_abort err)
  936. {
  937. j1939_session_list_lock(session->priv);
  938. if (session->state >= J1939_SESSION_ACTIVE &&
  939. session->state < J1939_SESSION_WAITING_ABORT) {
  940. j1939_tp_set_rxtimeout(session, J1939_XTP_ABORT_TIMEOUT_MS);
  941. __j1939_session_cancel(session, err);
  942. }
  943. j1939_session_list_unlock(session->priv);
  944. if (!session->sk)
  945. j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_ABORT);
  946. }
  947. static enum hrtimer_restart j1939_tp_txtimer(struct hrtimer *hrtimer)
  948. {
  949. struct j1939_session *session =
  950. container_of(hrtimer, struct j1939_session, txtimer);
  951. struct j1939_priv *priv = session->priv;
  952. int ret = 0;
  953. if (session->skcb.addr.type == J1939_SIMPLE) {
  954. ret = j1939_simple_txnext(session);
  955. } else {
  956. if (session->transmission)
  957. ret = j1939_xtp_txnext_transmiter(session);
  958. else
  959. ret = j1939_xtp_txnext_receiver(session);
  960. }
  961. switch (ret) {
  962. case -ENOBUFS:
  963. /* Retry limit is currently arbitrary chosen */
  964. if (session->tx_retry < J1939_XTP_TX_RETRY_LIMIT) {
  965. session->tx_retry++;
  966. j1939_tp_schedule_txtimer(session,
  967. 10 + prandom_u32_max(16));
  968. } else {
  969. netdev_alert(priv->ndev, "%s: 0x%p: tx retry count reached\n",
  970. __func__, session);
  971. session->err = -ENETUNREACH;
  972. j1939_session_rxtimer_cancel(session);
  973. j1939_session_deactivate_activate_next(session);
  974. }
  975. break;
  976. case -ENETDOWN:
  977. /* In this case we should get a netdev_event(), all active
  978. * sessions will be cleared by
  979. * j1939_cancel_all_active_sessions(). So handle this as an
  980. * error, but let j1939_cancel_all_active_sessions() do the
  981. * cleanup including propagation of the error to user space.
  982. */
  983. break;
  984. case -EOVERFLOW:
  985. j1939_session_cancel(session, J1939_XTP_ABORT_ECTS_TOO_BIG);
  986. break;
  987. case 0:
  988. session->tx_retry = 0;
  989. break;
  990. default:
  991. netdev_alert(priv->ndev, "%s: 0x%p: tx aborted with unknown reason: %i\n",
  992. __func__, session, ret);
  993. if (session->skcb.addr.type != J1939_SIMPLE) {
  994. j1939_session_cancel(session, J1939_XTP_ABORT_OTHER);
  995. } else {
  996. session->err = ret;
  997. j1939_session_rxtimer_cancel(session);
  998. j1939_session_deactivate_activate_next(session);
  999. }
  1000. }
  1001. j1939_session_put(session);
  1002. return HRTIMER_NORESTART;
  1003. }
  1004. static void j1939_session_completed(struct j1939_session *session)
  1005. {
  1006. struct sk_buff *se_skb;
  1007. if (!session->transmission) {
  1008. se_skb = j1939_session_skb_get(session);
  1009. /* distribute among j1939 receivers */
  1010. j1939_sk_recv(session->priv, se_skb);
  1011. consume_skb(se_skb);
  1012. }
  1013. j1939_session_deactivate_activate_next(session);
  1014. }
  1015. static enum hrtimer_restart j1939_tp_rxtimer(struct hrtimer *hrtimer)
  1016. {
  1017. struct j1939_session *session = container_of(hrtimer,
  1018. struct j1939_session,
  1019. rxtimer);
  1020. struct j1939_priv *priv = session->priv;
  1021. if (session->state == J1939_SESSION_WAITING_ABORT) {
  1022. netdev_alert(priv->ndev, "%s: 0x%p: abort rx timeout. Force session deactivation\n",
  1023. __func__, session);
  1024. j1939_session_deactivate_activate_next(session);
  1025. } else if (session->skcb.addr.type == J1939_SIMPLE) {
  1026. netdev_alert(priv->ndev, "%s: 0x%p: Timeout. Failed to send simple message.\n",
  1027. __func__, session);
  1028. /* The message is probably stuck in the CAN controller and can
  1029. * be send as soon as CAN bus is in working state again.
  1030. */
  1031. session->err = -ETIME;
  1032. j1939_session_deactivate(session);
  1033. } else {
  1034. j1939_session_list_lock(session->priv);
  1035. if (session->state >= J1939_SESSION_ACTIVE &&
  1036. session->state < J1939_SESSION_ACTIVE_MAX) {
  1037. netdev_alert(priv->ndev, "%s: 0x%p: rx timeout, send abort\n",
  1038. __func__, session);
  1039. j1939_session_get(session);
  1040. hrtimer_start(&session->rxtimer,
  1041. ms_to_ktime(J1939_XTP_ABORT_TIMEOUT_MS),
  1042. HRTIMER_MODE_REL_SOFT);
  1043. __j1939_session_cancel(session, J1939_XTP_ABORT_TIMEOUT);
  1044. }
  1045. j1939_session_list_unlock(session->priv);
  1046. if (!session->sk)
  1047. j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_ABORT);
  1048. }
  1049. j1939_session_put(session);
  1050. return HRTIMER_NORESTART;
  1051. }
  1052. static bool j1939_xtp_rx_cmd_bad_pgn(struct j1939_session *session,
  1053. const struct sk_buff *skb)
  1054. {
  1055. const struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1056. pgn_t pgn = j1939_xtp_ctl_to_pgn(skb->data);
  1057. struct j1939_priv *priv = session->priv;
  1058. enum j1939_xtp_abort abort = J1939_XTP_NO_ABORT;
  1059. u8 cmd = skb->data[0];
  1060. if (session->skcb.addr.pgn == pgn)
  1061. return false;
  1062. switch (cmd) {
  1063. case J1939_TP_CMD_BAM:
  1064. abort = J1939_XTP_NO_ABORT;
  1065. break;
  1066. case J1939_ETP_CMD_RTS:
  1067. fallthrough;
  1068. case J1939_TP_CMD_RTS:
  1069. abort = J1939_XTP_ABORT_BUSY;
  1070. break;
  1071. case J1939_ETP_CMD_CTS:
  1072. fallthrough;
  1073. case J1939_TP_CMD_CTS:
  1074. abort = J1939_XTP_ABORT_ECTS_UNXPECTED_PGN;
  1075. break;
  1076. case J1939_ETP_CMD_DPO:
  1077. abort = J1939_XTP_ABORT_BAD_EDPO_PGN;
  1078. break;
  1079. case J1939_ETP_CMD_EOMA:
  1080. fallthrough;
  1081. case J1939_TP_CMD_EOMA:
  1082. abort = J1939_XTP_ABORT_OTHER;
  1083. break;
  1084. case J1939_ETP_CMD_ABORT: /* && J1939_TP_CMD_ABORT */
  1085. abort = J1939_XTP_NO_ABORT;
  1086. break;
  1087. default:
  1088. WARN_ON_ONCE(1);
  1089. break;
  1090. }
  1091. netdev_warn(priv->ndev, "%s: 0x%p: CMD 0x%02x with PGN 0x%05x for running session with different PGN 0x%05x.\n",
  1092. __func__, session, cmd, pgn, session->skcb.addr.pgn);
  1093. if (abort != J1939_XTP_NO_ABORT)
  1094. j1939_xtp_tx_abort(priv, skcb, true, abort, pgn);
  1095. return true;
  1096. }
  1097. static void j1939_xtp_rx_abort_one(struct j1939_priv *priv, struct sk_buff *skb,
  1098. bool reverse, bool transmitter)
  1099. {
  1100. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1101. struct j1939_session *session;
  1102. u8 abort = skb->data[1];
  1103. session = j1939_session_get_by_addr(priv, &skcb->addr, reverse,
  1104. transmitter);
  1105. if (!session)
  1106. return;
  1107. if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
  1108. goto abort_put;
  1109. netdev_info(priv->ndev, "%s: 0x%p: 0x%05x: (%u) %s\n", __func__,
  1110. session, j1939_xtp_ctl_to_pgn(skb->data), abort,
  1111. j1939_xtp_abort_to_str(abort));
  1112. j1939_session_timers_cancel(session);
  1113. session->err = j1939_xtp_abort_to_errno(priv, abort);
  1114. if (session->sk)
  1115. j1939_sk_send_loop_abort(session->sk, session->err);
  1116. else
  1117. j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_ABORT);
  1118. j1939_session_deactivate_activate_next(session);
  1119. abort_put:
  1120. j1939_session_put(session);
  1121. }
  1122. /* abort packets may come in 2 directions */
  1123. static void
  1124. j1939_xtp_rx_abort(struct j1939_priv *priv, struct sk_buff *skb,
  1125. bool transmitter)
  1126. {
  1127. j1939_xtp_rx_abort_one(priv, skb, false, transmitter);
  1128. j1939_xtp_rx_abort_one(priv, skb, true, transmitter);
  1129. }
  1130. static void
  1131. j1939_xtp_rx_eoma_one(struct j1939_session *session, struct sk_buff *skb)
  1132. {
  1133. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1134. const u8 *dat;
  1135. int len;
  1136. if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
  1137. return;
  1138. dat = skb->data;
  1139. if (skcb->addr.type == J1939_ETP)
  1140. len = j1939_etp_ctl_to_size(dat);
  1141. else
  1142. len = j1939_tp_ctl_to_size(dat);
  1143. if (session->total_message_size != len) {
  1144. netdev_warn_once(session->priv->ndev,
  1145. "%s: 0x%p: Incorrect size. Expected: %i; got: %i.\n",
  1146. __func__, session, session->total_message_size,
  1147. len);
  1148. }
  1149. netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
  1150. session->pkt.tx_acked = session->pkt.total;
  1151. j1939_session_timers_cancel(session);
  1152. /* transmitted without problems */
  1153. j1939_session_completed(session);
  1154. }
  1155. static void
  1156. j1939_xtp_rx_eoma(struct j1939_priv *priv, struct sk_buff *skb,
  1157. bool transmitter)
  1158. {
  1159. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1160. struct j1939_session *session;
  1161. session = j1939_session_get_by_addr(priv, &skcb->addr, true,
  1162. transmitter);
  1163. if (!session)
  1164. return;
  1165. j1939_xtp_rx_eoma_one(session, skb);
  1166. j1939_session_put(session);
  1167. }
  1168. static void
  1169. j1939_xtp_rx_cts_one(struct j1939_session *session, struct sk_buff *skb)
  1170. {
  1171. enum j1939_xtp_abort err = J1939_XTP_ABORT_FAULT;
  1172. unsigned int pkt;
  1173. const u8 *dat;
  1174. dat = skb->data;
  1175. if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
  1176. return;
  1177. netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
  1178. if (session->last_cmd == dat[0]) {
  1179. err = J1939_XTP_ABORT_DUP_SEQ;
  1180. goto out_session_cancel;
  1181. }
  1182. if (session->skcb.addr.type == J1939_ETP)
  1183. pkt = j1939_etp_ctl_to_packet(dat);
  1184. else
  1185. pkt = dat[2];
  1186. if (!pkt)
  1187. goto out_session_cancel;
  1188. else if (dat[1] > session->pkt.block /* 0xff for etp */)
  1189. goto out_session_cancel;
  1190. /* set packet counters only when not CTS(0) */
  1191. session->pkt.tx_acked = pkt - 1;
  1192. j1939_session_skb_drop_old(session);
  1193. session->pkt.last = session->pkt.tx_acked + dat[1];
  1194. if (session->pkt.last > session->pkt.total)
  1195. /* safety measure */
  1196. session->pkt.last = session->pkt.total;
  1197. /* TODO: do not set tx here, do it in txtimer */
  1198. session->pkt.tx = session->pkt.tx_acked;
  1199. session->last_cmd = dat[0];
  1200. if (dat[1]) {
  1201. j1939_tp_set_rxtimeout(session, 1250);
  1202. if (session->transmission) {
  1203. if (session->pkt.tx_acked)
  1204. j1939_sk_errqueue(session,
  1205. J1939_ERRQUEUE_TX_SCHED);
  1206. j1939_session_txtimer_cancel(session);
  1207. j1939_tp_schedule_txtimer(session, 0);
  1208. }
  1209. } else {
  1210. /* CTS(0) */
  1211. j1939_tp_set_rxtimeout(session, 550);
  1212. }
  1213. return;
  1214. out_session_cancel:
  1215. j1939_session_timers_cancel(session);
  1216. j1939_session_cancel(session, err);
  1217. }
  1218. static void
  1219. j1939_xtp_rx_cts(struct j1939_priv *priv, struct sk_buff *skb, bool transmitter)
  1220. {
  1221. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1222. struct j1939_session *session;
  1223. session = j1939_session_get_by_addr(priv, &skcb->addr, true,
  1224. transmitter);
  1225. if (!session)
  1226. return;
  1227. j1939_xtp_rx_cts_one(session, skb);
  1228. j1939_session_put(session);
  1229. }
  1230. static struct j1939_session *j1939_session_new(struct j1939_priv *priv,
  1231. struct sk_buff *skb, size_t size)
  1232. {
  1233. struct j1939_session *session;
  1234. struct j1939_sk_buff_cb *skcb;
  1235. session = kzalloc(sizeof(*session), gfp_any());
  1236. if (!session)
  1237. return NULL;
  1238. INIT_LIST_HEAD(&session->active_session_list_entry);
  1239. INIT_LIST_HEAD(&session->sk_session_queue_entry);
  1240. kref_init(&session->kref);
  1241. j1939_priv_get(priv);
  1242. session->priv = priv;
  1243. session->total_message_size = size;
  1244. session->state = J1939_SESSION_NEW;
  1245. skb_queue_head_init(&session->skb_queue);
  1246. skb_queue_tail(&session->skb_queue, skb);
  1247. skcb = j1939_skb_to_cb(skb);
  1248. memcpy(&session->skcb, skcb, sizeof(session->skcb));
  1249. hrtimer_init(&session->txtimer, CLOCK_MONOTONIC,
  1250. HRTIMER_MODE_REL_SOFT);
  1251. session->txtimer.function = j1939_tp_txtimer;
  1252. hrtimer_init(&session->rxtimer, CLOCK_MONOTONIC,
  1253. HRTIMER_MODE_REL_SOFT);
  1254. session->rxtimer.function = j1939_tp_rxtimer;
  1255. netdev_dbg(priv->ndev, "%s: 0x%p: sa: %02x, da: %02x\n",
  1256. __func__, session, skcb->addr.sa, skcb->addr.da);
  1257. return session;
  1258. }
  1259. static struct
  1260. j1939_session *j1939_session_fresh_new(struct j1939_priv *priv,
  1261. int size,
  1262. const struct j1939_sk_buff_cb *rel_skcb)
  1263. {
  1264. struct sk_buff *skb;
  1265. struct j1939_sk_buff_cb *skcb;
  1266. struct j1939_session *session;
  1267. skb = alloc_skb(size + sizeof(struct can_skb_priv), GFP_ATOMIC);
  1268. if (unlikely(!skb))
  1269. return NULL;
  1270. skb->dev = priv->ndev;
  1271. can_skb_reserve(skb);
  1272. can_skb_prv(skb)->ifindex = priv->ndev->ifindex;
  1273. can_skb_prv(skb)->skbcnt = 0;
  1274. skcb = j1939_skb_to_cb(skb);
  1275. memcpy(skcb, rel_skcb, sizeof(*skcb));
  1276. session = j1939_session_new(priv, skb, size);
  1277. if (!session) {
  1278. kfree_skb(skb);
  1279. return NULL;
  1280. }
  1281. /* alloc data area */
  1282. skb_put(skb, size);
  1283. /* skb is recounted in j1939_session_new() */
  1284. return session;
  1285. }
  1286. int j1939_session_activate(struct j1939_session *session)
  1287. {
  1288. struct j1939_priv *priv = session->priv;
  1289. struct j1939_session *active = NULL;
  1290. int ret = 0;
  1291. j1939_session_list_lock(priv);
  1292. if (session->skcb.addr.type != J1939_SIMPLE)
  1293. active = j1939_session_get_by_addr_locked(priv,
  1294. &priv->active_session_list,
  1295. &session->skcb.addr, false,
  1296. session->transmission);
  1297. if (active) {
  1298. j1939_session_put(active);
  1299. ret = -EAGAIN;
  1300. } else {
  1301. WARN_ON_ONCE(session->state != J1939_SESSION_NEW);
  1302. list_add_tail(&session->active_session_list_entry,
  1303. &priv->active_session_list);
  1304. j1939_session_get(session);
  1305. session->state = J1939_SESSION_ACTIVE;
  1306. netdev_dbg(session->priv->ndev, "%s: 0x%p\n",
  1307. __func__, session);
  1308. }
  1309. j1939_session_list_unlock(priv);
  1310. return ret;
  1311. }
  1312. static struct
  1313. j1939_session *j1939_xtp_rx_rts_session_new(struct j1939_priv *priv,
  1314. struct sk_buff *skb)
  1315. {
  1316. enum j1939_xtp_abort abort = J1939_XTP_NO_ABORT;
  1317. struct j1939_sk_buff_cb skcb = *j1939_skb_to_cb(skb);
  1318. struct j1939_session *session;
  1319. const u8 *dat;
  1320. pgn_t pgn;
  1321. int len;
  1322. netdev_dbg(priv->ndev, "%s\n", __func__);
  1323. dat = skb->data;
  1324. pgn = j1939_xtp_ctl_to_pgn(dat);
  1325. skcb.addr.pgn = pgn;
  1326. if (!j1939_sk_recv_match(priv, &skcb))
  1327. return NULL;
  1328. if (skcb.addr.type == J1939_ETP) {
  1329. len = j1939_etp_ctl_to_size(dat);
  1330. if (len > J1939_MAX_ETP_PACKET_SIZE)
  1331. abort = J1939_XTP_ABORT_FAULT;
  1332. else if (len > priv->tp_max_packet_size)
  1333. abort = J1939_XTP_ABORT_RESOURCE;
  1334. else if (len <= J1939_MAX_TP_PACKET_SIZE)
  1335. abort = J1939_XTP_ABORT_FAULT;
  1336. } else {
  1337. len = j1939_tp_ctl_to_size(dat);
  1338. if (len > J1939_MAX_TP_PACKET_SIZE)
  1339. abort = J1939_XTP_ABORT_FAULT;
  1340. else if (len > priv->tp_max_packet_size)
  1341. abort = J1939_XTP_ABORT_RESOURCE;
  1342. else if (len < J1939_MIN_TP_PACKET_SIZE)
  1343. abort = J1939_XTP_ABORT_FAULT;
  1344. }
  1345. if (abort != J1939_XTP_NO_ABORT) {
  1346. j1939_xtp_tx_abort(priv, &skcb, true, abort, pgn);
  1347. return NULL;
  1348. }
  1349. session = j1939_session_fresh_new(priv, len, &skcb);
  1350. if (!session) {
  1351. j1939_xtp_tx_abort(priv, &skcb, true,
  1352. J1939_XTP_ABORT_RESOURCE, pgn);
  1353. return NULL;
  1354. }
  1355. /* initialize the control buffer: plain copy */
  1356. session->pkt.total = (len + 6) / 7;
  1357. session->pkt.block = 0xff;
  1358. if (skcb.addr.type != J1939_ETP) {
  1359. if (dat[3] != session->pkt.total)
  1360. netdev_alert(priv->ndev, "%s: 0x%p: strange total, %u != %u\n",
  1361. __func__, session, session->pkt.total,
  1362. dat[3]);
  1363. session->pkt.total = dat[3];
  1364. session->pkt.block = min(dat[3], dat[4]);
  1365. }
  1366. session->pkt.rx = 0;
  1367. session->pkt.tx = 0;
  1368. session->tskey = priv->rx_tskey++;
  1369. j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_RTS);
  1370. WARN_ON_ONCE(j1939_session_activate(session));
  1371. return session;
  1372. }
  1373. static int j1939_xtp_rx_rts_session_active(struct j1939_session *session,
  1374. struct sk_buff *skb)
  1375. {
  1376. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1377. struct j1939_priv *priv = session->priv;
  1378. if (!session->transmission) {
  1379. if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
  1380. return -EBUSY;
  1381. /* RTS on active session */
  1382. j1939_session_timers_cancel(session);
  1383. j1939_session_cancel(session, J1939_XTP_ABORT_BUSY);
  1384. }
  1385. if (session->last_cmd != 0) {
  1386. /* we received a second rts on the same connection */
  1387. netdev_alert(priv->ndev, "%s: 0x%p: connection exists (%02x %02x). last cmd: %x\n",
  1388. __func__, session, skcb->addr.sa, skcb->addr.da,
  1389. session->last_cmd);
  1390. j1939_session_timers_cancel(session);
  1391. j1939_session_cancel(session, J1939_XTP_ABORT_BUSY);
  1392. return -EBUSY;
  1393. }
  1394. if (session->skcb.addr.sa != skcb->addr.sa ||
  1395. session->skcb.addr.da != skcb->addr.da)
  1396. 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",
  1397. __func__, session,
  1398. session->skcb.addr.sa, skcb->addr.sa,
  1399. session->skcb.addr.da, skcb->addr.da);
  1400. /* make sure 'sa' & 'da' are correct !
  1401. * They may be 'not filled in yet' for sending
  1402. * skb's, since they did not pass the Address Claim ever.
  1403. */
  1404. session->skcb.addr.sa = skcb->addr.sa;
  1405. session->skcb.addr.da = skcb->addr.da;
  1406. netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
  1407. return 0;
  1408. }
  1409. static void j1939_xtp_rx_rts(struct j1939_priv *priv, struct sk_buff *skb,
  1410. bool transmitter)
  1411. {
  1412. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1413. struct j1939_session *session;
  1414. u8 cmd = skb->data[0];
  1415. session = j1939_session_get_by_addr(priv, &skcb->addr, false,
  1416. transmitter);
  1417. if (!session) {
  1418. if (transmitter) {
  1419. /* If we're the transmitter and this function is called,
  1420. * we received our own RTS. A session has already been
  1421. * created.
  1422. *
  1423. * For some reasons however it might have been destroyed
  1424. * already. So don't create a new one here (using
  1425. * "j1939_xtp_rx_rts_session_new()") as this will be a
  1426. * receiver session.
  1427. *
  1428. * The reasons the session is already destroyed might
  1429. * be:
  1430. * - user space closed socket was and the session was
  1431. * aborted
  1432. * - session was aborted due to external abort message
  1433. */
  1434. return;
  1435. }
  1436. session = j1939_xtp_rx_rts_session_new(priv, skb);
  1437. if (!session) {
  1438. if (cmd == J1939_TP_CMD_BAM && j1939_sk_recv_match(priv, skcb))
  1439. netdev_info(priv->ndev, "%s: failed to create TP BAM session\n",
  1440. __func__);
  1441. return;
  1442. }
  1443. } else {
  1444. if (j1939_xtp_rx_rts_session_active(session, skb)) {
  1445. j1939_session_put(session);
  1446. return;
  1447. }
  1448. }
  1449. session->last_cmd = cmd;
  1450. if (cmd == J1939_TP_CMD_BAM) {
  1451. if (!session->transmission)
  1452. j1939_tp_set_rxtimeout(session, 750);
  1453. } else {
  1454. if (!session->transmission) {
  1455. j1939_session_txtimer_cancel(session);
  1456. j1939_tp_schedule_txtimer(session, 0);
  1457. }
  1458. j1939_tp_set_rxtimeout(session, 1250);
  1459. }
  1460. j1939_session_put(session);
  1461. }
  1462. static void j1939_xtp_rx_dpo_one(struct j1939_session *session,
  1463. struct sk_buff *skb)
  1464. {
  1465. const u8 *dat = skb->data;
  1466. if (j1939_xtp_rx_cmd_bad_pgn(session, skb))
  1467. return;
  1468. netdev_dbg(session->priv->ndev, "%s: 0x%p\n", __func__, session);
  1469. /* transmitted without problems */
  1470. session->pkt.dpo = j1939_etp_ctl_to_packet(skb->data);
  1471. session->last_cmd = dat[0];
  1472. j1939_tp_set_rxtimeout(session, 750);
  1473. if (!session->transmission)
  1474. j1939_sk_errqueue(session, J1939_ERRQUEUE_RX_DPO);
  1475. }
  1476. static void j1939_xtp_rx_dpo(struct j1939_priv *priv, struct sk_buff *skb,
  1477. bool transmitter)
  1478. {
  1479. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1480. struct j1939_session *session;
  1481. session = j1939_session_get_by_addr(priv, &skcb->addr, false,
  1482. transmitter);
  1483. if (!session) {
  1484. netdev_info(priv->ndev,
  1485. "%s: no connection found\n", __func__);
  1486. return;
  1487. }
  1488. j1939_xtp_rx_dpo_one(session, skb);
  1489. j1939_session_put(session);
  1490. }
  1491. static void j1939_xtp_rx_dat_one(struct j1939_session *session,
  1492. struct sk_buff *skb)
  1493. {
  1494. enum j1939_xtp_abort abort = J1939_XTP_ABORT_FAULT;
  1495. struct j1939_priv *priv = session->priv;
  1496. struct j1939_sk_buff_cb *skcb, *se_skcb;
  1497. struct sk_buff *se_skb = NULL;
  1498. const u8 *dat;
  1499. u8 *tpdat;
  1500. int offset;
  1501. int nbytes;
  1502. bool final = false;
  1503. bool remain = false;
  1504. bool do_cts_eoma = false;
  1505. int packet;
  1506. skcb = j1939_skb_to_cb(skb);
  1507. dat = skb->data;
  1508. if (skb->len != 8) {
  1509. /* makes no sense */
  1510. abort = J1939_XTP_ABORT_UNEXPECTED_DATA;
  1511. goto out_session_cancel;
  1512. }
  1513. switch (session->last_cmd) {
  1514. case 0xff:
  1515. break;
  1516. case J1939_ETP_CMD_DPO:
  1517. if (skcb->addr.type == J1939_ETP)
  1518. break;
  1519. fallthrough;
  1520. case J1939_TP_CMD_BAM:
  1521. fallthrough;
  1522. case J1939_TP_CMD_CTS:
  1523. if (skcb->addr.type != J1939_ETP)
  1524. break;
  1525. fallthrough;
  1526. default:
  1527. netdev_info(priv->ndev, "%s: 0x%p: last %02x\n", __func__,
  1528. session, session->last_cmd);
  1529. goto out_session_cancel;
  1530. }
  1531. packet = (dat[0] - 1 + session->pkt.dpo);
  1532. if (packet > session->pkt.total ||
  1533. (session->pkt.rx + 1) > session->pkt.total) {
  1534. netdev_info(priv->ndev, "%s: 0x%p: should have been completed\n",
  1535. __func__, session);
  1536. goto out_session_cancel;
  1537. }
  1538. se_skb = j1939_session_skb_get_by_offset(session, packet * 7);
  1539. if (!se_skb) {
  1540. netdev_warn(priv->ndev, "%s: 0x%p: no skb found\n", __func__,
  1541. session);
  1542. goto out_session_cancel;
  1543. }
  1544. se_skcb = j1939_skb_to_cb(se_skb);
  1545. offset = packet * 7 - se_skcb->offset;
  1546. nbytes = se_skb->len - offset;
  1547. if (nbytes > 7)
  1548. nbytes = 7;
  1549. if (nbytes <= 0 || (nbytes + 1) > skb->len) {
  1550. netdev_info(priv->ndev, "%s: 0x%p: nbytes %i, len %i\n",
  1551. __func__, session, nbytes, skb->len);
  1552. goto out_session_cancel;
  1553. }
  1554. tpdat = se_skb->data;
  1555. if (!session->transmission) {
  1556. memcpy(&tpdat[offset], &dat[1], nbytes);
  1557. } else {
  1558. int err;
  1559. err = memcmp(&tpdat[offset], &dat[1], nbytes);
  1560. if (err)
  1561. netdev_err_once(priv->ndev,
  1562. "%s: 0x%p: Data of RX-looped back packet (%*ph) doesn't match TX data (%*ph)!\n",
  1563. __func__, session,
  1564. nbytes, &dat[1],
  1565. nbytes, &tpdat[offset]);
  1566. }
  1567. if (packet == session->pkt.rx)
  1568. session->pkt.rx++;
  1569. if (se_skcb->addr.type != J1939_ETP &&
  1570. j1939_cb_is_broadcast(&session->skcb)) {
  1571. if (session->pkt.rx >= session->pkt.total)
  1572. final = true;
  1573. else
  1574. remain = true;
  1575. } else {
  1576. /* never final, an EOMA must follow */
  1577. if (session->pkt.rx >= session->pkt.last)
  1578. do_cts_eoma = true;
  1579. }
  1580. if (final) {
  1581. j1939_session_timers_cancel(session);
  1582. j1939_session_completed(session);
  1583. } else if (remain) {
  1584. if (!session->transmission)
  1585. j1939_tp_set_rxtimeout(session, 750);
  1586. } else if (do_cts_eoma) {
  1587. j1939_tp_set_rxtimeout(session, 1250);
  1588. if (!session->transmission)
  1589. j1939_tp_schedule_txtimer(session, 0);
  1590. } else {
  1591. j1939_tp_set_rxtimeout(session, 750);
  1592. }
  1593. session->last_cmd = 0xff;
  1594. consume_skb(se_skb);
  1595. j1939_session_put(session);
  1596. return;
  1597. out_session_cancel:
  1598. kfree_skb(se_skb);
  1599. j1939_session_timers_cancel(session);
  1600. j1939_session_cancel(session, abort);
  1601. j1939_session_put(session);
  1602. }
  1603. static void j1939_xtp_rx_dat(struct j1939_priv *priv, struct sk_buff *skb)
  1604. {
  1605. struct j1939_sk_buff_cb *skcb;
  1606. struct j1939_session *session;
  1607. skcb = j1939_skb_to_cb(skb);
  1608. if (j1939_tp_im_transmitter(skcb)) {
  1609. session = j1939_session_get_by_addr(priv, &skcb->addr, false,
  1610. true);
  1611. if (!session)
  1612. netdev_info(priv->ndev, "%s: no tx connection found\n",
  1613. __func__);
  1614. else
  1615. j1939_xtp_rx_dat_one(session, skb);
  1616. }
  1617. if (j1939_tp_im_receiver(skcb)) {
  1618. session = j1939_session_get_by_addr(priv, &skcb->addr, false,
  1619. false);
  1620. if (!session)
  1621. netdev_info(priv->ndev, "%s: no rx connection found\n",
  1622. __func__);
  1623. else
  1624. j1939_xtp_rx_dat_one(session, skb);
  1625. }
  1626. if (j1939_cb_is_broadcast(skcb)) {
  1627. session = j1939_session_get_by_addr(priv, &skcb->addr, false,
  1628. false);
  1629. if (session)
  1630. j1939_xtp_rx_dat_one(session, skb);
  1631. }
  1632. }
  1633. /* j1939 main intf */
  1634. struct j1939_session *j1939_tp_send(struct j1939_priv *priv,
  1635. struct sk_buff *skb, size_t size)
  1636. {
  1637. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1638. struct j1939_session *session;
  1639. int ret;
  1640. if (skcb->addr.pgn == J1939_TP_PGN_DAT ||
  1641. skcb->addr.pgn == J1939_TP_PGN_CTL ||
  1642. skcb->addr.pgn == J1939_ETP_PGN_DAT ||
  1643. skcb->addr.pgn == J1939_ETP_PGN_CTL)
  1644. /* avoid conflict */
  1645. return ERR_PTR(-EDOM);
  1646. if (size > priv->tp_max_packet_size)
  1647. return ERR_PTR(-EMSGSIZE);
  1648. if (size <= 8)
  1649. skcb->addr.type = J1939_SIMPLE;
  1650. else if (size > J1939_MAX_TP_PACKET_SIZE)
  1651. skcb->addr.type = J1939_ETP;
  1652. else
  1653. skcb->addr.type = J1939_TP;
  1654. if (skcb->addr.type == J1939_ETP &&
  1655. j1939_cb_is_broadcast(skcb))
  1656. return ERR_PTR(-EDESTADDRREQ);
  1657. /* fill in addresses from names */
  1658. ret = j1939_ac_fixup(priv, skb);
  1659. if (unlikely(ret))
  1660. return ERR_PTR(ret);
  1661. /* fix DST flags, it may be used there soon */
  1662. if (j1939_address_is_unicast(skcb->addr.da) &&
  1663. priv->ents[skcb->addr.da].nusers)
  1664. skcb->flags |= J1939_ECU_LOCAL_DST;
  1665. /* src is always local, I'm sending ... */
  1666. skcb->flags |= J1939_ECU_LOCAL_SRC;
  1667. /* prepare new session */
  1668. session = j1939_session_new(priv, skb, size);
  1669. if (!session)
  1670. return ERR_PTR(-ENOMEM);
  1671. /* skb is recounted in j1939_session_new() */
  1672. sock_hold(skb->sk);
  1673. session->sk = skb->sk;
  1674. session->transmission = true;
  1675. session->pkt.total = (size + 6) / 7;
  1676. session->pkt.block = skcb->addr.type == J1939_ETP ? 255 :
  1677. min(j1939_tp_block ?: 255, session->pkt.total);
  1678. if (j1939_cb_is_broadcast(&session->skcb))
  1679. /* set the end-packet for broadcast */
  1680. session->pkt.last = session->pkt.total;
  1681. skcb->tskey = atomic_inc_return(&session->sk->sk_tskey) - 1;
  1682. session->tskey = skcb->tskey;
  1683. return session;
  1684. }
  1685. static void j1939_tp_cmd_recv(struct j1939_priv *priv, struct sk_buff *skb)
  1686. {
  1687. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1688. int extd = J1939_TP;
  1689. u8 cmd = skb->data[0];
  1690. switch (cmd) {
  1691. case J1939_ETP_CMD_RTS:
  1692. extd = J1939_ETP;
  1693. fallthrough;
  1694. case J1939_TP_CMD_BAM:
  1695. if (cmd == J1939_TP_CMD_BAM && !j1939_cb_is_broadcast(skcb)) {
  1696. netdev_err_once(priv->ndev, "%s: BAM to unicast (%02x), ignoring!\n",
  1697. __func__, skcb->addr.sa);
  1698. return;
  1699. }
  1700. fallthrough;
  1701. case J1939_TP_CMD_RTS:
  1702. if (skcb->addr.type != extd)
  1703. return;
  1704. if (cmd == J1939_TP_CMD_RTS && j1939_cb_is_broadcast(skcb)) {
  1705. netdev_alert(priv->ndev, "%s: rts without destination (%02x)\n",
  1706. __func__, skcb->addr.sa);
  1707. return;
  1708. }
  1709. if (j1939_tp_im_transmitter(skcb))
  1710. j1939_xtp_rx_rts(priv, skb, true);
  1711. if (j1939_tp_im_receiver(skcb) || j1939_cb_is_broadcast(skcb))
  1712. j1939_xtp_rx_rts(priv, skb, false);
  1713. break;
  1714. case J1939_ETP_CMD_CTS:
  1715. extd = J1939_ETP;
  1716. fallthrough;
  1717. case J1939_TP_CMD_CTS:
  1718. if (skcb->addr.type != extd)
  1719. return;
  1720. if (j1939_tp_im_transmitter(skcb))
  1721. j1939_xtp_rx_cts(priv, skb, false);
  1722. if (j1939_tp_im_receiver(skcb))
  1723. j1939_xtp_rx_cts(priv, skb, true);
  1724. break;
  1725. case J1939_ETP_CMD_DPO:
  1726. if (skcb->addr.type != J1939_ETP)
  1727. return;
  1728. if (j1939_tp_im_transmitter(skcb))
  1729. j1939_xtp_rx_dpo(priv, skb, true);
  1730. if (j1939_tp_im_receiver(skcb))
  1731. j1939_xtp_rx_dpo(priv, skb, false);
  1732. break;
  1733. case J1939_ETP_CMD_EOMA:
  1734. extd = J1939_ETP;
  1735. fallthrough;
  1736. case J1939_TP_CMD_EOMA:
  1737. if (skcb->addr.type != extd)
  1738. return;
  1739. if (j1939_tp_im_transmitter(skcb))
  1740. j1939_xtp_rx_eoma(priv, skb, false);
  1741. if (j1939_tp_im_receiver(skcb))
  1742. j1939_xtp_rx_eoma(priv, skb, true);
  1743. break;
  1744. case J1939_ETP_CMD_ABORT: /* && J1939_TP_CMD_ABORT */
  1745. if (j1939_cb_is_broadcast(skcb)) {
  1746. netdev_err_once(priv->ndev, "%s: abort to broadcast (%02x), ignoring!\n",
  1747. __func__, skcb->addr.sa);
  1748. return;
  1749. }
  1750. if (j1939_tp_im_transmitter(skcb))
  1751. j1939_xtp_rx_abort(priv, skb, true);
  1752. if (j1939_tp_im_receiver(skcb))
  1753. j1939_xtp_rx_abort(priv, skb, false);
  1754. break;
  1755. default:
  1756. return;
  1757. }
  1758. }
  1759. int j1939_tp_recv(struct j1939_priv *priv, struct sk_buff *skb)
  1760. {
  1761. struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
  1762. if (!j1939_tp_im_involved_anydir(skcb) && !j1939_cb_is_broadcast(skcb))
  1763. return 0;
  1764. switch (skcb->addr.pgn) {
  1765. case J1939_ETP_PGN_DAT:
  1766. skcb->addr.type = J1939_ETP;
  1767. fallthrough;
  1768. case J1939_TP_PGN_DAT:
  1769. j1939_xtp_rx_dat(priv, skb);
  1770. break;
  1771. case J1939_ETP_PGN_CTL:
  1772. skcb->addr.type = J1939_ETP;
  1773. fallthrough;
  1774. case J1939_TP_PGN_CTL:
  1775. if (skb->len < 8)
  1776. return 0; /* Don't care. Nothing to extract here */
  1777. j1939_tp_cmd_recv(priv, skb);
  1778. break;
  1779. default:
  1780. return 0; /* no problem */
  1781. }
  1782. return 1; /* "I processed the message" */
  1783. }
  1784. void j1939_simple_recv(struct j1939_priv *priv, struct sk_buff *skb)
  1785. {
  1786. struct j1939_session *session;
  1787. if (!skb->sk)
  1788. return;
  1789. if (skb->sk->sk_family != AF_CAN ||
  1790. skb->sk->sk_protocol != CAN_J1939)
  1791. return;
  1792. j1939_session_list_lock(priv);
  1793. session = j1939_session_get_simple(priv, skb);
  1794. j1939_session_list_unlock(priv);
  1795. if (!session) {
  1796. netdev_warn(priv->ndev,
  1797. "%s: Received already invalidated message\n",
  1798. __func__);
  1799. return;
  1800. }
  1801. j1939_session_timers_cancel(session);
  1802. j1939_session_deactivate(session);
  1803. j1939_session_put(session);
  1804. }
  1805. int j1939_cancel_active_session(struct j1939_priv *priv, struct sock *sk)
  1806. {
  1807. struct j1939_session *session, *saved;
  1808. netdev_dbg(priv->ndev, "%s, sk: %p\n", __func__, sk);
  1809. j1939_session_list_lock(priv);
  1810. list_for_each_entry_safe(session, saved,
  1811. &priv->active_session_list,
  1812. active_session_list_entry) {
  1813. if (!sk || sk == session->sk) {
  1814. if (hrtimer_try_to_cancel(&session->txtimer) == 1)
  1815. j1939_session_put(session);
  1816. if (hrtimer_try_to_cancel(&session->rxtimer) == 1)
  1817. j1939_session_put(session);
  1818. session->err = ESHUTDOWN;
  1819. j1939_session_deactivate_locked(session);
  1820. }
  1821. }
  1822. j1939_session_list_unlock(priv);
  1823. return NOTIFY_DONE;
  1824. }
  1825. void j1939_tp_init(struct j1939_priv *priv)
  1826. {
  1827. spin_lock_init(&priv->active_session_list_lock);
  1828. INIT_LIST_HEAD(&priv->active_session_list);
  1829. priv->tp_max_packet_size = J1939_MAX_ETP_PACKET_SIZE;
  1830. }