Merge git://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux-2.6 into for-linus
[linux-2.6] / drivers / isdn / gigaset / isocdata.c
1 /*
2  * Common data handling layer for bas_gigaset
3  *
4  * Copyright (c) 2005 by Tilman Schmidt <tilman@imap.cc>,
5  *                       Hansjoerg Lipp <hjlipp@web.de>.
6  *
7  * =====================================================================
8  *      This program is free software; you can redistribute it and/or
9  *      modify it under the terms of the GNU General Public License as
10  *      published by the Free Software Foundation; either version 2 of
11  *      the License, or (at your option) any later version.
12  * =====================================================================
13  */
14
15 #include "gigaset.h"
16 #include <linux/crc-ccitt.h>
17 #include <linux/bitrev.h>
18
19 /* access methods for isowbuf_t */
20 /* ============================ */
21
22 /* initialize buffer structure
23  */
24 void gigaset_isowbuf_init(struct isowbuf_t *iwb, unsigned char idle)
25 {
26         iwb->read = 0;
27         iwb->nextread = 0;
28         iwb->write = 0;
29         atomic_set(&iwb->writesem, 1);
30         iwb->wbits = 0;
31         iwb->idle = idle;
32         memset(iwb->data + BAS_OUTBUFSIZE, idle, BAS_OUTBUFPAD);
33 }
34
35 /* compute number of bytes which can be appended to buffer
36  * so that there is still room to append a maximum frame of flags
37  */
38 static inline int isowbuf_freebytes(struct isowbuf_t *iwb)
39 {
40         int read, write, freebytes;
41
42         read = iwb->read;
43         write = iwb->write;
44         if ((freebytes = read - write) > 0) {
45                 /* no wraparound: need padding space within regular area */
46                 return freebytes - BAS_OUTBUFPAD;
47         } else if (read < BAS_OUTBUFPAD) {
48                 /* wraparound: can use space up to end of regular area */
49                 return BAS_OUTBUFSIZE - write;
50         } else {
51                 /* following the wraparound yields more space */
52                 return freebytes + BAS_OUTBUFSIZE - BAS_OUTBUFPAD;
53         }
54 }
55
56 /* compare two offsets within the buffer
57  * The buffer is seen as circular, with the read position as start
58  * returns -1/0/1 if position a </=/> position b without crossing 'read'
59  */
60 static inline int isowbuf_poscmp(struct isowbuf_t *iwb, int a, int b)
61 {
62         int read;
63         if (a == b)
64                 return 0;
65         read = iwb->read;
66         if (a < b) {
67                 if (a < read && read <= b)
68                         return +1;
69                 else
70                         return -1;
71         } else {
72                 if (b < read && read <= a)
73                         return -1;
74                 else
75                         return +1;
76         }
77 }
78
79 /* start writing
80  * acquire the write semaphore
81  * return true if acquired, false if busy
82  */
83 static inline int isowbuf_startwrite(struct isowbuf_t *iwb)
84 {
85         if (!atomic_dec_and_test(&iwb->writesem)) {
86                 atomic_inc(&iwb->writesem);
87                 gig_dbg(DEBUG_ISO, "%s: couldn't acquire iso write semaphore",
88                         __func__);
89                 return 0;
90         }
91 #ifdef CONFIG_GIGASET_DEBUG
92         gig_dbg(DEBUG_ISO,
93                 "%s: acquired iso write semaphore, data[write]=%02x, nbits=%d",
94                 __func__, iwb->data[iwb->write], iwb->wbits);
95 #endif
96         return 1;
97 }
98
99 /* finish writing
100  * release the write semaphore
101  * returns the current write position
102  */
103 static inline int isowbuf_donewrite(struct isowbuf_t *iwb)
104 {
105         int write = iwb->write;
106         atomic_inc(&iwb->writesem);
107         return write;
108 }
109
110 /* append bits to buffer without any checks
111  * - data contains bits to append, starting at LSB
112  * - nbits is number of bits to append (0..24)
113  * must be called with the write semaphore held
114  * If more than nbits bits are set in data, the extraneous bits are set in the
115  * buffer too, but the write position is only advanced by nbits.
116  */
117 static inline void isowbuf_putbits(struct isowbuf_t *iwb, u32 data, int nbits)
118 {
119         int write = iwb->write;
120         data <<= iwb->wbits;
121         data |= iwb->data[write];
122         nbits += iwb->wbits;
123         while (nbits >= 8) {
124                 iwb->data[write++] = data & 0xff;
125                 write %= BAS_OUTBUFSIZE;
126                 data >>= 8;
127                 nbits -= 8;
128         }
129         iwb->wbits = nbits;
130         iwb->data[write] = data & 0xff;
131         iwb->write = write;
132 }
133
134 /* put final flag on HDLC bitstream
135  * also sets the idle fill byte to the correspondingly shifted flag pattern
136  * must be called with the write semaphore held
137  */
138 static inline void isowbuf_putflag(struct isowbuf_t *iwb)
139 {
140         int write;
141
142         /* add two flags, thus reliably covering one byte */
143         isowbuf_putbits(iwb, 0x7e7e, 8);
144         /* recover the idle flag byte */
145         write = iwb->write;
146         iwb->idle = iwb->data[write];
147         gig_dbg(DEBUG_ISO, "idle fill byte %02x", iwb->idle);
148         /* mask extraneous bits in buffer */
149         iwb->data[write] &= (1 << iwb->wbits) - 1;
150 }
151
152 /* retrieve a block of bytes for sending
153  * The requested number of bytes is provided as a contiguous block.
154  * If necessary, the frame is filled to the requested number of bytes
155  * with the idle value.
156  * returns offset to frame, < 0 on busy or error
157  */
158 int gigaset_isowbuf_getbytes(struct isowbuf_t *iwb, int size)
159 {
160         int read, write, limit, src, dst;
161         unsigned char pbyte;
162
163         read = iwb->nextread;
164         write = iwb->write;
165         if (likely(read == write)) {
166                 /* return idle frame */
167                 return read < BAS_OUTBUFPAD ?
168                         BAS_OUTBUFSIZE : read - BAS_OUTBUFPAD;
169         }
170
171         limit = read + size;
172         gig_dbg(DEBUG_STREAM, "%s: read=%d write=%d limit=%d",
173                 __func__, read, write, limit);
174 #ifdef CONFIG_GIGASET_DEBUG
175         if (unlikely(size < 0 || size > BAS_OUTBUFPAD)) {
176                 err("invalid size %d", size);
177                 return -EINVAL;
178         }
179         src = iwb->read;
180         if (unlikely(limit > BAS_OUTBUFSIZE + BAS_OUTBUFPAD ||
181                      (read < src && limit >= src))) {
182                 err("isoc write buffer frame reservation violated");
183                 return -EFAULT;
184         }
185 #endif
186
187         if (read < write) {
188                 /* no wraparound in valid data */
189                 if (limit >= write) {
190                         /* append idle frame */
191                         if (!isowbuf_startwrite(iwb))
192                                 return -EBUSY;
193                         /* write position could have changed */
194                         write = iwb->write;
195                         if (limit >= write) {
196                                 pbyte = iwb->data[write]; /* save
197                                                              partial byte */
198                                 limit = write + BAS_OUTBUFPAD;
199                                 gig_dbg(DEBUG_STREAM,
200                                         "%s: filling %d->%d with %02x",
201                                         __func__, write, limit, iwb->idle);
202                                 if (write + BAS_OUTBUFPAD < BAS_OUTBUFSIZE)
203                                         memset(iwb->data + write, iwb->idle,
204                                                BAS_OUTBUFPAD);
205                                 else {
206                                         /* wraparound, fill entire pad area */
207                                         memset(iwb->data + write, iwb->idle,
208                                                BAS_OUTBUFSIZE + BAS_OUTBUFPAD
209                                                - write);
210                                         limit = 0;
211                                 }
212                                 gig_dbg(DEBUG_STREAM,
213                                         "%s: restoring %02x at %d",
214                                         __func__, pbyte, limit);
215                                 iwb->data[limit] = pbyte; /* restore
216                                                              partial byte */
217                                 iwb->write = limit;
218                         }
219                         isowbuf_donewrite(iwb);
220                 }
221         } else {
222                 /* valid data wraparound */
223                 if (limit >= BAS_OUTBUFSIZE) {
224                         /* copy wrapped part into pad area */
225                         src = 0;
226                         dst = BAS_OUTBUFSIZE;
227                         while (dst < limit && src < write)
228                                 iwb->data[dst++] = iwb->data[src++];
229                         if (dst <= limit) {
230                                 /* fill pad area with idle byte */
231                                 memset(iwb->data + dst, iwb->idle,
232                                        BAS_OUTBUFSIZE + BAS_OUTBUFPAD - dst);
233                         }
234                         limit = src;
235                 }
236         }
237         iwb->nextread = limit;
238         return read;
239 }
240
241 /* dump_bytes
242  * write hex bytes to syslog for debugging
243  */
244 static inline void dump_bytes(enum debuglevel level, const char *tag,
245                               unsigned char *bytes, int count)
246 {
247 #ifdef CONFIG_GIGASET_DEBUG
248         unsigned char c;
249         static char dbgline[3 * 32 + 1];
250         static const char hexdigit[] = "0123456789abcdef";
251         int i = 0;
252         while (count-- > 0) {
253                 if (i > sizeof(dbgline) - 4) {
254                         dbgline[i] = '\0';
255                         gig_dbg(level, "%s:%s", tag, dbgline);
256                         i = 0;
257                 }
258                 c = *bytes++;
259                 dbgline[i] = (i && !(i % 12)) ? '-' : ' ';
260                 i++;
261                 dbgline[i++] = hexdigit[(c >> 4) & 0x0f];
262                 dbgline[i++] = hexdigit[c & 0x0f];
263         }
264         dbgline[i] = '\0';
265         gig_dbg(level, "%s:%s", tag, dbgline);
266 #endif
267 }
268
269 /*============================================================================*/
270
271 /* bytewise HDLC bitstuffing via table lookup
272  * lookup table: 5 subtables for 0..4 preceding consecutive '1' bits
273  * index: 256*(number of preceding '1' bits) + (next byte to stuff)
274  * value: bit  9.. 0 = result bits
275  *        bit 12..10 = number of trailing '1' bits in result
276  *        bit 14..13 = number of bits added by stuffing
277  */
278 static const u16 stufftab[5 * 256] = {
279 // previous 1s = 0:
280  0x0000, 0x0001, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009, 0x000a, 0x000b, 0x000c, 0x000d, 0x000e, 0x000f,
281  0x0010, 0x0011, 0x0012, 0x0013, 0x0014, 0x0015, 0x0016, 0x0017, 0x0018, 0x0019, 0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0x201f,
282  0x0020, 0x0021, 0x0022, 0x0023, 0x0024, 0x0025, 0x0026, 0x0027, 0x0028, 0x0029, 0x002a, 0x002b, 0x002c, 0x002d, 0x002e, 0x002f,
283  0x0030, 0x0031, 0x0032, 0x0033, 0x0034, 0x0035, 0x0036, 0x0037, 0x0038, 0x0039, 0x003a, 0x003b, 0x003c, 0x003d, 0x203e, 0x205f,
284  0x0040, 0x0041, 0x0042, 0x0043, 0x0044, 0x0045, 0x0046, 0x0047, 0x0048, 0x0049, 0x004a, 0x004b, 0x004c, 0x004d, 0x004e, 0x004f,
285  0x0050, 0x0051, 0x0052, 0x0053, 0x0054, 0x0055, 0x0056, 0x0057, 0x0058, 0x0059, 0x005a, 0x005b, 0x005c, 0x005d, 0x005e, 0x209f,
286  0x0060, 0x0061, 0x0062, 0x0063, 0x0064, 0x0065, 0x0066, 0x0067, 0x0068, 0x0069, 0x006a, 0x006b, 0x006c, 0x006d, 0x006e, 0x006f,
287  0x0070, 0x0071, 0x0072, 0x0073, 0x0074, 0x0075, 0x0076, 0x0077, 0x0078, 0x0079, 0x007a, 0x007b, 0x207c, 0x207d, 0x20be, 0x20df,
288  0x0480, 0x0481, 0x0482, 0x0483, 0x0484, 0x0485, 0x0486, 0x0487, 0x0488, 0x0489, 0x048a, 0x048b, 0x048c, 0x048d, 0x048e, 0x048f,
289  0x0490, 0x0491, 0x0492, 0x0493, 0x0494, 0x0495, 0x0496, 0x0497, 0x0498, 0x0499, 0x049a, 0x049b, 0x049c, 0x049d, 0x049e, 0x251f,
290  0x04a0, 0x04a1, 0x04a2, 0x04a3, 0x04a4, 0x04a5, 0x04a6, 0x04a7, 0x04a8, 0x04a9, 0x04aa, 0x04ab, 0x04ac, 0x04ad, 0x04ae, 0x04af,
291  0x04b0, 0x04b1, 0x04b2, 0x04b3, 0x04b4, 0x04b5, 0x04b6, 0x04b7, 0x04b8, 0x04b9, 0x04ba, 0x04bb, 0x04bc, 0x04bd, 0x253e, 0x255f,
292  0x08c0, 0x08c1, 0x08c2, 0x08c3, 0x08c4, 0x08c5, 0x08c6, 0x08c7, 0x08c8, 0x08c9, 0x08ca, 0x08cb, 0x08cc, 0x08cd, 0x08ce, 0x08cf,
293  0x08d0, 0x08d1, 0x08d2, 0x08d3, 0x08d4, 0x08d5, 0x08d6, 0x08d7, 0x08d8, 0x08d9, 0x08da, 0x08db, 0x08dc, 0x08dd, 0x08de, 0x299f,
294  0x0ce0, 0x0ce1, 0x0ce2, 0x0ce3, 0x0ce4, 0x0ce5, 0x0ce6, 0x0ce7, 0x0ce8, 0x0ce9, 0x0cea, 0x0ceb, 0x0cec, 0x0ced, 0x0cee, 0x0cef,
295  0x10f0, 0x10f1, 0x10f2, 0x10f3, 0x10f4, 0x10f5, 0x10f6, 0x10f7, 0x20f8, 0x20f9, 0x20fa, 0x20fb, 0x257c, 0x257d, 0x29be, 0x2ddf,
296
297 // previous 1s = 1:
298  0x0000, 0x0001, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009, 0x000a, 0x000b, 0x000c, 0x000d, 0x000e, 0x200f,
299  0x0010, 0x0011, 0x0012, 0x0013, 0x0014, 0x0015, 0x0016, 0x0017, 0x0018, 0x0019, 0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0x202f,
300  0x0020, 0x0021, 0x0022, 0x0023, 0x0024, 0x0025, 0x0026, 0x0027, 0x0028, 0x0029, 0x002a, 0x002b, 0x002c, 0x002d, 0x002e, 0x204f,
301  0x0030, 0x0031, 0x0032, 0x0033, 0x0034, 0x0035, 0x0036, 0x0037, 0x0038, 0x0039, 0x003a, 0x003b, 0x003c, 0x003d, 0x203e, 0x206f,
302  0x0040, 0x0041, 0x0042, 0x0043, 0x0044, 0x0045, 0x0046, 0x0047, 0x0048, 0x0049, 0x004a, 0x004b, 0x004c, 0x004d, 0x004e, 0x208f,
303  0x0050, 0x0051, 0x0052, 0x0053, 0x0054, 0x0055, 0x0056, 0x0057, 0x0058, 0x0059, 0x005a, 0x005b, 0x005c, 0x005d, 0x005e, 0x20af,
304  0x0060, 0x0061, 0x0062, 0x0063, 0x0064, 0x0065, 0x0066, 0x0067, 0x0068, 0x0069, 0x006a, 0x006b, 0x006c, 0x006d, 0x006e, 0x20cf,
305  0x0070, 0x0071, 0x0072, 0x0073, 0x0074, 0x0075, 0x0076, 0x0077, 0x0078, 0x0079, 0x007a, 0x007b, 0x207c, 0x207d, 0x20be, 0x20ef,
306  0x0480, 0x0481, 0x0482, 0x0483, 0x0484, 0x0485, 0x0486, 0x0487, 0x0488, 0x0489, 0x048a, 0x048b, 0x048c, 0x048d, 0x048e, 0x250f,
307  0x0490, 0x0491, 0x0492, 0x0493, 0x0494, 0x0495, 0x0496, 0x0497, 0x0498, 0x0499, 0x049a, 0x049b, 0x049c, 0x049d, 0x049e, 0x252f,
308  0x04a0, 0x04a1, 0x04a2, 0x04a3, 0x04a4, 0x04a5, 0x04a6, 0x04a7, 0x04a8, 0x04a9, 0x04aa, 0x04ab, 0x04ac, 0x04ad, 0x04ae, 0x254f,
309  0x04b0, 0x04b1, 0x04b2, 0x04b3, 0x04b4, 0x04b5, 0x04b6, 0x04b7, 0x04b8, 0x04b9, 0x04ba, 0x04bb, 0x04bc, 0x04bd, 0x253e, 0x256f,
310  0x08c0, 0x08c1, 0x08c2, 0x08c3, 0x08c4, 0x08c5, 0x08c6, 0x08c7, 0x08c8, 0x08c9, 0x08ca, 0x08cb, 0x08cc, 0x08cd, 0x08ce, 0x298f,
311  0x08d0, 0x08d1, 0x08d2, 0x08d3, 0x08d4, 0x08d5, 0x08d6, 0x08d7, 0x08d8, 0x08d9, 0x08da, 0x08db, 0x08dc, 0x08dd, 0x08de, 0x29af,
312  0x0ce0, 0x0ce1, 0x0ce2, 0x0ce3, 0x0ce4, 0x0ce5, 0x0ce6, 0x0ce7, 0x0ce8, 0x0ce9, 0x0cea, 0x0ceb, 0x0cec, 0x0ced, 0x0cee, 0x2dcf,
313  0x10f0, 0x10f1, 0x10f2, 0x10f3, 0x10f4, 0x10f5, 0x10f6, 0x10f7, 0x20f8, 0x20f9, 0x20fa, 0x20fb, 0x257c, 0x257d, 0x29be, 0x31ef,
314
315 // previous 1s = 2:
316  0x0000, 0x0001, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x2007, 0x0008, 0x0009, 0x000a, 0x000b, 0x000c, 0x000d, 0x000e, 0x2017,
317  0x0010, 0x0011, 0x0012, 0x0013, 0x0014, 0x0015, 0x0016, 0x2027, 0x0018, 0x0019, 0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0x2037,
318  0x0020, 0x0021, 0x0022, 0x0023, 0x0024, 0x0025, 0x0026, 0x2047, 0x0028, 0x0029, 0x002a, 0x002b, 0x002c, 0x002d, 0x002e, 0x2057,
319  0x0030, 0x0031, 0x0032, 0x0033, 0x0034, 0x0035, 0x0036, 0x2067, 0x0038, 0x0039, 0x003a, 0x003b, 0x003c, 0x003d, 0x203e, 0x2077,
320  0x0040, 0x0041, 0x0042, 0x0043, 0x0044, 0x0045, 0x0046, 0x2087, 0x0048, 0x0049, 0x004a, 0x004b, 0x004c, 0x004d, 0x004e, 0x2097,
321  0x0050, 0x0051, 0x0052, 0x0053, 0x0054, 0x0055, 0x0056, 0x20a7, 0x0058, 0x0059, 0x005a, 0x005b, 0x005c, 0x005d, 0x005e, 0x20b7,
322  0x0060, 0x0061, 0x0062, 0x0063, 0x0064, 0x0065, 0x0066, 0x20c7, 0x0068, 0x0069, 0x006a, 0x006b, 0x006c, 0x006d, 0x006e, 0x20d7,
323  0x0070, 0x0071, 0x0072, 0x0073, 0x0074, 0x0075, 0x0076, 0x20e7, 0x0078, 0x0079, 0x007a, 0x007b, 0x207c, 0x207d, 0x20be, 0x20f7,
324  0x0480, 0x0481, 0x0482, 0x0483, 0x0484, 0x0485, 0x0486, 0x2507, 0x0488, 0x0489, 0x048a, 0x048b, 0x048c, 0x048d, 0x048e, 0x2517,
325  0x0490, 0x0491, 0x0492, 0x0493, 0x0494, 0x0495, 0x0496, 0x2527, 0x0498, 0x0499, 0x049a, 0x049b, 0x049c, 0x049d, 0x049e, 0x2537,
326  0x04a0, 0x04a1, 0x04a2, 0x04a3, 0x04a4, 0x04a5, 0x04a6, 0x2547, 0x04a8, 0x04a9, 0x04aa, 0x04ab, 0x04ac, 0x04ad, 0x04ae, 0x2557,
327  0x04b0, 0x04b1, 0x04b2, 0x04b3, 0x04b4, 0x04b5, 0x04b6, 0x2567, 0x04b8, 0x04b9, 0x04ba, 0x04bb, 0x04bc, 0x04bd, 0x253e, 0x2577,
328  0x08c0, 0x08c1, 0x08c2, 0x08c3, 0x08c4, 0x08c5, 0x08c6, 0x2987, 0x08c8, 0x08c9, 0x08ca, 0x08cb, 0x08cc, 0x08cd, 0x08ce, 0x2997,
329  0x08d0, 0x08d1, 0x08d2, 0x08d3, 0x08d4, 0x08d5, 0x08d6, 0x29a7, 0x08d8, 0x08d9, 0x08da, 0x08db, 0x08dc, 0x08dd, 0x08de, 0x29b7,
330  0x0ce0, 0x0ce1, 0x0ce2, 0x0ce3, 0x0ce4, 0x0ce5, 0x0ce6, 0x2dc7, 0x0ce8, 0x0ce9, 0x0cea, 0x0ceb, 0x0cec, 0x0ced, 0x0cee, 0x2dd7,
331  0x10f0, 0x10f1, 0x10f2, 0x10f3, 0x10f4, 0x10f5, 0x10f6, 0x31e7, 0x20f8, 0x20f9, 0x20fa, 0x20fb, 0x257c, 0x257d, 0x29be, 0x41f7,
332
333 // previous 1s = 3:
334  0x0000, 0x0001, 0x0002, 0x2003, 0x0004, 0x0005, 0x0006, 0x200b, 0x0008, 0x0009, 0x000a, 0x2013, 0x000c, 0x000d, 0x000e, 0x201b,
335  0x0010, 0x0011, 0x0012, 0x2023, 0x0014, 0x0015, 0x0016, 0x202b, 0x0018, 0x0019, 0x001a, 0x2033, 0x001c, 0x001d, 0x001e, 0x203b,
336  0x0020, 0x0021, 0x0022, 0x2043, 0x0024, 0x0025, 0x0026, 0x204b, 0x0028, 0x0029, 0x002a, 0x2053, 0x002c, 0x002d, 0x002e, 0x205b,
337  0x0030, 0x0031, 0x0032, 0x2063, 0x0034, 0x0035, 0x0036, 0x206b, 0x0038, 0x0039, 0x003a, 0x2073, 0x003c, 0x003d, 0x203e, 0x207b,
338  0x0040, 0x0041, 0x0042, 0x2083, 0x0044, 0x0045, 0x0046, 0x208b, 0x0048, 0x0049, 0x004a, 0x2093, 0x004c, 0x004d, 0x004e, 0x209b,
339  0x0050, 0x0051, 0x0052, 0x20a3, 0x0054, 0x0055, 0x0056, 0x20ab, 0x0058, 0x0059, 0x005a, 0x20b3, 0x005c, 0x005d, 0x005e, 0x20bb,
340  0x0060, 0x0061, 0x0062, 0x20c3, 0x0064, 0x0065, 0x0066, 0x20cb, 0x0068, 0x0069, 0x006a, 0x20d3, 0x006c, 0x006d, 0x006e, 0x20db,
341  0x0070, 0x0071, 0x0072, 0x20e3, 0x0074, 0x0075, 0x0076, 0x20eb, 0x0078, 0x0079, 0x007a, 0x20f3, 0x207c, 0x207d, 0x20be, 0x40fb,
342  0x0480, 0x0481, 0x0482, 0x2503, 0x0484, 0x0485, 0x0486, 0x250b, 0x0488, 0x0489, 0x048a, 0x2513, 0x048c, 0x048d, 0x048e, 0x251b,
343  0x0490, 0x0491, 0x0492, 0x2523, 0x0494, 0x0495, 0x0496, 0x252b, 0x0498, 0x0499, 0x049a, 0x2533, 0x049c, 0x049d, 0x049e, 0x253b,
344  0x04a0, 0x04a1, 0x04a2, 0x2543, 0x04a4, 0x04a5, 0x04a6, 0x254b, 0x04a8, 0x04a9, 0x04aa, 0x2553, 0x04ac, 0x04ad, 0x04ae, 0x255b,
345  0x04b0, 0x04b1, 0x04b2, 0x2563, 0x04b4, 0x04b5, 0x04b6, 0x256b, 0x04b8, 0x04b9, 0x04ba, 0x2573, 0x04bc, 0x04bd, 0x253e, 0x257b,
346  0x08c0, 0x08c1, 0x08c2, 0x2983, 0x08c4, 0x08c5, 0x08c6, 0x298b, 0x08c8, 0x08c9, 0x08ca, 0x2993, 0x08cc, 0x08cd, 0x08ce, 0x299b,
347  0x08d0, 0x08d1, 0x08d2, 0x29a3, 0x08d4, 0x08d5, 0x08d6, 0x29ab, 0x08d8, 0x08d9, 0x08da, 0x29b3, 0x08dc, 0x08dd, 0x08de, 0x29bb,
348  0x0ce0, 0x0ce1, 0x0ce2, 0x2dc3, 0x0ce4, 0x0ce5, 0x0ce6, 0x2dcb, 0x0ce8, 0x0ce9, 0x0cea, 0x2dd3, 0x0cec, 0x0ced, 0x0cee, 0x2ddb,
349  0x10f0, 0x10f1, 0x10f2, 0x31e3, 0x10f4, 0x10f5, 0x10f6, 0x31eb, 0x20f8, 0x20f9, 0x20fa, 0x41f3, 0x257c, 0x257d, 0x29be, 0x46fb,
350
351 // previous 1s = 4:
352  0x0000, 0x2001, 0x0002, 0x2005, 0x0004, 0x2009, 0x0006, 0x200d, 0x0008, 0x2011, 0x000a, 0x2015, 0x000c, 0x2019, 0x000e, 0x201d,
353  0x0010, 0x2021, 0x0012, 0x2025, 0x0014, 0x2029, 0x0016, 0x202d, 0x0018, 0x2031, 0x001a, 0x2035, 0x001c, 0x2039, 0x001e, 0x203d,
354  0x0020, 0x2041, 0x0022, 0x2045, 0x0024, 0x2049, 0x0026, 0x204d, 0x0028, 0x2051, 0x002a, 0x2055, 0x002c, 0x2059, 0x002e, 0x205d,
355  0x0030, 0x2061, 0x0032, 0x2065, 0x0034, 0x2069, 0x0036, 0x206d, 0x0038, 0x2071, 0x003a, 0x2075, 0x003c, 0x2079, 0x203e, 0x407d,
356  0x0040, 0x2081, 0x0042, 0x2085, 0x0044, 0x2089, 0x0046, 0x208d, 0x0048, 0x2091, 0x004a, 0x2095, 0x004c, 0x2099, 0x004e, 0x209d,
357  0x0050, 0x20a1, 0x0052, 0x20a5, 0x0054, 0x20a9, 0x0056, 0x20ad, 0x0058, 0x20b1, 0x005a, 0x20b5, 0x005c, 0x20b9, 0x005e, 0x20bd,
358  0x0060, 0x20c1, 0x0062, 0x20c5, 0x0064, 0x20c9, 0x0066, 0x20cd, 0x0068, 0x20d1, 0x006a, 0x20d5, 0x006c, 0x20d9, 0x006e, 0x20dd,
359  0x0070, 0x20e1, 0x0072, 0x20e5, 0x0074, 0x20e9, 0x0076, 0x20ed, 0x0078, 0x20f1, 0x007a, 0x20f5, 0x207c, 0x40f9, 0x20be, 0x417d,
360  0x0480, 0x2501, 0x0482, 0x2505, 0x0484, 0x2509, 0x0486, 0x250d, 0x0488, 0x2511, 0x048a, 0x2515, 0x048c, 0x2519, 0x048e, 0x251d,
361  0x0490, 0x2521, 0x0492, 0x2525, 0x0494, 0x2529, 0x0496, 0x252d, 0x0498, 0x2531, 0x049a, 0x2535, 0x049c, 0x2539, 0x049e, 0x253d,
362  0x04a0, 0x2541, 0x04a2, 0x2545, 0x04a4, 0x2549, 0x04a6, 0x254d, 0x04a8, 0x2551, 0x04aa, 0x2555, 0x04ac, 0x2559, 0x04ae, 0x255d,
363  0x04b0, 0x2561, 0x04b2, 0x2565, 0x04b4, 0x2569, 0x04b6, 0x256d, 0x04b8, 0x2571, 0x04ba, 0x2575, 0x04bc, 0x2579, 0x253e, 0x467d,
364  0x08c0, 0x2981, 0x08c2, 0x2985, 0x08c4, 0x2989, 0x08c6, 0x298d, 0x08c8, 0x2991, 0x08ca, 0x2995, 0x08cc, 0x2999, 0x08ce, 0x299d,
365  0x08d0, 0x29a1, 0x08d2, 0x29a5, 0x08d4, 0x29a9, 0x08d6, 0x29ad, 0x08d8, 0x29b1, 0x08da, 0x29b5, 0x08dc, 0x29b9, 0x08de, 0x29bd,
366  0x0ce0, 0x2dc1, 0x0ce2, 0x2dc5, 0x0ce4, 0x2dc9, 0x0ce6, 0x2dcd, 0x0ce8, 0x2dd1, 0x0cea, 0x2dd5, 0x0cec, 0x2dd9, 0x0cee, 0x2ddd,
367  0x10f0, 0x31e1, 0x10f2, 0x31e5, 0x10f4, 0x31e9, 0x10f6, 0x31ed, 0x20f8, 0x41f1, 0x20fa, 0x41f5, 0x257c, 0x46f9, 0x29be, 0x4b7d
368 };
369
370 /* hdlc_bitstuff_byte
371  * perform HDLC bitstuffing for one input byte (8 bits, LSB first)
372  * parameters:
373  *      cin     input byte
374  *      ones    number of trailing '1' bits in result before this step
375  *      iwb     pointer to output buffer structure (write semaphore must be held)
376  * return value:
377  *      number of trailing '1' bits in result after this step
378  */
379
380 static inline int hdlc_bitstuff_byte(struct isowbuf_t *iwb, unsigned char cin,
381                                      int ones)
382 {
383         u16 stuff;
384         int shiftinc, newones;
385
386         /* get stuffing information for input byte
387          * value: bit  9.. 0 = result bits
388          *        bit 12..10 = number of trailing '1' bits in result
389          *        bit 14..13 = number of bits added by stuffing
390          */
391         stuff = stufftab[256 * ones + cin];
392         shiftinc = (stuff >> 13) & 3;
393         newones = (stuff >> 10) & 7;
394         stuff &= 0x3ff;
395
396         /* append stuffed byte to output stream */
397         isowbuf_putbits(iwb, stuff, 8 + shiftinc);
398         return newones;
399 }
400
401 /* hdlc_buildframe
402  * Perform HDLC framing with bitstuffing on a byte buffer
403  * The input buffer is regarded as a sequence of bits, starting with the least
404  * significant bit of the first byte and ending with the most significant bit
405  * of the last byte. A 16 bit FCS is appended as defined by RFC 1662.
406  * Whenever five consecutive '1' bits appear in the resulting bit sequence, a
407  * '0' bit is inserted after them.
408  * The resulting bit string and a closing flag pattern (PPP_FLAG, '01111110')
409  * are appended to the output buffer starting at the given bit position, which
410  * is assumed to already contain a leading flag.
411  * The output buffer must have sufficient length; count + count/5 + 6 bytes
412  * starting at *out are safe and are verified to be present.
413  * parameters:
414  *      in      input buffer
415  *      count   number of bytes in input buffer
416  *      iwb     pointer to output buffer structure (write semaphore must be held)
417  * return value:
418  *      position of end of packet in output buffer on success,
419  *      -EAGAIN if write semaphore busy or buffer full
420  */
421
422 static inline int hdlc_buildframe(struct isowbuf_t *iwb,
423                                   unsigned char *in, int count)
424 {
425         int ones;
426         u16 fcs;
427         int end;
428         unsigned char c;
429
430         if (isowbuf_freebytes(iwb) < count + count / 5 + 6 ||
431             !isowbuf_startwrite(iwb)) {
432                 gig_dbg(DEBUG_ISO, "%s: %d bytes free -> -EAGAIN",
433                         __func__, isowbuf_freebytes(iwb));
434                 return -EAGAIN;
435         }
436
437         dump_bytes(DEBUG_STREAM, "snd data", in, count);
438
439         /* bitstuff and checksum input data */
440         fcs = PPP_INITFCS;
441         ones = 0;
442         while (count-- > 0) {
443                 c = *in++;
444                 ones = hdlc_bitstuff_byte(iwb, c, ones);
445                 fcs = crc_ccitt_byte(fcs, c);
446         }
447
448         /* bitstuff and append FCS (complemented, least significant byte first) */
449         fcs ^= 0xffff;
450         ones = hdlc_bitstuff_byte(iwb, fcs & 0x00ff, ones);
451         ones = hdlc_bitstuff_byte(iwb, (fcs >> 8) & 0x00ff, ones);
452
453         /* put closing flag and repeat byte for flag idle */
454         isowbuf_putflag(iwb);
455         end = isowbuf_donewrite(iwb);
456         dump_bytes(DEBUG_STREAM_DUMP, "isowbuf", iwb->data, end + 1);
457         return end;
458 }
459
460 /* trans_buildframe
461  * Append a block of 'transparent' data to the output buffer,
462  * inverting the bytes.
463  * The output buffer must have sufficient length; count bytes
464  * starting at *out are safe and are verified to be present.
465  * parameters:
466  *      in      input buffer
467  *      count   number of bytes in input buffer
468  *      iwb     pointer to output buffer structure (write semaphore must be held)
469  * return value:
470  *      position of end of packet in output buffer on success,
471  *      -EAGAIN if write semaphore busy or buffer full
472  */
473
474 static inline int trans_buildframe(struct isowbuf_t *iwb,
475                                    unsigned char *in, int count)
476 {
477         int write;
478         unsigned char c;
479
480         if (unlikely(count <= 0))
481                 return iwb->write;
482
483         if (isowbuf_freebytes(iwb) < count ||
484             !isowbuf_startwrite(iwb)) {
485                 gig_dbg(DEBUG_ISO, "can't put %d bytes", count);
486                 return -EAGAIN;
487         }
488
489         gig_dbg(DEBUG_STREAM, "put %d bytes", count);
490         write = iwb->write;
491         do {
492                 c = bitrev8(*in++);
493                 iwb->data[write++] = c;
494                 write %= BAS_OUTBUFSIZE;
495         } while (--count > 0);
496         iwb->write = write;
497         iwb->idle = c;
498
499         return isowbuf_donewrite(iwb);
500 }
501
502 int gigaset_isoc_buildframe(struct bc_state *bcs, unsigned char *in, int len)
503 {
504         int result;
505
506         switch (bcs->proto2) {
507         case ISDN_PROTO_L2_HDLC:
508                 result = hdlc_buildframe(bcs->hw.bas->isooutbuf, in, len);
509                 gig_dbg(DEBUG_ISO, "%s: %d bytes HDLC -> %d",
510                         __func__, len, result);
511                 break;
512         default:                        /* assume transparent */
513                 result = trans_buildframe(bcs->hw.bas->isooutbuf, in, len);
514                 gig_dbg(DEBUG_ISO, "%s: %d bytes trans -> %d",
515                         __func__, len, result);
516         }
517         return result;
518 }
519
520 /* hdlc_putbyte
521  * append byte c to current skb of B channel structure *bcs, updating fcs
522  */
523 static inline void hdlc_putbyte(unsigned char c, struct bc_state *bcs)
524 {
525         bcs->fcs = crc_ccitt_byte(bcs->fcs, c);
526         if (unlikely(bcs->skb == NULL)) {
527                 /* skipping */
528                 return;
529         }
530         if (unlikely(bcs->skb->len == SBUFSIZE)) {
531                 dev_warn(bcs->cs->dev, "received oversized packet discarded\n");
532                 bcs->hw.bas->giants++;
533                 dev_kfree_skb_any(bcs->skb);
534                 bcs->skb = NULL;
535                 return;
536         }
537         *__skb_put(bcs->skb, 1) = c;
538 }
539
540 /* hdlc_flush
541  * drop partial HDLC data packet
542  */
543 static inline void hdlc_flush(struct bc_state *bcs)
544 {
545         /* clear skb or allocate new if not skipping */
546         if (likely(bcs->skb != NULL))
547                 skb_trim(bcs->skb, 0);
548         else if (!bcs->ignore) {
549                 if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
550                         skb_reserve(bcs->skb, HW_HDR_LEN);
551                 else
552                         dev_err(bcs->cs->dev, "could not allocate skb\n");
553         }
554
555         /* reset packet state */
556         bcs->fcs = PPP_INITFCS;
557 }
558
559 /* hdlc_done
560  * process completed HDLC data packet
561  */
562 static inline void hdlc_done(struct bc_state *bcs)
563 {
564         struct sk_buff *procskb;
565
566         if (unlikely(bcs->ignore)) {
567                 bcs->ignore--;
568                 hdlc_flush(bcs);
569                 return;
570         }
571
572         if ((procskb = bcs->skb) == NULL) {
573                 /* previous error */
574                 gig_dbg(DEBUG_ISO, "%s: skb=NULL", __func__);
575                 gigaset_rcv_error(NULL, bcs->cs, bcs);
576         } else if (procskb->len < 2) {
577                 dev_notice(bcs->cs->dev, "received short frame (%d octets)\n",
578                            procskb->len);
579                 bcs->hw.bas->runts++;
580                 gigaset_rcv_error(procskb, bcs->cs, bcs);
581         } else if (bcs->fcs != PPP_GOODFCS) {
582                 dev_notice(bcs->cs->dev, "frame check error (0x%04x)\n",
583                            bcs->fcs);
584                 bcs->hw.bas->fcserrs++;
585                 gigaset_rcv_error(procskb, bcs->cs, bcs);
586         } else {
587                 procskb->len -= 2;              /* subtract FCS */
588                 procskb->tail -= 2;
589                 gig_dbg(DEBUG_ISO, "%s: good frame (%d octets)",
590                         __func__, procskb->len);
591                 dump_bytes(DEBUG_STREAM,
592                            "rcv data", procskb->data, procskb->len);
593                 bcs->hw.bas->goodbytes += procskb->len;
594                 gigaset_rcv_skb(procskb, bcs->cs, bcs);
595         }
596
597         if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
598                 skb_reserve(bcs->skb, HW_HDR_LEN);
599         else
600                 dev_err(bcs->cs->dev, "could not allocate skb\n");
601         bcs->fcs = PPP_INITFCS;
602 }
603
604 /* hdlc_frag
605  * drop HDLC data packet with non-integral last byte
606  */
607 static inline void hdlc_frag(struct bc_state *bcs, unsigned inbits)
608 {
609         if (unlikely(bcs->ignore)) {
610                 bcs->ignore--;
611                 hdlc_flush(bcs);
612                 return;
613         }
614
615         dev_notice(bcs->cs->dev, "received partial byte (%d bits)\n", inbits);
616         bcs->hw.bas->alignerrs++;
617         gigaset_rcv_error(bcs->skb, bcs->cs, bcs);
618
619         if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
620                 skb_reserve(bcs->skb, HW_HDR_LEN);
621         else
622                 dev_err(bcs->cs->dev, "could not allocate skb\n");
623         bcs->fcs = PPP_INITFCS;
624 }
625
626 /* bit counts lookup table for HDLC bit unstuffing
627  * index: input byte
628  * value: bit 0..3 = number of consecutive '1' bits starting from LSB
629  *        bit 4..6 = number of consecutive '1' bits starting from MSB
630  *                   (replacing 8 by 7 to make it fit; the algorithm won't care)
631  *        bit 7 set if there are 5 or more "interior" consecutive '1' bits
632  */
633 static const unsigned char bitcounts[256] = {
634   0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
635   0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x05,
636   0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
637   0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x80, 0x06,
638   0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
639   0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x05,
640   0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
641   0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x80, 0x81, 0x80, 0x07,
642   0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x14,
643   0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x15,
644   0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x14,
645   0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x90, 0x16,
646   0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x23, 0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x24,
647   0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x23, 0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x25,
648   0x30, 0x31, 0x30, 0x32, 0x30, 0x31, 0x30, 0x33, 0x30, 0x31, 0x30, 0x32, 0x30, 0x31, 0x30, 0x34,
649   0x40, 0x41, 0x40, 0x42, 0x40, 0x41, 0x40, 0x43, 0x50, 0x51, 0x50, 0x52, 0x60, 0x61, 0x70, 0x78
650 };
651
652 /* hdlc_unpack
653  * perform HDLC frame processing (bit unstuffing, flag detection, FCS calculation)
654  * on a sequence of received data bytes (8 bits each, LSB first)
655  * pass on successfully received, complete frames as SKBs via gigaset_rcv_skb
656  * notify of errors via gigaset_rcv_error
657  * tally frames, errors etc. in BC structure counters
658  * parameters:
659  *      src     received data
660  *      count   number of received bytes
661  *      bcs     receiving B channel structure
662  */
663 static inline void hdlc_unpack(unsigned char *src, unsigned count,
664                                struct bc_state *bcs)
665 {
666         struct bas_bc_state *ubc = bcs->hw.bas;
667         int inputstate;
668         unsigned seqlen, inbyte, inbits;
669
670         /* load previous state:
671          * inputstate = set of flag bits:
672          * - INS_flag_hunt: no complete opening flag received since connection setup or last abort
673          * - INS_have_data: at least one complete data byte received since last flag
674          * seqlen = number of consecutive '1' bits in last 7 input stream bits (0..7)
675          * inbyte = accumulated partial data byte (if !INS_flag_hunt)
676          * inbits = number of valid bits in inbyte, starting at LSB (0..6)
677          */
678         inputstate = bcs->inputstate;
679         seqlen = ubc->seqlen;
680         inbyte = ubc->inbyte;
681         inbits = ubc->inbits;
682
683         /* bit unstuffing a byte a time
684          * Take your time to understand this; it's straightforward but tedious.
685          * The "bitcounts" lookup table is used to speed up the counting of
686          * leading and trailing '1' bits.
687          */
688         while (count--) {
689                 unsigned char c = *src++;
690                 unsigned char tabentry = bitcounts[c];
691                 unsigned lead1 = tabentry & 0x0f;
692                 unsigned trail1 = (tabentry >> 4) & 0x0f;
693
694                 seqlen += lead1;
695
696                 if (unlikely(inputstate & INS_flag_hunt)) {
697                         if (c == PPP_FLAG) {
698                                 /* flag-in-one */
699                                 inputstate &= ~(INS_flag_hunt | INS_have_data);
700                                 inbyte = 0;
701                                 inbits = 0;
702                         } else if (seqlen == 6 && trail1 != 7) {
703                                 /* flag completed & not followed by abort */
704                                 inputstate &= ~(INS_flag_hunt | INS_have_data);
705                                 inbyte = c >> (lead1 + 1);
706                                 inbits = 7 - lead1;
707                                 if (trail1 >= 8) {
708                                         /* interior stuffing: omitting the MSB handles most cases */
709                                         inbits--;
710                                         /* correct the incorrectly handled cases individually */
711                                         switch (c) {
712                                         case 0xbe:
713                                                 inbyte = 0x3f;
714                                                 break;
715                                         }
716                                 }
717                         }
718                         /* else: continue flag-hunting */
719                 } else if (likely(seqlen < 5 && trail1 < 7)) {
720                         /* streamlined case: 8 data bits, no stuffing */
721                         inbyte |= c << inbits;
722                         hdlc_putbyte(inbyte & 0xff, bcs);
723                         inputstate |= INS_have_data;
724                         inbyte >>= 8;
725                         /* inbits unchanged */
726                 } else if (likely(seqlen == 6 && inbits == 7 - lead1 &&
727                                   trail1 + 1 == inbits &&
728                                   !(inputstate & INS_have_data))) {
729                         /* streamlined case: flag idle - state unchanged */
730                 } else if (unlikely(seqlen > 6)) {
731                         /* abort sequence */
732                         ubc->aborts++;
733                         hdlc_flush(bcs);
734                         inputstate |= INS_flag_hunt;
735                 } else if (seqlen == 6) {
736                         /* closing flag, including (6 - lead1) '1's and one '0' from inbits */
737                         if (inbits > 7 - lead1) {
738                                 hdlc_frag(bcs, inbits + lead1 - 7);
739                                 inputstate &= ~INS_have_data;
740                         } else {
741                                 if (inbits < 7 - lead1)
742                                         ubc->stolen0s ++;
743                                 if (inputstate & INS_have_data) {
744                                         hdlc_done(bcs);
745                                         inputstate &= ~INS_have_data;
746                                 }
747                         }
748
749                         if (c == PPP_FLAG) {
750                                 /* complete flag, LSB overlaps preceding flag */
751                                 ubc->shared0s ++;
752                                 inbits = 0;
753                                 inbyte = 0;
754                         } else if (trail1 != 7) {
755                                 /* remaining bits */
756                                 inbyte = c >> (lead1 + 1);
757                                 inbits = 7 - lead1;
758                                 if (trail1 >= 8) {
759                                         /* interior stuffing: omitting the MSB handles most cases */
760                                         inbits--;
761                                         /* correct the incorrectly handled cases individually */
762                                         switch (c) {
763                                         case 0xbe:
764                                                 inbyte = 0x3f;
765                                                 break;
766                                         }
767                                 }
768                         } else {
769                                 /* abort sequence follows, skb already empty anyway */
770                                 ubc->aborts++;
771                                 inputstate |= INS_flag_hunt;
772                         }
773                 } else { /* (seqlen < 6) && (seqlen == 5 || trail1 >= 7) */
774
775                         if (c == PPP_FLAG) {
776                                 /* complete flag */
777                                 if (seqlen == 5)
778                                         ubc->stolen0s++;
779                                 if (inbits) {
780                                         hdlc_frag(bcs, inbits);
781                                         inbits = 0;
782                                         inbyte = 0;
783                                 } else if (inputstate & INS_have_data)
784                                         hdlc_done(bcs);
785                                 inputstate &= ~INS_have_data;
786                         } else if (trail1 == 7) {
787                                 /* abort sequence */
788                                 ubc->aborts++;
789                                 hdlc_flush(bcs);
790                                 inputstate |= INS_flag_hunt;
791                         } else {
792                                 /* stuffed data */
793                                 if (trail1 < 7) { /* => seqlen == 5 */
794                                         /* stuff bit at position lead1, no interior stuffing */
795                                         unsigned char mask = (1 << lead1) - 1;
796                                         c = (c & mask) | ((c & ~mask) >> 1);
797                                         inbyte |= c << inbits;
798                                         inbits += 7;
799                                 } else if (seqlen < 5) { /* trail1 >= 8 */
800                                         /* interior stuffing: omitting the MSB handles most cases */
801                                         /* correct the incorrectly handled cases individually */
802                                         switch (c) {
803                                         case 0xbe:
804                                                 c = 0x7e;
805                                                 break;
806                                         }
807                                         inbyte |= c << inbits;
808                                         inbits += 7;
809                                 } else { /* seqlen == 5 && trail1 >= 8 */
810
811                                         /* stuff bit at lead1 *and* interior stuffing */
812                                         switch (c) {    /* unstuff individually */
813                                         case 0x7d:
814                                                 c = 0x3f;
815                                                 break;
816                                         case 0xbe:
817                                                 c = 0x3f;
818                                                 break;
819                                         case 0x3e:
820                                                 c = 0x1f;
821                                                 break;
822                                         case 0x7c:
823                                                 c = 0x3e;
824                                                 break;
825                                         }
826                                         inbyte |= c << inbits;
827                                         inbits += 6;
828                                 }
829                                 if (inbits >= 8) {
830                                         inbits -= 8;
831                                         hdlc_putbyte(inbyte & 0xff, bcs);
832                                         inputstate |= INS_have_data;
833                                         inbyte >>= 8;
834                                 }
835                         }
836                 }
837                 seqlen = trail1 & 7;
838         }
839
840         /* save new state */
841         bcs->inputstate = inputstate;
842         ubc->seqlen = seqlen;
843         ubc->inbyte = inbyte;
844         ubc->inbits = inbits;
845 }
846
847 /* trans_receive
848  * pass on received USB frame transparently as SKB via gigaset_rcv_skb
849  * invert bytes
850  * tally frames, errors etc. in BC structure counters
851  * parameters:
852  *      src     received data
853  *      count   number of received bytes
854  *      bcs     receiving B channel structure
855  */
856 static inline void trans_receive(unsigned char *src, unsigned count,
857                                  struct bc_state *bcs)
858 {
859         struct sk_buff *skb;
860         int dobytes;
861         unsigned char *dst;
862
863         if (unlikely(bcs->ignore)) {
864                 bcs->ignore--;
865                 hdlc_flush(bcs);
866                 return;
867         }
868         if (unlikely((skb = bcs->skb) == NULL)) {
869                 bcs->skb = skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN);
870                 if (!skb) {
871                         dev_err(bcs->cs->dev, "could not allocate skb\n");
872                         return;
873                 }
874                 skb_reserve(skb, HW_HDR_LEN);
875         }
876         bcs->hw.bas->goodbytes += skb->len;
877         dobytes = TRANSBUFSIZE - skb->len;
878         while (count > 0) {
879                 dst = skb_put(skb, count < dobytes ? count : dobytes);
880                 while (count > 0 && dobytes > 0) {
881                         *dst++ = bitrev8(*src++);
882                         count--;
883                         dobytes--;
884                 }
885                 if (dobytes == 0) {
886                         gigaset_rcv_skb(skb, bcs->cs, bcs);
887                         bcs->skb = skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN);
888                         if (!skb) {
889                                 dev_err(bcs->cs->dev,
890                                         "could not allocate skb\n");
891                                 return;
892                         }
893                         skb_reserve(bcs->skb, HW_HDR_LEN);
894                         dobytes = TRANSBUFSIZE;
895                 }
896         }
897 }
898
899 void gigaset_isoc_receive(unsigned char *src, unsigned count, struct bc_state *bcs)
900 {
901         switch (bcs->proto2) {
902         case ISDN_PROTO_L2_HDLC:
903                 hdlc_unpack(src, count, bcs);
904                 break;
905         default:                /* assume transparent */
906                 trans_receive(src, count, bcs);
907         }
908 }
909
910 /* == data input =========================================================== */
911
912 static void cmd_loop(unsigned char *src, int numbytes, struct inbuf_t *inbuf)
913 {
914         struct cardstate *cs = inbuf->cs;
915         unsigned cbytes      = cs->cbytes;
916
917         while (numbytes--) {
918                 /* copy next character, check for end of line */
919                 switch (cs->respdata[cbytes] = *src++) {
920                 case '\r':
921                 case '\n':
922                         /* end of line */
923                         gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)",
924                                 __func__, cbytes);
925                         if (cbytes >= MAX_RESP_SIZE - 1)
926                                 dev_warn(cs->dev, "response too large\n");
927                         cs->cbytes = cbytes;
928                         gigaset_handle_modem_response(cs);
929                         cbytes = 0;
930                         break;
931                 default:
932                         /* advance in line buffer, checking for overflow */
933                         if (cbytes < MAX_RESP_SIZE - 1)
934                                 cbytes++;
935                 }
936         }
937
938         /* save state */
939         cs->cbytes = cbytes;
940 }
941
942
943 /* process a block of data received through the control channel
944  */
945 void gigaset_isoc_input(struct inbuf_t *inbuf)
946 {
947         struct cardstate *cs = inbuf->cs;
948         unsigned tail, head, numbytes;
949         unsigned char *src;
950
951         head = inbuf->head;
952         while (head != (tail = inbuf->tail)) {
953                 gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
954                 if (head > tail)
955                         tail = RBUFSIZE;
956                 src = inbuf->data + head;
957                 numbytes = tail - head;
958                 gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
959
960                 if (cs->mstate == MS_LOCKED) {
961                         gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
962                                            numbytes, src);
963                         gigaset_if_receive(inbuf->cs, src, numbytes);
964                 } else {
965                         gigaset_dbg_buffer(DEBUG_CMD, "received response",
966                                            numbytes, src);
967                         cmd_loop(src, numbytes, inbuf);
968                 }
969
970                 head += numbytes;
971                 if (head == RBUFSIZE)
972                         head = 0;
973                 gig_dbg(DEBUG_INTR, "setting head to %u", head);
974                 inbuf->head = head;
975         }
976 }
977
978
979 /* == data output ========================================================== */
980
981 /* gigaset_send_skb
982  * called by common.c to queue an skb for sending
983  * and start transmission if necessary
984  * parameters:
985  *      B Channel control structure
986  *      skb
987  * return value:
988  *      number of bytes accepted for sending
989  *      (skb->len if ok, 0 if out of buffer space)
990  *      or error code (< 0, eg. -EINVAL)
991  */
992 int gigaset_isoc_send_skb(struct bc_state *bcs, struct sk_buff *skb)
993 {
994         int len = skb->len;
995         unsigned long flags;
996
997         spin_lock_irqsave(&bcs->cs->lock, flags);
998         if (!bcs->cs->connected) {
999                 spin_unlock_irqrestore(&bcs->cs->lock, flags);
1000                 return -ENODEV;
1001         }
1002
1003         skb_queue_tail(&bcs->squeue, skb);
1004         gig_dbg(DEBUG_ISO, "%s: skb queued, qlen=%d",
1005                 __func__, skb_queue_len(&bcs->squeue));
1006
1007         /* tasklet submits URB if necessary */
1008         tasklet_schedule(&bcs->hw.bas->sent_tasklet);
1009         spin_unlock_irqrestore(&bcs->cs->lock, flags);
1010
1011         return len;     /* ok so far */
1012 }