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