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