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