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 * '' 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, "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 dump_bytes(DEBUG_STREAM_DUMP, "isowbuf", iwb->data, end + 1);
452 return end;
453 }
454
455 /* trans_buildframe
456 * Append a block of 'transparent' data to the output buffer,
457 * inverting the bytes.
458 * The output buffer must have sufficient length; count bytes
459 * starting at *out are safe and are verified to be present.
460 * parameters:
461 * in input buffer
462 * count number of bytes in input buffer
463 * iwb pointer to output buffer structure (write semaphore must be held)
464 * return value:
465 * position of end of packet in output buffer on success,
466 * -EAGAIN if write semaphore busy or buffer full
467 */
468
469 static inline int trans_buildframe(struct isowbuf_t *iwb,
470 unsigned char *in, int count)
471 {
472 int write;
473 unsigned char c;
474
475 if (unlikely(count <= 0))
476 return iwb->write;
477
478 if (isowbuf_freebytes(iwb) < count ||
479 !isowbuf_startwrite(iwb)) {
480 gig_dbg(DEBUG_ISO, "can't put %d bytes", count);
481 return -EAGAIN;
482 }
483
484 gig_dbg(DEBUG_STREAM, "put %d bytes", count);
485 write = iwb->write;
486 do {
487 c = bitrev8(*in++);
488 iwb->data[write++] = c;
489 write %= BAS_OUTBUFSIZE;
490 } while (--count > 0);
491 iwb->write = write;
492 iwb->idle = c;
493
494 return isowbuf_donewrite(iwb);
495 }
496
497 int gigaset_isoc_buildframe(struct bc_state *bcs, unsigned char *in, int len)
498 {
499 int result;
500
501 switch (bcs->proto2) {
502 case ISDN_PROTO_L2_HDLC:
503 result = hdlc_buildframe(bcs->hw.bas->isooutbuf, in, len);
504 gig_dbg(DEBUG_ISO, "%s: %d bytes HDLC -> %d",
505 __func__, len, result);
506 break;
507 default: /* assume transparent */
508 result = trans_buildframe(bcs->hw.bas->isooutbuf, in, len);
509 gig_dbg(DEBUG_ISO, "%s: %d bytes trans -> %d",
510 __func__, len, result);
511 }
512 return result;
513 }
514
515 /* hdlc_putbyte
516 * append byte c to current skb of B channel structure *bcs, updating fcs
517 */
518 static inline void hdlc_putbyte(unsigned char c, struct bc_state *bcs)
519 {
520 bcs->fcs = crc_ccitt_byte(bcs->fcs, c);
521 if (unlikely(bcs->skb == NULL)) {
522 /* skipping */
523 return;
524 }
525 if (unlikely(bcs->skb->len == SBUFSIZE)) {
526 dev_warn(bcs->cs->dev, "received oversized packet discarded\n");
527 bcs->hw.bas->giants++;
528 dev_kfree_skb_any(bcs->skb);
529 bcs->skb = NULL;
530 return;
531 }
532 *__skb_put(bcs->skb, 1) = c;
533 }
534
535 /* hdlc_flush
536 * drop partial HDLC data packet
537 */
538 static inline void hdlc_flush(struct bc_state *bcs)
539 {
540 /* clear skb or allocate new if not skipping */
541 if (likely(bcs->skb != NULL))
542 skb_trim(bcs->skb, 0);
543 else if (!bcs->ignore) {
544 if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
545 skb_reserve(bcs->skb, HW_HDR_LEN);
546 else
547 dev_err(bcs->cs->dev, "could not allocate skb\n");
548 }
549
550 /* reset packet state */
551 bcs->fcs = PPP_INITFCS;
552 }
553
554 /* hdlc_done
555 * process completed HDLC data packet
556 */
557 static inline void hdlc_done(struct bc_state *bcs)
558 {
559 struct sk_buff *procskb;
560
561 if (unlikely(bcs->ignore)) {
562 bcs->ignore--;
563 hdlc_flush(bcs);
564 return;
565 }
566
567 if ((procskb = bcs->skb) == NULL) {
568 /* previous error */
569 gig_dbg(DEBUG_ISO, "%s: skb=NULL", __func__);
570 gigaset_rcv_error(NULL, bcs->cs, bcs);
571 } else if (procskb->len < 2) {
572 dev_notice(bcs->cs->dev, "received short frame (%d octets)\n",
573 procskb->len);
574 bcs->hw.bas->runts++;
575 gigaset_rcv_error(procskb, bcs->cs, bcs);
576 } else if (bcs->fcs != PPP_GOODFCS) {
577 dev_notice(bcs->cs->dev, "frame check error (0x%04x)\n",
578 bcs->fcs);
579 bcs->hw.bas->fcserrs++;
580 gigaset_rcv_error(procskb, bcs->cs, bcs);
581 } else {
582 procskb->len -= 2; /* subtract FCS */
583 procskb->tail -= 2;
584 gig_dbg(DEBUG_ISO, "%s: good frame (%d octets)",
585 __func__, procskb->len);
586 dump_bytes(DEBUG_STREAM,
587 "rcv data", procskb->data, procskb->len);
588 bcs->hw.bas->goodbytes += procskb->len;
589 gigaset_rcv_skb(procskb, bcs->cs, bcs);
590 }
591
592 if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
593 skb_reserve(bcs->skb, HW_HDR_LEN);
594 else
595 dev_err(bcs->cs->dev, "could not allocate skb\n");
596 bcs->fcs = PPP_INITFCS;
597 }
598
599 /* hdlc_frag
600 * drop HDLC data packet with non-integral last byte
601 */
602 static inline void hdlc_frag(struct bc_state *bcs, unsigned inbits)
603 {
604 if (unlikely(bcs->ignore)) {
605 bcs->ignore--;
606 hdlc_flush(bcs);
607 return;
608 }
609
610 dev_notice(bcs->cs->dev, "received partial byte (%d bits)\n", inbits);
611 bcs->hw.bas->alignerrs++;
612 gigaset_rcv_error(bcs->skb, bcs->cs, bcs);
613
614 if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
615 skb_reserve(bcs->skb, HW_HDR_LEN);
616 else
617 dev_err(bcs->cs->dev, "could not allocate skb\n");
618 bcs->fcs = PPP_INITFCS;
619 }
620
621 /* bit counts lookup table for HDLC bit unstuffing
622 * index: input byte
623 * value: bit 0..3 = number of consecutive '1' bits starting from LSB
624 * bit 4..6 = number of consecutive '1' bits starting from MSB
625 * (replacing 8 by 7 to make it fit; the algorithm won't care)
626 * bit 7 set if there are 5 or more "interior" consecutive '1' bits
627 */
628 static const unsigned char bitcounts[256] = {
629 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
630 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x05,
631 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
632 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x80, 0x06,
633 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
634 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x05,
635 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
636 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x80, 0x81, 0x80, 0x07,
637 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x14,
638 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x15,
639 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x14,
640 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x90, 0x16,
641 0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x23, 0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x24,
642 0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x23, 0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x25,
643 0x30, 0x31, 0x30, 0x32, 0x30, 0x31, 0x30, 0x33, 0x30, 0x31, 0x30, 0x32, 0x30, 0x31, 0x30, 0x34,
644 0x40, 0x41, 0x40, 0x42, 0x40, 0x41, 0x40, 0x43, 0x50, 0x51, 0x50, 0x52, 0x60, 0x61, 0x70, 0x78
645 };
646
647 /* hdlc_unpack
648 * perform HDLC frame processing (bit unstuffing, flag detection, FCS calculation)
649 * on a sequence of received data bytes (8 bits each, LSB first)
650 * pass on successfully received, complete frames as SKBs via gigaset_rcv_skb
651 * notify of errors via gigaset_rcv_error
652 * tally frames, errors etc. in BC structure counters
653 * parameters:
654 * src received data
655 * count number of received bytes
656 * bcs receiving B channel structure
657 */
658 static inline void hdlc_unpack(unsigned char *src, unsigned count,
659 struct bc_state *bcs)
660 {
661 struct bas_bc_state *ubc = bcs->hw.bas;
662 int inputstate;
663 unsigned seqlen, inbyte, inbits;
664
665 /* load previous state:
666 * inputstate = set of flag bits:
667 * - INS_flag_hunt: no complete opening flag received since connection setup or last abort
668 * - INS_have_data: at least one complete data byte received since last flag
669 * seqlen = number of consecutive '1' bits in last 7 input stream bits (0..7)
670 * inbyte = accumulated partial data byte (if !INS_flag_hunt)
671 * inbits = number of valid bits in inbyte, starting at LSB (0..6)
672 */
673 inputstate = bcs->inputstate;
674 seqlen = ubc->seqlen;
675 inbyte = ubc->inbyte;
676 inbits = ubc->inbits;
677
678 /* bit unstuffing a byte a time
679 * Take your time to understand this; it's straightforward but tedious.
680 * The "bitcounts" lookup table is used to speed up the counting of
681 * leading and trailing '1' bits.
682 */
683 while (count--) {
684 unsigned char c = *src++;
685 unsigned char tabentry = bitcounts[c];
686 unsigned lead1 = tabentry & 0x0f;
687 unsigned trail1 = (tabentry >> 4) & 0x0f;
688
689 seqlen += lead1;
690
691 if (unlikely(inputstate & INS_flag_hunt)) {
692 if (c == PPP_FLAG) {
693 /* flag-in-one */
694 inputstate &= ~(INS_flag_hunt | INS_have_data);
695 inbyte = 0;
696 inbits = 0;
697 } else if (seqlen == 6 && trail1 != 7) {
698 /* flag completed & not followed by abort */
699 inputstate &= ~(INS_flag_hunt | INS_have_data);
700 inbyte = c >> (lead1 + 1);
701 inbits = 7 - lead1;
702 if (trail1 >= 8) {
703 /* interior stuffing: omitting the MSB handles most cases */
704 inbits--;
705 /* correct the incorrectly handled cases individually */
706 switch (c) {
707 case 0xbe:
708 inbyte = 0x3f;
709 break;
710 }
711 }
712 }
713 /* else: continue flag-hunting */
714 } else if (likely(seqlen < 5 && trail1 < 7)) {
715 /* streamlined case: 8 data bits, no stuffing */
716 inbyte |= c << inbits;
717 hdlc_putbyte(inbyte & 0xff, bcs);
718 inputstate |= INS_have_data;
719 inbyte >>= 8;
720 /* inbits unchanged */
721 } else if (likely(seqlen == 6 && inbits == 7 - lead1 &&
722 trail1 + 1 == inbits &&
723 !(inputstate & INS_have_data))) {
724 /* streamlined case: flag idle - state unchanged */
725 } else if (unlikely(seqlen > 6)) {
726 /* abort sequence */
727 ubc->aborts++;
728 hdlc_flush(bcs);
729 inputstate |= INS_flag_hunt;
730 } else if (seqlen == 6) {
731 /* closing flag, including (6 - lead1) '1's and one '' from inbits */
732 if (inbits > 7 - lead1) {
733 hdlc_frag(bcs, inbits + lead1 - 7);
734 inputstate &= ~INS_have_data;
735 } else {
736 if (inbits < 7 - lead1)
737 ubc->stolen0s ++;
738 if (inputstate & INS_have_data) {
739 hdlc_done(bcs);
740 inputstate &= ~INS_have_data;
741 }
742 }
743
744 if (c == PPP_FLAG) {
745 /* complete flag, LSB overlaps preceding flag */
746 ubc->shared0s ++;
747 inbits = 0;
748 inbyte = 0;
749 } else if (trail1 != 7) {
750 /* remaining bits */
751 inbyte = c >> (lead1 + 1);
752 inbits = 7 - lead1;
753 if (trail1 >= 8) {
754 /* interior stuffing: omitting the MSB handles most cases */
755 inbits--;
756 /* correct the incorrectly handled cases individually */
757 switch (c) {
758 case 0xbe:
759 inbyte = 0x3f;
760 break;
761 }
762 }
763 } else {
764 /* abort sequence follows, skb already empty anyway */
765 ubc->aborts++;
766 inputstate |= INS_flag_hunt;
767 }
768 } else { /* (seqlen < 6) && (seqlen == 5 || trail1 >= 7) */
769
770 if (c == PPP_FLAG) {
771 /* complete flag */
772 if (seqlen == 5)
773 ubc->stolen0s++;
774 if (inbits) {
775 hdlc_frag(bcs, inbits);
776 inbits = 0;
777 inbyte = 0;
778 } else if (inputstate & INS_have_data)
779 hdlc_done(bcs);
780 inputstate &= ~INS_have_data;
781 } else if (trail1 == 7) {
782 /* abort sequence */
783 ubc->aborts++;
784 hdlc_flush(bcs);
785 inputstate |= INS_flag_hunt;
786 } else {
787 /* stuffed data */
788 if (trail1 < 7) { /* => seqlen == 5 */
789 /* stuff bit at position lead1, no interior stuffing */
790 unsigned char mask = (1 << lead1) - 1;
791 c = (c & mask) | ((c & ~mask) >> 1);
792 inbyte |= c << inbits;
793 inbits += 7;
794 } else if (seqlen < 5) { /* trail1 >= 8 */
795 /* interior stuffing: omitting the MSB handles most cases */
796 /* correct the incorrectly handled cases individually */
797 switch (c) {
798 case 0xbe:
799 c = 0x7e;
800 break;
801 }
802 inbyte |= c << inbits;
803 inbits += 7;
804 } else { /* seqlen == 5 && trail1 >= 8 */
805
806 /* stuff bit at lead1 *and* interior stuffing */
807 switch (c) { /* unstuff individually */
808 case 0x7d:
809 c = 0x3f;
810 break;
811 case 0xbe:
812 c = 0x3f;
813 break;
814 case 0x3e:
815 c = 0x1f;
816 break;
817 case 0x7c:
818 c = 0x3e;
819 break;
820 }
821 inbyte |= c << inbits;
822 inbits += 6;
823 }
824 if (inbits >= 8) {
825 inbits -= 8;
826 hdlc_putbyte(inbyte & 0xff, bcs);
827 inputstate |= INS_have_data;
828 inbyte >>= 8;
829 }
830 }
831 }
832 seqlen = trail1 & 7;
833 }
834
835 /* save new state */
836 bcs->inputstate = inputstate;
837 ubc->seqlen = seqlen;
838 ubc->inbyte = inbyte;
839 ubc->inbits = inbits;
840 }
841
842 /* trans_receive
843 * pass on received USB frame transparently as SKB via gigaset_rcv_skb
844 * invert bytes
845 * tally frames, errors etc. in BC structure counters
846 * parameters:
847 * src received data
848 * count number of received bytes
849 * bcs receiving B channel structure
850 */
851 static inline void trans_receive(unsigned char *src, unsigned count,
852 struct bc_state *bcs)
853 {
854 struct sk_buff *skb;
855 int dobytes;
856 unsigned char *dst;
857
858 if (unlikely(bcs->ignore)) {
859 bcs->ignore--;
860 hdlc_flush(bcs);
861 return;
862 }
863 if (unlikely((skb = bcs->skb) == NULL)) {
864 bcs->skb = skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN);
865 if (!skb) {
866 dev_err(bcs->cs->dev, "could not allocate skb\n");
867 return;
868 }
869 skb_reserve(skb, HW_HDR_LEN);
870 }
871 bcs->hw.bas->goodbytes += skb->len;
872 dobytes = TRANSBUFSIZE - skb->len;
873 while (count > 0) {
874 dst = skb_put(skb, count < dobytes ? count : dobytes);
875 while (count > 0 && dobytes > 0) {
876 *dst++ = bitrev8(*src++);
877 count--;
878 dobytes--;
879 }
880 if (dobytes == 0) {
881 gigaset_rcv_skb(skb, bcs->cs, bcs);
882 bcs->skb = skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN);
883 if (!skb) {
884 dev_err(bcs->cs->dev,
885 "could not allocate skb\n");
886 return;
887 }
888 skb_reserve(bcs->skb, HW_HDR_LEN);
889 dobytes = TRANSBUFSIZE;
890 }
891 }
892 }
893
894 void gigaset_isoc_receive(unsigned char *src, unsigned count, struct bc_state *bcs)
895 {
896 switch (bcs->proto2) {
897 case ISDN_PROTO_L2_HDLC:
898 hdlc_unpack(src, count, bcs);
899 break;
900 default: /* assume transparent */
901 trans_receive(src, count, bcs);
902 }
903 }
904
905 /* == data input =========================================================== */
906
907 static void cmd_loop(unsigned char *src, int numbytes, struct inbuf_t *inbuf)
908 {
909 struct cardstate *cs = inbuf->cs;
910 unsigned cbytes = cs->cbytes;
911
912 while (numbytes--) {
913 /* copy next character, check for end of line */
914 switch (cs->respdata[cbytes] = *src++) {
915 case '\r':
916 case '\n':
917 /* end of line */
918 gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)",
919 __func__, cbytes);
920 if (cbytes >= MAX_RESP_SIZE - 1)
921 dev_warn(cs->dev, "response too large\n");
922 cs->cbytes = cbytes;
923 gigaset_handle_modem_response(cs);
924 cbytes = 0;
925 break;
926 default:
927 /* advance in line buffer, checking for overflow */
928 if (cbytes < MAX_RESP_SIZE - 1)
929 cbytes++;
930 }
931 }
932
933 /* save state */
934 cs->cbytes = cbytes;
935 }
936
937
938 /* process a block of data received through the control channel
939 */
940 void gigaset_isoc_input(struct inbuf_t *inbuf)
941 {
942 struct cardstate *cs = inbuf->cs;
943 unsigned tail, head, numbytes;
944 unsigned char *src;
945
946 head = inbuf->head;
947 while (head != (tail = inbuf->tail)) {
948 gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
949 if (head > tail)
950 tail = RBUFSIZE;
951 src = inbuf->data + head;
952 numbytes = tail - head;
953 gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
954
955 if (cs->mstate == MS_LOCKED) {
956 gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
957 numbytes, src);
958 gigaset_if_receive(inbuf->cs, src, numbytes);
959 } else {
960 gigaset_dbg_buffer(DEBUG_CMD, "received response",
961 numbytes, src);
962 cmd_loop(src, numbytes, inbuf);
963 }
964
965 head += numbytes;
966 if (head == RBUFSIZE)
967 head = 0;
968 gig_dbg(DEBUG_INTR, "setting head to %u", head);
969 inbuf->head = head;
970 }
971 }
972
973
974 /* == data output ========================================================== */
975
976 /* gigaset_send_skb
977 * called by common.c to queue an skb for sending
978 * and start transmission if necessary
979 * parameters:
980 * B Channel control structure
981 * skb
982 * return value:
983 * number of bytes accepted for sending
984 * (skb->len if ok, 0 if out of buffer space)
985 * or error code (< 0, eg. -EINVAL)
986 */
987 int gigaset_isoc_send_skb(struct bc_state *bcs, struct sk_buff *skb)
988 {
989 int len = skb->len;
990 unsigned long flags;
991
992 spin_lock_irqsave(&bcs->cs->lock, flags);
993 if (!bcs->cs->connected) {
994 spin_unlock_irqrestore(&bcs->cs->lock, flags);
995 return -ENODEV;
996 }
997
998 skb_queue_tail(&bcs->squeue, skb);
999 gig_dbg(DEBUG_ISO, "%s: skb queued, qlen=%d",
1000 __func__, skb_queue_len(&bcs->squeue));
1001
1002 /* tasklet submits URB if necessary */
1003 tasklet_schedule(&bcs->hw.bas->sent_tasklet);
1004 spin_unlock_irqrestore(&bcs->cs->lock, flags);
1005
1006 return len; /* ok so far */
1007 }
1008
|
This page was automatically generated by the
LXR engine.
|