blob: f39ccdf87a17e816f92a43afd93484ad704a6f8c [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;
Tilman Schmidt7891adf2009-10-25 09:30:37 +000044 freebytes = read - write;
45 if (freebytes > 0) {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080046 /* 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
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080057/* start writing
58 * acquire the write semaphore
59 * return true if acquired, false if busy
60 */
61static inline int isowbuf_startwrite(struct isowbuf_t *iwb)
62{
63 if (!atomic_dec_and_test(&iwb->writesem)) {
64 atomic_inc(&iwb->writesem);
Tilman Schmidt784d5852006-04-10 22:55:04 -070065 gig_dbg(DEBUG_ISO, "%s: couldn't acquire iso write semaphore",
66 __func__);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080067 return 0;
68 }
Tilman Schmidt784d5852006-04-10 22:55:04 -070069 gig_dbg(DEBUG_ISO,
70 "%s: acquired iso write semaphore, data[write]=%02x, nbits=%d",
Tilman Schmidt9d4bee22008-02-06 01:38:28 -080071 __func__, iwb->data[iwb->write], iwb->wbits);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080072 return 1;
73}
74
75/* finish writing
Tilman Schmidt9d4bee22008-02-06 01:38:28 -080076 * release the write semaphore
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080077 * returns the current write position
78 */
79static inline int isowbuf_donewrite(struct isowbuf_t *iwb)
80{
Tilman Schmidt9d4bee22008-02-06 01:38:28 -080081 int write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080082 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 */
93static inline void isowbuf_putbits(struct isowbuf_t *iwb, u32 data, int nbits)
94{
Tilman Schmidt9d4bee22008-02-06 01:38:28 -080095 int write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -080096 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;
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800107 iwb->write = write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800108}
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 */
114static 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 */
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800121 write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800122 iwb->idle = iwb->data[write];
Tilman Schmidt784d5852006-04-10 22:55:04 -0700123 gig_dbg(DEBUG_ISO, "idle fill byte %02x", iwb->idle);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800124 /* 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 */
134int gigaset_isowbuf_getbytes(struct isowbuf_t *iwb, int size)
135{
136 int read, write, limit, src, dst;
137 unsigned char pbyte;
138
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800139 read = iwb->nextread;
140 write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800141 if (likely(read == write)) {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800142 /* return idle frame */
143 return read < BAS_OUTBUFPAD ?
Tilman Schmidt784d5852006-04-10 22:55:04 -0700144 BAS_OUTBUFSIZE : read - BAS_OUTBUFPAD;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800145 }
146
147 limit = read + size;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700148 gig_dbg(DEBUG_STREAM, "%s: read=%d write=%d limit=%d",
149 __func__, read, write, limit);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800150#ifdef CONFIG_GIGASET_DEBUG
151 if (unlikely(size < 0 || size > BAS_OUTBUFPAD)) {
Tilman Schmidtc8770dc2008-12-26 01:21:29 -0800152 pr_err("invalid size %d\n", size);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800153 return -EINVAL;
154 }
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800155#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 */
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800164 write = iwb->write;
165 if (limit >= write) {
Tilman Schmidt917f5082006-04-10 22:55:00 -0700166 pbyte = iwb->data[write]; /* save
167 partial byte */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800168 limit = write + BAS_OUTBUFPAD;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700169 gig_dbg(DEBUG_STREAM,
170 "%s: filling %d->%d with %02x",
171 __func__, write, limit, iwb->idle);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800172 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 }
Tilman Schmidt784d5852006-04-10 22:55:04 -0700182 gig_dbg(DEBUG_STREAM,
183 "%s: restoring %02x at %d",
184 __func__, pbyte, limit);
Tilman Schmidt917f5082006-04-10 22:55:00 -0700185 iwb->data[limit] = pbyte; /* restore
186 partial byte */
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800187 iwb->write = limit;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800188 }
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 }
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800207 iwb->nextread = limit;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800208 return read;
209}
210
211/* dump_bytes
212 * write hex bytes to syslog for debugging
213 */
214static inline void dump_bytes(enum debuglevel level, const char *tag,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700215 unsigned char *bytes, int count)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800216{
217#ifdef CONFIG_GIGASET_DEBUG
218 unsigned char c;
219 static char dbgline[3 * 32 + 1];
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800220 int i = 0;
Tilman Schmidt1315d692009-05-13 12:44:17 +0000221
222 if (!(gigaset_debuglevel & level))
223 return;
224
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800225 while (count-- > 0) {
226 if (i > sizeof(dbgline) - 4) {
227 dbgline[i] = '\0';
Tilman Schmidt784d5852006-04-10 22:55:04 -0700228 gig_dbg(level, "%s:%s", tag, dbgline);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800229 i = 0;
230 }
231 c = *bytes++;
232 dbgline[i] = (i && !(i % 12)) ? '-' : ' ';
233 i++;
Harvey Harrison02137f22008-07-30 16:40:22 -0700234 dbgline[i++] = hex_asc_hi(c);
235 dbgline[i++] = hex_asc_lo(c);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800236 }
237 dbgline[i] = '\0';
Tilman Schmidt784d5852006-04-10 22:55:04 -0700238 gig_dbg(level, "%s:%s", tag, dbgline);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800239#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 */
Tilman Schmidt35dc8452007-03-29 01:20:34 -0700251static const u16 stufftab[5 * 256] = {
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000252/* previous 1s = 0: */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800253 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
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000270/* previous 1s = 1: */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800271 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
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000288/* previous 1s = 2: */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800289 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
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000306/* previous 1s = 3: */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800307 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
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000324/* previous 1s = 4: */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800325 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
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000348 * iwb pointer to output buffer structure
349 * (write semaphore must be held)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800350 * return value:
351 * number of trailing '1' bits in result after this step
352 */
353
354static inline int hdlc_bitstuff_byte(struct isowbuf_t *iwb, unsigned char cin,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700355 int ones)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800356{
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
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000390 * iwb pointer to output buffer structure
391 * (write semaphore must be held)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800392 * return value:
393 * position of end of packet in output buffer on success,
394 * -EAGAIN if write semaphore busy or buffer full
395 */
396
397static inline int hdlc_buildframe(struct isowbuf_t *iwb,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700398 unsigned char *in, int count)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800399{
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)) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700407 gig_dbg(DEBUG_ISO, "%s: %d bytes free -> -EAGAIN",
408 __func__, isowbuf_freebytes(iwb));
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800409 return -EAGAIN;
410 }
411
Tilman Schmidtcd7f50e2009-10-06 12:18:56 +0000412 dump_bytes(DEBUG_STREAM_DUMP, "snd data", in, count);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800413
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
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000423 /* bitstuff and append FCS
424 * (complemented, least significant byte first) */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800425 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);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800432 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
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000443 * iwb pointer to output buffer structure
444 * (write semaphore must be held)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800445 * return value:
446 * position of end of packet in output buffer on success,
447 * -EAGAIN if write semaphore busy or buffer full
448 */
449
450static 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))
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800457 return iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800458
459 if (isowbuf_freebytes(iwb) < count ||
460 !isowbuf_startwrite(iwb)) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700461 gig_dbg(DEBUG_ISO, "can't put %d bytes", count);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800462 return -EAGAIN;
463 }
464
Tilman Schmidt784d5852006-04-10 22:55:04 -0700465 gig_dbg(DEBUG_STREAM, "put %d bytes", count);
Tilman Schmidtcd7f50e2009-10-06 12:18:56 +0000466 dump_bytes(DEBUG_STREAM_DUMP, "snd data", in, count);
467
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800468 write = iwb->write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800469 do {
Akinobu Mita17b3cff2006-12-08 02:36:30 -0800470 c = bitrev8(*in++);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800471 iwb->data[write++] = c;
472 write %= BAS_OUTBUFSIZE;
473 } while (--count > 0);
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800474 iwb->write = write;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800475 iwb->idle = c;
476
477 return isowbuf_donewrite(iwb);
478}
479
480int gigaset_isoc_buildframe(struct bc_state *bcs, unsigned char *in, int len)
481{
482 int result;
483
484 switch (bcs->proto2) {
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000485 case L2_HDLC:
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800486 result = hdlc_buildframe(bcs->hw.bas->isooutbuf, in, len);
Tilman Schmidt784d5852006-04-10 22:55:04 -0700487 gig_dbg(DEBUG_ISO, "%s: %d bytes HDLC -> %d",
488 __func__, len, result);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800489 break;
490 default: /* assume transparent */
491 result = trans_buildframe(bcs->hw.bas->isooutbuf, in, len);
Tilman Schmidt784d5852006-04-10 22:55:04 -0700492 gig_dbg(DEBUG_ISO, "%s: %d bytes trans -> %d",
493 __func__, len, result);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800494 }
495 return result;
496}
497
498/* hdlc_putbyte
499 * append byte c to current skb of B channel structure *bcs, updating fcs
500 */
501static inline void hdlc_putbyte(unsigned char c, struct bc_state *bcs)
502{
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000503 bcs->rx_fcs = crc_ccitt_byte(bcs->rx_fcs, c);
504 if (bcs->rx_skb == NULL)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800505 /* skipping */
506 return;
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000507 if (bcs->rx_skb->len >= bcs->rx_bufsize) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700508 dev_warn(bcs->cs->dev, "received oversized packet discarded\n");
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800509 bcs->hw.bas->giants++;
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000510 dev_kfree_skb_any(bcs->rx_skb);
511 bcs->rx_skb = NULL;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800512 return;
513 }
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000514 *__skb_put(bcs->rx_skb, 1) = c;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800515}
516
517/* hdlc_flush
518 * drop partial HDLC data packet
519 */
520static inline void hdlc_flush(struct bc_state *bcs)
521{
522 /* clear skb or allocate new if not skipping */
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000523 if (bcs->rx_skb != NULL)
524 skb_trim(bcs->rx_skb, 0);
525 else
526 gigaset_new_rx_skb(bcs);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800527
528 /* reset packet state */
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000529 bcs->rx_fcs = PPP_INITFCS;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800530}
531
532/* hdlc_done
533 * process completed HDLC data packet
534 */
535static inline void hdlc_done(struct bc_state *bcs)
536{
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000537 struct cardstate *cs = bcs->cs;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800538 struct sk_buff *procskb;
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000539 unsigned int len;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800540
541 if (unlikely(bcs->ignore)) {
542 bcs->ignore--;
543 hdlc_flush(bcs);
544 return;
545 }
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000546 procskb = bcs->rx_skb;
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000547 if (procskb == NULL) {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800548 /* previous error */
Tilman Schmidt784d5852006-04-10 22:55:04 -0700549 gig_dbg(DEBUG_ISO, "%s: skb=NULL", __func__);
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000550 gigaset_isdn_rcv_err(bcs);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800551 } else if (procskb->len < 2) {
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000552 dev_notice(cs->dev, "received short frame (%d octets)\n",
Tilman Schmidt784d5852006-04-10 22:55:04 -0700553 procskb->len);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800554 bcs->hw.bas->runts++;
Tilman Schmidt4dd82302009-10-25 09:29:57 +0000555 dev_kfree_skb_any(procskb);
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000556 gigaset_isdn_rcv_err(bcs);
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000557 } else if (bcs->rx_fcs != PPP_GOODFCS) {
558 dev_notice(cs->dev, "frame check error\n");
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800559 bcs->hw.bas->fcserrs++;
Tilman Schmidt4dd82302009-10-25 09:29:57 +0000560 dev_kfree_skb_any(procskb);
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000561 gigaset_isdn_rcv_err(bcs);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800562 } else {
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000563 len = procskb->len;
564 __skb_trim(procskb, len -= 2); /* subtract FCS */
565 gig_dbg(DEBUG_ISO, "%s: good frame (%d octets)", __func__, len);
Tilman Schmidtcd7f50e2009-10-06 12:18:56 +0000566 dump_bytes(DEBUG_STREAM_DUMP,
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000567 "rcv data", procskb->data, len);
568 bcs->hw.bas->goodbytes += len;
569 gigaset_skb_rcvd(bcs, procskb);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800570 }
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000571 gigaset_new_rx_skb(bcs);
572 bcs->rx_fcs = PPP_INITFCS;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800573}
574
575/* hdlc_frag
576 * drop HDLC data packet with non-integral last byte
577 */
578static 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
Tilman Schmidt784d5852006-04-10 22:55:04 -0700586 dev_notice(bcs->cs->dev, "received partial byte (%d bits)\n", inbits);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800587 bcs->hw.bas->alignerrs++;
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000588 gigaset_isdn_rcv_err(bcs);
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000589 __skb_trim(bcs->rx_skb, 0);
590 bcs->rx_fcs = PPP_INITFCS;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800591}
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 */
Tilman Schmidt35dc8452007-03-29 01:20:34 -0700600static const unsigned char bitcounts[256] = {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800601 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
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000620 * perform HDLC frame processing (bit unstuffing, flag detection, FCS
621 * calculation) on a sequence of received data bytes (8 bits each, LSB first)
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000622 * pass on successfully received, complete frames as SKBs via gigaset_skb_rcvd
623 * notify of errors via gigaset_isdn_rcv_err
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800624 * 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 */
630static inline void hdlc_unpack(unsigned char *src, unsigned count,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700631 struct bc_state *bcs)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800632{
Tilman Schmidtd48c7782006-04-10 22:55:08 -0700633 struct bas_bc_state *ubc = bcs->hw.bas;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800634 int inputstate;
635 unsigned seqlen, inbyte, inbits;
636
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800637 /* load previous state:
638 * inputstate = set of flag bits:
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000639 * - 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)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800645 * 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) {
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000678 /* interior stuffing:
679 * omitting the MSB handles most cases,
680 * correct the incorrectly handled
681 * cases individually */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800682 inbits--;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800683 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) {
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000708 /* closing flag, including (6 - lead1) '1's
709 * and one '0' from inbits */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800710 if (inbits > 7 - lead1) {
711 hdlc_frag(bcs, inbits + lead1 - 7);
712 inputstate &= ~INS_have_data;
713 } else {
714 if (inbits < 7 - lead1)
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000715 ubc->stolen0s++;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800716 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 */
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000724 ubc->shared0s++;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800725 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) {
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000732 /* interior stuffing:
733 * omitting the MSB handles most cases,
734 * correct the incorrectly handled
735 * cases individually */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800736 inbits--;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800737 switch (c) {
738 case 0xbe:
739 inbyte = 0x3f;
740 break;
741 }
742 }
743 } else {
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000744 /* abort sequence follows,
745 * skb already empty anyway */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800746 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 */
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000770 /* stuff bit at position lead1,
771 * no interior stuffing */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800772 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 */
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000777 /* interior stuffing:
778 * omitting the MSB handles most cases,
779 * correct the incorrectly handled
780 * cases individually */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800781 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
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000790 /* stuff bit at lead1 *and* interior
791 * stuffing -- unstuff individually */
792 switch (c) {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800793 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
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000828 * pass on received USB frame transparently as SKB via gigaset_skb_rcvd
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800829 * 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 */
836static inline void trans_receive(unsigned char *src, unsigned count,
Tilman Schmidt784d5852006-04-10 22:55:04 -0700837 struct bc_state *bcs)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800838{
839 struct sk_buff *skb;
840 int dobytes;
841 unsigned char *dst;
842
843 if (unlikely(bcs->ignore)) {
844 bcs->ignore--;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800845 return;
846 }
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000847 skb = bcs->rx_skb;
Tilman Schmidtf3d531b2010-09-30 13:34:51 +0000848 if (skb == NULL) {
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000849 skb = gigaset_new_rx_skb(bcs);
Tilman Schmidtf3d531b2010-09-30 13:34:51 +0000850 if (skb == NULL)
851 return;
852 }
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000853 dobytes = bcs->rx_bufsize - skb->len;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800854 while (count > 0) {
855 dst = skb_put(skb, count < dobytes ? count : dobytes);
856 while (count > 0 && dobytes > 0) {
Akinobu Mita17b3cff2006-12-08 02:36:30 -0800857 *dst++ = bitrev8(*src++);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800858 count--;
859 dobytes--;
860 }
861 if (dobytes == 0) {
Tilman Schmidtcd7f50e2009-10-06 12:18:56 +0000862 dump_bytes(DEBUG_STREAM_DUMP,
863 "rcv data", skb->data, skb->len);
Tilman Schmidtf3d531b2010-09-30 13:34:51 +0000864 bcs->hw.bas->goodbytes += skb->len;
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000865 gigaset_skb_rcvd(bcs, skb);
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000866 skb = gigaset_new_rx_skb(bcs);
867 if (skb == NULL)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800868 return;
Tilman Schmidte7752ee2010-06-21 13:54:19 +0000869 dobytes = bcs->rx_bufsize;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800870 }
871 }
872}
873
Tilman Schmidt7891adf2009-10-25 09:30:37 +0000874void gigaset_isoc_receive(unsigned char *src, unsigned count,
875 struct bc_state *bcs)
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800876{
877 switch (bcs->proto2) {
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000878 case L2_HDLC:
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800879 hdlc_unpack(src, count, bcs);
880 break;
881 default: /* assume transparent */
882 trans_receive(src, count, bcs);
883 }
884}
885
886/* == data input =========================================================== */
887
Tilman Schmidt63e055d2010-02-22 13:09:22 +0000888/* 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 */
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800894static void cmd_loop(unsigned char *src, int numbytes, struct inbuf_t *inbuf)
895{
896 struct cardstate *cs = inbuf->cs;
897 unsigned cbytes = cs->cbytes;
Tilman Schmidt63e055d2010-02-22 13:09:22 +0000898 unsigned char c;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800899
900 while (numbytes--) {
Tilman Schmidt63e055d2010-02-22 13:09:22 +0000901 c = *src++;
902 switch (c) {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800903 case '\n':
Tilman Schmidt63e055d2010-02-22 13:09:22 +0000904 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 }
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800917 cs->cbytes = cbytes;
Tilman Schmidt63e055d2010-02-22 13:09:22 +0000918 gigaset_dbg_buffer(DEBUG_TRANSCMD, "received response",
919 cbytes, cs->respdata);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800920 gigaset_handle_modem_response(cs);
921 cbytes = 0;
Tilman Schmidt63e055d2010-02-22 13:09:22 +0000922
923 /* store EOL byte for CRLF collapsing */
924 cs->respdata[0] = c;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800925 break;
926 default:
Tilman Schmidt63e055d2010-02-22 13:09:22 +0000927 /* append to line buffer if possible */
928 if (cbytes < MAX_RESP_SIZE)
929 cs->respdata[cbytes] = c;
930 cbytes++;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800931 }
932 }
933
934 /* save state */
935 cs->cbytes = cbytes;
936}
937
938
939/* process a block of data received through the control channel
940 */
941void gigaset_isoc_input(struct inbuf_t *inbuf)
942{
943 struct cardstate *cs = inbuf->cs;
944 unsigned tail, head, numbytes;
945 unsigned char *src;
946
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800947 head = inbuf->head;
948 while (head != (tail = inbuf->tail)) {
Tilman Schmidt784d5852006-04-10 22:55:04 -0700949 gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800950 if (head > tail)
951 tail = RBUFSIZE;
952 src = inbuf->data + head;
953 numbytes = tail - head;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700954 gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800955
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800956 if (cs->mstate == MS_LOCKED) {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800957 gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
Tilman Schmidt01371502006-04-10 22:55:11 -0700958 numbytes, src);
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800959 gigaset_if_receive(inbuf->cs, src, numbytes);
960 } else {
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800961 cmd_loop(src, numbytes, inbuf);
962 }
963
964 head += numbytes;
965 if (head == RBUFSIZE)
966 head = 0;
Tilman Schmidt784d5852006-04-10 22:55:04 -0700967 gig_dbg(DEBUG_INTR, "setting head to %u", head);
Tilman Schmidt9d4bee22008-02-06 01:38:28 -0800968 inbuf->head = head;
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800969 }
970}
971
972
973/* == data output ========================================================== */
974
Tilman Schmidt1cec9722009-10-06 12:19:01 +0000975/**
976 * gigaset_isoc_send_skb() - queue an skb for sending
977 * @bcs: B channel descriptor structure.
978 * @skb: data to send.
979 *
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000980 * Called by LL to queue an skb for sending, and start transmission if
Tilman Schmidt1cec9722009-10-06 12:19:01 +0000981 * necessary.
Tilman Schmidt088ec0c2009-10-06 12:19:07 +0000982 * Once the payload data has been transmitted completely, gigaset_skb_sent()
Tilman Schmidt4dd82302009-10-25 09:29:57 +0000983 * will be called with the skb's link layer header preserved.
Tilman Schmidt1cec9722009-10-06 12:19:01 +0000984 *
985 * Return value:
986 * number of bytes accepted for sending (skb->len) if ok,
987 * error code < 0 (eg. -ENODEV) on error
Hansjoerg Lipp76bb4682006-03-26 01:38:36 -0800988 */
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}