1af873fceSThomas Gleixner // SPDX-License-Identifier: GPL-2.0-only
2b482cd20SSjur Braendeland /*
3b482cd20SSjur Braendeland * Copyright (C) ST-Ericsson AB 2010
426ee65e6Ssjur.brandeland@stericsson.com * Author: Sjur Brendeland
5b482cd20SSjur Braendeland */
6b482cd20SSjur Braendeland
7b31fa5baSJoe Perches #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
8b31fa5baSJoe Perches
9b482cd20SSjur Braendeland #include <linux/stddef.h>
10b482cd20SSjur Braendeland #include <linux/spinlock.h>
11b482cd20SSjur Braendeland #include <linux/slab.h>
127e368739SJeff Mahoney #include <asm/unaligned.h>
13b482cd20SSjur Braendeland #include <net/caif/caif_layer.h>
14b482cd20SSjur Braendeland #include <net/caif/cfsrvl.h>
15b482cd20SSjur Braendeland #include <net/caif/cfpkt.h>
16b482cd20SSjur Braendeland
17a7da1f55SSjur Braendeland #define container_obj(layr) container_of(layr, struct cfrfml, serv.layer)
18b482cd20SSjur Braendeland #define RFM_SEGMENTATION_BIT 0x01
19a7da1f55SSjur Braendeland #define RFM_HEAD_SIZE 7
20b482cd20SSjur Braendeland
21b482cd20SSjur Braendeland static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt);
22b482cd20SSjur Braendeland static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt);
23b482cd20SSjur Braendeland
24a7da1f55SSjur Braendeland struct cfrfml {
25a7da1f55SSjur Braendeland struct cfsrvl serv;
26a7da1f55SSjur Braendeland struct cfpkt *incomplete_frm;
27a7da1f55SSjur Braendeland int fragment_size;
28a7da1f55SSjur Braendeland u8 seghead[6];
29a7da1f55SSjur Braendeland u16 pdu_size;
30a7da1f55SSjur Braendeland /* Protects serialized processing of packets */
31a7da1f55SSjur Braendeland spinlock_t sync;
32a7da1f55SSjur Braendeland };
33b1c74247SSjur Braendeland
cfrfml_release(struct cflayer * layer)3443e36921Ssjur.brandeland@stericsson.com static void cfrfml_release(struct cflayer *layer)
35a7da1f55SSjur Braendeland {
3643e36921Ssjur.brandeland@stericsson.com struct cfsrvl *srvl = container_of(layer, struct cfsrvl, layer);
37a7da1f55SSjur Braendeland struct cfrfml *rfml = container_obj(&srvl->layer);
38a7da1f55SSjur Braendeland
39a7da1f55SSjur Braendeland if (rfml->incomplete_frm)
40a7da1f55SSjur Braendeland cfpkt_destroy(rfml->incomplete_frm);
41a7da1f55SSjur Braendeland
42a7da1f55SSjur Braendeland kfree(srvl);
43a7da1f55SSjur Braendeland }
44a7da1f55SSjur Braendeland
cfrfml_create(u8 channel_id,struct dev_info * dev_info,int mtu_size)45a7da1f55SSjur Braendeland struct cflayer *cfrfml_create(u8 channel_id, struct dev_info *dev_info,
46a7da1f55SSjur Braendeland int mtu_size)
47a7da1f55SSjur Braendeland {
48a7da1f55SSjur Braendeland int tmp;
497ac2ed0cSJoe Perches struct cfrfml *this = kzalloc(sizeof(struct cfrfml), GFP_ATOMIC);
50a7da1f55SSjur Braendeland
517ac2ed0cSJoe Perches if (!this)
52b482cd20SSjur Braendeland return NULL;
53b1c74247SSjur Braendeland
54a7da1f55SSjur Braendeland cfsrvl_init(&this->serv, channel_id, dev_info, false);
55a7da1f55SSjur Braendeland this->serv.release = cfrfml_release;
56a7da1f55SSjur Braendeland this->serv.layer.receive = cfrfml_receive;
57a7da1f55SSjur Braendeland this->serv.layer.transmit = cfrfml_transmit;
58b1c74247SSjur Braendeland
59a7da1f55SSjur Braendeland /* Round down to closest multiple of 16 */
60a7da1f55SSjur Braendeland tmp = (mtu_size - RFM_HEAD_SIZE - 6) / 16;
61a7da1f55SSjur Braendeland tmp *= 16;
62a7da1f55SSjur Braendeland
63a7da1f55SSjur Braendeland this->fragment_size = tmp;
64a7da1f55SSjur Braendeland spin_lock_init(&this->sync);
65a7da1f55SSjur Braendeland snprintf(this->serv.layer.name, CAIF_LAYER_NAME_SZ,
66a7da1f55SSjur Braendeland "rfm%d", channel_id);
67a7da1f55SSjur Braendeland
68a7da1f55SSjur Braendeland return &this->serv.layer;
69a7da1f55SSjur Braendeland }
70a7da1f55SSjur Braendeland
rfm_append(struct cfrfml * rfml,char * seghead,struct cfpkt * pkt,int * err)71a7da1f55SSjur Braendeland static struct cfpkt *rfm_append(struct cfrfml *rfml, char *seghead,
72a7da1f55SSjur Braendeland struct cfpkt *pkt, int *err)
73a7da1f55SSjur Braendeland {
74a7da1f55SSjur Braendeland struct cfpkt *tmppkt;
75a7da1f55SSjur Braendeland *err = -EPROTO;
76a7da1f55SSjur Braendeland /* n-th but not last segment */
77a7da1f55SSjur Braendeland
78a7da1f55SSjur Braendeland if (cfpkt_extr_head(pkt, seghead, 6) < 0)
79a7da1f55SSjur Braendeland return NULL;
80a7da1f55SSjur Braendeland
81a7da1f55SSjur Braendeland /* Verify correct header */
82a7da1f55SSjur Braendeland if (memcmp(seghead, rfml->seghead, 6) != 0)
83a7da1f55SSjur Braendeland return NULL;
84a7da1f55SSjur Braendeland
85a7da1f55SSjur Braendeland tmppkt = cfpkt_append(rfml->incomplete_frm, pkt,
86a7da1f55SSjur Braendeland rfml->pdu_size + RFM_HEAD_SIZE);
87a7da1f55SSjur Braendeland
88a7da1f55SSjur Braendeland /* If cfpkt_append failes input pkts are not freed */
89a7da1f55SSjur Braendeland *err = -ENOMEM;
90a7da1f55SSjur Braendeland if (tmppkt == NULL)
91a7da1f55SSjur Braendeland return NULL;
92a7da1f55SSjur Braendeland
93a7da1f55SSjur Braendeland *err = 0;
94a7da1f55SSjur Braendeland return tmppkt;
95b482cd20SSjur Braendeland }
96b482cd20SSjur Braendeland
cfrfml_receive(struct cflayer * layr,struct cfpkt * pkt)97b482cd20SSjur Braendeland static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
98b482cd20SSjur Braendeland {
99b482cd20SSjur Braendeland u8 tmp;
100b482cd20SSjur Braendeland bool segmented;
101a7da1f55SSjur Braendeland int err;
102a7da1f55SSjur Braendeland u8 seghead[6];
103a7da1f55SSjur Braendeland struct cfrfml *rfml;
104a7da1f55SSjur Braendeland struct cfpkt *tmppkt = NULL;
105a7da1f55SSjur Braendeland
106b482cd20SSjur Braendeland caif_assert(layr->up != NULL);
107b482cd20SSjur Braendeland caif_assert(layr->receive != NULL);
108a7da1f55SSjur Braendeland rfml = container_obj(layr);
109a7da1f55SSjur Braendeland spin_lock(&rfml->sync);
110b482cd20SSjur Braendeland
111a7da1f55SSjur Braendeland err = -EPROTO;
112a7da1f55SSjur Braendeland if (cfpkt_extr_head(pkt, &tmp, 1) < 0)
113a7da1f55SSjur Braendeland goto out;
114b482cd20SSjur Braendeland segmented = tmp & RFM_SEGMENTATION_BIT;
115b482cd20SSjur Braendeland
116a7da1f55SSjur Braendeland if (segmented) {
117a7da1f55SSjur Braendeland if (rfml->incomplete_frm == NULL) {
118a7da1f55SSjur Braendeland /* Initial Segment */
119*e1046841STong Zhang if (cfpkt_peek_head(pkt, rfml->seghead, 6) != 0)
120a7da1f55SSjur Braendeland goto out;
121a7da1f55SSjur Braendeland
122a7da1f55SSjur Braendeland rfml->pdu_size = get_unaligned_le16(rfml->seghead+4);
123a7da1f55SSjur Braendeland
124a7da1f55SSjur Braendeland if (cfpkt_erroneous(pkt))
125a7da1f55SSjur Braendeland goto out;
126a7da1f55SSjur Braendeland rfml->incomplete_frm = pkt;
127a7da1f55SSjur Braendeland pkt = NULL;
128a7da1f55SSjur Braendeland } else {
129a7da1f55SSjur Braendeland
130a7da1f55SSjur Braendeland tmppkt = rfm_append(rfml, seghead, pkt, &err);
131a7da1f55SSjur Braendeland if (tmppkt == NULL)
132a7da1f55SSjur Braendeland goto out;
133a7da1f55SSjur Braendeland
134a7da1f55SSjur Braendeland if (cfpkt_erroneous(tmppkt))
135a7da1f55SSjur Braendeland goto out;
136a7da1f55SSjur Braendeland
137a7da1f55SSjur Braendeland rfml->incomplete_frm = tmppkt;
138a7da1f55SSjur Braendeland
139a7da1f55SSjur Braendeland
140a7da1f55SSjur Braendeland if (cfpkt_erroneous(tmppkt))
141a7da1f55SSjur Braendeland goto out;
142a7da1f55SSjur Braendeland }
143a7da1f55SSjur Braendeland err = 0;
144a7da1f55SSjur Braendeland goto out;
145b482cd20SSjur Braendeland }
146b482cd20SSjur Braendeland
147a7da1f55SSjur Braendeland if (rfml->incomplete_frm) {
148a7da1f55SSjur Braendeland
149a7da1f55SSjur Braendeland /* Last Segment */
150a7da1f55SSjur Braendeland tmppkt = rfm_append(rfml, seghead, pkt, &err);
151a7da1f55SSjur Braendeland if (tmppkt == NULL)
152a7da1f55SSjur Braendeland goto out;
153a7da1f55SSjur Braendeland
154a7da1f55SSjur Braendeland if (cfpkt_erroneous(tmppkt))
155a7da1f55SSjur Braendeland goto out;
156a7da1f55SSjur Braendeland
157a7da1f55SSjur Braendeland rfml->incomplete_frm = NULL;
158a7da1f55SSjur Braendeland pkt = tmppkt;
159a7da1f55SSjur Braendeland tmppkt = NULL;
160a7da1f55SSjur Braendeland
161a7da1f55SSjur Braendeland /* Verify that length is correct */
162449f14f0SAnton Protopopov err = -EPROTO;
163a7da1f55SSjur Braendeland if (rfml->pdu_size != cfpkt_getlen(pkt) - RFM_HEAD_SIZE + 1)
164a7da1f55SSjur Braendeland goto out;
165a7da1f55SSjur Braendeland }
166a7da1f55SSjur Braendeland
167a7da1f55SSjur Braendeland err = rfml->serv.layer.up->receive(rfml->serv.layer.up, pkt);
168a7da1f55SSjur Braendeland
169a7da1f55SSjur Braendeland out:
170a7da1f55SSjur Braendeland
171a7da1f55SSjur Braendeland if (err != 0) {
172a7da1f55SSjur Braendeland if (tmppkt)
173a7da1f55SSjur Braendeland cfpkt_destroy(tmppkt);
174a7da1f55SSjur Braendeland if (pkt)
175a7da1f55SSjur Braendeland cfpkt_destroy(pkt);
176a7da1f55SSjur Braendeland if (rfml->incomplete_frm)
177a7da1f55SSjur Braendeland cfpkt_destroy(rfml->incomplete_frm);
178a7da1f55SSjur Braendeland rfml->incomplete_frm = NULL;
179a7da1f55SSjur Braendeland
180b31fa5baSJoe Perches pr_info("Connection error %d triggered on RFM link\n", err);
181a7da1f55SSjur Braendeland
182a7da1f55SSjur Braendeland /* Trigger connection error upon failure.*/
183a7da1f55SSjur Braendeland layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
184a7da1f55SSjur Braendeland rfml->serv.dev_info.id);
185a7da1f55SSjur Braendeland }
186a7da1f55SSjur Braendeland spin_unlock(&rfml->sync);
187374458b3SDmitry Tarnyagin
188374458b3SDmitry Tarnyagin if (unlikely(err == -EAGAIN))
189374458b3SDmitry Tarnyagin /* It is not possible to recover after drop of a fragment */
190374458b3SDmitry Tarnyagin err = -EIO;
191374458b3SDmitry Tarnyagin
192a7da1f55SSjur Braendeland return err;
193a7da1f55SSjur Braendeland }
194a7da1f55SSjur Braendeland
195a7da1f55SSjur Braendeland
cfrfml_transmit_segment(struct cfrfml * rfml,struct cfpkt * pkt)196a7da1f55SSjur Braendeland static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
197b482cd20SSjur Braendeland {
198005b0b07Ssjur.brandeland@stericsson.com caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size + RFM_HEAD_SIZE);
199b482cd20SSjur Braendeland
200b482cd20SSjur Braendeland /* Add info for MUX-layer to route the packet out. */
201a7da1f55SSjur Braendeland cfpkt_info(pkt)->channel_id = rfml->serv.layer.id;
202a7da1f55SSjur Braendeland
203b482cd20SSjur Braendeland /*
204b482cd20SSjur Braendeland * To optimize alignment, we add up the size of CAIF header before
205b482cd20SSjur Braendeland * payload.
206b482cd20SSjur Braendeland */
207a7da1f55SSjur Braendeland cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE;
208a7da1f55SSjur Braendeland cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info;
209a7da1f55SSjur Braendeland
210a7da1f55SSjur Braendeland return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt);
211a7da1f55SSjur Braendeland }
212a7da1f55SSjur Braendeland
cfrfml_transmit(struct cflayer * layr,struct cfpkt * pkt)213a7da1f55SSjur Braendeland static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
214a7da1f55SSjur Braendeland {
215a7da1f55SSjur Braendeland int err;
216a7da1f55SSjur Braendeland u8 seg;
217a7da1f55SSjur Braendeland u8 head[6];
218a7da1f55SSjur Braendeland struct cfpkt *rearpkt = NULL;
219a7da1f55SSjur Braendeland struct cfpkt *frontpkt = pkt;
220a7da1f55SSjur Braendeland struct cfrfml *rfml = container_obj(layr);
221a7da1f55SSjur Braendeland
222a7da1f55SSjur Braendeland caif_assert(layr->dn != NULL);
223a7da1f55SSjur Braendeland caif_assert(layr->dn->transmit != NULL);
224a7da1f55SSjur Braendeland
225a7da1f55SSjur Braendeland if (!cfsrvl_ready(&rfml->serv, &err))
226374458b3SDmitry Tarnyagin goto out;
227a7da1f55SSjur Braendeland
228a7da1f55SSjur Braendeland err = -EPROTO;
229a7da1f55SSjur Braendeland if (cfpkt_getlen(pkt) <= RFM_HEAD_SIZE-1)
230a7da1f55SSjur Braendeland goto out;
231a7da1f55SSjur Braendeland
232a7da1f55SSjur Braendeland err = 0;
233a7da1f55SSjur Braendeland if (cfpkt_getlen(pkt) > rfml->fragment_size + RFM_HEAD_SIZE)
234a7da1f55SSjur Braendeland err = cfpkt_peek_head(pkt, head, 6);
235a7da1f55SSjur Braendeland
236*e1046841STong Zhang if (err != 0)
237a7da1f55SSjur Braendeland goto out;
238a7da1f55SSjur Braendeland
239a7da1f55SSjur Braendeland while (cfpkt_getlen(frontpkt) > rfml->fragment_size + RFM_HEAD_SIZE) {
240a7da1f55SSjur Braendeland
241a7da1f55SSjur Braendeland seg = 1;
242a7da1f55SSjur Braendeland err = -EPROTO;
243a7da1f55SSjur Braendeland
244a7da1f55SSjur Braendeland if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
245a7da1f55SSjur Braendeland goto out;
246a7da1f55SSjur Braendeland /*
247a7da1f55SSjur Braendeland * On OOM error cfpkt_split returns NULL.
248a7da1f55SSjur Braendeland *
249a7da1f55SSjur Braendeland * NOTE: Segmented pdu is not correctly aligned.
250a7da1f55SSjur Braendeland * This has negative performance impact.
251a7da1f55SSjur Braendeland */
252a7da1f55SSjur Braendeland
253a7da1f55SSjur Braendeland rearpkt = cfpkt_split(frontpkt, rfml->fragment_size);
254a7da1f55SSjur Braendeland if (rearpkt == NULL)
255a7da1f55SSjur Braendeland goto out;
256a7da1f55SSjur Braendeland
257a7da1f55SSjur Braendeland err = cfrfml_transmit_segment(rfml, frontpkt);
258a7da1f55SSjur Braendeland
259374458b3SDmitry Tarnyagin if (err != 0) {
260374458b3SDmitry Tarnyagin frontpkt = NULL;
261a7da1f55SSjur Braendeland goto out;
262374458b3SDmitry Tarnyagin }
263374458b3SDmitry Tarnyagin
264a7da1f55SSjur Braendeland frontpkt = rearpkt;
265a7da1f55SSjur Braendeland rearpkt = NULL;
266a7da1f55SSjur Braendeland
267a7da1f55SSjur Braendeland err = -EPROTO;
268a7da1f55SSjur Braendeland if (cfpkt_add_head(frontpkt, head, 6) < 0)
269a7da1f55SSjur Braendeland goto out;
270a7da1f55SSjur Braendeland
271a7da1f55SSjur Braendeland }
272a7da1f55SSjur Braendeland
273a7da1f55SSjur Braendeland seg = 0;
274a7da1f55SSjur Braendeland err = -EPROTO;
275a7da1f55SSjur Braendeland
276a7da1f55SSjur Braendeland if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
277a7da1f55SSjur Braendeland goto out;
278a7da1f55SSjur Braendeland
279a7da1f55SSjur Braendeland err = cfrfml_transmit_segment(rfml, frontpkt);
280a7da1f55SSjur Braendeland
281a7da1f55SSjur Braendeland frontpkt = NULL;
282a7da1f55SSjur Braendeland out:
283a7da1f55SSjur Braendeland
284a7da1f55SSjur Braendeland if (err != 0) {
285b31fa5baSJoe Perches pr_info("Connection error %d triggered on RFM link\n", err);
286a7da1f55SSjur Braendeland /* Trigger connection error upon failure.*/
287a7da1f55SSjur Braendeland
288a7da1f55SSjur Braendeland layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
289a7da1f55SSjur Braendeland rfml->serv.dev_info.id);
290a7da1f55SSjur Braendeland
291a7da1f55SSjur Braendeland if (rearpkt)
292a7da1f55SSjur Braendeland cfpkt_destroy(rearpkt);
293a7da1f55SSjur Braendeland
294374458b3SDmitry Tarnyagin if (frontpkt)
295a7da1f55SSjur Braendeland cfpkt_destroy(frontpkt);
296a7da1f55SSjur Braendeland }
297a7da1f55SSjur Braendeland
298a7da1f55SSjur Braendeland return err;
299b482cd20SSjur Braendeland }
300