1 files changed, 131 insertions, 88 deletions

diff --git a/drivers/tty/n_gsm.c b/drivers/tty/n_gsm.c
index 81b46585edf7..44b8412a04e8 100644
--- a/drivers/tty/n_gsm.c
+++ b/drivers/tty/n_gsm.c
@@ -19,7 +19,7 @@
  *
  * TO DO:
  *	Mostly done:	ioctls for setting modes/timing
- *	Partly done: 	hooks so you can pull off frames to non tty devs
+ *	Partly done:	hooks so you can pull off frames to non tty devs
  *	Restart DLCI 0 when it closes ?
  *	Test basic encoding
  *	Improve the tx engine
@@ -73,8 +73,10 @@ module_param(debug, int, 0600);
 #define T2	(2 * HZ)
 #endif
 
-/* Semi-arbitary buffer size limits. 0710 is normally run with 32-64 byte
-   limits so this is plenty */
+/*
+ * Semi-arbitary buffer size limits. 0710 is normally run with 32-64 byte
+ * limits so this is plenty
+ */
 #define MAX_MRU 512
 #define MAX_MTU 512
 
@@ -184,6 +186,9 @@ struct gsm_mux {
 #define GSM_DATA		5
 #define GSM_FCS			6
 #define GSM_OVERRUN		7
+#define GSM_LEN0		8
+#define GSM_LEN1		9
+#define GSM_SSOF		10
 	unsigned int len;
 	unsigned int address;
 	unsigned int count;
@@ -191,6 +196,7 @@ struct gsm_mux {
 	int encoding;
 	u8 control;
 	u8 fcs;
+	u8 received_fcs;
 	u8 *txframe;			/* TX framing buffer */
 
 	/* Methods for the receiver side */
@@ -286,7 +292,7 @@ static spinlock_t gsm_mux_lock;
 #define MDM_DV			0x40
 
 #define GSM0_SOF		0xF9
-#define GSM1_SOF 		0x7E
+#define GSM1_SOF		0x7E
 #define GSM1_ESCAPE		0x7D
 #define GSM1_ESCAPE_BITS	0x20
 #define XON			0x11
@@ -429,61 +435,63 @@ static void gsm_print_packet(const char *hdr, int addr, int cr,
 	if (!(debug & 1))
 		return;
 
-	printk(KERN_INFO "%s %d) %c: ", hdr, addr, "RC"[cr]);
+	pr_info("%s %d) %c: ", hdr, addr, "RC"[cr]);
 
 	switch (control & ~PF) {
 	case SABM:
-		printk(KERN_CONT "SABM");
+		pr_cont("SABM");
 		break;
 	case UA:
-		printk(KERN_CONT "UA");
+		pr_cont("UA");
 		break;
 	case DISC:
-		printk(KERN_CONT "DISC");
+		pr_cont("DISC");
 		break;
 	case DM:
-		printk(KERN_CONT "DM");
+		pr_cont("DM");
 		break;
 	case UI:
-		printk(KERN_CONT "UI");
+		pr_cont("UI");
 		break;
 	case UIH:
-		printk(KERN_CONT "UIH");
+		pr_cont("UIH");
 		break;
 	default:
 		if (!(control & 0x01)) {
-			printk(KERN_CONT "I N(S)%d N(R)%d",
-				(control & 0x0E) >> 1, (control & 0xE)>> 5);
+			pr_cont("I N(S)%d N(R)%d",
+				(control & 0x0E) >> 1, (control & 0xE) >> 5);
 		} else switch (control & 0x0F) {
-		case RR:
-			printk("RR(%d)", (control & 0xE0) >> 5);
-			break;
-		case RNR:
-			printk("RNR(%d)", (control & 0xE0) >> 5);
-			break;
-		case REJ:
-			printk("REJ(%d)", (control & 0xE0) >> 5);
-			break;
-		default:
-			printk(KERN_CONT "[%02X]", control);
+			case RR:
+				pr_cont("RR(%d)", (control & 0xE0) >> 5);
+				break;
+			case RNR:
+				pr_cont("RNR(%d)", (control & 0xE0) >> 5);
+				break;
+			case REJ:
+				pr_cont("REJ(%d)", (control & 0xE0) >> 5);
+				break;
+			default:
+				pr_cont("[%02X]", control);
 		}
 	}
 
 	if (control & PF)
-		printk(KERN_CONT "(P)");
+		pr_cont("(P)");
 	else
-		printk(KERN_CONT "(F)");
+		pr_cont("(F)");
 
 	if (dlen) {
 		int ct = 0;
 		while (dlen--) {
-			if (ct % 8 == 0)
-				printk(KERN_CONT "\n    ");
-			printk(KERN_CONT "%02X ", *data++);
+			if (ct % 8 == 0) {
+				pr_cont("\n");
+				pr_debug("    ");
+			}
+			pr_cont("%02X ", *data++);
 			ct++;
 		}
 	}
-	printk(KERN_CONT "\n");
+	pr_cont("\n");
 }
 
 
@@ -522,11 +530,13 @@ static void hex_packet(const unsigned char *p, int len)
 {
 	int i;
 	for (i = 0; i < len; i++) {
-		if (i && (i % 16) == 0)
-			printk("\n");
-		printk("%02X ", *p++);
+		if (i && (i % 16) == 0) {
+			pr_cont("\n");
+			pr_debug("");
+		}
+		pr_cont("%02X ", *p++);
 	}
-	printk("\n");
+	pr_cont("\n");
 }
 
 /**
@@ -676,7 +686,7 @@ static void gsm_data_kick(struct gsm_mux *gsm)
 		}
 
 		if (debug & 4) {
-			printk("gsm_data_kick: \n");
+			pr_debug("gsm_data_kick:\n");
 			hex_packet(gsm->txframe, len);
 		}
 
@@ -716,8 +726,8 @@ static void __gsm_data_queue(struct gsm_dlci *dlci, struct gsm_msg *msg)
 		if (msg->len < 128)
 			*--dp = (msg->len << 1) | EA;
 		else {
-			*--dp = ((msg->len & 127) << 1) | EA;
-			*--dp = (msg->len >> 6) & 0xfe;
+			*--dp = (msg->len >> 7);	/* bits 7 - 15 */
+			*--dp = (msg->len & 127) << 1;	/* bits 0 - 6 */
 		}
 	}
 
@@ -968,6 +978,8 @@ static void gsm_control_reply(struct gsm_mux *gsm, int cmd, u8 *data,
 {
 	struct gsm_msg *msg;
 	msg = gsm_data_alloc(gsm, 0, dlen + 2, gsm->ftype);
+	if (msg == NULL)
+		return;
 	msg->data[0] = (cmd & 0xFE) << 1 | EA;	/* Clear C/R */
 	msg->data[1] = (dlen << 1) | EA;
 	memcpy(msg->data + 2, data, dlen);
@@ -1229,7 +1241,7 @@ static void gsm_control_response(struct gsm_mux *gsm, unsigned int command,
 }
 
 /**
- *	gsm_control_transmit 	-	send control packet
+ *	gsm_control_transmit	-	send control packet
  *	@gsm: gsm mux
  *	@ctrl: frame to send
  *
@@ -1359,7 +1371,7 @@ static void gsm_dlci_close(struct gsm_dlci *dlci)
 {
 	del_timer(&dlci->t1);
 	if (debug & 8)
-		printk("DLCI %d goes closed.\n", dlci->addr);
+		pr_debug("DLCI %d goes closed.\n", dlci->addr);
 	dlci->state = DLCI_CLOSED;
 	if (dlci->addr != 0) {
 		struct tty_struct  *tty = tty_port_tty_get(&dlci->port);
@@ -1390,7 +1402,7 @@ static void gsm_dlci_open(struct gsm_dlci *dlci)
 	/* This will let a tty open continue */
 	dlci->state = DLCI_OPEN;
 	if (debug & 8)
-		printk("DLCI %d goes open.\n", dlci->addr);
+		pr_debug("DLCI %d goes open.\n", dlci->addr);
 	wake_up(&dlci->gsm->event);
 }
 
@@ -1492,29 +1504,29 @@ static void gsm_dlci_data(struct gsm_dlci *dlci, u8 *data, int len)
 	unsigned int modem = 0;
 
 	if (debug & 16)
-		printk("%d bytes for tty %p\n", len, tty);
+		pr_debug("%d bytes for tty %p\n", len, tty);
 	if (tty) {
 		switch (dlci->adaption)  {
-			/* Unsupported types */
-			/* Packetised interruptible data */
-			case 4:
-				break;
-			/* Packetised uininterruptible voice/data */
-			case 3:
-				break;
-			/* Asynchronous serial with line state in each frame */
-			case 2:
-				while (gsm_read_ea(&modem, *data++) == 0) {
-					len--;
-					if (len == 0)
-						return;
-				}
-				gsm_process_modem(tty, dlci, modem);
-			/* Line state will go via DLCI 0 controls only */
-			case 1:
-			default:
-				tty_insert_flip_string(tty, data, len);
-				tty_flip_buffer_push(tty);
+		/* Unsupported types */
+		/* Packetised interruptible data */
+		case 4:
+			break;
+		/* Packetised uininterruptible voice/data */
+		case 3:
+			break;
+		/* Asynchronous serial with line state in each frame */
+		case 2:
+			while (gsm_read_ea(&modem, *data++) == 0) {
+				len--;
+				if (len == 0)
+					return;
+			}
+			gsm_process_modem(tty, dlci, modem);
+		/* Line state will go via DLCI 0 controls only */
+		case 1:
+		default:
+			tty_insert_flip_string(tty, data, len);
+			tty_flip_buffer_push(tty);
 		}
 		tty_kref_put(tty);
 	}
@@ -1623,7 +1635,6 @@ static void gsm_dlci_free(struct gsm_dlci *dlci)
 	kfree(dlci);
 }
 
-
 /*
  *	LAPBish link layer logic
  */
@@ -1648,10 +1659,12 @@ static void gsm_queue(struct gsm_mux *gsm)
 
 	if ((gsm->control & ~PF) == UI)
 		gsm->fcs = gsm_fcs_add_block(gsm->fcs, gsm->buf, gsm->len);
+	/* generate final CRC with received FCS */
+	gsm->fcs = gsm_fcs_add(gsm->fcs, gsm->received_fcs);
 	if (gsm->fcs != GOOD_FCS) {
 		gsm->bad_fcs++;
 		if (debug & 4)
-			printk("BAD FCS %02x\n", gsm->fcs);
+			pr_debug("BAD FCS %02x\n", gsm->fcs);
 		return;
 	}
 	address = gsm->address >> 1;
@@ -1746,6 +1759,8 @@ invalid:
 
 static void gsm0_receive(struct gsm_mux *gsm, unsigned char c)
 {
+	unsigned int len;
+
 	switch (gsm->state) {
 	case GSM_SEARCH:	/* SOF marker */
 		if (c == GSM0_SOF) {
@@ -1754,8 +1769,8 @@ static void gsm0_receive(struct gsm_mux *gsm, unsigned char c)
 			gsm->len = 0;
 			gsm->fcs = INIT_FCS;
 		}
-		break;		/* Address EA */
-	case GSM_ADDRESS:
+		break;
+	case GSM_ADDRESS:	/* Address EA */
 		gsm->fcs = gsm_fcs_add(gsm->fcs, c);
 		if (gsm_read_ea(&gsm->address, c))
 			gsm->state = GSM_CONTROL;
@@ -1763,9 +1778,9 @@ static void gsm0_receive(struct gsm_mux *gsm, unsigned char c)
 	case GSM_CONTROL:	/* Control Byte */
 		gsm->fcs = gsm_fcs_add(gsm->fcs, c);
 		gsm->control = c;
-		gsm->state = GSM_LEN;
+		gsm->state = GSM_LEN0;
 		break;
-	case GSM_LEN:		/* Length EA */
+	case GSM_LEN0:		/* Length EA */
 		gsm->fcs = gsm_fcs_add(gsm->fcs, c);
 		if (gsm_read_ea(&gsm->len, c)) {
 			if (gsm->len > gsm->mru) {
@@ -1774,8 +1789,28 @@ static void gsm0_receive(struct gsm_mux *gsm, unsigned char c)
 				break;
 			}
 			gsm->count = 0;
-			gsm->state = GSM_DATA;
+			if (!gsm->len)
+				gsm->state = GSM_FCS;
+			else
+				gsm->state = GSM_DATA;
+			break;
+		}
+		gsm->state = GSM_LEN1;
+		break;
+	case GSM_LEN1:
+		gsm->fcs = gsm_fcs_add(gsm->fcs, c);
+		len = c;
+		gsm->len |= len << 7;
+		if (gsm->len > gsm->mru) {
+			gsm->bad_size++;
+			gsm->state = GSM_SEARCH;
+			break;
 		}
+		gsm->count = 0;
+		if (!gsm->len)
+			gsm->state = GSM_FCS;
+		else
+			gsm->state = GSM_DATA;
 		break;
 	case GSM_DATA:		/* Data */
 		gsm->buf[gsm->count++] = c;
@@ -1783,16 +1818,25 @@ static void gsm0_receive(struct gsm_mux *gsm, unsigned char c)
 			gsm->state = GSM_FCS;
 		break;
 	case GSM_FCS:		/* FCS follows the packet */
-		gsm->fcs = c;
+		gsm->received_fcs = c;
+		if (c == GSM0_SOF) {
+			gsm->state = GSM_SEARCH;
+			break;
+		}
 		gsm_queue(gsm);
-		/* And then back for the next frame */
-		gsm->state = GSM_SEARCH;
+		gsm->state = GSM_SSOF;
+		break;
+	case GSM_SSOF:
+		if (c == GSM0_SOF) {
+			gsm->state = GSM_SEARCH;
+			break;
+		}
 		break;
 	}
 }
 
 /**
- *	gsm0_receive	-	perform processing for non-transparency
+ *	gsm1_receive	-	perform processing for non-transparency
  *	@gsm: gsm data for this ldisc instance
  *	@c: character
  *
@@ -1854,7 +1898,7 @@ static void gsm1_receive(struct gsm_mux *gsm, unsigned char c)
 		gsm->state = GSM_DATA;
 		break;
 	case GSM_DATA:		/* Data */
-		if (gsm->count > gsm->mru ) {	/* Allow one for the FCS */
+		if (gsm->count > gsm->mru) {	/* Allow one for the FCS */
 			gsm->state = GSM_OVERRUN;
 			gsm->bad_size++;
 		} else
@@ -2032,9 +2076,6 @@ struct gsm_mux *gsm_alloc_mux(void)
 }
 EXPORT_SYMBOL_GPL(gsm_alloc_mux);
 
-
-
-
 /**
  *	gsmld_output		-	write to link
  *	@gsm: our mux
@@ -2052,7 +2093,7 @@ static int gsmld_output(struct gsm_mux *gsm, u8 *data, int len)
 		return -ENOSPC;
 	}
 	if (debug & 4) {
-		printk("-->%d bytes out\n", len);
+		pr_debug("-->%d bytes out\n", len);
 		hex_packet(data, len);
 	}
 	gsm->tty->ops->write(gsm->tty, data, len);
@@ -2109,7 +2150,7 @@ static void gsmld_receive_buf(struct tty_struct *tty, const unsigned char *cp,
 	char flags;
 
 	if (debug & 4) {
-		printk("Inbytes %dd\n", count);
+		pr_debug("Inbytes %dd\n", count);
 		hex_packet(cp, count);
 	}
 
@@ -2126,7 +2167,7 @@ static void gsmld_receive_buf(struct tty_struct *tty, const unsigned char *cp,
 			gsm->error(gsm, *dp, flags);
 			break;
 		default:
-			printk(KERN_ERR "%s: unknown flag %d\n",
+			WARN_ONCE("%s: unknown flag %d\n",
 			       tty_name(tty, buf), flags);
 			break;
 		}
@@ -2321,7 +2362,7 @@ static int gsmld_config(struct tty_struct *tty, struct gsm_mux *gsm,
 	int need_restart = 0;
 
 	/* Stuff we don't support yet - UI or I frame transport, windowing */
-	if ((c->adaption !=1 && c->adaption != 2) || c->k)
+	if ((c->adaption != 1 && c->adaption != 2) || c->k)
 		return -EOPNOTSUPP;
 	/* Check the MRU/MTU range looks sane */
 	if (c->mru > MAX_MRU || c->mtu > MAX_MTU || c->mru < 8 || c->mtu < 8)
@@ -2416,7 +2457,7 @@ static int gsmld_ioctl(struct tty_struct *tty, struct file *file,
 			c.i = 1;
 		else
 			c.i = 2;
-		printk("Ftype %d i %d\n", gsm->ftype, c.i);
+		pr_debug("Ftype %d i %d\n", gsm->ftype, c.i);
 		c.mru = gsm->mru;
 		c.mtu = gsm->mtu;
 		c.k = 0;
@@ -2710,14 +2751,15 @@ static int __init gsm_init(void)
 	/* Fill in our line protocol discipline, and register it */
 	int status = tty_register_ldisc(N_GSM0710, &tty_ldisc_packet);
 	if (status != 0) {
-		printk(KERN_ERR "n_gsm: can't register line discipline (err = %d)\n", status);
+		pr_err("n_gsm: can't register line discipline (err = %d)\n",
+								status);
 		return status;
 	}
 
 	gsm_tty_driver = alloc_tty_driver(256);
 	if (!gsm_tty_driver) {
 		tty_unregister_ldisc(N_GSM0710);
-		printk(KERN_ERR "gsm_init: tty allocation failed.\n");
+		pr_err("gsm_init: tty allocation failed.\n");
 		return -EINVAL;
 	}
 	gsm_tty_driver->owner	= THIS_MODULE;
@@ -2728,7 +2770,7 @@ static int __init gsm_init(void)
 	gsm_tty_driver->type		= TTY_DRIVER_TYPE_SERIAL;
 	gsm_tty_driver->subtype	= SERIAL_TYPE_NORMAL;
 	gsm_tty_driver->flags	= TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV
-							| TTY_DRIVER_HARDWARE_BREAK;
+						| TTY_DRIVER_HARDWARE_BREAK;
 	gsm_tty_driver->init_termios	= tty_std_termios;
 	/* Fixme */
 	gsm_tty_driver->init_termios.c_lflag &= ~ECHO;
@@ -2739,10 +2781,11 @@ static int __init gsm_init(void)
 	if (tty_register_driver(gsm_tty_driver)) {
 		put_tty_driver(gsm_tty_driver);
 		tty_unregister_ldisc(N_GSM0710);
-		printk(KERN_ERR "gsm_init: tty registration failed.\n");
+		pr_err("gsm_init: tty registration failed.\n");
 		return -EBUSY;
 	}
-	printk(KERN_INFO "gsm_init: loaded as %d,%d.\n", gsm_tty_driver->major, gsm_tty_driver->minor_start);
+	pr_debug("gsm_init: loaded as %d,%d.\n",
+			gsm_tty_driver->major, gsm_tty_driver->minor_start);
 	return 0;
 }
 
@@ -2750,10 +2793,10 @@ static void __exit gsm_exit(void)
 {
 	int status = tty_unregister_ldisc(N_GSM0710);
 	if (status != 0)
-		printk(KERN_ERR "n_gsm: can't unregister line discipline (err = %d)\n", status);
+		pr_err("n_gsm: can't unregister line discipline (err = %d)\n",
+								status);
 	tty_unregister_driver(gsm_tty_driver);
 	put_tty_driver(gsm_tty_driver);
-	printk(KERN_INFO "gsm_init: unloaded.\n");
 }
 
 module_init(gsm_init);