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