rmnet_descriptor.c 43 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696
  1. /* Copyright (c) 2013-2021, The Linux Foundation. All rights reserved.
  2. *
  3. * This program is free software; you can redistribute it and/or modify
  4. * it under the terms of the GNU General Public License version 2 and
  5. * only version 2 as published by the Free Software Foundation.
  6. *
  7. * This program is distributed in the hope that it will be useful,
  8. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  9. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  10. * GNU General Public License for more details.
  11. *
  12. * RMNET Packet Descriptor Framework
  13. *
  14. */
  15. #include <linux/ip.h>
  16. #include <linux/ipv6.h>
  17. #include <net/ipv6.h>
  18. #include <net/ip6_checksum.h>
  19. #include "rmnet_config.h"
  20. #include "rmnet_descriptor.h"
  21. #include "rmnet_handlers.h"
  22. #include "rmnet_private.h"
  23. #include "rmnet_vnd.h"
  24. #include "rmnet_qmi.h"
  25. #include "qmi_rmnet.h"
  26. #define RMNET_FRAG_DESCRIPTOR_POOL_SIZE 64
  27. #define RMNET_DL_IND_HDR_SIZE (sizeof(struct rmnet_map_dl_ind_hdr) + \
  28. sizeof(struct rmnet_map_header) + \
  29. sizeof(struct rmnet_map_control_command_header))
  30. #define RMNET_DL_IND_TRL_SIZE (sizeof(struct rmnet_map_dl_ind_trl) + \
  31. sizeof(struct rmnet_map_header) + \
  32. sizeof(struct rmnet_map_control_command_header))
  33. typedef void (*rmnet_perf_desc_hook_t)(struct rmnet_frag_descriptor *frag_desc,
  34. struct rmnet_port *port);
  35. typedef void (*rmnet_perf_chain_hook_t)(void);
  36. typedef void (*rmnet_perf_tether_ingress_hook_t)(struct tcphdr *tp, struct sk_buff *skb);
  37. rmnet_perf_tether_ingress_hook_t rmnet_perf_tether_ingress_hook __rcu __read_mostly;
  38. EXPORT_SYMBOL(rmnet_perf_tether_ingress_hook);
  39. struct rmnet_frag_descriptor *
  40. rmnet_get_frag_descriptor(struct rmnet_port *port)
  41. {
  42. struct rmnet_frag_descriptor_pool *pool = port->frag_desc_pool;
  43. struct rmnet_frag_descriptor *frag_desc;
  44. unsigned long flags;
  45. spin_lock_irqsave(&port->desc_pool_lock, flags);
  46. if (!list_empty(&pool->free_list)) {
  47. frag_desc = list_first_entry(&pool->free_list,
  48. struct rmnet_frag_descriptor,
  49. list);
  50. list_del_init(&frag_desc->list);
  51. } else {
  52. frag_desc = kzalloc(sizeof(*frag_desc), GFP_ATOMIC);
  53. if (!frag_desc)
  54. goto out;
  55. INIT_LIST_HEAD(&frag_desc->list);
  56. INIT_LIST_HEAD(&frag_desc->frags);
  57. pool->pool_size++;
  58. }
  59. out:
  60. spin_unlock_irqrestore(&port->desc_pool_lock, flags);
  61. return frag_desc;
  62. }
  63. EXPORT_SYMBOL(rmnet_get_frag_descriptor);
  64. void rmnet_recycle_frag_descriptor(struct rmnet_frag_descriptor *frag_desc,
  65. struct rmnet_port *port)
  66. {
  67. struct rmnet_frag_descriptor_pool *pool = port->frag_desc_pool;
  68. struct rmnet_fragment *frag, *tmp;
  69. unsigned long flags;
  70. list_del(&frag_desc->list);
  71. list_for_each_entry_safe(frag, tmp, &frag_desc->frags, list) {
  72. struct page *page = skb_frag_page(&frag->frag);
  73. if (page)
  74. put_page(page);
  75. list_del(&frag->list);
  76. kfree(frag);
  77. }
  78. memset(frag_desc, 0, sizeof(*frag_desc));
  79. INIT_LIST_HEAD(&frag_desc->list);
  80. INIT_LIST_HEAD(&frag_desc->frags);
  81. spin_lock_irqsave(&port->desc_pool_lock, flags);
  82. list_add_tail(&frag_desc->list, &pool->free_list);
  83. spin_unlock_irqrestore(&port->desc_pool_lock, flags);
  84. }
  85. EXPORT_SYMBOL(rmnet_recycle_frag_descriptor);
  86. void *rmnet_frag_pull(struct rmnet_frag_descriptor *frag_desc,
  87. struct rmnet_port *port, unsigned int size)
  88. {
  89. struct rmnet_fragment *frag, *tmp;
  90. if (size >= frag_desc->len) {
  91. pr_info("%s(): Pulling %u bytes from %u byte pkt. Dropping\n",
  92. __func__, size, frag_desc->len);
  93. rmnet_recycle_frag_descriptor(frag_desc, port);
  94. return NULL;
  95. }
  96. list_for_each_entry_safe(frag, tmp, &frag_desc->frags, list) {
  97. u32 frag_size = skb_frag_size(&frag->frag);
  98. if (!size)
  99. break;
  100. if (size >= frag_size) {
  101. /* Remove the whole frag */
  102. struct page *page = skb_frag_page(&frag->frag);
  103. if (page)
  104. put_page(page);
  105. list_del(&frag->list);
  106. size -= frag_size;
  107. frag_desc->len -= frag_size;
  108. kfree(frag);
  109. continue;
  110. }
  111. /* Pull off 'size' bytes */
  112. skb_frag_off_add(&frag->frag, size);
  113. skb_frag_size_sub(&frag->frag, size);
  114. frag_desc->len -= size;
  115. break;
  116. }
  117. return rmnet_frag_data_ptr(frag_desc);
  118. }
  119. EXPORT_SYMBOL(rmnet_frag_pull);
  120. void *rmnet_frag_trim(struct rmnet_frag_descriptor *frag_desc,
  121. struct rmnet_port *port, unsigned int size)
  122. {
  123. struct rmnet_fragment *frag, *tmp;
  124. unsigned int eat;
  125. if (!size) {
  126. pr_info("%s(): Trimming %u byte pkt to 0. Dropping\n",
  127. __func__, frag_desc->len);
  128. rmnet_recycle_frag_descriptor(frag_desc, port);
  129. return NULL;
  130. }
  131. /* Growing bigger doesn't make sense */
  132. if (size >= frag_desc->len)
  133. goto out;
  134. /* Compute number of bytes to remove from the end */
  135. eat = frag_desc->len - size;
  136. list_for_each_entry_safe_reverse(frag, tmp, &frag_desc->frags, list) {
  137. u32 frag_size = skb_frag_size(&frag->frag);
  138. if (!eat)
  139. goto out;
  140. if (eat >= frag_size) {
  141. /* Remove the whole frag */
  142. struct page *page = skb_frag_page(&frag->frag);
  143. if (page)
  144. put_page(page);
  145. list_del(&frag->list);
  146. eat -= frag_size;
  147. frag_desc->len -= frag_size;
  148. kfree(frag);
  149. continue;
  150. }
  151. /* Chop off 'eat' bytes from the end */
  152. skb_frag_size_sub(&frag->frag, eat);
  153. frag_desc->len -= eat;
  154. goto out;
  155. }
  156. out:
  157. return rmnet_frag_data_ptr(frag_desc);
  158. }
  159. EXPORT_SYMBOL(rmnet_frag_trim);
  160. static int rmnet_frag_copy_data(struct rmnet_frag_descriptor *frag_desc,
  161. u32 off, u32 len, void *buf)
  162. {
  163. struct rmnet_fragment *frag;
  164. u32 frag_size, copy_len;
  165. u32 buf_offset = 0;
  166. /* Don't make me do something we'd both regret */
  167. if (off > frag_desc->len || len > frag_desc->len ||
  168. off + len > frag_desc->len)
  169. return -EINVAL;
  170. /* Copy 'len' bytes into the bufer starting from 'off' */
  171. list_for_each_entry(frag, &frag_desc->frags, list) {
  172. if (!len)
  173. break;
  174. frag_size = skb_frag_size(&frag->frag);
  175. if (off < frag_size) {
  176. copy_len = min_t(u32, len, frag_size - off);
  177. memcpy(buf + buf_offset,
  178. skb_frag_address(&frag->frag) + off,
  179. copy_len);
  180. buf_offset += copy_len;
  181. len -= copy_len;
  182. off = 0;
  183. } else {
  184. off -= frag_size;
  185. }
  186. }
  187. return 0;
  188. }
  189. void *rmnet_frag_header_ptr(struct rmnet_frag_descriptor *frag_desc, u32 off,
  190. u32 len, void *buf)
  191. {
  192. struct rmnet_fragment *frag;
  193. u8 *start;
  194. u32 frag_size, offset;
  195. /* Don't take a long pointer off a short frag */
  196. if (off > frag_desc->len || len > frag_desc->len ||
  197. off + len > frag_desc->len)
  198. return NULL;
  199. /* Find the starting fragment */
  200. offset = off;
  201. list_for_each_entry(frag, &frag_desc->frags, list) {
  202. frag_size = skb_frag_size(&frag->frag);
  203. if (off < frag_size) {
  204. start = skb_frag_address(&frag->frag) + off;
  205. /* If the header is entirely on this frag, just return
  206. * a pointer to it.
  207. */
  208. if (off + len <= frag_size)
  209. return start;
  210. /* Otherwise, we need to copy the data into a linear
  211. * buffer.
  212. */
  213. break;
  214. }
  215. off -= frag_size;
  216. }
  217. if (rmnet_frag_copy_data(frag_desc, offset, len, buf) < 0)
  218. return NULL;
  219. return buf;
  220. }
  221. EXPORT_SYMBOL(rmnet_frag_header_ptr);
  222. int rmnet_frag_descriptor_add_frag(struct rmnet_frag_descriptor *frag_desc,
  223. struct page *p, u32 page_offset, u32 len)
  224. {
  225. struct rmnet_fragment *frag;
  226. frag = kzalloc(sizeof(*frag), GFP_ATOMIC);
  227. if (!frag)
  228. return -ENOMEM;
  229. INIT_LIST_HEAD(&frag->list);
  230. get_page(p);
  231. __skb_frag_set_page(&frag->frag, p);
  232. skb_frag_size_set(&frag->frag, len);
  233. skb_frag_off_set(&frag->frag, page_offset);
  234. list_add_tail(&frag->list, &frag_desc->frags);
  235. frag_desc->len += len;
  236. return 0;
  237. }
  238. EXPORT_SYMBOL(rmnet_frag_descriptor_add_frag);
  239. int rmnet_frag_descriptor_add_frags_from(struct rmnet_frag_descriptor *to,
  240. struct rmnet_frag_descriptor *from,
  241. u32 off, u32 len)
  242. {
  243. struct rmnet_fragment *frag;
  244. int rc;
  245. /* Sanity check the lengths */
  246. if (off > from->len || len > from->len || off + len > from->len)
  247. return -EINVAL;
  248. list_for_each_entry(frag, &from->frags, list) {
  249. u32 frag_size;
  250. if (!len)
  251. break;
  252. frag_size = skb_frag_size(&frag->frag);
  253. if (off < frag_size) {
  254. struct page *p = skb_frag_page(&frag->frag);
  255. u32 page_off = skb_frag_off(&frag->frag);
  256. u32 copy_len = min_t(u32, len, frag_size - off);
  257. rc = rmnet_frag_descriptor_add_frag(to, p,
  258. page_off + off,
  259. copy_len);
  260. if (rc < 0)
  261. return rc;
  262. len -= copy_len;
  263. off = 0;
  264. } else {
  265. off -= frag_size;
  266. }
  267. }
  268. return 0;
  269. }
  270. EXPORT_SYMBOL(rmnet_frag_descriptor_add_frags_from);
  271. int rmnet_frag_ipv6_skip_exthdr(struct rmnet_frag_descriptor *frag_desc,
  272. int start, u8 *nexthdrp, __be16 *fragp)
  273. {
  274. u8 nexthdr = *nexthdrp;
  275. *fragp = 0;
  276. while (ipv6_ext_hdr(nexthdr)) {
  277. struct ipv6_opt_hdr *hp, __hp;
  278. int hdrlen;
  279. if (nexthdr == NEXTHDR_NONE)
  280. return -EINVAL;
  281. hp = rmnet_frag_header_ptr(frag_desc, (u32)start, sizeof(*hp),
  282. &__hp);
  283. if (!hp)
  284. return -EINVAL;
  285. if (nexthdr == NEXTHDR_FRAGMENT) {
  286. u32 off = offsetof(struct frag_hdr, frag_off);
  287. __be16 *fp, __fp;
  288. fp = rmnet_frag_header_ptr(frag_desc, (u32)start + off,
  289. sizeof(*fp), &__fp);
  290. if (!fp)
  291. return -EINVAL;
  292. *fragp = *fp;
  293. if (ntohs(*fragp) & ~0x7)
  294. break;
  295. hdrlen = 8;
  296. } else if (nexthdr == NEXTHDR_AUTH) {
  297. hdrlen = (hp->hdrlen + 2) << 2;
  298. } else {
  299. hdrlen = ipv6_optlen(hp);
  300. }
  301. nexthdr = hp->nexthdr;
  302. start += hdrlen;
  303. }
  304. *nexthdrp = nexthdr;
  305. return start;
  306. }
  307. EXPORT_SYMBOL(rmnet_frag_ipv6_skip_exthdr);
  308. static u8 rmnet_frag_do_flow_control(struct rmnet_map_header *qmap,
  309. struct rmnet_map_control_command *cmd,
  310. struct rmnet_port *port,
  311. int enable)
  312. {
  313. struct rmnet_endpoint *ep;
  314. struct net_device *vnd;
  315. u16 ip_family;
  316. u16 fc_seq;
  317. u32 qos_id;
  318. u8 mux_id;
  319. int r;
  320. mux_id = qmap->mux_id;
  321. if (mux_id >= RMNET_MAX_LOGICAL_EP)
  322. return RX_HANDLER_CONSUMED;
  323. ep = rmnet_get_endpoint(port, mux_id);
  324. if (!ep)
  325. return RX_HANDLER_CONSUMED;
  326. vnd = ep->egress_dev;
  327. ip_family = cmd->flow_control.ip_family;
  328. fc_seq = ntohs(cmd->flow_control.flow_control_seq_num);
  329. qos_id = ntohl(cmd->flow_control.qos_id);
  330. /* Ignore the ip family and pass the sequence number for both v4 and v6
  331. * sequence. User space does not support creating dedicated flows for
  332. * the 2 protocols
  333. */
  334. r = rmnet_vnd_do_flow_control(vnd, enable);
  335. if (r)
  336. return RMNET_MAP_COMMAND_UNSUPPORTED;
  337. else
  338. return RMNET_MAP_COMMAND_ACK;
  339. }
  340. static void rmnet_frag_send_ack(struct rmnet_map_header *qmap,
  341. unsigned char type,
  342. struct rmnet_port *port)
  343. {
  344. struct rmnet_map_control_command *cmd;
  345. struct net_device *dev = port->dev;
  346. struct sk_buff *skb;
  347. u16 alloc_len = ntohs(qmap->pkt_len) + sizeof(*qmap);
  348. skb = alloc_skb(alloc_len, GFP_ATOMIC);
  349. if (!skb)
  350. return;
  351. skb->protocol = htons(ETH_P_MAP);
  352. skb->dev = dev;
  353. cmd = rmnet_map_get_cmd_start(skb);
  354. cmd->cmd_type = type & 0x03;
  355. netif_tx_lock(dev);
  356. dev->netdev_ops->ndo_start_xmit(skb, dev);
  357. netif_tx_unlock(dev);
  358. }
  359. static void
  360. rmnet_frag_process_flow_start(struct rmnet_frag_descriptor *frag_desc,
  361. struct rmnet_map_control_command_header *cmd,
  362. struct rmnet_port *port,
  363. u16 cmd_len)
  364. {
  365. struct rmnet_map_dl_ind_hdr *dlhdr, __dlhdr;
  366. u32 offset = sizeof(struct rmnet_map_header);
  367. u32 data_format;
  368. bool is_dl_mark_v2;
  369. if (cmd_len + offset < RMNET_DL_IND_HDR_SIZE)
  370. return;
  371. data_format = port->data_format;
  372. is_dl_mark_v2 = data_format & RMNET_INGRESS_FORMAT_DL_MARKER_V2;
  373. dlhdr = rmnet_frag_header_ptr(frag_desc, offset + sizeof(*cmd),
  374. sizeof(*dlhdr), &__dlhdr);
  375. if (!dlhdr)
  376. return;
  377. port->stats.dl_hdr_last_ep_id = cmd->source_id;
  378. port->stats.dl_hdr_last_qmap_vers = cmd->reserved;
  379. port->stats.dl_hdr_last_trans_id = cmd->transaction_id;
  380. port->stats.dl_hdr_last_seq = dlhdr->le.seq;
  381. port->stats.dl_hdr_last_bytes = dlhdr->le.bytes;
  382. port->stats.dl_hdr_last_pkts = dlhdr->le.pkts;
  383. port->stats.dl_hdr_last_flows = dlhdr->le.flows;
  384. port->stats.dl_hdr_total_bytes += port->stats.dl_hdr_last_bytes;
  385. port->stats.dl_hdr_total_pkts += port->stats.dl_hdr_last_pkts;
  386. port->stats.dl_hdr_count++;
  387. /* If a target is taking frag path, we can assume DL marker v2 is in
  388. * play
  389. */
  390. if (is_dl_mark_v2)
  391. rmnet_map_dl_hdr_notify_v2(port, dlhdr, cmd);
  392. }
  393. static void
  394. rmnet_frag_process_flow_end(struct rmnet_frag_descriptor *frag_desc,
  395. struct rmnet_map_control_command_header *cmd,
  396. struct rmnet_port *port, u16 cmd_len)
  397. {
  398. struct rmnet_map_dl_ind_trl *dltrl, __dltrl;
  399. u32 offset = sizeof(struct rmnet_map_header);
  400. u32 data_format;
  401. bool is_dl_mark_v2;
  402. if (cmd_len + offset < RMNET_DL_IND_TRL_SIZE)
  403. return;
  404. data_format = port->data_format;
  405. is_dl_mark_v2 = data_format & RMNET_INGRESS_FORMAT_DL_MARKER_V2;
  406. dltrl = rmnet_frag_header_ptr(frag_desc, offset + sizeof(*cmd),
  407. sizeof(*dltrl), &__dltrl);
  408. if (!dltrl)
  409. return;
  410. port->stats.dl_trl_last_seq = dltrl->seq_le;
  411. port->stats.dl_trl_count++;
  412. /* If a target is taking frag path, we can assume DL marker v2 is in
  413. * play
  414. */
  415. if (is_dl_mark_v2)
  416. rmnet_map_dl_trl_notify_v2(port, dltrl, cmd);
  417. }
  418. /* Process MAP command frame and send N/ACK message as appropriate. Message cmd
  419. * name is decoded here and appropriate handler is called.
  420. */
  421. void rmnet_frag_command(struct rmnet_frag_descriptor *frag_desc,
  422. struct rmnet_map_header *qmap, struct rmnet_port *port)
  423. {
  424. struct rmnet_map_control_command *cmd, __cmd;
  425. unsigned char rc = 0;
  426. cmd = rmnet_frag_header_ptr(frag_desc, sizeof(*qmap), sizeof(*cmd),
  427. &__cmd);
  428. if (!cmd)
  429. return;
  430. switch (cmd->command_name) {
  431. case RMNET_MAP_COMMAND_FLOW_ENABLE:
  432. rc = rmnet_frag_do_flow_control(qmap, cmd, port, 1);
  433. break;
  434. case RMNET_MAP_COMMAND_FLOW_DISABLE:
  435. rc = rmnet_frag_do_flow_control(qmap, cmd, port, 0);
  436. break;
  437. default:
  438. rc = RMNET_MAP_COMMAND_UNSUPPORTED;
  439. break;
  440. }
  441. if (rc == RMNET_MAP_COMMAND_ACK)
  442. rmnet_frag_send_ack(qmap, rc, port);
  443. }
  444. int rmnet_frag_flow_command(struct rmnet_frag_descriptor *frag_desc,
  445. struct rmnet_port *port, u16 pkt_len)
  446. {
  447. struct rmnet_map_control_command_header *cmd, __cmd;
  448. cmd = rmnet_frag_header_ptr(frag_desc, sizeof(struct rmnet_map_header),
  449. sizeof(*cmd), &__cmd);
  450. if (!cmd)
  451. return -1;
  452. /* Silently discard any marksers recived over the LL channel */
  453. if (frag_desc->priority == 0xda1a &&
  454. (cmd->command_name == RMNET_MAP_COMMAND_FLOW_START ||
  455. cmd->command_name == RMNET_MAP_COMMAND_FLOW_END))
  456. return 0;
  457. switch (cmd->command_name) {
  458. case RMNET_MAP_COMMAND_FLOW_START:
  459. rmnet_frag_process_flow_start(frag_desc, cmd, port, pkt_len);
  460. break;
  461. case RMNET_MAP_COMMAND_FLOW_END:
  462. rmnet_frag_process_flow_end(frag_desc, cmd, port, pkt_len);
  463. break;
  464. default:
  465. return 1;
  466. }
  467. return 0;
  468. }
  469. EXPORT_SYMBOL(rmnet_frag_flow_command);
  470. static int rmnet_frag_deaggregate_one(struct sk_buff *skb,
  471. struct rmnet_port *port,
  472. struct list_head *list,
  473. u32 start, u32 priority)
  474. {
  475. struct skb_shared_info *shinfo = skb_shinfo(skb);
  476. struct rmnet_frag_descriptor *frag_desc;
  477. struct rmnet_map_header *maph, __maph;
  478. skb_frag_t *frag;
  479. u32 start_frag, offset, i;
  480. u32 start_frag_size, start_frag_off;
  481. u32 pkt_len, copy_len = 0;
  482. int rc;
  483. for (start_frag = 0, offset = 0; start_frag < shinfo->nr_frags;
  484. start_frag++) {
  485. frag = &shinfo->frags[start_frag];
  486. if (start < skb_frag_size(frag) + offset)
  487. break;
  488. offset += skb_frag_size(frag);
  489. }
  490. if (start_frag == shinfo->nr_frags)
  491. return -1;
  492. /* start - offset is the additional offset into the page to account
  493. * for any data on it we've already used.
  494. */
  495. start_frag_size = skb_frag_size(frag) - (start - offset);
  496. start_frag_off = skb_frag_off(frag) + (start - offset);
  497. /* Grab the QMAP header. Careful, as there's no guarantee that it's
  498. * continugous!
  499. */
  500. if (likely(start_frag_size >= sizeof(*maph))) {
  501. maph = skb_frag_address(frag) + (start - offset);
  502. } else {
  503. /* The header's split across pages. We can rebuild it.
  504. * Probably not faster or stronger than before. But certainly
  505. * more linear.
  506. */
  507. if (skb_copy_bits(skb, start, &__maph, sizeof(__maph)) < 0)
  508. return -1;
  509. maph = &__maph;
  510. }
  511. pkt_len = ntohs(maph->pkt_len);
  512. /* Catch empty frames */
  513. if (!pkt_len)
  514. return -1;
  515. frag_desc = rmnet_get_frag_descriptor(port);
  516. if (!frag_desc)
  517. return -1;
  518. frag_desc->priority = priority;
  519. pkt_len += sizeof(*maph);
  520. if (port->data_format & RMNET_FLAGS_INGRESS_MAP_CKSUMV4) {
  521. pkt_len += sizeof(struct rmnet_map_dl_csum_trailer);
  522. } else if ((port->data_format & (RMNET_FLAGS_INGRESS_MAP_CKSUMV5 |
  523. RMNET_FLAGS_INGRESS_COALESCE)) &&
  524. !maph->cd_bit) {
  525. u32 hsize = 0;
  526. u8 type;
  527. /* Check the type. This seems like should be overkill for less
  528. * than a single byte, doesn't it?
  529. */
  530. if (likely(start_frag_size >= sizeof(*maph) + 1)) {
  531. type = *((u8 *)maph + sizeof(*maph));
  532. } else {
  533. if (skb_copy_bits(skb, start + sizeof(*maph), &type,
  534. sizeof(type)) < 0)
  535. return -1;
  536. }
  537. /* Type only uses the first 7 bits */
  538. switch ((type & 0xFE) >> 1) {
  539. case RMNET_MAP_HEADER_TYPE_COALESCING:
  540. hsize = sizeof(struct rmnet_map_v5_coal_header);
  541. break;
  542. case RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD:
  543. hsize = sizeof(struct rmnet_map_v5_csum_header);
  544. break;
  545. }
  546. pkt_len += hsize;
  547. }
  548. /* Add all frags containing the packet data to the descriptor */
  549. for (i = start_frag; pkt_len > 0 && i < shinfo->nr_frags; ) {
  550. u32 size, off;
  551. u32 copy;
  552. frag = &shinfo->frags[i];
  553. size = skb_frag_size(frag);
  554. off = skb_frag_off(frag);
  555. if (i == start_frag) {
  556. /* These are different for the first one to account for
  557. * the starting offset.
  558. */
  559. size = start_frag_size;
  560. off = start_frag_off;
  561. }
  562. copy = min_t(u32, size, pkt_len);
  563. rc = rmnet_frag_descriptor_add_frag(frag_desc,
  564. skb_frag_page(frag), off,
  565. copy);
  566. if (rc < 0) {
  567. rmnet_recycle_frag_descriptor(frag_desc, port);
  568. return -1;
  569. }
  570. pkt_len -= copy;
  571. copy_len += copy;
  572. /* If the fragment is exhausted, we can move to the next one */
  573. if (!(size - copy_len)) {
  574. i++;
  575. copy_len = 0;
  576. }
  577. }
  578. if (pkt_len) {
  579. /* Packet length is larger than the amount of data we have */
  580. rmnet_recycle_frag_descriptor(frag_desc, port);
  581. return -1;
  582. }
  583. list_add_tail(&frag_desc->list, list);
  584. return (int)frag_desc->len;
  585. }
  586. void rmnet_frag_deaggregate(struct sk_buff *skb, struct rmnet_port *port,
  587. struct list_head *list, u32 priority)
  588. {
  589. u32 start = 0;
  590. int rc;
  591. while (start < skb->len) {
  592. rc = rmnet_frag_deaggregate_one(skb, port, list, start,
  593. priority);
  594. if (rc < 0)
  595. return;
  596. start += (u32)rc;
  597. }
  598. }
  599. /* Fill in GSO metadata to allow the SKB to be segmented by the NW stack
  600. * if needed (i.e. forwarding, UDP GRO)
  601. */
  602. static void rmnet_frag_gso_stamp(struct sk_buff *skb,
  603. struct rmnet_frag_descriptor *frag_desc)
  604. {
  605. struct skb_shared_info *shinfo = skb_shinfo(skb);
  606. if (frag_desc->trans_proto == IPPROTO_TCP)
  607. shinfo->gso_type = (frag_desc->ip_proto == 4) ?
  608. SKB_GSO_TCPV4 : SKB_GSO_TCPV6;
  609. else
  610. shinfo->gso_type = SKB_GSO_UDP_L4;
  611. shinfo->gso_size = frag_desc->gso_size;
  612. shinfo->gso_segs = frag_desc->gso_segs;
  613. }
  614. /* Set the partial checksum information. Sets the transport checksum to the
  615. * pseudoheader checksum and sets the offload metadata.
  616. */
  617. static void rmnet_frag_partial_csum(struct sk_buff *skb,
  618. struct rmnet_frag_descriptor *frag_desc)
  619. {
  620. rmnet_perf_tether_ingress_hook_t rmnet_perf_tether_ingress;
  621. struct iphdr *iph = (struct iphdr *)skb->data;
  622. __sum16 pseudo;
  623. u16 pkt_len = skb->len - frag_desc->ip_len;
  624. if (frag_desc->ip_proto == 4) {
  625. iph->tot_len = htons(skb->len);
  626. iph->check = 0;
  627. iph->check = ip_fast_csum(iph, iph->ihl);
  628. pseudo = ~csum_tcpudp_magic(iph->saddr, iph->daddr,
  629. pkt_len, frag_desc->trans_proto,
  630. 0);
  631. } else {
  632. struct ipv6hdr *ip6h = (struct ipv6hdr *)iph;
  633. /* Payload length includes any extension headers */
  634. ip6h->payload_len = htons(skb->len - sizeof(*ip6h));
  635. pseudo = ~csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr,
  636. pkt_len, frag_desc->trans_proto, 0);
  637. }
  638. if (frag_desc->trans_proto == IPPROTO_TCP) {
  639. struct tcphdr *tp = (struct tcphdr *)
  640. ((u8 *)iph + frag_desc->ip_len);
  641. tp->check = pseudo;
  642. skb->csum_offset = offsetof(struct tcphdr, check);
  643. rmnet_perf_tether_ingress = rcu_dereference(rmnet_perf_tether_ingress_hook);
  644. if (rmnet_perf_tether_ingress)
  645. rmnet_perf_tether_ingress(tp, skb);
  646. } else {
  647. struct udphdr *up = (struct udphdr *)
  648. ((u8 *)iph + frag_desc->ip_len);
  649. up->len = htons(pkt_len);
  650. up->check = pseudo;
  651. skb->csum_offset = offsetof(struct udphdr, check);
  652. }
  653. skb->ip_summed = CHECKSUM_PARTIAL;
  654. skb->csum_start = (u8 *)iph + frag_desc->ip_len - skb->head;
  655. }
  656. /* Allocate and populate an skb to contain the packet represented by the
  657. * frag descriptor.
  658. */
  659. static struct sk_buff *rmnet_alloc_skb(struct rmnet_frag_descriptor *frag_desc,
  660. struct rmnet_port *port)
  661. {
  662. struct sk_buff *head_skb, *current_skb, *skb;
  663. struct skb_shared_info *shinfo;
  664. struct rmnet_fragment *frag, *tmp;
  665. /* Use the exact sizes if we know them (i.e. RSB/RSC, rmnet_perf) */
  666. if (frag_desc->hdrs_valid) {
  667. u16 hdr_len = frag_desc->ip_len + frag_desc->trans_len;
  668. head_skb = alloc_skb(hdr_len + RMNET_MAP_DEAGGR_HEADROOM,
  669. GFP_ATOMIC);
  670. if (!head_skb)
  671. return NULL;
  672. skb_reserve(head_skb, RMNET_MAP_DEAGGR_HEADROOM);
  673. rmnet_frag_copy_data(frag_desc, 0, hdr_len,
  674. skb_put(head_skb, hdr_len));
  675. skb_reset_network_header(head_skb);
  676. if (frag_desc->trans_len)
  677. skb_set_transport_header(head_skb, frag_desc->ip_len);
  678. /* Pull the headers off carefully */
  679. if (hdr_len == frag_desc->len)
  680. /* Fast forward "header only" packets */
  681. goto skip_frags;
  682. if (!rmnet_frag_pull(frag_desc, port, hdr_len)) {
  683. kfree(head_skb);
  684. return NULL;
  685. }
  686. } else {
  687. /* Allocate enough space to avoid penalties in the stack
  688. * from __pskb_pull_tail()
  689. */
  690. head_skb = alloc_skb(256 + RMNET_MAP_DEAGGR_HEADROOM,
  691. GFP_ATOMIC);
  692. if (!head_skb)
  693. return NULL;
  694. skb_reserve(head_skb, RMNET_MAP_DEAGGR_HEADROOM);
  695. }
  696. shinfo = skb_shinfo(head_skb);
  697. current_skb = head_skb;
  698. /* Add in the page fragments */
  699. list_for_each_entry_safe(frag, tmp, &frag_desc->frags, list) {
  700. struct page *p = skb_frag_page(&frag->frag);
  701. u32 frag_size = skb_frag_size(&frag->frag);
  702. add_frag:
  703. if (shinfo->nr_frags < MAX_SKB_FRAGS) {
  704. get_page(p);
  705. skb_add_rx_frag(current_skb, shinfo->nr_frags, p,
  706. skb_frag_off(&frag->frag), frag_size,
  707. frag_size);
  708. if (current_skb != head_skb) {
  709. head_skb->len += frag_size;
  710. head_skb->data_len += frag_size;
  711. }
  712. } else {
  713. /* Alloc a new skb and try again */
  714. skb = alloc_skb(0, GFP_ATOMIC);
  715. if (!skb)
  716. break;
  717. if (current_skb == head_skb)
  718. shinfo->frag_list = skb;
  719. else
  720. current_skb->next = skb;
  721. current_skb = skb;
  722. shinfo = skb_shinfo(current_skb);
  723. goto add_frag;
  724. }
  725. }
  726. skip_frags:
  727. head_skb->dev = frag_desc->dev;
  728. rmnet_set_skb_proto(head_skb);
  729. /* Handle any header metadata that needs to be updated after RSB/RSC
  730. * segmentation
  731. */
  732. if (frag_desc->ip_id_set) {
  733. struct iphdr *iph;
  734. iph = (struct iphdr *)rmnet_map_data_ptr(head_skb);
  735. csum_replace2(&iph->check, iph->id, frag_desc->ip_id);
  736. iph->id = frag_desc->ip_id;
  737. }
  738. if (frag_desc->tcp_seq_set) {
  739. struct tcphdr *th;
  740. th = (struct tcphdr *)
  741. (rmnet_map_data_ptr(head_skb) + frag_desc->ip_len);
  742. th->seq = frag_desc->tcp_seq;
  743. }
  744. if (frag_desc->tcp_flags_set) {
  745. struct tcphdr *th;
  746. __be16 *flags;
  747. th = (struct tcphdr *)
  748. (rmnet_map_data_ptr(head_skb) + frag_desc->ip_len);
  749. flags = (__be16 *)&tcp_flag_word(th);
  750. *flags = frag_desc->tcp_flags;
  751. }
  752. /* Handle csum offloading */
  753. if (frag_desc->csum_valid && frag_desc->hdrs_valid) {
  754. /* Set the partial checksum information */
  755. rmnet_frag_partial_csum(head_skb, frag_desc);
  756. } else if (frag_desc->csum_valid) {
  757. /* Non-RSB/RSC/perf packet. The current checksum is fine */
  758. head_skb->ip_summed = CHECKSUM_UNNECESSARY;
  759. } else if (frag_desc->hdrs_valid &&
  760. (frag_desc->trans_proto == IPPROTO_TCP ||
  761. frag_desc->trans_proto == IPPROTO_UDP)) {
  762. /* Unfortunately, we have to fake a bad checksum here, since
  763. * the original bad value is lost by the hardware. The only
  764. * reliable way to do it is to calculate the actual checksum
  765. * and corrupt it.
  766. */
  767. __sum16 *check;
  768. __wsum csum;
  769. unsigned int offset = skb_transport_offset(head_skb);
  770. __sum16 pseudo;
  771. /* Calculate pseudo header and update header fields */
  772. if (frag_desc->ip_proto == 4) {
  773. struct iphdr *iph = ip_hdr(head_skb);
  774. __be16 tot_len = htons(head_skb->len);
  775. csum_replace2(&iph->check, iph->tot_len, tot_len);
  776. iph->tot_len = tot_len;
  777. pseudo = ~csum_tcpudp_magic(iph->saddr, iph->daddr,
  778. head_skb->len -
  779. frag_desc->ip_len,
  780. frag_desc->trans_proto, 0);
  781. } else {
  782. struct ipv6hdr *ip6h = ipv6_hdr(head_skb);
  783. ip6h->payload_len = htons(head_skb->len -
  784. sizeof(*ip6h));
  785. pseudo = ~csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr,
  786. head_skb->len -
  787. frag_desc->ip_len,
  788. frag_desc->trans_proto, 0);
  789. }
  790. if (frag_desc->trans_proto == IPPROTO_TCP) {
  791. check = &tcp_hdr(head_skb)->check;
  792. } else {
  793. udp_hdr(head_skb)->len = htons(head_skb->len -
  794. frag_desc->ip_len);
  795. check = &udp_hdr(head_skb)->check;
  796. }
  797. *check = pseudo;
  798. csum = skb_checksum(head_skb, offset, head_skb->len - offset,
  799. 0);
  800. /* Add 1 to corrupt. This cannot produce a final value of 0
  801. * since csum_fold() can't return a value of 0xFFFF
  802. */
  803. *check = csum16_add(csum_fold(csum), htons(1));
  804. head_skb->ip_summed = CHECKSUM_NONE;
  805. }
  806. /* Handle any rmnet_perf metadata */
  807. if (frag_desc->hash) {
  808. head_skb->hash = frag_desc->hash;
  809. head_skb->sw_hash = 1;
  810. }
  811. if (frag_desc->flush_shs)
  812. head_skb->cb[0] = 1;
  813. /* Handle coalesced packets */
  814. if (frag_desc->gso_segs > 1)
  815. rmnet_frag_gso_stamp(head_skb, frag_desc);
  816. /* Propagate original priority value */
  817. head_skb->priority = frag_desc->priority;
  818. return head_skb;
  819. }
  820. /* Deliver the packets contained within a frag descriptor */
  821. void rmnet_frag_deliver(struct rmnet_frag_descriptor *frag_desc,
  822. struct rmnet_port *port)
  823. {
  824. struct sk_buff *skb;
  825. skb = rmnet_alloc_skb(frag_desc, port);
  826. if (skb)
  827. rmnet_deliver_skb(skb, port);
  828. rmnet_recycle_frag_descriptor(frag_desc, port);
  829. }
  830. EXPORT_SYMBOL(rmnet_frag_deliver);
  831. static void __rmnet_frag_segment_data(struct rmnet_frag_descriptor *coal_desc,
  832. struct rmnet_port *port,
  833. struct list_head *list, u8 pkt_id,
  834. bool csum_valid)
  835. {
  836. struct rmnet_priv *priv = netdev_priv(coal_desc->dev);
  837. struct rmnet_frag_descriptor *new_desc;
  838. u32 dlen = coal_desc->gso_size * coal_desc->gso_segs;
  839. u32 hlen = coal_desc->ip_len + coal_desc->trans_len;
  840. u32 offset = hlen + coal_desc->data_offset;
  841. int rc;
  842. new_desc = rmnet_get_frag_descriptor(port);
  843. if (!new_desc)
  844. return;
  845. /* Header information and most metadata is the same as the original */
  846. memcpy(new_desc, coal_desc, sizeof(*coal_desc));
  847. INIT_LIST_HEAD(&new_desc->list);
  848. INIT_LIST_HEAD(&new_desc->frags);
  849. new_desc->len = 0;
  850. /* Add the header fragments */
  851. rc = rmnet_frag_descriptor_add_frags_from(new_desc, coal_desc, 0,
  852. hlen);
  853. if (rc < 0)
  854. goto recycle;
  855. /* Add in the data fragments */
  856. rc = rmnet_frag_descriptor_add_frags_from(new_desc, coal_desc, offset,
  857. dlen);
  858. if (rc < 0)
  859. goto recycle;
  860. /* Update protocol-specific metadata */
  861. if (coal_desc->trans_proto == IPPROTO_TCP) {
  862. struct tcphdr *th, __th;
  863. th = rmnet_frag_header_ptr(coal_desc, coal_desc->ip_len,
  864. sizeof(*th), &__th);
  865. if (!th)
  866. goto recycle;
  867. new_desc->tcp_seq_set = 1;
  868. new_desc->tcp_seq = htonl(ntohl(th->seq) +
  869. coal_desc->data_offset);
  870. /* Don't allow any dangerous flags to appear in any segments
  871. * other than the last.
  872. */
  873. if (th->fin || th->psh) {
  874. if (offset + dlen < coal_desc->len) {
  875. __be32 flag_word = tcp_flag_word(th);
  876. /* Clear the FIN and PSH flags from this
  877. * segment.
  878. */
  879. flag_word &= ~TCP_FLAG_FIN;
  880. flag_word &= ~TCP_FLAG_PSH;
  881. new_desc->tcp_flags_set = 1;
  882. new_desc->tcp_flags = *((__be16 *)&flag_word);
  883. }
  884. }
  885. } else if (coal_desc->trans_proto == IPPROTO_UDP) {
  886. struct udphdr *uh, __uh;
  887. uh = rmnet_frag_header_ptr(coal_desc, coal_desc->ip_len,
  888. sizeof(*uh), &__uh);
  889. if (!uh)
  890. goto recycle;
  891. if (coal_desc->ip_proto == 4 && !uh->check)
  892. csum_valid = true;
  893. }
  894. if (coal_desc->ip_proto == 4) {
  895. struct iphdr *iph, __iph;
  896. iph = rmnet_frag_header_ptr(coal_desc, 0, sizeof(*iph),
  897. &__iph);
  898. if (!iph)
  899. goto recycle;
  900. new_desc->ip_id_set = 1;
  901. new_desc->ip_id = htons(ntohs(iph->id) + coal_desc->pkt_id);
  902. }
  903. new_desc->csum_valid = csum_valid;
  904. priv->stats.coal.coal_reconstruct++;
  905. /* Update meta information to move past the data we just segmented */
  906. coal_desc->data_offset += dlen;
  907. coal_desc->pkt_id = pkt_id + 1;
  908. coal_desc->gso_segs = 0;
  909. list_add_tail(&new_desc->list, list);
  910. return;
  911. recycle:
  912. rmnet_recycle_frag_descriptor(new_desc, port);
  913. }
  914. static bool rmnet_frag_validate_csum(struct rmnet_frag_descriptor *frag_desc)
  915. {
  916. u8 *data = rmnet_frag_data_ptr(frag_desc);
  917. unsigned int datagram_len;
  918. __wsum csum;
  919. __sum16 pseudo;
  920. /* Keep analysis tools happy, since they will see that
  921. * rmnet_frag_data_ptr() could return NULL. It can't in this case,
  922. * since we can't get this far otherwise...
  923. */
  924. if (unlikely(!data))
  925. return false;
  926. datagram_len = frag_desc->len - frag_desc->ip_len;
  927. if (frag_desc->ip_proto == 4) {
  928. struct iphdr *iph = (struct iphdr *)data;
  929. pseudo = ~csum_tcpudp_magic(iph->saddr, iph->daddr,
  930. datagram_len,
  931. frag_desc->trans_proto, 0);
  932. } else {
  933. struct ipv6hdr *ip6h = (struct ipv6hdr *)data;
  934. pseudo = ~csum_ipv6_magic(&ip6h->saddr, &ip6h->daddr,
  935. datagram_len, frag_desc->trans_proto,
  936. 0);
  937. }
  938. csum = csum_partial(data + frag_desc->ip_len, datagram_len,
  939. csum_unfold(pseudo));
  940. return !csum_fold(csum);
  941. }
  942. /* Converts the coalesced frame into a list of descriptors.
  943. * NLOs containing csum erros will not be included.
  944. */
  945. static void
  946. rmnet_frag_segment_coal_data(struct rmnet_frag_descriptor *coal_desc,
  947. u64 nlo_err_mask, struct rmnet_port *port,
  948. struct list_head *list)
  949. {
  950. struct rmnet_priv *priv = netdev_priv(coal_desc->dev);
  951. struct rmnet_map_v5_coal_header coal_hdr;
  952. u8 *version;
  953. u16 pkt_len;
  954. u8 pkt, total_pkt = 0;
  955. u8 nlo;
  956. bool gro = coal_desc->dev->features & NETIF_F_GRO_HW;
  957. bool zero_csum = false;
  958. /* Copy the coal header into our local storage before pulling it. It's
  959. * possible that this header (or part of it) is the last port of a page
  960. * a pulling it off would cause it to be freed. Referring back to the
  961. * header would be invalid in that case.
  962. */
  963. if (rmnet_frag_copy_data(coal_desc, sizeof(struct rmnet_map_header),
  964. sizeof(coal_hdr), &coal_hdr) < 0)
  965. return;
  966. /* Pull off the headers we no longer need */
  967. if (!rmnet_frag_pull(coal_desc, port, sizeof(struct rmnet_map_header) +
  968. sizeof(coal_hdr)))
  969. return;
  970. /* By definition, this byte is linear, and the first byte on the
  971. * first fragment. ;) Hence why no header_ptr() call is needed
  972. * for it.
  973. */
  974. version = rmnet_frag_data_ptr(coal_desc);
  975. if (unlikely(!version))
  976. return;
  977. if ((*version & 0xF0) == 0x40) {
  978. struct iphdr *iph, __iph;
  979. iph = rmnet_frag_header_ptr(coal_desc, 0, sizeof(*iph),
  980. &__iph);
  981. if (!iph)
  982. return;
  983. coal_desc->ip_proto = 4;
  984. coal_desc->ip_len = iph->ihl * 4;
  985. coal_desc->trans_proto = iph->protocol;
  986. /* Don't allow coalescing of any packets with IP options */
  987. if (iph->ihl != 5)
  988. gro = false;
  989. } else if ((*version & 0xF0) == 0x60) {
  990. struct ipv6hdr *ip6h, __ip6h;
  991. int ip_len;
  992. __be16 frag_off;
  993. u8 protocol;
  994. ip6h = rmnet_frag_header_ptr(coal_desc, 0, sizeof(*ip6h),
  995. &__ip6h);
  996. if (!ip6h)
  997. return;
  998. coal_desc->ip_proto = 6;
  999. protocol = ip6h->nexthdr;
  1000. ip_len = rmnet_frag_ipv6_skip_exthdr(coal_desc,
  1001. sizeof(*ip6h),
  1002. &protocol,
  1003. &frag_off);
  1004. coal_desc->trans_proto = protocol;
  1005. /* If we run into a problem, or this has a fragment header
  1006. * (which should technically not be possible, if the HW
  1007. * works as intended...), bail.
  1008. */
  1009. if (ip_len < 0 || frag_off) {
  1010. priv->stats.coal.coal_ip_invalid++;
  1011. return;
  1012. }
  1013. coal_desc->ip_len = (u16)ip_len;
  1014. if (coal_desc->ip_len > sizeof(*ip6h)) {
  1015. /* Don't allow coalescing of any packets with IPv6
  1016. * extension headers.
  1017. */
  1018. gro = false;
  1019. }
  1020. } else {
  1021. priv->stats.coal.coal_ip_invalid++;
  1022. return;
  1023. }
  1024. if (coal_desc->trans_proto == IPPROTO_TCP) {
  1025. struct tcphdr *th, __th;
  1026. th = rmnet_frag_header_ptr(coal_desc,
  1027. coal_desc->ip_len, sizeof(*th),
  1028. &__th);
  1029. if (!th)
  1030. return;
  1031. coal_desc->trans_len = th->doff * 4;
  1032. priv->stats.coal.coal_tcp++;
  1033. priv->stats.coal.coal_tcp_bytes += coal_desc->len;
  1034. } else if (coal_desc->trans_proto == IPPROTO_UDP) {
  1035. struct udphdr *uh, __uh;
  1036. uh = rmnet_frag_header_ptr(coal_desc,
  1037. coal_desc->ip_len, sizeof(*uh),
  1038. &__uh);
  1039. if (!uh)
  1040. return;
  1041. coal_desc->trans_len = sizeof(*uh);
  1042. priv->stats.coal.coal_udp++;
  1043. priv->stats.coal.coal_udp_bytes += coal_desc->len;
  1044. if (coal_desc->ip_proto == 4 && !uh->check)
  1045. zero_csum = true;
  1046. } else {
  1047. priv->stats.coal.coal_trans_invalid++;
  1048. return;
  1049. }
  1050. coal_desc->hdrs_valid = 1;
  1051. if (rmnet_map_v5_csum_buggy(&coal_hdr) && !zero_csum) {
  1052. /* Mark the checksum as valid if it checks out */
  1053. if (rmnet_frag_validate_csum(coal_desc))
  1054. coal_desc->csum_valid = true;
  1055. coal_desc->gso_size = ntohs(coal_hdr.nl_pairs[0].pkt_len);
  1056. coal_desc->gso_size -= coal_desc->ip_len + coal_desc->trans_len;
  1057. coal_desc->gso_segs = coal_hdr.nl_pairs[0].num_packets;
  1058. list_add_tail(&coal_desc->list, list);
  1059. return;
  1060. }
  1061. /* Fast-forward the case where we have 1 NLO (i.e. 1 packet length),
  1062. * no checksum errors, and are allowing GRO. We can just reuse this
  1063. * descriptor unchanged.
  1064. */
  1065. if (gro && coal_hdr.num_nlos == 1 && coal_hdr.csum_valid) {
  1066. coal_desc->csum_valid = true;
  1067. coal_desc->gso_size = ntohs(coal_hdr.nl_pairs[0].pkt_len);
  1068. coal_desc->gso_size -= coal_desc->ip_len + coal_desc->trans_len;
  1069. coal_desc->gso_segs = coal_hdr.nl_pairs[0].num_packets;
  1070. list_add_tail(&coal_desc->list, list);
  1071. return;
  1072. }
  1073. /* Segment the coalesced descriptor into new packets */
  1074. for (nlo = 0; nlo < coal_hdr.num_nlos; nlo++) {
  1075. pkt_len = ntohs(coal_hdr.nl_pairs[nlo].pkt_len);
  1076. pkt_len -= coal_desc->ip_len + coal_desc->trans_len;
  1077. coal_desc->gso_size = pkt_len;
  1078. for (pkt = 0; pkt < coal_hdr.nl_pairs[nlo].num_packets;
  1079. pkt++, total_pkt++, nlo_err_mask >>= 1) {
  1080. bool csum_err = nlo_err_mask & 1;
  1081. /* Segment the packet if we're not sending the larger
  1082. * packet up the stack.
  1083. */
  1084. if (!gro) {
  1085. coal_desc->gso_segs = 1;
  1086. if (csum_err)
  1087. priv->stats.coal.coal_csum_err++;
  1088. __rmnet_frag_segment_data(coal_desc, port,
  1089. list, total_pkt,
  1090. !csum_err);
  1091. continue;
  1092. }
  1093. if (csum_err) {
  1094. priv->stats.coal.coal_csum_err++;
  1095. /* Segment out the good data */
  1096. if (coal_desc->gso_segs)
  1097. __rmnet_frag_segment_data(coal_desc,
  1098. port,
  1099. list,
  1100. total_pkt,
  1101. true);
  1102. /* Segment out the bad checksum */
  1103. coal_desc->gso_segs = 1;
  1104. __rmnet_frag_segment_data(coal_desc, port,
  1105. list, total_pkt,
  1106. false);
  1107. } else {
  1108. coal_desc->gso_segs++;
  1109. }
  1110. }
  1111. /* If we're switching NLOs, we need to send out everything from
  1112. * the previous one, if we haven't done so. NLOs only switch
  1113. * when the packet length changes.
  1114. */
  1115. if (coal_desc->gso_segs)
  1116. __rmnet_frag_segment_data(coal_desc, port, list,
  1117. total_pkt, true);
  1118. }
  1119. }
  1120. /* Record reason for coalescing pipe closure */
  1121. static void rmnet_frag_data_log_close_stats(struct rmnet_priv *priv, u8 type,
  1122. u8 code)
  1123. {
  1124. struct rmnet_coal_close_stats *stats = &priv->stats.coal.close;
  1125. switch (type) {
  1126. case RMNET_MAP_COAL_CLOSE_NON_COAL:
  1127. stats->non_coal++;
  1128. break;
  1129. case RMNET_MAP_COAL_CLOSE_IP_MISS:
  1130. stats->ip_miss++;
  1131. break;
  1132. case RMNET_MAP_COAL_CLOSE_TRANS_MISS:
  1133. stats->trans_miss++;
  1134. break;
  1135. case RMNET_MAP_COAL_CLOSE_HW:
  1136. switch (code) {
  1137. case RMNET_MAP_COAL_CLOSE_HW_NL:
  1138. stats->hw_nl++;
  1139. break;
  1140. case RMNET_MAP_COAL_CLOSE_HW_PKT:
  1141. stats->hw_pkt++;
  1142. break;
  1143. case RMNET_MAP_COAL_CLOSE_HW_BYTE:
  1144. stats->hw_byte++;
  1145. break;
  1146. case RMNET_MAP_COAL_CLOSE_HW_TIME:
  1147. stats->hw_time++;
  1148. break;
  1149. case RMNET_MAP_COAL_CLOSE_HW_EVICT:
  1150. stats->hw_evict++;
  1151. break;
  1152. default:
  1153. break;
  1154. }
  1155. break;
  1156. case RMNET_MAP_COAL_CLOSE_COAL:
  1157. stats->coal++;
  1158. break;
  1159. default:
  1160. break;
  1161. }
  1162. }
  1163. /* Check if the coalesced header has any incorrect values, in which case, the
  1164. * entire coalesced frame must be dropped. Then check if there are any
  1165. * checksum issues
  1166. */
  1167. static int
  1168. rmnet_frag_data_check_coal_header(struct rmnet_frag_descriptor *frag_desc,
  1169. u64 *nlo_err_mask)
  1170. {
  1171. struct rmnet_map_v5_coal_header *coal_hdr, __coal_hdr;
  1172. struct rmnet_priv *priv = netdev_priv(frag_desc->dev);
  1173. u64 mask = 0;
  1174. int i;
  1175. u8 veid, pkts = 0;
  1176. coal_hdr = rmnet_frag_header_ptr(frag_desc,
  1177. sizeof(struct rmnet_map_header),
  1178. sizeof(*coal_hdr), &__coal_hdr);
  1179. if (!coal_hdr)
  1180. return -EINVAL;
  1181. veid = coal_hdr->virtual_channel_id;
  1182. if (coal_hdr->num_nlos == 0 ||
  1183. coal_hdr->num_nlos > RMNET_MAP_V5_MAX_NLOS) {
  1184. priv->stats.coal.coal_hdr_nlo_err++;
  1185. return -EINVAL;
  1186. }
  1187. for (i = 0; i < RMNET_MAP_V5_MAX_NLOS; i++) {
  1188. /* If there is a checksum issue, we need to split
  1189. * up the skb. Rebuild the full csum error field
  1190. */
  1191. u8 err = coal_hdr->nl_pairs[i].csum_error_bitmap;
  1192. u8 pkt = coal_hdr->nl_pairs[i].num_packets;
  1193. mask |= ((u64)err) << (8 * i);
  1194. /* Track total packets in frame */
  1195. pkts += pkt;
  1196. if (pkts > RMNET_MAP_V5_MAX_PACKETS) {
  1197. priv->stats.coal.coal_hdr_pkt_err++;
  1198. return -EINVAL;
  1199. }
  1200. }
  1201. /* Track number of packets we get inside of coalesced frames */
  1202. priv->stats.coal.coal_pkts += pkts;
  1203. /* Update ethtool stats */
  1204. rmnet_frag_data_log_close_stats(priv,
  1205. coal_hdr->close_type,
  1206. coal_hdr->close_value);
  1207. if (veid < RMNET_MAX_VEID)
  1208. priv->stats.coal.coal_veid[veid]++;
  1209. *nlo_err_mask = mask;
  1210. return 0;
  1211. }
  1212. /* Process a QMAPv5 packet header */
  1213. int rmnet_frag_process_next_hdr_packet(struct rmnet_frag_descriptor *frag_desc,
  1214. struct rmnet_port *port,
  1215. struct list_head *list,
  1216. u16 len)
  1217. {
  1218. struct rmnet_map_v5_csum_header *csum_hdr, __csum_hdr;
  1219. struct rmnet_priv *priv = netdev_priv(frag_desc->dev);
  1220. u64 nlo_err_mask;
  1221. u32 offset = sizeof(struct rmnet_map_header);
  1222. int rc = 0;
  1223. /* Grab the header type. It's easier to grab enough for a full csum
  1224. * offload header here since it's only 8 bytes and then check the
  1225. * header type using that. This also doubles as a check to make sure
  1226. * there's enough data after the QMAP header to ensure that another
  1227. * header is present.
  1228. */
  1229. csum_hdr = rmnet_frag_header_ptr(frag_desc, offset, sizeof(*csum_hdr),
  1230. &__csum_hdr);
  1231. if (!csum_hdr)
  1232. return -EINVAL;
  1233. switch (csum_hdr->header_type) {
  1234. case RMNET_MAP_HEADER_TYPE_COALESCING:
  1235. priv->stats.coal.coal_rx++;
  1236. rc = rmnet_frag_data_check_coal_header(frag_desc,
  1237. &nlo_err_mask);
  1238. if (rc)
  1239. return rc;
  1240. rmnet_frag_segment_coal_data(frag_desc, nlo_err_mask, port,
  1241. list);
  1242. if (list_first_entry(list, struct rmnet_frag_descriptor,
  1243. list) != frag_desc)
  1244. rmnet_recycle_frag_descriptor(frag_desc, port);
  1245. break;
  1246. case RMNET_MAP_HEADER_TYPE_CSUM_OFFLOAD:
  1247. if (unlikely(!(frag_desc->dev->features & NETIF_F_RXCSUM))) {
  1248. priv->stats.csum_sw++;
  1249. } else if (csum_hdr->csum_valid_required) {
  1250. priv->stats.csum_ok++;
  1251. frag_desc->csum_valid = true;
  1252. } else {
  1253. priv->stats.csum_valid_unset++;
  1254. }
  1255. if (!rmnet_frag_pull(frag_desc, port,
  1256. offset + sizeof(*csum_hdr))) {
  1257. rc = -EINVAL;
  1258. break;
  1259. }
  1260. /* Remove padding only for csum offload packets.
  1261. * Coalesced packets should never have padding.
  1262. */
  1263. if (!rmnet_frag_trim(frag_desc, port, len)) {
  1264. rc = -EINVAL;
  1265. break;
  1266. }
  1267. list_del_init(&frag_desc->list);
  1268. list_add_tail(&frag_desc->list, list);
  1269. break;
  1270. default:
  1271. rc = -EINVAL;
  1272. break;
  1273. }
  1274. return rc;
  1275. }
  1276. /* Perf hook handler */
  1277. rmnet_perf_desc_hook_t rmnet_perf_desc_entry __rcu __read_mostly;
  1278. EXPORT_SYMBOL(rmnet_perf_desc_entry);
  1279. static void
  1280. __rmnet_frag_ingress_handler(struct rmnet_frag_descriptor *frag_desc,
  1281. struct rmnet_port *port)
  1282. {
  1283. rmnet_perf_desc_hook_t rmnet_perf_ingress;
  1284. struct rmnet_map_header *qmap, __qmap;
  1285. struct rmnet_endpoint *ep;
  1286. struct rmnet_frag_descriptor *frag, *tmp;
  1287. LIST_HEAD(segs);
  1288. u16 len, pad;
  1289. u8 mux_id;
  1290. bool skip_perf = (frag_desc->priority == 0xda1a);
  1291. qmap = rmnet_frag_header_ptr(frag_desc, 0, sizeof(*qmap), &__qmap);
  1292. if (!qmap)
  1293. goto recycle;
  1294. mux_id = qmap->mux_id;
  1295. pad = qmap->pad_len;
  1296. len = ntohs(qmap->pkt_len) - pad;
  1297. if (qmap->cd_bit) {
  1298. qmi_rmnet_set_dl_msg_active(port);
  1299. if (port->data_format & RMNET_INGRESS_FORMAT_DL_MARKER) {
  1300. rmnet_frag_flow_command(frag_desc, port, len);
  1301. goto recycle;
  1302. }
  1303. if (port->data_format & RMNET_FLAGS_INGRESS_MAP_COMMANDS)
  1304. rmnet_frag_command(frag_desc, qmap, port);
  1305. goto recycle;
  1306. }
  1307. if (mux_id >= RMNET_MAX_LOGICAL_EP)
  1308. goto recycle;
  1309. ep = rmnet_get_endpoint(port, mux_id);
  1310. if (!ep)
  1311. goto recycle;
  1312. frag_desc->dev = ep->egress_dev;
  1313. /* Handle QMAPv5 packet */
  1314. if (qmap->next_hdr &&
  1315. (port->data_format & (RMNET_FLAGS_INGRESS_COALESCE |
  1316. RMNET_FLAGS_INGRESS_MAP_CKSUMV5))) {
  1317. if (rmnet_frag_process_next_hdr_packet(frag_desc, port, &segs,
  1318. len))
  1319. goto recycle;
  1320. } else {
  1321. /* We only have the main QMAP header to worry about */
  1322. if (!rmnet_frag_pull(frag_desc, port, sizeof(*qmap)))
  1323. return;
  1324. if (!rmnet_frag_trim(frag_desc, port, len))
  1325. return;
  1326. list_add_tail(&frag_desc->list, &segs);
  1327. }
  1328. if (port->data_format & RMNET_INGRESS_FORMAT_PS)
  1329. qmi_rmnet_work_maybe_restart(port);
  1330. if (skip_perf)
  1331. goto no_perf;
  1332. rcu_read_lock();
  1333. rmnet_perf_ingress = rcu_dereference(rmnet_perf_desc_entry);
  1334. if (rmnet_perf_ingress) {
  1335. list_for_each_entry_safe(frag, tmp, &segs, list) {
  1336. list_del_init(&frag->list);
  1337. rmnet_perf_ingress(frag, port);
  1338. }
  1339. rcu_read_unlock();
  1340. return;
  1341. }
  1342. rcu_read_unlock();
  1343. no_perf:
  1344. list_for_each_entry_safe(frag, tmp, &segs, list) {
  1345. list_del_init(&frag->list);
  1346. rmnet_frag_deliver(frag, port);
  1347. }
  1348. return;
  1349. recycle:
  1350. rmnet_recycle_frag_descriptor(frag_desc, port);
  1351. }
  1352. /* Notify perf at the end of SKB chain */
  1353. rmnet_perf_chain_hook_t rmnet_perf_chain_end __rcu __read_mostly;
  1354. EXPORT_SYMBOL(rmnet_perf_chain_end);
  1355. void rmnet_frag_ingress_handler(struct sk_buff *skb,
  1356. struct rmnet_port *port)
  1357. {
  1358. rmnet_perf_chain_hook_t rmnet_perf_opt_chain_end;
  1359. LIST_HEAD(desc_list);
  1360. bool skip_perf = (skb->priority == 0xda1a);
  1361. /* Deaggregation and freeing of HW originating
  1362. * buffers is done within here
  1363. */
  1364. while (skb) {
  1365. struct sk_buff *skb_frag;
  1366. rmnet_frag_deaggregate(skb, port, &desc_list, skb->priority);
  1367. if (!list_empty(&desc_list)) {
  1368. struct rmnet_frag_descriptor *frag_desc, *tmp;
  1369. list_for_each_entry_safe(frag_desc, tmp, &desc_list,
  1370. list) {
  1371. list_del_init(&frag_desc->list);
  1372. __rmnet_frag_ingress_handler(frag_desc, port);
  1373. }
  1374. }
  1375. skb_frag = skb_shinfo(skb)->frag_list;
  1376. skb_shinfo(skb)->frag_list = NULL;
  1377. consume_skb(skb);
  1378. skb = skb_frag;
  1379. }
  1380. if (skip_perf)
  1381. return;
  1382. rcu_read_lock();
  1383. rmnet_perf_opt_chain_end = rcu_dereference(rmnet_perf_chain_end);
  1384. if (rmnet_perf_opt_chain_end)
  1385. rmnet_perf_opt_chain_end();
  1386. rcu_read_unlock();
  1387. }
  1388. void rmnet_descriptor_deinit(struct rmnet_port *port)
  1389. {
  1390. struct rmnet_frag_descriptor_pool *pool;
  1391. struct rmnet_frag_descriptor *frag_desc, *tmp;
  1392. pool = port->frag_desc_pool;
  1393. list_for_each_entry_safe(frag_desc, tmp, &pool->free_list, list) {
  1394. kfree(frag_desc);
  1395. pool->pool_size--;
  1396. }
  1397. kfree(pool);
  1398. }
  1399. int rmnet_descriptor_init(struct rmnet_port *port)
  1400. {
  1401. struct rmnet_frag_descriptor_pool *pool;
  1402. int i;
  1403. spin_lock_init(&port->desc_pool_lock);
  1404. pool = kzalloc(sizeof(*pool), GFP_ATOMIC);
  1405. if (!pool)
  1406. return -ENOMEM;
  1407. INIT_LIST_HEAD(&pool->free_list);
  1408. port->frag_desc_pool = pool;
  1409. for (i = 0; i < RMNET_FRAG_DESCRIPTOR_POOL_SIZE; i++) {
  1410. struct rmnet_frag_descriptor *frag_desc;
  1411. frag_desc = kzalloc(sizeof(*frag_desc), GFP_ATOMIC);
  1412. if (!frag_desc)
  1413. return -ENOMEM;
  1414. INIT_LIST_HEAD(&frag_desc->list);
  1415. INIT_LIST_HEAD(&frag_desc->frags);
  1416. list_add_tail(&frag_desc->list, &pool->free_list);
  1417. pool->pool_size++;
  1418. }
  1419. return 0;
  1420. }