blob: b171e75cb52e51f920125abc984648f22415e961 [file] [log] [blame]
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -08001/*
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 * =====================================================================
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080013 */
14
15#include "gigaset.h"
16#include <linux/crc-ccitt.h>
Akinobu Mita17b3cff2006-12-08 02:36:30 -080017#include <linux/bitrev.h>
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080018
19/* access methods for isowbuf_t */
20/* ============================ */
21
22/* initialize buffer structure
23 */
24void gigaset_isowbuf_init(struct isowbuf_t *iwb, unsigned char idle)
25{
Tilman Schmidt9d4bee22008-02-06 01:38:28 -080026 iwb->read = 0;
27 iwb->nextread = 0;
28 iwb->write = 0;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080029 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 */
38static inline int isowbuf_freebytes(struct isowbuf_t *iwb)
39{
40 int read, write, freebytes;
41
Tilman Schmidt9d4bee22008-02-06 01:38:28 -080042 read = iwb->read;
43 write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080044 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 */
60static inline int isowbuf_poscmp(struct isowbuf_t *iwb, int a, int b)
61{
62 int read;
63 if (a == b)
64 return 0;
Tilman Schmidt9d4bee22008-02-06 01:38:28 -080065 read = iwb->read;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080066 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 */
83static inline int isowbuf_startwrite(struct isowbuf_t *iwb)
84{
85 if (!atomic_dec_and_test(&iwb->writesem)) {
86 atomic_inc(&iwb->writesem);
Tilman Schmidt784d5852006-04-10 22:55:04 -070087 gig_dbg(DEBUG_ISO, "%s: couldn't acquire iso write semaphore",
88 __func__);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080089 return 0;
90 }
Tilman Schmidt784d5852006-04-10 22:55:04 -070091 gig_dbg(DEBUG_ISO,
92 "%s: acquired iso write semaphore, data[write]=%02x, nbits=%d",
Tilman Schmidt9d4bee22008-02-06 01:38:28 -080093 __func__, iwb->data[iwb->write], iwb->wbits);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080094 return 1;
95}
96
97/* finish writing
Tilman Schmidt9d4bee22008-02-06 01:38:28 -080098 * release the write semaphore
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080099 * returns the current write position
100 */
101static inline int isowbuf_donewrite(struct isowbuf_t *iwb)
102{
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800103 int write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800104 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 */
115static inline void isowbuf_putbits(struct isowbuf_t *iwb, u32 data, int nbits)
116{
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800117 int write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800118 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;
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800129 iwb->write = write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800130}
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 */
136static 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 */
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800143 write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800144 iwb->idle = iwb->data[write];
Tilman Schmidt784d5852006-04-10 22:55:04 -0700145 gig_dbg(DEBUG_ISO, "idle fill byte %02x", iwb->idle);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800146 /* 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 */
156int gigaset_isowbuf_getbytes(struct isowbuf_t *iwb, int size)
157{
158 int read, write, limit, src, dst;
159 unsigned char pbyte;
160
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800161 read = iwb->nextread;
162 write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800163 if (likely(read == write)) {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800164 /* return idle frame */
165 return read < BAS_OUTBUFPAD ?
Tilman Schmidt784d5852006-04-10 22:55:04 -0700166 BAS_OUTBUFSIZE : read - BAS_OUTBUFPAD;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800167 }
168
169 limit = read + size;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700170 gig_dbg(DEBUG_STREAM, "%s: read=%d write=%d limit=%d",
171 __func__, read, write, limit);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800172#ifdef CONFIG_GIGASET_DEBUG
173 if (unlikely(size < 0 || size > BAS_OUTBUFPAD)) {
Tilman Schmidtc8770dc2008-12-26 01:21:29 -0800174 pr_err("invalid size %d\n", size);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800175 return -EINVAL;
176 }
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800177 src = iwb->read;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800178 if (unlikely(limit > BAS_OUTBUFSIZE + BAS_OUTBUFPAD ||
179 (read < src && limit >= src))) {
Tilman Schmidtc8770dc2008-12-26 01:21:29 -0800180 pr_err("isoc write buffer frame reservation violated\n");
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800181 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 */
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800192 write = iwb->write;
193 if (limit >= write) {
Tilman Schmidt917f5082006-04-10 22:55:00 -0700194 pbyte = iwb->data[write]; /* save
195 partial byte */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800196 limit = write + BAS_OUTBUFPAD;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700197 gig_dbg(DEBUG_STREAM,
198 "%s: filling %d->%d with %02x",
199 __func__, write, limit, iwb->idle);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800200 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 }
Tilman Schmidt784d5852006-04-10 22:55:04 -0700210 gig_dbg(DEBUG_STREAM,
211 "%s: restoring %02x at %d",
212 __func__, pbyte, limit);
Tilman Schmidt917f5082006-04-10 22:55:00 -0700213 iwb->data[limit] = pbyte; /* restore
214 partial byte */
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800215 iwb->write = limit;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800216 }
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 }
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800235 iwb->nextread = limit;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800236 return read;
237}
238
239/* dump_bytes
240 * write hex bytes to syslog for debugging
241 */
242static inline void dump_bytes(enum debuglevel level, const char *tag,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700243 unsigned char *bytes, int count)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800244{
245#ifdef CONFIG_GIGASET_DEBUG
246 unsigned char c;
247 static char dbgline[3 * 32 + 1];
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800248 int i = 0;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800249 while (count-- > 0) {
250 if (i > sizeof(dbgline) - 4) {
251 dbgline[i] = '\0';
Tilman Schmidt784d5852006-04-10 22:55:04 -0700252 gig_dbg(level, "%s:%s", tag, dbgline);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800253 i = 0;
254 }
255 c = *bytes++;
256 dbgline[i] = (i && !(i % 12)) ? '-' : ' ';
257 i++;
Harvey Harrison02137f22008-07-30 16:40:22 -0700258 dbgline[i++] = hex_asc_hi(c);
259 dbgline[i++] = hex_asc_lo(c);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800260 }
261 dbgline[i] = '\0';
Tilman Schmidt784d5852006-04-10 22:55:04 -0700262 gig_dbg(level, "%s:%s", tag, dbgline);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800263#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 */
Tilman Schmidt35dc8452007-03-29 01:20:34 -0700275static const u16 stufftab[5 * 256] = {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800276// 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
377static inline int hdlc_bitstuff_byte(struct isowbuf_t *iwb, unsigned char cin,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700378 int ones)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800379{
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
419static inline int hdlc_buildframe(struct isowbuf_t *iwb,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700420 unsigned char *in, int count)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800421{
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)) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700429 gig_dbg(DEBUG_ISO, "%s: %d bytes free -> -EAGAIN",
430 __func__, isowbuf_freebytes(iwb));
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800431 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
471static 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))
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800478 return iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800479
480 if (isowbuf_freebytes(iwb) < count ||
481 !isowbuf_startwrite(iwb)) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700482 gig_dbg(DEBUG_ISO, "can't put %d bytes", count);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800483 return -EAGAIN;
484 }
485
Tilman Schmidt784d5852006-04-10 22:55:04 -0700486 gig_dbg(DEBUG_STREAM, "put %d bytes", count);
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800487 write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800488 do {
Akinobu Mita17b3cff2006-12-08 02:36:30 -0800489 c = bitrev8(*in++);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800490 iwb->data[write++] = c;
491 write %= BAS_OUTBUFSIZE;
492 } while (--count > 0);
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800493 iwb->write = write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800494 iwb->idle = c;
495
496 return isowbuf_donewrite(iwb);
497}
498
499int 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);
Tilman Schmidt784d5852006-04-10 22:55:04 -0700506 gig_dbg(DEBUG_ISO, "%s: %d bytes HDLC -> %d",
507 __func__, len, result);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800508 break;
509 default: /* assume transparent */
510 result = trans_buildframe(bcs->hw.bas->isooutbuf, in, len);
Tilman Schmidt784d5852006-04-10 22:55:04 -0700511 gig_dbg(DEBUG_ISO, "%s: %d bytes trans -> %d",
512 __func__, len, result);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800513 }
514 return result;
515}
516
517/* hdlc_putbyte
518 * append byte c to current skb of B channel structure *bcs, updating fcs
519 */
520static 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)) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700528 dev_warn(bcs->cs->dev, "received oversized packet discarded\n");
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800529 bcs->hw.bas->giants++;
530 dev_kfree_skb_any(bcs->skb);
531 bcs->skb = NULL;
532 return;
533 }
Tilman Schmidt443e1f42006-04-10 22:55:13 -0700534 *__skb_put(bcs->skb, 1) = c;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800535}
536
537/* hdlc_flush
538 * drop partial HDLC data packet
539 */
540static 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
Tilman Schmidt784d5852006-04-10 22:55:04 -0700549 dev_err(bcs->cs->dev, "could not allocate skb\n");
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800550 }
551
552 /* reset packet state */
553 bcs->fcs = PPP_INITFCS;
554}
555
556/* hdlc_done
557 * process completed HDLC data packet
558 */
559static 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 */
Tilman Schmidt784d5852006-04-10 22:55:04 -0700571 gig_dbg(DEBUG_ISO, "%s: skb=NULL", __func__);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800572 gigaset_rcv_error(NULL, bcs->cs, bcs);
573 } else if (procskb->len < 2) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700574 dev_notice(bcs->cs->dev, "received short frame (%d octets)\n",
575 procskb->len);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800576 bcs->hw.bas->runts++;
577 gigaset_rcv_error(procskb, bcs->cs, bcs);
578 } else if (bcs->fcs != PPP_GOODFCS) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700579 dev_notice(bcs->cs->dev, "frame check error (0x%04x)\n",
580 bcs->fcs);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800581 bcs->hw.bas->fcserrs++;
582 gigaset_rcv_error(procskb, bcs->cs, bcs);
583 } else {
584 procskb->len -= 2; /* subtract FCS */
585 procskb->tail -= 2;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700586 gig_dbg(DEBUG_ISO, "%s: good frame (%d octets)",
587 __func__, procskb->len);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800588 dump_bytes(DEBUG_STREAM,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700589 "rcv data", procskb->data, procskb->len);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800590 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
Tilman Schmidt784d5852006-04-10 22:55:04 -0700597 dev_err(bcs->cs->dev, "could not allocate skb\n");
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800598 bcs->fcs = PPP_INITFCS;
599}
600
601/* hdlc_frag
602 * drop HDLC data packet with non-integral last byte
603 */
604static 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
Tilman Schmidt784d5852006-04-10 22:55:04 -0700612 dev_notice(bcs->cs->dev, "received partial byte (%d bits)\n", inbits);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800613 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
Tilman Schmidt784d5852006-04-10 22:55:04 -0700619 dev_err(bcs->cs->dev, "could not allocate skb\n");
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800620 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 */
Tilman Schmidt35dc8452007-03-29 01:20:34 -0700630static const unsigned char bitcounts[256] = {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800631 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 */
660static inline void hdlc_unpack(unsigned char *src, unsigned count,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700661 struct bc_state *bcs)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800662{
Tilman Schmidtd48c7782006-04-10 22:55:08 -0700663 struct bas_bc_state *ubc = bcs->hw.bas;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800664 int inputstate;
665 unsigned seqlen, inbyte, inbits;
666
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800667 /* 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 */
853static inline void trans_receive(unsigned char *src, unsigned count,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700854 struct bc_state *bcs)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800855{
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) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700868 dev_err(bcs->cs->dev, "could not allocate skb\n");
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800869 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) {
Akinobu Mita17b3cff2006-12-08 02:36:30 -0800878 *dst++ = bitrev8(*src++);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800879 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) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700886 dev_err(bcs->cs->dev,
887 "could not allocate skb\n");
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800888 return;
889 }
890 skb_reserve(bcs->skb, HW_HDR_LEN);
891 dobytes = TRANSBUFSIZE;
892 }
893 }
894}
895
896void 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
909static 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 */
Tilman Schmidt784d5852006-04-10 22:55:04 -0700920 gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)",
921 __func__, cbytes);
Tilman Schmidt92ba0ee2007-02-12 00:52:26 -0800922 if (cbytes >= MAX_RESP_SIZE - 1)
923 dev_warn(cs->dev, "response too large\n");
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800924 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++;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800932 }
933 }
934
935 /* save state */
936 cs->cbytes = cbytes;
937}
938
939
940/* process a block of data received through the control channel
941 */
942void gigaset_isoc_input(struct inbuf_t *inbuf)
943{
944 struct cardstate *cs = inbuf->cs;
945 unsigned tail, head, numbytes;
946 unsigned char *src;
947
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800948 head = inbuf->head;
949 while (head != (tail = inbuf->tail)) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700950 gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800951 if (head > tail)
952 tail = RBUFSIZE;
953 src = inbuf->data + head;
954 numbytes = tail - head;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700955 gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800956
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800957 if (cs->mstate == MS_LOCKED) {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800958 gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
Tilman Schmidt01371502006-04-10 22:55:11 -0700959 numbytes, src);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800960 gigaset_if_receive(inbuf->cs, src, numbytes);
961 } else {
962 gigaset_dbg_buffer(DEBUG_CMD, "received response",
Tilman Schmidt01371502006-04-10 22:55:11 -0700963 numbytes, src);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800964 cmd_loop(src, numbytes, inbuf);
965 }
966
967 head += numbytes;
968 if (head == RBUFSIZE)
969 head = 0;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700970 gig_dbg(DEBUG_INTR, "setting head to %u", head);
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800971 inbuf->head = head;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800972 }
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 */
989int gigaset_isoc_send_skb(struct bc_state *bcs, struct sk_buff *skb)
990{
Tilman Schmidtd48c7782006-04-10 22:55:08 -0700991 int len = skb->len;
Tilman Schmidt69049cc2006-04-10 22:55:16 -0700992 unsigned long flags;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800993
Tilman Schmidt73a88812006-04-22 02:35:30 -0700994 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
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -08001000 skb_queue_tail(&bcs->squeue, skb);
Tilman Schmidt784d5852006-04-10 22:55:04 -07001001 gig_dbg(DEBUG_ISO, "%s: skb queued, qlen=%d",
1002 __func__, skb_queue_len(&bcs->squeue));
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -08001003
1004 /* tasklet submits URB if necessary */
Tilman Schmidt73a88812006-04-22 02:35:30 -07001005 tasklet_schedule(&bcs->hw.bas->sent_tasklet);
Tilman Schmidt69049cc2006-04-10 22:55:16 -07001006 spin_unlock_irqrestore(&bcs->cs->lock, flags);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -08001007
1008 return len; /* ok so far */
1009}