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         int startbytes = numbytes;
 123
 124         if (unlikely(inputstate & INS_byte_stuff)) {
 125                 inputstate &= ~INS_byte_stuff;
 126                 goto byte_stuff;
 127         }
 128         for (;;) {
 129                 if (unlikely(c == PPP_ESCAPE)) {
 130                         if (unlikely(!numbytes)) {
 131                                 inputstate |= INS_byte_stuff;
 132                                 break;
 133                         }
 134                         c = *src++;
 135                         --numbytes;
 136                         if (unlikely(c == DLE_FLAG &&
 137                                      (cs->dle ||
 138                                       inbuf->inputstate & INS_DLE_command))) {
 139                                 inbuf->inputstate |= INS_DLE_char;
 140                                 inputstate |= INS_byte_stuff;
 141                                 break;
 142                         }
 143 byte_stuff:
 144                         c ^= PPP_TRANS;
 145                         if (unlikely(!muststuff(c)))
 146                                 gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c);
 147                 } else if (unlikely(c == PPP_FLAG)) {
 148                         if (unlikely(inputstate & INS_skip_frame)) {
 149 #ifdef CONFIG_GIGASET_DEBUG
 150                                 if (!(inputstate & INS_have_data)) { /* 7E 7E */
 151                                         ++bcs->emptycount;
 152                                 } else
 153                                         gig_dbg(DEBUG_HDLC,
 154                                             "7e----------------------------");
 155 #endif
 156
 157                                 /* end of frame */
 158                                 gigaset_isdn_rcv_err(bcs);
 159                                 dev_kfree_skb_any(skb);
 160                         } else if (!(inputstate & INS_have_data)) { /* 7E 7E */
 161 #ifdef CONFIG_GIGASET_DEBUG
 162                                 ++bcs->emptycount;
 163 #endif
 164                                 break;
 165                         } else {
 166                                 gig_dbg(DEBUG_HDLC,
 167                                         "7e----------------------------");
 168
 169                                 /* end of frame */
 170                                 if (unlikely(fcs != PPP_GOODFCS)) {
 171                                         dev_err(cs->dev,
 172                                 "Checksum failed, %u bytes corrupted!\n",
 173                                                 skb->len);
 174                                         gigaset_isdn_rcv_err(bcs);
 175                                         dev_kfree_skb_any(skb);
 176                                 } else if (likely(skb->len > 2)) {
 177                                         __skb_trim(skb, skb->len - 2);
 178                                         gigaset_skb_rcvd(bcs, skb);
 179                                 } else {
 180                                         if (skb->len) {
 181                                                 dev_err(cs->dev,
 182                                         "invalid packet size (%d)\n", skb->len);
 183                                                 gigaset_isdn_rcv_err(bcs);
 184                                         }
 185                                         dev_kfree_skb_any(skb);
 186                                 }
 187                         }
 188
 189                         fcs = PPP_INITFCS;
 190                         inputstate &= ~(INS_have_data | INS_skip_frame);
 191                         if (unlikely(bcs->ignore)) {
 192                                 inputstate |= INS_skip_frame;
 193                                 skb = NULL;
 194                         } else {
 195                                 skb = dev_alloc_skb(SBUFSIZE + cs->hw_hdr_len);
 196                                 if (skb != NULL) {
 197                                         skb_reserve(skb, cs->hw_hdr_len);
 198                                 } else {
 199                                         dev_warn(cs->dev,
 200                                                 "could not allocate new skb\n");
 201                                         inputstate |= INS_skip_frame;
 202                                 }
 203                         }
 204
 205                         break;
 206                 } else if (unlikely(muststuff(c))) {
 207                         /* Should not happen. Possible after ZDLE=1<CR><LF>. */
 208                         gig_dbg(DEBUG_HDLC, "not byte stuffed: 0x%02x", c);
 209                 }
 210
 211                 /* add character */
 212
 213 #ifdef CONFIG_GIGASET_DEBUG
 214                 if (unlikely(!(inputstate & INS_have_data))) {
 215                         gig_dbg(DEBUG_HDLC, "7e (%d x) ================",
 216                                 bcs->emptycount);
 217                         bcs->emptycount = 0;
 218                 }
 219 #endif
 220
 221                 inputstate |= INS_have_data;
 222
 223                 if (likely(!(inputstate & INS_skip_frame))) {
 224                         if (unlikely(skb->len == SBUFSIZE)) {
 225                                 dev_warn(cs->dev, "received packet too long\n");
 226                                 dev_kfree_skb_any(skb);
 227                                 skb = NULL;
 228                                 inputstate |= INS_skip_frame;
 229                                 break;
 230                         }
 231                         *__skb_put(skb, 1) = c;
 232                         fcs = crc_ccitt_byte(fcs, c);
 233                 }
 234
 235                 if (unlikely(!numbytes))
 236                         break;
 237                 c = *src++;
 238                 --numbytes;
 239                 if (unlikely(c == DLE_FLAG &&
 240                              (cs->dle ||
 241                               inbuf->inputstate & INS_DLE_command))) {
 242                         inbuf->inputstate |= INS_DLE_char;
 243                         break;
 244                 }
 245         }
 246         bcs->inputstate = inputstate;
 247         bcs->fcs = fcs;
 248         bcs->skb = skb;
 249         return startbytes - numbytes;
 250 }
 251
 252 /* process a block of received bytes in transparent data mode
 253  * Invert bytes, undoing byte stuffing and watching for DLE escapes.
 254  * If DLE is encountered, return immediately to let the caller handle it.
 255  * Return value:
 256  *      number of processed bytes
 257  *      numbytes (all bytes processed) on error --FIXME
 258  */
 259 static inline int iraw_loop(unsigned char c, unsigned char *src, int numbytes,
 260                             struct inbuf_t *inbuf)
 261 {
 262         struct cardstate *cs = inbuf->cs;
 263         struct bc_state *bcs = inbuf->bcs;
 264         int inputstate = bcs->inputstate;
 265         struct sk_buff *skb = bcs->skb;
 266         int startbytes = numbytes;
 267
 268         for (;;) {
 269                 /* add character */
 270                 inputstate |= INS_have_data;
 271
 272                 if (likely(!(inputstate & INS_skip_frame))) {
 273                         if (unlikely(skb->len == SBUFSIZE)) {
 274                                 //FIXME just pass skb up and allocate a new one
 275                                 dev_warn(cs->dev, "received packet too long\n");
 276                                 dev_kfree_skb_any(skb);
 277                                 skb = NULL;
 278                                 inputstate |= INS_skip_frame;
 279                                 break;
 280                         }
 281                         *__skb_put(skb, 1) = bitrev8(c);
 282                 }
 283
 284                 if (unlikely(!numbytes))
 285                         break;
 286                 c = *src++;
 287                 --numbytes;
 288                 if (unlikely(c == DLE_FLAG &&
 289                              (cs->dle ||
 290                               inbuf->inputstate & INS_DLE_command))) {
 291                         inbuf->inputstate |= INS_DLE_char;
 292                         break;
 293                 }
 294         }
 295
 296         /* pass data up */
 297         if (likely(inputstate & INS_have_data)) {
 298                 if (likely(!(inputstate & INS_skip_frame))) {
 299                         gigaset_skb_rcvd(bcs, skb);
 300                 }
 301                 inputstate &= ~(INS_have_data | INS_skip_frame);
 302                 if (unlikely(bcs->ignore)) {
 303                         inputstate |= INS_skip_frame;
 304                         skb = NULL;
 305                 } else {
 306                         skb = dev_alloc_skb(SBUFSIZE + cs->hw_hdr_len);
 307                         if (skb != NULL) {
 308                                 skb_reserve(skb, cs->hw_hdr_len);
 309                         } else {
 310                                 dev_warn(cs->dev,
 311                                          "could not allocate new skb\n");
 312                                 inputstate |= INS_skip_frame;
 313                         }
 314                 }
 315         }
 316
 317         bcs->inputstate = inputstate;
 318         bcs->skb = skb;
 319         return startbytes - numbytes;
 320 }
 321
 322 /**
 323  * gigaset_m10x_input() - process a block of data received from the device
 324  * @inbuf:      received data and device descriptor structure.
 325  *
 326  * Called by hardware module {ser,usb}_gigaset with a block of received
 327  * bytes. Separates the bytes received over the serial data channel into
 328  * user data and command replies (locked/unlocked) according to the
 329  * current state of the interface.
 330  */
 331 void gigaset_m10x_input(struct inbuf_t *inbuf)
 332 {
 333         struct cardstate *cs;
 334         unsigned tail, head, numbytes;
 335         unsigned char *src, c;
 336         int procbytes;
 337
 338         head = inbuf->head;
 339         tail = inbuf->tail;
 340         gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
 341
 342         if (head != tail) {
 343                 cs = inbuf->cs;
 344                 src = inbuf->data + head;
 345                 numbytes = (head > tail ? RBUFSIZE : tail) - head;
 346                 gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
 347
 348                 while (numbytes) {
 349                         if (cs->mstate == MS_LOCKED) {
 350                                 procbytes = lock_loop(src, numbytes, inbuf);
 351                                 src += procbytes;
 352                                 numbytes -= procbytes;
 353                         } else {
 354                                 c = *src++;
 355                                 --numbytes;
 356                                 if (c == DLE_FLAG && (cs->dle ||
 357                                     inbuf->inputstate & INS_DLE_command)) {
 358                                         if (!(inbuf->inputstate & INS_DLE_char)) {
 359                                                 inbuf->inputstate |= INS_DLE_char;
 360                                                 goto nextbyte;
 361                                         }
 362                                         /* <DLE> <DLE> => <DLE> in data stream */
 363                                         inbuf->inputstate &= ~INS_DLE_char;
 364                                 }
 365
 366                                 if (!(inbuf->inputstate & INS_DLE_char)) {
 367
 368                                         /* FIXME use function pointers?  */
 369                                         if (inbuf->inputstate & INS_command)
 370                                                 procbytes = cmd_loop(c, src, numbytes, inbuf);
 371                                         else if (inbuf->bcs->proto2 == L2_HDLC)
 372                                                 procbytes = hdlc_loop(c, src, numbytes, inbuf);
 373                                         else
 374                                                 procbytes = iraw_loop(c, src, numbytes, inbuf);
 375
 376                                         src += procbytes;
 377                                         numbytes -= procbytes;
 378                                 } else {  /* DLE char */
 379                                         inbuf->inputstate &= ~INS_DLE_char;
 380                                         switch (c) {
 381                                         case 'X': /*begin of command*/
 382                                                 if (inbuf->inputstate & INS_command)
 383                                                         dev_warn(cs->dev,
 384                                         "received <DLE> 'X' in command mode\n");
 385                                                 inbuf->inputstate |=
 386                                                         INS_command | INS_DLE_command;
 387                                                 break;
 388                                         case '.': /*end of command*/
 389                                                 if (!(inbuf->inputstate & INS_command))
 390                                                         dev_warn(cs->dev,
 391                                         "received <DLE> '.' in hdlc mode\n");
 392                                                 inbuf->inputstate &= cs->dle ?
 393                                                         ~(INS_DLE_command|INS_command)
 394                                                         : ~INS_DLE_command;
 395                                                 break;
 396                                         //case DLE_FLAG: /*DLE_FLAG in data stream*/ /* schon oben behandelt! */
 397                                         default:
 398                                                 dev_err(cs->dev,
 399                                                       "received 0x10 0x%02x!\n",
 400                                                         (int) c);
 401                                                 /* FIXME: reset driver?? */
 402                                         }
 403                                 }
 404                         }
 405 nextbyte:
 406                         if (!numbytes) {
 407                                 /* end of buffer, check for wrap */
 408                                 if (head > tail) {
 409                                         head = 0;
 410                                         src = inbuf->data;
 411                                         numbytes = tail;
 412                                 } else {
 413                                         head = tail;
 414                                         break;
 415                                 }
 416                         }
 417                 }
 418
 419                 gig_dbg(DEBUG_INTR, "setting head to %u", head);
 420                 inbuf->head = head;
 421         }
 422 }
 423 EXPORT_SYMBOL_GPL(gigaset_m10x_input);
 424
 425
 426 /* == data output ========================================================== */
 427
 428 /*
 429  * Encode a data packet into an octet stuffed HDLC frame with FCS,
 430  * opening and closing flags, preserving headroom data.
 431  * parameters:
 432  *      skb             skb containing original packet (freed upon return)
 433  * Return value:
 434  *      pointer to newly allocated skb containing the result frame
 435  *      and the original link layer header, NULL on error
 436  */
 437 static struct sk_buff *HDLC_Encode(struct sk_buff *skb)
 438 {
 439         struct sk_buff *hdlc_skb;
 440         __u16 fcs;
 441         unsigned char c;
 442         unsigned char *cp;
 443         int len;
 444         unsigned int stuf_cnt;
 445
 446         stuf_cnt = 0;
 447         fcs = PPP_INITFCS;
 448         cp = skb->data;
 449         len = skb->len;
 450         while (len--) {
 451                 if (muststuff(*cp))
 452                         stuf_cnt++;
 453                 fcs = crc_ccitt_byte(fcs, *cp++);
 454         }
 455         fcs ^= 0xffff;                  /* complement */
 456
 457         /* size of new buffer: original size + number of stuffing bytes
 458          * + 2 bytes FCS + 2 stuffing bytes for FCS (if needed) + 2 flag bytes
 459          * + room for link layer header
 460          */
 461         hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + skb->mac_len);
 462         if (!hdlc_skb) {
 463                 dev_kfree_skb_any(skb);
 464                 return NULL;
 465         }
 466
 467         /* Copy link layer header into new skb */
 468         skb_reset_mac_header(hdlc_skb);
 469         skb_reserve(hdlc_skb, skb->mac_len);
 470         memcpy(skb_mac_header(hdlc_skb), skb_mac_header(skb), skb->mac_len);
 471         hdlc_skb->mac_len = skb->mac_len;
 472
 473         /* Add flag sequence in front of everything.. */
 474         *(skb_put(hdlc_skb, 1)) = PPP_FLAG;
 475
 476         /* Perform byte stuffing while copying data. */
 477         while (skb->len--) {
 478                 if (muststuff(*skb->data)) {
 479                         *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
 480                         *(skb_put(hdlc_skb, 1)) = (*skb->data++) ^ PPP_TRANS;
 481                 } else
 482                         *(skb_put(hdlc_skb, 1)) = *skb->data++;
 483         }
 484
 485         /* Finally add FCS (byte stuffed) and flag sequence */
 486         c = (fcs & 0x00ff);     /* least significant byte first */
 487         if (muststuff(c)) {
 488                 *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
 489                 c ^= PPP_TRANS;
 490         }
 491         *(skb_put(hdlc_skb, 1)) = c;
 492
 493         c = ((fcs >> 8) & 0x00ff);
 494         if (muststuff(c)) {
 495                 *(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
 496                 c ^= PPP_TRANS;
 497         }
 498         *(skb_put(hdlc_skb, 1)) = c;
 499
 500         *(skb_put(hdlc_skb, 1)) = PPP_FLAG;
 501
 502         dev_kfree_skb_any(skb);
 503         return hdlc_skb;
 504 }
 505
 506 /*
 507  * Encode a data packet into an octet stuffed raw bit inverted frame,
 508  * preserving headroom data.
 509  * parameters:
 510  *      skb             skb containing original packet (freed upon return)
 511  * Return value:
 512  *      pointer to newly allocated skb containing the result frame
 513  *      and the original link layer header, NULL on error
 514  */
 515 static struct sk_buff *iraw_encode(struct sk_buff *skb)
 516 {
 517         struct sk_buff *iraw_skb;
 518         unsigned char c;
 519         unsigned char *cp;
 520         int len;
 521
 522         /* size of new buffer (worst case = every byte must be stuffed):
 523          * 2 * original size + room for link layer header
 524          */
 525         iraw_skb = dev_alloc_skb(2*skb->len + skb->mac_len);
 526         if (!iraw_skb) {
 527                 dev_kfree_skb_any(skb);
 528                 return NULL;
 529         }
 530
 531         /* copy link layer header into new skb */
 532         skb_reset_mac_header(iraw_skb);
 533         skb_reserve(iraw_skb, skb->mac_len);
 534         memcpy(skb_mac_header(iraw_skb), skb_mac_header(skb), skb->mac_len);
 535         iraw_skb->mac_len = skb->mac_len;
 536
 537         /* copy and stuff data */
 538         cp = skb->data;
 539         len = skb->len;
 540         while (len--) {
 541                 c = bitrev8(*cp++);
 542                 if (c == DLE_FLAG)
 543                         *(skb_put(iraw_skb, 1)) = c;
 544                 *(skb_put(iraw_skb, 1)) = c;
 545         }
 546         dev_kfree_skb_any(skb);
 547         return iraw_skb;
 548 }
 549
 550 /**
 551  * gigaset_m10x_send_skb() - queue an skb for sending
 552  * @bcs:        B channel descriptor structure.
 553  * @skb:        data to send.
 554  *
 555  * Called by LL to encode and queue an skb for sending, and start
 556  * transmission if necessary.
 557  * Once the payload data has been transmitted completely, gigaset_skb_sent()
 558  * will be called with the skb's link layer header preserved.
 559  *
 560  * Return value:
 561  *      number of bytes accepted for sending (skb->len) if ok,
 562  *      error code < 0 (eg. -ENOMEM) on error
 563  */
 564 int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb)
 565 {
 566         unsigned len = skb->len;
 567         unsigned long flags;
 568
 569         if (bcs->proto2 == L2_HDLC)
 570                 skb = HDLC_Encode(skb);
 571         else
 572                 skb = iraw_encode(skb);
 573         if (!skb) {
 574                 dev_err(bcs->cs->dev,
 575                         "unable to allocate memory for encoding!\n");
 576                 return -ENOMEM;
 577         }
 578
 579         skb_queue_tail(&bcs->squeue, skb);
 580         spin_lock_irqsave(&bcs->cs->lock, flags);
 581         if (bcs->cs->connected)
 582                 tasklet_schedule(&bcs->cs->write_tasklet);
 583         spin_unlock_irqrestore(&bcs->cs->lock, flags);
 584
 585         return len;     /* ok so far */
 586 }
 587 EXPORT_SYMBOL_GPL(gigaset_m10x_send_skb);