cfpkt_skbuff.c 8.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Copyright (C) ST-Ericsson AB 2010
  4. * Author: Sjur Brendeland
  5. */
  6. #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
  7. #include <linux/string.h>
  8. #include <linux/skbuff.h>
  9. #include <linux/export.h>
  10. #include <net/caif/cfpkt.h>
  11. #define PKT_PREFIX 48
  12. #define PKT_POSTFIX 2
  13. #define PKT_LEN_WHEN_EXTENDING 128
  14. #define PKT_ERROR(pkt, errmsg) \
  15. do { \
  16. cfpkt_priv(pkt)->erronous = true; \
  17. skb_reset_tail_pointer(&pkt->skb); \
  18. pr_warn(errmsg); \
  19. } while (0)
  20. struct cfpktq {
  21. struct sk_buff_head head;
  22. atomic_t count;
  23. /* Lock protects count updates */
  24. spinlock_t lock;
  25. };
  26. /*
  27. * net/caif/ is generic and does not
  28. * understand SKB, so we do this typecast
  29. */
  30. struct cfpkt {
  31. struct sk_buff skb;
  32. };
  33. /* Private data inside SKB */
  34. struct cfpkt_priv_data {
  35. struct dev_info dev_info;
  36. bool erronous;
  37. };
  38. static inline struct cfpkt_priv_data *cfpkt_priv(struct cfpkt *pkt)
  39. {
  40. return (struct cfpkt_priv_data *) pkt->skb.cb;
  41. }
  42. static inline bool is_erronous(struct cfpkt *pkt)
  43. {
  44. return cfpkt_priv(pkt)->erronous;
  45. }
  46. static inline struct sk_buff *pkt_to_skb(struct cfpkt *pkt)
  47. {
  48. return &pkt->skb;
  49. }
  50. static inline struct cfpkt *skb_to_pkt(struct sk_buff *skb)
  51. {
  52. return (struct cfpkt *) skb;
  53. }
  54. struct cfpkt *cfpkt_fromnative(enum caif_direction dir, void *nativepkt)
  55. {
  56. struct cfpkt *pkt = skb_to_pkt(nativepkt);
  57. cfpkt_priv(pkt)->erronous = false;
  58. return pkt;
  59. }
  60. EXPORT_SYMBOL(cfpkt_fromnative);
  61. void *cfpkt_tonative(struct cfpkt *pkt)
  62. {
  63. return (void *) pkt;
  64. }
  65. EXPORT_SYMBOL(cfpkt_tonative);
  66. static struct cfpkt *cfpkt_create_pfx(u16 len, u16 pfx)
  67. {
  68. struct sk_buff *skb;
  69. skb = alloc_skb(len + pfx, GFP_ATOMIC);
  70. if (unlikely(skb == NULL))
  71. return NULL;
  72. skb_reserve(skb, pfx);
  73. return skb_to_pkt(skb);
  74. }
  75. inline struct cfpkt *cfpkt_create(u16 len)
  76. {
  77. return cfpkt_create_pfx(len + PKT_POSTFIX, PKT_PREFIX);
  78. }
  79. void cfpkt_destroy(struct cfpkt *pkt)
  80. {
  81. struct sk_buff *skb = pkt_to_skb(pkt);
  82. kfree_skb(skb);
  83. }
  84. inline bool cfpkt_more(struct cfpkt *pkt)
  85. {
  86. struct sk_buff *skb = pkt_to_skb(pkt);
  87. return skb->len > 0;
  88. }
  89. int cfpkt_peek_head(struct cfpkt *pkt, void *data, u16 len)
  90. {
  91. struct sk_buff *skb = pkt_to_skb(pkt);
  92. if (skb_headlen(skb) >= len) {
  93. memcpy(data, skb->data, len);
  94. return 0;
  95. }
  96. return !cfpkt_extr_head(pkt, data, len) &&
  97. !cfpkt_add_head(pkt, data, len);
  98. }
  99. int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len)
  100. {
  101. struct sk_buff *skb = pkt_to_skb(pkt);
  102. u8 *from;
  103. if (unlikely(is_erronous(pkt)))
  104. return -EPROTO;
  105. if (unlikely(len > skb->len)) {
  106. PKT_ERROR(pkt, "read beyond end of packet\n");
  107. return -EPROTO;
  108. }
  109. if (unlikely(len > skb_headlen(skb))) {
  110. if (unlikely(skb_linearize(skb) != 0)) {
  111. PKT_ERROR(pkt, "linearize failed\n");
  112. return -EPROTO;
  113. }
  114. }
  115. from = skb_pull(skb, len);
  116. from -= len;
  117. if (data)
  118. memcpy(data, from, len);
  119. return 0;
  120. }
  121. EXPORT_SYMBOL(cfpkt_extr_head);
  122. int cfpkt_extr_trail(struct cfpkt *pkt, void *dta, u16 len)
  123. {
  124. struct sk_buff *skb = pkt_to_skb(pkt);
  125. u8 *data = dta;
  126. u8 *from;
  127. if (unlikely(is_erronous(pkt)))
  128. return -EPROTO;
  129. if (unlikely(skb_linearize(skb) != 0)) {
  130. PKT_ERROR(pkt, "linearize failed\n");
  131. return -EPROTO;
  132. }
  133. if (unlikely(skb->data + len > skb_tail_pointer(skb))) {
  134. PKT_ERROR(pkt, "read beyond end of packet\n");
  135. return -EPROTO;
  136. }
  137. from = skb_tail_pointer(skb) - len;
  138. skb_trim(skb, skb->len - len);
  139. memcpy(data, from, len);
  140. return 0;
  141. }
  142. int cfpkt_pad_trail(struct cfpkt *pkt, u16 len)
  143. {
  144. return cfpkt_add_body(pkt, NULL, len);
  145. }
  146. int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len)
  147. {
  148. struct sk_buff *skb = pkt_to_skb(pkt);
  149. struct sk_buff *lastskb;
  150. u8 *to;
  151. u16 addlen = 0;
  152. if (unlikely(is_erronous(pkt)))
  153. return -EPROTO;
  154. lastskb = skb;
  155. /* Check whether we need to add space at the tail */
  156. if (unlikely(skb_tailroom(skb) < len)) {
  157. if (likely(len < PKT_LEN_WHEN_EXTENDING))
  158. addlen = PKT_LEN_WHEN_EXTENDING;
  159. else
  160. addlen = len;
  161. }
  162. /* Check whether we need to change the SKB before writing to the tail */
  163. if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) {
  164. /* Make sure data is writable */
  165. if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) {
  166. PKT_ERROR(pkt, "cow failed\n");
  167. return -EPROTO;
  168. }
  169. }
  170. /* All set to put the last SKB and optionally write data there. */
  171. to = pskb_put(skb, lastskb, len);
  172. if (likely(data))
  173. memcpy(to, data, len);
  174. return 0;
  175. }
  176. inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data)
  177. {
  178. return cfpkt_add_body(pkt, &data, 1);
  179. }
  180. int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len)
  181. {
  182. struct sk_buff *skb = pkt_to_skb(pkt);
  183. struct sk_buff *lastskb;
  184. u8 *to;
  185. const u8 *data = data2;
  186. int ret;
  187. if (unlikely(is_erronous(pkt)))
  188. return -EPROTO;
  189. if (unlikely(skb_headroom(skb) < len)) {
  190. PKT_ERROR(pkt, "no headroom\n");
  191. return -EPROTO;
  192. }
  193. /* Make sure data is writable */
  194. ret = skb_cow_data(skb, 0, &lastskb);
  195. if (unlikely(ret < 0)) {
  196. PKT_ERROR(pkt, "cow failed\n");
  197. return ret;
  198. }
  199. to = skb_push(skb, len);
  200. memcpy(to, data, len);
  201. return 0;
  202. }
  203. EXPORT_SYMBOL(cfpkt_add_head);
  204. inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len)
  205. {
  206. return cfpkt_add_body(pkt, data, len);
  207. }
  208. inline u16 cfpkt_getlen(struct cfpkt *pkt)
  209. {
  210. struct sk_buff *skb = pkt_to_skb(pkt);
  211. return skb->len;
  212. }
  213. int cfpkt_iterate(struct cfpkt *pkt,
  214. u16 (*iter_func)(u16, void *, u16),
  215. u16 data)
  216. {
  217. /*
  218. * Don't care about the performance hit of linearizing,
  219. * Checksum should not be used on high-speed interfaces anyway.
  220. */
  221. if (unlikely(is_erronous(pkt)))
  222. return -EPROTO;
  223. if (unlikely(skb_linearize(&pkt->skb) != 0)) {
  224. PKT_ERROR(pkt, "linearize failed\n");
  225. return -EPROTO;
  226. }
  227. return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt));
  228. }
  229. int cfpkt_setlen(struct cfpkt *pkt, u16 len)
  230. {
  231. struct sk_buff *skb = pkt_to_skb(pkt);
  232. if (unlikely(is_erronous(pkt)))
  233. return -EPROTO;
  234. if (likely(len <= skb->len)) {
  235. if (unlikely(skb->data_len))
  236. ___pskb_trim(skb, len);
  237. else
  238. skb_trim(skb, len);
  239. return cfpkt_getlen(pkt);
  240. }
  241. /* Need to expand SKB */
  242. if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len)))
  243. PKT_ERROR(pkt, "skb_pad_trail failed\n");
  244. return cfpkt_getlen(pkt);
  245. }
  246. struct cfpkt *cfpkt_append(struct cfpkt *dstpkt,
  247. struct cfpkt *addpkt,
  248. u16 expectlen)
  249. {
  250. struct sk_buff *dst = pkt_to_skb(dstpkt);
  251. struct sk_buff *add = pkt_to_skb(addpkt);
  252. u16 addlen = skb_headlen(add);
  253. u16 neededtailspace;
  254. struct sk_buff *tmp;
  255. u16 dstlen;
  256. u16 createlen;
  257. if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) {
  258. return dstpkt;
  259. }
  260. if (expectlen > addlen)
  261. neededtailspace = expectlen;
  262. else
  263. neededtailspace = addlen;
  264. if (dst->tail + neededtailspace > dst->end) {
  265. /* Create a dumplicate of 'dst' with more tail space */
  266. struct cfpkt *tmppkt;
  267. dstlen = skb_headlen(dst);
  268. createlen = dstlen + neededtailspace;
  269. tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX);
  270. if (tmppkt == NULL)
  271. return NULL;
  272. tmp = pkt_to_skb(tmppkt);
  273. skb_put_data(tmp, dst->data, dstlen);
  274. cfpkt_destroy(dstpkt);
  275. dst = tmp;
  276. }
  277. skb_put_data(dst, add->data, skb_headlen(add));
  278. cfpkt_destroy(addpkt);
  279. return skb_to_pkt(dst);
  280. }
  281. struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos)
  282. {
  283. struct sk_buff *skb2;
  284. struct sk_buff *skb = pkt_to_skb(pkt);
  285. struct cfpkt *tmppkt;
  286. u8 *split = skb->data + pos;
  287. u16 len2nd = skb_tail_pointer(skb) - split;
  288. if (unlikely(is_erronous(pkt)))
  289. return NULL;
  290. if (skb->data + pos > skb_tail_pointer(skb)) {
  291. PKT_ERROR(pkt, "trying to split beyond end of packet\n");
  292. return NULL;
  293. }
  294. /* Create a new packet for the second part of the data */
  295. tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX,
  296. PKT_PREFIX);
  297. if (tmppkt == NULL)
  298. return NULL;
  299. skb2 = pkt_to_skb(tmppkt);
  300. if (skb2 == NULL)
  301. return NULL;
  302. skb_put_data(skb2, split, len2nd);
  303. /* Reduce the length of the original packet */
  304. skb_trim(skb, pos);
  305. skb2->priority = skb->priority;
  306. return skb_to_pkt(skb2);
  307. }
  308. bool cfpkt_erroneous(struct cfpkt *pkt)
  309. {
  310. return cfpkt_priv(pkt)->erronous;
  311. }
  312. struct caif_payload_info *cfpkt_info(struct cfpkt *pkt)
  313. {
  314. return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb;
  315. }
  316. EXPORT_SYMBOL(cfpkt_info);
  317. void cfpkt_set_prio(struct cfpkt *pkt, int prio)
  318. {
  319. pkt_to_skb(pkt)->priority = prio;
  320. }
  321. EXPORT_SYMBOL(cfpkt_set_prio);