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>, | |
6 | * Stefan Eilers <Eilers.Stefan@epost.de>. | |
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> | |
18 | ||
19 | //#define GIG_M10x_STUFF_VOICE_DATA | |
20 | ||
21 | /* check if byte must be stuffed/escaped | |
22 | * I'm not sure which data should be encoded. | |
23 | * Therefore I will go the hard way and decode every value | |
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 | ||
39 | /* process a block of received bytes in command mode (modem response) | |
40 | * Return value: | |
41 | * number of processed bytes | |
42 | */ | |
43 | static inline int cmd_loop(unsigned char c, unsigned char *src, int numbytes, | |
784d5858 | 44 | struct inbuf_t *inbuf) |
07dc1f9f HL |
45 | { |
46 | struct cardstate *cs = inbuf->cs; | |
47 | unsigned cbytes = cs->cbytes; | |
48 | int inputstate = inbuf->inputstate; | |
49 | int startbytes = numbytes; | |
50 | ||
51 | for (;;) { | |
52 | cs->respdata[cbytes] = c; | |
53 | if (c == 10 || c == 13) { | |
784d5858 TS |
54 | gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)", |
55 | __func__, cbytes); | |
07dc1f9f | 56 | cs->cbytes = cbytes; |
917f5085 TS |
57 | gigaset_handle_modem_response(cs); /* can change |
58 | cs->dle */ | |
07dc1f9f HL |
59 | cbytes = 0; |
60 | ||
61 | if (cs->dle && | |
62 | !(inputstate & INS_DLE_command)) { | |
63 | inputstate &= ~INS_command; | |
64 | break; | |
65 | } | |
66 | } else { | |
67 | /* advance in line buffer, checking for overflow */ | |
68 | if (cbytes < MAX_RESP_SIZE - 1) | |
69 | cbytes++; | |
70 | else | |
784d5858 | 71 | dev_warn(cs->dev, "response too large\n"); |
07dc1f9f HL |
72 | } |
73 | ||
74 | if (!numbytes) | |
75 | break; | |
76 | c = *src++; | |
77 | --numbytes; | |
78 | if (c == DLE_FLAG && | |
79 | (cs->dle || inputstate & INS_DLE_command)) { | |
80 | inputstate |= INS_DLE_char; | |
81 | break; | |
82 | } | |
83 | } | |
84 | ||
85 | cs->cbytes = cbytes; | |
86 | inbuf->inputstate = inputstate; | |
87 | ||
88 | return startbytes - numbytes; | |
89 | } | |
90 | ||
91 | /* process a block of received bytes in lock mode (tty i/f) | |
92 | * Return value: | |
93 | * number of processed bytes | |
94 | */ | |
95 | static inline int lock_loop(unsigned char *src, int numbytes, | |
784d5858 | 96 | struct inbuf_t *inbuf) |
07dc1f9f HL |
97 | { |
98 | struct cardstate *cs = inbuf->cs; | |
99 | ||
917f5085 | 100 | gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response", |
01371500 | 101 | numbytes, src); |
07dc1f9f HL |
102 | gigaset_if_receive(cs, src, numbytes); |
103 | ||
104 | return numbytes; | |
105 | } | |
106 | ||
107 | /* process a block of received bytes in HDLC data mode | |
108 | * Collect HDLC frames, undoing byte stuffing and watching for DLE escapes. | |
109 | * When a frame is complete, check the FCS and pass valid frames to the LL. | |
110 | * If DLE is encountered, return immediately to let the caller handle it. | |
111 | * Return value: | |
112 | * number of processed bytes | |
113 | * numbytes (all bytes processed) on error --FIXME | |
114 | */ | |
115 | static inline int hdlc_loop(unsigned char c, unsigned char *src, int numbytes, | |
784d5858 | 116 | struct inbuf_t *inbuf) |
07dc1f9f HL |
117 | { |
118 | struct cardstate *cs = inbuf->cs; | |
119 | struct bc_state *bcs = inbuf->bcs; | |
d48c7784 TS |
120 | int inputstate = bcs->inputstate; |
121 | __u16 fcs = bcs->fcs; | |
122 | struct sk_buff *skb = bcs->skb; | |
07dc1f9f HL |
123 | unsigned char error; |
124 | struct sk_buff *compskb; | |
125 | int startbytes = numbytes; | |
126 | int l; | |
127 | ||
07dc1f9f HL |
128 | if (unlikely(inputstate & INS_byte_stuff)) { |
129 | inputstate &= ~INS_byte_stuff; | |
130 | goto byte_stuff; | |
131 | } | |
132 | for (;;) { | |
133 | if (unlikely(c == PPP_ESCAPE)) { | |
134 | if (unlikely(!numbytes)) { | |
135 | inputstate |= INS_byte_stuff; | |
136 | break; | |
137 | } | |
138 | c = *src++; | |
139 | --numbytes; | |
140 | if (unlikely(c == DLE_FLAG && | |
141 | (cs->dle || | |
142 | inbuf->inputstate & INS_DLE_command))) { | |
143 | inbuf->inputstate |= INS_DLE_char; | |
144 | inputstate |= INS_byte_stuff; | |
145 | break; | |
146 | } | |
147 | byte_stuff: | |
148 | c ^= PPP_TRANS; | |
149 | #ifdef CONFIG_GIGASET_DEBUG | |
150 | if (unlikely(!muststuff(c))) | |
784d5858 | 151 | gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c); |
07dc1f9f HL |
152 | #endif |
153 | } else if (unlikely(c == PPP_FLAG)) { | |
154 | if (unlikely(inputstate & INS_skip_frame)) { | |
155 | if (!(inputstate & INS_have_data)) { /* 7E 7E */ | |
07dc1f9f HL |
156 | #ifdef CONFIG_GIGASET_DEBUG |
157 | ++bcs->emptycount; | |
158 | #endif | |
159 | } else | |
784d5858 | 160 | gig_dbg(DEBUG_HDLC, |
07dc1f9f HL |
161 | "7e----------------------------"); |
162 | ||
163 | /* end of frame */ | |
164 | error = 1; | |
165 | gigaset_rcv_error(NULL, cs, bcs); | |
166 | } else if (!(inputstate & INS_have_data)) { /* 7E 7E */ | |
07dc1f9f HL |
167 | #ifdef CONFIG_GIGASET_DEBUG |
168 | ++bcs->emptycount; | |
169 | #endif | |
170 | break; | |
171 | } else { | |
784d5858 TS |
172 | gig_dbg(DEBUG_HDLC, |
173 | "7e----------------------------"); | |
07dc1f9f HL |
174 | |
175 | /* end of frame */ | |
176 | error = 0; | |
177 | ||
178 | if (unlikely(fcs != PPP_GOODFCS)) { | |
784d5858 TS |
179 | dev_err(cs->dev, |
180 | "Packet checksum at %lu failed, " | |
181 | "packet is corrupted (%u bytes)!\n", | |
07dc1f9f HL |
182 | bcs->rcvbytes, skb->len); |
183 | compskb = NULL; | |
184 | gigaset_rcv_error(compskb, cs, bcs); | |
185 | error = 1; | |
186 | } else { | |
187 | if (likely((l = skb->len) > 2)) { | |
188 | skb->tail -= 2; | |
189 | skb->len -= 2; | |
190 | } else { | |
191 | dev_kfree_skb(skb); | |
192 | skb = NULL; | |
193 | inputstate |= INS_skip_frame; | |
194 | if (l == 1) { | |
784d5858 TS |
195 | dev_err(cs->dev, |
196 | "invalid packet size (1)!\n"); | |
07dc1f9f | 197 | error = 1; |
784d5858 TS |
198 | gigaset_rcv_error(NULL, |
199 | cs, bcs); | |
07dc1f9f HL |
200 | } |
201 | } | |
202 | if (likely(!(error || | |
203 | (inputstate & | |
204 | INS_skip_frame)))) { | |
205 | gigaset_rcv_skb(skb, cs, bcs); | |
206 | } | |
207 | } | |
208 | } | |
209 | ||
210 | if (unlikely(error)) | |
211 | if (skb) | |
212 | dev_kfree_skb(skb); | |
213 | ||
214 | fcs = PPP_INITFCS; | |
215 | inputstate &= ~(INS_have_data | INS_skip_frame); | |
216 | if (unlikely(bcs->ignore)) { | |
217 | inputstate |= INS_skip_frame; | |
218 | skb = NULL; | |
219 | } else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)) { | |
220 | skb_reserve(skb, HW_HDR_LEN); | |
221 | } else { | |
784d5858 TS |
222 | dev_warn(cs->dev, |
223 | "could not allocate new skb\n"); | |
07dc1f9f HL |
224 | inputstate |= INS_skip_frame; |
225 | } | |
226 | ||
227 | break; | |
228 | #ifdef CONFIG_GIGASET_DEBUG | |
229 | } else if (unlikely(muststuff(c))) { | |
230 | /* Should not happen. Possible after ZDLE=1<CR><LF>. */ | |
784d5858 | 231 | gig_dbg(DEBUG_HDLC, "not byte stuffed: 0x%02x", c); |
07dc1f9f HL |
232 | #endif |
233 | } | |
234 | ||
235 | /* add character */ | |
236 | ||
237 | #ifdef CONFIG_GIGASET_DEBUG | |
238 | if (unlikely(!(inputstate & INS_have_data))) { | |
784d5858 TS |
239 | gig_dbg(DEBUG_HDLC, "7e (%d x) ================", |
240 | bcs->emptycount); | |
07dc1f9f HL |
241 | bcs->emptycount = 0; |
242 | } | |
243 | #endif | |
244 | ||
245 | inputstate |= INS_have_data; | |
246 | ||
247 | if (likely(!(inputstate & INS_skip_frame))) { | |
248 | if (unlikely(skb->len == SBUFSIZE)) { | |
784d5858 | 249 | dev_warn(cs->dev, "received packet too long\n"); |
07dc1f9f HL |
250 | dev_kfree_skb_any(skb); |
251 | skb = NULL; | |
252 | inputstate |= INS_skip_frame; | |
253 | break; | |
254 | } | |
255 | *gigaset_skb_put_quick(skb, 1) = c; | |
256 | /* *__skb_put (skb, 1) = c; */ | |
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 | } | |
306 | *gigaset_skb_put_quick(skb, 1) = gigaset_invtab[c]; | |
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 | ||
353 | head = atomic_read(&inbuf->head); | |
354 | tail = atomic_read(&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) { | |
364 | if (atomic_read(&cs->mstate) == MS_LOCKED) { | |
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); |
07dc1f9f HL |
439 | atomic_set(&inbuf->head, head); |
440 | } | |
441 | } | |
442 | ||
443 | ||
444 | /* == data output ========================================================== */ | |
445 | ||
446 | /* Encoding of a PPP packet into an octet stuffed HDLC frame | |
447 | * with FCS, opening and closing flags. | |
448 | * parameters: | |
449 | * skb skb containing original packet (freed upon return) | |
450 | * head number of headroom bytes to allocate in result skb | |
451 | * tail number of tailroom bytes to allocate in result skb | |
452 | * Return value: | |
453 | * pointer to newly allocated skb containing the result frame | |
454 | */ | |
455 | static struct sk_buff *HDLC_Encode(struct sk_buff *skb, int head, int tail) | |
456 | { | |
457 | struct sk_buff *hdlc_skb; | |
458 | __u16 fcs; | |
459 | unsigned char c; | |
460 | unsigned char *cp; | |
461 | int len; | |
462 | unsigned int stuf_cnt; | |
463 | ||
464 | stuf_cnt = 0; | |
465 | fcs = PPP_INITFCS; | |
466 | cp = skb->data; | |
467 | len = skb->len; | |
468 | while (len--) { | |
469 | if (muststuff(*cp)) | |
470 | stuf_cnt++; | |
471 | fcs = crc_ccitt_byte(fcs, *cp++); | |
472 | } | |
784d5858 | 473 | fcs ^= 0xffff; /* complement */ |
07dc1f9f HL |
474 | |
475 | /* size of new buffer: original size + number of stuffing bytes | |
476 | * + 2 bytes FCS + 2 stuffing bytes for FCS (if needed) + 2 flag bytes | |
477 | */ | |
478 | hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + tail + head); | |
479 | if (!hdlc_skb) { | |
07dc1f9f HL |
480 | dev_kfree_skb(skb); |
481 | return NULL; | |
482 | } | |
483 | skb_reserve(hdlc_skb, head); | |
484 | ||
485 | /* Copy acknowledge request into new skb */ | |
486 | memcpy(hdlc_skb->head, skb->head, 2); | |
487 | ||
488 | /* Add flag sequence in front of everything.. */ | |
489 | *(skb_put(hdlc_skb, 1)) = PPP_FLAG; | |
490 | ||
491 | /* Perform byte stuffing while copying data. */ | |
492 | while (skb->len--) { | |
493 | if (muststuff(*skb->data)) { | |
494 | *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE; | |
495 | *(skb_put(hdlc_skb, 1)) = (*skb->data++) ^ PPP_TRANS; | |
496 | } else | |
497 | *(skb_put(hdlc_skb, 1)) = *skb->data++; | |
498 | } | |
499 | ||
500 | /* Finally add FCS (byte stuffed) and flag sequence */ | |
784d5858 | 501 | c = (fcs & 0x00ff); /* least significant byte first */ |
07dc1f9f HL |
502 | if (muststuff(c)) { |
503 | *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE; | |
504 | c ^= PPP_TRANS; | |
505 | } | |
506 | *(skb_put(hdlc_skb, 1)) = c; | |
507 | ||
508 | c = ((fcs >> 8) & 0x00ff); | |
509 | if (muststuff(c)) { | |
510 | *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE; | |
511 | c ^= PPP_TRANS; | |
512 | } | |
513 | *(skb_put(hdlc_skb, 1)) = c; | |
514 | ||
515 | *(skb_put(hdlc_skb, 1)) = PPP_FLAG; | |
516 | ||
517 | dev_kfree_skb(skb); | |
518 | return hdlc_skb; | |
519 | } | |
520 | ||
521 | /* Encoding of a raw packet into an octet stuffed bit inverted frame | |
522 | * parameters: | |
523 | * skb skb containing original packet (freed upon return) | |
524 | * head number of headroom bytes to allocate in result skb | |
525 | * tail number of tailroom bytes to allocate in result skb | |
526 | * Return value: | |
527 | * pointer to newly allocated skb containing the result frame | |
528 | */ | |
529 | static struct sk_buff *iraw_encode(struct sk_buff *skb, int head, int tail) | |
530 | { | |
531 | struct sk_buff *iraw_skb; | |
532 | unsigned char c; | |
533 | unsigned char *cp; | |
534 | int len; | |
535 | ||
536 | /* worst case: every byte must be stuffed */ | |
537 | iraw_skb = dev_alloc_skb(2*skb->len + tail + head); | |
538 | if (!iraw_skb) { | |
07dc1f9f HL |
539 | dev_kfree_skb(skb); |
540 | return NULL; | |
541 | } | |
542 | skb_reserve(iraw_skb, head); | |
543 | ||
544 | cp = skb->data; | |
545 | len = skb->len; | |
546 | while (len--) { | |
547 | c = gigaset_invtab[*cp++]; | |
548 | if (c == DLE_FLAG) | |
549 | *(skb_put(iraw_skb, 1)) = c; | |
550 | *(skb_put(iraw_skb, 1)) = c; | |
551 | } | |
552 | dev_kfree_skb(skb); | |
553 | return iraw_skb; | |
554 | } | |
555 | ||
556 | /* gigaset_send_skb | |
557 | * called by common.c to queue an skb for sending | |
558 | * and start transmission if necessary | |
559 | * parameters: | |
560 | * B Channel control structure | |
561 | * skb | |
562 | * Return value: | |
563 | * number of bytes accepted for sending | |
564 | * (skb->len if ok, 0 if out of buffer space) | |
565 | * or error code (< 0, eg. -EINVAL) | |
566 | */ | |
567 | int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb) | |
568 | { | |
d48c7784 | 569 | unsigned len = skb->len; |
07dc1f9f HL |
570 | |
571 | if (bcs->proto2 == ISDN_PROTO_L2_HDLC) | |
572 | skb = HDLC_Encode(skb, HW_HDR_LEN, 0); | |
573 | else | |
574 | skb = iraw_encode(skb, HW_HDR_LEN, 0); | |
784d5858 TS |
575 | if (!skb) { |
576 | dev_err(bcs->cs->dev, | |
577 | "unable to allocate memory for encoding!\n"); | |
07dc1f9f | 578 | return -ENOMEM; |
784d5858 | 579 | } |
07dc1f9f HL |
580 | |
581 | skb_queue_tail(&bcs->squeue, skb); | |
582 | tasklet_schedule(&bcs->cs->write_tasklet); | |
583 | ||
584 | return len; /* ok so far */ | |
585 | } |