Commit | Line | Data |
---|---|---|
07dc1f9f HL |
1 | /* |
2 | * Common data handling layer for ser_gigaset and usb_gigaset | |
3 | * | |
4 | * Copyright (c) 2005 by Tilman Schmidt <tilman@imap.cc>, | |
5 | * Hansjoerg Lipp <hjlipp@web.de>, | |
70440cf2 | 6 | * Stefan Eilers. |
07dc1f9f HL |
7 | * |
8 | * ===================================================================== | |
9 | * This program is free software; you can redistribute it and/or | |
10 | * modify it under the terms of the GNU General Public License as | |
11 | * published by the Free Software Foundation; either version 2 of | |
12 | * the License, or (at your option) any later version. | |
13 | * ===================================================================== | |
07dc1f9f HL |
14 | */ |
15 | ||
16 | #include "gigaset.h" | |
17 | #include <linux/crc-ccitt.h> | |
17b3cff0 | 18 | #include <linux/bitrev.h> |
07dc1f9f | 19 | |
07dc1f9f HL |
20 | /* check if byte must be stuffed/escaped |
21 | * I'm not sure which data should be encoded. | |
2032e2c2 | 22 | * Therefore I will go the hard way and encode every value |
07dc1f9f HL |
23 | * less than 0x20, the flag sequence and the control escape char. |
24 | */ | |
25 | static inline int muststuff(unsigned char c) | |
26 | { | |
27 | if (c < PPP_TRANS) return 1; | |
28 | if (c == PPP_FLAG) return 1; | |
29 | if (c == PPP_ESCAPE) return 1; | |
30 | /* other possible candidates: */ | |
31 | /* 0x91: XON with parity set */ | |
32 | /* 0x93: XOFF with parity set */ | |
33 | return 0; | |
34 | } | |
35 | ||
36 | /* == data input =========================================================== */ | |
37 | ||
2032e2c2 TS |
38 | /* process a block of received bytes in command mode |
39 | * (mstate != MS_LOCKED && (inputstate & INS_command)) | |
40 | * Append received bytes to the command response buffer and forward them | |
41 | * line by line to the response handler. Exit whenever a mode/state change | |
42 | * might have occurred. | |
63e055d1 TS |
43 | * Note: Received lines may be terminated by CR, LF, or CR LF, which will be |
44 | * removed before passing the line to the response handler. | |
07dc1f9f HL |
45 | * Return value: |
46 | * number of processed bytes | |
47 | */ | |
2032e2c2 | 48 | static unsigned cmd_loop(unsigned numbytes, struct inbuf_t *inbuf) |
07dc1f9f | 49 | { |
2032e2c2 | 50 | unsigned char *src = inbuf->data + inbuf->head; |
07dc1f9f | 51 | struct cardstate *cs = inbuf->cs; |
2032e2c2 TS |
52 | unsigned cbytes = cs->cbytes; |
53 | unsigned procbytes = 0; | |
54 | unsigned char c; | |
55 | ||
56 | while (procbytes < numbytes) { | |
57 | c = *src++; | |
58 | procbytes++; | |
59 | ||
60 | switch (c) { | |
61 | case '\n': | |
62 | if (cbytes == 0 && cs->respdata[0] == '\r') { | |
63 | /* collapse LF with preceding CR */ | |
64 | cs->respdata[0] = 0; | |
65 | break; | |
66 | } | |
67 | /* --v-- fall through --v-- */ | |
68 | case '\r': | |
69 | /* end of message line, pass to response handler */ | |
2032e2c2 TS |
70 | if (cbytes >= MAX_RESP_SIZE) { |
71 | dev_warn(cs->dev, "response too large (%d)\n", | |
72 | cbytes); | |
73 | cbytes = MAX_RESP_SIZE; | |
74 | } | |
07dc1f9f | 75 | cs->cbytes = cbytes; |
1528b18f TS |
76 | gigaset_dbg_buffer(DEBUG_TRANSCMD, "received response", |
77 | cbytes, cs->respdata); | |
2032e2c2 | 78 | gigaset_handle_modem_response(cs); |
07dc1f9f HL |
79 | cbytes = 0; |
80 | ||
2032e2c2 TS |
81 | /* store EOL byte for CRLF collapsing */ |
82 | cs->respdata[0] = c; | |
07dc1f9f | 83 | |
2032e2c2 TS |
84 | /* cs->dle may have changed */ |
85 | if (cs->dle && !(inbuf->inputstate & INS_DLE_command)) | |
86 | inbuf->inputstate &= ~INS_command; | |
87 | ||
88 | /* return for reevaluating state */ | |
89 | goto exit; | |
90 | ||
91 | case DLE_FLAG: | |
92 | if (inbuf->inputstate & INS_DLE_char) { | |
93 | /* quoted DLE: clear quote flag */ | |
94 | inbuf->inputstate &= ~INS_DLE_char; | |
95 | } else if (cs->dle || | |
96 | (inbuf->inputstate & INS_DLE_command)) { | |
97 | /* DLE escape, pass up for handling */ | |
98 | inbuf->inputstate |= INS_DLE_char; | |
99 | goto exit; | |
100 | } | |
101 | /* quoted or not in DLE mode: treat as regular data */ | |
102 | /* --v-- fall through --v-- */ | |
103 | default: | |
104 | /* append to line buffer if possible */ | |
105 | if (cbytes < MAX_RESP_SIZE) | |
106 | cs->respdata[cbytes] = c; | |
107 | cbytes++; | |
07dc1f9f HL |
108 | } |
109 | } | |
2032e2c2 | 110 | exit: |
07dc1f9f | 111 | cs->cbytes = cbytes; |
2032e2c2 | 112 | return procbytes; |
07dc1f9f HL |
113 | } |
114 | ||
2032e2c2 TS |
115 | /* process a block of received bytes in lock mode |
116 | * All received bytes are passed unmodified to the tty i/f. | |
07dc1f9f HL |
117 | * Return value: |
118 | * number of processed bytes | |
119 | */ | |
2032e2c2 | 120 | static unsigned lock_loop(unsigned numbytes, struct inbuf_t *inbuf) |
07dc1f9f | 121 | { |
2032e2c2 | 122 | unsigned char *src = inbuf->data + inbuf->head; |
07dc1f9f | 123 | |
2032e2c2 TS |
124 | gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response", numbytes, src); |
125 | gigaset_if_receive(inbuf->cs, src, numbytes); | |
07dc1f9f HL |
126 | return numbytes; |
127 | } | |
128 | ||
2032e2c2 TS |
129 | /* set up next receive skb for data mode |
130 | */ | |
131 | static void new_rcv_skb(struct bc_state *bcs) | |
132 | { | |
133 | struct cardstate *cs = bcs->cs; | |
134 | unsigned short hw_hdr_len = cs->hw_hdr_len; | |
135 | ||
136 | if (bcs->ignore) { | |
137 | bcs->skb = NULL; | |
138 | return; | |
139 | } | |
140 | ||
141 | bcs->skb = dev_alloc_skb(SBUFSIZE + hw_hdr_len); | |
142 | if (bcs->skb == NULL) { | |
143 | dev_warn(cs->dev, "could not allocate new skb\n"); | |
144 | return; | |
145 | } | |
146 | skb_reserve(bcs->skb, hw_hdr_len); | |
147 | } | |
148 | ||
07dc1f9f | 149 | /* process a block of received bytes in HDLC data mode |
2032e2c2 | 150 | * (mstate != MS_LOCKED && !(inputstate & INS_command) && proto2 == L2_HDLC) |
07dc1f9f HL |
151 | * Collect HDLC frames, undoing byte stuffing and watching for DLE escapes. |
152 | * When a frame is complete, check the FCS and pass valid frames to the LL. | |
153 | * If DLE is encountered, return immediately to let the caller handle it. | |
154 | * Return value: | |
155 | * number of processed bytes | |
07dc1f9f | 156 | */ |
2032e2c2 | 157 | static unsigned hdlc_loop(unsigned numbytes, struct inbuf_t *inbuf) |
07dc1f9f HL |
158 | { |
159 | struct cardstate *cs = inbuf->cs; | |
2032e2c2 | 160 | struct bc_state *bcs = cs->bcs; |
d48c7784 TS |
161 | int inputstate = bcs->inputstate; |
162 | __u16 fcs = bcs->fcs; | |
163 | struct sk_buff *skb = bcs->skb; | |
2032e2c2 TS |
164 | unsigned char *src = inbuf->data + inbuf->head; |
165 | unsigned procbytes = 0; | |
166 | unsigned char c; | |
07dc1f9f | 167 | |
2032e2c2 TS |
168 | if (inputstate & INS_byte_stuff) { |
169 | if (!numbytes) | |
170 | return 0; | |
07dc1f9f HL |
171 | inputstate &= ~INS_byte_stuff; |
172 | goto byte_stuff; | |
173 | } | |
2032e2c2 TS |
174 | |
175 | while (procbytes < numbytes) { | |
176 | c = *src++; | |
177 | procbytes++; | |
178 | if (c == DLE_FLAG) { | |
179 | if (inputstate & INS_DLE_char) { | |
180 | /* quoted DLE: clear quote flag */ | |
181 | inputstate &= ~INS_DLE_char; | |
182 | } else if (cs->dle || (inputstate & INS_DLE_command)) { | |
183 | /* DLE escape, pass up for handling */ | |
184 | inputstate |= INS_DLE_char; | |
07dc1f9f HL |
185 | break; |
186 | } | |
2032e2c2 TS |
187 | } |
188 | ||
189 | if (c == PPP_ESCAPE) { | |
190 | /* byte stuffing indicator: pull in next byte */ | |
191 | if (procbytes >= numbytes) { | |
192 | /* end of buffer, save for later processing */ | |
07dc1f9f HL |
193 | inputstate |= INS_byte_stuff; |
194 | break; | |
195 | } | |
196 | byte_stuff: | |
2032e2c2 TS |
197 | c = *src++; |
198 | procbytes++; | |
199 | if (c == DLE_FLAG) { | |
200 | if (inputstate & INS_DLE_char) { | |
201 | /* quoted DLE: clear quote flag */ | |
202 | inputstate &= ~INS_DLE_char; | |
203 | } else if (cs->dle || | |
204 | (inputstate & INS_DLE_command)) { | |
205 | /* DLE escape, pass up for handling */ | |
206 | inputstate |= | |
207 | INS_DLE_char | INS_byte_stuff; | |
208 | break; | |
209 | } | |
210 | } | |
07dc1f9f | 211 | c ^= PPP_TRANS; |
07dc1f9f | 212 | #ifdef CONFIG_GIGASET_DEBUG |
2032e2c2 TS |
213 | if (!muststuff(c)) |
214 | gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c); | |
07dc1f9f | 215 | #endif |
2032e2c2 TS |
216 | } else if (c == PPP_FLAG) { |
217 | /* end of frame: process content if any */ | |
218 | if (inputstate & INS_have_data) { | |
784d5858 TS |
219 | gig_dbg(DEBUG_HDLC, |
220 | "7e----------------------------"); | |
07dc1f9f | 221 | |
2032e2c2 TS |
222 | /* check and pass received frame */ |
223 | if (!skb) { | |
224 | /* skipped frame */ | |
225 | gigaset_isdn_rcv_err(bcs); | |
226 | } else if (skb->len < 2) { | |
227 | /* frame too short for FCS */ | |
228 | dev_warn(cs->dev, | |
229 | "short frame (%d)\n", | |
230 | skb->len); | |
231 | gigaset_isdn_rcv_err(bcs); | |
232 | dev_kfree_skb_any(skb); | |
233 | } else if (fcs != PPP_GOODFCS) { | |
234 | /* frame check error */ | |
784d5858 | 235 | dev_err(cs->dev, |
0a305720 TS |
236 | "Checksum failed, %u bytes corrupted!\n", |
237 | skb->len); | |
088ec0cc | 238 | gigaset_isdn_rcv_err(bcs); |
4dd8230a | 239 | dev_kfree_skb_any(skb); |
2032e2c2 TS |
240 | } else { |
241 | /* good frame */ | |
088ec0cc TS |
242 | __skb_trim(skb, skb->len - 2); |
243 | gigaset_skb_rcvd(bcs, skb); | |
07dc1f9f | 244 | } |
07dc1f9f | 245 | |
2032e2c2 TS |
246 | /* prepare reception of next frame */ |
247 | inputstate &= ~INS_have_data; | |
248 | new_rcv_skb(bcs); | |
249 | skb = bcs->skb; | |
07dc1f9f | 250 | } else { |
2032e2c2 TS |
251 | /* empty frame (7E 7E) */ |
252 | #ifdef CONFIG_GIGASET_DEBUG | |
253 | ++bcs->emptycount; | |
254 | #endif | |
255 | if (!skb) { | |
256 | /* skipped (?) */ | |
257 | gigaset_isdn_rcv_err(bcs); | |
258 | new_rcv_skb(bcs); | |
259 | skb = bcs->skb; | |
088ec0cc | 260 | } |
07dc1f9f HL |
261 | } |
262 | ||
2032e2c2 TS |
263 | fcs = PPP_INITFCS; |
264 | continue; | |
265 | #ifdef CONFIG_GIGASET_DEBUG | |
266 | } else if (muststuff(c)) { | |
07dc1f9f | 267 | /* Should not happen. Possible after ZDLE=1<CR><LF>. */ |
784d5858 | 268 | gig_dbg(DEBUG_HDLC, "not byte stuffed: 0x%02x", c); |
2032e2c2 | 269 | #endif |
07dc1f9f HL |
270 | } |
271 | ||
2032e2c2 | 272 | /* regular data byte, append to skb */ |
07dc1f9f | 273 | #ifdef CONFIG_GIGASET_DEBUG |
2032e2c2 | 274 | if (!(inputstate & INS_have_data)) { |
784d5858 TS |
275 | gig_dbg(DEBUG_HDLC, "7e (%d x) ================", |
276 | bcs->emptycount); | |
07dc1f9f HL |
277 | bcs->emptycount = 0; |
278 | } | |
279 | #endif | |
07dc1f9f | 280 | inputstate |= INS_have_data; |
2032e2c2 TS |
281 | if (skb) { |
282 | if (skb->len == SBUFSIZE) { | |
784d5858 | 283 | dev_warn(cs->dev, "received packet too long\n"); |
07dc1f9f | 284 | dev_kfree_skb_any(skb); |
2032e2c2 TS |
285 | /* skip remainder of packet */ |
286 | bcs->skb = skb = NULL; | |
287 | } else { | |
288 | *__skb_put(skb, 1) = c; | |
289 | fcs = crc_ccitt_byte(fcs, c); | |
07dc1f9f | 290 | } |
07dc1f9f HL |
291 | } |
292 | } | |
2032e2c2 | 293 | |
07dc1f9f HL |
294 | bcs->inputstate = inputstate; |
295 | bcs->fcs = fcs; | |
2032e2c2 | 296 | return procbytes; |
07dc1f9f HL |
297 | } |
298 | ||
299 | /* process a block of received bytes in transparent data mode | |
2032e2c2 | 300 | * (mstate != MS_LOCKED && !(inputstate & INS_command) && proto2 != L2_HDLC) |
07dc1f9f HL |
301 | * Invert bytes, undoing byte stuffing and watching for DLE escapes. |
302 | * If DLE is encountered, return immediately to let the caller handle it. | |
303 | * Return value: | |
304 | * number of processed bytes | |
07dc1f9f | 305 | */ |
2032e2c2 | 306 | static unsigned iraw_loop(unsigned numbytes, struct inbuf_t *inbuf) |
07dc1f9f HL |
307 | { |
308 | struct cardstate *cs = inbuf->cs; | |
2032e2c2 | 309 | struct bc_state *bcs = cs->bcs; |
d48c7784 TS |
310 | int inputstate = bcs->inputstate; |
311 | struct sk_buff *skb = bcs->skb; | |
2032e2c2 TS |
312 | unsigned char *src = inbuf->data + inbuf->head; |
313 | unsigned procbytes = 0; | |
314 | unsigned char c; | |
07dc1f9f | 315 | |
2032e2c2 TS |
316 | if (!skb) { |
317 | /* skip this block */ | |
318 | new_rcv_skb(bcs); | |
319 | return numbytes; | |
320 | } | |
07dc1f9f | 321 | |
2032e2c2 TS |
322 | while (procbytes < numbytes && skb->len < SBUFSIZE) { |
323 | c = *src++; | |
324 | procbytes++; | |
325 | ||
326 | if (c == DLE_FLAG) { | |
327 | if (inputstate & INS_DLE_char) { | |
328 | /* quoted DLE: clear quote flag */ | |
329 | inputstate &= ~INS_DLE_char; | |
330 | } else if (cs->dle || (inputstate & INS_DLE_command)) { | |
331 | /* DLE escape, pass up for handling */ | |
332 | inputstate |= INS_DLE_char; | |
07dc1f9f HL |
333 | break; |
334 | } | |
07dc1f9f HL |
335 | } |
336 | ||
2032e2c2 TS |
337 | /* regular data byte: append to current skb */ |
338 | inputstate |= INS_have_data; | |
339 | *__skb_put(skb, 1) = bitrev8(c); | |
07dc1f9f HL |
340 | } |
341 | ||
342 | /* pass data up */ | |
2032e2c2 TS |
343 | if (inputstate & INS_have_data) { |
344 | gigaset_skb_rcvd(bcs, skb); | |
345 | inputstate &= ~INS_have_data; | |
346 | new_rcv_skb(bcs); | |
347 | } | |
348 | ||
349 | bcs->inputstate = inputstate; | |
350 | return procbytes; | |
351 | } | |
352 | ||
353 | /* process DLE escapes | |
354 | * Called whenever a DLE sequence might be encountered in the input stream. | |
355 | * Either processes the entire DLE sequence or, if that isn't possible, | |
356 | * notes the fact that an initial DLE has been received in the INS_DLE_char | |
357 | * inputstate flag and resumes processing of the sequence on the next call. | |
358 | */ | |
359 | static void handle_dle(struct inbuf_t *inbuf) | |
360 | { | |
361 | struct cardstate *cs = inbuf->cs; | |
362 | ||
363 | if (cs->mstate == MS_LOCKED) | |
364 | return; /* no DLE processing in lock mode */ | |
365 | ||
366 | if (!(inbuf->inputstate & INS_DLE_char)) { | |
367 | /* no DLE pending */ | |
368 | if (inbuf->data[inbuf->head] == DLE_FLAG && | |
369 | (cs->dle || inbuf->inputstate & INS_DLE_command)) { | |
370 | /* start of DLE sequence */ | |
371 | inbuf->head++; | |
372 | if (inbuf->head == inbuf->tail || | |
373 | inbuf->head == RBUFSIZE) { | |
374 | /* end of buffer, save for later processing */ | |
375 | inbuf->inputstate |= INS_DLE_char; | |
376 | return; | |
088ec0cc | 377 | } |
2032e2c2 TS |
378 | } else { |
379 | /* regular data byte */ | |
380 | return; | |
07dc1f9f HL |
381 | } |
382 | } | |
383 | ||
2032e2c2 TS |
384 | /* consume pending DLE */ |
385 | inbuf->inputstate &= ~INS_DLE_char; | |
386 | ||
387 | switch (inbuf->data[inbuf->head]) { | |
388 | case 'X': /* begin of event message */ | |
389 | if (inbuf->inputstate & INS_command) | |
390 | dev_notice(cs->dev, | |
391 | "received <DLE>X in command mode\n"); | |
392 | inbuf->inputstate |= INS_command | INS_DLE_command; | |
393 | inbuf->head++; /* byte consumed */ | |
394 | break; | |
395 | case '.': /* end of event message */ | |
396 | if (!(inbuf->inputstate & INS_DLE_command)) | |
397 | dev_notice(cs->dev, | |
398 | "received <DLE>. without <DLE>X\n"); | |
399 | inbuf->inputstate &= ~INS_DLE_command; | |
400 | /* return to data mode if in DLE mode */ | |
401 | if (cs->dle) | |
402 | inbuf->inputstate &= ~INS_command; | |
403 | inbuf->head++; /* byte consumed */ | |
404 | break; | |
405 | case DLE_FLAG: /* DLE in data stream */ | |
406 | /* mark as quoted */ | |
407 | inbuf->inputstate |= INS_DLE_char; | |
408 | if (!(cs->dle || inbuf->inputstate & INS_DLE_command)) | |
409 | dev_notice(cs->dev, | |
410 | "received <DLE><DLE> not in DLE mode\n"); | |
411 | break; /* quoted byte left in buffer */ | |
412 | default: | |
413 | dev_notice(cs->dev, "received <DLE><%02x>\n", | |
414 | inbuf->data[inbuf->head]); | |
415 | /* quoted byte left in buffer */ | |
416 | } | |
07dc1f9f HL |
417 | } |
418 | ||
1cec9727 TS |
419 | /** |
420 | * gigaset_m10x_input() - process a block of data received from the device | |
421 | * @inbuf: received data and device descriptor structure. | |
422 | * | |
423 | * Called by hardware module {ser,usb}_gigaset with a block of received | |
424 | * bytes. Separates the bytes received over the serial data channel into | |
425 | * user data and command replies (locked/unlocked) according to the | |
426 | * current state of the interface. | |
07dc1f9f HL |
427 | */ |
428 | void gigaset_m10x_input(struct inbuf_t *inbuf) | |
429 | { | |
2032e2c2 TS |
430 | struct cardstate *cs = inbuf->cs; |
431 | unsigned numbytes, procbytes; | |
07dc1f9f | 432 | |
2032e2c2 | 433 | gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", inbuf->head, inbuf->tail); |
07dc1f9f | 434 | |
2032e2c2 TS |
435 | while (inbuf->head != inbuf->tail) { |
436 | /* check for DLE escape */ | |
437 | handle_dle(inbuf); | |
07dc1f9f | 438 | |
2032e2c2 TS |
439 | /* process a contiguous block of bytes */ |
440 | numbytes = (inbuf->head > inbuf->tail ? | |
441 | RBUFSIZE : inbuf->tail) - inbuf->head; | |
442 | gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes); | |
443 | /* | |
444 | * numbytes may be 0 if handle_dle() ate the last byte. | |
445 | * This does no harm, *_loop() will just return 0 immediately. | |
446 | */ | |
447 | ||
448 | if (cs->mstate == MS_LOCKED) | |
449 | procbytes = lock_loop(numbytes, inbuf); | |
450 | else if (inbuf->inputstate & INS_command) | |
451 | procbytes = cmd_loop(numbytes, inbuf); | |
452 | else if (cs->bcs->proto2 == L2_HDLC) | |
453 | procbytes = hdlc_loop(numbytes, inbuf); | |
454 | else | |
455 | procbytes = iraw_loop(numbytes, inbuf); | |
456 | inbuf->head += procbytes; | |
457 | ||
458 | /* check for buffer wraparound */ | |
459 | if (inbuf->head >= RBUFSIZE) | |
460 | inbuf->head = 0; | |
461 | ||
462 | gig_dbg(DEBUG_INTR, "head set to %u", inbuf->head); | |
07dc1f9f HL |
463 | } |
464 | } | |
d5c1682f | 465 | EXPORT_SYMBOL_GPL(gigaset_m10x_input); |
07dc1f9f HL |
466 | |
467 | ||
468 | /* == data output ========================================================== */ | |
469 | ||
088ec0cc TS |
470 | /* |
471 | * Encode a data packet into an octet stuffed HDLC frame with FCS, | |
472 | * opening and closing flags, preserving headroom data. | |
07dc1f9f | 473 | * parameters: |
088ec0cc | 474 | * skb skb containing original packet (freed upon return) |
07dc1f9f HL |
475 | * Return value: |
476 | * pointer to newly allocated skb containing the result frame | |
4dd8230a | 477 | * and the original link layer header, NULL on error |
07dc1f9f | 478 | */ |
4dd8230a | 479 | static struct sk_buff *HDLC_Encode(struct sk_buff *skb) |
07dc1f9f HL |
480 | { |
481 | struct sk_buff *hdlc_skb; | |
482 | __u16 fcs; | |
483 | unsigned char c; | |
484 | unsigned char *cp; | |
485 | int len; | |
486 | unsigned int stuf_cnt; | |
487 | ||
488 | stuf_cnt = 0; | |
489 | fcs = PPP_INITFCS; | |
490 | cp = skb->data; | |
491 | len = skb->len; | |
492 | while (len--) { | |
493 | if (muststuff(*cp)) | |
494 | stuf_cnt++; | |
495 | fcs = crc_ccitt_byte(fcs, *cp++); | |
496 | } | |
784d5858 | 497 | fcs ^= 0xffff; /* complement */ |
07dc1f9f HL |
498 | |
499 | /* size of new buffer: original size + number of stuffing bytes | |
500 | * + 2 bytes FCS + 2 stuffing bytes for FCS (if needed) + 2 flag bytes | |
4dd8230a | 501 | * + room for link layer header |
07dc1f9f | 502 | */ |
4dd8230a | 503 | hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + skb->mac_len); |
07dc1f9f | 504 | if (!hdlc_skb) { |
4dd8230a | 505 | dev_kfree_skb_any(skb); |
07dc1f9f HL |
506 | return NULL; |
507 | } | |
07dc1f9f | 508 | |
4dd8230a TS |
509 | /* Copy link layer header into new skb */ |
510 | skb_reset_mac_header(hdlc_skb); | |
511 | skb_reserve(hdlc_skb, skb->mac_len); | |
512 | memcpy(skb_mac_header(hdlc_skb), skb_mac_header(skb), skb->mac_len); | |
513 | hdlc_skb->mac_len = skb->mac_len; | |
07dc1f9f HL |
514 | |
515 | /* Add flag sequence in front of everything.. */ | |
516 | *(skb_put(hdlc_skb, 1)) = PPP_FLAG; | |
517 | ||
518 | /* Perform byte stuffing while copying data. */ | |
519 | while (skb->len--) { | |
520 | if (muststuff(*skb->data)) { | |
521 | *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE; | |
522 | *(skb_put(hdlc_skb, 1)) = (*skb->data++) ^ PPP_TRANS; | |
523 | } else | |
524 | *(skb_put(hdlc_skb, 1)) = *skb->data++; | |
525 | } | |
526 | ||
527 | /* Finally add FCS (byte stuffed) and flag sequence */ | |
784d5858 | 528 | c = (fcs & 0x00ff); /* least significant byte first */ |
07dc1f9f HL |
529 | if (muststuff(c)) { |
530 | *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE; | |
531 | c ^= PPP_TRANS; | |
532 | } | |
533 | *(skb_put(hdlc_skb, 1)) = c; | |
534 | ||
535 | c = ((fcs >> 8) & 0x00ff); | |
536 | if (muststuff(c)) { | |
537 | *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE; | |
538 | c ^= PPP_TRANS; | |
539 | } | |
540 | *(skb_put(hdlc_skb, 1)) = c; | |
541 | ||
542 | *(skb_put(hdlc_skb, 1)) = PPP_FLAG; | |
543 | ||
4dd8230a | 544 | dev_kfree_skb_any(skb); |
07dc1f9f HL |
545 | return hdlc_skb; |
546 | } | |
547 | ||
088ec0cc TS |
548 | /* |
549 | * Encode a data packet into an octet stuffed raw bit inverted frame, | |
550 | * preserving headroom data. | |
07dc1f9f | 551 | * parameters: |
088ec0cc | 552 | * skb skb containing original packet (freed upon return) |
07dc1f9f HL |
553 | * Return value: |
554 | * pointer to newly allocated skb containing the result frame | |
4dd8230a | 555 | * and the original link layer header, NULL on error |
07dc1f9f | 556 | */ |
4dd8230a | 557 | static struct sk_buff *iraw_encode(struct sk_buff *skb) |
07dc1f9f HL |
558 | { |
559 | struct sk_buff *iraw_skb; | |
560 | unsigned char c; | |
561 | unsigned char *cp; | |
562 | int len; | |
563 | ||
4dd8230a TS |
564 | /* size of new buffer (worst case = every byte must be stuffed): |
565 | * 2 * original size + room for link layer header | |
566 | */ | |
567 | iraw_skb = dev_alloc_skb(2*skb->len + skb->mac_len); | |
07dc1f9f | 568 | if (!iraw_skb) { |
4dd8230a | 569 | dev_kfree_skb_any(skb); |
07dc1f9f HL |
570 | return NULL; |
571 | } | |
088ec0cc | 572 | |
4dd8230a TS |
573 | /* copy link layer header into new skb */ |
574 | skb_reset_mac_header(iraw_skb); | |
575 | skb_reserve(iraw_skb, skb->mac_len); | |
576 | memcpy(skb_mac_header(iraw_skb), skb_mac_header(skb), skb->mac_len); | |
577 | iraw_skb->mac_len = skb->mac_len; | |
07dc1f9f | 578 | |
4dd8230a | 579 | /* copy and stuff data */ |
07dc1f9f HL |
580 | cp = skb->data; |
581 | len = skb->len; | |
582 | while (len--) { | |
17b3cff0 | 583 | c = bitrev8(*cp++); |
07dc1f9f HL |
584 | if (c == DLE_FLAG) |
585 | *(skb_put(iraw_skb, 1)) = c; | |
586 | *(skb_put(iraw_skb, 1)) = c; | |
587 | } | |
4dd8230a | 588 | dev_kfree_skb_any(skb); |
07dc1f9f HL |
589 | return iraw_skb; |
590 | } | |
591 | ||
1cec9727 TS |
592 | /** |
593 | * gigaset_m10x_send_skb() - queue an skb for sending | |
594 | * @bcs: B channel descriptor structure. | |
595 | * @skb: data to send. | |
596 | * | |
088ec0cc | 597 | * Called by LL to encode and queue an skb for sending, and start |
1cec9727 | 598 | * transmission if necessary. |
088ec0cc | 599 | * Once the payload data has been transmitted completely, gigaset_skb_sent() |
4dd8230a | 600 | * will be called with the skb's link layer header preserved. |
1cec9727 | 601 | * |
07dc1f9f | 602 | * Return value: |
1cec9727 TS |
603 | * number of bytes accepted for sending (skb->len) if ok, |
604 | * error code < 0 (eg. -ENOMEM) on error | |
07dc1f9f HL |
605 | */ |
606 | int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb) | |
607 | { | |
2032e2c2 | 608 | struct cardstate *cs = bcs->cs; |
d48c7784 | 609 | unsigned len = skb->len; |
69049cc8 | 610 | unsigned long flags; |
07dc1f9f | 611 | |
088ec0cc | 612 | if (bcs->proto2 == L2_HDLC) |
4dd8230a | 613 | skb = HDLC_Encode(skb); |
07dc1f9f | 614 | else |
4dd8230a | 615 | skb = iraw_encode(skb); |
784d5858 | 616 | if (!skb) { |
2032e2c2 | 617 | dev_err(cs->dev, |
5002779d | 618 | "unable to allocate memory for encoding!\n"); |
07dc1f9f | 619 | return -ENOMEM; |
784d5858 | 620 | } |
07dc1f9f HL |
621 | |
622 | skb_queue_tail(&bcs->squeue, skb); | |
2032e2c2 TS |
623 | spin_lock_irqsave(&cs->lock, flags); |
624 | if (cs->connected) | |
625 | tasklet_schedule(&cs->write_tasklet); | |
626 | spin_unlock_irqrestore(&cs->lock, flags); | |
07dc1f9f HL |
627 | |
628 | return len; /* ok so far */ | |
629 | } | |
d5c1682f | 630 | EXPORT_SYMBOL_GPL(gigaset_m10x_send_skb); |