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