isotp.c 39 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563
  1. // SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
  2. /* isotp.c - ISO 15765-2 CAN transport protocol for protocol family CAN
  3. *
  4. * This implementation does not provide ISO-TP specific return values to the
  5. * userspace.
  6. *
  7. * - RX path timeout of data reception leads to -ETIMEDOUT
  8. * - RX path SN mismatch leads to -EILSEQ
  9. * - RX path data reception with wrong padding leads to -EBADMSG
  10. * - TX path flowcontrol reception timeout leads to -ECOMM
  11. * - TX path flowcontrol reception overflow leads to -EMSGSIZE
  12. * - TX path flowcontrol reception with wrong layout/padding leads to -EBADMSG
  13. * - when a transfer (tx) is on the run the next write() blocks until it's done
  14. * - use CAN_ISOTP_WAIT_TX_DONE flag to block the caller until the PDU is sent
  15. * - as we have static buffers the check whether the PDU fits into the buffer
  16. * is done at FF reception time (no support for sending 'wait frames')
  17. * - take care of the tx-queue-len as traffic shaping is still on the TODO list
  18. *
  19. * Copyright (c) 2020 Volkswagen Group Electronic Research
  20. * All rights reserved.
  21. *
  22. * Redistribution and use in source and binary forms, with or without
  23. * modification, are permitted provided that the following conditions
  24. * are met:
  25. * 1. Redistributions of source code must retain the above copyright
  26. * notice, this list of conditions and the following disclaimer.
  27. * 2. Redistributions in binary form must reproduce the above copyright
  28. * notice, this list of conditions and the following disclaimer in the
  29. * documentation and/or other materials provided with the distribution.
  30. * 3. Neither the name of Volkswagen nor the names of its contributors
  31. * may be used to endorse or promote products derived from this software
  32. * without specific prior written permission.
  33. *
  34. * Alternatively, provided that this notice is retained in full, this
  35. * software may be distributed under the terms of the GNU General
  36. * Public License ("GPL") version 2, in which case the provisions of the
  37. * GPL apply INSTEAD OF those given above.
  38. *
  39. * The provided data structures and external interfaces from this code
  40. * are not restricted to be used by modules with a GPL compatible license.
  41. *
  42. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  43. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  44. * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  45. * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  46. * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  47. * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  48. * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  49. * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  50. * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  51. * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  52. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  53. * DAMAGE.
  54. */
  55. #include <linux/module.h>
  56. #include <linux/init.h>
  57. #include <linux/interrupt.h>
  58. #include <linux/spinlock.h>
  59. #include <linux/hrtimer.h>
  60. #include <linux/wait.h>
  61. #include <linux/uio.h>
  62. #include <linux/net.h>
  63. #include <linux/netdevice.h>
  64. #include <linux/socket.h>
  65. #include <linux/if_arp.h>
  66. #include <linux/skbuff.h>
  67. #include <linux/can.h>
  68. #include <linux/can/core.h>
  69. #include <linux/can/skb.h>
  70. #include <linux/can/isotp.h>
  71. #include <linux/slab.h>
  72. #include <net/sock.h>
  73. #include <net/net_namespace.h>
  74. MODULE_DESCRIPTION("PF_CAN isotp 15765-2:2016 protocol");
  75. MODULE_LICENSE("Dual BSD/GPL");
  76. MODULE_AUTHOR("Oliver Hartkopp <socketcan@hartkopp.net>");
  77. MODULE_ALIAS("can-proto-6");
  78. #define ISOTP_MIN_NAMELEN CAN_REQUIRED_SIZE(struct sockaddr_can, can_addr.tp)
  79. #define SINGLE_MASK(id) (((id) & CAN_EFF_FLAG) ? \
  80. (CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG) : \
  81. (CAN_SFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG))
  82. /* ISO 15765-2:2016 supports more than 4095 byte per ISO PDU as the FF_DL can
  83. * take full 32 bit values (4 Gbyte). We would need some good concept to handle
  84. * this between user space and kernel space. For now increase the static buffer
  85. * to something about 8 kbyte to be able to test this new functionality.
  86. */
  87. #define MAX_MSG_LENGTH 8200
  88. /* N_PCI type values in bits 7-4 of N_PCI bytes */
  89. #define N_PCI_SF 0x00 /* single frame */
  90. #define N_PCI_FF 0x10 /* first frame */
  91. #define N_PCI_CF 0x20 /* consecutive frame */
  92. #define N_PCI_FC 0x30 /* flow control */
  93. #define N_PCI_SZ 1 /* size of the PCI byte #1 */
  94. #define SF_PCI_SZ4 1 /* size of SingleFrame PCI including 4 bit SF_DL */
  95. #define SF_PCI_SZ8 2 /* size of SingleFrame PCI including 8 bit SF_DL */
  96. #define FF_PCI_SZ12 2 /* size of FirstFrame PCI including 12 bit FF_DL */
  97. #define FF_PCI_SZ32 6 /* size of FirstFrame PCI including 32 bit FF_DL */
  98. #define FC_CONTENT_SZ 3 /* flow control content size in byte (FS/BS/STmin) */
  99. #define ISOTP_CHECK_PADDING (CAN_ISOTP_CHK_PAD_LEN | CAN_ISOTP_CHK_PAD_DATA)
  100. /* Flow Status given in FC frame */
  101. #define ISOTP_FC_CTS 0 /* clear to send */
  102. #define ISOTP_FC_WT 1 /* wait */
  103. #define ISOTP_FC_OVFLW 2 /* overflow */
  104. enum {
  105. ISOTP_IDLE = 0,
  106. ISOTP_WAIT_FIRST_FC,
  107. ISOTP_WAIT_FC,
  108. ISOTP_WAIT_DATA,
  109. ISOTP_SENDING
  110. };
  111. struct tpcon {
  112. unsigned int idx;
  113. unsigned int len;
  114. u32 state;
  115. u8 bs;
  116. u8 sn;
  117. u8 ll_dl;
  118. u8 buf[MAX_MSG_LENGTH + 1];
  119. };
  120. struct isotp_sock {
  121. struct sock sk;
  122. int bound;
  123. int ifindex;
  124. canid_t txid;
  125. canid_t rxid;
  126. ktime_t tx_gap;
  127. ktime_t lastrxcf_tstamp;
  128. struct hrtimer rxtimer, txtimer;
  129. struct can_isotp_options opt;
  130. struct can_isotp_fc_options rxfc, txfc;
  131. struct can_isotp_ll_options ll;
  132. u32 frame_txtime;
  133. u32 force_tx_stmin;
  134. u32 force_rx_stmin;
  135. struct tpcon rx, tx;
  136. struct list_head notifier;
  137. wait_queue_head_t wait;
  138. spinlock_t rx_lock; /* protect single thread state machine */
  139. };
  140. static LIST_HEAD(isotp_notifier_list);
  141. static DEFINE_SPINLOCK(isotp_notifier_lock);
  142. static struct isotp_sock *isotp_busy_notifier;
  143. static inline struct isotp_sock *isotp_sk(const struct sock *sk)
  144. {
  145. return (struct isotp_sock *)sk;
  146. }
  147. static enum hrtimer_restart isotp_rx_timer_handler(struct hrtimer *hrtimer)
  148. {
  149. struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
  150. rxtimer);
  151. struct sock *sk = &so->sk;
  152. if (so->rx.state == ISOTP_WAIT_DATA) {
  153. /* we did not get new data frames in time */
  154. /* report 'connection timed out' */
  155. sk->sk_err = ETIMEDOUT;
  156. if (!sock_flag(sk, SOCK_DEAD))
  157. sk->sk_error_report(sk);
  158. /* reset rx state */
  159. so->rx.state = ISOTP_IDLE;
  160. }
  161. return HRTIMER_NORESTART;
  162. }
  163. static int isotp_send_fc(struct sock *sk, int ae, u8 flowstatus)
  164. {
  165. struct net_device *dev;
  166. struct sk_buff *nskb;
  167. struct canfd_frame *ncf;
  168. struct isotp_sock *so = isotp_sk(sk);
  169. int can_send_ret;
  170. nskb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv), gfp_any());
  171. if (!nskb)
  172. return 1;
  173. dev = dev_get_by_index(sock_net(sk), so->ifindex);
  174. if (!dev) {
  175. kfree_skb(nskb);
  176. return 1;
  177. }
  178. can_skb_reserve(nskb);
  179. can_skb_prv(nskb)->ifindex = dev->ifindex;
  180. can_skb_prv(nskb)->skbcnt = 0;
  181. nskb->dev = dev;
  182. can_skb_set_owner(nskb, sk);
  183. ncf = (struct canfd_frame *)nskb->data;
  184. skb_put_zero(nskb, so->ll.mtu);
  185. /* create & send flow control reply */
  186. ncf->can_id = so->txid;
  187. if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
  188. memset(ncf->data, so->opt.txpad_content, CAN_MAX_DLEN);
  189. ncf->len = CAN_MAX_DLEN;
  190. } else {
  191. ncf->len = ae + FC_CONTENT_SZ;
  192. }
  193. ncf->data[ae] = N_PCI_FC | flowstatus;
  194. ncf->data[ae + 1] = so->rxfc.bs;
  195. ncf->data[ae + 2] = so->rxfc.stmin;
  196. if (ae)
  197. ncf->data[0] = so->opt.ext_address;
  198. ncf->flags = so->ll.tx_flags;
  199. can_send_ret = can_send(nskb, 1);
  200. if (can_send_ret)
  201. pr_notice_once("can-isotp: %s: can_send_ret %d\n",
  202. __func__, can_send_ret);
  203. dev_put(dev);
  204. /* reset blocksize counter */
  205. so->rx.bs = 0;
  206. /* reset last CF frame rx timestamp for rx stmin enforcement */
  207. so->lastrxcf_tstamp = ktime_set(0, 0);
  208. /* start rx timeout watchdog */
  209. hrtimer_start(&so->rxtimer, ktime_set(1, 0), HRTIMER_MODE_REL_SOFT);
  210. return 0;
  211. }
  212. static void isotp_rcv_skb(struct sk_buff *skb, struct sock *sk)
  213. {
  214. struct sockaddr_can *addr = (struct sockaddr_can *)skb->cb;
  215. BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can));
  216. memset(addr, 0, sizeof(*addr));
  217. addr->can_family = AF_CAN;
  218. addr->can_ifindex = skb->dev->ifindex;
  219. if (sock_queue_rcv_skb(sk, skb) < 0)
  220. kfree_skb(skb);
  221. }
  222. static u8 padlen(u8 datalen)
  223. {
  224. static const u8 plen[] = {
  225. 8, 8, 8, 8, 8, 8, 8, 8, 8, /* 0 - 8 */
  226. 12, 12, 12, 12, /* 9 - 12 */
  227. 16, 16, 16, 16, /* 13 - 16 */
  228. 20, 20, 20, 20, /* 17 - 20 */
  229. 24, 24, 24, 24, /* 21 - 24 */
  230. 32, 32, 32, 32, 32, 32, 32, 32, /* 25 - 32 */
  231. 48, 48, 48, 48, 48, 48, 48, 48, /* 33 - 40 */
  232. 48, 48, 48, 48, 48, 48, 48, 48 /* 41 - 48 */
  233. };
  234. if (datalen > 48)
  235. return 64;
  236. return plen[datalen];
  237. }
  238. /* check for length optimization and return 1/true when the check fails */
  239. static int check_optimized(struct canfd_frame *cf, int start_index)
  240. {
  241. /* for CAN_DL <= 8 the start_index is equal to the CAN_DL as the
  242. * padding would start at this point. E.g. if the padding would
  243. * start at cf.data[7] cf->len has to be 7 to be optimal.
  244. * Note: The data[] index starts with zero.
  245. */
  246. if (cf->len <= CAN_MAX_DLEN)
  247. return (cf->len != start_index);
  248. /* This relation is also valid in the non-linear DLC range, where
  249. * we need to take care of the minimal next possible CAN_DL.
  250. * The correct check would be (padlen(cf->len) != padlen(start_index)).
  251. * But as cf->len can only take discrete values from 12, .., 64 at this
  252. * point the padlen(cf->len) is always equal to cf->len.
  253. */
  254. return (cf->len != padlen(start_index));
  255. }
  256. /* check padding and return 1/true when the check fails */
  257. static int check_pad(struct isotp_sock *so, struct canfd_frame *cf,
  258. int start_index, u8 content)
  259. {
  260. int i;
  261. /* no RX_PADDING value => check length of optimized frame length */
  262. if (!(so->opt.flags & CAN_ISOTP_RX_PADDING)) {
  263. if (so->opt.flags & CAN_ISOTP_CHK_PAD_LEN)
  264. return check_optimized(cf, start_index);
  265. /* no valid test against empty value => ignore frame */
  266. return 1;
  267. }
  268. /* check datalength of correctly padded CAN frame */
  269. if ((so->opt.flags & CAN_ISOTP_CHK_PAD_LEN) &&
  270. cf->len != padlen(cf->len))
  271. return 1;
  272. /* check padding content */
  273. if (so->opt.flags & CAN_ISOTP_CHK_PAD_DATA) {
  274. for (i = start_index; i < cf->len; i++)
  275. if (cf->data[i] != content)
  276. return 1;
  277. }
  278. return 0;
  279. }
  280. static int isotp_rcv_fc(struct isotp_sock *so, struct canfd_frame *cf, int ae)
  281. {
  282. struct sock *sk = &so->sk;
  283. if (so->tx.state != ISOTP_WAIT_FC &&
  284. so->tx.state != ISOTP_WAIT_FIRST_FC)
  285. return 0;
  286. hrtimer_cancel(&so->txtimer);
  287. if ((cf->len < ae + FC_CONTENT_SZ) ||
  288. ((so->opt.flags & ISOTP_CHECK_PADDING) &&
  289. check_pad(so, cf, ae + FC_CONTENT_SZ, so->opt.rxpad_content))) {
  290. /* malformed PDU - report 'not a data message' */
  291. sk->sk_err = EBADMSG;
  292. if (!sock_flag(sk, SOCK_DEAD))
  293. sk->sk_error_report(sk);
  294. so->tx.state = ISOTP_IDLE;
  295. wake_up_interruptible(&so->wait);
  296. return 1;
  297. }
  298. /* get communication parameters only from the first FC frame */
  299. if (so->tx.state == ISOTP_WAIT_FIRST_FC) {
  300. so->txfc.bs = cf->data[ae + 1];
  301. so->txfc.stmin = cf->data[ae + 2];
  302. /* fix wrong STmin values according spec */
  303. if (so->txfc.stmin > 0x7F &&
  304. (so->txfc.stmin < 0xF1 || so->txfc.stmin > 0xF9))
  305. so->txfc.stmin = 0x7F;
  306. so->tx_gap = ktime_set(0, 0);
  307. /* add transmission time for CAN frame N_As */
  308. so->tx_gap = ktime_add_ns(so->tx_gap, so->frame_txtime);
  309. /* add waiting time for consecutive frames N_Cs */
  310. if (so->opt.flags & CAN_ISOTP_FORCE_TXSTMIN)
  311. so->tx_gap = ktime_add_ns(so->tx_gap,
  312. so->force_tx_stmin);
  313. else if (so->txfc.stmin < 0x80)
  314. so->tx_gap = ktime_add_ns(so->tx_gap,
  315. so->txfc.stmin * 1000000);
  316. else
  317. so->tx_gap = ktime_add_ns(so->tx_gap,
  318. (so->txfc.stmin - 0xF0)
  319. * 100000);
  320. so->tx.state = ISOTP_WAIT_FC;
  321. }
  322. switch (cf->data[ae] & 0x0F) {
  323. case ISOTP_FC_CTS:
  324. so->tx.bs = 0;
  325. so->tx.state = ISOTP_SENDING;
  326. /* start cyclic timer for sending CF frame */
  327. hrtimer_start(&so->txtimer, so->tx_gap,
  328. HRTIMER_MODE_REL_SOFT);
  329. break;
  330. case ISOTP_FC_WT:
  331. /* start timer to wait for next FC frame */
  332. hrtimer_start(&so->txtimer, ktime_set(1, 0),
  333. HRTIMER_MODE_REL_SOFT);
  334. break;
  335. case ISOTP_FC_OVFLW:
  336. /* overflow on receiver side - report 'message too long' */
  337. sk->sk_err = EMSGSIZE;
  338. if (!sock_flag(sk, SOCK_DEAD))
  339. sk->sk_error_report(sk);
  340. fallthrough;
  341. default:
  342. /* stop this tx job */
  343. so->tx.state = ISOTP_IDLE;
  344. wake_up_interruptible(&so->wait);
  345. }
  346. return 0;
  347. }
  348. static int isotp_rcv_sf(struct sock *sk, struct canfd_frame *cf, int pcilen,
  349. struct sk_buff *skb, int len)
  350. {
  351. struct isotp_sock *so = isotp_sk(sk);
  352. struct sk_buff *nskb;
  353. hrtimer_cancel(&so->rxtimer);
  354. so->rx.state = ISOTP_IDLE;
  355. if (!len || len > cf->len - pcilen)
  356. return 1;
  357. if ((so->opt.flags & ISOTP_CHECK_PADDING) &&
  358. check_pad(so, cf, pcilen + len, so->opt.rxpad_content)) {
  359. /* malformed PDU - report 'not a data message' */
  360. sk->sk_err = EBADMSG;
  361. if (!sock_flag(sk, SOCK_DEAD))
  362. sk->sk_error_report(sk);
  363. return 1;
  364. }
  365. nskb = alloc_skb(len, gfp_any());
  366. if (!nskb)
  367. return 1;
  368. memcpy(skb_put(nskb, len), &cf->data[pcilen], len);
  369. nskb->tstamp = skb->tstamp;
  370. nskb->dev = skb->dev;
  371. isotp_rcv_skb(nskb, sk);
  372. return 0;
  373. }
  374. static int isotp_rcv_ff(struct sock *sk, struct canfd_frame *cf, int ae)
  375. {
  376. struct isotp_sock *so = isotp_sk(sk);
  377. int i;
  378. int off;
  379. int ff_pci_sz;
  380. hrtimer_cancel(&so->rxtimer);
  381. so->rx.state = ISOTP_IDLE;
  382. /* get the used sender LL_DL from the (first) CAN frame data length */
  383. so->rx.ll_dl = padlen(cf->len);
  384. /* the first frame has to use the entire frame up to LL_DL length */
  385. if (cf->len != so->rx.ll_dl)
  386. return 1;
  387. /* get the FF_DL */
  388. so->rx.len = (cf->data[ae] & 0x0F) << 8;
  389. so->rx.len += cf->data[ae + 1];
  390. /* Check for FF_DL escape sequence supporting 32 bit PDU length */
  391. if (so->rx.len) {
  392. ff_pci_sz = FF_PCI_SZ12;
  393. } else {
  394. /* FF_DL = 0 => get real length from next 4 bytes */
  395. so->rx.len = cf->data[ae + 2] << 24;
  396. so->rx.len += cf->data[ae + 3] << 16;
  397. so->rx.len += cf->data[ae + 4] << 8;
  398. so->rx.len += cf->data[ae + 5];
  399. ff_pci_sz = FF_PCI_SZ32;
  400. }
  401. /* take care of a potential SF_DL ESC offset for TX_DL > 8 */
  402. off = (so->rx.ll_dl > CAN_MAX_DLEN) ? 1 : 0;
  403. if (so->rx.len + ae + off + ff_pci_sz < so->rx.ll_dl)
  404. return 1;
  405. if (so->rx.len > MAX_MSG_LENGTH) {
  406. /* send FC frame with overflow status */
  407. isotp_send_fc(sk, ae, ISOTP_FC_OVFLW);
  408. return 1;
  409. }
  410. /* copy the first received data bytes */
  411. so->rx.idx = 0;
  412. for (i = ae + ff_pci_sz; i < so->rx.ll_dl; i++)
  413. so->rx.buf[so->rx.idx++] = cf->data[i];
  414. /* initial setup for this pdu reception */
  415. so->rx.sn = 1;
  416. so->rx.state = ISOTP_WAIT_DATA;
  417. /* no creation of flow control frames */
  418. if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
  419. return 0;
  420. /* send our first FC frame */
  421. isotp_send_fc(sk, ae, ISOTP_FC_CTS);
  422. return 0;
  423. }
  424. static int isotp_rcv_cf(struct sock *sk, struct canfd_frame *cf, int ae,
  425. struct sk_buff *skb)
  426. {
  427. struct isotp_sock *so = isotp_sk(sk);
  428. struct sk_buff *nskb;
  429. int i;
  430. if (so->rx.state != ISOTP_WAIT_DATA)
  431. return 0;
  432. /* drop if timestamp gap is less than force_rx_stmin nano secs */
  433. if (so->opt.flags & CAN_ISOTP_FORCE_RXSTMIN) {
  434. if (ktime_to_ns(ktime_sub(skb->tstamp, so->lastrxcf_tstamp)) <
  435. so->force_rx_stmin)
  436. return 0;
  437. so->lastrxcf_tstamp = skb->tstamp;
  438. }
  439. hrtimer_cancel(&so->rxtimer);
  440. /* CFs are never longer than the FF */
  441. if (cf->len > so->rx.ll_dl)
  442. return 1;
  443. /* CFs have usually the LL_DL length */
  444. if (cf->len < so->rx.ll_dl) {
  445. /* this is only allowed for the last CF */
  446. if (so->rx.len - so->rx.idx > so->rx.ll_dl - ae - N_PCI_SZ)
  447. return 1;
  448. }
  449. if ((cf->data[ae] & 0x0F) != so->rx.sn) {
  450. /* wrong sn detected - report 'illegal byte sequence' */
  451. sk->sk_err = EILSEQ;
  452. if (!sock_flag(sk, SOCK_DEAD))
  453. sk->sk_error_report(sk);
  454. /* reset rx state */
  455. so->rx.state = ISOTP_IDLE;
  456. return 1;
  457. }
  458. so->rx.sn++;
  459. so->rx.sn %= 16;
  460. for (i = ae + N_PCI_SZ; i < cf->len; i++) {
  461. so->rx.buf[so->rx.idx++] = cf->data[i];
  462. if (so->rx.idx >= so->rx.len)
  463. break;
  464. }
  465. if (so->rx.idx >= so->rx.len) {
  466. /* we are done */
  467. so->rx.state = ISOTP_IDLE;
  468. if ((so->opt.flags & ISOTP_CHECK_PADDING) &&
  469. check_pad(so, cf, i + 1, so->opt.rxpad_content)) {
  470. /* malformed PDU - report 'not a data message' */
  471. sk->sk_err = EBADMSG;
  472. if (!sock_flag(sk, SOCK_DEAD))
  473. sk->sk_error_report(sk);
  474. return 1;
  475. }
  476. nskb = alloc_skb(so->rx.len, gfp_any());
  477. if (!nskb)
  478. return 1;
  479. memcpy(skb_put(nskb, so->rx.len), so->rx.buf,
  480. so->rx.len);
  481. nskb->tstamp = skb->tstamp;
  482. nskb->dev = skb->dev;
  483. isotp_rcv_skb(nskb, sk);
  484. return 0;
  485. }
  486. /* perform blocksize handling, if enabled */
  487. if (!so->rxfc.bs || ++so->rx.bs < so->rxfc.bs) {
  488. /* start rx timeout watchdog */
  489. hrtimer_start(&so->rxtimer, ktime_set(1, 0),
  490. HRTIMER_MODE_REL_SOFT);
  491. return 0;
  492. }
  493. /* no creation of flow control frames */
  494. if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
  495. return 0;
  496. /* we reached the specified blocksize so->rxfc.bs */
  497. isotp_send_fc(sk, ae, ISOTP_FC_CTS);
  498. return 0;
  499. }
  500. static void isotp_rcv(struct sk_buff *skb, void *data)
  501. {
  502. struct sock *sk = (struct sock *)data;
  503. struct isotp_sock *so = isotp_sk(sk);
  504. struct canfd_frame *cf;
  505. int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
  506. u8 n_pci_type, sf_dl;
  507. /* Strictly receive only frames with the configured MTU size
  508. * => clear separation of CAN2.0 / CAN FD transport channels
  509. */
  510. if (skb->len != so->ll.mtu)
  511. return;
  512. cf = (struct canfd_frame *)skb->data;
  513. /* if enabled: check reception of my configured extended address */
  514. if (ae && cf->data[0] != so->opt.rx_ext_address)
  515. return;
  516. n_pci_type = cf->data[ae] & 0xF0;
  517. /* Make sure the state changes and data structures stay consistent at
  518. * CAN frame reception time. This locking is not needed in real world
  519. * use cases but the inconsistency can be triggered with syzkaller.
  520. */
  521. spin_lock(&so->rx_lock);
  522. if (so->opt.flags & CAN_ISOTP_HALF_DUPLEX) {
  523. /* check rx/tx path half duplex expectations */
  524. if ((so->tx.state != ISOTP_IDLE && n_pci_type != N_PCI_FC) ||
  525. (so->rx.state != ISOTP_IDLE && n_pci_type == N_PCI_FC))
  526. goto out_unlock;
  527. }
  528. switch (n_pci_type) {
  529. case N_PCI_FC:
  530. /* tx path: flow control frame containing the FC parameters */
  531. isotp_rcv_fc(so, cf, ae);
  532. break;
  533. case N_PCI_SF:
  534. /* rx path: single frame
  535. *
  536. * As we do not have a rx.ll_dl configuration, we can only test
  537. * if the CAN frames payload length matches the LL_DL == 8
  538. * requirements - no matter if it's CAN 2.0 or CAN FD
  539. */
  540. /* get the SF_DL from the N_PCI byte */
  541. sf_dl = cf->data[ae] & 0x0F;
  542. if (cf->len <= CAN_MAX_DLEN) {
  543. isotp_rcv_sf(sk, cf, SF_PCI_SZ4 + ae, skb, sf_dl);
  544. } else {
  545. if (skb->len == CANFD_MTU) {
  546. /* We have a CAN FD frame and CAN_DL is greater than 8:
  547. * Only frames with the SF_DL == 0 ESC value are valid.
  548. *
  549. * If so take care of the increased SF PCI size
  550. * (SF_PCI_SZ8) to point to the message content behind
  551. * the extended SF PCI info and get the real SF_DL
  552. * length value from the formerly first data byte.
  553. */
  554. if (sf_dl == 0)
  555. isotp_rcv_sf(sk, cf, SF_PCI_SZ8 + ae, skb,
  556. cf->data[SF_PCI_SZ4 + ae]);
  557. }
  558. }
  559. break;
  560. case N_PCI_FF:
  561. /* rx path: first frame */
  562. isotp_rcv_ff(sk, cf, ae);
  563. break;
  564. case N_PCI_CF:
  565. /* rx path: consecutive frame */
  566. isotp_rcv_cf(sk, cf, ae, skb);
  567. break;
  568. }
  569. out_unlock:
  570. spin_unlock(&so->rx_lock);
  571. }
  572. static void isotp_fill_dataframe(struct canfd_frame *cf, struct isotp_sock *so,
  573. int ae, int off)
  574. {
  575. int pcilen = N_PCI_SZ + ae + off;
  576. int space = so->tx.ll_dl - pcilen;
  577. int num = min_t(int, so->tx.len - so->tx.idx, space);
  578. int i;
  579. cf->can_id = so->txid;
  580. cf->len = num + pcilen;
  581. if (num < space) {
  582. if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
  583. /* user requested padding */
  584. cf->len = padlen(cf->len);
  585. memset(cf->data, so->opt.txpad_content, cf->len);
  586. } else if (cf->len > CAN_MAX_DLEN) {
  587. /* mandatory padding for CAN FD frames */
  588. cf->len = padlen(cf->len);
  589. memset(cf->data, CAN_ISOTP_DEFAULT_PAD_CONTENT,
  590. cf->len);
  591. }
  592. }
  593. for (i = 0; i < num; i++)
  594. cf->data[pcilen + i] = so->tx.buf[so->tx.idx++];
  595. if (ae)
  596. cf->data[0] = so->opt.ext_address;
  597. }
  598. static void isotp_create_fframe(struct canfd_frame *cf, struct isotp_sock *so,
  599. int ae)
  600. {
  601. int i;
  602. int ff_pci_sz;
  603. cf->can_id = so->txid;
  604. cf->len = so->tx.ll_dl;
  605. if (ae)
  606. cf->data[0] = so->opt.ext_address;
  607. /* create N_PCI bytes with 12/32 bit FF_DL data length */
  608. if (so->tx.len > 4095) {
  609. /* use 32 bit FF_DL notation */
  610. cf->data[ae] = N_PCI_FF;
  611. cf->data[ae + 1] = 0;
  612. cf->data[ae + 2] = (u8)(so->tx.len >> 24) & 0xFFU;
  613. cf->data[ae + 3] = (u8)(so->tx.len >> 16) & 0xFFU;
  614. cf->data[ae + 4] = (u8)(so->tx.len >> 8) & 0xFFU;
  615. cf->data[ae + 5] = (u8)so->tx.len & 0xFFU;
  616. ff_pci_sz = FF_PCI_SZ32;
  617. } else {
  618. /* use 12 bit FF_DL notation */
  619. cf->data[ae] = (u8)(so->tx.len >> 8) | N_PCI_FF;
  620. cf->data[ae + 1] = (u8)so->tx.len & 0xFFU;
  621. ff_pci_sz = FF_PCI_SZ12;
  622. }
  623. /* add first data bytes depending on ae */
  624. for (i = ae + ff_pci_sz; i < so->tx.ll_dl; i++)
  625. cf->data[i] = so->tx.buf[so->tx.idx++];
  626. so->tx.sn = 1;
  627. so->tx.state = ISOTP_WAIT_FIRST_FC;
  628. }
  629. static enum hrtimer_restart isotp_tx_timer_handler(struct hrtimer *hrtimer)
  630. {
  631. struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
  632. txtimer);
  633. struct sock *sk = &so->sk;
  634. struct sk_buff *skb;
  635. struct net_device *dev;
  636. struct canfd_frame *cf;
  637. enum hrtimer_restart restart = HRTIMER_NORESTART;
  638. int can_send_ret;
  639. int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
  640. switch (so->tx.state) {
  641. case ISOTP_WAIT_FC:
  642. case ISOTP_WAIT_FIRST_FC:
  643. /* we did not get any flow control frame in time */
  644. /* report 'communication error on send' */
  645. sk->sk_err = ECOMM;
  646. if (!sock_flag(sk, SOCK_DEAD))
  647. sk->sk_error_report(sk);
  648. /* reset tx state */
  649. so->tx.state = ISOTP_IDLE;
  650. wake_up_interruptible(&so->wait);
  651. break;
  652. case ISOTP_SENDING:
  653. /* push out the next segmented pdu */
  654. dev = dev_get_by_index(sock_net(sk), so->ifindex);
  655. if (!dev)
  656. break;
  657. isotp_tx_burst:
  658. skb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv),
  659. GFP_ATOMIC);
  660. if (!skb) {
  661. dev_put(dev);
  662. break;
  663. }
  664. can_skb_reserve(skb);
  665. can_skb_prv(skb)->ifindex = dev->ifindex;
  666. can_skb_prv(skb)->skbcnt = 0;
  667. cf = (struct canfd_frame *)skb->data;
  668. skb_put_zero(skb, so->ll.mtu);
  669. /* create consecutive frame */
  670. isotp_fill_dataframe(cf, so, ae, 0);
  671. /* place consecutive frame N_PCI in appropriate index */
  672. cf->data[ae] = N_PCI_CF | so->tx.sn++;
  673. so->tx.sn %= 16;
  674. so->tx.bs++;
  675. cf->flags = so->ll.tx_flags;
  676. skb->dev = dev;
  677. can_skb_set_owner(skb, sk);
  678. can_send_ret = can_send(skb, 1);
  679. if (can_send_ret)
  680. pr_notice_once("can-isotp: %s: can_send_ret %d\n",
  681. __func__, can_send_ret);
  682. if (so->tx.idx >= so->tx.len) {
  683. /* we are done */
  684. so->tx.state = ISOTP_IDLE;
  685. dev_put(dev);
  686. wake_up_interruptible(&so->wait);
  687. break;
  688. }
  689. if (so->txfc.bs && so->tx.bs >= so->txfc.bs) {
  690. /* stop and wait for FC */
  691. so->tx.state = ISOTP_WAIT_FC;
  692. dev_put(dev);
  693. hrtimer_set_expires(&so->txtimer,
  694. ktime_add(ktime_get(),
  695. ktime_set(1, 0)));
  696. restart = HRTIMER_RESTART;
  697. break;
  698. }
  699. /* no gap between data frames needed => use burst mode */
  700. if (!so->tx_gap)
  701. goto isotp_tx_burst;
  702. /* start timer to send next data frame with correct delay */
  703. dev_put(dev);
  704. hrtimer_set_expires(&so->txtimer,
  705. ktime_add(ktime_get(), so->tx_gap));
  706. restart = HRTIMER_RESTART;
  707. break;
  708. default:
  709. WARN_ON_ONCE(1);
  710. }
  711. return restart;
  712. }
  713. static int isotp_sendmsg(struct socket *sock, struct msghdr *msg, size_t size)
  714. {
  715. struct sock *sk = sock->sk;
  716. struct isotp_sock *so = isotp_sk(sk);
  717. u32 old_state = so->tx.state;
  718. struct sk_buff *skb;
  719. struct net_device *dev;
  720. struct canfd_frame *cf;
  721. int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
  722. int wait_tx_done = (so->opt.flags & CAN_ISOTP_WAIT_TX_DONE) ? 1 : 0;
  723. s64 hrtimer_sec = 0;
  724. int off;
  725. int err;
  726. if (!so->bound)
  727. return -EADDRNOTAVAIL;
  728. /* we do not support multiple buffers - for now */
  729. if (cmpxchg(&so->tx.state, ISOTP_IDLE, ISOTP_SENDING) != ISOTP_IDLE ||
  730. wq_has_sleeper(&so->wait)) {
  731. if (msg->msg_flags & MSG_DONTWAIT) {
  732. err = -EAGAIN;
  733. goto err_out;
  734. }
  735. /* wait for complete transmission of current pdu */
  736. err = wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
  737. if (err)
  738. goto err_out;
  739. }
  740. if (!size || size > MAX_MSG_LENGTH) {
  741. err = -EINVAL;
  742. goto err_out_drop;
  743. }
  744. /* take care of a potential SF_DL ESC offset for TX_DL > 8 */
  745. off = (so->tx.ll_dl > CAN_MAX_DLEN) ? 1 : 0;
  746. /* does the given data fit into a single frame for SF_BROADCAST? */
  747. if ((so->opt.flags & CAN_ISOTP_SF_BROADCAST) &&
  748. (size > so->tx.ll_dl - SF_PCI_SZ4 - ae - off)) {
  749. err = -EINVAL;
  750. goto err_out_drop;
  751. }
  752. err = memcpy_from_msg(so->tx.buf, msg, size);
  753. if (err < 0)
  754. goto err_out_drop;
  755. dev = dev_get_by_index(sock_net(sk), so->ifindex);
  756. if (!dev) {
  757. err = -ENXIO;
  758. goto err_out_drop;
  759. }
  760. skb = sock_alloc_send_skb(sk, so->ll.mtu + sizeof(struct can_skb_priv),
  761. msg->msg_flags & MSG_DONTWAIT, &err);
  762. if (!skb) {
  763. dev_put(dev);
  764. goto err_out_drop;
  765. }
  766. can_skb_reserve(skb);
  767. can_skb_prv(skb)->ifindex = dev->ifindex;
  768. can_skb_prv(skb)->skbcnt = 0;
  769. so->tx.len = size;
  770. so->tx.idx = 0;
  771. cf = (struct canfd_frame *)skb->data;
  772. skb_put_zero(skb, so->ll.mtu);
  773. /* check for single frame transmission depending on TX_DL */
  774. if (size <= so->tx.ll_dl - SF_PCI_SZ4 - ae - off) {
  775. /* The message size generally fits into a SingleFrame - good.
  776. *
  777. * SF_DL ESC offset optimization:
  778. *
  779. * When TX_DL is greater 8 but the message would still fit
  780. * into a 8 byte CAN frame, we can omit the offset.
  781. * This prevents a protocol caused length extension from
  782. * CAN_DL = 8 to CAN_DL = 12 due to the SF_SL ESC handling.
  783. */
  784. if (size <= CAN_MAX_DLEN - SF_PCI_SZ4 - ae)
  785. off = 0;
  786. isotp_fill_dataframe(cf, so, ae, off);
  787. /* place single frame N_PCI w/o length in appropriate index */
  788. cf->data[ae] = N_PCI_SF;
  789. /* place SF_DL size value depending on the SF_DL ESC offset */
  790. if (off)
  791. cf->data[SF_PCI_SZ4 + ae] = size;
  792. else
  793. cf->data[ae] |= size;
  794. so->tx.state = ISOTP_IDLE;
  795. wake_up_interruptible(&so->wait);
  796. /* don't enable wait queue for a single frame transmission */
  797. wait_tx_done = 0;
  798. } else {
  799. /* send first frame and wait for FC */
  800. isotp_create_fframe(cf, so, ae);
  801. /* start timeout for FC */
  802. hrtimer_sec = 1;
  803. hrtimer_start(&so->txtimer, ktime_set(hrtimer_sec, 0),
  804. HRTIMER_MODE_REL_SOFT);
  805. }
  806. /* send the first or only CAN frame */
  807. cf->flags = so->ll.tx_flags;
  808. skb->dev = dev;
  809. skb->sk = sk;
  810. err = can_send(skb, 1);
  811. dev_put(dev);
  812. if (err) {
  813. pr_notice_once("can-isotp: %s: can_send_ret %d\n",
  814. __func__, err);
  815. /* no transmission -> no timeout monitoring */
  816. if (hrtimer_sec)
  817. hrtimer_cancel(&so->txtimer);
  818. goto err_out_drop;
  819. }
  820. if (wait_tx_done) {
  821. /* wait for complete transmission of current pdu */
  822. wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
  823. if (sk->sk_err)
  824. return -sk->sk_err;
  825. }
  826. return size;
  827. err_out_drop:
  828. /* drop this PDU and unlock a potential wait queue */
  829. old_state = ISOTP_IDLE;
  830. err_out:
  831. so->tx.state = old_state;
  832. if (so->tx.state == ISOTP_IDLE)
  833. wake_up_interruptible(&so->wait);
  834. return err;
  835. }
  836. static int isotp_recvmsg(struct socket *sock, struct msghdr *msg, size_t size,
  837. int flags)
  838. {
  839. struct sock *sk = sock->sk;
  840. struct sk_buff *skb;
  841. struct isotp_sock *so = isotp_sk(sk);
  842. int noblock = flags & MSG_DONTWAIT;
  843. int ret = 0;
  844. if (flags & ~(MSG_DONTWAIT | MSG_TRUNC | MSG_PEEK))
  845. return -EINVAL;
  846. if (!so->bound)
  847. return -EADDRNOTAVAIL;
  848. flags &= ~MSG_DONTWAIT;
  849. skb = skb_recv_datagram(sk, flags, noblock, &ret);
  850. if (!skb)
  851. return ret;
  852. if (size < skb->len)
  853. msg->msg_flags |= MSG_TRUNC;
  854. else
  855. size = skb->len;
  856. ret = memcpy_to_msg(msg, skb->data, size);
  857. if (ret < 0)
  858. goto out_err;
  859. sock_recv_timestamp(msg, sk, skb);
  860. if (msg->msg_name) {
  861. __sockaddr_check_size(ISOTP_MIN_NAMELEN);
  862. msg->msg_namelen = ISOTP_MIN_NAMELEN;
  863. memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
  864. }
  865. /* set length of return value */
  866. ret = (flags & MSG_TRUNC) ? skb->len : size;
  867. out_err:
  868. skb_free_datagram(sk, skb);
  869. return ret;
  870. }
  871. static int isotp_release(struct socket *sock)
  872. {
  873. struct sock *sk = sock->sk;
  874. struct isotp_sock *so;
  875. struct net *net;
  876. if (!sk)
  877. return 0;
  878. so = isotp_sk(sk);
  879. net = sock_net(sk);
  880. /* wait for complete transmission of current pdu */
  881. wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
  882. spin_lock(&isotp_notifier_lock);
  883. while (isotp_busy_notifier == so) {
  884. spin_unlock(&isotp_notifier_lock);
  885. schedule_timeout_uninterruptible(1);
  886. spin_lock(&isotp_notifier_lock);
  887. }
  888. list_del(&so->notifier);
  889. spin_unlock(&isotp_notifier_lock);
  890. lock_sock(sk);
  891. /* remove current filters & unregister */
  892. if (so->bound && (!(so->opt.flags & CAN_ISOTP_SF_BROADCAST))) {
  893. if (so->ifindex) {
  894. struct net_device *dev;
  895. dev = dev_get_by_index(net, so->ifindex);
  896. if (dev) {
  897. can_rx_unregister(net, dev, so->rxid,
  898. SINGLE_MASK(so->rxid),
  899. isotp_rcv, sk);
  900. dev_put(dev);
  901. synchronize_rcu();
  902. }
  903. }
  904. }
  905. hrtimer_cancel(&so->txtimer);
  906. hrtimer_cancel(&so->rxtimer);
  907. so->ifindex = 0;
  908. so->bound = 0;
  909. sock_orphan(sk);
  910. sock->sk = NULL;
  911. release_sock(sk);
  912. sock_put(sk);
  913. return 0;
  914. }
  915. static int isotp_bind(struct socket *sock, struct sockaddr *uaddr, int len)
  916. {
  917. struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
  918. struct sock *sk = sock->sk;
  919. struct isotp_sock *so = isotp_sk(sk);
  920. struct net *net = sock_net(sk);
  921. int ifindex;
  922. struct net_device *dev;
  923. canid_t tx_id, rx_id;
  924. int err = 0;
  925. int notify_enetdown = 0;
  926. int do_rx_reg = 1;
  927. if (len < ISOTP_MIN_NAMELEN)
  928. return -EINVAL;
  929. /* sanitize tx/rx CAN identifiers */
  930. tx_id = addr->can_addr.tp.tx_id;
  931. if (tx_id & CAN_EFF_FLAG)
  932. tx_id &= (CAN_EFF_FLAG | CAN_EFF_MASK);
  933. else
  934. tx_id &= CAN_SFF_MASK;
  935. rx_id = addr->can_addr.tp.rx_id;
  936. if (rx_id & CAN_EFF_FLAG)
  937. rx_id &= (CAN_EFF_FLAG | CAN_EFF_MASK);
  938. else
  939. rx_id &= CAN_SFF_MASK;
  940. if (!addr->can_ifindex)
  941. return -ENODEV;
  942. lock_sock(sk);
  943. /* do not register frame reception for functional addressing */
  944. if (so->opt.flags & CAN_ISOTP_SF_BROADCAST)
  945. do_rx_reg = 0;
  946. /* do not validate rx address for functional addressing */
  947. if (do_rx_reg && rx_id == tx_id) {
  948. err = -EADDRNOTAVAIL;
  949. goto out;
  950. }
  951. if (so->bound && addr->can_ifindex == so->ifindex &&
  952. rx_id == so->rxid && tx_id == so->txid)
  953. goto out;
  954. dev = dev_get_by_index(net, addr->can_ifindex);
  955. if (!dev) {
  956. err = -ENODEV;
  957. goto out;
  958. }
  959. if (dev->type != ARPHRD_CAN) {
  960. dev_put(dev);
  961. err = -ENODEV;
  962. goto out;
  963. }
  964. if (dev->mtu < so->ll.mtu) {
  965. dev_put(dev);
  966. err = -EINVAL;
  967. goto out;
  968. }
  969. if (!(dev->flags & IFF_UP))
  970. notify_enetdown = 1;
  971. ifindex = dev->ifindex;
  972. if (do_rx_reg)
  973. can_rx_register(net, dev, rx_id, SINGLE_MASK(rx_id),
  974. isotp_rcv, sk, "isotp", sk);
  975. dev_put(dev);
  976. if (so->bound && do_rx_reg) {
  977. /* unregister old filter */
  978. if (so->ifindex) {
  979. dev = dev_get_by_index(net, so->ifindex);
  980. if (dev) {
  981. can_rx_unregister(net, dev, so->rxid,
  982. SINGLE_MASK(so->rxid),
  983. isotp_rcv, sk);
  984. dev_put(dev);
  985. }
  986. }
  987. }
  988. /* switch to new settings */
  989. so->ifindex = ifindex;
  990. so->rxid = rx_id;
  991. so->txid = tx_id;
  992. so->bound = 1;
  993. out:
  994. release_sock(sk);
  995. if (notify_enetdown) {
  996. sk->sk_err = ENETDOWN;
  997. if (!sock_flag(sk, SOCK_DEAD))
  998. sk->sk_error_report(sk);
  999. }
  1000. return err;
  1001. }
  1002. static int isotp_getname(struct socket *sock, struct sockaddr *uaddr, int peer)
  1003. {
  1004. struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
  1005. struct sock *sk = sock->sk;
  1006. struct isotp_sock *so = isotp_sk(sk);
  1007. if (peer)
  1008. return -EOPNOTSUPP;
  1009. memset(addr, 0, ISOTP_MIN_NAMELEN);
  1010. addr->can_family = AF_CAN;
  1011. addr->can_ifindex = so->ifindex;
  1012. addr->can_addr.tp.rx_id = so->rxid;
  1013. addr->can_addr.tp.tx_id = so->txid;
  1014. return ISOTP_MIN_NAMELEN;
  1015. }
  1016. static int isotp_setsockopt_locked(struct socket *sock, int level, int optname,
  1017. sockptr_t optval, unsigned int optlen)
  1018. {
  1019. struct sock *sk = sock->sk;
  1020. struct isotp_sock *so = isotp_sk(sk);
  1021. int ret = 0;
  1022. if (so->bound)
  1023. return -EISCONN;
  1024. switch (optname) {
  1025. case CAN_ISOTP_OPTS:
  1026. if (optlen != sizeof(struct can_isotp_options))
  1027. return -EINVAL;
  1028. if (copy_from_sockptr(&so->opt, optval, optlen))
  1029. return -EFAULT;
  1030. /* no separate rx_ext_address is given => use ext_address */
  1031. if (!(so->opt.flags & CAN_ISOTP_RX_EXT_ADDR))
  1032. so->opt.rx_ext_address = so->opt.ext_address;
  1033. /* check for frame_txtime changes (0 => no changes) */
  1034. if (so->opt.frame_txtime) {
  1035. if (so->opt.frame_txtime == CAN_ISOTP_FRAME_TXTIME_ZERO)
  1036. so->frame_txtime = 0;
  1037. else
  1038. so->frame_txtime = so->opt.frame_txtime;
  1039. }
  1040. break;
  1041. case CAN_ISOTP_RECV_FC:
  1042. if (optlen != sizeof(struct can_isotp_fc_options))
  1043. return -EINVAL;
  1044. if (copy_from_sockptr(&so->rxfc, optval, optlen))
  1045. return -EFAULT;
  1046. break;
  1047. case CAN_ISOTP_TX_STMIN:
  1048. if (optlen != sizeof(u32))
  1049. return -EINVAL;
  1050. if (copy_from_sockptr(&so->force_tx_stmin, optval, optlen))
  1051. return -EFAULT;
  1052. break;
  1053. case CAN_ISOTP_RX_STMIN:
  1054. if (optlen != sizeof(u32))
  1055. return -EINVAL;
  1056. if (copy_from_sockptr(&so->force_rx_stmin, optval, optlen))
  1057. return -EFAULT;
  1058. break;
  1059. case CAN_ISOTP_LL_OPTS:
  1060. if (optlen == sizeof(struct can_isotp_ll_options)) {
  1061. struct can_isotp_ll_options ll;
  1062. if (copy_from_sockptr(&ll, optval, optlen))
  1063. return -EFAULT;
  1064. /* check for correct ISO 11898-1 DLC data length */
  1065. if (ll.tx_dl != padlen(ll.tx_dl))
  1066. return -EINVAL;
  1067. if (ll.mtu != CAN_MTU && ll.mtu != CANFD_MTU)
  1068. return -EINVAL;
  1069. if (ll.mtu == CAN_MTU &&
  1070. (ll.tx_dl > CAN_MAX_DLEN || ll.tx_flags != 0))
  1071. return -EINVAL;
  1072. memcpy(&so->ll, &ll, sizeof(ll));
  1073. /* set ll_dl for tx path to similar place as for rx */
  1074. so->tx.ll_dl = ll.tx_dl;
  1075. } else {
  1076. return -EINVAL;
  1077. }
  1078. break;
  1079. default:
  1080. ret = -ENOPROTOOPT;
  1081. }
  1082. return ret;
  1083. }
  1084. static int isotp_setsockopt(struct socket *sock, int level, int optname,
  1085. sockptr_t optval, unsigned int optlen)
  1086. {
  1087. struct sock *sk = sock->sk;
  1088. int ret;
  1089. if (level != SOL_CAN_ISOTP)
  1090. return -EINVAL;
  1091. lock_sock(sk);
  1092. ret = isotp_setsockopt_locked(sock, level, optname, optval, optlen);
  1093. release_sock(sk);
  1094. return ret;
  1095. }
  1096. static int isotp_getsockopt(struct socket *sock, int level, int optname,
  1097. char __user *optval, int __user *optlen)
  1098. {
  1099. struct sock *sk = sock->sk;
  1100. struct isotp_sock *so = isotp_sk(sk);
  1101. int len;
  1102. void *val;
  1103. if (level != SOL_CAN_ISOTP)
  1104. return -EINVAL;
  1105. if (get_user(len, optlen))
  1106. return -EFAULT;
  1107. if (len < 0)
  1108. return -EINVAL;
  1109. switch (optname) {
  1110. case CAN_ISOTP_OPTS:
  1111. len = min_t(int, len, sizeof(struct can_isotp_options));
  1112. val = &so->opt;
  1113. break;
  1114. case CAN_ISOTP_RECV_FC:
  1115. len = min_t(int, len, sizeof(struct can_isotp_fc_options));
  1116. val = &so->rxfc;
  1117. break;
  1118. case CAN_ISOTP_TX_STMIN:
  1119. len = min_t(int, len, sizeof(u32));
  1120. val = &so->force_tx_stmin;
  1121. break;
  1122. case CAN_ISOTP_RX_STMIN:
  1123. len = min_t(int, len, sizeof(u32));
  1124. val = &so->force_rx_stmin;
  1125. break;
  1126. case CAN_ISOTP_LL_OPTS:
  1127. len = min_t(int, len, sizeof(struct can_isotp_ll_options));
  1128. val = &so->ll;
  1129. break;
  1130. default:
  1131. return -ENOPROTOOPT;
  1132. }
  1133. if (put_user(len, optlen))
  1134. return -EFAULT;
  1135. if (copy_to_user(optval, val, len))
  1136. return -EFAULT;
  1137. return 0;
  1138. }
  1139. static void isotp_notify(struct isotp_sock *so, unsigned long msg,
  1140. struct net_device *dev)
  1141. {
  1142. struct sock *sk = &so->sk;
  1143. if (!net_eq(dev_net(dev), sock_net(sk)))
  1144. return;
  1145. if (so->ifindex != dev->ifindex)
  1146. return;
  1147. switch (msg) {
  1148. case NETDEV_UNREGISTER:
  1149. lock_sock(sk);
  1150. /* remove current filters & unregister */
  1151. if (so->bound && (!(so->opt.flags & CAN_ISOTP_SF_BROADCAST)))
  1152. can_rx_unregister(dev_net(dev), dev, so->rxid,
  1153. SINGLE_MASK(so->rxid),
  1154. isotp_rcv, sk);
  1155. so->ifindex = 0;
  1156. so->bound = 0;
  1157. release_sock(sk);
  1158. sk->sk_err = ENODEV;
  1159. if (!sock_flag(sk, SOCK_DEAD))
  1160. sk->sk_error_report(sk);
  1161. break;
  1162. case NETDEV_DOWN:
  1163. sk->sk_err = ENETDOWN;
  1164. if (!sock_flag(sk, SOCK_DEAD))
  1165. sk->sk_error_report(sk);
  1166. break;
  1167. }
  1168. }
  1169. static int isotp_notifier(struct notifier_block *nb, unsigned long msg,
  1170. void *ptr)
  1171. {
  1172. struct net_device *dev = netdev_notifier_info_to_dev(ptr);
  1173. if (dev->type != ARPHRD_CAN)
  1174. return NOTIFY_DONE;
  1175. if (msg != NETDEV_UNREGISTER && msg != NETDEV_DOWN)
  1176. return NOTIFY_DONE;
  1177. if (unlikely(isotp_busy_notifier)) /* Check for reentrant bug. */
  1178. return NOTIFY_DONE;
  1179. spin_lock(&isotp_notifier_lock);
  1180. list_for_each_entry(isotp_busy_notifier, &isotp_notifier_list, notifier) {
  1181. spin_unlock(&isotp_notifier_lock);
  1182. isotp_notify(isotp_busy_notifier, msg, dev);
  1183. spin_lock(&isotp_notifier_lock);
  1184. }
  1185. isotp_busy_notifier = NULL;
  1186. spin_unlock(&isotp_notifier_lock);
  1187. return NOTIFY_DONE;
  1188. }
  1189. static int isotp_init(struct sock *sk)
  1190. {
  1191. struct isotp_sock *so = isotp_sk(sk);
  1192. so->ifindex = 0;
  1193. so->bound = 0;
  1194. so->opt.flags = CAN_ISOTP_DEFAULT_FLAGS;
  1195. so->opt.ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
  1196. so->opt.rx_ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
  1197. so->opt.rxpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT;
  1198. so->opt.txpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT;
  1199. so->opt.frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME;
  1200. so->frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME;
  1201. so->rxfc.bs = CAN_ISOTP_DEFAULT_RECV_BS;
  1202. so->rxfc.stmin = CAN_ISOTP_DEFAULT_RECV_STMIN;
  1203. so->rxfc.wftmax = CAN_ISOTP_DEFAULT_RECV_WFTMAX;
  1204. so->ll.mtu = CAN_ISOTP_DEFAULT_LL_MTU;
  1205. so->ll.tx_dl = CAN_ISOTP_DEFAULT_LL_TX_DL;
  1206. so->ll.tx_flags = CAN_ISOTP_DEFAULT_LL_TX_FLAGS;
  1207. /* set ll_dl for tx path to similar place as for rx */
  1208. so->tx.ll_dl = so->ll.tx_dl;
  1209. so->rx.state = ISOTP_IDLE;
  1210. so->tx.state = ISOTP_IDLE;
  1211. hrtimer_init(&so->rxtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
  1212. so->rxtimer.function = isotp_rx_timer_handler;
  1213. hrtimer_init(&so->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
  1214. so->txtimer.function = isotp_tx_timer_handler;
  1215. init_waitqueue_head(&so->wait);
  1216. spin_lock_init(&so->rx_lock);
  1217. spin_lock(&isotp_notifier_lock);
  1218. list_add_tail(&so->notifier, &isotp_notifier_list);
  1219. spin_unlock(&isotp_notifier_lock);
  1220. return 0;
  1221. }
  1222. static int isotp_sock_no_ioctlcmd(struct socket *sock, unsigned int cmd,
  1223. unsigned long arg)
  1224. {
  1225. /* no ioctls for socket layer -> hand it down to NIC layer */
  1226. return -ENOIOCTLCMD;
  1227. }
  1228. static const struct proto_ops isotp_ops = {
  1229. .family = PF_CAN,
  1230. .release = isotp_release,
  1231. .bind = isotp_bind,
  1232. .connect = sock_no_connect,
  1233. .socketpair = sock_no_socketpair,
  1234. .accept = sock_no_accept,
  1235. .getname = isotp_getname,
  1236. .poll = datagram_poll,
  1237. .ioctl = isotp_sock_no_ioctlcmd,
  1238. .gettstamp = sock_gettstamp,
  1239. .listen = sock_no_listen,
  1240. .shutdown = sock_no_shutdown,
  1241. .setsockopt = isotp_setsockopt,
  1242. .getsockopt = isotp_getsockopt,
  1243. .sendmsg = isotp_sendmsg,
  1244. .recvmsg = isotp_recvmsg,
  1245. .mmap = sock_no_mmap,
  1246. .sendpage = sock_no_sendpage,
  1247. };
  1248. static struct proto isotp_proto __read_mostly = {
  1249. .name = "CAN_ISOTP",
  1250. .owner = THIS_MODULE,
  1251. .obj_size = sizeof(struct isotp_sock),
  1252. .init = isotp_init,
  1253. };
  1254. static const struct can_proto isotp_can_proto = {
  1255. .type = SOCK_DGRAM,
  1256. .protocol = CAN_ISOTP,
  1257. .ops = &isotp_ops,
  1258. .prot = &isotp_proto,
  1259. };
  1260. static struct notifier_block canisotp_notifier = {
  1261. .notifier_call = isotp_notifier
  1262. };
  1263. static __init int isotp_module_init(void)
  1264. {
  1265. int err;
  1266. pr_info("can: isotp protocol\n");
  1267. err = can_proto_register(&isotp_can_proto);
  1268. if (err < 0)
  1269. pr_err("can: registration of isotp protocol failed\n");
  1270. else
  1271. register_netdevice_notifier(&canisotp_notifier);
  1272. return err;
  1273. }
  1274. static __exit void isotp_module_exit(void)
  1275. {
  1276. can_proto_unregister(&isotp_can_proto);
  1277. unregister_netdevice_notifier(&canisotp_notifier);
  1278. }
  1279. module_init(isotp_module_init);
  1280. module_exit(isotp_module_exit);