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