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