blob: 2a4ce96f04bd7ecfea5ce9111b231e076bc80a28 [file] [log] [blame]
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -08001/*
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>,
Tilman Schmidt70440cf2006-04-10 22:55:14 -07006 * Stefan Eilers.
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -08007 *
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 * =====================================================================
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -080014 */
15
16#include "gigaset.h"
17#include <linux/crc-ccitt.h>
Akinobu Mita17b3cff2006-12-08 02:36:30 -080018#include <linux/bitrev.h>
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -080019
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -080020/* check if byte must be stuffed/escaped
21 * I'm not sure which data should be encoded.
22 * Therefore I will go the hard way and decode every value
23 * less than 0x20, the flag sequence and the control escape char.
24 */
25static 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
38/* process a block of received bytes in command mode (modem response)
39 * Return value:
40 * number of processed bytes
41 */
42static inline int cmd_loop(unsigned char c, unsigned char *src, int numbytes,
Tilman Schmidt784d5852006-04-10 22:55:04 -070043 struct inbuf_t *inbuf)
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -080044{
45 struct cardstate *cs = inbuf->cs;
46 unsigned cbytes = cs->cbytes;
47 int inputstate = inbuf->inputstate;
48 int startbytes = numbytes;
49
50 for (;;) {
51 cs->respdata[cbytes] = c;
52 if (c == 10 || c == 13) {
Tilman Schmidt784d5852006-04-10 22:55:04 -070053 gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)",
54 __func__, cbytes);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -080055 cs->cbytes = cbytes;
Tilman Schmidt917f5082006-04-10 22:55:00 -070056 gigaset_handle_modem_response(cs); /* can change
57 cs->dle */
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -080058 cbytes = 0;
59
60 if (cs->dle &&
61 !(inputstate & INS_DLE_command)) {
62 inputstate &= ~INS_command;
63 break;
64 }
65 } else {
66 /* advance in line buffer, checking for overflow */
67 if (cbytes < MAX_RESP_SIZE - 1)
68 cbytes++;
69 else
Tilman Schmidt784d5852006-04-10 22:55:04 -070070 dev_warn(cs->dev, "response too large\n");
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -080071 }
72
73 if (!numbytes)
74 break;
75 c = *src++;
76 --numbytes;
77 if (c == DLE_FLAG &&
78 (cs->dle || inputstate & INS_DLE_command)) {
79 inputstate |= INS_DLE_char;
80 break;
81 }
82 }
83
84 cs->cbytes = cbytes;
85 inbuf->inputstate = inputstate;
86
87 return startbytes - numbytes;
88}
89
90/* process a block of received bytes in lock mode (tty i/f)
91 * Return value:
92 * number of processed bytes
93 */
94static inline int lock_loop(unsigned char *src, int numbytes,
Tilman Schmidt784d5852006-04-10 22:55:04 -070095 struct inbuf_t *inbuf)
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -080096{
97 struct cardstate *cs = inbuf->cs;
98
Tilman Schmidt917f5082006-04-10 22:55:00 -070099 gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
Tilman Schmidt01371502006-04-10 22:55:11 -0700100 numbytes, src);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800101 gigaset_if_receive(cs, src, numbytes);
102
103 return numbytes;
104}
105
106/* process a block of received bytes in HDLC data mode
107 * Collect HDLC frames, undoing byte stuffing and watching for DLE escapes.
108 * When a frame is complete, check the FCS and pass valid frames to the LL.
109 * If DLE is encountered, return immediately to let the caller handle it.
110 * Return value:
111 * number of processed bytes
112 * numbytes (all bytes processed) on error --FIXME
113 */
114static inline int hdlc_loop(unsigned char c, unsigned char *src, int numbytes,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700115 struct inbuf_t *inbuf)
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800116{
117 struct cardstate *cs = inbuf->cs;
118 struct bc_state *bcs = inbuf->bcs;
Tilman Schmidtd48c7782006-04-10 22:55:08 -0700119 int inputstate = bcs->inputstate;
120 __u16 fcs = bcs->fcs;
121 struct sk_buff *skb = bcs->skb;
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800122 unsigned char error;
123 struct sk_buff *compskb;
124 int startbytes = numbytes;
125 int l;
126
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800127 if (unlikely(inputstate & INS_byte_stuff)) {
128 inputstate &= ~INS_byte_stuff;
129 goto byte_stuff;
130 }
131 for (;;) {
132 if (unlikely(c == PPP_ESCAPE)) {
133 if (unlikely(!numbytes)) {
134 inputstate |= INS_byte_stuff;
135 break;
136 }
137 c = *src++;
138 --numbytes;
139 if (unlikely(c == DLE_FLAG &&
140 (cs->dle ||
141 inbuf->inputstate & INS_DLE_command))) {
142 inbuf->inputstate |= INS_DLE_char;
143 inputstate |= INS_byte_stuff;
144 break;
145 }
146byte_stuff:
147 c ^= PPP_TRANS;
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800148 if (unlikely(!muststuff(c)))
Tilman Schmidt784d5852006-04-10 22:55:04 -0700149 gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800150 } else if (unlikely(c == PPP_FLAG)) {
151 if (unlikely(inputstate & INS_skip_frame)) {
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800152#ifdef CONFIG_GIGASET_DEBUG
Tilman Schmidt236b87c2008-12-26 01:22:03 -0800153 if (!(inputstate & INS_have_data)) { /* 7E 7E */
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800154 ++bcs->emptycount;
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800155 } else
Tilman Schmidt784d5852006-04-10 22:55:04 -0700156 gig_dbg(DEBUG_HDLC,
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800157 "7e----------------------------");
Tilman Schmidt236b87c2008-12-26 01:22:03 -0800158#endif
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800159
160 /* end of frame */
161 error = 1;
162 gigaset_rcv_error(NULL, cs, bcs);
163 } else if (!(inputstate & INS_have_data)) { /* 7E 7E */
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800164#ifdef CONFIG_GIGASET_DEBUG
165 ++bcs->emptycount;
166#endif
167 break;
168 } else {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700169 gig_dbg(DEBUG_HDLC,
170 "7e----------------------------");
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800171
172 /* end of frame */
173 error = 0;
174
175 if (unlikely(fcs != PPP_GOODFCS)) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700176 dev_err(cs->dev,
177 "Packet checksum at %lu failed, "
178 "packet is corrupted (%u bytes)!\n",
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800179 bcs->rcvbytes, skb->len);
180 compskb = NULL;
181 gigaset_rcv_error(compskb, cs, bcs);
182 error = 1;
183 } else {
184 if (likely((l = skb->len) > 2)) {
185 skb->tail -= 2;
186 skb->len -= 2;
187 } else {
188 dev_kfree_skb(skb);
189 skb = NULL;
190 inputstate |= INS_skip_frame;
191 if (l == 1) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700192 dev_err(cs->dev,
193 "invalid packet size (1)!\n");
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800194 error = 1;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700195 gigaset_rcv_error(NULL,
196 cs, bcs);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800197 }
198 }
199 if (likely(!(error ||
200 (inputstate &
201 INS_skip_frame)))) {
202 gigaset_rcv_skb(skb, cs, bcs);
203 }
204 }
205 }
206
207 if (unlikely(error))
208 if (skb)
209 dev_kfree_skb(skb);
210
211 fcs = PPP_INITFCS;
212 inputstate &= ~(INS_have_data | INS_skip_frame);
213 if (unlikely(bcs->ignore)) {
214 inputstate |= INS_skip_frame;
215 skb = NULL;
216 } else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)) {
217 skb_reserve(skb, HW_HDR_LEN);
218 } else {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700219 dev_warn(cs->dev,
220 "could not allocate new skb\n");
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800221 inputstate |= INS_skip_frame;
222 }
223
224 break;
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800225 } else if (unlikely(muststuff(c))) {
226 /* Should not happen. Possible after ZDLE=1<CR><LF>. */
Tilman Schmidt784d5852006-04-10 22:55:04 -0700227 gig_dbg(DEBUG_HDLC, "not byte stuffed: 0x%02x", c);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800228 }
229
230 /* add character */
231
232#ifdef CONFIG_GIGASET_DEBUG
233 if (unlikely(!(inputstate & INS_have_data))) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700234 gig_dbg(DEBUG_HDLC, "7e (%d x) ================",
235 bcs->emptycount);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800236 bcs->emptycount = 0;
237 }
238#endif
239
240 inputstate |= INS_have_data;
241
242 if (likely(!(inputstate & INS_skip_frame))) {
243 if (unlikely(skb->len == SBUFSIZE)) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700244 dev_warn(cs->dev, "received packet too long\n");
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800245 dev_kfree_skb_any(skb);
246 skb = NULL;
247 inputstate |= INS_skip_frame;
248 break;
249 }
Tilman Schmidt443e1f42006-04-10 22:55:13 -0700250 *__skb_put(skb, 1) = c;
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800251 fcs = crc_ccitt_byte(fcs, c);
252 }
253
254 if (unlikely(!numbytes))
255 break;
256 c = *src++;
257 --numbytes;
258 if (unlikely(c == DLE_FLAG &&
259 (cs->dle ||
260 inbuf->inputstate & INS_DLE_command))) {
261 inbuf->inputstate |= INS_DLE_char;
262 break;
263 }
264 }
265 bcs->inputstate = inputstate;
266 bcs->fcs = fcs;
267 bcs->skb = skb;
268 return startbytes - numbytes;
269}
270
271/* process a block of received bytes in transparent data mode
272 * Invert bytes, undoing byte stuffing and watching for DLE escapes.
273 * If DLE is encountered, return immediately to let the caller handle it.
274 * Return value:
275 * number of processed bytes
276 * numbytes (all bytes processed) on error --FIXME
277 */
278static inline int iraw_loop(unsigned char c, unsigned char *src, int numbytes,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700279 struct inbuf_t *inbuf)
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800280{
281 struct cardstate *cs = inbuf->cs;
282 struct bc_state *bcs = inbuf->bcs;
Tilman Schmidtd48c7782006-04-10 22:55:08 -0700283 int inputstate = bcs->inputstate;
284 struct sk_buff *skb = bcs->skb;
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800285 int startbytes = numbytes;
286
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800287 for (;;) {
288 /* add character */
289 inputstate |= INS_have_data;
290
291 if (likely(!(inputstate & INS_skip_frame))) {
292 if (unlikely(skb->len == SBUFSIZE)) {
293 //FIXME just pass skb up and allocate a new one
Tilman Schmidt784d5852006-04-10 22:55:04 -0700294 dev_warn(cs->dev, "received packet too long\n");
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800295 dev_kfree_skb_any(skb);
296 skb = NULL;
297 inputstate |= INS_skip_frame;
298 break;
299 }
Akinobu Mita17b3cff2006-12-08 02:36:30 -0800300 *__skb_put(skb, 1) = bitrev8(c);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800301 }
302
303 if (unlikely(!numbytes))
304 break;
305 c = *src++;
306 --numbytes;
307 if (unlikely(c == DLE_FLAG &&
308 (cs->dle ||
309 inbuf->inputstate & INS_DLE_command))) {
310 inbuf->inputstate |= INS_DLE_char;
311 break;
312 }
313 }
314
315 /* pass data up */
316 if (likely(inputstate & INS_have_data)) {
317 if (likely(!(inputstate & INS_skip_frame))) {
318 gigaset_rcv_skb(skb, cs, bcs);
319 }
320 inputstate &= ~(INS_have_data | INS_skip_frame);
321 if (unlikely(bcs->ignore)) {
322 inputstate |= INS_skip_frame;
323 skb = NULL;
324 } else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN))
325 != NULL)) {
326 skb_reserve(skb, HW_HDR_LEN);
327 } else {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700328 dev_warn(cs->dev, "could not allocate new skb\n");
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800329 inputstate |= INS_skip_frame;
330 }
331 }
332
333 bcs->inputstate = inputstate;
334 bcs->skb = skb;
335 return startbytes - numbytes;
336}
337
338/* process a block of data received from the device
339 */
340void gigaset_m10x_input(struct inbuf_t *inbuf)
341{
342 struct cardstate *cs;
343 unsigned tail, head, numbytes;
344 unsigned char *src, c;
345 int procbytes;
346
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800347 head = inbuf->head;
348 tail = inbuf->tail;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700349 gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800350
351 if (head != tail) {
352 cs = inbuf->cs;
353 src = inbuf->data + head;
354 numbytes = (head > tail ? RBUFSIZE : tail) - head;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700355 gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800356
357 while (numbytes) {
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800358 if (cs->mstate == MS_LOCKED) {
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800359 procbytes = lock_loop(src, numbytes, inbuf);
360 src += procbytes;
361 numbytes -= procbytes;
362 } else {
363 c = *src++;
364 --numbytes;
365 if (c == DLE_FLAG && (cs->dle ||
366 inbuf->inputstate & INS_DLE_command)) {
367 if (!(inbuf->inputstate & INS_DLE_char)) {
368 inbuf->inputstate |= INS_DLE_char;
369 goto nextbyte;
370 }
371 /* <DLE> <DLE> => <DLE> in data stream */
372 inbuf->inputstate &= ~INS_DLE_char;
373 }
374
375 if (!(inbuf->inputstate & INS_DLE_char)) {
376
Tilman Schmidt917f5082006-04-10 22:55:00 -0700377 /* FIXME use function pointers? */
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800378 if (inbuf->inputstate & INS_command)
379 procbytes = cmd_loop(c, src, numbytes, inbuf);
380 else if (inbuf->bcs->proto2 == ISDN_PROTO_L2_HDLC)
381 procbytes = hdlc_loop(c, src, numbytes, inbuf);
382 else
383 procbytes = iraw_loop(c, src, numbytes, inbuf);
384
385 src += procbytes;
386 numbytes -= procbytes;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700387 } else { /* DLE char */
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800388 inbuf->inputstate &= ~INS_DLE_char;
389 switch (c) {
390 case 'X': /*begin of command*/
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800391 if (inbuf->inputstate & INS_command)
Tilman Schmidt236b87c2008-12-26 01:22:03 -0800392 dev_warn(cs->dev,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700393 "received <DLE> 'X' in command mode\n");
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800394 inbuf->inputstate |=
395 INS_command | INS_DLE_command;
396 break;
397 case '.': /*end of command*/
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800398 if (!(inbuf->inputstate & INS_command))
Tilman Schmidt236b87c2008-12-26 01:22:03 -0800399 dev_warn(cs->dev,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700400 "received <DLE> '.' in hdlc mode\n");
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800401 inbuf->inputstate &= cs->dle ?
402 ~(INS_DLE_command|INS_command)
403 : ~INS_DLE_command;
404 break;
405 //case DLE_FLAG: /*DLE_FLAG in data stream*/ /* schon oben behandelt! */
406 default:
Tilman Schmidt784d5852006-04-10 22:55:04 -0700407 dev_err(cs->dev,
408 "received 0x10 0x%02x!\n",
409 (int) c);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800410 /* FIXME: reset driver?? */
411 }
412 }
413 }
414nextbyte:
415 if (!numbytes) {
416 /* end of buffer, check for wrap */
417 if (head > tail) {
418 head = 0;
419 src = inbuf->data;
420 numbytes = tail;
421 } else {
422 head = tail;
423 break;
424 }
425 }
426 }
427
Tilman Schmidt784d5852006-04-10 22:55:04 -0700428 gig_dbg(DEBUG_INTR, "setting head to %u", head);
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800429 inbuf->head = head;
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800430 }
431}
Adrian Bunkd5c16822007-02-20 13:58:17 -0800432EXPORT_SYMBOL_GPL(gigaset_m10x_input);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800433
434
435/* == data output ========================================================== */
436
437/* Encoding of a PPP packet into an octet stuffed HDLC frame
438 * with FCS, opening and closing flags.
439 * parameters:
440 * skb skb containing original packet (freed upon return)
441 * head number of headroom bytes to allocate in result skb
442 * tail number of tailroom bytes to allocate in result skb
443 * Return value:
444 * pointer to newly allocated skb containing the result frame
445 */
446static struct sk_buff *HDLC_Encode(struct sk_buff *skb, int head, int tail)
447{
448 struct sk_buff *hdlc_skb;
449 __u16 fcs;
450 unsigned char c;
451 unsigned char *cp;
452 int len;
453 unsigned int stuf_cnt;
454
455 stuf_cnt = 0;
456 fcs = PPP_INITFCS;
457 cp = skb->data;
458 len = skb->len;
459 while (len--) {
460 if (muststuff(*cp))
461 stuf_cnt++;
462 fcs = crc_ccitt_byte(fcs, *cp++);
463 }
Tilman Schmidt784d5852006-04-10 22:55:04 -0700464 fcs ^= 0xffff; /* complement */
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800465
466 /* size of new buffer: original size + number of stuffing bytes
467 * + 2 bytes FCS + 2 stuffing bytes for FCS (if needed) + 2 flag bytes
468 */
469 hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + tail + head);
470 if (!hdlc_skb) {
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800471 dev_kfree_skb(skb);
472 return NULL;
473 }
474 skb_reserve(hdlc_skb, head);
475
476 /* Copy acknowledge request into new skb */
477 memcpy(hdlc_skb->head, skb->head, 2);
478
479 /* Add flag sequence in front of everything.. */
480 *(skb_put(hdlc_skb, 1)) = PPP_FLAG;
481
482 /* Perform byte stuffing while copying data. */
483 while (skb->len--) {
484 if (muststuff(*skb->data)) {
485 *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
486 *(skb_put(hdlc_skb, 1)) = (*skb->data++) ^ PPP_TRANS;
487 } else
488 *(skb_put(hdlc_skb, 1)) = *skb->data++;
489 }
490
491 /* Finally add FCS (byte stuffed) and flag sequence */
Tilman Schmidt784d5852006-04-10 22:55:04 -0700492 c = (fcs & 0x00ff); /* least significant byte first */
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800493 if (muststuff(c)) {
494 *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
495 c ^= PPP_TRANS;
496 }
497 *(skb_put(hdlc_skb, 1)) = c;
498
499 c = ((fcs >> 8) & 0x00ff);
500 if (muststuff(c)) {
501 *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
502 c ^= PPP_TRANS;
503 }
504 *(skb_put(hdlc_skb, 1)) = c;
505
506 *(skb_put(hdlc_skb, 1)) = PPP_FLAG;
507
508 dev_kfree_skb(skb);
509 return hdlc_skb;
510}
511
512/* Encoding of a raw packet into an octet stuffed bit inverted frame
513 * parameters:
514 * skb skb containing original packet (freed upon return)
515 * head number of headroom bytes to allocate in result skb
516 * tail number of tailroom bytes to allocate in result skb
517 * Return value:
518 * pointer to newly allocated skb containing the result frame
519 */
520static struct sk_buff *iraw_encode(struct sk_buff *skb, int head, int tail)
521{
522 struct sk_buff *iraw_skb;
523 unsigned char c;
524 unsigned char *cp;
525 int len;
526
527 /* worst case: every byte must be stuffed */
528 iraw_skb = dev_alloc_skb(2*skb->len + tail + head);
529 if (!iraw_skb) {
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800530 dev_kfree_skb(skb);
531 return NULL;
532 }
533 skb_reserve(iraw_skb, head);
534
535 cp = skb->data;
536 len = skb->len;
537 while (len--) {
Akinobu Mita17b3cff2006-12-08 02:36:30 -0800538 c = bitrev8(*cp++);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800539 if (c == DLE_FLAG)
540 *(skb_put(iraw_skb, 1)) = c;
541 *(skb_put(iraw_skb, 1)) = c;
542 }
543 dev_kfree_skb(skb);
544 return iraw_skb;
545}
546
547/* gigaset_send_skb
548 * called by common.c to queue an skb for sending
549 * and start transmission if necessary
550 * parameters:
551 * B Channel control structure
552 * skb
553 * Return value:
554 * number of bytes accepted for sending
555 * (skb->len if ok, 0 if out of buffer space)
556 * or error code (< 0, eg. -EINVAL)
557 */
558int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb)
559{
Tilman Schmidtd48c7782006-04-10 22:55:08 -0700560 unsigned len = skb->len;
Tilman Schmidt69049cc2006-04-10 22:55:16 -0700561 unsigned long flags;
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800562
563 if (bcs->proto2 == ISDN_PROTO_L2_HDLC)
564 skb = HDLC_Encode(skb, HW_HDR_LEN, 0);
565 else
566 skb = iraw_encode(skb, HW_HDR_LEN, 0);
Tilman Schmidt784d5852006-04-10 22:55:04 -0700567 if (!skb) {
Tilman Schmidt50027792008-07-23 21:28:27 -0700568 dev_err(bcs->cs->dev,
569 "unable to allocate memory for encoding!\n");
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800570 return -ENOMEM;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700571 }
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800572
573 skb_queue_tail(&bcs->squeue, skb);
Tilman Schmidt69049cc2006-04-10 22:55:16 -0700574 spin_lock_irqsave(&bcs->cs->lock, flags);
575 if (bcs->cs->connected)
576 tasklet_schedule(&bcs->cs->write_tasklet);
577 spin_unlock_irqrestore(&bcs->cs->lock, flags);
Hansjoerg Lipp07dc1f92006-03-26 01:38:37 -0800578
579 return len; /* ok so far */
580}
Adrian Bunkd5c16822007-02-20 13:58:17 -0800581EXPORT_SYMBOL_GPL(gigaset_m10x_send_skb);