drivers/isdn/gigaset/isocdata.c

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