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