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