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