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 0 if acquired, <0 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 -EBUSY;
  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 0;
  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) < 0)
 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) < 0) {
 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) < 0) {
 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->rx_fcs = crc_ccitt_byte(bcs->rx_fcs, c);
 504         if (bcs->rx_skb == NULL)
 505                 /* skipping */
 506                 return;
 507         if (bcs->rx_skb->len >= bcs->rx_bufsize) {
 508                 dev_warn(bcs->cs->dev, "received oversized packet discarded\n");
 509                 bcs->hw.bas->giants++;
 510                 dev_kfree_skb_any(bcs->rx_skb);
 511                 bcs->rx_skb = NULL;
 512                 return;
 513         }
 514         __skb_put_u8(bcs->rx_skb, c);
 515 }
 516
 517 /* hdlc_flush
 518  * drop partial HDLC data packet
 519  */
 520 static inline void hdlc_flush(struct bc_state *bcs)
 521 {
 522         /* clear skb or allocate new if not skipping */
 523         if (bcs->rx_skb != NULL)
 524                 skb_trim(bcs->rx_skb, 0);
 525         else
 526                 gigaset_new_rx_skb(bcs);
 527
 528         /* reset packet state */
 529         bcs->rx_fcs = PPP_INITFCS;
 530 }
 531
 532 /* hdlc_done
 533  * process completed HDLC data packet
 534  */
 535 static inline void hdlc_done(struct bc_state *bcs)
 536 {
 537         struct cardstate *cs = bcs->cs;
 538         struct sk_buff *procskb;
 539         unsigned int len;
 540
 541         if (unlikely(bcs->ignore)) {
 542                 bcs->ignore--;
 543                 hdlc_flush(bcs);
 544                 return;
 545         }
 546         procskb = bcs->rx_skb;
 547         if (procskb == NULL) {
 548                 /* previous error */
 549                 gig_dbg(DEBUG_ISO, "%s: skb=NULL", __func__);
 550                 gigaset_isdn_rcv_err(bcs);
 551         } else if (procskb->len < 2) {
 552                 dev_notice(cs->dev, "received short frame (%d octets)\n",
 553                            procskb->len);
 554                 bcs->hw.bas->runts++;
 555                 dev_kfree_skb_any(procskb);
 556                 gigaset_isdn_rcv_err(bcs);
 557         } else if (bcs->rx_fcs != PPP_GOODFCS) {
 558                 dev_notice(cs->dev, "frame check error\n");
 559                 bcs->hw.bas->fcserrs++;
 560                 dev_kfree_skb_any(procskb);
 561                 gigaset_isdn_rcv_err(bcs);
 562         } else {
 563                 len = procskb->len;
 564                 __skb_trim(procskb, len -= 2);  /* subtract FCS */
 565                 gig_dbg(DEBUG_ISO, "%s: good frame (%d octets)", __func__, len);
 566                 dump_bytes(DEBUG_STREAM_DUMP,
 567                            "rcv data", procskb->data, len);
 568                 bcs->hw.bas->goodbytes += len;
 569                 gigaset_skb_rcvd(bcs, procskb);
 570         }
 571         gigaset_new_rx_skb(bcs);
 572         bcs->rx_fcs = PPP_INITFCS;
 573 }
 574
 575 /* hdlc_frag
 576  * drop HDLC data packet with non-integral last byte
 577  */
 578 static inline void hdlc_frag(struct bc_state *bcs, unsigned inbits)
 579 {
 580         if (unlikely(bcs->ignore)) {
 581                 bcs->ignore--;
 582                 hdlc_flush(bcs);
 583                 return;
 584         }
 585
 586         dev_notice(bcs->cs->dev, "received partial byte (%d bits)\n", inbits);
 587         bcs->hw.bas->alignerrs++;
 588         gigaset_isdn_rcv_err(bcs);
 589         __skb_trim(bcs->rx_skb, 0);
 590         bcs->rx_fcs = PPP_INITFCS;
 591 }
 592
 593 /* bit counts lookup table for HDLC bit unstuffing
 594  * index: input byte
 595  * value: bit 0..3 = number of consecutive '1' bits starting from LSB
 596  *        bit 4..6 = number of consecutive '1' bits starting from MSB
 597  *                   (replacing 8 by 7 to make it fit; the algorithm won't care)
 598  *        bit 7 set if there are 5 or more "interior" consecutive '1' bits
 599  */
 600 static const unsigned char bitcounts[256] = {
 601         0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
 602         0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x05,
 603         0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
 604         0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x80, 0x06,
 605         0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
 606         0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x05,
 607         0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x04,
 608         0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x02, 0x80, 0x81, 0x80, 0x07,
 609         0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x14,
 610         0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x15,
 611         0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x14,
 612         0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x10, 0x13, 0x10, 0x11, 0x10, 0x12, 0x10, 0x11, 0x90, 0x16,
 613         0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x23, 0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x24,
 614         0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x23, 0x20, 0x21, 0x20, 0x22, 0x20, 0x21, 0x20, 0x25,
 615         0x30, 0x31, 0x30, 0x32, 0x30, 0x31, 0x30, 0x33, 0x30, 0x31, 0x30, 0x32, 0x30, 0x31, 0x30, 0x34,
 616         0x40, 0x41, 0x40, 0x42, 0x40, 0x41, 0x40, 0x43, 0x50, 0x51, 0x50, 0x52, 0x60, 0x61, 0x70, 0x78
 617 };
 618
 619 /* hdlc_unpack
 620  * perform HDLC frame processing (bit unstuffing, flag detection, FCS
 621  * calculation) on a sequence of received data bytes (8 bits each, LSB first)
 622  * pass on successfully received, complete frames as SKBs via gigaset_skb_rcvd
 623  * notify of errors via gigaset_isdn_rcv_err
 624  * tally frames, errors etc. in BC structure counters
 625  * parameters:
 626  *      src     received data
 627  *      count   number of received bytes
 628  *      bcs     receiving B channel structure
 629  */
 630 static inline void hdlc_unpack(unsigned char *src, unsigned count,
 631                                struct bc_state *bcs)
 632 {
 633         struct bas_bc_state *ubc = bcs->hw.bas;
 634         int inputstate;
 635         unsigned seqlen, inbyte, inbits;
 636
 637         /* load previous state:
 638          * inputstate = set of flag bits:
 639          * - INS_flag_hunt: no complete opening flag received since connection
 640          *                  setup or last abort
 641          * - INS_have_data: at least one complete data byte received since last
 642          *                  flag
 643          * seqlen = number of consecutive '1' bits in last 7 input stream bits
 644          *          (0..7)
 645          * inbyte = accumulated partial data byte (if !INS_flag_hunt)
 646          * inbits = number of valid bits in inbyte, starting at LSB (0..6)
 647          */
 648         inputstate = bcs->inputstate;
 649         seqlen = ubc->seqlen;
 650         inbyte = ubc->inbyte;
 651         inbits = ubc->inbits;
 652
 653         /* bit unstuffing a byte a time
 654          * Take your time to understand this; it's straightforward but tedious.
 655          * The "bitcounts" lookup table is used to speed up the counting of
 656          * leading and trailing '1' bits.
 657          */
 658         while (count--) {
 659                 unsigned char c = *src++;
 660                 unsigned char tabentry = bitcounts[c];
 661                 unsigned lead1 = tabentry & 0x0f;
 662                 unsigned trail1 = (tabentry >> 4) & 0x0f;
 663
 664                 seqlen += lead1;
 665
 666                 if (unlikely(inputstate & INS_flag_hunt)) {
 667                         if (c == PPP_FLAG) {
 668                                 /* flag-in-one */
 669                                 inputstate &= ~(INS_flag_hunt | INS_have_data);
 670                                 inbyte = 0;
 671                                 inbits = 0;
 672                         } else if (seqlen == 6 && trail1 != 7) {
 673                                 /* flag completed & not followed by abort */
 674                                 inputstate &= ~(INS_flag_hunt | INS_have_data);
 675                                 inbyte = c >> (lead1 + 1);
 676                                 inbits = 7 - lead1;
 677                                 if (trail1 >= 8) {
 678                                         /* interior stuffing:
 679                                          * omitting the MSB handles most cases,
 680                                          * correct the incorrectly handled
 681                                          * cases individually */
 682                                         inbits--;
 683                                         switch (c) {
 684                                         case 0xbe:
 685                                                 inbyte = 0x3f;
 686                                                 break;
 687                                         }
 688                                 }
 689                         }
 690                         /* else: continue flag-hunting */
 691                 } else if (likely(seqlen < 5 && trail1 < 7)) {
 692                         /* streamlined case: 8 data bits, no stuffing */
 693                         inbyte |= c << inbits;
 694                         hdlc_putbyte(inbyte & 0xff, bcs);
 695                         inputstate |= INS_have_data;
 696                         inbyte >>= 8;
 697                         /* inbits unchanged */
 698                 } else if (likely(seqlen == 6 && inbits == 7 - lead1 &&
 699                                   trail1 + 1 == inbits &&
 700                                   !(inputstate & INS_have_data))) {
 701                         /* streamlined case: flag idle - state unchanged */
 702                 } else if (unlikely(seqlen > 6)) {
 703                         /* abort sequence */
 704                         ubc->aborts++;
 705                         hdlc_flush(bcs);
 706                         inputstate |= INS_flag_hunt;
 707                 } else if (seqlen == 6) {
 708                         /* closing flag, including (6 - lead1) '1's
 709                          * and one '0' from inbits */
 710                         if (inbits > 7 - lead1) {
 711                                 hdlc_frag(bcs, inbits + lead1 - 7);
 712                                 inputstate &= ~INS_have_data;
 713                         } else {
 714                                 if (inbits < 7 - lead1)
 715                                         ubc->stolen0s++;
 716                                 if (inputstate & INS_have_data) {
 717                                         hdlc_done(bcs);
 718                                         inputstate &= ~INS_have_data;
 719                                 }
 720                         }
 721
 722                         if (c == PPP_FLAG) {
 723                                 /* complete flag, LSB overlaps preceding flag */
 724                                 ubc->shared0s++;
 725                                 inbits = 0;
 726                                 inbyte = 0;
 727                         } else if (trail1 != 7) {
 728                                 /* remaining bits */
 729                                 inbyte = c >> (lead1 + 1);
 730                                 inbits = 7 - lead1;
 731                                 if (trail1 >= 8) {
 732                                         /* interior stuffing:
 733                                          * omitting the MSB handles most cases,
 734                                          * correct the incorrectly handled
 735                                          * cases individually */
 736                                         inbits--;
 737                                         switch (c) {
 738                                         case 0xbe:
 739                                                 inbyte = 0x3f;
 740                                                 break;
 741                                         }
 742                                 }
 743                         } else {
 744                                 /* abort sequence follows,
 745                                  * skb already empty anyway */
 746                                 ubc->aborts++;
 747                                 inputstate |= INS_flag_hunt;
 748                         }
 749                 } else { /* (seqlen < 6) && (seqlen == 5 || trail1 >= 7) */
 750
 751                         if (c == PPP_FLAG) {
 752                                 /* complete flag */
 753                                 if (seqlen == 5)
 754                                         ubc->stolen0s++;
 755                                 if (inbits) {
 756                                         hdlc_frag(bcs, inbits);
 757                                         inbits = 0;
 758                                         inbyte = 0;
 759                                 } else if (inputstate & INS_have_data)
 760                                         hdlc_done(bcs);
 761                                 inputstate &= ~INS_have_data;
 762                         } else if (trail1 == 7) {
 763                                 /* abort sequence */
 764                                 ubc->aborts++;
 765                                 hdlc_flush(bcs);
 766                                 inputstate |= INS_flag_hunt;
 767                         } else {
 768                                 /* stuffed data */
 769                                 if (trail1 < 7) { /* => seqlen == 5 */
 770                                         /* stuff bit at position lead1,
 771                                          * no interior stuffing */
 772                                         unsigned char mask = (1 << lead1) - 1;
 773                                         c = (c & mask) | ((c & ~mask) >> 1);
 774                                         inbyte |= c << inbits;
 775                                         inbits += 7;
 776                                 } else if (seqlen < 5) { /* trail1 >= 8 */
 777                                         /* interior stuffing:
 778                                          * omitting the MSB handles most cases,
 779                                          * correct the incorrectly handled
 780                                          * cases individually */
 781                                         switch (c) {
 782                                         case 0xbe:
 783                                                 c = 0x7e;
 784                                                 break;
 785                                         }
 786                                         inbyte |= c << inbits;
 787                                         inbits += 7;
 788                                 } else { /* seqlen == 5 && trail1 >= 8 */
 789
 790                                         /* stuff bit at lead1 *and* interior
 791                                          * stuffing -- unstuff individually */
 792                                         switch (c) {
 793                                         case 0x7d:
 794                                                 c = 0x3f;
 795                                                 break;
 796                                         case 0xbe:
 797                                                 c = 0x3f;
 798                                                 break;
 799                                         case 0x3e:
 800                                                 c = 0x1f;
 801                                                 break;
 802                                         case 0x7c:
 803                                                 c = 0x3e;
 804                                                 break;
 805                                         }
 806                                         inbyte |= c << inbits;
 807                                         inbits += 6;
 808                                 }
 809                                 if (inbits >= 8) {
 810                                         inbits -= 8;
 811                                         hdlc_putbyte(inbyte & 0xff, bcs);
 812                                         inputstate |= INS_have_data;
 813                                         inbyte >>= 8;
 814                                 }
 815                         }
 816                 }
 817                 seqlen = trail1 & 7;
 818         }
 819
 820         /* save new state */
 821         bcs->inputstate = inputstate;
 822         ubc->seqlen = seqlen;
 823         ubc->inbyte = inbyte;
 824         ubc->inbits = inbits;
 825 }
 826
 827 /* trans_receive
 828  * pass on received USB frame transparently as SKB via gigaset_skb_rcvd
 829  * invert bytes
 830  * tally frames, errors etc. in BC structure counters
 831  * parameters:
 832  *      src     received data
 833  *      count   number of received bytes
 834  *      bcs     receiving B channel structure
 835  */
 836 static inline void trans_receive(unsigned char *src, unsigned count,
 837                                  struct bc_state *bcs)
 838 {
 839         struct sk_buff *skb;
 840         int dobytes;
 841         unsigned char *dst;
 842
 843         if (unlikely(bcs->ignore)) {
 844                 bcs->ignore--;
 845                 return;
 846         }
 847         skb = bcs->rx_skb;
 848         if (skb == NULL) {
 849                 skb = gigaset_new_rx_skb(bcs);
 850                 if (skb == NULL)
 851                         return;
 852         }
 853         dobytes = bcs->rx_bufsize - skb->len;
 854         while (count > 0) {
 855                 dst = skb_put(skb, count < dobytes ? count : dobytes);
 856                 while (count > 0 && dobytes > 0) {
 857                         *dst++ = bitrev8(*src++);
 858                         count--;
 859                         dobytes--;
 860                 }
 861                 if (dobytes == 0) {
 862                         dump_bytes(DEBUG_STREAM_DUMP,
 863                                    "rcv data", skb->data, skb->len);
 864                         bcs->hw.bas->goodbytes += skb->len;
 865                         gigaset_skb_rcvd(bcs, skb);
 866                         skb = gigaset_new_rx_skb(bcs);
 867                         if (skb == NULL)
 868                                 return;
 869                         dobytes = bcs->rx_bufsize;
 870                 }
 871         }
 872 }
 873
 874 void gigaset_isoc_receive(unsigned char *src, unsigned count,
 875                           struct bc_state *bcs)
 876 {
 877         switch (bcs->proto2) {
 878         case L2_HDLC:
 879                 hdlc_unpack(src, count, bcs);
 880                 break;
 881         default:                /* assume transparent */
 882                 trans_receive(src, count, bcs);
 883         }
 884 }
 885
 886 /* == data input =========================================================== */
 887
 888 /* process a block of received bytes in command mode (mstate != MS_LOCKED)
 889  * Append received bytes to the command response buffer and forward them
 890  * line by line to the response handler.
 891  * Note: Received lines may be terminated by CR, LF, or CR LF, which will be
 892  * removed before passing the line to the response handler.
 893  */
 894 static void cmd_loop(unsigned char *src, int numbytes, struct inbuf_t *inbuf)
 895 {
 896         struct cardstate *cs = inbuf->cs;
 897         unsigned cbytes      = cs->cbytes;
 898         unsigned char c;
 899
 900         while (numbytes--) {
 901                 c = *src++;
 902                 switch (c) {
 903                 case '\n':
 904                         if (cbytes == 0 && cs->respdata[0] == '\r') {
 905                                 /* collapse LF with preceding CR */
 906                                 cs->respdata[0] = 0;
 907                                 break;
 908                         }
 909                         /* --v-- fall through --v-- */
 910                 case '\r':
 911                         /* end of message line, pass to response handler */
 912                         if (cbytes >= MAX_RESP_SIZE) {
 913                                 dev_warn(cs->dev, "response too large (%d)\n",
 914                                          cbytes);
 915                                 cbytes = MAX_RESP_SIZE;
 916                         }
 917                         cs->cbytes = cbytes;
 918                         gigaset_dbg_buffer(DEBUG_TRANSCMD, "received response",
 919                                            cbytes, cs->respdata);
 920                         gigaset_handle_modem_response(cs);
 921                         cbytes = 0;
 922
 923                         /* store EOL byte for CRLF collapsing */
 924                         cs->respdata[0] = c;
 925                         break;
 926                 default:
 927                         /* append to line buffer if possible */
 928                         if (cbytes < MAX_RESP_SIZE)
 929                                 cs->respdata[cbytes] = c;
 930                         cbytes++;
 931                 }
 932         }
 933
 934         /* save state */
 935         cs->cbytes = cbytes;
 936 }
 937
 938
 939 /* process a block of data received through the control channel
 940  */
 941 void gigaset_isoc_input(struct inbuf_t *inbuf)
 942 {
 943         struct cardstate *cs = inbuf->cs;
 944         unsigned tail, head, numbytes;
 945         unsigned char *src;
 946
 947         head = inbuf->head;
 948         while (head != (tail = inbuf->tail)) {
 949                 gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
 950                 if (head > tail)
 951                         tail = RBUFSIZE;
 952                 src = inbuf->data + head;
 953                 numbytes = tail - head;
 954                 gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
 955
 956                 if (cs->mstate == MS_LOCKED) {
 957                         gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
 958                                            numbytes, src);
 959                         gigaset_if_receive(inbuf->cs, src, numbytes);
 960                 } else {
 961                         cmd_loop(src, numbytes, inbuf);
 962                 }
 963
 964                 head += numbytes;
 965                 if (head == RBUFSIZE)
 966                         head = 0;
 967                 gig_dbg(DEBUG_INTR, "setting head to %u", head);
 968                 inbuf->head = head;
 969         }
 970 }
 971
 972
 973 /* == data output ========================================================== */
 974
 975 /**
 976  * gigaset_isoc_send_skb() - queue an skb for sending
 977  * @bcs:        B channel descriptor structure.
 978  * @skb:        data to send.
 979  *
 980  * Called by LL to queue an skb for sending, and start transmission if
 981  * necessary.
 982  * Once the payload data has been transmitted completely, gigaset_skb_sent()
 983  * will be called with the skb's link layer header preserved.
 984  *
 985  * Return value:
 986  *      number of bytes accepted for sending (skb->len) if ok,
 987  *      error code < 0 (eg. -ENODEV) on error
 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 }