blob: 2b563ad04597de1b049e8d52c4944d9ed48b5da3 [file] [log] [blame]
Sjur Braendelandb482cd22010-03-30 13:56:23 +00001/*
2 * Copyright (C) ST-Ericsson AB 2010
3 * Author: Sjur Brendeland/sjur.brandeland@stericsson.com
4 * License terms: GNU General Public License (GPL) version 2
5 */
6
Joe Perchesb31fa5b2010-09-05 21:31:11 +00007#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
8
Sjur Braendelandb482cd22010-03-30 13:56:23 +00009#include <linux/stddef.h>
10#include <linux/spinlock.h>
11#include <linux/slab.h>
Jeff Mahoney7e368732010-08-26 16:11:08 -070012#include <asm/unaligned.h>
Sjur Braendelandb482cd22010-03-30 13:56:23 +000013#include <net/caif/caif_layer.h>
14#include <net/caif/cfsrvl.h>
15#include <net/caif/cfpkt.h>
16
Sjur Braendelanda7da1f52010-06-17 06:55:39 +000017#define container_obj(layr) container_of(layr, struct cfrfml, serv.layer)
Sjur Braendelandb482cd22010-03-30 13:56:23 +000018#define RFM_SEGMENTATION_BIT 0x01
Sjur Braendelanda7da1f52010-06-17 06:55:39 +000019#define RFM_HEAD_SIZE 7
Sjur Braendelandb482cd22010-03-30 13:56:23 +000020
21static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt);
22static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt);
Sjur Braendelandb482cd22010-03-30 13:56:23 +000023
Sjur Braendelanda7da1f52010-06-17 06:55:39 +000024struct cfrfml {
25 struct cfsrvl serv;
26 struct cfpkt *incomplete_frm;
27 int fragment_size;
28 u8 seghead[6];
29 u16 pdu_size;
30 /* Protects serialized processing of packets */
31 spinlock_t sync;
32};
Sjur Braendelandb1c74242010-06-17 06:55:38 +000033
sjur.brandeland@stericsson.com43e36922011-05-13 02:44:03 +000034static void cfrfml_release(struct cflayer *layer)
Sjur Braendelanda7da1f52010-06-17 06:55:39 +000035{
sjur.brandeland@stericsson.com43e36922011-05-13 02:44:03 +000036 struct cfsrvl *srvl = container_of(layer, struct cfsrvl, layer);
Sjur Braendelanda7da1f52010-06-17 06:55:39 +000037 struct cfrfml *rfml = container_obj(&srvl->layer);
38
39 if (rfml->incomplete_frm)
40 cfpkt_destroy(rfml->incomplete_frm);
41
42 kfree(srvl);
43}
44
45struct cflayer *cfrfml_create(u8 channel_id, struct dev_info *dev_info,
46 int mtu_size)
47{
48 int tmp;
Joe Perches7ac2ed02011-08-25 13:22:24 +000049 struct cfrfml *this = kzalloc(sizeof(struct cfrfml), GFP_ATOMIC);
Sjur Braendelanda7da1f52010-06-17 06:55:39 +000050
Joe Perches7ac2ed02011-08-25 13:22:24 +000051 if (!this)
Sjur Braendelandb482cd22010-03-30 13:56:23 +000052 return NULL;
Sjur Braendelandb1c74242010-06-17 06:55:38 +000053
Sjur Braendelanda7da1f52010-06-17 06:55:39 +000054 cfsrvl_init(&this->serv, channel_id, dev_info, false);
55 this->serv.release = cfrfml_release;
56 this->serv.layer.receive = cfrfml_receive;
57 this->serv.layer.transmit = cfrfml_transmit;
Sjur Braendelandb1c74242010-06-17 06:55:38 +000058
Sjur Braendelanda7da1f52010-06-17 06:55:39 +000059 /* Round down to closest multiple of 16 */
60 tmp = (mtu_size - RFM_HEAD_SIZE - 6) / 16;
61 tmp *= 16;
62
63 this->fragment_size = tmp;
64 spin_lock_init(&this->sync);
65 snprintf(this->serv.layer.name, CAIF_LAYER_NAME_SZ,
66 "rfm%d", channel_id);
67
68 return &this->serv.layer;
69}
70
71static struct cfpkt *rfm_append(struct cfrfml *rfml, char *seghead,
72 struct cfpkt *pkt, int *err)
73{
74 struct cfpkt *tmppkt;
75 *err = -EPROTO;
76 /* n-th but not last segment */
77
78 if (cfpkt_extr_head(pkt, seghead, 6) < 0)
79 return NULL;
80
81 /* Verify correct header */
82 if (memcmp(seghead, rfml->seghead, 6) != 0)
83 return NULL;
84
85 tmppkt = cfpkt_append(rfml->incomplete_frm, pkt,
86 rfml->pdu_size + RFM_HEAD_SIZE);
87
88 /* If cfpkt_append failes input pkts are not freed */
89 *err = -ENOMEM;
90 if (tmppkt == NULL)
91 return NULL;
92
93 *err = 0;
94 return tmppkt;
Sjur Braendelandb482cd22010-03-30 13:56:23 +000095}
96
Sjur Braendelandb482cd22010-03-30 13:56:23 +000097static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
98{
99 u8 tmp;
100 bool segmented;
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000101 int err;
102 u8 seghead[6];
103 struct cfrfml *rfml;
104 struct cfpkt *tmppkt = NULL;
105
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000106 caif_assert(layr->up != NULL);
107 caif_assert(layr->receive != NULL);
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000108 rfml = container_obj(layr);
109 spin_lock(&rfml->sync);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000110
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000111 err = -EPROTO;
112 if (cfpkt_extr_head(pkt, &tmp, 1) < 0)
113 goto out;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000114 segmented = tmp & RFM_SEGMENTATION_BIT;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000115
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000116 if (segmented) {
117 if (rfml->incomplete_frm == NULL) {
118 /* Initial Segment */
119 if (cfpkt_peek_head(pkt, rfml->seghead, 6) < 0)
120 goto out;
121
122 rfml->pdu_size = get_unaligned_le16(rfml->seghead+4);
123
124 if (cfpkt_erroneous(pkt))
125 goto out;
126 rfml->incomplete_frm = pkt;
127 pkt = NULL;
128 } else {
129
130 tmppkt = rfm_append(rfml, seghead, pkt, &err);
131 if (tmppkt == NULL)
132 goto out;
133
134 if (cfpkt_erroneous(tmppkt))
135 goto out;
136
137 rfml->incomplete_frm = tmppkt;
138
139
140 if (cfpkt_erroneous(tmppkt))
141 goto out;
142 }
143 err = 0;
144 goto out;
145 }
146
147 if (rfml->incomplete_frm) {
148
149 /* Last Segment */
150 tmppkt = rfm_append(rfml, seghead, pkt, &err);
151 if (tmppkt == NULL)
152 goto out;
153
154 if (cfpkt_erroneous(tmppkt))
155 goto out;
156
157 rfml->incomplete_frm = NULL;
158 pkt = tmppkt;
159 tmppkt = NULL;
160
161 /* Verify that length is correct */
162 err = EPROTO;
163 if (rfml->pdu_size != cfpkt_getlen(pkt) - RFM_HEAD_SIZE + 1)
164 goto out;
165 }
166
167 err = rfml->serv.layer.up->receive(rfml->serv.layer.up, pkt);
168
169out:
170
171 if (err != 0) {
172 if (tmppkt)
173 cfpkt_destroy(tmppkt);
174 if (pkt)
175 cfpkt_destroy(pkt);
176 if (rfml->incomplete_frm)
177 cfpkt_destroy(rfml->incomplete_frm);
178 rfml->incomplete_frm = NULL;
179
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000180 pr_info("Connection error %d triggered on RFM link\n", err);
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000181
182 /* Trigger connection error upon failure.*/
183 layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
184 rfml->serv.dev_info.id);
185 }
186 spin_unlock(&rfml->sync);
Dmitry Tarnyagin374458b2012-03-11 10:28:31 +0000187
188 if (unlikely(err == -EAGAIN))
189 /* It is not possible to recover after drop of a fragment */
190 err = -EIO;
191
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000192 return err;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000193}
194
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000195
196static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000197{
sjur.brandeland@stericsson.com005b0b02011-12-06 12:15:42 +0000198 caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size + RFM_HEAD_SIZE);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000199
200 /* Add info for MUX-layer to route the packet out. */
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000201 cfpkt_info(pkt)->channel_id = rfml->serv.layer.id;
202
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000203 /*
204 * To optimize alignment, we add up the size of CAIF header before
205 * payload.
206 */
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000207 cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE;
208 cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info;
209
210 return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt);
211}
212
213static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
214{
215 int err;
216 u8 seg;
217 u8 head[6];
218 struct cfpkt *rearpkt = NULL;
219 struct cfpkt *frontpkt = pkt;
220 struct cfrfml *rfml = container_obj(layr);
221
222 caif_assert(layr->dn != NULL);
223 caif_assert(layr->dn->transmit != NULL);
224
225 if (!cfsrvl_ready(&rfml->serv, &err))
Dmitry Tarnyagin374458b2012-03-11 10:28:31 +0000226 goto out;
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000227
228 err = -EPROTO;
229 if (cfpkt_getlen(pkt) <= RFM_HEAD_SIZE-1)
230 goto out;
231
232 err = 0;
233 if (cfpkt_getlen(pkt) > rfml->fragment_size + RFM_HEAD_SIZE)
234 err = cfpkt_peek_head(pkt, head, 6);
235
236 if (err < 0)
237 goto out;
238
239 while (cfpkt_getlen(frontpkt) > rfml->fragment_size + RFM_HEAD_SIZE) {
240
241 seg = 1;
242 err = -EPROTO;
243
244 if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
245 goto out;
246 /*
247 * On OOM error cfpkt_split returns NULL.
248 *
249 * NOTE: Segmented pdu is not correctly aligned.
250 * This has negative performance impact.
251 */
252
253 rearpkt = cfpkt_split(frontpkt, rfml->fragment_size);
254 if (rearpkt == NULL)
255 goto out;
256
257 err = cfrfml_transmit_segment(rfml, frontpkt);
258
Dmitry Tarnyagin374458b2012-03-11 10:28:31 +0000259 if (err != 0) {
260 frontpkt = NULL;
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000261 goto out;
Dmitry Tarnyagin374458b2012-03-11 10:28:31 +0000262 }
263
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000264 frontpkt = rearpkt;
265 rearpkt = NULL;
266
267 err = -ENOMEM;
268 if (frontpkt == NULL)
269 goto out;
270 err = -EPROTO;
271 if (cfpkt_add_head(frontpkt, head, 6) < 0)
272 goto out;
273
274 }
275
276 seg = 0;
277 err = -EPROTO;
278
279 if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
280 goto out;
281
282 err = cfrfml_transmit_segment(rfml, frontpkt);
283
284 frontpkt = NULL;
285out:
286
287 if (err != 0) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000288 pr_info("Connection error %d triggered on RFM link\n", err);
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000289 /* Trigger connection error upon failure.*/
290
291 layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
292 rfml->serv.dev_info.id);
293
294 if (rearpkt)
295 cfpkt_destroy(rearpkt);
296
Dmitry Tarnyagin374458b2012-03-11 10:28:31 +0000297 if (frontpkt)
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000298 cfpkt_destroy(frontpkt);
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000299 }
300
301 return err;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000302}