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