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