cfrfml.c 6.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299
  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/stddef.h>
  8. #include <linux/spinlock.h>
  9. #include <linux/slab.h>
  10. #include <asm/unaligned.h>
  11. #include <net/caif/caif_layer.h>
  12. #include <net/caif/cfsrvl.h>
  13. #include <net/caif/cfpkt.h>
  14. #define container_obj(layr) container_of(layr, struct cfrfml, serv.layer)
  15. #define RFM_SEGMENTATION_BIT 0x01
  16. #define RFM_HEAD_SIZE 7
  17. static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt);
  18. static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt);
  19. struct cfrfml {
  20. struct cfsrvl serv;
  21. struct cfpkt *incomplete_frm;
  22. int fragment_size;
  23. u8 seghead[6];
  24. u16 pdu_size;
  25. /* Protects serialized processing of packets */
  26. spinlock_t sync;
  27. };
  28. static void cfrfml_release(struct cflayer *layer)
  29. {
  30. struct cfsrvl *srvl = container_of(layer, struct cfsrvl, layer);
  31. struct cfrfml *rfml = container_obj(&srvl->layer);
  32. if (rfml->incomplete_frm)
  33. cfpkt_destroy(rfml->incomplete_frm);
  34. kfree(srvl);
  35. }
  36. struct cflayer *cfrfml_create(u8 channel_id, struct dev_info *dev_info,
  37. int mtu_size)
  38. {
  39. int tmp;
  40. struct cfrfml *this = kzalloc(sizeof(struct cfrfml), GFP_ATOMIC);
  41. if (!this)
  42. return NULL;
  43. cfsrvl_init(&this->serv, channel_id, dev_info, false);
  44. this->serv.release = cfrfml_release;
  45. this->serv.layer.receive = cfrfml_receive;
  46. this->serv.layer.transmit = cfrfml_transmit;
  47. /* Round down to closest multiple of 16 */
  48. tmp = (mtu_size - RFM_HEAD_SIZE - 6) / 16;
  49. tmp *= 16;
  50. this->fragment_size = tmp;
  51. spin_lock_init(&this->sync);
  52. snprintf(this->serv.layer.name, CAIF_LAYER_NAME_SZ,
  53. "rfm%d", channel_id);
  54. return &this->serv.layer;
  55. }
  56. static struct cfpkt *rfm_append(struct cfrfml *rfml, char *seghead,
  57. struct cfpkt *pkt, int *err)
  58. {
  59. struct cfpkt *tmppkt;
  60. *err = -EPROTO;
  61. /* n-th but not last segment */
  62. if (cfpkt_extr_head(pkt, seghead, 6) < 0)
  63. return NULL;
  64. /* Verify correct header */
  65. if (memcmp(seghead, rfml->seghead, 6) != 0)
  66. return NULL;
  67. tmppkt = cfpkt_append(rfml->incomplete_frm, pkt,
  68. rfml->pdu_size + RFM_HEAD_SIZE);
  69. /* If cfpkt_append failes input pkts are not freed */
  70. *err = -ENOMEM;
  71. if (tmppkt == NULL)
  72. return NULL;
  73. *err = 0;
  74. return tmppkt;
  75. }
  76. static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
  77. {
  78. u8 tmp;
  79. bool segmented;
  80. int err;
  81. u8 seghead[6];
  82. struct cfrfml *rfml;
  83. struct cfpkt *tmppkt = NULL;
  84. caif_assert(layr->up != NULL);
  85. caif_assert(layr->receive != NULL);
  86. rfml = container_obj(layr);
  87. spin_lock(&rfml->sync);
  88. err = -EPROTO;
  89. if (cfpkt_extr_head(pkt, &tmp, 1) < 0)
  90. goto out;
  91. segmented = tmp & RFM_SEGMENTATION_BIT;
  92. if (segmented) {
  93. if (rfml->incomplete_frm == NULL) {
  94. /* Initial Segment */
  95. if (cfpkt_peek_head(pkt, rfml->seghead, 6) != 0)
  96. goto out;
  97. rfml->pdu_size = get_unaligned_le16(rfml->seghead+4);
  98. if (cfpkt_erroneous(pkt))
  99. goto out;
  100. rfml->incomplete_frm = pkt;
  101. pkt = NULL;
  102. } else {
  103. tmppkt = rfm_append(rfml, seghead, pkt, &err);
  104. if (tmppkt == NULL)
  105. goto out;
  106. if (cfpkt_erroneous(tmppkt))
  107. goto out;
  108. rfml->incomplete_frm = tmppkt;
  109. if (cfpkt_erroneous(tmppkt))
  110. goto out;
  111. }
  112. err = 0;
  113. goto out;
  114. }
  115. if (rfml->incomplete_frm) {
  116. /* Last Segment */
  117. tmppkt = rfm_append(rfml, seghead, pkt, &err);
  118. if (tmppkt == NULL)
  119. goto out;
  120. if (cfpkt_erroneous(tmppkt))
  121. goto out;
  122. rfml->incomplete_frm = NULL;
  123. pkt = tmppkt;
  124. tmppkt = NULL;
  125. /* Verify that length is correct */
  126. err = -EPROTO;
  127. if (rfml->pdu_size != cfpkt_getlen(pkt) - RFM_HEAD_SIZE + 1)
  128. goto out;
  129. }
  130. err = rfml->serv.layer.up->receive(rfml->serv.layer.up, pkt);
  131. out:
  132. if (err != 0) {
  133. if (tmppkt)
  134. cfpkt_destroy(tmppkt);
  135. if (pkt)
  136. cfpkt_destroy(pkt);
  137. if (rfml->incomplete_frm)
  138. cfpkt_destroy(rfml->incomplete_frm);
  139. rfml->incomplete_frm = NULL;
  140. pr_info("Connection error %d triggered on RFM link\n", err);
  141. /* Trigger connection error upon failure.*/
  142. layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
  143. rfml->serv.dev_info.id);
  144. }
  145. spin_unlock(&rfml->sync);
  146. if (unlikely(err == -EAGAIN))
  147. /* It is not possible to recover after drop of a fragment */
  148. err = -EIO;
  149. return err;
  150. }
  151. static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
  152. {
  153. caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size + RFM_HEAD_SIZE);
  154. /* Add info for MUX-layer to route the packet out. */
  155. cfpkt_info(pkt)->channel_id = rfml->serv.layer.id;
  156. /*
  157. * To optimize alignment, we add up the size of CAIF header before
  158. * payload.
  159. */
  160. cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE;
  161. cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info;
  162. return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt);
  163. }
  164. static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
  165. {
  166. int err;
  167. u8 seg;
  168. u8 head[6];
  169. struct cfpkt *rearpkt = NULL;
  170. struct cfpkt *frontpkt = pkt;
  171. struct cfrfml *rfml = container_obj(layr);
  172. caif_assert(layr->dn != NULL);
  173. caif_assert(layr->dn->transmit != NULL);
  174. if (!cfsrvl_ready(&rfml->serv, &err))
  175. goto out;
  176. err = -EPROTO;
  177. if (cfpkt_getlen(pkt) <= RFM_HEAD_SIZE-1)
  178. goto out;
  179. err = 0;
  180. if (cfpkt_getlen(pkt) > rfml->fragment_size + RFM_HEAD_SIZE)
  181. err = cfpkt_peek_head(pkt, head, 6);
  182. if (err != 0)
  183. goto out;
  184. while (cfpkt_getlen(frontpkt) > rfml->fragment_size + RFM_HEAD_SIZE) {
  185. seg = 1;
  186. err = -EPROTO;
  187. if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
  188. goto out;
  189. /*
  190. * On OOM error cfpkt_split returns NULL.
  191. *
  192. * NOTE: Segmented pdu is not correctly aligned.
  193. * This has negative performance impact.
  194. */
  195. rearpkt = cfpkt_split(frontpkt, rfml->fragment_size);
  196. if (rearpkt == NULL)
  197. goto out;
  198. err = cfrfml_transmit_segment(rfml, frontpkt);
  199. if (err != 0) {
  200. frontpkt = NULL;
  201. goto out;
  202. }
  203. frontpkt = rearpkt;
  204. rearpkt = NULL;
  205. err = -EPROTO;
  206. if (cfpkt_add_head(frontpkt, head, 6) < 0)
  207. goto out;
  208. }
  209. seg = 0;
  210. err = -EPROTO;
  211. if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
  212. goto out;
  213. err = cfrfml_transmit_segment(rfml, frontpkt);
  214. frontpkt = NULL;
  215. out:
  216. if (err != 0) {
  217. pr_info("Connection error %d triggered on RFM link\n", err);
  218. /* Trigger connection error upon failure.*/
  219. layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
  220. rfml->serv.dev_info.id);
  221. if (rearpkt)
  222. cfpkt_destroy(rearpkt);
  223. if (frontpkt)
  224. cfpkt_destroy(frontpkt);
  225. }
  226. return err;
  227. }