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