blob: 0deabb4400517e9d49205661955b3f06f6f1343c [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;
49 struct cfrfml *this =
50 kzalloc(sizeof(struct cfrfml), GFP_ATOMIC);
51
52 if (!this) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +000053 pr_warn("Out of memory\n");
Sjur Braendelandb482cd22010-03-30 13:56:23 +000054 return NULL;
55 }
Sjur Braendelandb1c74242010-06-17 06:55:38 +000056
Sjur Braendelanda7da1f52010-06-17 06:55:39 +000057 cfsrvl_init(&this->serv, channel_id, dev_info, false);
58 this->serv.release = cfrfml_release;
59 this->serv.layer.receive = cfrfml_receive;
60 this->serv.layer.transmit = cfrfml_transmit;
Sjur Braendelandb1c74242010-06-17 06:55:38 +000061
Sjur Braendelanda7da1f52010-06-17 06:55:39 +000062 /* Round down to closest multiple of 16 */
63 tmp = (mtu_size - RFM_HEAD_SIZE - 6) / 16;
64 tmp *= 16;
65
66 this->fragment_size = tmp;
67 spin_lock_init(&this->sync);
68 snprintf(this->serv.layer.name, CAIF_LAYER_NAME_SZ,
69 "rfm%d", channel_id);
70
71 return &this->serv.layer;
72}
73
74static struct cfpkt *rfm_append(struct cfrfml *rfml, char *seghead,
75 struct cfpkt *pkt, int *err)
76{
77 struct cfpkt *tmppkt;
78 *err = -EPROTO;
79 /* n-th but not last segment */
80
81 if (cfpkt_extr_head(pkt, seghead, 6) < 0)
82 return NULL;
83
84 /* Verify correct header */
85 if (memcmp(seghead, rfml->seghead, 6) != 0)
86 return NULL;
87
88 tmppkt = cfpkt_append(rfml->incomplete_frm, pkt,
89 rfml->pdu_size + RFM_HEAD_SIZE);
90
91 /* If cfpkt_append failes input pkts are not freed */
92 *err = -ENOMEM;
93 if (tmppkt == NULL)
94 return NULL;
95
96 *err = 0;
97 return tmppkt;
Sjur Braendelandb482cd22010-03-30 13:56:23 +000098}
99
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000100static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
101{
102 u8 tmp;
103 bool segmented;
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000104 int err;
105 u8 seghead[6];
106 struct cfrfml *rfml;
107 struct cfpkt *tmppkt = NULL;
108
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000109 caif_assert(layr->up != NULL);
110 caif_assert(layr->receive != NULL);
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000111 rfml = container_obj(layr);
112 spin_lock(&rfml->sync);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000113
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000114 err = -EPROTO;
115 if (cfpkt_extr_head(pkt, &tmp, 1) < 0)
116 goto out;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000117 segmented = tmp & RFM_SEGMENTATION_BIT;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000118
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000119 if (segmented) {
120 if (rfml->incomplete_frm == NULL) {
121 /* Initial Segment */
122 if (cfpkt_peek_head(pkt, rfml->seghead, 6) < 0)
123 goto out;
124
125 rfml->pdu_size = get_unaligned_le16(rfml->seghead+4);
126
127 if (cfpkt_erroneous(pkt))
128 goto out;
129 rfml->incomplete_frm = pkt;
130 pkt = NULL;
131 } else {
132
133 tmppkt = rfm_append(rfml, seghead, pkt, &err);
134 if (tmppkt == NULL)
135 goto out;
136
137 if (cfpkt_erroneous(tmppkt))
138 goto out;
139
140 rfml->incomplete_frm = tmppkt;
141
142
143 if (cfpkt_erroneous(tmppkt))
144 goto out;
145 }
146 err = 0;
147 goto out;
148 }
149
150 if (rfml->incomplete_frm) {
151
152 /* Last Segment */
153 tmppkt = rfm_append(rfml, seghead, pkt, &err);
154 if (tmppkt == NULL)
155 goto out;
156
157 if (cfpkt_erroneous(tmppkt))
158 goto out;
159
160 rfml->incomplete_frm = NULL;
161 pkt = tmppkt;
162 tmppkt = NULL;
163
164 /* Verify that length is correct */
165 err = EPROTO;
166 if (rfml->pdu_size != cfpkt_getlen(pkt) - RFM_HEAD_SIZE + 1)
167 goto out;
168 }
169
170 err = rfml->serv.layer.up->receive(rfml->serv.layer.up, pkt);
171
172out:
173
174 if (err != 0) {
175 if (tmppkt)
176 cfpkt_destroy(tmppkt);
177 if (pkt)
178 cfpkt_destroy(pkt);
179 if (rfml->incomplete_frm)
180 cfpkt_destroy(rfml->incomplete_frm);
181 rfml->incomplete_frm = NULL;
182
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000183 pr_info("Connection error %d triggered on RFM link\n", err);
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000184
185 /* Trigger connection error upon failure.*/
186 layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
187 rfml->serv.dev_info.id);
188 }
189 spin_unlock(&rfml->sync);
190 return err;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000191}
192
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000193
194static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000195{
André Carvalho de Matosf2527ec2010-11-01 11:52:47 +0000196 caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size);
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000197
198 /* Add info for MUX-layer to route the packet out. */
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000199 cfpkt_info(pkt)->channel_id = rfml->serv.layer.id;
200
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000201 /*
202 * To optimize alignment, we add up the size of CAIF header before
203 * payload.
204 */
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000205 cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE;
206 cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info;
207
208 return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt);
209}
210
211static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
212{
213 int err;
214 u8 seg;
215 u8 head[6];
216 struct cfpkt *rearpkt = NULL;
217 struct cfpkt *frontpkt = pkt;
218 struct cfrfml *rfml = container_obj(layr);
219
220 caif_assert(layr->dn != NULL);
221 caif_assert(layr->dn->transmit != NULL);
222
223 if (!cfsrvl_ready(&rfml->serv, &err))
224 return err;
225
226 err = -EPROTO;
227 if (cfpkt_getlen(pkt) <= RFM_HEAD_SIZE-1)
228 goto out;
229
230 err = 0;
231 if (cfpkt_getlen(pkt) > rfml->fragment_size + RFM_HEAD_SIZE)
232 err = cfpkt_peek_head(pkt, head, 6);
233
234 if (err < 0)
235 goto out;
236
237 while (cfpkt_getlen(frontpkt) > rfml->fragment_size + RFM_HEAD_SIZE) {
238
239 seg = 1;
240 err = -EPROTO;
241
242 if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
243 goto out;
244 /*
245 * On OOM error cfpkt_split returns NULL.
246 *
247 * NOTE: Segmented pdu is not correctly aligned.
248 * This has negative performance impact.
249 */
250
251 rearpkt = cfpkt_split(frontpkt, rfml->fragment_size);
252 if (rearpkt == NULL)
253 goto out;
254
255 err = cfrfml_transmit_segment(rfml, frontpkt);
256
257 if (err != 0)
258 goto out;
259 frontpkt = rearpkt;
260 rearpkt = NULL;
261
262 err = -ENOMEM;
263 if (frontpkt == NULL)
264 goto out;
265 err = -EPROTO;
266 if (cfpkt_add_head(frontpkt, head, 6) < 0)
267 goto out;
268
269 }
270
271 seg = 0;
272 err = -EPROTO;
273
274 if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
275 goto out;
276
277 err = cfrfml_transmit_segment(rfml, frontpkt);
278
279 frontpkt = NULL;
280out:
281
282 if (err != 0) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000283 pr_info("Connection error %d triggered on RFM link\n", err);
Sjur Braendelanda7da1f52010-06-17 06:55:39 +0000284 /* Trigger connection error upon failure.*/
285
286 layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
287 rfml->serv.dev_info.id);
288
289 if (rearpkt)
290 cfpkt_destroy(rearpkt);
291
292 if (frontpkt && frontpkt != pkt) {
293
294 cfpkt_destroy(frontpkt);
295 /*
296 * Socket layer will free the original packet,
297 * but this packet may already be sent and
298 * freed. So we have to return 0 in this case
299 * to avoid socket layer to re-free this packet.
300 * The return of shutdown indication will
301 * cause connection to be invalidated anyhow.
302 */
303 err = 0;
304 }
305 }
306
307 return err;
Sjur Braendelandb482cd22010-03-30 13:56:23 +0000308}