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