drivers/staging/isdn/gigaset/isocdata.c

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