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