From: Jan Kriz <krizj1@fel.cvut.cz>
Date: Sun, 27 Jul 2008 15:47:46 +0000 (+0200)
Subject: Fully working solution!
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/lincan.git/commitdiff_plain/b909661a225163abe7c02f91d73e6915b0eca443

Fully working solution!

fixed simple getting the same device name when reconnected
fixed more usb urbs to send and receive messages
pending split kernel thread to smaller functions
pending commentaries
---

diff --git a/lincan/include/usbcan.h b/lincan/include/usbcan.h
index 5ddbb12..a7da77d 100644
--- a/lincan/include/usbcan.h
+++ b/lincan/include/usbcan.h
@@ -15,6 +15,7 @@
 #include <asm/uaccess.h>
 #include <linux/usb.h>
 #include <linux/mutex.h>
+#include <linux/wait.h>
 
 #include "../include/kthread.h"
 
@@ -66,7 +67,7 @@ int usbcan_irq_handler(int irq, struct canchip_t *chip);
 int usbcan_init(void);
 void usbcan_exit(void);
 
-int usbcan_read_kthread(void *data);
+int usbcan_kthread(void *data);
 int usbcan_chip_queue_status(struct canchip_t *chip);
 
 #ifdef CONFIG_OC_LINCAN_DETAILED_ERRORS
@@ -115,22 +116,30 @@ int usbcan_chip_queue_status(struct canchip_t *chip);
 
 #endif /*CONFIG_OC_LINCAN_DETAILED_ERRORS*/
 
+struct usbcan_usb;
+
+#define USBCAN_TOT_RX_URBS 8
+#define USBCAN_TOT_TX_URBS 8
+
+#define USBCAN_TRANSFER_SIZE 16
+
 struct usbcan_message {
 	struct urb	*u;
-	u8	msg[16];
+	struct usbcan_usb *dev;
+	u8	msg[USBCAN_TRANSFER_SIZE];
 	spinlock_t		acc;		/* access lock */
+	struct canque_edge_t *qedge;
+	struct canque_slot_t *slot;
 	volatile long	flags;
 };
 
-#define USBCAN_MESSAGE_FREE (1)
-#define USBCAN_MESSAGE_READY_TO_SEND (2)
-#define USBCAN_MESSAGE_SENDING			(3)
-#define USBCAN_MESSAGE_TERMINATE		(4)
-#define USBCAN_MESSAGE_ERROR				(5)
-#define USBCAN_MESSAGE_DATA_READ		(6)
-
-#define USBCAN_TOT_RX_URBS 8
-#define USBCAN_TOT_TX_URBS 8
+#define USBCAN_MESSAGE_FREE 					(1)
+#define USBCAN_MESSAGE_URB_PENDING		(2)
+#define USBCAN_MESSAGE_TERMINATE			(3)
+#define USBCAN_MESSAGE_ERROR					(4)
+#define USBCAN_MESSAGE_DATA_OK				(5)
+#define USBCAN_MESSAGE_TYPE_RX				(6)
+#define USBCAN_MESSAGE_TYPE_TX				(7)
 
 /* Structure to hold all of our device specific stuff */
 struct usbcan_usb {
@@ -141,22 +150,24 @@ struct usbcan_usb {
 	u8			bulk_in_endpointAddr;	/* the address of the bulk in endpoint */
 	u8			bulk_out_endpointAddr;	/* the address of the bulk out endpoint */
 	struct mutex		io_mutex;		/* synchronize I/O with disconnect */
-	struct urb		*rx;
-	struct urb		*tx;
-	u8	rx_msg[16];
-	u8	tx_msg[16];
+	struct usbcan_message	rx[USBCAN_TOT_RX_URBS];
+	struct usbcan_message	tx[USBCAN_TOT_TX_URBS];
 
 	struct task_struct *comthread;                      /* usb communication kernel thread  */
+	wait_queue_head_t queue;
 
 	struct canchip_t *chip;
 	volatile long flags;
 };
 
-#define USBCAN_DATA_READ			(1)
-#define USBCAN_TERMINATE			(2)
-#define USBCAN_ERROR					(3)
-#define USBCAN_TX_PENDING			(4)
-#define USBCAN_THREAD_RUNNING	(5)
+#define USBCAN_DATA_OK				(1)
+#define USBCAN_DATA_RX				(2)
+#define USBCAN_DATA_TX				(3)
+#define USBCAN_TERMINATE			(4)
+#define USBCAN_ERROR					(5)
+#define USBCAN_TX_PENDING			(6)
+#define USBCAN_THREAD_RUNNING	(7)
+#define USBCAN_FREE_TX_URB		(8)
 
 #define USBCAN_VENDOR_BAUD_RATE_SET	(1)
 #define USBCAN_VENDOR_BAUD_RATE_STATUS	(2)
diff --git a/lincan/src/finish.c b/lincan/src/finish.c
index d25e936..5e789ae 100644
--- a/lincan/src/finish.c
+++ b/lincan/src/finish.c
@@ -14,7 +14,7 @@
 #include "../include/finish.h"
 #include "../include/setup.h"
 
-
+extern int next_minor;
 /**
  * msgobj_done - destroys one CAN message object
  * @obj: pointer to CAN message object structure
@@ -37,8 +37,11 @@ void msgobj_done(struct msgobj_t *obj)
 	}
 
 	if((obj->minor>=0)) {
-		if(objects_p[obj->minor] == obj)
+		if(objects_p[obj->minor] == obj){
 			objects_p[obj->minor] = NULL;
+			if (--next_minor<0)
+				next_minor=0;
+		}
 		else
 			CANMSG("msgobj_done: not registered as minor\n");
 	}
@@ -127,7 +130,7 @@ void canhardware_done(struct canhardware_t *canhw)
 	int i;
 	struct candevice_t *candev;
 
-  	for(i=0; i<canhw->nr_boards; i++){
+  	for(i=0; i<MAX_HW_CARDS; i++){
 		if((candev=canhw->candevice[i])==NULL)
 			continue;
 		candevice_done(candev);
diff --git a/lincan/src/setup.c b/lincan/src/setup.c
index bd6d59c..cb6550d 100644
--- a/lincan/src/setup.c
+++ b/lincan/src/setup.c
@@ -19,6 +19,8 @@ int init_device_struct(int card, int *chan_param_idx_p, int *irq_param_idx_p);
 int init_chip_struct(struct candevice_t *candev, int chipnr, int irq, long baudrate, long clock);
 int init_obj_struct(struct candevice_t *candev, struct canchip_t *hostchip, int objnr);
 
+int next_minor=0;
+
 /**
  * can_base_addr_fixup - relocates board physical memory addresses to the CPU accessible ones
  * @candev: pointer to the previously filled device/board, chips and message objects structures
@@ -78,7 +80,6 @@ int can_check_dev_taken(void *anydev)
  */
 int register_obj_struct(struct msgobj_t *obj, int minorbase)
 {
-	static int next_minor=0;
 	int i;
 
 	if(minorbase>=0)
diff --git a/lincan/src/usbcan.c b/lincan/src/usbcan.c
index f29e549..c813715 100644
--- a/lincan/src/usbcan.c
+++ b/lincan/src/usbcan.c
@@ -263,7 +263,7 @@ int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned  l
 	int retval;
 	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 
-	u8 usbbuf[16];
+	u8 usbbuf[USBCAN_TRANSFER_SIZE];
 
 	if (!dev)
 		return -ENODEV;
@@ -276,7 +276,7 @@ int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned  l
 		USBCAN_VENDOR_EXT_MASK_SET,
 		USB_TYPE_VENDOR,
 		cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
-		&usbbuf, 16,
+		&usbbuf, USBCAN_TRANSFER_SIZE,
 		10000);
 	if (retval<0)
 		return -ENODEV;
@@ -286,7 +286,7 @@ int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned  l
 		USBCAN_VENDOR_EXT_MASK_STATUS,
 		USB_TYPE_VENDOR,
 		cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
-		&usbbuf, 16,
+		&usbbuf, USBCAN_TRANSFER_SIZE,
 		10000);
 
 	if (retval==1){
@@ -319,7 +319,7 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw,
 	int retval;
 	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 
-	u8 usbbuf[16];
+	u8 usbbuf[USBCAN_TRANSFER_SIZE];
 
 	if (!dev)
 		return -ENODEV;
@@ -334,7 +334,7 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw,
 		USBCAN_VENDOR_BAUD_RATE_SET,
 		USB_TYPE_VENDOR,
 		cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
-		&usbbuf, 16,
+		&usbbuf, USBCAN_TRANSFER_SIZE,
 		10000);
 	if (retval<0)
 		return -ENODEV;
@@ -344,7 +344,7 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw,
 		USBCAN_VENDOR_BAUD_RATE_STATUS,
 		USB_TYPE_VENDOR,
 		cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
-		usbbuf, 16,
+		usbbuf, USBCAN_TRANSFER_SIZE,
 		10000);
 
 	if (retval==1){
@@ -388,38 +388,6 @@ int usbcan_pre_read_config(struct canchip_t *chip, struct msgobj_t *obj)
 int usbcan_pre_write_config(struct canchip_t *chip, struct msgobj_t *obj,
 							struct canmsg_t *msg)
 {
-	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
-	int i=0;
-	int len;
-	u8 *ptr;
-
-	if (!dev)
-		return -ENODEV;
-	/* Wait until Transmit Buffer Status is released */
-	while ( usbcan_chip_queue_status(chip) &&
-						i++<MAX_TRANSMIT_WAIT_LOOPS) {
-		udelay(i);
-	}
-	if (usbcan_chip_queue_status(chip)){
-		CANMSG("Buffer full, cannot send message\n");
-		return -EIO;
-	}
-
-	*(uint8_t *)(dev->tx_msg)=chip->chip_idx & 0xFF;
-
-	len = msg->length;
-	if(len > CAN_MSG_LENGTH) len = CAN_MSG_LENGTH;
-
-	*(uint8_t *)(dev->tx_msg+1)=len & 0xFF;
-	*(uint16_t *)(dev->tx_msg+2)=cpu_to_le16(msg->flags);
-	*(uint32_t *)(dev->tx_msg+4)=cpu_to_le32(msg->id);
-
-	for(ptr=dev->tx_msg+8,i=0; i < len; ptr++,i++) {
-		*ptr=msg->data[i] & 0xFF;
-	}
-	for(; i < 8; ptr++,i++) {
-		*ptr=0;
-	}
 	return 0;
 }
 
@@ -437,43 +405,6 @@ int usbcan_pre_write_config(struct canchip_t *chip, struct msgobj_t *obj,
 int usbcan_send_msg(struct canchip_t *chip, struct msgobj_t *obj,
 							struct canmsg_t *msg)
 {
-	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
-	int len,retval;
-
-	if (!dev)
-		return -ENODEV;
-
-	set_bit(USBCAN_TX_PENDING,&dev->flags);
-	retval=usb_bulk_msg(dev->udev,
-			usb_sndbulkpipe(dev->udev, dev->bulk_out_endpointAddr),
-			&dev->tx_msg, 16,
-			&len,10000);
-	clear_bit(USBCAN_TX_PENDING,&dev->flags);
-	if (retval){
-		CANMSG("URB error %d\n",retval);
-		return -EIO;
-	}
-	if (len!=16){
-		CANMSG("CAN message not sent\n");
-		return -EIO;
-	}
-	CANMSG("Message sent\n");
-
-	if(obj->tx_slot){
-		// Do local transmitted message distribution if enabled
-		if (processlocal){
-			// fill CAN message timestamp
-			can_filltimestamp(&obj->tx_slot->msg.timestamp);
-
-			obj->tx_slot->msg.flags |= MSG_LOCAL;
-			canque_filter_msg2edges(obj->qends, &obj->tx_slot->msg);
-		}
-		// Free transmitted slot
-		canque_free_outslot(obj->qends, obj->tx_qedge, obj->tx_slot);
-		obj->tx_slot=NULL;
-	}
-
-	can_msgobj_clear_fl(obj,TX_PENDING);
 	return 0;
 }
 
@@ -509,7 +440,7 @@ int usbcan_set_btregs(struct canchip_t *chip, unsigned short btr0,
 							unsigned short btr1)
 {
 	int retval;
-	u8	buf[16];
+	u8	buf[USBCAN_TRANSFER_SIZE];
 	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 	uint16_t value=(btr1&0xFF)<<8 | (btr0&0xFF);
 
@@ -521,7 +452,7 @@ int usbcan_set_btregs(struct canchip_t *chip, unsigned short btr0,
 	USBCAN_VENDOR_SET_BTREGS,
 	USB_TYPE_VENDOR,
 	cpu_to_le16(value), cpu_to_le16(chip->chip_idx),
-	&buf, 16,
+	&buf, USBCAN_TRANSFER_SIZE,
 	10000);
 
 	if (retval==1){
@@ -541,7 +472,7 @@ int usbcan_set_btregs(struct canchip_t *chip, unsigned short btr0,
 int usbcan_start_chip(struct canchip_t *chip)
 {
 	int retval;
-	u8	buf[16];
+	u8	buf[USBCAN_TRANSFER_SIZE];
 	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 
 	if (!dev)
@@ -552,7 +483,7 @@ int usbcan_start_chip(struct canchip_t *chip)
 	USBCAN_VENDOR_START_CHIP,
 	USB_TYPE_VENDOR,
 	cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
-	&buf, 16,
+	&buf, USBCAN_TRANSFER_SIZE,
 	10000);
 
 	if (retval==1){
@@ -574,7 +505,7 @@ int usbcan_start_chip(struct canchip_t *chip)
 int usbcan_chip_queue_status(struct canchip_t *chip)
 {
 	int retval;
-	u8	buf[16];
+	u8	buf[USBCAN_TRANSFER_SIZE];
 	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 
 	if (!dev)
@@ -584,7 +515,7 @@ int usbcan_chip_queue_status(struct canchip_t *chip)
 	USBCAN_VENDOR_CHECK_TX_STAT,
 	USB_TYPE_VENDOR,
 	cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
-	&buf, 16,
+	&buf, USBCAN_TRANSFER_SIZE,
 	10000);
 
 	if (retval==1){
@@ -608,7 +539,7 @@ int usbcan_chip_queue_status(struct canchip_t *chip)
 int usbcan_stop_chip(struct canchip_t *chip)
 {
 	int retval;
-	u8	buf[16];
+	u8	buf[USBCAN_TRANSFER_SIZE];
 	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 
 	if (!dev)
@@ -619,7 +550,7 @@ int usbcan_stop_chip(struct canchip_t *chip)
 	USBCAN_VENDOR_STOP_CHIP,
 	USB_TYPE_VENDOR,
 	cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
-	&buf, 16,
+	&buf, USBCAN_TRANSFER_SIZE,
 	10000);
 
 	if (retval==1){
@@ -663,7 +594,7 @@ int usbcan_attach_to_chip(struct canchip_t *chip)
 	struct usbcan_usb *dev = (struct usbcan_usb *)chip->chip_data;
 
 	/* start kernel thread */
- 	dev->comthread=can_kthread_run(usbcan_read_kthread, (void *)dev, "usbcan_%d",chip->chip_idx);
+ 	dev->comthread=can_kthread_run(usbcan_kthread, (void *)dev, "usbcan_%d",chip->chip_idx);
 
 	return 0;
 }
@@ -682,7 +613,9 @@ int usbcan_release_chip(struct canchip_t *chip)
 	usbcan_stop_chip(chip);
 
 	/* terminate the kernel thread */
- 	can_kthread_stop(dev->comthread);
+	set_bit(USBCAN_TERMINATE,&dev->flags);
+	wake_up_process(dev->comthread);
+//  	can_kthread_stop(dev->comthread);
 
 	return 0;
 }
@@ -811,95 +744,6 @@ void usbcan_irq_write_handler(struct canchip_t *chip, struct msgobj_t *obj)
  */
 int usbcan_irq_handler(int irq, struct canchip_t *chip)
 {
-/*	int irq_register, status, error_code;
-	struct msgobj_t *obj=chip->msgobj[0];
-	int loop_cnt=CHIP_MAX_IRQLOOP;
-
-	irq_register=can_read_reg(chip,SJAIR);
-//	DEBUGMSG("sja1000_irq_handler: SJAIR:%02x\n",irq_register);
-//	DEBUGMSG("sja1000_irq_handler: SJASR:%02x\n",
-//					can_read_reg(chip,SJASR));
-
-	if ((irq_register & (sjaIR_BEI|sjaIR_EPI|sjaIR_DOI|sjaIR_EI|sjaIR_TI|sjaIR_RI)) == 0)
-		return CANCHIP_IRQ_NONE;
-
-	if(!(chip->flags&CHIP_CONFIGURED)) {
-		CANMSG("usbcan_irq_handler: called for non-configured device, irq_register 0x%02x\n", irq_register);
-		return CANCHIP_IRQ_NONE;
-	}
-
-	status=can_read_reg(chip,SJASR);
-
-	do {
-
-		if(!loop_cnt--) {
-			CANMSG("usbcan_irq_handler IRQ %d stuck\n",irq);
-			return CANCHIP_IRQ_STUCK;
-		}
-
-		// (irq_register & sjaIR_TI)
-		//	old variant using SJAIR, collides with intended use with irq_accept
-		if (((status & sjaSR_TBS) && can_msgobj_test_fl(obj,TX_PENDING))||
-		    (can_msgobj_test_fl(obj,TX_REQUEST))) {
-			DEBUGMSG("sja1000_irq_handler: TI or TX_PENDING and TBS\n");
-			obj->ret = 0;
-			can_msgobj_set_fl(obj,TX_REQUEST);
-			while(!can_msgobj_test_and_set_fl(obj,TX_LOCK)){
-				can_msgobj_clear_fl(obj,TX_REQUEST);
-
-				if (can_read_reg(chip, SJASR) & sjaSR_TBS)
-					usbcan_irq_write_handler(chip, obj);
-
-				can_msgobj_clear_fl(obj,TX_LOCK);
-				if(!can_msgobj_test_fl(obj,TX_REQUEST)) break;
-				DEBUGMSG("TX looping in sja1000_irq_handler\n");
-			}
-		}
-		if ((irq_register & (sjaIR_EI|sjaIR_BEI|sjaIR_EPI|sjaIR_DOI)) != 0) {
-			// Some error happened
-			error_code=can_read_reg(chip,SJAECC);
-			sja1000_report_error(chip, status, irq_register, error_code);
-// FIXME: chip should be brought to usable state. Transmission cancelled if in progress.
-// Reset flag set to 0 if chip is already off the bus. Full state report
-			obj->ret=-1;
-
-			if(error_code == 0xd9) {
-				obj->ret= -ENXIO;
-				// no such device or address - no ACK received
-			}
-			if(obj->tx_retry_cnt++>MAX_RETR) {
-				can_write_reg(chip, sjaCMR_AT, SJACMR); // cancel any transmition
-				obj->tx_retry_cnt = 0;
-			}
-			if(status&sjaSR_BS) {
-				CANMSG("bus-off, resetting usbcan\n");
-				can_write_reg(chip, 0, SJAMOD);
-			}
-
-			if(obj->tx_slot){
-				canque_notify_inends(obj->tx_qedge, CANQUEUE_NOTIFY_ERRTX_BUS);
-				//canque_free_outslot(obj->qends, obj->tx_qedge, obj->tx_slot);
-				//obj->tx_slot=NULL;
-			}
-
-		} else {
-			if(sja1000_report_error_limit_counter)
-				sja1000_report_error_limit_counter--;
-			obj->tx_retry_cnt=0;
-		}
-
-		irq_register=can_read_reg(chip,SJAIR);
-
-		status=can_read_reg(chip,SJASR);
-
-		if(((status & sjaSR_TBS) && can_msgobj_test_fl(obj,TX_PENDING)) ||
-		   (irq_register & sjaIR_TI))
-			 can_msgobj_set_fl(obj,TX_REQUEST);
-
-	} while((irq_register & (sjaIR_BEI|sjaIR_EPI|sjaIR_DOI|sjaIR_EI|sjaIR_RI)) ||
-	        (can_msgobj_test_fl(obj,TX_REQUEST) && !can_msgobj_test_fl(obj,TX_LOCK)) ||
-		(status & sjaSR_RBS));
-*/
 	return CANCHIP_IRQ_HANDLED;
 }
 
@@ -916,6 +760,8 @@ int usbcan_irq_handler(int irq, struct canchip_t *chip)
  */
 int usbcan_wakeup_tx(struct canchip_t *chip, struct msgobj_t *obj)
 {
+	struct usbcan_usb *dev=(struct usbcan_usb *)chip->chip_data;
+
 	CANMSG("Trying to send message\n");
 	can_preempt_disable();
 
@@ -924,14 +770,16 @@ int usbcan_wakeup_tx(struct canchip_t *chip, struct msgobj_t *obj)
 	while(!can_msgobj_test_and_set_fl(obj,TX_LOCK)){
 		can_msgobj_clear_fl(obj,TX_REQUEST);
 
-		if (!usbcan_chip_queue_status(chip)){
+		if (test_and_clear_bit(USBCAN_FREE_TX_URB,&dev->flags)){
 			obj->tx_retry_cnt=0;
-			usbcan_irq_write_handler(chip, obj);
+			set_bit(USBCAN_TX_PENDING,&dev->flags);
+			if (test_bit(USBCAN_THREAD_RUNNING,&dev->flags))
+				wake_up_process(dev->comthread);
 		}
 
 		can_msgobj_clear_fl(obj,TX_LOCK);
 		if(!can_msgobj_test_fl(obj,TX_REQUEST)) break;
-		DEBUGMSG("TX looping in usbcan_wakeup_tx\n");
+		CANMSG("TX looping in usbcan_wakeup_tx\n");
 	}
 
 	can_preempt_enable();
@@ -1044,7 +892,8 @@ static int usbcan_sleep_thread(struct usbcan_usb *dev)
 		}
 		if (
 			can_kthread_should_stop() ||
-			test_bit(USBCAN_DATA_READ,&dev->flags) ||
+			test_bit(USBCAN_DATA_OK,&dev->flags) ||
+			test_bit(USBCAN_TX_PENDING,&dev->flags) ||
 			test_bit(USBCAN_TERMINATE,&dev->flags) ||
 			test_bit(USBCAN_ERROR,&dev->flags)
 		)
@@ -1055,130 +904,297 @@ static int usbcan_sleep_thread(struct usbcan_usb *dev)
 	return rc;
 }
 
-static void usbcan_rcv(struct urb *urb)
+static void usbcan_callback(struct urb *urb)
 {
-	struct usbcan_usb *dev = urb->context;
+	struct usbcan_message *mess = urb->context;
 	int retval;
 
-	if (!test_bit(USBCAN_THREAD_RUNNING,&dev->flags))
+	if (!test_bit(USBCAN_THREAD_RUNNING,&mess->dev->flags))
+		return;
+	if (test_bit(USBCAN_MESSAGE_TERMINATE,&mess->flags))
 		return;
 
 	switch (urb->status) {
 	case 0:
 		/* success */
-		set_bit(USBCAN_DATA_READ,&dev->flags);
-		CANMSG("Message received\n");
-		if (test_bit(USBCAN_THREAD_RUNNING,&dev->flags))
-			wake_up_process(dev->comthread);
+		CANMSG("%s > Message OK\n", __FUNCTION__);
+		set_bit(USBCAN_DATA_OK,&mess->dev->flags);
+		set_bit(USBCAN_MESSAGE_DATA_OK,&mess->flags);
+		if (test_bit(USBCAN_MESSAGE_TYPE_RX,&mess->flags)){
+			CANMSG("%s > RX flag set\n", __FUNCTION__);
+			set_bit(USBCAN_DATA_RX,&mess->dev->flags);
+		}
+		if (test_bit(USBCAN_MESSAGE_TYPE_TX,&mess->flags))
+			CANMSG("%s > TX flag set\n", __FUNCTION__);
+			set_bit(USBCAN_DATA_TX,&mess->dev->flags);
+		clear_bit(USBCAN_MESSAGE_URB_PENDING,&mess->flags);
+		if (test_bit(USBCAN_THREAD_RUNNING,&mess->dev->flags))
+			wake_up_process(mess->dev->comthread);
+		else
+			CANMSG("%s > USBCAN thread not running\n", __FUNCTION__);
+// 			wake_up(&mess->dev->queue);
 		return;
 	case -ECONNRESET:
 	case -ENOENT:
 	case -ESHUTDOWN:
 		/* this urb is terminated, clean up */
-		CANMSG("%s - urb shutting down with status: %d\n", __FUNCTION__, urb->status);
-		set_bit(USBCAN_TERMINATE,&dev->flags);
+		CANMSG("%s > Urb shutting down with status: %d\n", __FUNCTION__, urb->status);
+		set_bit(USBCAN_TERMINATE,&mess->dev->flags);
+		set_bit(USBCAN_MESSAGE_TERMINATE,&mess->flags);
+		clear_bit(USBCAN_MESSAGE_URB_PENDING,&mess->flags);
 		return;
 	default:
-// 		CANMSG("%s - nonzero urb status received: %d\n", __FUNCTION__, urb->status);
+		//CANMSG("%s > Nonzero status received: %d\n", __FUNCTION__, urb->status);
 		break;
 	}
 
+	// Try to send urb again on non significant errors
 	retval = usb_submit_urb (urb, GFP_ATOMIC);
 	if (retval<0){
-		CANMSG("%s - usb_submit_urb failed with result %d ... retrying\n",
-		     __FUNCTION__, retval);
-		set_bit(USBCAN_ERROR,&dev->flags);
-		if (test_bit(USBCAN_THREAD_RUNNING,&dev->flags))
-			wake_up_process(dev->comthread);
+		CANMSG("%s > Retrying urb failed with result %d\n", __FUNCTION__, retval);
+		set_bit(USBCAN_ERROR,&mess->dev->flags);
+		clear_bit(USBCAN_MESSAGE_URB_PENDING,&mess->flags);
+		if (test_bit(USBCAN_THREAD_RUNNING,&mess->dev->flags))
+			wake_up_process(mess->dev->comthread);
+// 			wake_up(&mess->dev->queue);
 	}
 }
 
-int usbcan_read_kthread(void *data)
+int usbcan_kthread(void *data)
 {
-	int retval=0;
+	int i,retval=0;
  	struct usbcan_usb *dev=(struct usbcan_usb *)data;
 	struct msgobj_t *obj;
 
-  /* this is normal work to do */
-  CANMSG ("usbcan thread started!\n");
+  CANMSG("Usbcan thread started...\n");
 
-	dev->rx = usb_alloc_urb(0, GFP_KERNEL);
-	if (!dev->rx){
-		CANMSG("Error allocating usb urb\n");
+	if (!dev->chip)
 		goto error;
+	obj=dev->chip->msgobj[0];
+
+	/* Prepare receive urbs  */
+	for (i=0;i<USBCAN_TOT_RX_URBS;i++){
+		dev->rx[i].u = usb_alloc_urb(0, GFP_KERNEL);
+		if (!dev->rx[i].u){
+			CANMSG("Error allocating %d. usb receive urb\n",i);
+			goto error;
+		}
+		dev->rx[i].u->dev = dev->udev;
+		dev->rx[i].dev = dev;
+		usb_fill_bulk_urb(dev->rx[i].u, dev->udev,
+				usb_rcvbulkpipe(dev->udev, dev->bulk_in_endpointAddr),
+				dev->rx[i].msg, USBCAN_TRANSFER_SIZE,
+				usbcan_callback, &dev->rx[i]);
+		set_bit(USBCAN_MESSAGE_TYPE_RX,&dev->rx[i].flags);
+	}
+
+	/* Prepare transmit urbs  */
+	for (i=0;i<USBCAN_TOT_TX_URBS;i++){
+		dev->tx[i].u = usb_alloc_urb(0, GFP_KERNEL);
+		if (!dev->tx[i].u){
+			CANMSG("Error allocating %d. usb transmit urb\n",i);
+			goto error;
+		}
+		dev->tx[i].u->dev = dev->udev;
+		dev->tx[i].dev = dev;
+		usb_fill_bulk_urb(dev->tx[i].u, dev->udev,
+				usb_sndbulkpipe(dev->udev, dev->bulk_out_endpointAddr),
+				dev->tx[i].msg, USBCAN_TRANSFER_SIZE,
+				usbcan_callback, &dev->tx[i]);
+		set_bit(USBCAN_MESSAGE_FREE,&dev->tx[i].flags);
+		set_bit(USBCAN_MESSAGE_TYPE_TX,&dev->tx[i].flags);
 	}
-	dev->rx->dev = dev->udev;
-	usb_fill_bulk_urb(dev->rx, dev->udev,
-			 usb_rcvbulkpipe(dev->udev, dev->bulk_in_endpointAddr),
-			 &dev->rx_msg, 16,
-			 usbcan_rcv, dev);
 
 	set_bit(USBCAN_THREAD_RUNNING,&dev->flags);
-  /* an endless loop in which we are doing our work */
-  for(;;)
-  {
-		retval=usb_submit_urb(dev->rx, GFP_ATOMIC);
+	set_bit(USBCAN_FREE_TX_URB,&dev->flags);
+
+	for (i=0;i<USBCAN_TOT_RX_URBS;i++){
+		retval=usb_submit_urb(dev->rx[i].u, GFP_KERNEL);
 		if (retval){
-			CANMSG("URB error %d\n",retval);
+			CANMSG("%d. URB error %d\n",i,retval);
 			set_bit(USBCAN_ERROR,&dev->flags);
-			break;
+			goto exit;
 		}
+		else
+			set_bit(USBCAN_MESSAGE_URB_PENDING,&dev->rx[i].flags);
+	}
+  /* an endless loop in which we are doing our work */
+  for(;;)
+  {
 		/* We need to do a memory barrier here to be sure that
 		the flags are visible on all CPUs. */
 		mb();
 		/* fall asleep */
-		usbcan_sleep_thread(dev);
+		if (!(can_kthread_should_stop() || test_bit(USBCAN_TERMINATE,&dev->flags))){
+			if (usbcan_sleep_thread(dev)<0)
+				break;
+/*			wait_event_interruptible(dev->queue,
+				can_kthread_should_stop() ||
+				test_bit(USBCAN_DATA_OK,&dev->flags) ||
+				test_bit(USBCAN_TX_PENDING,&dev->flags) ||
+				test_bit(USBCAN_TERMINATE,&dev->flags) ||
+				test_bit(USBCAN_ERROR,&dev->flags)
+			);*/
+		}
 		/* We need to do a memory barrier here to be sure that
 		the flags are visible on all CPUs. */
 		mb();
 
 		/* here we are back from sleep because we caught a signal. */
-		if (can_kthread_should_stop())
-		{
+		if (can_kthread_should_stop()){
 			/* we received a request to terminate ourself */
 			break;
 		}
 
-		{ /* Normal work to do */
-			if (test_bit(USBCAN_DATA_READ,&dev->flags)){
-				int i, len;
-				clear_bit(USBCAN_DATA_READ,&dev->flags);
-				CANMSG("Thread got received message\n");
-
-				if ((dev->chip)&&
-					(dev->chip->flags & CHIP_CONFIGURED)
-				){
-					u8 *ptr;
-
-					obj=dev->chip->msgobj[0];
-
-					len=*(uint8_t *)(dev->rx_msg+1);
-					if(len > CAN_MSG_LENGTH) len = CAN_MSG_LENGTH;
-					obj->rx_msg.length = len;
-
-					obj->rx_msg.flags=le16_to_cpu(*(uint16_t *)(dev->rx_msg+2));
-					obj->rx_msg.id=le32_to_cpu((*(uint32_t *)(dev->rx_msg+4)));
+		/* here we are back from sleep because we caught a signal. */
+		if (test_bit(USBCAN_TERMINATE,&dev->flags)){
+			/* we received a request to terminate ourself */
+			break;
+		}
 
-					for(ptr=dev->rx_msg+8,i=0; i < len; ptr++,i++) {
-						obj->rx_msg.data[i]=*ptr;
+		{ /* Normal work to do */
+			if (test_and_clear_bit(USBCAN_DATA_OK,&dev->flags)){
+				int j, len;
+				CANMSG("USBCAN Succesfull data transfer\n");
+				if (test_and_clear_bit(USBCAN_DATA_RX,&dev->flags)){
+					CANMSG("USBCAN RX handler\n");
+					for (i=0;i<USBCAN_TOT_RX_URBS;i++){
+						if (test_and_clear_bit(USBCAN_MESSAGE_DATA_OK,&dev->rx[i].flags)){
+							CANMSG("USBCAN Thread has received a message\n");
+							if ((dev->chip)&&(dev->chip->flags & CHIP_CONFIGURED)){
+								u8 *ptr;
+								struct usbcan_message *mess=&dev->rx[i];
+
+								len=*(u8 *)(mess->msg+1);
+								if(len > CAN_MSG_LENGTH) len = CAN_MSG_LENGTH;
+								obj->rx_msg.length = len;
+
+								obj->rx_msg.flags=le16_to_cpu(*(u16 *)(mess->msg+2));
+								obj->rx_msg.id=le32_to_cpu((*(u32 *)(mess->msg+4)));
+
+								for(ptr=mess->msg+8,j=0; j < len; ptr++,j++) {
+									obj->rx_msg.data[j]=*ptr;
+								}
+
+								// fill CAN message timestamp
+								can_filltimestamp(&obj->rx_msg.timestamp);
+								canque_filter_msg2edges(obj->qends, &obj->rx_msg);
+							}
+							else
+								CANMSG("Destination chip not found\n");
+						}
+						if (!test_bit(USBCAN_MESSAGE_URB_PENDING,&dev->rx[i].flags)){
+							CANMSG("Renewing RX urb\n");
+							retval = usb_submit_urb (dev->rx[i].u, GFP_KERNEL);
+							if (retval<0){
+								CANMSG("%d. URB error %d\n", i, retval);
+								set_bit(USBCAN_ERROR,&dev->flags);
+							}
+							else
+								set_bit(USBCAN_MESSAGE_URB_PENDING,&dev->rx[i].flags);
+						}
+					}
+				}
+				if (test_and_clear_bit(USBCAN_DATA_TX,&dev->flags)){
+					CANMSG("USBCAN TX handler\n");
+					for (i=0;i<USBCAN_TOT_TX_URBS;i++){
+						if (test_and_clear_bit(USBCAN_MESSAGE_DATA_OK,&dev->tx[i].flags)){
+							struct usbcan_message *mess=&dev->tx[i];
+							CANMSG("USBCAN Message successfully sent\n");
+
+							if(mess->slot){
+								// Do local transmitted message distribution if enabled
+								if (processlocal){
+									// fill CAN message timestamp
+									can_filltimestamp(&mess->slot->msg.timestamp);
+
+									mess->slot->msg.flags |= MSG_LOCAL;
+									canque_filter_msg2edges(obj->qends, &mess->slot->msg);
+								}
+								// Free transmitted slot
+								canque_free_outslot(obj->qends, mess->qedge, mess->slot);
+								mess->slot=NULL;
+							}
+							can_msgobj_clear_fl(obj,TX_PENDING);
+
+							set_bit(USBCAN_FREE_TX_URB,&dev->flags);
+							set_bit(USBCAN_MESSAGE_FREE,&dev->tx[i].flags);
+
+							// Test if some new messages arrived
+							set_bit(USBCAN_TX_PENDING,&dev->flags);
+						}
+					}
+				}
+			}
+			if (test_and_clear_bit(USBCAN_TX_PENDING,&dev->flags)){
+				int i, cmd,j,len;
+				for (i=0;i<USBCAN_TOT_TX_URBS;i++){
+					if (test_bit(USBCAN_MESSAGE_FREE,&dev->tx[i].flags)){
+						struct usbcan_message *mess=&dev->tx[i];
+						u8 *ptr;
+						cmd=canque_test_outslot(obj->qends, &mess->qedge, &mess->slot);
+						if(cmd>=0){
+							CANMSG("USBCAN Sending a message\n");
+
+							can_msgobj_set_fl(obj,TX_PENDING);
+							clear_bit(USBCAN_FREE_TX_URB,&dev->flags);
+							clear_bit(USBCAN_MESSAGE_FREE,&dev->tx[i].flags);
+
+							*(u8 *)(mess->msg)=0;
+							len = mess->slot->msg.length;
+							if(len > CAN_MSG_LENGTH)
+								len = CAN_MSG_LENGTH;
+							*(u8 *)(mess->msg+1)=len & 0xFF;
+							*(u16 *)(mess->msg+2)=cpu_to_le16(mess->slot->msg.flags);
+							*(u32 *)(mess->msg+4)=cpu_to_le32(mess->slot->msg.id);
+
+							for(ptr=mess->msg+8,j=0; j < len; ptr++,j++) {
+								*ptr=mess->slot->msg.data[j] & 0xFF;
+							}
+							for(; j < 8; ptr++,j++) {
+								*ptr=0;
+							}
+
+							set_bit(USBCAN_MESSAGE_URB_PENDING,&mess->flags);
+							retval = usb_submit_urb (dev->tx[i].u, GFP_KERNEL);
+							if (retval){
+								CANMSG("%d. URB error %d\n",i,retval);
+								clear_bit(USBCAN_MESSAGE_URB_PENDING,&mess->flags);
+								set_bit(USBCAN_FREE_TX_URB,&dev->flags);
+								set_bit(USBCAN_MESSAGE_FREE,&dev->tx[i].flags);
+								obj->ret = -1;
+								canque_notify_inends(mess->qedge, CANQUEUE_NOTIFY_ERRTX_SEND);
+								canque_free_outslot(obj->qends, mess->qedge, mess->slot);
+								mess->slot=NULL;
+							}
+						}
+						else{
+							set_bit(USBCAN_FREE_TX_URB,&dev->flags);
+							break;
+						}
 					}
-
-					// fill CAN message timestamp
-					can_filltimestamp(&obj->rx_msg.timestamp);
-					canque_filter_msg2edges(obj->qends, &obj->rx_msg);
 				}
-				else
-					CANMSG("Destination chip not found\n");
 			}
     }
   }
-	clear_bit(USBCAN_THREAD_RUNNING,&dev->flags);
-
+	set_bit(USBCAN_TERMINATE,&dev->flags);
+exit:
   /* here we go only in case of termination of the thread */
-	if (dev->rx){
-		usb_kill_urb(dev->rx);
-		usb_free_urb(dev->rx);
+	for (i=0;i<USBCAN_TOT_RX_URBS;i++){
+		if (dev->rx[i].u){
+			set_bit(USBCAN_MESSAGE_TERMINATE,&dev->rx[i].flags);
+			usb_kill_urb(dev->rx[i].u);
+			usb_free_urb(dev->rx[i].u);
+		}
+	}
+	for (i=0;i<USBCAN_TOT_TX_URBS;i++){
+		if (dev->tx[i].u){
+			set_bit(USBCAN_MESSAGE_TERMINATE,&dev->tx[i].flags);
+			usb_kill_urb(dev->tx[i].u);
+			usb_free_urb(dev->tx[i].u);
+		}
 	}
+	clear_bit(USBCAN_THREAD_RUNNING,&dev->flags);
 
   CANMSG ("usbcan thread finished!\n");
   return 0;
@@ -1235,6 +1251,7 @@ static int usbcan_probe(struct usb_interface *interface, const struct usb_device
 		dev=usbdevs->devs[j];
 
 		mutex_init(&dev->io_mutex);
+		init_waitqueue_head(&dev->queue);
 		dev->udev = interface_to_usbdev(interface);
 		dev->interface = interface;
 
@@ -1288,7 +1305,7 @@ static int usbcan_probe(struct usb_interface *interface, const struct usb_device
 		goto register_error;
 
 	/* let the user know what node this device is now attached to */
-	DEBUGMSG("USBCAN device now attached");
+	CANMSG("USBCAN device now attached\n");
 	return 0;
 
 register_error:
@@ -1342,6 +1359,15 @@ static void usbcan_disconnect(struct usb_interface *interface)
 			usbdevs->devs[j]->interface = NULL;
 			mutex_unlock(&usbdevs->devs[j]->io_mutex);
 
+			while (test_bit(USBCAN_THREAD_RUNNING,&usbdevs->devs[j]->flags))
+			{
+				CANMSG("USBCAN thread has not stopped, trying to wake...\n");
+				set_bit(USBCAN_TERMINATE,&usbdevs->devs[j]->flags);
+				wake_up_process(usbdevs->devs[j]->comthread);
+				schedule();
+// 			can_kthread_stop(dev->comthread);
+			}
+
 			if (usbdevs->devs[j]->bulk_in_buffer)
 				can_checked_free(usbdevs->devs[j]->bulk_in_buffer);
 			// if (usbdevs->devs[j]->chip){
@@ -1354,7 +1380,7 @@ static void usbcan_disconnect(struct usb_interface *interface)
 	}
 	can_checked_free(usbdevs);
 
-	DEBUGMSG("USBCAN now disconnected");
+	CANMSG("USBCAN now disconnected\n");
 }
 
 int usbcan_init(void){