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