asyncdata.c 14.9 KB
Newer Older
1 2 3 4 5
/*
 * Common data handling layer for ser_gigaset and usb_gigaset
 *
 * Copyright (c) 2005 by Tilman Schmidt <tilman@imap.cc>,
 *                       Hansjoerg Lipp <hjlipp@web.de>,
6
 *                       Stefan Eilers.
7 8 9 10 11 12 13 14 15 16 17
 *
 * =====================================================================
 *	This program is free software; you can redistribute it and/or
 *	modify it under the terms of the GNU General Public License as
 *	published by the Free Software Foundation; either version 2 of
 *	the License, or (at your option) any later version.
 * =====================================================================
 */

#include "gigaset.h"
#include <linux/crc-ccitt.h>
18
#include <linux/bitrev.h>
19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44

//#define GIG_M10x_STUFF_VOICE_DATA

/* check if byte must be stuffed/escaped
 * I'm not sure which data should be encoded.
 * Therefore I will go the hard way and decode every value
 * less than 0x20, the flag sequence and the control escape char.
 */
static inline int muststuff(unsigned char c)
{
	if (c < PPP_TRANS) return 1;
	if (c == PPP_FLAG) return 1;
	if (c == PPP_ESCAPE) return 1;
	/* other possible candidates: */
	/* 0x91: XON with parity set */
	/* 0x93: XOFF with parity set */
	return 0;
}

/* == data input =========================================================== */

/* process a block of received bytes in command mode (modem response)
 * Return value:
 *	number of processed bytes
 */
static inline int cmd_loop(unsigned char c, unsigned char *src, int numbytes,
45
			   struct inbuf_t *inbuf)
46 47 48 49 50 51 52 53 54
{
	struct cardstate *cs = inbuf->cs;
	unsigned cbytes      = cs->cbytes;
	int inputstate = inbuf->inputstate;
	int startbytes = numbytes;

	for (;;) {
		cs->respdata[cbytes] = c;
		if (c == 10 || c == 13) {
55 56
			gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)",
				__func__, cbytes);
57
			cs->cbytes = cbytes;
58 59
			gigaset_handle_modem_response(cs); /* can change
							      cs->dle */
60 61 62 63 64 65 66 67 68 69 70 71
			cbytes = 0;

			if (cs->dle &&
			    !(inputstate & INS_DLE_command)) {
				inputstate &= ~INS_command;
				break;
			}
		} else {
			/* advance in line buffer, checking for overflow */
			if (cbytes < MAX_RESP_SIZE - 1)
				cbytes++;
			else
72
				dev_warn(cs->dev, "response too large\n");
73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96
		}

		if (!numbytes)
			break;
		c = *src++;
		--numbytes;
		if (c == DLE_FLAG &&
		    (cs->dle || inputstate & INS_DLE_command)) {
			inputstate |= INS_DLE_char;
			break;
		}
	}

	cs->cbytes = cbytes;
	inbuf->inputstate = inputstate;

	return startbytes - numbytes;
}

/* process a block of received bytes in lock mode (tty i/f)
 * Return value:
 *	number of processed bytes
 */
static inline int lock_loop(unsigned char *src, int numbytes,
97
			    struct inbuf_t *inbuf)
98 99 100
{
	struct cardstate *cs = inbuf->cs;

101
	gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
102
			   numbytes, src);
103 104 105 106 107 108 109 110 111 112 113 114 115 116
	gigaset_if_receive(cs, src, numbytes);

	return numbytes;
}

/* process a block of received bytes in HDLC data mode
 * Collect HDLC frames, undoing byte stuffing and watching for DLE escapes.
 * When a frame is complete, check the FCS and pass valid frames to the LL.
 * If DLE is encountered, return immediately to let the caller handle it.
 * Return value:
 *	number of processed bytes
 *	numbytes (all bytes processed) on error --FIXME
 */
static inline int hdlc_loop(unsigned char c, unsigned char *src, int numbytes,
117
			    struct inbuf_t *inbuf)
118 119 120
{
	struct cardstate *cs = inbuf->cs;
	struct bc_state *bcs = inbuf->bcs;
121 122 123
	int inputstate = bcs->inputstate;
	__u16 fcs = bcs->fcs;
	struct sk_buff *skb = bcs->skb;
124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151
	unsigned char error;
	struct sk_buff *compskb;
	int startbytes = numbytes;
	int l;

	if (unlikely(inputstate & INS_byte_stuff)) {
		inputstate &= ~INS_byte_stuff;
		goto byte_stuff;
	}
	for (;;) {
		if (unlikely(c == PPP_ESCAPE)) {
			if (unlikely(!numbytes)) {
				inputstate |= INS_byte_stuff;
				break;
			}
			c = *src++;
			--numbytes;
			if (unlikely(c == DLE_FLAG &&
				     (cs->dle ||
				      inbuf->inputstate & INS_DLE_command))) {
				inbuf->inputstate |= INS_DLE_char;
				inputstate |= INS_byte_stuff;
				break;
			}
byte_stuff:
			c ^= PPP_TRANS;
#ifdef CONFIG_GIGASET_DEBUG
			if (unlikely(!muststuff(c)))
152
				gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c);
153 154 155 156 157 158 159 160
#endif
		} else if (unlikely(c == PPP_FLAG)) {
			if (unlikely(inputstate & INS_skip_frame)) {
				if (!(inputstate & INS_have_data)) { /* 7E 7E */
#ifdef CONFIG_GIGASET_DEBUG
					++bcs->emptycount;
#endif
				} else
161
					gig_dbg(DEBUG_HDLC,
162 163 164 165 166 167 168 169 170 171 172
					    "7e----------------------------");

				/* end of frame */
				error = 1;
				gigaset_rcv_error(NULL, cs, bcs);
			} else if (!(inputstate & INS_have_data)) { /* 7E 7E */
#ifdef CONFIG_GIGASET_DEBUG
				++bcs->emptycount;
#endif
				break;
			} else {
173 174
				gig_dbg(DEBUG_HDLC,
					"7e----------------------------");
175 176 177 178 179

				/* end of frame */
				error = 0;

				if (unlikely(fcs != PPP_GOODFCS)) {
180 181 182
					dev_err(cs->dev,
					    "Packet checksum at %lu failed, "
					    "packet is corrupted (%u bytes)!\n",
183 184 185 186 187 188 189 190 191 192 193 194 195
					    bcs->rcvbytes, skb->len);
					compskb = NULL;
					gigaset_rcv_error(compskb, cs, bcs);
					error = 1;
				} else {
					if (likely((l = skb->len) > 2)) {
						skb->tail -= 2;
						skb->len -= 2;
					} else {
						dev_kfree_skb(skb);
						skb = NULL;
						inputstate |= INS_skip_frame;
						if (l == 1) {
196 197
							dev_err(cs->dev,
						  "invalid packet size (1)!\n");
198
							error = 1;
199 200
							gigaset_rcv_error(NULL,
								cs, bcs);
201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222
						}
					}
					if (likely(!(error ||
						     (inputstate &
						      INS_skip_frame)))) {
						gigaset_rcv_skb(skb, cs, bcs);
					}
				}
			}

			if (unlikely(error))
				if (skb)
					dev_kfree_skb(skb);

			fcs = PPP_INITFCS;
			inputstate &= ~(INS_have_data | INS_skip_frame);
			if (unlikely(bcs->ignore)) {
				inputstate |= INS_skip_frame;
				skb = NULL;
			} else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)) {
				skb_reserve(skb, HW_HDR_LEN);
			} else {
223 224
				dev_warn(cs->dev,
					 "could not allocate new skb\n");
225 226 227 228 229 230 231
				inputstate |= INS_skip_frame;
			}

			break;
#ifdef CONFIG_GIGASET_DEBUG
		} else if (unlikely(muststuff(c))) {
			/* Should not happen. Possible after ZDLE=1<CR><LF>. */
232
			gig_dbg(DEBUG_HDLC, "not byte stuffed: 0x%02x", c);
233 234 235 236 237 238 239
#endif
		}

		/* add character */

#ifdef CONFIG_GIGASET_DEBUG
		if (unlikely(!(inputstate & INS_have_data))) {
240 241
			gig_dbg(DEBUG_HDLC, "7e (%d x) ================",
				bcs->emptycount);
242 243 244 245 246 247 248 249
			bcs->emptycount = 0;
		}
#endif

		inputstate |= INS_have_data;

		if (likely(!(inputstate & INS_skip_frame))) {
			if (unlikely(skb->len == SBUFSIZE)) {
250
				dev_warn(cs->dev, "received packet too long\n");
251 252 253 254 255
				dev_kfree_skb_any(skb);
				skb = NULL;
				inputstate |= INS_skip_frame;
				break;
			}
256
			*__skb_put(skb, 1) = c;
257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284
			fcs = crc_ccitt_byte(fcs, c);
		}

		if (unlikely(!numbytes))
			break;
		c = *src++;
		--numbytes;
		if (unlikely(c == DLE_FLAG &&
			     (cs->dle ||
			      inbuf->inputstate & INS_DLE_command))) {
			inbuf->inputstate |= INS_DLE_char;
			break;
		}
	}
	bcs->inputstate = inputstate;
	bcs->fcs = fcs;
	bcs->skb = skb;
	return startbytes - numbytes;
}

/* process a block of received bytes in transparent data mode
 * Invert bytes, undoing byte stuffing and watching for DLE escapes.
 * If DLE is encountered, return immediately to let the caller handle it.
 * Return value:
 *	number of processed bytes
 *	numbytes (all bytes processed) on error --FIXME
 */
static inline int iraw_loop(unsigned char c, unsigned char *src, int numbytes,
285
			    struct inbuf_t *inbuf)
286 287 288
{
	struct cardstate *cs = inbuf->cs;
	struct bc_state *bcs = inbuf->bcs;
289 290
	int inputstate = bcs->inputstate;
	struct sk_buff *skb = bcs->skb;
291 292 293 294 295 296 297 298 299
	int startbytes = numbytes;

	for (;;) {
		/* add character */
		inputstate |= INS_have_data;

		if (likely(!(inputstate & INS_skip_frame))) {
			if (unlikely(skb->len == SBUFSIZE)) {
				//FIXME just pass skb up and allocate a new one
300
				dev_warn(cs->dev, "received packet too long\n");
301 302 303 304 305
				dev_kfree_skb_any(skb);
				skb = NULL;
				inputstate |= INS_skip_frame;
				break;
			}
306
			*__skb_put(skb, 1) = bitrev8(c);
307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333
		}

		if (unlikely(!numbytes))
			break;
		c = *src++;
		--numbytes;
		if (unlikely(c == DLE_FLAG &&
			     (cs->dle ||
			      inbuf->inputstate & INS_DLE_command))) {
			inbuf->inputstate |= INS_DLE_char;
			break;
		}
	}

	/* pass data up */
	if (likely(inputstate & INS_have_data)) {
		if (likely(!(inputstate & INS_skip_frame))) {
			gigaset_rcv_skb(skb, cs, bcs);
		}
		inputstate &= ~(INS_have_data | INS_skip_frame);
		if (unlikely(bcs->ignore)) {
			inputstate |= INS_skip_frame;
			skb = NULL;
		} else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN))
				  != NULL)) {
			skb_reserve(skb, HW_HDR_LEN);
		} else {
334
			dev_warn(cs->dev, "could not allocate new skb\n");
335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352
			inputstate |= INS_skip_frame;
		}
	}

	bcs->inputstate = inputstate;
	bcs->skb = skb;
	return startbytes - numbytes;
}

/* process a block of data received from the device
 */
void gigaset_m10x_input(struct inbuf_t *inbuf)
{
	struct cardstate *cs;
	unsigned tail, head, numbytes;
	unsigned char *src, c;
	int procbytes;

Tilman Schmidt's avatar
Tilman Schmidt committed
353 354
	head = inbuf->head;
	tail = inbuf->tail;
355
	gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
356 357 358 359 360

	if (head != tail) {
		cs = inbuf->cs;
		src = inbuf->data + head;
		numbytes = (head > tail ? RBUFSIZE : tail) - head;
361
		gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
362 363

		while (numbytes) {
Tilman Schmidt's avatar
Tilman Schmidt committed
364
			if (cs->mstate == MS_LOCKED) {
365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382
				procbytes = lock_loop(src, numbytes, inbuf);
				src += procbytes;
				numbytes -= procbytes;
			} else {
				c = *src++;
				--numbytes;
				if (c == DLE_FLAG && (cs->dle ||
				    inbuf->inputstate & INS_DLE_command)) {
					if (!(inbuf->inputstate & INS_DLE_char)) {
						inbuf->inputstate |= INS_DLE_char;
						goto nextbyte;
					}
					/* <DLE> <DLE> => <DLE> in data stream */
					inbuf->inputstate &= ~INS_DLE_char;
				}

				if (!(inbuf->inputstate & INS_DLE_char)) {

383
					/* FIXME use function pointers?  */
384 385 386 387 388 389 390 391 392
					if (inbuf->inputstate & INS_command)
						procbytes = cmd_loop(c, src, numbytes, inbuf);
					else if (inbuf->bcs->proto2 == ISDN_PROTO_L2_HDLC)
						procbytes = hdlc_loop(c, src, numbytes, inbuf);
					else
						procbytes = iraw_loop(c, src, numbytes, inbuf);

					src += procbytes;
					numbytes -= procbytes;
393
				} else {  /* DLE char */
394 395 396 397 398
					inbuf->inputstate &= ~INS_DLE_char;
					switch (c) {
					case 'X': /*begin of command*/
#ifdef CONFIG_GIGASET_DEBUG
						if (inbuf->inputstate & INS_command)
399 400
							dev_err(cs->dev,
					"received <DLE> 'X' in command mode\n");
401 402 403 404 405 406 407
#endif
						inbuf->inputstate |=
							INS_command | INS_DLE_command;
						break;
					case '.': /*end of command*/
#ifdef CONFIG_GIGASET_DEBUG
						if (!(inbuf->inputstate & INS_command))
408 409
							dev_err(cs->dev,
					"received <DLE> '.' in hdlc mode\n");
410 411 412 413 414 415 416
#endif
						inbuf->inputstate &= cs->dle ?
							~(INS_DLE_command|INS_command)
							: ~INS_DLE_command;
						break;
					//case DLE_FLAG: /*DLE_FLAG in data stream*/ /* schon oben behandelt! */
					default:
417 418 419
						dev_err(cs->dev,
						      "received 0x10 0x%02x!\n",
							(int) c);
420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437
						/* FIXME: reset driver?? */
					}
				}
			}
nextbyte:
			if (!numbytes) {
				/* end of buffer, check for wrap */
				if (head > tail) {
					head = 0;
					src = inbuf->data;
					numbytes = tail;
				} else {
					head = tail;
					break;
				}
			}
		}

438
		gig_dbg(DEBUG_INTR, "setting head to %u", head);
Tilman Schmidt's avatar
Tilman Schmidt committed
439
		inbuf->head = head;
440 441
	}
}
442
EXPORT_SYMBOL_GPL(gigaset_m10x_input);
443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473


/* == data output ========================================================== */

/* Encoding of a PPP packet into an octet stuffed HDLC frame
 * with FCS, opening and closing flags.
 * parameters:
 *	skb	skb containing original packet (freed upon return)
 *	head	number of headroom bytes to allocate in result skb
 *	tail	number of tailroom bytes to allocate in result skb
 * Return value:
 *	pointer to newly allocated skb containing the result frame
 */
static struct sk_buff *HDLC_Encode(struct sk_buff *skb, int head, int tail)
{
	struct sk_buff *hdlc_skb;
	__u16 fcs;
	unsigned char c;
	unsigned char *cp;
	int len;
	unsigned int stuf_cnt;

	stuf_cnt = 0;
	fcs = PPP_INITFCS;
	cp = skb->data;
	len = skb->len;
	while (len--) {
		if (muststuff(*cp))
			stuf_cnt++;
		fcs = crc_ccitt_byte(fcs, *cp++);
	}
474
	fcs ^= 0xffff;			/* complement */
475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501

	/* size of new buffer: original size + number of stuffing bytes
	 * + 2 bytes FCS + 2 stuffing bytes for FCS (if needed) + 2 flag bytes
	 */
	hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + tail + head);
	if (!hdlc_skb) {
		dev_kfree_skb(skb);
		return NULL;
	}
	skb_reserve(hdlc_skb, head);

	/* Copy acknowledge request into new skb */
	memcpy(hdlc_skb->head, skb->head, 2);

	/* Add flag sequence in front of everything.. */
	*(skb_put(hdlc_skb, 1)) = PPP_FLAG;

	/* Perform byte stuffing while copying data. */
	while (skb->len--) {
		if (muststuff(*skb->data)) {
			*(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
			*(skb_put(hdlc_skb, 1)) = (*skb->data++) ^ PPP_TRANS;
		} else
			*(skb_put(hdlc_skb, 1)) = *skb->data++;
	}

	/* Finally add FCS (byte stuffed) and flag sequence */
502
	c = (fcs & 0x00ff);	/* least significant byte first */
503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547
	if (muststuff(c)) {
		*(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
		c ^= PPP_TRANS;
	}
	*(skb_put(hdlc_skb, 1)) = c;

	c = ((fcs >> 8) & 0x00ff);
	if (muststuff(c)) {
		*(skb_put(hdlc_skb, 1)) = PPP_ESCAPE;
		c ^= PPP_TRANS;
	}
	*(skb_put(hdlc_skb, 1)) = c;

	*(skb_put(hdlc_skb, 1)) = PPP_FLAG;

	dev_kfree_skb(skb);
	return hdlc_skb;
}

/* Encoding of a raw packet into an octet stuffed bit inverted frame
 * parameters:
 *	skb	skb containing original packet (freed upon return)
 *	head	number of headroom bytes to allocate in result skb
 *	tail	number of tailroom bytes to allocate in result skb
 * Return value:
 *	pointer to newly allocated skb containing the result frame
 */
static struct sk_buff *iraw_encode(struct sk_buff *skb, int head, int tail)
{
	struct sk_buff *iraw_skb;
	unsigned char c;
	unsigned char *cp;
	int len;

	/* worst case: every byte must be stuffed */
	iraw_skb = dev_alloc_skb(2*skb->len + tail + head);
	if (!iraw_skb) {
		dev_kfree_skb(skb);
		return NULL;
	}
	skb_reserve(iraw_skb, head);

	cp = skb->data;
	len = skb->len;
	while (len--) {
548
		c = bitrev8(*cp++);
549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569
		if (c == DLE_FLAG)
			*(skb_put(iraw_skb, 1)) = c;
		*(skb_put(iraw_skb, 1)) = c;
	}
	dev_kfree_skb(skb);
	return iraw_skb;
}

/* gigaset_send_skb
 * called by common.c to queue an skb for sending
 * and start transmission if necessary
 * parameters:
 *	B Channel control structure
 *	skb
 * Return value:
 *	number of bytes accepted for sending
 *	(skb->len if ok, 0 if out of buffer space)
 *	or error code (< 0, eg. -EINVAL)
 */
int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb)
{
570
	unsigned len = skb->len;
571
	unsigned long flags;
572 573 574 575 576

	if (bcs->proto2 == ISDN_PROTO_L2_HDLC)
		skb = HDLC_Encode(skb, HW_HDR_LEN, 0);
	else
		skb = iraw_encode(skb, HW_HDR_LEN, 0);
577
	if (!skb) {
578
		err("unable to allocate memory for encoding!\n");
579
		return -ENOMEM;
580
	}
581 582

	skb_queue_tail(&bcs->squeue, skb);
583 584 585 586
	spin_lock_irqsave(&bcs->cs->lock, flags);
	if (bcs->cs->connected)
		tasklet_schedule(&bcs->cs->write_tasklet);
	spin_unlock_irqrestore(&bcs->cs->lock, flags);
587 588 589

	return len;	/* ok so far */
}
590
EXPORT_SYMBOL_GPL(gigaset_m10x_send_skb);