isotp.c 43 KB

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