X-Git-Url: http://rtime.felk.cvut.cz/gitweb/linux-lin.git/blobdiff_plain/7f0102aa259dec7f56c0a36ac52a90eec60f7400..0310db26c5a5a9520fd4486d12943f3d9acb8fb7:/sllin/sllin.c

diff --git a/sllin/sllin.c b/sllin/sllin.c
index cf387e7..636e89a 100644
--- a/sllin/sllin.c
+++ b/sllin/sllin.c
@@ -71,6 +71,7 @@ MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Oliver Hartkopp <socketcan@hartkopp.net>");
 
 #define SLLIN_MAGIC 0x53CA
+// #define BREAK_BY_BAUD
 
 static int maxdev = 10;		/* MAX number of SLLIN channels;
 				   This can be overridden with
@@ -114,7 +115,7 @@ struct sllin {
 	char			lin_master;	/* node is a master node */
 	int			lin_baud;	/* LIN baudrate */
 	int 			lin_state;	/* state */
-	int 			id_to_sent;	/* there is ID to be sent */
+	int 			id_to_send;	/* there is ID to be sent */
 
 	unsigned long		flags;		/* Flag values/ mode etc     */
 #define SLF_INUSE		0		/* Channel in use            */
@@ -123,10 +124,11 @@ struct sllin {
 #define SLF_TXEVENT		3               /* Tx wake event             */
 #define SLF_MSGEVENT		4               /* CAN message to sent       */
 
-	unsigned char		leased;
 	dev_t			line;
 	struct task_struct	*kwthread;
 	wait_queue_head_t	kwt_wq;
+
+	struct sk_buff          *rec_skb;	/* Socket buffer with received CAN frame */
 };
 
 static struct net_device **sllin_devs;
@@ -148,12 +150,18 @@ static int sltty_change_speed(struct tty_struct *tty, unsigned speed)
 	int cflag;
 
 	mutex_lock(&tty->termios_mutex);
+
 	old_termios = *(tty->termios);
 	cflag = tty->termios->c_cflag;
+	cflag &= ~(CBAUD | CIBAUD);
+	cflag |= BOTHER;
+	tty->termios->c_cflag = cflag;
+
 	tty_encode_baud_rate(tty, speed, speed);
+
 	if (tty->ops->set_termios)
 		tty->ops->set_termios(tty, &old_termios);
-	//priv->io.speed = speed;
+
 	mutex_unlock(&tty->termios_mutex);
 
 	return 0;
@@ -234,49 +242,49 @@ static void sll_bump(struct sllin *sl)
 /* Convert particular CAN frame into LIN frame and send it to TTY queue. */
 static void sll_encaps(struct sllin *sl, struct can_frame *cf)
 {
-	int actual, idx, i;
-	char lframe[16] = {0x00, 0x55}; /* Fake break, Sync byte */
-	struct tty_struct *tty = sl->tty;
-
-	pr_debug("sllin: %s() invoked\n", __FUNCTION__);
-
-	/* We do care only about SFF frames */
-	if (cf->can_id & CAN_EFF_FLAG)
-		return;
-
-	/* Send only header */
-	if (cf->can_id & CAN_RTR_FLAG) {
-		pr_debug("sllin: %s() RTR CAN frame\n", __FUNCTION__);
-		lframe[2] = (u8)cf->can_id; /* Get one byte LIN ID */
-
-		sltty_change_speed(tty, sl->lin_baud / 2);
-		tty->ops->write(tty, &lframe[0], 1);
-		sltty_change_speed(tty, sl->lin_baud);
-		tty->ops->write(tty, &lframe[1], 1);
-		tty->ops->write(tty, &lframe[2], 1);
-	} else {
-		pr_debug("sllin: %s() non-RTR CAN frame\n", __FUNCTION__);
-		/*	idx = strlen(sl->xbuff);
-
-			for (i = 0; i < cf->can_dlc; i++)
-			sprintf(&sl->xbuff[idx + 2*i], "%02X", cf->data[i]);
-
-		 * Order of next two lines is *very* important.
-		 * When we are sending a little amount of data,
-		 * the transfer may be completed inside the ops->write()
-		 * routine, because it's running with interrupts enabled.
-		 * In this case we *never* got WRITE_WAKEUP event,
-		 * if we did not request it before write operation.
-		 *       14 Oct 1994  Dmitry Gorodchanin.
-
-		 set_bit(TTY_DO_WRITE_WAKEUP, &sl->tty->flags);
-		 actual = sl->tty->ops->write(sl->tty, sl->xbuff, strlen(sl->xbuff));
-		 sl->xleft = strlen(sl->xbuff) - actual;
-		 sl->xhead = sl->xbuff + actual;
-		 sl->dev->stats.tx_bytes += cf->can_dlc;
-		 */
-	}
-
+//	int actual, idx, i;
+//	char lframe[16] = {0x00, 0x55}; /* Fake break, Sync byte */
+//	struct tty_struct *tty = sl->tty;
+//
+//	pr_debug("sllin: %s() invoked\n", __FUNCTION__);
+//
+//	/* We do care only about SFF frames */
+//	if (cf->can_id & CAN_EFF_FLAG)
+//		return;
+//
+//	/* Send only header */
+//	if (cf->can_id & CAN_RTR_FLAG) {
+//		pr_debug("sllin: %s() RTR CAN frame\n", __FUNCTION__);
+//		lframe[2] = (u8)cf->can_id; /* Get one byte LIN ID */
+//
+//		sltty_change_speed(tty, sl->lin_baud * 2 / 3);
+//		tty->ops->write(tty, &lframe[0], 1);
+//		sltty_change_speed(tty, sl->lin_baud);
+//		tty->ops->write(tty, &lframe[1], 1);
+//		tty->ops->write(tty, &lframe[2], 1);
+//	} else {
+//		pr_debug("sllin: %s() non-RTR CAN frame\n", __FUNCTION__);
+//		/*	idx = strlen(sl->xbuff);
+//
+//			for (i = 0; i < cf->can_dlc; i++)
+//			sprintf(&sl->xbuff[idx + 2*i], "%02X", cf->data[i]);
+//
+//		 * Order of next two lines is *very* important.
+//		 * When we are sending a little amount of data,
+//		 * the transfer may be completed inside the ops->write()
+//		 * routine, because it's running with interrupts enabled.
+//		 * In this case we *never* got WRITE_WAKEUP event,
+//		 * if we did not request it before write operation.
+//		 *       14 Oct 1994  Dmitry Gorodchanin.
+//
+//		 set_bit(TTY_DO_WRITE_WAKEUP, &sl->tty->flags);
+//		 actual = sl->tty->ops->write(sl->tty, sl->xbuff, strlen(sl->xbuff));
+//		 sl->xleft = strlen(sl->xbuff) - actual;
+//		 sl->xhead = sl->xbuff + actual;
+//		 sl->dev->stats.tx_bytes += cf->can_dlc;
+//		 */
+//	}
+//
 }
 
 /*
@@ -302,14 +310,18 @@ static void sllin_write_wakeup(struct tty_struct *tty)
 		actual = tty->ops->write(tty, sl->tx_buff + sl->tx_cnt, sl->tx_cnt - sl->tx_lim);
 		sl->tx_cnt += actual;
 
-		if(sl->tx_cnt < sl->tx_lim)
+		if (sl->tx_cnt < sl->tx_lim) {
+			printk(KERN_INFO "sllin_write_wakeup sent %d, remains %d, waiting\n",
+				sl->tx_cnt, sl->tx_lim - sl->tx_cnt);
 			return;
+		}
 	}
 
 	clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
 	set_bit(SLF_TXEVENT, &sl->flags);
 	wake_up(&sl->kwt_wq);
 
+	printk(KERN_INFO "sllin_write_wakeup sent %d, wakeup\n", sl->tx_cnt);
 }
 
 /* Send a can_frame to a TTY queue. */
@@ -318,24 +330,28 @@ static netdev_tx_t sll_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct sllin *sl = netdev_priv(dev);
 
 	if (skb->len != sizeof(struct can_frame))
-		goto out;
+		goto err_out;
 
 	spin_lock(&sl->lock);
 	if (!netif_running(dev))  {
 		spin_unlock(&sl->lock);
 		printk(KERN_WARNING "%s: xmit: iface is down\n", dev->name);
-		goto out;
+		goto err_out;
 	}
 	if (sl->tty == NULL) {
 		spin_unlock(&sl->lock);
-		goto out;
+		goto err_out;
 	}
 
 	netif_stop_queue(sl->dev);
-	sll_encaps(sl, (struct can_frame *) skb->data); /* encaps & send */
+
+	sl->rec_skb = skb;
+	set_bit(SLF_MSGEVENT, &sl->flags);
+	wake_up(&sl->kwt_wq);
 	spin_unlock(&sl->lock);
+	return NETDEV_TX_OK;
 
-out:
+err_out:
 	kfree_skb(skb);
 	return NETDEV_TX_OK;
 }
@@ -427,6 +443,8 @@ static void sllin_receive_buf(struct tty_struct *tty,
 {
 	struct sllin *sl = (struct sllin *) tty->disc_data;
 
+	printk(KERN_INFO "sllin_receive_buf invoked\n");
+
 	if (!sl || sl->magic != SLLIN_MAGIC || !netif_running(sl->dev))
 		return;
 
@@ -435,18 +453,31 @@ static void sllin_receive_buf(struct tty_struct *tty,
 		if (fp && *fp++) {
 			if (!test_and_set_bit(SLF_ERROR, &sl->flags))
 				sl->dev->stats.rx_errors++;
+			printk(KERN_INFO "sllin_receive_buf char 0x%02x ignored "
+				"due marker 0x%02x, flags 0x%lx\n",
+				*cp, *(fp-1), sl->flags);
 			cp++;
 			continue;
 		}
 
-		if (sl->rx_cnt < SLLIN_BUFF_LEN)  {
+		if (sl->rx_cnt < SLLIN_BUFF_LEN) {
+#ifndef BREAK_BY_BAUD
+			/* We didn't receive Break character */
+			if ((sl->rx_cnt == SLLIN_BUFF_BREAK) && (*cp == 0x55)) {
+				sl->rx_buff[sl->rx_cnt++] = 0x00;
+			}
+#endif
+			printk(KERN_INFO "LIN_RX[%d]: 0x%02x\n", sl->rx_cnt, *cp);
 			sl->rx_buff[sl->rx_cnt++] = *cp++;
 		}
 	}
 
-	if(sl->rx_cnt >= sl->rx_expect) {
+	if (sl->rx_cnt >= sl->rx_expect) {
 		set_bit(SLF_RXEVENT, &sl->flags);
 		wake_up(&sl->kwt_wq);
+		printk(KERN_INFO "sllin_receive_buf count %d, wakeup\n", sl->rx_cnt);
+	} else {
+		printk(KERN_INFO "sllin_receive_buf count %d, waiting\n", sl->rx_cnt);
 	}
 }
 
@@ -470,10 +501,22 @@ int sllin_setup_msg(struct sllin *sl, int mode, int id,
 	sl->tx_lim = SLLIN_BUFF_DATA;
 
 	if ((data != NULL) && len) {
+		int i;
+		unsigned csum  = 0;
+
 		sl->tx_lim += len;
 		memcpy(sl->tx_buff + SLLIN_BUFF_DATA, data, len);
+		/* compute data parity there */
+		for (i = SLLIN_BUFF_DATA; i < sl->tx_lim; i++) {
+			csum += sl->tx_buff[i];
+			if (csum > 255)
+				csum -= 255;
+		}
+
+		sl->tx_buff[sl->tx_lim++] = csum;
 	}
-	sl->rx_lim += len;
+	if (len != 0)
+		sl->rx_lim += len + 1;
 
 	return 0;
 }
@@ -485,11 +528,14 @@ int sllin_send_tx_buff(struct sllin *sl)
 	int remains;
 	int res;
 
+#ifdef BREAK_BY_BAUD
 	if (sl->lin_state != SLSTATE_BREAK_SENT)
 		remains = sl->tx_lim - sl->tx_cnt;
 	else
 		remains = 1;
-
+#else
+	remains = sl->tx_lim - sl->tx_cnt;
+#endif
 
 	res = tty->ops->write(tty, sl->tx_buff + sl->tx_cnt, remains);
 	if (res < 0)
@@ -505,24 +551,31 @@ int sllin_send_tx_buff(struct sllin *sl)
 			clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
 			return -1;
 		}
+		
+		remains -= res;
+		sl->tx_cnt += res;
 	}
-	sl->tx_cnt += res;
+
+	printk(KERN_INFO "sllin_send_tx_buff sent %d, remains %d\n",
+			sl->tx_cnt, remains);
 
 	return 0;
 }
 
+#ifdef BREAK_BY_BAUD
 int sllin_send_break(struct sllin *sl)
 {
 	struct tty_struct *tty = sl->tty;
-	unsigned long break_baud = sl->lin_baud;
+	unsigned long break_baud;
 	int res;
 
-	break_baud = (break_baud * 8) / 14;
-
+	break_baud = ((sl->lin_baud * 2) / 3);
 	sltty_change_speed(tty, break_baud);
 
-	sl->rx_expect = SLLIN_BUFF_BREAK + 1;
+	tty->ops->flush_buffer(tty);
+	sl->rx_cnt = SLLIN_BUFF_BREAK;
 
+	sl->rx_expect = SLLIN_BUFF_BREAK + 1;
 	sl->lin_state = SLSTATE_BREAK_SENT;
 
 	res = sllin_send_tx_buff(sl);
@@ -533,6 +586,42 @@ int sllin_send_break(struct sllin *sl)
 
 	return 0;
 }
+#else /* BREAK_BY_BAUD */
+
+int sllin_send_break(struct sllin *sl)
+{
+	struct tty_struct *tty = sl->tty;
+	unsigned long break_baud;
+	unsigned long flags;	
+	int retval;
+
+	sl->rx_cnt = SLLIN_BUFF_BREAK;
+	sl->rx_expect = SLLIN_BUFF_BREAK + 1;
+	sl->lin_state = SLSTATE_BREAK_SENT;
+
+	/* Do the break ourselves; Inspired by 
+	   http://lxr.linux.no/#linux+v3.1.2/drivers/tty/tty_io.c#L2452 */
+	retval = tty->ops->break_ctl(tty, -1);
+	if (retval)
+		return retval;
+
+	//udelay(712);
+	usleep_range(650, 750);
+
+	retval = tty->ops->break_ctl(tty, 0);
+	usleep_range(50, 100);
+	
+	tty->ops->flush_buffer(tty);
+
+	sl->tx_cnt = SLLIN_BUFF_SYNC;
+
+	printk(KERN_INFO "sllin: Break sent.\n");
+	set_bit(SLF_RXEVENT, &sl->flags);
+	wake_up(&sl->kwt_wq);
+
+	return 0;
+}
+#endif /* BREAK_BY_BAUD */
 
 /*****************************************
  *  sllin_kwthread - kernel worker thread
@@ -542,49 +631,83 @@ int sllin_kwthread(void *ptr)
 {
 	struct sllin *sl = (struct sllin *)ptr;
 	struct tty_struct *tty = sl->tty;
+	struct sched_param schparam = { .sched_priority = 40 };
 	int res;
+	struct can_frame *cf;
 
 	printk(KERN_INFO "sllin: sllin_kwthread started.\n");
+	sched_setscheduler(current, SCHED_FIFO, &schparam);
 
 	clear_bit(SLF_ERROR, &sl->flags);
-
 	sltty_change_speed(tty, sl->lin_baud);
 
-	sllin_setup_msg(sl, 0, 0x33, NULL, 0);
-
 	while (!kthread_should_stop()) {
-
 		if ((sl->lin_state == SLSTATE_IDLE) && sl->lin_master &&
-			sl->id_to_sent) {
-			if(sllin_send_break(sl)<0) {
+			sl->id_to_send) {
+			if(sllin_send_break(sl) < 0) {
 				/* error processing */
 			}
-
 		}
 
 		wait_event_killable(sl->kwt_wq, kthread_should_stop() ||
 			test_bit(SLF_RXEVENT, &sl->flags) ||
-			test_bit(SLF_TXEVENT, &sl->flags));
+			test_bit(SLF_TXEVENT, &sl->flags) ||
+			((sl->lin_state == SLSTATE_IDLE) && test_bit(SLF_MSGEVENT, &sl->flags)));
 
 		if (test_and_clear_bit(SLF_RXEVENT, &sl->flags)) {
-
+			printk(KERN_INFO "sllin_kthread RXEVENT \n");
 		}
 
 		if (test_and_clear_bit(SLF_TXEVENT, &sl->flags)) {
+			printk(KERN_INFO "sllin_kthread TXEVENT \n");
+		}
+
+		if ((sl->lin_state == SLSTATE_IDLE) && test_bit(SLF_MSGEVENT, &sl->flags)) {
+			cf = (struct can_frame *)sl->rec_skb->data;
+
+			/* We do care only about SFF frames */
+			if (cf->can_id & CAN_EFF_FLAG)
+				goto release_skb;
+
+			if (cf->can_id & CAN_RTR_FLAG) {
+				printk(KERN_INFO "%s: RTR CAN frame, ID = %x\n",
+					__FUNCTION__, cf->can_id & CAN_SFF_MASK);
+				if (sllin_setup_msg(sl, 0, 
+					cf->can_id & CAN_SFF_MASK, NULL, 0) != -1) {
+					sl->id_to_send = true;
+				}
+			} else {
+				printk(KERN_INFO "%s: NON-RTR CAN frame, ID = %x\n",
+					__FUNCTION__, (int)cf->can_id & CAN_SFF_MASK);
+
+				if (sllin_setup_msg(sl, 0, cf->can_id & CAN_SFF_MASK, 
+					cf->data, cf->can_dlc) != -1) {
+					sl->id_to_send = true;
+				}
+			}
 
+		release_skb:
+			clear_bit(SLF_MSGEVENT, &sl->flags);
+			kfree_skb(sl->rec_skb);
+			netif_wake_queue(sl->dev);
 		}
 
 		switch (sl->lin_state) {
 			case SLSTATE_BREAK_SENT:
+#ifdef BREAK_BY_BAUD
 				if (sl->rx_cnt <= SLLIN_BUFF_BREAK)
 					continue;
 
 				res = sltty_change_speed(tty, sl->lin_baud);
-
-				sllin_send_tx_buff(sl);
+#endif
 
 				sl->lin_state = SLSTATE_ID_SENT;
+				sllin_send_tx_buff(sl);
+				break;
 
+			case SLSTATE_ID_SENT:
+				sl->id_to_send = false;
+				sl->lin_state = SLSTATE_IDLE;
 				break;
 		}
 
@@ -620,7 +743,7 @@ static void sll_sync(void)
 			break;
 
 		sl = netdev_priv(dev);
-		if (sl->tty || sl->leased)
+		if (sl->tty)
 			continue;
 		if (dev->flags & IFF_UP)
 			dev_close(dev);
@@ -730,10 +853,11 @@ static int sllin_open(struct tty_struct *tty)
 		/* Perform the low-level SLLIN initialization. */
 		sl->rx_cnt    = 0;
 		sl->rx_expect = 0;
+		sl->rx_lim    = 0;
 		sl->tx_cnt    = 0;
 		sl->tx_lim    = 0;
 
-		sl->lin_baud  = 2400;
+		sl->lin_baud  = 19200;
 
 		sl->lin_master = 1;
 		sl->lin_state = SLSTATE_IDLE;
@@ -794,8 +918,6 @@ static void sllin_close(struct tty_struct *tty)
 
 	tty->disc_data = NULL;
 	sl->tty = NULL;
-	if (!sl->leased)
-		sl->line = 0;
 
 	/* Flush network side */
 	unregister_netdev(sl->dev);