]> bbs.cooldavid.org Git - net-next-2.6.git/blame - drivers/isdn/gigaset/isocdata.c
git://bbs.cooldavid.org / net-next-2.6.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
[PATCH] isdn4linux: Siemens Gigaset drivers - isochronous data handler
[net-next-2.6.git] / drivers / isdn / gigaset / isocdata.c
CommitLineData
76bb4685
HL		 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 */
27void 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 */
41static 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 */
63static 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 */
86static 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 */
106static 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 */
120static 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 */
141static 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 */
161int 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 */
244static 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 */
280static 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
382static 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
424static 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
476static 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
504int 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 */
523static 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 */
543static 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 */
562static 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 */
605static 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 */
631static 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 */
661static 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 */
858static 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
900void 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
913static 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 */
946void 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 */
993int 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}
Clone of davem's net-next-2.6 tree