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                 dbg(DEBUG_ISO,
  87                     "%s: couldn't acquire iso write semaphore", __func__);
  88                 return 0;
  89         }
  90 #ifdef CONFIG_GIGASET_DEBUG
  91         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         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                 //dbg(DEBUG_STREAM, "%s: send buffer empty", __func__);
 166                 /* return idle frame */
 167                 return read < BAS_OUTBUFPAD ?
 168                         BAS_OUTBUFSIZE : read - BAS_OUTBUFPAD;
 169         }
 170
 171         limit = read + size;
 172         dbg(DEBUG_STREAM,
 173             "%s: read=%d write=%d limit=%d", __func__, read, write, limit);
 174 #ifdef CONFIG_GIGASET_DEBUG
 175         if (unlikely(size < 0 || size > BAS_OUTBUFPAD)) {
 176                 err("invalid size %d", size);
 177                 return -EINVAL;
 178         }
 179         src = atomic_read(&iwb->read);
 180         if (unlikely(limit > BAS_OUTBUFSIZE + BAS_OUTBUFPAD ||
 181                      (read < src && limit >= src))) {
 182                 err("isoc write buffer frame reservation violated");
 183                 return -EFAULT;
 184         }
 185 #endif
 186
 187         if (read < write) {
 188                 /* no wraparound in valid data */
 189                 if (limit >= write) {
 190                         /* append idle frame */
 191                         if (!isowbuf_startwrite(iwb))
 192                                 return -EBUSY;
 193                         /* write position could have changed */
 194                         if (limit >= (write = atomic_read(&iwb->write))) {
 195                                 pbyte = iwb->data[write]; /* save
 196                                                              partial byte */
 197                                 limit = write + BAS_OUTBUFPAD;
 198                                 dbg(DEBUG_STREAM,
 199                                     "%s: filling %d->%d with %02x",
 200                                     __func__, write, limit, iwb->idle);
 201                                 if (write + BAS_OUTBUFPAD < BAS_OUTBUFSIZE)
 202                                         memset(iwb->data + write, iwb->idle,
 203                                                BAS_OUTBUFPAD);
 204                                 else {
 205                                         /* wraparound, fill entire pad area */
 206                                         memset(iwb->data + write, iwb->idle,
 207                                                BAS_OUTBUFSIZE + BAS_OUTBUFPAD
 208                                                - write);
 209                                         limit = 0;
 210                                 }
 211                                 dbg(DEBUG_STREAM, "%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                         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         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                 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                 dbg(DEBUG_ISO, "can't put %d bytes", count);
 486                 return -EAGAIN;
 487         }
 488
 489         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                 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                 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                 warn("received oversized packet discarded");
 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                         err("could not allocate skb");
 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                 dbg(DEBUG_ISO, "%s: skb=NULL", __func__);
 575                 gigaset_rcv_error(NULL, bcs->cs, bcs);
 576         } else if (procskb->len < 2) {
 577                 notice("received short frame (%d octets)", procskb->len);
 578                 bcs->hw.bas->runts++;
 579                 gigaset_rcv_error(procskb, bcs->cs, bcs);
 580         } else if (bcs->fcs != PPP_GOODFCS) {
 581                 notice("frame check error (0x%04x)", 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                 dbg(DEBUG_ISO,
 588                     "%s: good frame (%d octets)", __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                 err("could not allocate skb");
 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         notice("received partial byte (%d bits)", 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                 err("could not allocate skb");
 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;
 665         int inputstate;
 666         unsigned seqlen, inbyte, inbits;
 667
 668         IFNULLRET(bcs);
 669         ubc = bcs->hw.bas;
 670         IFNULLRET(ubc);
 671
 672         /* load previous state:
 673          * inputstate = set of flag bits:
 674          * - INS_flag_hunt: no complete opening flag received since connection setup or last abort
 675          * - INS_have_data: at least one complete data byte received since last flag
 676          * seqlen = number of consecutive '1' bits in last 7 input stream bits (0..7)
 677          * inbyte = accumulated partial data byte (if !INS_flag_hunt)
 678          * inbits = number of valid bits in inbyte, starting at LSB (0..6)
 679          */
 680         inputstate = bcs->inputstate;
 681         seqlen = ubc->seqlen;
 682         inbyte = ubc->inbyte;
 683         inbits = ubc->inbits;
 684
 685         /* bit unstuffing a byte a time
 686          * Take your time to understand this; it's straightforward but tedious.
 687          * The "bitcounts" lookup table is used to speed up the counting of
 688          * leading and trailing '1' bits.
 689          */
 690         while (count--) {
 691                 unsigned char c = *src++;
 692                 unsigned char tabentry = bitcounts[c];
 693                 unsigned lead1 = tabentry & 0x0f;
 694                 unsigned trail1 = (tabentry >> 4) & 0x0f;
 695
 696                 seqlen += lead1;
 697
 698                 if (unlikely(inputstate & INS_flag_hunt)) {
 699                         if (c == PPP_FLAG) {
 700                                 /* flag-in-one */
 701                                 inputstate &= ~(INS_flag_hunt | INS_have_data);
 702                                 inbyte = 0;
 703                                 inbits = 0;
 704                         } else if (seqlen == 6 && trail1 != 7) {
 705                                 /* flag completed & not followed by abort */
 706                                 inputstate &= ~(INS_flag_hunt | INS_have_data);
 707                                 inbyte = c >> (lead1 + 1);
 708                                 inbits = 7 - lead1;
 709                                 if (trail1 >= 8) {
 710                                         /* interior stuffing: omitting the MSB handles most cases */
 711                                         inbits--;
 712                                         /* correct the incorrectly handled cases individually */
 713                                         switch (c) {
 714                                         case 0xbe:
 715                                                 inbyte = 0x3f;
 716                                                 break;
 717                                         }
 718                                 }
 719                         }
 720                         /* else: continue flag-hunting */
 721                 } else if (likely(seqlen < 5 && trail1 < 7)) {
 722                         /* streamlined case: 8 data bits, no stuffing */
 723                         inbyte |= c << inbits;
 724                         hdlc_putbyte(inbyte & 0xff, bcs);
 725                         inputstate |= INS_have_data;
 726                         inbyte >>= 8;
 727                         /* inbits unchanged */
 728                 } else if (likely(seqlen == 6 && inbits == 7 - lead1 &&
 729                                   trail1 + 1 == inbits &&
 730                                   !(inputstate & INS_have_data))) {
 731                         /* streamlined case: flag idle - state unchanged */
 732                 } else if (unlikely(seqlen > 6)) {
 733                         /* abort sequence */
 734                         ubc->aborts++;
 735                         hdlc_flush(bcs);
 736                         inputstate |= INS_flag_hunt;
 737                 } else if (seqlen == 6) {
 738                         /* closing flag, including (6 - lead1) '1's and one '0' from inbits */
 739                         if (inbits > 7 - lead1) {
 740                                 hdlc_frag(bcs, inbits + lead1 - 7);
 741                                 inputstate &= ~INS_have_data;
 742                         } else {
 743                                 if (inbits < 7 - lead1)
 744                                         ubc->stolen0s ++;
 745                                 if (inputstate & INS_have_data) {
 746                                         hdlc_done(bcs);
 747                                         inputstate &= ~INS_have_data;
 748                                 }
 749                         }
 750
 751                         if (c == PPP_FLAG) {
 752                                 /* complete flag, LSB overlaps preceding flag */
 753                                 ubc->shared0s ++;
 754                                 inbits = 0;
 755                                 inbyte = 0;
 756                         } else if (trail1 != 7) {
 757                                 /* remaining bits */
 758                                 inbyte = c >> (lead1 + 1);
 759                                 inbits = 7 - lead1;
 760                                 if (trail1 >= 8) {
 761                                         /* interior stuffing: omitting the MSB handles most cases */
 762                                         inbits--;
 763                                         /* correct the incorrectly handled cases individually */
 764                                         switch (c) {
 765                                         case 0xbe:
 766                                                 inbyte = 0x3f;
 767                                                 break;
 768                                         }
 769                                 }
 770                         } else {
 771                                 /* abort sequence follows, skb already empty anyway */
 772                                 ubc->aborts++;
 773                                 inputstate |= INS_flag_hunt;
 774                         }
 775                 } else { /* (seqlen < 6) && (seqlen == 5 || trail1 >= 7) */
 776
 777                         if (c == PPP_FLAG) {
 778                                 /* complete flag */
 779                                 if (seqlen == 5)
 780                                         ubc->stolen0s++;
 781                                 if (inbits) {
 782                                         hdlc_frag(bcs, inbits);
 783                                         inbits = 0;
 784                                         inbyte = 0;
 785                                 } else if (inputstate & INS_have_data)
 786                                         hdlc_done(bcs);
 787                                 inputstate &= ~INS_have_data;
 788                         } else if (trail1 == 7) {
 789                                 /* abort sequence */
 790                                 ubc->aborts++;
 791                                 hdlc_flush(bcs);
 792                                 inputstate |= INS_flag_hunt;
 793                         } else {
 794                                 /* stuffed data */
 795                                 if (trail1 < 7) { /* => seqlen == 5 */
 796                                         /* stuff bit at position lead1, no interior stuffing */
 797                                         unsigned char mask = (1 << lead1) - 1;
 798                                         c = (c & mask) | ((c & ~mask) >> 1);
 799                                         inbyte |= c << inbits;
 800                                         inbits += 7;
 801                                 } else if (seqlen < 5) { /* trail1 >= 8 */
 802                                         /* interior stuffing: omitting the MSB handles most cases */
 803                                         /* correct the incorrectly handled cases individually */
 804                                         switch (c) {
 805                                         case 0xbe:
 806                                                 c = 0x7e;
 807                                                 break;
 808                                         }
 809                                         inbyte |= c << inbits;
 810                                         inbits += 7;
 811                                 } else { /* seqlen == 5 && trail1 >= 8 */
 812
 813                                         /* stuff bit at lead1 *and* interior stuffing */
 814                                         switch (c) {    /* unstuff individually */
 815                                         case 0x7d:
 816                                                 c = 0x3f;
 817                                                 break;
 818                                         case 0xbe:
 819                                                 c = 0x3f;
 820                                                 break;
 821                                         case 0x3e:
 822                                                 c = 0x1f;
 823                                                 break;
 824                                         case 0x7c:
 825                                                 c = 0x3e;
 826                                                 break;
 827                                         }
 828                                         inbyte |= c << inbits;
 829                                         inbits += 6;
 830                                 }
 831                                 if (inbits >= 8) {
 832                                         inbits -= 8;
 833                                         hdlc_putbyte(inbyte & 0xff, bcs);
 834                                         inputstate |= INS_have_data;
 835                                         inbyte >>= 8;
 836                                 }
 837                         }
 838                 }
 839                 seqlen = trail1 & 7;
 840         }
 841
 842         /* save new state */
 843         bcs->inputstate = inputstate;
 844         ubc->seqlen = seqlen;
 845         ubc->inbyte = inbyte;
 846         ubc->inbits = inbits;
 847 }
 848
 849 /* trans_receive
 850  * pass on received USB frame transparently as SKB via gigaset_rcv_skb
 851  * invert bytes
 852  * tally frames, errors etc. in BC structure counters
 853  * parameters:
 854  *      src     received data
 855  *      count   number of received bytes
 856  *      bcs     receiving B channel structure
 857  */
 858 static inline void trans_receive(unsigned char *src, unsigned count,
 859                                  struct bc_state *bcs)
 860 {
 861         struct sk_buff *skb;
 862         int dobytes;
 863         unsigned char *dst;
 864
 865         if (unlikely(bcs->ignore)) {
 866                 bcs->ignore--;
 867                 hdlc_flush(bcs);
 868                 return;
 869         }
 870         if (unlikely((skb = bcs->skb) == NULL)) {
 871                 bcs->skb = skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN);
 872                 if (!skb) {
 873                         err("could not allocate skb");
 874                         return;
 875                 }
 876                 skb_reserve(skb, HW_HDR_LEN);
 877         }
 878         bcs->hw.bas->goodbytes += skb->len;
 879         dobytes = TRANSBUFSIZE - skb->len;
 880         while (count > 0) {
 881                 dst = skb_put(skb, count < dobytes ? count : dobytes);
 882                 while (count > 0 && dobytes > 0) {
 883                         *dst++ = gigaset_invtab[*src++];
 884                         count--;
 885                         dobytes--;
 886                 }
 887                 if (dobytes == 0) {
 888                         gigaset_rcv_skb(skb, bcs->cs, bcs);
 889                         bcs->skb = skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN);
 890                         if (!skb) {
 891                                 err("could not allocate skb");
 892                                 return;
 893                         }
 894                         skb_reserve(bcs->skb, HW_HDR_LEN);
 895                         dobytes = TRANSBUFSIZE;
 896                 }
 897         }
 898 }
 899
 900 void gigaset_isoc_receive(unsigned char *src, unsigned count, struct bc_state *bcs)
 901 {
 902         switch (bcs->proto2) {
 903         case ISDN_PROTO_L2_HDLC:
 904                 hdlc_unpack(src, count, bcs);
 905                 break;
 906         default:                /* assume transparent */
 907                 trans_receive(src, count, bcs);
 908         }
 909 }
 910
 911 /* == data input =========================================================== */
 912
 913 static void cmd_loop(unsigned char *src, int numbytes, struct inbuf_t *inbuf)
 914 {
 915         struct cardstate *cs = inbuf->cs;
 916         unsigned cbytes      = cs->cbytes;
 917
 918         while (numbytes--) {
 919                 /* copy next character, check for end of line */
 920                 switch (cs->respdata[cbytes] = *src++) {
 921                 case '\r':
 922                 case '\n':
 923                         /* end of line */
 924                         dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)",
 925                             __func__, cbytes);
 926                         cs->cbytes = cbytes;
 927                         gigaset_handle_modem_response(cs);
 928                         cbytes = 0;
 929                         break;
 930                 default:
 931                         /* advance in line buffer, checking for overflow */
 932                         if (cbytes < MAX_RESP_SIZE - 1)
 933                                 cbytes++;
 934                         else
 935                                 warn("response too large");
 936                 }
 937         }
 938
 939         /* save state */
 940         cs->cbytes = cbytes;
 941 }
 942
 943
 944 /* process a block of data received through the control channel
 945  */
 946 void gigaset_isoc_input(struct inbuf_t *inbuf)
 947 {
 948         struct cardstate *cs = inbuf->cs;
 949         unsigned tail, head, numbytes;
 950         unsigned char *src;
 951
 952         head = atomic_read(&inbuf->head);
 953         while (head != (tail = atomic_read(&inbuf->tail))) {
 954                 dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
 955                 if (head > tail)
 956                         tail = RBUFSIZE;
 957                 src = inbuf->data + head;
 958                 numbytes = tail - head;
 959                 dbg(DEBUG_INTR, "processing %u bytes", numbytes);
 960
 961                 if (atomic_read(&cs->mstate) == MS_LOCKED) {
 962                         gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
 963                                            numbytes, src, 0);
 964                         gigaset_if_receive(inbuf->cs, src, numbytes);
 965                 } else {
 966                         gigaset_dbg_buffer(DEBUG_CMD, "received response",
 967                                            numbytes, src, 0);
 968                         cmd_loop(src, numbytes, inbuf);
 969                 }
 970
 971                 head += numbytes;
 972                 if (head == RBUFSIZE)
 973                         head = 0;
 974                 dbg(DEBUG_INTR, "setting head to %u", head);
 975                 atomic_set(&inbuf->head, head);
 976         }
 977 }
 978
 979
 980 /* == data output ========================================================== */
 981
 982 /* gigaset_send_skb
 983  * called by common.c to queue an skb for sending
 984  * and start transmission if necessary
 985  * parameters:
 986  *      B Channel control structure
 987  *      skb
 988  * return value:
 989  *      number of bytes accepted for sending
 990  *      (skb->len if ok, 0 if out of buffer space)
 991  *      or error code (< 0, eg. -EINVAL)
 992  */
 993 int gigaset_isoc_send_skb(struct bc_state *bcs, struct sk_buff *skb)
 994 {
 995         int len;
 996
 997         IFNULLRETVAL(bcs, -EFAULT);
 998         IFNULLRETVAL(skb, -EFAULT);
 999         len = skb->len;
1000
1001         skb_queue_tail(&bcs->squeue, skb);
1002         dbg(DEBUG_ISO,
1003             "%s: skb queued, qlen=%d", __func__, skb_queue_len(&bcs->squeue));
1004
1005         /* tasklet submits URB if necessary */
1006         tasklet_schedule(&bcs->hw.bas->sent_tasklet);
1007
1008         return len;     /* ok so far */
1009 }