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 HL |
19 | |
20 | //#define GIG_M10x_STUFF_VOICE_DATA | |
21 | ||
22 | /* check if byte must be stuffed/escaped | |
23 | * I'm not sure which data should be encoded. | |
24 | * Therefore I will go the hard way and decode every value | |
25 | * less than 0x20, the flag sequence and the control escape char. | |
26 | */ | |
27 | static inline int muststuff(unsigned char c) | |
28 | { | |
29 | if (c < PPP_TRANS) return 1; | |
30 | if (c == PPP_FLAG) return 1; | |
31 | if (c == PPP_ESCAPE) return 1; | |
32 | /* other possible candidates: */ | |
33 | /* 0x91: XON with parity set */ | |
34 | /* 0x93: XOFF with parity set */ | |
35 | return 0; | |
36 | } | |
37 | ||
38 | /* == data input =========================================================== */ | |
39 | ||
40 | /* process a block of received bytes in command mode (modem response) | |
41 | * Return value: | |
42 | * number of processed bytes | |
43 | */ | |
44 | static inline int cmd_loop(unsigned char c, unsigned char *src, int numbytes, | |
784d5858 | 45 | struct inbuf_t *inbuf) |
07dc1f9f HL |
46 | { |
47 | struct cardstate *cs = inbuf->cs; | |
48 | unsigned cbytes = cs->cbytes; | |
49 | int inputstate = inbuf->inputstate; | |
50 | int startbytes = numbytes; | |
51 | ||
52 | for (;;) { | |
53 | cs->respdata[cbytes] = c; | |
54 | if (c == 10 || c == 13) { | |
784d5858 TS |
55 | gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)", |
56 | __func__, cbytes); | |
07dc1f9f | 57 | cs->cbytes = cbytes; |
917f5085 TS |
58 | gigaset_handle_modem_response(cs); /* can change |
59 | cs->dle */ | |
07dc1f9f HL |
60 | cbytes = 0; |
61 | ||
62 | if (cs->dle && | |
63 | !(inputstate & INS_DLE_command)) { | |
64 | inputstate &= ~INS_command; | |
65 | break; | |
66 | } | |
67 | } else { | |
68 | /* advance in line buffer, checking for overflow */ | |
69 | if (cbytes < MAX_RESP_SIZE - 1) | |
70 | cbytes++; | |
71 | else | |
784d5858 | 72 | dev_warn(cs->dev, "response too large\n"); |
07dc1f9f HL |
73 | } |
74 | ||
75 | if (!numbytes) | |
76 | break; | |
77 | c = *src++; | |
78 | --numbytes; | |
79 | if (c == DLE_FLAG && | |
80 | (cs->dle || inputstate & INS_DLE_command)) { | |
81 | inputstate |= INS_DLE_char; | |
82 | break; | |
83 | } | |
84 | } | |
85 | ||
86 | cs->cbytes = cbytes; | |
87 | inbuf->inputstate = inputstate; | |
88 | ||
89 | return startbytes - numbytes; | |
90 | } | |
91 | ||
92 | /* process a block of received bytes in lock mode (tty i/f) | |
93 | * Return value: | |
94 | * number of processed bytes | |
95 | */ | |
96 | static inline int lock_loop(unsigned char *src, int numbytes, | |
784d5858 | 97 | struct inbuf_t *inbuf) |
07dc1f9f HL |
98 | { |
99 | struct cardstate *cs = inbuf->cs; | |
100 | ||
917f5085 | 101 | gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response", |
01371500 | 102 | numbytes, src); |
07dc1f9f HL |
103 | gigaset_if_receive(cs, src, numbytes); |
104 | ||
105 | return numbytes; | |
106 | } | |
107 | ||
108 | /* process a block of received bytes in HDLC data mode | |
109 | * Collect HDLC frames, undoing byte stuffing and watching for DLE escapes. | |
110 | * When a frame is complete, check the FCS and pass valid frames to the LL. | |
111 | * If DLE is encountered, return immediately to let the caller handle it. | |
112 | * Return value: | |
113 | * number of processed bytes | |
114 | * numbytes (all bytes processed) on error --FIXME | |
115 | */ | |
116 | static inline int hdlc_loop(unsigned char c, unsigned char *src, int numbytes, | |
784d5858 | 117 | struct inbuf_t *inbuf) |
07dc1f9f HL |
118 | { |
119 | struct cardstate *cs = inbuf->cs; | |
120 | struct bc_state *bcs = inbuf->bcs; | |
d48c7784 TS |
121 | int inputstate = bcs->inputstate; |
122 | __u16 fcs = bcs->fcs; | |
123 | struct sk_buff *skb = bcs->skb; | |
07dc1f9f HL |
124 | unsigned char error; |
125 | struct sk_buff *compskb; | |
126 | int startbytes = numbytes; | |
127 | int l; | |
128 | ||
07dc1f9f HL |
129 | if (unlikely(inputstate & INS_byte_stuff)) { |
130 | inputstate &= ~INS_byte_stuff; | |
131 | goto byte_stuff; | |
132 | } | |
133 | for (;;) { | |
134 | if (unlikely(c == PPP_ESCAPE)) { | |
135 | if (unlikely(!numbytes)) { | |
136 | inputstate |= INS_byte_stuff; | |
137 | break; | |
138 | } | |
139 | c = *src++; | |
140 | --numbytes; | |
141 | if (unlikely(c == DLE_FLAG && | |
142 | (cs->dle || | |
143 | inbuf->inputstate & INS_DLE_command))) { | |
144 | inbuf->inputstate |= INS_DLE_char; | |
145 | inputstate |= INS_byte_stuff; | |
146 | break; | |
147 | } | |
148 | byte_stuff: | |
149 | c ^= PPP_TRANS; | |
150 | #ifdef CONFIG_GIGASET_DEBUG | |
151 | if (unlikely(!muststuff(c))) | |
784d5858 | 152 | gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c); |
07dc1f9f HL |
153 | #endif |
154 | } else if (unlikely(c == PPP_FLAG)) { | |
155 | if (unlikely(inputstate & INS_skip_frame)) { | |
156 | if (!(inputstate & INS_have_data)) { /* 7E 7E */ | |
07dc1f9f HL |
157 | #ifdef CONFIG_GIGASET_DEBUG |
158 | ++bcs->emptycount; | |
159 | #endif | |
160 | } else | |
784d5858 | 161 | gig_dbg(DEBUG_HDLC, |
07dc1f9f HL |
162 | "7e----------------------------"); |
163 | ||
164 | /* end of frame */ | |
165 | error = 1; | |
166 | gigaset_rcv_error(NULL, cs, bcs); | |
167 | } else if (!(inputstate & INS_have_data)) { /* 7E 7E */ | |
07dc1f9f HL |
168 | #ifdef CONFIG_GIGASET_DEBUG |
169 | ++bcs->emptycount; | |
170 | #endif | |
171 | break; | |
172 | } else { | |
784d5858 TS |
173 | gig_dbg(DEBUG_HDLC, |
174 | "7e----------------------------"); | |
07dc1f9f HL |
175 | |
176 | /* end of frame */ | |
177 | error = 0; | |
178 | ||
179 | if (unlikely(fcs != PPP_GOODFCS)) { | |
784d5858 TS |
180 | dev_err(cs->dev, |
181 | "Packet checksum at %lu failed, " | |
182 | "packet is corrupted (%u bytes)!\n", | |
07dc1f9f HL |
183 | bcs->rcvbytes, skb->len); |
184 | compskb = NULL; | |
185 | gigaset_rcv_error(compskb, cs, bcs); | |
186 | error = 1; | |
187 | } else { | |
188 | if (likely((l = skb->len) > 2)) { | |
189 | skb->tail -= 2; | |
190 | skb->len -= 2; | |
191 | } else { | |
192 | dev_kfree_skb(skb); | |
193 | skb = NULL; | |
194 | inputstate |= INS_skip_frame; | |
195 | if (l == 1) { | |
784d5858 TS |
196 | dev_err(cs->dev, |
197 | "invalid packet size (1)!\n"); | |
07dc1f9f | 198 | error = 1; |
784d5858 TS |
199 | gigaset_rcv_error(NULL, |
200 | cs, bcs); | |
07dc1f9f HL |
201 | } |
202 | } | |
203 | if (likely(!(error || | |
204 | (inputstate & | |
205 | INS_skip_frame)))) { | |
206 | gigaset_rcv_skb(skb, cs, bcs); | |
207 | } | |
208 | } | |
209 | } | |
210 | ||
211 | if (unlikely(error)) | |
212 | if (skb) | |
213 | dev_kfree_skb(skb); | |
214 | ||
215 | fcs = PPP_INITFCS; | |
216 | inputstate &= ~(INS_have_data | INS_skip_frame); | |
217 | if (unlikely(bcs->ignore)) { | |
218 | inputstate |= INS_skip_frame; | |
219 | skb = NULL; | |
220 | } else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)) { | |
221 | skb_reserve(skb, HW_HDR_LEN); | |
222 | } else { | |
784d5858 TS |
223 | dev_warn(cs->dev, |
224 | "could not allocate new skb\n"); | |
07dc1f9f HL |
225 | inputstate |= INS_skip_frame; |
226 | } | |
227 | ||
228 | break; | |
229 | #ifdef CONFIG_GIGASET_DEBUG | |
230 | } else if (unlikely(muststuff(c))) { | |
231 | /* Should not happen. Possible after ZDLE=1<CR><LF>. */ | |
784d5858 | 232 | gig_dbg(DEBUG_HDLC, "not byte stuffed: 0x%02x", c); |
07dc1f9f HL |
233 | #endif |
234 | } | |
235 | ||
236 | /* add character */ | |
237 | ||
238 | #ifdef CONFIG_GIGASET_DEBUG | |
239 | if (unlikely(!(inputstate & INS_have_data))) { | |
784d5858 TS |
240 | gig_dbg(DEBUG_HDLC, "7e (%d x) ================", |
241 | bcs->emptycount); | |
07dc1f9f HL |
242 | bcs->emptycount = 0; |
243 | } | |
244 | #endif | |
245 | ||
246 | inputstate |= INS_have_data; | |
247 | ||
248 | if (likely(!(inputstate & INS_skip_frame))) { | |
249 | if (unlikely(skb->len == SBUFSIZE)) { | |
784d5858 | 250 | dev_warn(cs->dev, "received packet too long\n"); |
07dc1f9f HL |
251 | dev_kfree_skb_any(skb); |
252 | skb = NULL; | |
253 | inputstate |= INS_skip_frame; | |
254 | break; | |
255 | } | |
443e1f45 | 256 | *__skb_put(skb, 1) = c; |
07dc1f9f HL |
257 | fcs = crc_ccitt_byte(fcs, c); |
258 | } | |
259 | ||
260 | if (unlikely(!numbytes)) | |
261 | break; | |
262 | c = *src++; | |
263 | --numbytes; | |
264 | if (unlikely(c == DLE_FLAG && | |
265 | (cs->dle || | |
266 | inbuf->inputstate & INS_DLE_command))) { | |
267 | inbuf->inputstate |= INS_DLE_char; | |
268 | break; | |
269 | } | |
270 | } | |
271 | bcs->inputstate = inputstate; | |
272 | bcs->fcs = fcs; | |
273 | bcs->skb = skb; | |
274 | return startbytes - numbytes; | |
275 | } | |
276 | ||
277 | /* process a block of received bytes in transparent data mode | |
278 | * Invert bytes, undoing byte stuffing and watching for DLE escapes. | |
279 | * If DLE is encountered, return immediately to let the caller handle it. | |
280 | * Return value: | |
281 | * number of processed bytes | |
282 | * numbytes (all bytes processed) on error --FIXME | |
283 | */ | |
284 | static inline int iraw_loop(unsigned char c, unsigned char *src, int numbytes, | |
784d5858 | 285 | struct inbuf_t *inbuf) |
07dc1f9f HL |
286 | { |
287 | struct cardstate *cs = inbuf->cs; | |
288 | struct bc_state *bcs = inbuf->bcs; | |
d48c7784 TS |
289 | int inputstate = bcs->inputstate; |
290 | struct sk_buff *skb = bcs->skb; | |
07dc1f9f HL |
291 | int startbytes = numbytes; |
292 | ||
07dc1f9f HL |
293 | for (;;) { |
294 | /* add character */ | |
295 | inputstate |= INS_have_data; | |
296 | ||
297 | if (likely(!(inputstate & INS_skip_frame))) { | |
298 | if (unlikely(skb->len == SBUFSIZE)) { | |
299 | //FIXME just pass skb up and allocate a new one | |
784d5858 | 300 | dev_warn(cs->dev, "received packet too long\n"); |
07dc1f9f HL |
301 | dev_kfree_skb_any(skb); |
302 | skb = NULL; | |
303 | inputstate |= INS_skip_frame; | |
304 | break; | |
305 | } | |
17b3cff0 | 306 | *__skb_put(skb, 1) = bitrev8(c); |
07dc1f9f HL |
307 | } |
308 | ||
309 | if (unlikely(!numbytes)) | |
310 | break; | |
311 | c = *src++; | |
312 | --numbytes; | |
313 | if (unlikely(c == DLE_FLAG && | |
314 | (cs->dle || | |
315 | inbuf->inputstate & INS_DLE_command))) { | |
316 | inbuf->inputstate |= INS_DLE_char; | |
317 | break; | |
318 | } | |
319 | } | |
320 | ||
321 | /* pass data up */ | |
322 | if (likely(inputstate & INS_have_data)) { | |
323 | if (likely(!(inputstate & INS_skip_frame))) { | |
324 | gigaset_rcv_skb(skb, cs, bcs); | |
325 | } | |
326 | inputstate &= ~(INS_have_data | INS_skip_frame); | |
327 | if (unlikely(bcs->ignore)) { | |
328 | inputstate |= INS_skip_frame; | |
329 | skb = NULL; | |
330 | } else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) | |
331 | != NULL)) { | |
332 | skb_reserve(skb, HW_HDR_LEN); | |
333 | } else { | |
784d5858 | 334 | dev_warn(cs->dev, "could not allocate new skb\n"); |
07dc1f9f HL |
335 | inputstate |= INS_skip_frame; |
336 | } | |
337 | } | |
338 | ||
339 | bcs->inputstate = inputstate; | |
340 | bcs->skb = skb; | |
341 | return startbytes - numbytes; | |
342 | } | |
343 | ||
344 | /* process a block of data received from the device | |
345 | */ | |
346 | void gigaset_m10x_input(struct inbuf_t *inbuf) | |
347 | { | |
348 | struct cardstate *cs; | |
349 | unsigned tail, head, numbytes; | |
350 | unsigned char *src, c; | |
351 | int procbytes; | |
352 | ||
9d4bee2b TS |
353 | head = inbuf->head; |
354 | tail = inbuf->tail; | |
784d5858 | 355 | gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail); |
07dc1f9f HL |
356 | |
357 | if (head != tail) { | |
358 | cs = inbuf->cs; | |
359 | src = inbuf->data + head; | |
360 | numbytes = (head > tail ? RBUFSIZE : tail) - head; | |
784d5858 | 361 | gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes); |
07dc1f9f HL |
362 | |
363 | while (numbytes) { | |
9d4bee2b | 364 | if (cs->mstate == MS_LOCKED) { |
07dc1f9f HL |
365 | procbytes = lock_loop(src, numbytes, inbuf); |
366 | src += procbytes; | |
367 | numbytes -= procbytes; | |
368 | } else { | |
369 | c = *src++; | |
370 | --numbytes; | |
371 | if (c == DLE_FLAG && (cs->dle || | |
372 | inbuf->inputstate & INS_DLE_command)) { | |
373 | if (!(inbuf->inputstate & INS_DLE_char)) { | |
374 | inbuf->inputstate |= INS_DLE_char; | |
375 | goto nextbyte; | |
376 | } | |
377 | /* <DLE> <DLE> => <DLE> in data stream */ | |
378 | inbuf->inputstate &= ~INS_DLE_char; | |
379 | } | |
380 | ||
381 | if (!(inbuf->inputstate & INS_DLE_char)) { | |
382 | ||
917f5085 | 383 | /* FIXME use function pointers? */ |
07dc1f9f HL |
384 | if (inbuf->inputstate & INS_command) |
385 | procbytes = cmd_loop(c, src, numbytes, inbuf); | |
386 | else if (inbuf->bcs->proto2 == ISDN_PROTO_L2_HDLC) | |
387 | procbytes = hdlc_loop(c, src, numbytes, inbuf); | |
388 | else | |
389 | procbytes = iraw_loop(c, src, numbytes, inbuf); | |
390 | ||
391 | src += procbytes; | |
392 | numbytes -= procbytes; | |
784d5858 | 393 | } else { /* DLE char */ |
07dc1f9f HL |
394 | inbuf->inputstate &= ~INS_DLE_char; |
395 | switch (c) { | |
396 | case 'X': /*begin of command*/ | |
397 | #ifdef CONFIG_GIGASET_DEBUG | |
398 | if (inbuf->inputstate & INS_command) | |
784d5858 TS |
399 | dev_err(cs->dev, |
400 | "received <DLE> 'X' in command mode\n"); | |
07dc1f9f HL |
401 | #endif |
402 | inbuf->inputstate |= | |
403 | INS_command | INS_DLE_command; | |
404 | break; | |
405 | case '.': /*end of command*/ | |
406 | #ifdef CONFIG_GIGASET_DEBUG | |
407 | if (!(inbuf->inputstate & INS_command)) | |
784d5858 TS |
408 | dev_err(cs->dev, |
409 | "received <DLE> '.' in hdlc mode\n"); | |
07dc1f9f HL |
410 | #endif |
411 | inbuf->inputstate &= cs->dle ? | |
412 | ~(INS_DLE_command|INS_command) | |
413 | : ~INS_DLE_command; | |
414 | break; | |
415 | //case DLE_FLAG: /*DLE_FLAG in data stream*/ /* schon oben behandelt! */ | |
416 | default: | |
784d5858 TS |
417 | dev_err(cs->dev, |
418 | "received 0x10 0x%02x!\n", | |
419 | (int) c); | |
07dc1f9f HL |
420 | /* FIXME: reset driver?? */ |
421 | } | |
422 | } | |
423 | } | |
424 | nextbyte: | |
425 | if (!numbytes) { | |
426 | /* end of buffer, check for wrap */ | |
427 | if (head > tail) { | |
428 | head = 0; | |
429 | src = inbuf->data; | |
430 | numbytes = tail; | |
431 | } else { | |
432 | head = tail; | |
433 | break; | |
434 | } | |
435 | } | |
436 | } | |
437 | ||
784d5858 | 438 | gig_dbg(DEBUG_INTR, "setting head to %u", head); |
9d4bee2b | 439 | inbuf->head = head; |
07dc1f9f HL |
440 | } |
441 | } | |
d5c1682f | 442 | EXPORT_SYMBOL_GPL(gigaset_m10x_input); |
07dc1f9f HL |
443 | |
444 | ||
445 | /* == data output ========================================================== */ | |
446 | ||
447 | /* Encoding of a PPP packet into an octet stuffed HDLC frame | |
448 | * with FCS, opening and closing flags. | |
449 | * parameters: | |
450 | * skb skb containing original packet (freed upon return) | |
451 | * head number of headroom bytes to allocate in result skb | |
452 | * tail number of tailroom bytes to allocate in result skb | |
453 | * Return value: | |
454 | * pointer to newly allocated skb containing the result frame | |
455 | */ | |
456 | static struct sk_buff *HDLC_Encode(struct sk_buff *skb, int head, int tail) | |
457 | { | |
458 | struct sk_buff *hdlc_skb; | |
459 | __u16 fcs; | |
460 | unsigned char c; | |
461 | unsigned char *cp; | |
462 | int len; | |
463 | unsigned int stuf_cnt; | |
464 | ||
465 | stuf_cnt = 0; | |
466 | fcs = PPP_INITFCS; | |
467 | cp = skb->data; | |
468 | len = skb->len; | |
469 | while (len--) { | |
470 | if (muststuff(*cp)) | |
471 | stuf_cnt++; | |
472 | fcs = crc_ccitt_byte(fcs, *cp++); | |
473 | } | |
784d5858 | 474 | fcs ^= 0xffff; /* complement */ |
07dc1f9f HL |
475 | |
476 | /* size of new buffer: original size + number of stuffing bytes | |
477 | * + 2 bytes FCS + 2 stuffing bytes for FCS (if needed) + 2 flag bytes | |
478 | */ | |
479 | hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + tail + head); | |
480 | if (!hdlc_skb) { | |
07dc1f9f HL |
481 | dev_kfree_skb(skb); |
482 | return NULL; | |
483 | } | |
484 | skb_reserve(hdlc_skb, head); | |
485 | ||
486 | /* Copy acknowledge request into new skb */ | |
487 | memcpy(hdlc_skb->head, skb->head, 2); | |
488 | ||
489 | /* Add flag sequence in front of everything.. */ | |
490 | *(skb_put(hdlc_skb, 1)) = PPP_FLAG; | |
491 | ||
492 | /* Perform byte stuffing while copying data. */ | |
493 | while (skb->len--) { | |
494 | if (muststuff(*skb->data)) { | |
495 | *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE; | |
496 | *(skb_put(hdlc_skb, 1)) = (*skb->data++) ^ PPP_TRANS; | |
497 | } else | |
498 | *(skb_put(hdlc_skb, 1)) = *skb->data++; | |
499 | } | |
500 | ||
501 | /* Finally add FCS (byte stuffed) and flag sequence */ | |
784d5858 | 502 | c = (fcs & 0x00ff); /* least significant byte first */ |
07dc1f9f HL |
503 | if (muststuff(c)) { |
504 | *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE; | |
505 | c ^= PPP_TRANS; | |
506 | } | |
507 | *(skb_put(hdlc_skb, 1)) = c; | |
508 | ||
509 | c = ((fcs >> 8) & 0x00ff); | |
510 | if (muststuff(c)) { | |
511 | *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE; | |
512 | c ^= PPP_TRANS; | |
513 | } | |
514 | *(skb_put(hdlc_skb, 1)) = c; | |
515 | ||
516 | *(skb_put(hdlc_skb, 1)) = PPP_FLAG; | |
517 | ||
518 | dev_kfree_skb(skb); | |
519 | return hdlc_skb; | |
520 | } | |
521 | ||
522 | /* Encoding of a raw packet into an octet stuffed bit inverted frame | |
523 | * parameters: | |
524 | * skb skb containing original packet (freed upon return) | |
525 | * head number of headroom bytes to allocate in result skb | |
526 | * tail number of tailroom bytes to allocate in result skb | |
527 | * Return value: | |
528 | * pointer to newly allocated skb containing the result frame | |
529 | */ | |
530 | static struct sk_buff *iraw_encode(struct sk_buff *skb, int head, int tail) | |
531 | { | |
532 | struct sk_buff *iraw_skb; | |
533 | unsigned char c; | |
534 | unsigned char *cp; | |
535 | int len; | |
536 | ||
537 | /* worst case: every byte must be stuffed */ | |
538 | iraw_skb = dev_alloc_skb(2*skb->len + tail + head); | |
539 | if (!iraw_skb) { | |
07dc1f9f HL |
540 | dev_kfree_skb(skb); |
541 | return NULL; | |
542 | } | |
543 | skb_reserve(iraw_skb, head); | |
544 | ||
545 | cp = skb->data; | |
546 | len = skb->len; | |
547 | while (len--) { | |
17b3cff0 | 548 | c = bitrev8(*cp++); |
07dc1f9f HL |
549 | if (c == DLE_FLAG) |
550 | *(skb_put(iraw_skb, 1)) = c; | |
551 | *(skb_put(iraw_skb, 1)) = c; | |
552 | } | |
553 | dev_kfree_skb(skb); | |
554 | return iraw_skb; | |
555 | } | |
556 | ||
557 | /* gigaset_send_skb | |
558 | * called by common.c to queue an skb for sending | |
559 | * and start transmission if necessary | |
560 | * parameters: | |
561 | * B Channel control structure | |
562 | * skb | |
563 | * Return value: | |
564 | * number of bytes accepted for sending | |
565 | * (skb->len if ok, 0 if out of buffer space) | |
566 | * or error code (< 0, eg. -EINVAL) | |
567 | */ | |
568 | int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb) | |
569 | { | |
d48c7784 | 570 | unsigned len = skb->len; |
69049cc8 | 571 | unsigned long flags; |
07dc1f9f HL |
572 | |
573 | if (bcs->proto2 == ISDN_PROTO_L2_HDLC) | |
574 | skb = HDLC_Encode(skb, HW_HDR_LEN, 0); | |
575 | else | |
576 | skb = iraw_encode(skb, HW_HDR_LEN, 0); | |
784d5858 | 577 | if (!skb) { |
69049cc8 | 578 | err("unable to allocate memory for encoding!\n"); |
07dc1f9f | 579 | return -ENOMEM; |
784d5858 | 580 | } |
07dc1f9f HL |
581 | |
582 | skb_queue_tail(&bcs->squeue, skb); | |
69049cc8 TS |
583 | spin_lock_irqsave(&bcs->cs->lock, flags); |
584 | if (bcs->cs->connected) | |
585 | tasklet_schedule(&bcs->cs->write_tasklet); | |
586 | spin_unlock_irqrestore(&bcs->cs->lock, flags); | |
07dc1f9f HL |
587 | |
588 | return len; /* ok so far */ | |
589 | } | |
d5c1682f | 590 | EXPORT_SYMBOL_GPL(gigaset_m10x_send_skb); |