drivers/isdn/gigaset/asyncdata.c

   1 /*
   2  * Common data handling layer for ser_gigaset and usb_gigaset
   3  *
   4  * Copyright (c) 2005 by Tilman Schmidt <tilman@imap.cc>,
   5  *                       Hansjoerg Lipp <hjlipp@web.de>,
   6  *                       Stefan Eilers.
   7  *
   8  * =====================================================================
   9  *      This program is free software; you can redistribute it and/or
  10  *      modify it under the terms of the GNU General Public License as
  11  *      published by the Free Software Foundation; either version 2 of
  12  *      the License, or (at your option) any later version.
  13  * =====================================================================
  14  */
  15
  16 #include "gigaset.h"
  17 #include <linux/crc-ccitt.h>
  18 #include <linux/bitrev.h>
  19
  20 //#define GIG_M10x_STUFF_VOICE_DATA
  21
  22 /* check if byte must be stuffed/escaped
  23  * I'm not sure which data should be encoded.
  24  * Therefore I will go the hard way and decode every value
  25  * less than 0x20, the flag sequence and the control escape char.
  26  */
  27 static inline int muststuff(unsigned char c)
  28 {
  29         if (c < PPP_TRANS) return 1;
  30         if (c == PPP_FLAG) return 1;
  31         if (c == PPP_ESCAPE) return 1;
  32         /* other possible candidates: */
  33         /* 0x91: XON with parity set */
  34         /* 0x93: XOFF with parity set */
  35         return 0;
  36 }
  37
  38 /* == data input =========================================================== */
  39
  40 /* process a block of received bytes in command mode (modem response)
  41  * Return value:
  42  *      number of processed bytes
  43  */
  44 static inline int cmd_loop(unsigned char c, unsigned char *src, int numbytes,
  45                            struct inbuf_t *inbuf)
  46 {
  47         struct cardstate *cs = inbuf->cs;
  48         unsigned cbytes      = cs->cbytes;
  49         int inputstate = inbuf->inputstate;
  50         int startbytes = numbytes;
  51
  52         for (;;) {
  53                 cs->respdata[cbytes] = c;
  54                 if (c == 10 || c == 13) {
  55                         gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)",
  56                                 __func__, cbytes);
  57                         cs->cbytes = cbytes;
  58                         gigaset_handle_modem_response(cs); /* can change
  59                                                               cs->dle */
  60                         cbytes = 0;
  61
  62                         if (cs->dle &&
  63                             !(inputstate & INS_DLE_command)) {
  64                                 inputstate &= ~INS_command;
  65                                 break;
  66                         }
  67                 } else {
  68                         /* advance in line buffer, checking for overflow */
  69                         if (cbytes < MAX_RESP_SIZE - 1)
  70                                 cbytes++;
  71                         else
  72                                 dev_warn(cs->dev, "response too large\n");
  73                 }
  74
  75                 if (!numbytes)
  76                         break;
  77                 c = *src++;
  78                 --numbytes;
  79                 if (c == DLE_FLAG &&
  80                     (cs->dle || inputstate & INS_DLE_command)) {
  81                         inputstate |= INS_DLE_char;
  82                         break;
  83                 }
  84         }
  85
  86         cs->cbytes = cbytes;
  87         inbuf->inputstate = inputstate;
  88
  89         return startbytes - numbytes;
  90 }
  91
  92 /* process a block of received bytes in lock mode (tty i/f)
  93  * Return value:
  94  *      number of processed bytes
  95  */
  96 static inline int lock_loop(unsigned char *src, int numbytes,
  97                             struct inbuf_t *inbuf)
  98 {
  99         struct cardstate *cs = inbuf->cs;
 100
 101         gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
 102                            numbytes, src);
 103         gigaset_if_receive(cs, src, numbytes);
 104
 105         return numbytes;
 106 }
 107
 108 /* process a block of received bytes in HDLC data mode
 109  * Collect HDLC frames, undoing byte stuffing and watching for DLE escapes.
 110  * When a frame is complete, check the FCS and pass valid frames to the LL.
 111  * If DLE is encountered, return immediately to let the caller handle it.
 112  * Return value:
 113  *      number of processed bytes
 114  *      numbytes (all bytes processed) on error --FIXME
 115  */
 116 static inline int hdlc_loop(unsigned char c, unsigned char *src, int numbytes,
 117                             struct inbuf_t *inbuf)
 118 {
 119         struct cardstate *cs = inbuf->cs;
 120         struct bc_state *bcs = inbuf->bcs;
 121         int inputstate = bcs->inputstate;
 122         __u16 fcs = bcs->fcs;
 123         struct sk_buff *skb = bcs->skb;
 124         unsigned char error;
 125         struct sk_buff *compskb;
 126         int startbytes = numbytes;
 127         int l;
 128
 129         if (unlikely(inputstate & INS_byte_stuff)) {
 130                 inputstate &= ~INS_byte_stuff;
 131                 goto byte_stuff;
 132         }
 133         for (;;) {
 134                 if (unlikely(c == PPP_ESCAPE)) {
 135                         if (unlikely(!numbytes)) {
 136                                 inputstate |= INS_byte_stuff;
 137                                 break;
 138                         }
 139                         c = *src++;
 140                         --numbytes;
 141                         if (unlikely(c == DLE_FLAG &&
 142                                      (cs->dle ||
 143                                       inbuf->inputstate & INS_DLE_command))) {
 144                                 inbuf->inputstate |= INS_DLE_char;
 145                                 inputstate |= INS_byte_stuff;
 146                                 break;
 147                         }
 148 byte_stuff:
 149                         c ^= PPP_TRANS;
 150 #ifdef CONFIG_GIGASET_DEBUG
 151                         if (unlikely(!muststuff(c)))
 152                                 gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c);
 153 #endif
 154                 } else if (unlikely(c == PPP_FLAG)) {
 155                         if (unlikely(inputstate & INS_skip_frame)) {
 156                                 if (!(inputstate & INS_have_data)) { /* 7E 7E */
 157 #ifdef CONFIG_GIGASET_DEBUG
 158                                         ++bcs->emptycount;
 159 #endif
 160                                 } else
 161                                         gig_dbg(DEBUG_HDLC,
 162                                             "7e----------------------------");
 163
 164                                 /* end of frame */
 165                                 error = 1;
 166                                 gigaset_rcv_error(NULL, cs, bcs);
 167                         } else if (!(inputstate & INS_have_data)) { /* 7E 7E */
 168 #ifdef CONFIG_GIGASET_DEBUG
 169                                 ++bcs->emptycount;
 170 #endif
 171                                 break;
 172                         } else {
 173                                 gig_dbg(DEBUG_HDLC,
 174                                         "7e----------------------------");
 175
 176                                 /* end of frame */
 177                                 error = 0;
 178
 179                                 if (unlikely(fcs != PPP_GOODFCS)) {
 180                                         dev_err(cs->dev,
 181                                             "Packet checksum at %lu failed, "
 182                                             "packet is corrupted (%u bytes)!\n",
 183                                             bcs->rcvbytes, skb->len);
 184                                         compskb = NULL;
 185                                         gigaset_rcv_error(compskb, cs, bcs);
 186                                         error = 1;
 187                                 } else {
 188                                         if (likely((l = skb->len) > 2)) {
 189                                                 skb->tail -= 2;
 190                                                 skb->len -= 2;
 191                                         } else {
 192                                                 dev_kfree_skb(skb);
 193                                                 skb = NULL;
 194                                                 inputstate |= INS_skip_frame;
 195                                                 if (l == 1) {
 196                                                         dev_err(cs->dev,
 197                                                   "invalid packet size (1)!\n");
 198                                                         error = 1;
 199                                                         gigaset_rcv_error(NULL,
 200                                                                 cs, bcs);
 201                                                 }
 202                                         }
 203                                         if (likely(!(error ||
 204                                                      (inputstate &
 205                                                       INS_skip_frame)))) {
 206                                                 gigaset_rcv_skb(skb, cs, bcs);
 207                                         }
 208                                 }
 209                         }
 210
 211                         if (unlikely(error))
 212                                 if (skb)
 213                                         dev_kfree_skb(skb);
 214
 215                         fcs = PPP_INITFCS;
 216                         inputstate &= ~(INS_have_data | INS_skip_frame);
 217                         if (unlikely(bcs->ignore)) {
 218                                 inputstate |= INS_skip_frame;
 219                                 skb = NULL;
 220                         } else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)) {
 221                                 skb_reserve(skb, HW_HDR_LEN);
 222                         } else {
 223                                 dev_warn(cs->dev,
 224                                          "could not allocate new skb\n");
 225                                 inputstate |= INS_skip_frame;
 226                         }
 227
 228                         break;
 229 #ifdef CONFIG_GIGASET_DEBUG
 230                 } else if (unlikely(muststuff(c))) {
 231                         /* Should not happen. Possible after ZDLE=1<CR><LF>. */
 232                         gig_dbg(DEBUG_HDLC, "not byte stuffed: 0x%02x", c);
 233 #endif
 234                 }
 235
 236                 /* add character */
 237
 238 #ifdef CONFIG_GIGASET_DEBUG
 239                 if (unlikely(!(inputstate & INS_have_data))) {
 240                         gig_dbg(DEBUG_HDLC, "7e (%d x) ================",
 241                                 bcs->emptycount);
 242                         bcs->emptycount = 0;
 243                 }
 244 #endif
 245
 246                 inputstate |= INS_have_data;
 247
 248                 if (likely(!(inputstate & INS_skip_frame))) {
 249                         if (unlikely(skb->len == SBUFSIZE)) {
 250                                 dev_warn(cs->dev, "received packet too long\n");
 251                                 dev_kfree_skb_any(skb);
 252                                 skb = NULL;
 253                                 inputstate |= INS_skip_frame;
 254                                 break;
 255                         }
 256                         *__skb_put(skb, 1) = c;
 257                         fcs = crc_ccitt_byte(fcs, c);
 258                 }
 259
 260                 if (unlikely(!numbytes))
 261                         break;
 262                 c = *src++;
 263                 --numbytes;
 264                 if (unlikely(c == DLE_FLAG &&
 265                              (cs->dle ||
 266                               inbuf->inputstate & INS_DLE_command))) {
 267                         inbuf->inputstate |= INS_DLE_char;
 268                         break;
 269                 }
 270         }
 271         bcs->inputstate = inputstate;
 272         bcs->fcs = fcs;
 273         bcs->skb = skb;
 274         return startbytes - numbytes;
 275 }
 276
 277 /* process a block of received bytes in transparent data mode
 278  * Invert bytes, undoing byte stuffing and watching for DLE escapes.
 279  * If DLE is encountered, return immediately to let the caller handle it.
 280  * Return value:
 281  *      number of processed bytes
 282  *      numbytes (all bytes processed) on error --FIXME
 283  */
 284 static inline int iraw_loop(unsigned char c, unsigned char *src, int numbytes,
 285                             struct inbuf_t *inbuf)
 286 {
 287         struct cardstate *cs = inbuf->cs;
 288         struct bc_state *bcs = inbuf->bcs;
 289         int inputstate = bcs->inputstate;
 290         struct sk_buff *skb = bcs->skb;
 291         int startbytes = numbytes;
 292
 293         for (;;) {
 294                 /* add character */
 295                 inputstate |= INS_have_data;
 296
 297                 if (likely(!(inputstate & INS_skip_frame))) {
 298                         if (unlikely(skb->len == SBUFSIZE)) {
 299                                 //FIXME just pass skb up and allocate a new one
 300                                 dev_warn(cs->dev, "received packet too long\n");
 301                                 dev_kfree_skb_any(skb);
 302                                 skb = NULL;
 303                                 inputstate |= INS_skip_frame;
 304                                 break;
 305                         }
 306                         *__skb_put(skb, 1) = bitrev8(c);
 307                 }
 308
 309                 if (unlikely(!numbytes))
 310                         break;
 311                 c = *src++;
 312                 --numbytes;
 313                 if (unlikely(c == DLE_FLAG &&
 314                              (cs->dle ||
 315                               inbuf->inputstate & INS_DLE_command))) {
 316                         inbuf->inputstate |= INS_DLE_char;
 317                         break;
 318                 }
 319         }
 320
 321         /* pass data up */
 322         if (likely(inputstate & INS_have_data)) {
 323                 if (likely(!(inputstate & INS_skip_frame))) {
 324                         gigaset_rcv_skb(skb, cs, bcs);
 325                 }
 326                 inputstate &= ~(INS_have_data | INS_skip_frame);
 327                 if (unlikely(bcs->ignore)) {
 328                         inputstate |= INS_skip_frame;
 329                         skb = NULL;
 330                 } else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN))
 331                                   != NULL)) {
 332                         skb_reserve(skb, HW_HDR_LEN);
 333                 } else {
 334                         dev_warn(cs->dev, "could not allocate new skb\n");
 335                         inputstate |= INS_skip_frame;
 336                 }
 337         }
 338
 339         bcs->inputstate = inputstate;
 340         bcs->skb = skb;
 341         return startbytes - numbytes;
 342 }
 343
 344 /* process a block of data received from the device
 345  */
 346 void gigaset_m10x_input(struct inbuf_t *inbuf)
 347 {
 348         struct cardstate *cs;
 349         unsigned tail, head, numbytes;
 350         unsigned char *src, c;
 351         int procbytes;
 352
 353         head = atomic_read(&inbuf->head);
 354         tail = atomic_read(&inbuf->tail);
 355         gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
 356
 357         if (head != tail) {
 358                 cs = inbuf->cs;
 359                 src = inbuf->data + head;
 360                 numbytes = (head > tail ? RBUFSIZE : tail) - head;
 361                 gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
 362
 363                 while (numbytes) {
 364                         if (atomic_read(&cs->mstate) == MS_LOCKED) {
 365                                 procbytes = lock_loop(src, numbytes, inbuf);
 366                                 src += procbytes;
 367                                 numbytes -= procbytes;
 368                         } else {
 369                                 c = *src++;
 370                                 --numbytes;
 371                                 if (c == DLE_FLAG && (cs->dle ||
 372                                     inbuf->inputstate & INS_DLE_command)) {
 373                                         if (!(inbuf->inputstate & INS_DLE_char)) {
 374                                                 inbuf->inputstate |= INS_DLE_char;
 375                                                 goto nextbyte;
 376                                         }
 377                                         /* <DLE> <DLE> => <DLE> in data stream */
 378                                         inbuf->inputstate &= ~INS_DLE_char;
 379                                 }
 380
 381                                 if (!(inbuf->inputstate & INS_DLE_char)) {
 382
 383                                         /* FIXME use function pointers?  */
 384                                         if (inbuf->inputstate & INS_command)
 385                                                 procbytes = cmd_loop(c, src, numbytes, inbuf);
 386                                         else if (inbuf->bcs->proto2 == ISDN_PROTO_L2_HDLC)
 387                                                 procbytes = hdlc_loop(c, src, numbytes, inbuf);
 388                                         else
 389                                                 procbytes = iraw_loop(c, src, numbytes, inbuf);
 390
 391                                         src += procbytes;
 392                                         numbytes -= procbytes;
 393                                 } else {  /* DLE char */
 394                                         inbuf->inputstate &= ~INS_DLE_char;
 395                                         switch (c) {
 396                                         case 'X': /*begin of command*/
 397 #ifdef CONFIG_GIGASET_DEBUG
 398                                                 if (inbuf->inputstate & INS_command)
 399                                                         dev_err(cs->dev,
 400                                         "received <DLE> 'X' in command mode\n");
 401 #endif
 402                                                 inbuf->inputstate |=
 403                                                         INS_command | INS_DLE_command;
 404                                                 break;
 405                                         case '.': /*end of command*/
 406 #ifdef CONFIG_GIGASET_DEBUG
 407                                                 if (!(inbuf->inputstate & INS_command))
 408                                                         dev_err(cs->dev,
 409                                         "received <DLE> '.' in hdlc mode\n");
 410 #endif
 411                                                 inbuf->inputstate &= cs->dle ?
 412                                                         ~(INS_DLE_command|INS_command)
 413                                                         : ~INS_DLE_command;
 414                                                 break;
 415                                         //case DLE_FLAG: /*DLE_FLAG in data stream*/ /* schon oben behandelt! */
 416                                         default:
 417                                                 dev_err(cs->dev,
 418                                                       "received 0x10 0x%02x!\n",
 419                                                         (int) c);
 420                                                 /* FIXME: reset driver?? */
 421                                         }
 422                                 }
 423                         }
 424 nextbyte:
 425                         if (!numbytes) {
 426                                 /* end of buffer, check for wrap */
 427                                 if (head > tail) {
 428                                         head = 0;
 429                                         src = inbuf->data;
 430                                         numbytes = tail;
 431                                 } else {
 432                                         head = tail;
 433                                         break;
 434                                 }
 435                         }
 436                 }
 437
 438                 gig_dbg(DEBUG_INTR, "setting head to %u", head);
 439                 atomic_set(&inbuf->head, head);
 440         }
 441 }
 442
 443
 444 /* == data output ========================================================== */
 445
 446 /* Encoding of a PPP packet into an octet stuffed HDLC frame
 447  * with FCS, opening and closing flags.
 448  * parameters:
 449  *      skb     skb containing original packet (freed upon return)
 450  *      head    number of headroom bytes to allocate in result skb
 451  *      tail    number of tailroom bytes to allocate in result skb
 452  * Return value:
 453  *      pointer to newly allocated skb containing the result frame
 454  */
 455 static struct sk_buff *HDLC_Encode(struct sk_buff *skb, int head, int tail)
 456 {
 457         struct sk_buff *hdlc_skb;
 458         __u16 fcs;
 459         unsigned char c;
 460         unsigned char *cp;
 461         int len;
 462         unsigned int stuf_cnt;
 463
 464         stuf_cnt = 0;
 465         fcs = PPP_INITFCS;
 466         cp = skb->data;
 467         len = skb->len;
 468         while (len--) {
 469                 if (muststuff(*cp))
 470                         stuf_cnt++;
 471                 fcs = crc_ccitt_byte(fcs, *cp++);
 472         }
 473         fcs ^= 0xffff;                  /* complement */
 474
 475         /* size of new buffer: original size + number of stuffing bytes
 476          * + 2 bytes FCS + 2 stuffing bytes for FCS (if needed) + 2 flag bytes
 477          */
 478         hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + tail + head);
 479         if (!hdlc_skb) {
 480                 dev_kfree_skb(skb);
 481                 return NULL;
 482         }
 483         skb_reserve(hdlc_skb, head);
 484
 485         /* Copy acknowledge request into new skb */
 486         memcpy(hdlc_skb->head, skb->head, 2);
 487
 488         /* Add flag sequence in front of everything.. */
 489         *(skb_put(hdlc_skb, 1)) = PPP_FLAG;
 490
 491         /* Perform byte stuffing while copying data. */
 492         while (skb->len--) {
 493                 if (muststuff(*skb->data)) {
 494                         *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
 495                         *(skb_put(hdlc_skb, 1)) = (*skb->data++) ^ PPP_TRANS;
 496                 } else
 497                         *(skb_put(hdlc_skb, 1)) = *skb->data++;
 498         }
 499
 500         /* Finally add FCS (byte stuffed) and flag sequence */
 501         c = (fcs & 0x00ff);     /* least significant byte first */
 502         if (muststuff(c)) {
 503                 *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
 504                 c ^= PPP_TRANS;
 505         }
 506         *(skb_put(hdlc_skb, 1)) = c;
 507
 508         c = ((fcs >> 8) & 0x00ff);
 509         if (muststuff(c)) {
 510                 *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
 511                 c ^= PPP_TRANS;
 512         }
 513         *(skb_put(hdlc_skb, 1)) = c;
 514
 515         *(skb_put(hdlc_skb, 1)) = PPP_FLAG;
 516
 517         dev_kfree_skb(skb);
 518         return hdlc_skb;
 519 }
 520
 521 /* Encoding of a raw packet into an octet stuffed bit inverted frame
 522  * parameters:
 523  *      skb     skb containing original packet (freed upon return)
 524  *      head    number of headroom bytes to allocate in result skb
 525  *      tail    number of tailroom bytes to allocate in result skb
 526  * Return value:
 527  *      pointer to newly allocated skb containing the result frame
 528  */
 529 static struct sk_buff *iraw_encode(struct sk_buff *skb, int head, int tail)
 530 {
 531         struct sk_buff *iraw_skb;
 532         unsigned char c;
 533         unsigned char *cp;
 534         int len;
 535
 536         /* worst case: every byte must be stuffed */
 537         iraw_skb = dev_alloc_skb(2*skb->len + tail + head);
 538         if (!iraw_skb) {
 539                 dev_kfree_skb(skb);
 540                 return NULL;
 541         }
 542         skb_reserve(iraw_skb, head);
 543
 544         cp = skb->data;
 545         len = skb->len;
 546         while (len--) {
 547                 c = bitrev8(*cp++);
 548                 if (c == DLE_FLAG)
 549                         *(skb_put(iraw_skb, 1)) = c;
 550                 *(skb_put(iraw_skb, 1)) = c;
 551         }
 552         dev_kfree_skb(skb);
 553         return iraw_skb;
 554 }
 555
 556 /* gigaset_send_skb
 557  * called by common.c to queue an skb for sending
 558  * and start transmission if necessary
 559  * parameters:
 560  *      B Channel control structure
 561  *      skb
 562  * Return value:
 563  *      number of bytes accepted for sending
 564  *      (skb->len if ok, 0 if out of buffer space)
 565  *      or error code (< 0, eg. -EINVAL)
 566  */
 567 int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb)
 568 {
 569         unsigned len = skb->len;
 570         unsigned long flags;
 571
 572         if (bcs->proto2 == ISDN_PROTO_L2_HDLC)
 573                 skb = HDLC_Encode(skb, HW_HDR_LEN, 0);
 574         else
 575                 skb = iraw_encode(skb, HW_HDR_LEN, 0);
 576         if (!skb) {
 577                 err("unable to allocate memory for encoding!\n");
 578                 return -ENOMEM;
 579         }
 580
 581         skb_queue_tail(&bcs->squeue, skb);
 582         spin_lock_irqsave(&bcs->cs->lock, flags);
 583         if (bcs->cs->connected)
 584                 tasklet_schedule(&bcs->cs->write_tasklet);
 585         spin_unlock_irqrestore(&bcs->cs->lock, flags);
 586
 587         return len;     /* ok so far */
 588 }