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