Root/
1 | /* |
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 | |
7 | #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ |
8 | |
9 | #include <linux/stddef.h> |
10 | #include <linux/spinlock.h> |
11 | #include <linux/slab.h> |
12 | #include <asm/unaligned.h> |
13 | #include <net/caif/caif_layer.h> |
14 | #include <net/caif/cfsrvl.h> |
15 | #include <net/caif/cfpkt.h> |
16 | |
17 | #define container_obj(layr) container_of(layr, struct cfrfml, serv.layer) |
18 | #define RFM_SEGMENTATION_BIT 0x01 |
19 | #define RFM_HEAD_SIZE 7 |
20 | |
21 | static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt); |
22 | static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt); |
23 | |
24 | struct 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 | }; |
33 | |
34 | static void cfrfml_release(struct kref *kref) |
35 | { |
36 | struct cfsrvl *srvl = container_of(kref, struct cfsrvl, ref); |
37 | 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 | |
45 | struct 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) { |
53 | pr_warn("Out of memory\n"); |
54 | return NULL; |
55 | } |
56 | |
57 | 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; |
61 | |
62 | /* 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 | |
74 | static 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; |
98 | } |
99 | |
100 | static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt) |
101 | { |
102 | u8 tmp; |
103 | bool segmented; |
104 | int err; |
105 | u8 seghead[6]; |
106 | struct cfrfml *rfml; |
107 | struct cfpkt *tmppkt = NULL; |
108 | |
109 | caif_assert(layr->up != NULL); |
110 | caif_assert(layr->receive != NULL); |
111 | rfml = container_obj(layr); |
112 | spin_lock(&rfml->sync); |
113 | |
114 | err = -EPROTO; |
115 | if (cfpkt_extr_head(pkt, &tmp, 1) < 0) |
116 | goto out; |
117 | segmented = tmp & RFM_SEGMENTATION_BIT; |
118 | |
119 | 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 | |
172 | out: |
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 | |
183 | pr_info("Connection error %d triggered on RFM link\n", err); |
184 | |
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; |
191 | } |
192 | |
193 | |
194 | static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt) |
195 | { |
196 | caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size); |
197 | |
198 | /* Add info for MUX-layer to route the packet out. */ |
199 | cfpkt_info(pkt)->channel_id = rfml->serv.layer.id; |
200 | |
201 | /* |
202 | * To optimize alignment, we add up the size of CAIF header before |
203 | * payload. |
204 | */ |
205 | 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 | |
211 | static 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; |
280 | out: |
281 | |
282 | if (err != 0) { |
283 | pr_info("Connection error %d triggered on RFM link\n", err); |
284 | /* 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; |
308 | } |
309 |
Branches:
ben-wpan
ben-wpan-stefan
javiroman/ks7010
jz-2.6.34
jz-2.6.34-rc5
jz-2.6.34-rc6
jz-2.6.34-rc7
jz-2.6.35
jz-2.6.36
jz-2.6.37
jz-2.6.38
jz-2.6.39
jz-3.0
jz-3.1
jz-3.11
jz-3.12
jz-3.13
jz-3.15
jz-3.16
jz-3.18-dt
jz-3.2
jz-3.3
jz-3.4
jz-3.5
jz-3.6
jz-3.6-rc2-pwm
jz-3.9
jz-3.9-clk
jz-3.9-rc8
jz47xx
jz47xx-2.6.38
master
Tags:
od-2011-09-04
od-2011-09-18
v2.6.34-rc5
v2.6.34-rc6
v2.6.34-rc7
v3.9