projects / deliverable / linux.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
blame | history | raw | HEAD
Merge branch 'for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/dtor/input
[deliverable/linux.git] / 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 }
Linux kernel - Deliverables
This page took 0.057474 seconds and 6 git commands to generate.