X-Git-Url: http://rtime.felk.cvut.cz/gitweb/lincan.git/blobdiff_plain/f6c113a5fb217e7495b93053cf29fa6966923445..a6d355f1cf48f39eecb2f1bdead51817d1fd5989:/lincan/src/usbcan.c?ds=sidebyside

diff --git a/lincan/src/usbcan.c b/lincan/src/usbcan.c
index b356373..42ece6f 100644
--- a/lincan/src/usbcan.c
+++ b/lincan/src/usbcan.c
@@ -15,6 +15,8 @@
 static int usbcan_probe(struct usb_interface *interface, const struct usb_device_id *id);
 static void usbcan_disconnect(struct usb_interface *interface);
 
+volatile int usbcan_chip_count=0;
+
 /* table of devices that work with this driver */
 static struct usb_device_id usbcan_table [] = {
 	{ USB_DEVICE(USBCAN_VENDOR_ID, USBCAN_PRODUCT_ID) },
@@ -45,19 +47,14 @@ static struct usb_driver usbcan_driver = {
  */
 int usbcan_request_io(struct candevice_t *candev)
 {
-	struct usbcan_usb *dev = (struct usbcan_usb*)candev->sysdevptr.anydev;
+	struct usbcan_devs *usbdevs = (struct usbcan_devs *)candev->sysdevptr.anydev;
 
-	if (!dev){
+	if (!usbdevs){
 		CANMSG("USBCAN_REQUEST_IO: Cannot register usbcan while usb device is not present.\n");
 		CANMSG("USBCAN_REQUEST_IO: Usbcan registers automatically on device insertion.\n");
 		return -ENODEV;
 	}
 
-	/* start kernel thread */
-	dev->comthread=can_kthread_run(usbcan_read_kthread, (void *)dev, "usbcan");
-
-	/* Adding link to can device into usbcan_usb struct */
-	((struct usbcan_usb*)candev->sysdevptr.anydev)->candev=candev;
 	return 0;
 }
 
@@ -75,13 +72,6 @@ int usbcan_request_io(struct candevice_t *candev)
  */
 int usbcan_release_io(struct candevice_t *candev)
 {
-	struct usbcan_usb *dev = ((struct usbcan_usb*)candev->sysdevptr.anydev);
-	if (!dev)
-		return 0;
-
-	/* terminate the kernel thread */
-	can_kthread_stop(dev->comthread);
-
 	return 0;
 }
 
@@ -119,7 +109,7 @@ int usbcan_init_hw_data(struct candevice_t *candev)
 	candev->res_addr=RESET_ADDR;
 	candev->nr_82527_chips=0;
 	candev->nr_sja1000_chips=0;
-	candev->nr_all_chips=1;
+	candev->nr_all_chips=usbcan_chip_count;
 	candev->flags |= CANDEV_PROGRAMMABLE_IRQ*0;
 
 	return 0;
@@ -144,7 +134,7 @@ int usbcan_init_hw_data(struct candevice_t *candev)
  */
 int usbcan_init_obj_data(struct canchip_t *chip, int objnr)
 {
- 	chip->msgobj[objnr]->obj_base_addr=chip->chip_base_addr+(objnr+1)*0x10;
+ 	chip->msgobj[objnr]->obj_base_addr=0;
 
 	return 0;
 }
@@ -271,9 +261,9 @@ int usbcan_chip_config(struct canchip_t *chip)
 int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned  long mask)
 {
 	int retval;
-	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
+	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 
-	u8 usbbuf[16];
+	u8 usbbuf[USBCAN_TRANSFER_SIZE];
 
 	if (!dev)
 		return -ENODEV;
@@ -282,21 +272,21 @@ int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned  l
 	*(uint32_t *)(usbbuf+4)=cpu_to_le32(code);
 
 	retval=usb_control_msg(dev->udev,
-		usb_sndctrlpipe(dev->udev, dev->ctl_out_endpointAddr),
+		usb_sndctrlpipe(dev->udev, 0),
 		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;
 
 	retval = usb_control_msg(dev->udev,
-		usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
+		usb_rcvctrlpipe(dev->udev, 0),
 		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){
@@ -327,9 +317,9 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw,
 							int sampl_pt, int flags)
 {
 	int retval;
-	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
+	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 
-	u8 usbbuf[16];
+	u8 usbbuf[USBCAN_TRANSFER_SIZE];
 
 	if (!dev)
 		return -ENODEV;
@@ -340,21 +330,21 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw,
 	*(int32_t *)(usbbuf+12)=cpu_to_le32(flags);
 
 	retval=usb_control_msg(dev->udev,
-		usb_sndctrlpipe(dev->udev, dev->ctl_out_endpointAddr),
+		usb_sndctrlpipe(dev->udev, 0),
 		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;
 
 	retval = usb_control_msg(dev->udev,
-		usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
+		usb_rcvctrlpipe(dev->udev, 0),
 		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){
@@ -398,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->hostdevice->sysdevptr.anydev;
-	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;
 }
 
@@ -447,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->hostdevice->sysdevptr.anydev;
-	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;
 }
 
@@ -498,7 +419,7 @@ int usbcan_send_msg(struct canchip_t *chip, struct msgobj_t *obj,
  */
 int usbcan_check_tx_stat(struct canchip_t *chip)
 {
-	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
+	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 	if (!dev)
 		return 0;
 	if (test_bit(USBCAN_TX_PENDING,&dev->flags))
@@ -519,22 +440,23 @@ int usbcan_set_btregs(struct canchip_t *chip, unsigned short btr0,
 							unsigned short btr1)
 {
 	int retval;
-	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
+	u8	buf[USBCAN_TRANSFER_SIZE];
+	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 	uint16_t value=(btr1&0xFF)<<8 | (btr0&0xFF);
 
 	if (!dev)
 		return -ENODEV;
 
 	retval = usb_control_msg(dev->udev,
-	usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
+	usb_rcvctrlpipe(dev->udev, 0),
 	USBCAN_VENDOR_SET_BTREGS,
 	USB_TYPE_VENDOR,
 	cpu_to_le16(value), cpu_to_le16(chip->chip_idx),
-	dev->ctl_in_buffer, dev->ctl_in_size,
+	&buf, USBCAN_TRANSFER_SIZE,
 	10000);
 
 	if (retval==1){
-		if(dev->ctl_in_buffer[0]==1)
+		if(buf[0]==1)
 			return 0;
 	}
 	return -ENODEV;
@@ -550,21 +472,22 @@ int usbcan_set_btregs(struct canchip_t *chip, unsigned short btr0,
 int usbcan_start_chip(struct canchip_t *chip)
 {
 	int retval;
-	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
+	u8	buf[USBCAN_TRANSFER_SIZE];
+	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 
 	if (!dev)
 		return -ENODEV;
 
 	retval = usb_control_msg(dev->udev,
-	usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
+	usb_rcvctrlpipe(dev->udev, 0),
 	USBCAN_VENDOR_START_CHIP,
 	USB_TYPE_VENDOR,
 	cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
-	dev->ctl_in_buffer, dev->ctl_in_size,
+	&buf, USBCAN_TRANSFER_SIZE,
 	10000);
 
 	if (retval==1){
-		if(dev->ctl_in_buffer[0]==1)
+		if(buf[0]==1)
 			return 0;
 	}
 	return -ENODEV;
@@ -581,26 +504,25 @@ int usbcan_start_chip(struct canchip_t *chip)
  */
 int usbcan_chip_queue_status(struct canchip_t *chip)
 {
-	int retval,i;
-	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
+	int retval;
+	u8	buf[USBCAN_TRANSFER_SIZE];
+	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 
 	if (!dev)
 		return -ENODEV;
 	retval = usb_control_msg(dev->udev,
-	usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
+	usb_rcvctrlpipe(dev->udev, 0),
 	USBCAN_VENDOR_CHECK_TX_STAT,
 	USB_TYPE_VENDOR,
 	cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
-	dev->ctl_in_buffer, dev->ctl_in_size,
+	&buf, USBCAN_TRANSFER_SIZE,
 	10000);
 
-	for (i=0;i<dev->ctl_in_size;i++)
-		CANMSG("Buffer content: %d\n",dev->ctl_in_buffer[i]);
 	if (retval==1){
-		CANMSG("Chip_queue_status: %d\n",dev->ctl_in_buffer[0]);
-		if(dev->ctl_in_buffer[0]==1)
+		CANMSG("Chip_queue_status: %d\n",buf[0]);
+		if(buf[0]==1)
 			return 0;
-		if(dev->ctl_in_buffer[0]==0)
+		if(buf[0]==0)
 			return 1;
 	}
 	CANMSG("Chip_queue_status error: %d\n",retval);
@@ -617,26 +539,49 @@ int usbcan_chip_queue_status(struct canchip_t *chip)
 int usbcan_stop_chip(struct canchip_t *chip)
 {
 	int retval;
-	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
+	u8	buf[USBCAN_TRANSFER_SIZE];
+	struct usbcan_usb *dev=(struct usbcan_usb*)chip->chip_data;
 
 	if (!dev)
 		return -ENODEV;
 
 	retval = usb_control_msg(dev->udev,
-	usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
+	usb_rcvctrlpipe(dev->udev, 0),
 	USBCAN_VENDOR_STOP_CHIP,
 	USB_TYPE_VENDOR,
 	cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
-	dev->ctl_in_buffer, dev->ctl_in_size,
+	&buf, USBCAN_TRANSFER_SIZE,
 	10000);
 
 	if (retval==1){
-		if(dev->ctl_in_buffer[0]==1)
+		if(buf[0]==1)
 			return 0;
 	}
 	return -ENODEV;
 }
 
+/**
+ * usbcan_register_devs: - attaches usb device data to the chip structure
+ * @chip: pointer to chip state structure
+ * @data: usb device data
+ *
+ * File: src/usbcan.c
+ */
+void usbcan_register_devs(struct canchip_t *chip,void *data){
+	struct usbcan_devs *usbdevs=(struct usbcan_devs *)data;
+	if (!usbdevs){
+	    CANMSG("Bad structure given\n");
+	    return;
+	}
+	if (chip->chip_idx>=usbdevs->count) {
+	    CANMSG("Requested chip number is bigger than chip count\n");
+	    return;
+	}
+
+	usbdevs->devs[chip->chip_idx]->chip=chip;
+	chip->chip_data=(void *)usbdevs->devs[chip->chip_idx];
+}
+
 /**
  * usbcan_attach_to_chip: - attaches to the chip, setups registers and state
  * @chip: pointer to chip state structure
@@ -646,6 +591,11 @@ int usbcan_stop_chip(struct canchip_t *chip)
  */
 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_kthread, (void *)dev, "usbcan_%d",chip->chip_idx);
+
 	return 0;
 }
 
@@ -658,7 +608,15 @@ int usbcan_attach_to_chip(struct canchip_t *chip)
  */
 int usbcan_release_chip(struct canchip_t *chip)
 {
+	struct usbcan_usb *dev = (struct usbcan_usb *)chip->chip_data;
+
 	usbcan_stop_chip(chip);
+
+	/* terminate the kernel thread */
+	set_bit(USBCAN_TERMINATE,&dev->flags);
+	wake_up_process(dev->comthread);
+//  	can_kthread_stop(dev->comthread);
+
 	return 0;
 }
 
@@ -720,53 +678,157 @@ int usbcan_config_irqs(struct canchip_t *chip, short irqs)
 }
 
 /**
- * usbcan_irq_write_handler: - part of ISR code responsible for transmit events
- * @chip: pointer to chip state structure
- * @obj: pointer to attached queue description
+ * usbcan_kthread_read_handler: - part of kthread code responsible for receive completed events
+ * @dev: pointer to usb device related structure
+ * @obj: pointer to attached message object description
  *
- * The main purpose of this function is to read message from attached queues
- * and transfer message contents into CAN controller chip.
+ * The main purpose of this function is to read message from usb urb
+ * and transfer message contents to CAN queue ends.
  * This subroutine is called by
- * usbcan_irq_write_handler() for transmit events.
+ * usbcan_kthread().
  * File: src/usbcan.c
  */
-void usbcan_irq_write_handler(struct canchip_t *chip, struct msgobj_t *obj)
-{
-	int cmd;
-
-	if(obj->tx_slot){
-		// Do local transmitted message distribution if enabled
-		if (processlocal){
-			// fill CAN message timestamp
-			can_filltimestamp(&obj->tx_slot->msg.timestamp);
+void usbcan_kthread_read_handler(struct usbcan_usb *dev, struct msgobj_t *obj){
+	int i, j, len, retval;
+	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;
+				}
 
-			obj->tx_slot->msg.flags |= MSG_LOCAL;
-			canque_filter_msg2edges(obj->qends, &obj->tx_slot->msg);
+				// 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);
 		}
-		// 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);
-	cmd=canque_test_outslot(obj->qends, &obj->tx_qedge, &obj->tx_slot);
-	if(cmd<0)
-		return;
-	can_msgobj_set_fl(obj,TX_PENDING);
+/**
+ * usbcan_kthread_write_handler: - part of kthread code responsible for transmit done events
+ * @dev: pointer to usb device related structure
+ * @obj: pointer to attached message object description
+ *
+ * The main purpose of this function is to free allocated resources on transmit done event
+ * This subroutine is called by
+ * usbcan_kthread().
+ * File: src/usbcan.c
+ */
+void usbcan_kthread_write_handler(struct usbcan_usb *dev, struct msgobj_t *obj){
+	int i;
+	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);
 
-	if (chip->chipspecops->pre_write_config(chip, obj, &obj->tx_slot->msg)) {
-		obj->ret = -1;
-		canque_notify_inends(obj->tx_qedge, CANQUEUE_NOTIFY_ERRTX_PREP);
-		canque_free_outslot(obj->qends, obj->tx_qedge, obj->tx_slot);
-		obj->tx_slot=NULL;
-		return;
+					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 (chip->chipspecops->send_msg(chip, obj, &obj->tx_slot->msg)) {
-		obj->ret = -1;
-		canque_notify_inends(obj->tx_qedge, CANQUEUE_NOTIFY_ERRTX_SEND);
-		canque_free_outslot(obj->qends, obj->tx_qedge, obj->tx_slot);
-		obj->tx_slot=NULL;
-		return;
+}
+
+/**
+ * usbcan_kthread_write_request_handler: - part of kthread code responsible for sending transmit urbs
+ * @dev: pointer to usb device related structure
+ * @obj: pointer to attached message object description
+ *
+ * The main purpose of this function is to create a usb transmit safe object
+ * and send it via free transmit usb urb
+ * This subroutine is called by
+ * usbcan_kthread().
+ * File: src/usbcan.c
+ */
+void usbcan_kthread_write_request_handler(struct usbcan_usb *dev, struct msgobj_t *obj){
+	int i, j, cmd, len, retval;
+	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;
+			}
+		}
 	}
 }
 
@@ -786,95 +848,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;
 }
 
@@ -891,6 +864,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();
 
@@ -899,14 +874,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();
@@ -1003,8 +980,10 @@ int usbcan_init_chip_data(struct candevice_t *candev, int chipnr)
 }
 
 
+/** *********************************
+ *    USB related functions
+ *  ********************************* */
 
-/* --------------------------------------------------------------------------------------------------- */
 static int usbcan_sleep_thread(struct usbcan_usb *dev)
 {
 	int	rc = 0;
@@ -1019,7 +998,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)
 		)
@@ -1030,255 +1010,374 @@ 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,&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");
-		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);
-		wake_up_process(dev->comthread);
+		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\n",
-		     __FUNCTION__, retval);
-		set_bit(USBCAN_ERROR,&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;
+	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);
+	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("%d. URB error %d\n",i,retval);
+			set_bit(USBCAN_ERROR,&dev->flags);
+			goto exit;
+		}
+		else
+			set_bit(USBCAN_MESSAGE_URB_PENDING,&dev->rx[i].flags);
+	}
   /* an endless loop in which we are doing our work */
   for(;;)
   {
-		if (!can_kthread_should_stop()){
-		    retval=usb_submit_urb(dev->rx, GFP_KERNEL);
-		    if (retval){
-		    	CANMSG("URB error %d\n",retval);
-		    	break;
-		    }
-
-
-		    /* fall asleep */
-		    usbcan_sleep_thread(dev);
+		/* We need to do a memory barrier here to be sure that
+		the flags are visible on all CPUs. */
+		mb();
+		/* fall asleep */
+		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;
 		}
 
-		if (test_bit(USBCAN_ERROR,&dev->flags)){
-			CANMSG("URB error %d\n",retval);
-			clear_bit(USBCAN_ERROR,&dev->flags);
+		/* 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;
 		}
 
 		{ /* 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->candev->chip[dev->rx_msg[0]])&&
-					(dev->candev->chip[dev->rx_msg[0]]->flags & CHIP_CONFIGURED)
-				){
-					u8 *ptr;
-
-					obj=dev->candev->chip[dev->rx_msg[0]]->msgobj[0];
-
-					len=*(uint8_t *)(dev->rx_msg+1);
-					if(len > CAN_MSG_LENGTH) len = CAN_MSG_LENGTH;
-					obj->rx_msg.length = len;
+			if (test_and_clear_bit(USBCAN_DATA_OK,&dev->flags)){
+				CANMSG("USBCAN Succesfull data transfer\n");
 
-					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)));
-
-					for(ptr=dev->rx_msg+8,i=0; i < len; ptr++,i++) {
-						obj->rx_msg.data[i]=*ptr;
-					}
-
-					// fill CAN message timestamp
-					can_filltimestamp(&obj->rx_msg.timestamp);
-					canque_filter_msg2edges(obj->qends, &obj->rx_msg);
+				if (test_and_clear_bit(USBCAN_DATA_RX,&dev->flags)){
+					usbcan_kthread_read_handler(dev, obj);
+				}
+				if (test_and_clear_bit(USBCAN_DATA_TX,&dev->flags)){
+					usbcan_kthread_write_handler(dev, obj);
 				}
-				else
-					CANMSG("Destination chip not found\n");
+			}
+			if (test_and_clear_bit(USBCAN_TX_PENDING,&dev->flags)){
+				usbcan_kthread_write_request_handler(dev, obj);
 			}
     }
   }
+	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;
 error:
   /* cleanup the thread, leave */
   CANMSG ("kernel thread terminated!\n");
   return -ENOMEM;
-//   exit_kthread(kthread);
-
-  /* returning from the thread here calls the exit functions */
 }
 
 static int usbcan_probe(struct usb_interface *interface, const struct usb_device_id *id)
 {
-	struct usbcan_usb *dev;
+	struct usbcan_devs *usbdevs=NULL;
 	struct usb_host_interface *iface_desc;
 	struct usb_endpoint_descriptor *endpoint;
 	size_t buffer_size;
-	int i;
+	int i,j,k;
 	int retval = -ENOMEM;
 
-	/* allocate memory for our device state and initialize it */
-	dev = (struct usbcan_usb *) can_checked_malloc(sizeof(struct usbcan_usb));
-	if (!dev) {
+	iface_desc = interface->cur_altsetting;
+	if (iface_desc->desc.bNumEndpoints % 2){
+		err("Endpoint count must be even");
+		goto noalloc;
+	}
+
+	usbcan_chip_count = iface_desc->desc.bNumEndpoints / 2;
+
+	usbdevs = (struct usbcan_devs *) can_checked_malloc(sizeof(struct usbcan_devs));
+	if (!usbdevs) {
 		err("Out of memory");
 		goto error;
 	}
-	memset(dev, 0, sizeof(struct usbcan_usb));
+	memset(usbdevs, 0, sizeof(struct usbcan_devs));
 
-	sema_init(&dev->limit_sem, WRITES_IN_FLIGHT);
-	mutex_init(&dev->io_mutex);
-	spin_lock_init(&dev->err_lock);
-	init_usb_anchor(&dev->submitted);
+	usbdevs->count=usbcan_chip_count;
 
-// 	dev->udev = usb_get_dev(interface_to_usbdev(interface));
-	dev->udev = interface_to_usbdev(interface);
-	dev->interface = interface;
+	usbdevs->devs = (struct usbcan_usb **) can_checked_malloc(usbcan_chip_count * sizeof(struct usbcan_usb *));
+	if (!usbdevs->devs) {
+		err("Out of memory");
+		goto error;
+	}
+	memset(usbdevs->devs, 0, usbcan_chip_count * sizeof(struct usbcan_usb *));
 
-	/* set up the endpoint information */
-	/* use only the first bulk-in and bulk-out endpoints */
-	iface_desc = interface->cur_altsetting;
-	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
-		endpoint = &iface_desc->endpoint[i].desc;
-
-		if (!dev->bulk_in_endpointAddr &&
-		    usb_endpoint_is_bulk_in(endpoint)) {
-			/* we found a bulk in endpoint */
-			buffer_size = le16_to_cpu(endpoint->wMaxPacketSize);
-			dev->bulk_in_size = buffer_size;
-			dev->bulk_in_endpointAddr = endpoint->bEndpointAddress;
-			dev->bulk_in_buffer = can_checked_malloc(buffer_size);
-			if (!dev->bulk_in_buffer) {
-				err("Could not allocate bulk_in_buffer");
-				goto error;
-			}
-		}
+	for (j=0;j<usbcan_chip_count;j++){
+		struct usbcan_usb *dev;
+		int epnum=-1,was;
 
-		if (!dev->bulk_out_endpointAddr &&
-		    usb_endpoint_is_bulk_out(endpoint)) {
-			/* we found a bulk out endpoint */
-				dev->bulk_out_endpointAddr = endpoint->bEndpointAddress;
+		/* allocate memory for our device state and initialize it */
+		usbdevs->devs[j] = (struct usbcan_usb *) can_checked_malloc(sizeof(struct usbcan_usb));
+		if (!usbdevs->devs[j]) {
+			err("Out of memory");
+			goto error;
 		}
+		memset(usbdevs->devs[j], 0, sizeof(struct usbcan_usb));
+		dev=usbdevs->devs[j];
+
+		mutex_init(&dev->io_mutex);
+		init_waitqueue_head(&dev->queue);
+		dev->udev = interface_to_usbdev(interface);
+		dev->interface = interface;
+
+		/* set up the endpoint information */
+		for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+			endpoint = &iface_desc->endpoint[i].desc;
+
+			if (epnum==-1){
+				was=0;
+				for (k=0;k<j;k++){
+					if ((usbdevs->devs[k]->bulk_in_endpointAddr & USB_ENDPOINT_NUMBER_MASK) == (endpoint->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK))
+						was=1;
+				}
+				if (was) continue;
+				epnum=endpoint->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
+			}
 
-	}
-	if (!(dev->bulk_in_endpointAddr && dev->bulk_out_endpointAddr)) {
-		err("Could not find all bulk-in and bulk-out endpoints");
-		goto error;
-	}
-	dev->ctl_in_endpointAddr=0;
-	dev->ctl_in_size=16;
-	dev->ctl_in_buffer = can_checked_malloc(dev->ctl_in_size);
-	dev->ctl_out_endpointAddr=0;
+			if (!dev->bulk_in_endpointAddr &&
+					usb_endpoint_is_bulk_in(endpoint)) {
+				if (epnum == (endpoint->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK)){
+					/* we found a bulk in endpoint */
+					buffer_size = le16_to_cpu(endpoint->wMaxPacketSize);
+					dev->bulk_in_size = buffer_size;
+					dev->bulk_in_endpointAddr = endpoint->bEndpointAddress;
+					dev->bulk_in_buffer = can_checked_malloc(buffer_size);
+					if (!dev->bulk_in_buffer) {
+						err("Could not allocate bulk_in_buffer");
+						goto error;
+					}
+				}
+			}
+
+			if (!dev->bulk_out_endpointAddr &&
+					usb_endpoint_is_bulk_out(endpoint)) {
+				if (epnum == (endpoint->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK)){
+				/* we found a bulk out endpoint */
+					dev->bulk_out_endpointAddr = endpoint->bEndpointAddress;
+				}
+			}
 
+		}
+		if (!(dev->bulk_in_endpointAddr && dev->bulk_out_endpointAddr)) {
+			err("Could not find all bulk-in and bulk-out endpoints for chip %d",j);
+			goto error;
+		}
+	}
 	/* save our data pointer in this interface device */
-	usb_set_intfdata(interface, dev);
+	usb_set_intfdata(interface, usbdevs);
 
-	register_usbdev("usbcan",(void *) dev);
+	if (!(usbdevs->candev=register_usbdev("usbcan",(void *) usbdevs, usbcan_register_devs)))
+		goto register_error;
 
 	/* let the user know what node this device is now attached to */
-	info("USB Skeleton device now attached");
+	CANMSG("USBCAN device now attached\n");
 	return 0;
 
+register_error:
+	cleanup_usbdev(usbdevs->candev);
 error:
-	usb_put_dev(dev->udev);
-	if (dev->bulk_in_buffer)
-		can_checked_free(dev->bulk_in_buffer);
-	if (dev->ctl_in_buffer)
-		can_checked_free(dev->ctl_in_buffer);
-	if (dev->candev){
-		dev->candev->sysdevptr.anydev=NULL;
-		cleanup_usbdev(dev->candev);
+	if (usbdevs){
+		if (usbdevs->devs){
+			if (usbdevs->devs[0]){
+				usb_put_dev(usbdevs->devs[0]->udev);
+			}
+			for (j=0;j<usbdevs->count;j++){
+				if (!usbdevs->devs[j])	continue;
+
+				if (usbdevs->devs[j]->bulk_in_buffer)
+					can_checked_free(usbdevs->devs[j]->bulk_in_buffer);
+				if (usbdevs->devs[j]->chip){
+					usbdevs->devs[j]->chip->chip_data=NULL;
+				}
+				can_checked_free(usbdevs->devs[j]);
+			}
+			can_checked_free(usbdevs->devs);
+		}
+		can_checked_free(usbdevs);
 	}
-	can_checked_free(dev);
+noalloc:
 	return retval;
 }
 
 // Physically disconnected device
 static void usbcan_disconnect(struct usb_interface *interface)
 {
-	struct usbcan_usb *dev;
-	int minor = interface->minor;
-
-	dev = usb_get_intfdata(interface);
-	if (!dev)
+	struct usbcan_devs *usbdevs;
+	int j;
+	usbdevs = usb_get_intfdata(interface);
+	if (usbdevs==NULL){
+		CANMSG("USBCAN device seems to be removed\n");
 		return;
+	}
 	usb_set_intfdata(interface, NULL);
 
-	/* prevent more I/O from starting */
-	mutex_lock(&dev->io_mutex);
-	dev->interface = NULL;
-	mutex_unlock(&dev->io_mutex);
-
-	//usb_kill_anchored_urbs(&dev->submitted);
-
-	usb_put_dev(dev->udev);
-
-	if (dev->candev){
-		dev->candev->sysdevptr.anydev=NULL;
-		cleanup_usbdev(dev->candev);
+	if (usbdevs->devs){
+		usb_put_dev((*usbdevs->devs)->udev);
 	}
+	cleanup_usbdev(usbdevs->candev);
+	if (usbdevs->devs){
+		for (j=0;j<usbdevs->count;j++){
+			if (!usbdevs->devs[j])	continue;
+
+			/* prevent more I/O from starting */
+			mutex_lock(&usbdevs->devs[j]->io_mutex);
+			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 (dev->bulk_in_buffer)
-		can_checked_free(dev->bulk_in_buffer);
-	if (dev->ctl_in_buffer)
-		can_checked_free(dev->ctl_in_buffer);
-
-	can_checked_free(dev);
+			if (usbdevs->devs[j]->bulk_in_buffer)
+				can_checked_free(usbdevs->devs[j]->bulk_in_buffer);
+			// if (usbdevs->devs[j]->chip){
+			// 	usbdevs->devs[j]->chip->chip_data=NULL;
+			// }
+			can_checked_free(usbdevs->devs[j]);
+			usbdevs->devs[j]=NULL;
+		}
+		can_checked_free(usbdevs->devs);
+	}
+	can_checked_free(usbdevs);
 
-	info("USB Skeleton now disconnected");
+	CANMSG("USBCAN now disconnected\n");
 }
 
 int usbcan_init(void){