drivers/isdn/gigaset/isocdata.c

   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  */
  24 void 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  */
  38 static 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  */
  60 static 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  */
  83 static 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  */
 103 static 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  */
 117 static 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  */
 138 static 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  */
 158 int 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  */
 243 static 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  */
 277 static 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
 379 static 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
 421 static 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
 473 static 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
 501 int 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  */
 522 static 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  */
 542 static 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  */
 561 static 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  */
 606 static 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  */
 632 static 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  */
 662 static 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  */
 855 static 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
 898 void 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
 911 static 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  */
 944 void 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  */
 991 int 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 }