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