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  * ToDo: ...
  14  * =====================================================================
  15  * Version: $Id: isocdata.c,v 1.2.2.5 2005/11/13 23:05:19 hjlipp Exp $
  16  * =====================================================================
  17  */
  18
  19 #include "gigaset.h"
  20 #include <linux/crc-ccitt.h>
  21
  22 /* access methods for isowbuf_t */
  23 /* ============================ */
  24
  25 /* initialize buffer structure
  26  */
  27 void gigaset_isowbuf_init(struct isowbuf_t *iwb, unsigned char idle)
  28 {
  29         atomic_set(&iwb->read, 0);
  30         atomic_set(&iwb->nextread, 0);
  31         atomic_set(&iwb->write, 0);
  32         atomic_set(&iwb->writesem, 1);
  33         iwb->wbits = 0;
  34         iwb->idle = idle;
  35         memset(iwb->data + BAS_OUTBUFSIZE, idle, BAS_OUTBUFPAD);
  36 }
  37
  38 /* compute number of bytes which can be appended to buffer
  39  * so that there is still room to append a maximum frame of flags
  40  */
  41 static inline int isowbuf_freebytes(struct isowbuf_t *iwb)
  42 {
  43         int read, write, freebytes;
  44
  45         read = atomic_read(&iwb->read);
  46         write = atomic_read(&iwb->write);
  47         if ((freebytes = read - write) > 0) {
  48                 /* no wraparound: need padding space within regular area */
  49                 return freebytes - BAS_OUTBUFPAD;
  50         } else if (read < BAS_OUTBUFPAD) {
  51                 /* wraparound: can use space up to end of regular area */
  52                 return BAS_OUTBUFSIZE - write;
  53         } else {
  54                 /* following the wraparound yields more space */
  55                 return freebytes + BAS_OUTBUFSIZE - BAS_OUTBUFPAD;
  56         }
  57 }
  58
  59 /* compare two offsets within the buffer
  60  * The buffer is seen as circular, with the read position as start
  61  * returns -1/0/1 if position a </=/> position b without crossing 'read'
  62  */
  63 static inline int isowbuf_poscmp(struct isowbuf_t *iwb, int a, int b)
  64 {
  65         int read;
  66         if (a == b)
  67                 return 0;
  68         read = atomic_read(&iwb->read);
  69         if (a < b) {
  70                 if (a < read && read <= b)
  71                         return +1;
  72                 else
  73                         return -1;
  74         } else {
  75                 if (b < read && read <= a)
  76                         return -1;
  77                 else
  78                         return +1;
  79         }
  80 }
  81
  82 /* start writing
  83  * acquire the write semaphore
  84  * return true if acquired, false if busy
  85  */
  86 static inline int isowbuf_startwrite(struct isowbuf_t *iwb)
  87 {
  88         if (!atomic_dec_and_test(&iwb->writesem)) {
  89                 atomic_inc(&iwb->writesem);
  90                 dbg(DEBUG_ISO,
  91                     "%s: couldn't acquire iso write semaphore", __func__);
  92                 return 0;
  93         }
  94 #ifdef CONFIG_GIGASET_DEBUG
  95         dbg(DEBUG_ISO,
  96             "%s: acquired iso write semaphore, data[write]=%02x, nbits=%d",
  97             __func__, iwb->data[atomic_read(&iwb->write)], iwb->wbits);
  98 #endif
  99         return 1;
 100 }
 101
 102 /* finish writing
 103  * release the write semaphore and update the maximum buffer fill level
 104  * returns the current write position
 105  */
 106 static inline int isowbuf_donewrite(struct isowbuf_t *iwb)
 107 {
 108         int write = atomic_read(&iwb->write);
 109         atomic_inc(&iwb->writesem);
 110         return write;
 111 }
 112
 113 /* append bits to buffer without any checks
 114  * - data contains bits to append, starting at LSB
 115  * - nbits is number of bits to append (0..24)
 116  * must be called with the write semaphore held
 117  * If more than nbits bits are set in data, the extraneous bits are set in the
 118  * buffer too, but the write position is only advanced by nbits.
 119  */
 120 static inline void isowbuf_putbits(struct isowbuf_t *iwb, u32 data, int nbits)
 121 {
 122         int write = atomic_read(&iwb->write);
 123         data <<= iwb->wbits;
 124         data |= iwb->data[write];
 125         nbits += iwb->wbits;
 126         while (nbits >= 8) {
 127                 iwb->data[write++] = data & 0xff;
 128                 write %= BAS_OUTBUFSIZE;
 129                 data >>= 8;
 130                 nbits -= 8;
 131         }
 132         iwb->wbits = nbits;
 133         iwb->data[write] = data & 0xff;
 134         atomic_set(&iwb->write, write);
 135 }
 136
 137 /* put final flag on HDLC bitstream
 138  * also sets the idle fill byte to the correspondingly shifted flag pattern
 139  * must be called with the write semaphore held
 140  */
 141 static inline void isowbuf_putflag(struct isowbuf_t *iwb)
 142 {
 143         int write;
 144
 145         /* add two flags, thus reliably covering one byte */
 146         isowbuf_putbits(iwb, 0x7e7e, 8);
 147         /* recover the idle flag byte */
 148         write = atomic_read(&iwb->write);
 149         iwb->idle = iwb->data[write];
 150         dbg(DEBUG_ISO, "idle fill byte %02x", iwb->idle);
 151         /* mask extraneous bits in buffer */
 152         iwb->data[write] &= (1 << iwb->wbits) - 1;
 153 }
 154
 155 /* retrieve a block of bytes for sending
 156  * The requested number of bytes is provided as a contiguous block.
 157  * If necessary, the frame is filled to the requested number of bytes
 158  * with the idle value.
 159  * returns offset to frame, < 0 on busy or error
 160  */
 161 int gigaset_isowbuf_getbytes(struct isowbuf_t *iwb, int size)
 162 {
 163         int read, write, limit, src, dst;
 164         unsigned char pbyte;
 165
 166         read = atomic_read(&iwb->nextread);
 167         write = atomic_read(&iwb->write);
 168         if (likely(read == write)) {
 169                 //dbg(DEBUG_STREAM, "%s: send buffer empty", __func__);
 170                 /* return idle frame */
 171                 return read < BAS_OUTBUFPAD ?
 172                         BAS_OUTBUFSIZE : read - BAS_OUTBUFPAD;
 173         }
 174
 175         limit = read + size;
 176         dbg(DEBUG_STREAM,
 177             "%s: read=%d write=%d limit=%d", __func__, read, write, limit);
 178 #ifdef CONFIG_GIGASET_DEBUG
 179         if (unlikely(size < 0 || size > BAS_OUTBUFPAD)) {
 180                 err("invalid size %d", size);
 181                 return -EINVAL;
 182         }
 183         src = atomic_read(&iwb->read);
 184         if (unlikely(limit > BAS_OUTBUFSIZE + BAS_OUTBUFPAD ||
 185                      (read < src && limit >= src))) {
 186                 err("isoc write buffer frame reservation violated");
 187                 return -EFAULT;
 188         }
 189 #endif
 190
 191         if (read < write) {
 192                 /* no wraparound in valid data */
 193                 if (limit >= write) {
 194                         /* append idle frame */
 195                         if (!isowbuf_startwrite(iwb))
 196                                 return -EBUSY;
 197                         /* write position could have changed */
 198                         if (limit >= (write = atomic_read(&iwb->write))) {
 199                                 pbyte = iwb->data[write]; /* save partial byte */
 200                                 limit = write + BAS_OUTBUFPAD;
 201                                 dbg(DEBUG_STREAM,
 202                                     "%s: filling %d->%d with %02x",
 203                                     __func__, write, limit, iwb->idle);
 204                                 if (write + BAS_OUTBUFPAD < BAS_OUTBUFSIZE)
 205                                         memset(iwb->data + write, iwb->idle,
 206                                                BAS_OUTBUFPAD);
 207                                 else {
 208                                         /* wraparound, fill entire pad area */
 209                                         memset(iwb->data + write, iwb->idle,
 210                                                BAS_OUTBUFSIZE + BAS_OUTBUFPAD
 211                                                - write);
 212                                         limit = 0;
 213                                 }
 214                                 dbg(DEBUG_STREAM, "%s: restoring %02x at %d",
 215                                     __func__, pbyte, limit);
 216                                 iwb->data[limit] = pbyte; /* restore partial byte */
 217                                 atomic_set(&iwb->write, limit);
 218                         }
 219                         isowbuf_donewrite(iwb);
 220                 }
 221         } else {
 222                 /* valid data wraparound */
 223                 if (limit >= BAS_OUTBUFSIZE) {
 224                         /* copy wrapped part into pad area */
 225                         src = 0;
 226                         dst = BAS_OUTBUFSIZE;
 227                         while (dst < limit && src < write)
 228                                 iwb->data[dst++] = iwb->data[src++];
 229                         if (dst <= limit) {
 230                                 /* fill pad area with idle byte */
 231                                 memset(iwb->data + dst, iwb->idle,
 232                                        BAS_OUTBUFSIZE + BAS_OUTBUFPAD - dst);
 233                         }
 234                         limit = src;
 235                 }
 236         }
 237         atomic_set(&iwb->nextread, limit);
 238         return read;
 239 }
 240
 241 /* dump_bytes
 242  * write hex bytes to syslog for debugging
 243  */
 244 static inline void dump_bytes(enum debuglevel level, const char *tag,
 245                               unsigned char *bytes, int count)
 246 {
 247 #ifdef CONFIG_GIGASET_DEBUG
 248         unsigned char c;
 249         static char dbgline[3 * 32 + 1];
 250         static const char hexdigit[] = "0123456789abcdef";
 251         int i = 0;
 252         IFNULLRET(tag);
 253         IFNULLRET(bytes);
 254         while (count-- > 0) {
 255                 if (i > sizeof(dbgline) - 4) {
 256                         dbgline[i] = '\0';
 257                         dbg(level, "%s:%s", tag, dbgline);
 258                         i = 0;
 259                 }
 260                 c = *bytes++;
 261                 dbgline[i] = (i && !(i % 12)) ? '-' : ' ';
 262                 i++;
 263                 dbgline[i++] = hexdigit[(c >> 4) & 0x0f];
 264                 dbgline[i++] = hexdigit[c & 0x0f];
 265         }
 266         dbgline[i] = '\0';
 267         dbg(level, "%s:%s", tag, dbgline);
 268 #endif
 269 }
 270
 271 /*============================================================================*/
 272
 273 /* bytewise HDLC bitstuffing via table lookup
 274  * lookup table: 5 subtables for 0..4 preceding consecutive '1' bits
 275  * index: 256*(number of preceding '1' bits) + (next byte to stuff)
 276  * value: bit  9.. 0 = result bits
 277  *        bit 12..10 = number of trailing '1' bits in result
 278  *        bit 14..13 = number of bits added by stuffing
 279  */
 280 static u16 stufftab[5 * 256] = {
 281 // previous 1s = 0:
 282  0x0000, 0x0001, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009, 0x000a, 0x000b, 0x000c, 0x000d, 0x000e, 0x000f,
 283  0x0010, 0x0011, 0x0012, 0x0013, 0x0014, 0x0015, 0x0016, 0x0017, 0x0018, 0x0019, 0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0x201f,
 284  0x0020, 0x0021, 0x0022, 0x0023, 0x0024, 0x0025, 0x0026, 0x0027, 0x0028, 0x0029, 0x002a, 0x002b, 0x002c, 0x002d, 0x002e, 0x002f,
 285  0x0030, 0x0031, 0x0032, 0x0033, 0x0034, 0x0035, 0x0036, 0x0037, 0x0038, 0x0039, 0x003a, 0x003b, 0x003c, 0x003d, 0x203e, 0x205f,
 286  0x0040, 0x0041, 0x0042, 0x0043, 0x0044, 0x0045, 0x0046, 0x0047, 0x0048, 0x0049, 0x004a, 0x004b, 0x004c, 0x004d, 0x004e, 0x004f,
 287  0x0050, 0x0051, 0x0052, 0x0053, 0x0054, 0x0055, 0x0056, 0x0057, 0x0058, 0x0059, 0x005a, 0x005b, 0x005c, 0x005d, 0x005e, 0x209f,
 288  0x0060, 0x0061, 0x0062, 0x0063, 0x0064, 0x0065, 0x0066, 0x0067, 0x0068, 0x0069, 0x006a, 0x006b, 0x006c, 0x006d, 0x006e, 0x006f,
 289  0x0070, 0x0071, 0x0072, 0x0073, 0x0074, 0x0075, 0x0076, 0x0077, 0x0078, 0x0079, 0x007a, 0x007b, 0x207c, 0x207d, 0x20be, 0x20df,
 290  0x0480, 0x0481, 0x0482, 0x0483, 0x0484, 0x0485, 0x0486, 0x0487, 0x0488, 0x0489, 0x048a, 0x048b, 0x048c, 0x048d, 0x048e, 0x048f,
 291  0x0490, 0x0491, 0x0492, 0x0493, 0x0494, 0x0495, 0x0496, 0x0497, 0x0498, 0x0499, 0x049a, 0x049b, 0x049c, 0x049d, 0x049e, 0x251f,
 292  0x04a0, 0x04a1, 0x04a2, 0x04a3, 0x04a4, 0x04a5, 0x04a6, 0x04a7, 0x04a8, 0x04a9, 0x04aa, 0x04ab, 0x04ac, 0x04ad, 0x04ae, 0x04af,
 293  0x04b0, 0x04b1, 0x04b2, 0x04b3, 0x04b4, 0x04b5, 0x04b6, 0x04b7, 0x04b8, 0x04b9, 0x04ba, 0x04bb, 0x04bc, 0x04bd, 0x253e, 0x255f,
 294  0x08c0, 0x08c1, 0x08c2, 0x08c3, 0x08c4, 0x08c5, 0x08c6, 0x08c7, 0x08c8, 0x08c9, 0x08ca, 0x08cb, 0x08cc, 0x08cd, 0x08ce, 0x08cf,
 295  0x08d0, 0x08d1, 0x08d2, 0x08d3, 0x08d4, 0x08d5, 0x08d6, 0x08d7, 0x08d8, 0x08d9, 0x08da, 0x08db, 0x08dc, 0x08dd, 0x08de, 0x299f,
 296  0x0ce0, 0x0ce1, 0x0ce2, 0x0ce3, 0x0ce4, 0x0ce5, 0x0ce6, 0x0ce7, 0x0ce8, 0x0ce9, 0x0cea, 0x0ceb, 0x0cec, 0x0ced, 0x0cee, 0x0cef,
 297  0x10f0, 0x10f1, 0x10f2, 0x10f3, 0x10f4, 0x10f5, 0x10f6, 0x10f7, 0x20f8, 0x20f9, 0x20fa, 0x20fb, 0x257c, 0x257d, 0x29be, 0x2ddf,
 298
 299 // previous 1s = 1:
 300  0x0000, 0x0001, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009, 0x000a, 0x000b, 0x000c, 0x000d, 0x000e, 0x200f,
 301  0x0010, 0x0011, 0x0012, 0x0013, 0x0014, 0x0015, 0x0016, 0x0017, 0x0018, 0x0019, 0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0x202f,
 302  0x0020, 0x0021, 0x0022, 0x0023, 0x0024, 0x0025, 0x0026, 0x0027, 0x0028, 0x0029, 0x002a, 0x002b, 0x002c, 0x002d, 0x002e, 0x204f,
 303  0x0030, 0x0031, 0x0032, 0x0033, 0x0034, 0x0035, 0x0036, 0x0037, 0x0038, 0x0039, 0x003a, 0x003b, 0x003c, 0x003d, 0x203e, 0x206f,
 304  0x0040, 0x0041, 0x0042, 0x0043, 0x0044, 0x0045, 0x0046, 0x0047, 0x0048, 0x0049, 0x004a, 0x004b, 0x004c, 0x004d, 0x004e, 0x208f,
 305  0x0050, 0x0051, 0x0052, 0x0053, 0x0054, 0x0055, 0x0056, 0x0057, 0x0058, 0x0059, 0x005a, 0x005b, 0x005c, 0x005d, 0x005e, 0x20af,
 306  0x0060, 0x0061, 0x0062, 0x0063, 0x0064, 0x0065, 0x0066, 0x0067, 0x0068, 0x0069, 0x006a, 0x006b, 0x006c, 0x006d, 0x006e, 0x20cf,
 307  0x0070, 0x0071, 0x0072, 0x0073, 0x0074, 0x0075, 0x0076, 0x0077, 0x0078, 0x0079, 0x007a, 0x007b, 0x207c, 0x207d, 0x20be, 0x20ef,
 308  0x0480, 0x0481, 0x0482, 0x0483, 0x0484, 0x0485, 0x0486, 0x0487, 0x0488, 0x0489, 0x048a, 0x048b, 0x048c, 0x048d, 0x048e, 0x250f,
 309  0x0490, 0x0491, 0x0492, 0x0493, 0x0494, 0x0495, 0x0496, 0x0497, 0x0498, 0x0499, 0x049a, 0x049b, 0x049c, 0x049d, 0x049e, 0x252f,
 310  0x04a0, 0x04a1, 0x04a2, 0x04a3, 0x04a4, 0x04a5, 0x04a6, 0x04a7, 0x04a8, 0x04a9, 0x04aa, 0x04ab, 0x04ac, 0x04ad, 0x04ae, 0x254f,
 311  0x04b0, 0x04b1, 0x04b2, 0x04b3, 0x04b4, 0x04b5, 0x04b6, 0x04b7, 0x04b8, 0x04b9, 0x04ba, 0x04bb, 0x04bc, 0x04bd, 0x253e, 0x256f,
 312  0x08c0, 0x08c1, 0x08c2, 0x08c3, 0x08c4, 0x08c5, 0x08c6, 0x08c7, 0x08c8, 0x08c9, 0x08ca, 0x08cb, 0x08cc, 0x08cd, 0x08ce, 0x298f,
 313  0x08d0, 0x08d1, 0x08d2, 0x08d3, 0x08d4, 0x08d5, 0x08d6, 0x08d7, 0x08d8, 0x08d9, 0x08da, 0x08db, 0x08dc, 0x08dd, 0x08de, 0x29af,
 314  0x0ce0, 0x0ce1, 0x0ce2, 0x0ce3, 0x0ce4, 0x0ce5, 0x0ce6, 0x0ce7, 0x0ce8, 0x0ce9, 0x0cea, 0x0ceb, 0x0cec, 0x0ced, 0x0cee, 0x2dcf,
 315  0x10f0, 0x10f1, 0x10f2, 0x10f3, 0x10f4, 0x10f5, 0x10f6, 0x10f7, 0x20f8, 0x20f9, 0x20fa, 0x20fb, 0x257c, 0x257d, 0x29be, 0x31ef,
 316
 317 // previous 1s = 2:
 318  0x0000, 0x0001, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x2007, 0x0008, 0x0009, 0x000a, 0x000b, 0x000c, 0x000d, 0x000e, 0x2017,
 319  0x0010, 0x0011, 0x0012, 0x0013, 0x0014, 0x0015, 0x0016, 0x2027, 0x0018, 0x0019, 0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0x2037,
 320  0x0020, 0x0021, 0x0022, 0x0023, 0x0024, 0x0025, 0x0026, 0x2047, 0x0028, 0x0029, 0x002a, 0x002b, 0x002c, 0x002d, 0x002e, 0x2057,
 321  0x0030, 0x0031, 0x0032, 0x0033, 0x0034, 0x0035, 0x0036, 0x2067, 0x0038, 0x0039, 0x003a, 0x003b, 0x003c, 0x003d, 0x203e, 0x2077,
 322  0x0040, 0x0041, 0x0042, 0x0043, 0x0044, 0x0045, 0x0046, 0x2087, 0x0048, 0x0049, 0x004a, 0x004b, 0x004c, 0x004d, 0x004e, 0x2097,
 323  0x0050, 0x0051, 0x0052, 0x0053, 0x0054, 0x0055, 0x0056, 0x20a7, 0x0058, 0x0059, 0x005a, 0x005b, 0x005c, 0x005d, 0x005e, 0x20b7,
 324  0x0060, 0x0061, 0x0062, 0x0063, 0x0064, 0x0065, 0x0066, 0x20c7, 0x0068, 0x0069, 0x006a, 0x006b, 0x006c, 0x006d, 0x006e, 0x20d7,
 325  0x0070, 0x0071, 0x0072, 0x0073, 0x0074, 0x0075, 0x0076, 0x20e7, 0x0078, 0x0079, 0x007a, 0x007b, 0x207c, 0x207d, 0x20be, 0x20f7,
 326  0x0480, 0x0481, 0x0482, 0x0483, 0x0484, 0x0485, 0x0486, 0x2507, 0x0488, 0x0489, 0x048a, 0x048b, 0x048c, 0x048d, 0x048e, 0x2517,
 327  0x0490, 0x0491, 0x0492, 0x0493, 0x0494, 0x0495, 0x0496, 0x2527, 0x0498, 0x0499, 0x049a, 0x049b, 0x049c, 0x049d, 0x049e, 0x2537,
 328  0x04a0, 0x04a1, 0x04a2, 0x04a3, 0x04a4, 0x04a5, 0x04a6, 0x2547, 0x04a8, 0x04a9, 0x04aa, 0x04ab, 0x04ac, 0x04ad, 0x04ae, 0x2557,
 329  0x04b0, 0x04b1, 0x04b2, 0x04b3, 0x04b4, 0x04b5, 0x04b6, 0x2567, 0x04b8, 0x04b9, 0x04ba, 0x04bb, 0x04bc, 0x04bd, 0x253e, 0x2577,
 330  0x08c0, 0x08c1, 0x08c2, 0x08c3, 0x08c4, 0x08c5, 0x08c6, 0x2987, 0x08c8, 0x08c9, 0x08ca, 0x08cb, 0x08cc, 0x08cd, 0x08ce, 0x2997,
 331  0x08d0, 0x08d1, 0x08d2, 0x08d3, 0x08d4, 0x08d5, 0x08d6, 0x29a7, 0x08d8, 0x08d9, 0x08da, 0x08db, 0x08dc, 0x08dd, 0x08de, 0x29b7,
 332  0x0ce0, 0x0ce1, 0x0ce2, 0x0ce3, 0x0ce4, 0x0ce5, 0x0ce6, 0x2dc7, 0x0ce8, 0x0ce9, 0x0cea, 0x0ceb, 0x0cec, 0x0ced, 0x0cee, 0x2dd7,
 333  0x10f0, 0x10f1, 0x10f2, 0x10f3, 0x10f4, 0x10f5, 0x10f6, 0x31e7, 0x20f8, 0x20f9, 0x20fa, 0x20fb, 0x257c, 0x257d, 0x29be, 0x41f7,
 334
 335 // previous 1s = 3:
 336  0x0000, 0x0001, 0x0002, 0x2003, 0x0004, 0x0005, 0x0006, 0x200b, 0x0008, 0x0009, 0x000a, 0x2013, 0x000c, 0x000d, 0x000e, 0x201b,
 337  0x0010, 0x0011, 0x0012, 0x2023, 0x0014, 0x0015, 0x0016, 0x202b, 0x0018, 0x0019, 0x001a, 0x2033, 0x001c, 0x001d, 0x001e, 0x203b,
 338  0x0020, 0x0021, 0x0022, 0x2043, 0x0024, 0x0025, 0x0026, 0x204b, 0x0028, 0x0029, 0x002a, 0x2053, 0x002c, 0x002d, 0x002e, 0x205b,
 339  0x0030, 0x0031, 0x0032, 0x2063, 0x0034, 0x0035, 0x0036, 0x206b, 0x0038, 0x0039, 0x003a, 0x2073, 0x003c, 0x003d, 0x203e, 0x207b,
 340  0x0040, 0x0041, 0x0042, 0x2083, 0x0044, 0x0045, 0x0046, 0x208b, 0x0048, 0x0049, 0x004a, 0x2093, 0x004c, 0x004d, 0x004e, 0x209b,
 341  0x0050, 0x0051, 0x0052, 0x20a3, 0x0054, 0x0055, 0x0056, 0x20ab, 0x0058, 0x0059, 0x005a, 0x20b3, 0x005c, 0x005d, 0x005e, 0x20bb,
 342  0x0060, 0x0061, 0x0062, 0x20c3, 0x0064, 0x0065, 0x0066, 0x20cb, 0x0068, 0x0069, 0x006a, 0x20d3, 0x006c, 0x006d, 0x006e, 0x20db,
 343  0x0070, 0x0071, 0x0072, 0x20e3, 0x0074, 0x0075, 0x0076, 0x20eb, 0x0078, 0x0079, 0x007a, 0x20f3, 0x207c, 0x207d, 0x20be, 0x40fb,
 344  0x0480, 0x0481, 0x0482, 0x2503, 0x0484, 0x0485, 0x0486, 0x250b, 0x0488, 0x0489, 0x048a, 0x2513, 0x048c, 0x048d, 0x048e, 0x251b,
 345  0x0490, 0x0491, 0x0492, 0x2523, 0x0494, 0x0495, 0x0496, 0x252b, 0x0498, 0x0499, 0x049a, 0x2533, 0x049c, 0x049d, 0x049e, 0x253b,
 346  0x04a0, 0x04a1, 0x04a2, 0x2543, 0x04a4, 0x04a5, 0x04a6, 0x254b, 0x04a8, 0x04a9, 0x04aa, 0x2553, 0x04ac, 0x04ad, 0x04ae, 0x255b,
 347  0x04b0, 0x04b1, 0x04b2, 0x2563, 0x04b4, 0x04b5, 0x04b6, 0x256b, 0x04b8, 0x04b9, 0x04ba, 0x2573, 0x04bc, 0x04bd, 0x253e, 0x257b,
 348  0x08c0, 0x08c1, 0x08c2, 0x2983, 0x08c4, 0x08c5, 0x08c6, 0x298b, 0x08c8, 0x08c9, 0x08ca, 0x2993, 0x08cc, 0x08cd, 0x08ce, 0x299b,
 349  0x08d0, 0x08d1, 0x08d2, 0x29a3, 0x08d4, 0x08d5, 0x08d6, 0x29ab, 0x08d8, 0x08d9, 0x08da, 0x29b3, 0x08dc, 0x08dd, 0x08de, 0x29bb,
 350  0x0ce0, 0x0ce1, 0x0ce2, 0x2dc3, 0x0ce4, 0x0ce5, 0x0ce6, 0x2dcb, 0x0ce8, 0x0ce9, 0x0cea, 0x2dd3, 0x0cec, 0x0ced, 0x0cee, 0x2ddb,
 351  0x10f0, 0x10f1, 0x10f2, 0x31e3, 0x10f4, 0x10f5, 0x10f6, 0x31eb, 0x20f8, 0x20f9, 0x20fa, 0x41f3, 0x257c, 0x257d, 0x29be, 0x46fb,
 352
 353 // previous 1s = 4:
 354  0x0000, 0x2001, 0x0002, 0x2005, 0x0004, 0x2009, 0x0006, 0x200d, 0x0008, 0x2011, 0x000a, 0x2015, 0x000c, 0x2019, 0x000e, 0x201d,
 355  0x0010, 0x2021, 0x0012, 0x2025, 0x0014, 0x2029, 0x0016, 0x202d, 0x0018, 0x2031, 0x001a, 0x2035, 0x001c, 0x2039, 0x001e, 0x203d,
 356  0x0020, 0x2041, 0x0022, 0x2045, 0x0024, 0x2049, 0x0026, 0x204d, 0x0028, 0x2051, 0x002a, 0x2055, 0x002c, 0x2059, 0x002e, 0x205d,
 357  0x0030, 0x2061, 0x0032, 0x2065, 0x0034, 0x2069, 0x0036, 0x206d, 0x0038, 0x2071, 0x003a, 0x2075, 0x003c, 0x2079, 0x203e, 0x407d,
 358  0x0040, 0x2081, 0x0042, 0x2085, 0x0044, 0x2089, 0x0046, 0x208d, 0x0048, 0x2091, 0x004a, 0x2095, 0x004c, 0x2099, 0x004e, 0x209d,
 359  0x0050, 0x20a1, 0x0052, 0x20a5, 0x0054, 0x20a9, 0x0056, 0x20ad, 0x0058, 0x20b1, 0x005a, 0x20b5, 0x005c, 0x20b9, 0x005e, 0x20bd,
 360  0x0060, 0x20c1, 0x0062, 0x20c5, 0x0064, 0x20c9, 0x0066, 0x20cd, 0x0068, 0x20d1, 0x006a, 0x20d5, 0x006c, 0x20d9, 0x006e, 0x20dd,
 361  0x0070, 0x20e1, 0x0072, 0x20e5, 0x0074, 0x20e9, 0x0076, 0x20ed, 0x0078, 0x20f1, 0x007a, 0x20f5, 0x207c, 0x40f9, 0x20be, 0x417d,
 362  0x0480, 0x2501, 0x0482, 0x2505, 0x0484, 0x2509, 0x0486, 0x250d, 0x0488, 0x2511, 0x048a, 0x2515, 0x048c, 0x2519, 0x048e, 0x251d,
 363  0x0490, 0x2521, 0x0492, 0x2525, 0x0494, 0x2529, 0x0496, 0x252d, 0x0498, 0x2531, 0x049a, 0x2535, 0x049c, 0x2539, 0x049e, 0x253d,
 364  0x04a0, 0x2541, 0x04a2, 0x2545, 0x04a4, 0x2549, 0x04a6, 0x254d, 0x04a8, 0x2551, 0x04aa, 0x2555, 0x04ac, 0x2559, 0x04ae, 0x255d,
 365  0x04b0, 0x2561, 0x04b2, 0x2565, 0x04b4, 0x2569, 0x04b6, 0x256d, 0x04b8, 0x2571, 0x04ba, 0x2575, 0x04bc, 0x2579, 0x253e, 0x467d,
 366  0x08c0, 0x2981, 0x08c2, 0x2985, 0x08c4, 0x2989, 0x08c6, 0x298d, 0x08c8, 0x2991, 0x08ca, 0x2995, 0x08cc, 0x2999, 0x08ce, 0x299d,
 367  0x08d0, 0x29a1, 0x08d2, 0x29a5, 0x08d4, 0x29a9, 0x08d6, 0x29ad, 0x08d8, 0x29b1, 0x08da, 0x29b5, 0x08dc, 0x29b9, 0x08de, 0x29bd,
 368  0x0ce0, 0x2dc1, 0x0ce2, 0x2dc5, 0x0ce4, 0x2dc9, 0x0ce6, 0x2dcd, 0x0ce8, 0x2dd1, 0x0cea, 0x2dd5, 0x0cec, 0x2dd9, 0x0cee, 0x2ddd,
 369  0x10f0, 0x31e1, 0x10f2, 0x31e5, 0x10f4, 0x31e9, 0x10f6, 0x31ed, 0x20f8, 0x41f1, 0x20fa, 0x41f5, 0x257c, 0x46f9, 0x29be, 0x4b7d
 370 };
 371
 372 /* hdlc_bitstuff_byte
 373  * perform HDLC bitstuffing for one input byte (8 bits, LSB first)
 374  * parameters:
 375  *      cin     input byte
 376  *      ones    number of trailing '1' bits in result before this step
 377  *      iwb     pointer to output buffer structure (write semaphore must be held)
 378  * return value:
 379  *      number of trailing '1' bits in result after this step
 380  */
 381
 382 static inline int hdlc_bitstuff_byte(struct isowbuf_t *iwb, unsigned char cin,
 383                                      int ones)
 384 {
 385         u16 stuff;
 386         int shiftinc, newones;
 387
 388         /* get stuffing information for input byte
 389          * value: bit  9.. 0 = result bits
 390          *        bit 12..10 = number of trailing '1' bits in result
 391          *        bit 14..13 = number of bits added by stuffing
 392          */
 393         stuff = stufftab[256 * ones + cin];
 394         shiftinc = (stuff >> 13) & 3;
 395         newones = (stuff >> 10) & 7;
 396         stuff &= 0x3ff;
 397
 398         /* append stuffed byte to output stream */
 399         isowbuf_putbits(iwb, stuff, 8 + shiftinc);
 400         return newones;
 401 }
 402
 403 /* hdlc_buildframe
 404  * Perform HDLC framing with bitstuffing on a byte buffer
 405  * The input buffer is regarded as a sequence of bits, starting with the least
 406  * significant bit of the first byte and ending with the most significant bit
 407  * of the last byte. A 16 bit FCS is appended as defined by RFC 1662.
 408  * Whenever five consecutive '1' bits appear in the resulting bit sequence, a
 409  * '0' bit is inserted after them.
 410  * The resulting bit string and a closing flag pattern (PPP_FLAG, '01111110')
 411  * are appended to the output buffer starting at the given bit position, which
 412  * is assumed to already contain a leading flag.
 413  * The output buffer must have sufficient length; count + count/5 + 6 bytes
 414  * starting at *out are safe and are verified to be present.
 415  * parameters:
 416  *      in      input buffer
 417  *      count   number of bytes in input buffer
 418  *      iwb     pointer to output buffer structure (write semaphore must be held)
 419  * return value:
 420  *      position of end of packet in output buffer on success,
 421  *      -EAGAIN if write semaphore busy or buffer full
 422  */
 423
 424 static inline int hdlc_buildframe(struct isowbuf_t *iwb,
 425                                   unsigned char *in, int count)
 426 {
 427         int ones;
 428         u16 fcs;
 429         int end;
 430         unsigned char c;
 431
 432         if (isowbuf_freebytes(iwb) < count + count / 5 + 6 ||
 433             !isowbuf_startwrite(iwb)) {
 434                 dbg(DEBUG_ISO, "%s: %d bytes free -> -EAGAIN",
 435                     __func__, isowbuf_freebytes(iwb));
 436                 return -EAGAIN;
 437         }
 438
 439         dump_bytes(DEBUG_STREAM, "snd data", in, count);
 440
 441         /* bitstuff and checksum input data */
 442         fcs = PPP_INITFCS;
 443         ones = 0;
 444         while (count-- > 0) {
 445                 c = *in++;
 446                 ones = hdlc_bitstuff_byte(iwb, c, ones);
 447                 fcs = crc_ccitt_byte(fcs, c);
 448         }
 449
 450         /* bitstuff and append FCS (complemented, least significant byte first) */
 451         fcs ^= 0xffff;
 452         ones = hdlc_bitstuff_byte(iwb, fcs & 0x00ff, ones);
 453         ones = hdlc_bitstuff_byte(iwb, (fcs >> 8) & 0x00ff, ones);
 454
 455         /* put closing flag and repeat byte for flag idle */
 456         isowbuf_putflag(iwb);
 457         end = isowbuf_donewrite(iwb);
 458         dump_bytes(DEBUG_STREAM_DUMP, "isowbuf", iwb->data, end + 1);
 459         return end;
 460 }
 461
 462 /* trans_buildframe
 463  * Append a block of 'transparent' data to the output buffer,
 464  * inverting the bytes.
 465  * The output buffer must have sufficient length; count bytes
 466  * starting at *out are safe and are verified to be present.
 467  * parameters:
 468  *      in      input buffer
 469  *      count   number of bytes in input buffer
 470  *      iwb     pointer to output buffer structure (write semaphore must be held)
 471  * return value:
 472  *      position of end of packet in output buffer on success,
 473  *      -EAGAIN if write semaphore busy or buffer full
 474  */
 475
 476 static inline int trans_buildframe(struct isowbuf_t *iwb,
 477                                    unsigned char *in, int count)
 478 {
 479         int write;
 480         unsigned char c;
 481
 482         if (unlikely(count <= 0))
 483                 return atomic_read(&iwb->write); /* better ideas? */
 484
 485         if (isowbuf_freebytes(iwb) < count ||
 486             !isowbuf_startwrite(iwb)) {
 487                 dbg(DEBUG_ISO, "can't put %d bytes", count);
 488                 return -EAGAIN;
 489         }
 490
 491         dbg(DEBUG_STREAM, "put %d bytes", count);
 492         write = atomic_read(&iwb->write);
 493         do {
 494                 c = gigaset_invtab[*in++];
 495                 iwb->data[write++] = c;
 496                 write %= BAS_OUTBUFSIZE;
 497         } while (--count > 0);
 498         atomic_set(&iwb->write, write);
 499         iwb->idle = c;
 500
 501         return isowbuf_donewrite(iwb);
 502 }
 503
 504 int gigaset_isoc_buildframe(struct bc_state *bcs, unsigned char *in, int len)
 505 {
 506         int result;
 507
 508         switch (bcs->proto2) {
 509         case ISDN_PROTO_L2_HDLC:
 510                 result = hdlc_buildframe(bcs->hw.bas->isooutbuf, in, len);
 511                 dbg(DEBUG_ISO, "%s: %d bytes HDLC -> %d", __func__, len, result);
 512                 break;
 513         default:                        /* assume transparent */
 514                 result = trans_buildframe(bcs->hw.bas->isooutbuf, in, len);
 515                 dbg(DEBUG_ISO, "%s: %d bytes trans -> %d", __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 }