From: Jan Kriz <krizj1@fel.cvut.cz>
Date: Sat, 26 Jul 2008 01:16:43 +0000 (+0200)
Subject: Embedded code and LinCAN driver now working
X-Git-Tag: CLT_COMM_CAN_usb_can1_kriz_bp~5
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/lincan.git/commitdiff_plain/f6c113a5fb217e7495b93053cf29fa6966923445

Embedded code and LinCAN driver now working

fixed kernel thread running in atomic context
fixed initialization of SJA chip in embedded code
fixed vendor functions
pending more usb urbs handling in LinCAN
---

diff --git a/embedded/app/usbcan/can.c b/embedded/app/usbcan/can.c
index ae9d530..25d7f34 100644
--- a/embedded/app/usbcan/can.c
+++ b/embedded/app/usbcan/can.c
@@ -35,13 +35,12 @@ void can_comm_init()
 
 	SET_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
 	CLR_OUT_PIN(IO1,P1_SJA1000_RST_PIN);
-	for (slowdown=0;slowdown<20*SJA1000_SCLK;slowdown++);
+	SJA1000_INIT_DELAY();
 	SET_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
 	// Due to change in design there is CS_PIN connected with ALE_PIN and ALE_PIN connection to LPC is interrupted
 	//	CLR_OUT_PIN(IO0,P0_SJA1000_ALE_PIN);
 	SET_OUT_PIN(IO1,P1_SJA1000_RST_PIN);
-	for (slowdown=0;slowdown<20*SJA1000_SCLK;slowdown++);
-
+	SJA1000_INIT_DELAY();
 }
 
 int can_write(uint8_t address,uint8_t* data)
@@ -52,25 +51,25 @@ int can_write(uint8_t address,uint8_t* data)
 	SET_OUT_PIN(IO0,P0_SJA1000_RD_PIN);	// Stays high on write
 	SET_OUT_PIN(IO0,P0_SJA1000_WR_PIN); // Stays high on address write
 	SET_OUT_PIN(IO0,P0_SJA1000_CS_PIN); // Sets output buffers to third state
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
+	SJA1000_DELAY();
 	//SET_OUT_PIN(IO0,P0_SJA1000_ALE_PIN); // Start command
 
 	// Set memory address
 	IO1SET=__val2mfld(0x00FF0000,address); // Shift data to SJA pins and output them
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
+	SJA1000_DELAY();
 	//CLR_OUT_PIN(IO0,P0_SJA1000_ALE_PIN); // Makes address active
 	CLR_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
+	SJA1000_DELAY();
 
 	// Setting data
 	CLR_OUT_PIN(IO0,P0_SJA1000_WR_PIN);
 
 	IO1CLR=0x00FF0000;
 	IO1SET=__val2mfld(0x00FF0000,*data);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
+	SJA1000_DELAY();
 	SET_OUT_PIN(IO0,P0_SJA1000_WR_PIN); // Data should be accepted by now
 	SET_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
+	SJA1000_DELAY();
 	return 0;
 }
 
@@ -82,12 +81,12 @@ int can_read(const uint8_t address,uint8_t* data)
 	SET_OUT_PIN(IO0,P0_SJA1000_WR_PIN); // Stays high on read
 	SET_OUT_PIN(IO0,P0_SJA1000_RD_PIN); // Stays high while entering address
 	SET_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
+	SJA1000_DELAY();
 	//SET_OUT_PIN(IO0,P0_SJA1000_ALE_PIN);
 
 	// Request memory address
 	IO1SET=__val2mfld(0x00FF0000,address);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
+	SJA1000_DELAY();
 	//CLR_OUT_PIN(IO0,P0_SJA1000_ALE_PIN);
 	CLR_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
 
@@ -95,11 +94,11 @@ int can_read(const uint8_t address,uint8_t* data)
 
 	IO1DIR&=~0x00FF0000; // Sets port as input
 	CLR_OUT_PIN(IO0,P0_SJA1000_RD_PIN);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
+	SJA1000_DELAY();
 	*data=__mfld2val(0x00FF0000,IO1PIN);
 	SET_OUT_PIN(IO0,P0_SJA1000_RD_PIN);
 	SET_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
+	SJA1000_DELAY();
 	return 0;
 }
 
@@ -135,138 +134,3 @@ int can_init(){
 
 	return 0;
 }
-
-int can_autobaud(int tries){
-	uint8_t data=0;
-	int i=0,j,baudset=0;
-	// Single acceptance filter, reset mode, listen mode for baud determination
-	data=sjaMOD_LOM|sjaMOD_AFM|sjaMOD_RM;
-	can_write((uint8_t)SJAMOD,(uint8_t*)&data);
-	do {
-		canbaud.baudrate=sja1000_freqs[i][0]*1000;
-		canbaud.sjw=(sja1000_freqs[i][1]&0x3F)|(sja1000_freqs[i][2]&0xC0);
-		canbaud.sample_pt=(sja1000_freqs[i][3]&0x0F)|(sja1000_freqs[i][4]&0x70);
-		can_write(SJABTR0,(uint8_t *)(&canbaud.sjw));
-		can_write(SJABTR1,(uint8_t *)(&canbaud.sample_pt));
-		uint8_t intstor,temp;
-		can_read(SJAIER,&intstor);
-		temp=sjaIER_RIE|sjaIER_BEIE;
-		can_write(SJAIER,&temp);
-
-		/// Leave reset mode
-		can_read(SJAMOD,&temp);
-		temp&=~sjaMOD_RM;
-		can_write(SJAMOD,&temp);
-		j=0;
-		do{
-			can_read(SJAMOD,&temp);
-			if (j++>20)
-				return -1;
-		} while (temp&sjaMOD_RM);
-
-		/// Wait for successfull packet receive or receive error
-		j=0;
-		while (!(IO0PIN & P0_SJA1000_INT_PIN)){
-		}
-
-		can_read(SJAIR,&temp);
-		if (temp & sjaIR_RI)
-			baudset=1;
-
-		/// Back to reset
-		can_read(SJAMOD,&temp);
-		temp|=sjaMOD_RM;
-		can_write(SJAMOD,&temp);
-		j=0;
-		do{
-			can_read(SJAMOD,&temp);
-			if (j++>20)
-				return -1;
-		} while (!(temp&sjaMOD_RM));
-
-		if (++i==sja1000_freq_cnt)
-			return -1;
-	} while (baudset==0);
-	return 0;
-}
-
-int can_poll(){
-
-	return 0;
-}
-
-int can_transmit_direct(uint8_t *data){
-	int len=0,ext=0,i;
-	uint8_t *ptr=data,tmitptr,temp;
-	ext=(*ptr)& sjaFRM_FF;
-	len=(*ptr)& 0xFF;
-
-	// check if transmit buffer is free
-	do {
-		if (can_poll()==-1)
-			return -1;
-		can_read(SJASR,&temp);
-		temp&=sjaSR_TBS;
-	} while (!temp);
-
-	tmitptr=SJAFRM;
-	can_write(tmitptr++,data++); // Header
-	can_write(tmitptr++,data++); // Ident 1
-	can_write(tmitptr++,data++); // Ident 2
-	if (ext){
-		can_write(tmitptr++,data++); // Ident 3
-		can_write(tmitptr++,data++); // Ident 4
-	}
-	for (i=0;i<len;i++)
-		can_write(tmitptr++,data++); // Data i;
-
-	// Set TR bit
-	temp=sjaCMR_TR;
-	can_write(SJACMR,&temp);
-
-	return 0;
-}
-
-int can_transmit(int ext_header,int rtr,uint8_t *mask,int numbytes,uint8_t *data){
-	uint8_t tmitptr,temp;
-	int i=0;
-
-	if (numbytes<0)
-		numbytes=0;
-
-	if (mask==NULL)
-	;	// use internal mask
-
-	// check if transmit buffer is free
-	do {
-		if (can_poll()==-1)
-			return -1;
-		can_read(SJASR,&temp);
-		temp&=sjaSR_TBS;
-	} while (!temp);
-
-	tmitptr=SJAFRM;
-	temp=(ext_header?sjaFRM_FF:0)|(rtr?sjaFRM_RTR:0)|(numbytes>8?0x08:numbytes);
-	can_write(tmitptr++,&temp); // Header
-
-	can_write(tmitptr++,mask++); // Ident 1
-	can_write(tmitptr++,mask++); // Ident 2
-	if (ext_header){
-		can_write(tmitptr++,mask++); // Ident 3
-		can_write(tmitptr++,mask++); // Ident 4
-	}
-	for (i=0;i<(numbytes>8?0x08:numbytes);i++)
-		can_write(tmitptr++,data++); // Data i;
-
-	// Set TR bit
-	temp=sjaCMR_TR;
-	can_write(SJACMR,&temp);
-
-	return 0;
-}
-
-int can_receive(uint8_t len,uint8_t *data){
-
-
-	return 0;
-}
diff --git a/embedded/app/usbcan/can/can.h b/embedded/app/usbcan/can/can.h
index 38c68dd..4dacadf 100644
--- a/embedded/app/usbcan/can/can.h
+++ b/embedded/app/usbcan/can/can.h
@@ -27,14 +27,25 @@
  */
 
  /*
-		NOP count for synchronizing with peripheral (SJA1000)
+		Delay for synchronization with peripheral (SJA1000)
  */
- #define SJA1000_CLK (24000000)	/* XTAL frequency */
- #define SJA1000_PRESC (2)	/* embedded prescaler */
- #define SJA1000_CCLK (SJA1000_CLK/SJA1000_PRESC)	/* SJA core frequency */
- #define SJA1000_SCLK (PCLK/SJA1000_CCLK)	/* Clock count to synchronize with LPC */
+	#define SJA1000_CLK (24000000)	/* XTAL frequency */
+	#define SJA1000_PRESC (2)	/* embedded prescaler */
+	#define SJA1000_CCLK (SJA1000_CLK/SJA1000_PRESC)	/* SJA core frequency */
+	#define SJA1000_SCLK (PCLK/SJA1000_CCLK)	/* Clock count to synchronize with LPC */
 
-	volatile int slowdown;
+	#define SJA1000_DELAY(void)	\
+			({ \
+				volatile int delay; \
+				for (delay=0;delay<SJA1000_SCLK;delay++) \
+					IO0SET=0; \
+			})
+	#define SJA1000_INIT_DELAY(void)	\
+			({ \
+				volatile int delay; \
+				for (delay=0;delay<20*SJA1000_SCLK;delay++) \
+					IO0SET=0; \
+			})
 
 /**
  * struct can_baudparams_t - datatype for calling CONF_BAUDPARAMS IOCTL
@@ -54,17 +65,7 @@ struct can_baudparams_t {
 };
 
 extern int can_init();
-extern int can_autobaud(int tries);
 extern int can_read(uint8_t address,uint8_t *data);
 extern int can_write(uint8_t address,uint8_t *data);
 
-extern int can_set_mask(int dual_mask,int count,uint8_t *mask,uint8_t *accept);
-
-extern int can_poll();
-extern int can_transmit_direct(uint8_t *data);
-//int can_transmit(char ext_header,char rtr,int mask_size,uint8_t *mask,int numbytes,uint8_t *data);
-extern int can_transmit(int ext_header,int rtr,uint8_t *mask,int numbytes,uint8_t *data);
-//int can_receive(uint8_t *data);
-extern int can_receive(uint8_t len,uint8_t *data);
-
 #endif /* SJA1000_CAN_H */
diff --git a/embedded/app/usbcan/can/main.h b/embedded/app/usbcan/can/main.h
index 4b06a3d..23c8ab3 100644
--- a/embedded/app/usbcan/can/main.h
+++ b/embedded/app/usbcan/can/main.h
@@ -20,13 +20,13 @@
 #include "./errno.h"
 
 #ifdef CAN_DEBUG
-	#define DEBUGMSG(fmt,args...) can_printk(KERN_ERR "can.o (debug): " fmt,\
+	#define DEBUGMSG(fmt,args...) can_printk(KERN_ERR "USBCAN(debug): " fmt,\
 	##args)
 #else
 	#define DEBUGMSG(fmt,args...)
 #endif
 
-#define CANMSG(fmt,args...) can_printk(KERN_ERR "can.o: " fmt,##args)
+#define CANMSG(fmt,args...) can_printk(KERN_ERR "USBCAN: " fmt,##args)
 
 
 extern can_spinlock_t canuser_manipulation_lock;
diff --git a/embedded/app/usbcan/can/ul_usb1.h b/embedded/app/usbcan/can/ul_usb1.h
index 0848d91..385f4e3 100644
--- a/embedded/app/usbcan/can/ul_usb1.h
+++ b/embedded/app/usbcan/can/ul_usb1.h
@@ -10,16 +10,6 @@
  * Version lincan-0.3  17 Jun 2004
  */
 
-// int ul_usb1_request_io(struct candevice_t *candev);
-// int ul_usb1_release_io(struct candevice_t *candev);
-// int ul_usb1_reset(struct candevice_t *candev);
-// int ul_usb1_init_hw_data(struct candevice_t *candev);
-// int ul_usb1_init_chip_data(struct candevice_t *candev, int chipnr);
-// int ul_usb1_init_obj_data(struct canchip_t *chip, int objnr);
-// void ul_usb1_write_register(struct candevice_t *candev,unsigned data, unsigned long address);
-// unsigned ul_usb1_read_register(struct candevice_t *candev,unsigned long address);
-// int ul_usb1_program_irq(struct candevice_t *candev);
-
 struct ul_usb1_chip_data
 {
 	int flags;
@@ -31,4 +21,6 @@ struct ul_usb1_chip_data
 int ul_usb1_init(void);
 void ul_usb1_exit(void);
 
+extern int ul_usb1_register(struct hwspecops_t *hwspecops);
+
 #endif /* UL_USB1_H */
diff --git a/embedded/app/usbcan/can_quesysless.c b/embedded/app/usbcan/can_quesysless.c
index b10b07e..89dcabf 100644
--- a/embedded/app/usbcan/can_quesysless.c
+++ b/embedded/app/usbcan/can_quesysless.c
@@ -10,19 +10,19 @@
 #include "./can/can_sysdep.h"
 #include "./can/can_queue.h"
 
-//#define CAN_DEBUG
+#define CAN_DEBUG
 
 extern atomic_t edge_num_cnt;
 
 #ifdef CAN_DEBUG
-	#define DEBUGQUE(fmt,args...) can_printk(KERN_ERR "can_quekern (debug): " fmt,\
+	#define DEBUGQUE(fmt,args...) can_printk(KERN_ERR "can_quesysless (debug): " fmt,\
 	##args)
 
 #else
 	#define DEBUGQUE(fmt,args...)
 #endif
 
-#define ERRMSGQUE(fmt,args...) can_printk(KERN_ERR "can_quekern: " fmt,\
+#define ERRMSGQUE(fmt,args...) can_printk(KERN_ERR "can_quesysless: " fmt,\
 	##args)
 
 
diff --git a/embedded/app/usbcan/can_queue.c b/embedded/app/usbcan/can_queue.c
index 5a098bb..91077d7 100644
--- a/embedded/app/usbcan/can_queue.c
+++ b/embedded/app/usbcan/can_queue.c
@@ -20,8 +20,8 @@ extern int processlocal;
 
 atomic_t edge_num_cnt;
 
-//#define CAN_DEBUG
-#undef CAN_DEBUG
+#define CAN_DEBUG
+// #undef CAN_DEBUG
 
 #ifdef CAN_DEBUG
 	#define DEBUGQUE(fmt,args...) can_printk(KERN_ERR "can_queue (debug): " fmt,\
@@ -353,7 +353,7 @@ int canque_test_outslot(struct canque_ends_t *qends,
 	}
 	can_spin_unlock_irqrestore(&qends->ends_lock, flags);
 	*qedgep=NULL;
-	DEBUGQUE("canque_test_outslot no ready slot\n");
+// 	DEBUGQUE("canque_test_outslot no ready slot\n");
 	return -1;
 }
 
diff --git a/embedded/app/usbcan/main.c b/embedded/app/usbcan/main.c
index b9271e3..fd12f59 100644
--- a/embedded/app/usbcan/main.c
+++ b/embedded/app/usbcan/main.c
@@ -24,7 +24,7 @@
 #include "./can/sja1000p.h"
 #include "./can/main.h"
 
-#include "./can/can_sysdep.h"
+// #include "./can/can_sysdep.h"
 #include "./can/modparms.h"
 #include "./can/devcommon.h"
 
@@ -36,15 +36,14 @@
 #define MASK_EP1RX  0x01
 #define MASK_EP1TX  0x02
 
-#ifdef USB_MAX_PACKET
-	#undef USB_MAX_PACKET
-	#define USB_MAX_PACKET 16
-#endif
-
 #define CAN_OP_MASK 0x80
 #define CAN_OP_READ 0x80
 #define CAN_OP_WRITE 0x00
 
+#ifdef USB_MAX_PACKET
+	#undef USB_MAX_PACKET
+	#define USB_MAX_PACKET 16
+#endif
 /***********************************************************************
  * Note:
  * Comparing to LinCAN, there is no need to sleep for processes
@@ -82,6 +81,8 @@ unsigned char ep1_tx_buff[USB_MAX_PACKET];
 uint8_t timer_str,timer_rx_off,timer_tx_off,timer_configured;
 volatile uint8_t bootloader_run;
 
+uint8_t vendor_ret;
+
 int processlocal;
 
 int baudrate[MAX_TOT_CHIPS];
@@ -92,9 +93,12 @@ struct msgobj_t *objects_p[MAX_TOT_MSGOBJS];
 
 struct canuser_t *canuser;
 
+extern int init_chip_struct(struct candevice_t *candev, int chipnr, int irq, long baudrate);
+extern int register_chip_struct(struct canchip_t *chip, int minorbase);
+extern int register_obj_struct(struct msgobj_t *obj, int minorbase);
 
 /***********************************************************************
- * SOMETHING BAD HAPPENED
+ * IF SOMETHING BAD HAPPENED
  ***********************************************************************/
 
 int sys_err(){
@@ -150,7 +154,9 @@ int main(void)
 	can_spin_irqflags_t iflags;
 
 	int chipnr,bd;
-	int i,size;
+	int i,size,m=0;
+
+	CANMSG("Starting USBCAN module firmware...\n");
 
 //	volatile int i=0;
 	bootloader_run=0;
@@ -162,11 +168,18 @@ int main(void)
 	SET_OUT_PIN(LED_PORT,LED_ERR);
 	CLR_OUT_PIN(LED_PORT,LED_GP);
 
+	if (USB_MAX_PACKET<16){
+		CANMSG("Maximum packet size less than 16B (is %dB)\n",USB_MAX_PACKET);
+		sys_err();
+	}
+
 	/***********************************************************************
 	 * CAN device initialization - device side (adapted from LinCAN setup.c)
 	 ***********************************************************************/
 
+	can_init();
 
+	DEBUGMSG("Initiating CAN device initialization\n");
 	baudrate[0]=1000;
 
 	canqueue_kern_initialize();
@@ -174,8 +187,11 @@ int main(void)
 	hardware_p->nr_boards=1;
 
 	candev=(struct candevice_t *)malloc(sizeof(struct candevice_t));
-		if (candev==NULL) sys_err();
-		memset(candev, 0, sizeof(struct candevice_t));
+	if (!candev){
+		CANMSG("No space left in memory\n");
+		sys_err();
+	}
+	memset(candev, 0, sizeof(struct candevice_t));
 
 	hardware_p->candevice[0]=candev;
 	candev->candev_idx=0;
@@ -183,18 +199,38 @@ int main(void)
 	candev->dev_base_addr=0;
 
 	candev->hwspecops=(struct hwspecops_t *)malloc(sizeof(struct hwspecops_t));
-		if (candev->hwspecops==NULL) sys_err();
-		memset(candev->hwspecops, 0, sizeof(struct hwspecops_t));
+	if (!candev->hwspecops){
+		CANMSG("No space left in memory\n");
+		sys_err();
+	}
+	memset(candev->hwspecops, 0, sizeof(struct hwspecops_t));
 
 	ul_usb1_register(candev->hwspecops);
 
 	bd=baudrate[0];
-	if (candev->hwspecops->init_hw_data(candev)) sys_err();
+	if (candev->hwspecops->init_hw_data(candev)){
+		CANMSG("HW data could not be initialized\n");
+		sys_err();
+	}
 	/* Alocate and initialize the chip structures */
 	for (chipnr=0; chipnr < candev->nr_all_chips; chipnr++) {
 /*		if(chipnr<irqnum)
 			irqsig=irq[*irq_param_idx_p+chipnr];*/
-		if (init_chip_struct(candev, chipnr, 0, bd*1000)) sys_err();
+		if (init_chip_struct(candev, chipnr, 0, bd*1000)){
+			CANMSG("Chip structure could not be initialized\n");
+			sys_err();
+		}
+	}
+	for (chipnr=0; chipnr < candev->nr_all_chips; chipnr++) {
+		struct canchip_t *chip=candev->chip[chipnr];
+		int objnr;
+
+		register_chip_struct(chip, m);
+
+		for (objnr=0; objnr<chip->max_objects; objnr++) {
+			register_obj_struct(chip->msgobj[objnr], m);
+			if(m>=0) m++;
+		}
 	}
 	if (candev->hwspecops->request_io(candev))
 		sys_err();
@@ -295,12 +331,11 @@ int main(void)
 	eps[0].udev = &usb_device;
 	eps[1].udev = &usb_device;
 
-//  usb_device.vendor_fnc=usb_loader;
+  usb_device.vendor_fnc=usbcan_vendor;
 
 	usb_init(&usb_device);
 	usb_connect(&usb_device);
-
-	can_init();
+	usb_device.ep_events |= MASK_EP1TX;
 
 	/***********************************************************************
 	 * Start
@@ -312,13 +347,13 @@ int main(void)
 		usb_check_events(&usb_device);
 		usb_control_response(&usb_device);
 
-		if (!(IO0PIN&P0_SJA1000_INT_PIN))
+		if (!(IO0PIN&P0_SJA1000_INT_PIN)) //INT PIN is inverted
 			chip->chipspecops->irq_handler(0,chip);
 
 		if (usb_device.ep_events & MASK_EP1RX) {  //EP1RX - data waiting to receive
 			uint8_t val;
 			if (canque_get_inslot(qends, &qedge, &slot, 0)>=0){ //Free slot obtained
-				size=usb_udev_read_endpoint(&eps[0],ep1_rx_buff,USB_MAX_PACKET);
+				size=usb_udev_read_endpoint(&eps[0],ep1_rx_buff,16);
 				if (size==16){
 					uint16_t msgflags;
 					uint32_t msgid;
@@ -374,6 +409,7 @@ int main(void)
 
 		if(usb_device.ep_events & MASK_EP1TX){ //EP1TX - data transmitted
 			if(canque_test_outslot(qends, &qedge, &slot)>=0){
+				DEBUGMSG("CAN message ready to send over usb\n");
 				uint16_t msgflags;
 				uint32_t msgid;
 
@@ -395,7 +431,7 @@ int main(void)
 				for (;i<CAN_MSG_LENGTH;i++){
 					*(uint8_t *)(ep1_tx_buff+8+i)=0;
 				}
-				usb_udev_write_endpoint(&eps[1],ep1_tx_buff,USB_MAX_PACKET);
+				usb_udev_write_endpoint(&eps[1],ep1_tx_buff,16);
 
 				canque_free_outslot(qends, qedge, slot);
 				timer_tx_off=50; 		//rozsviceni diod pri vysilani
diff --git a/embedded/app/usbcan/ul_usb1.c b/embedded/app/usbcan/ul_usb1.c
index 84e1632..abb1626 100644
--- a/embedded/app/usbcan/ul_usb1.c
+++ b/embedded/app/usbcan/ul_usb1.c
@@ -18,7 +18,6 @@
  *
  */
 
-#include "./can/ul_usb1.h"
 #include "./can/can.h"
 #include "./can/can_sysdep.h"
 #include "./can/main.h"
@@ -31,6 +30,8 @@
 
 #include "./can/errno.h"
 
+#include "./can/ul_usb1.h"
+
 /* Get a minor range for your devices from the usb maintainer */
 #define USB_SKEL_MINOR_BASE	192
 
@@ -72,30 +73,6 @@
  */
 #define IO_RANGE 0x100
 
-/* Structure to hold all of our device specific stuff */
-// struct usb_ul_usb1 {
-// 	struct usb_device	*udev;			/* the usb device for this device */
-// 	struct usb_interface	*interface;		/* the interface for this device */
-// 	struct semaphore	limit_sem;		/* limiting the number of writes in progress */
-// 	struct usb_anchor	submitted;		/* in case we need to retract our submissions */
-// 	unsigned char           *bulk_in_buffer;	/* the buffer to receive data */
-// 	size_t			bulk_in_size;		/* the size of the receive buffer */
-// 	unsigned char           *int_in_buffer;	/* the buffer to receive data */
-// 	size_t			int_in_size;		/* the size of the receive buffer */
-// 	__u8			bulk_in_endpointAddr;	/* the address of the bulk in endpoint */
-// 	__u8			bulk_out_endpointAddr;	/* the address of the bulk out endpoint */
-// 	__u8			int_in_endpointAddr;	/* the address of the interrupt in endpoint */
-// 	int				int_in_interval;
-// 	int			errors;			/* the last request tanked */
-// 	int			open_count;		/* count the number of openers */
-// 	spinlock_t		err_lock;		/* lock for errors */
-// 	struct mutex		io_mutex;		/* synchronize I/O with disconnect */
-// 	struct urb		*irq;
-// 	struct candevice_t		*candev;
-// };
-//
-// static struct usb_driver ul_usb1_driver;
-
 /** ul_usb1_request_io
  * ul_usb1_request_io: - reserve io or memory range for can board
  * @candev: pointer to candevice/board which asks for io. Field @io_addr
@@ -300,63 +277,7 @@ int ul_usb1_program_irq(struct candevice_t *candev)
  */
 void ul_usb1_write_register(unsigned data, unsigned long address)
 {
-	IO1DIR|=0x00FF0000; // Port as output to send data
-	IO1CLR=0x00FF0000; // Clear all data on port
-	// Init
-	SET_OUT_PIN(IO0,P0_SJA1000_RD_PIN);	// Stays high on write
-	SET_OUT_PIN(IO0,P0_SJA1000_WR_PIN); // Stays high on address write
-	SET_OUT_PIN(IO0,P0_SJA1000_CS_PIN); // Sets output buffers to third state
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
-	//SET_OUT_PIN(IO0,P0_SJA1000_ALE_PIN); // Start command
-
-	// Set memory address
-	IO1SET=__val2mfld(0x00FF0000,address&0xFF); // Shift data to SJA pins and output them
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
-	//CLR_OUT_PIN(IO0,P0_SJA1000_ALE_PIN); // Makes address active
-	CLR_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
-
-	// Setting data
-	CLR_OUT_PIN(IO0,P0_SJA1000_WR_PIN);
-
-	IO1CLR=0x00FF0000;
-	IO1SET=__val2mfld(0x00FF0000,data&0xFF);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
-	SET_OUT_PIN(IO0,P0_SJA1000_WR_PIN); // Data should be accepted by now
-	SET_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
-
-// 	struct usb_ul_usb1 *dev;
-// 	int retval;
-// 	int bytes_transferred;
-// 	unsigned char buffer[2];
-// 	buffer[0]=((unsigned char)address & ~CAN_OP_MASK)+CAN_OP_WRITE;
-// 	buffer[1]=(unsigned char)data;
-//
-// 	dev = (struct usb_ul_usb1 *)candev->sysdevptr.anydev;
-
-// 	mutex_lock(&dev->io_mutex);
-// 	if (!dev) {		/* disconnect() was called */
-// 		CANMSG("Sending %lu:%X : ERR No device\n",address,(uint8_t)data);
-// 		retval = -ENODEV;
-// 		goto exit;
-// 	}
-// 	if (!dev->interface) {		/* disconnect() was called */
-// 		CANMSG("Sending %lu:%X : ERR No interface\n",address,(uint8_t)data);
-// 		retval = -ENODEV;
-// 		goto exit;
-// 	}
-
-	/* do a blocking bulk write to send data to the device */
-/*	retval = usb_bulk_msg(dev->udev,
-			      usb_sndbulkpipe(dev->udev, dev->bulk_out_endpointAddr),
-			      buffer,
-			      2,
-			      &bytes_transferred, 10000);
-	CANMSG("Sending %lu:%X : retval %d, transferred %d bytes\n",address,(uint8_t)data,retval,bytes_transferred);
-
-exit:
-	mutex_unlock(&dev->io_mutex);*/
+	can_write((uint8_t)(address & 0xFF), (uint8_t *)&data);
 }
 
 /** ul_usb1_read_register
@@ -371,84 +292,9 @@ exit:
  */
 unsigned ul_usb1_read_register(unsigned long address)
 {
-	unsigned data;
-	IO1DIR|=0x00FF0000; // Port as output to set address
-	IO1CLR=0x00FF0000; // Clear all data
-	// Init
-	SET_OUT_PIN(IO0,P0_SJA1000_WR_PIN); // Stays high on read
-	SET_OUT_PIN(IO0,P0_SJA1000_RD_PIN); // Stays high while entering address
-	SET_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
-	//SET_OUT_PIN(IO0,P0_SJA1000_ALE_PIN);
-
-	// Request memory address
-	IO1SET=__val2mfld(0x00FF0000,address&0xFF);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
-	//CLR_OUT_PIN(IO0,P0_SJA1000_ALE_PIN);
-	CLR_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
-
-	// Get data
-
-	IO1DIR&=~0x00FF0000; // Sets port as input
-	CLR_OUT_PIN(IO0,P0_SJA1000_RD_PIN);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
-	data=__mfld2val(0x00FF0000,IO1PIN);
-	SET_OUT_PIN(IO0,P0_SJA1000_RD_PIN);
-	SET_OUT_PIN(IO0,P0_SJA1000_CS_PIN);
-	for (slowdown=0;slowdown<SJA1000_SCLK;slowdown++);
+	uint8_t data;
+	can_read((uint8_t)(address & 0xFF), (uint8_t *)&data);
 	return data;
-
-// 	struct usb_ul_usb1 *dev;
-// 	int retval;
-// 	int bytes_transferred;
-// 	unsigned char buffer[2];
-// 	buffer[0]=((unsigned char)address & ~CAN_OP_MASK)+CAN_OP_READ;
-// 	buffer[1]=0x00;
-//
-// 	dev = (struct usb_ul_usb1 *)candev->sysdevptr.anydev;
-
-// 	mutex_lock(&dev->io_mutex);
-// 	if (!dev) {		/* disconnect() was called */
-// 		retval = -ENODEV;
-// 		goto exit;
-// 	}
-// 	if (!dev->interface) {		/* disconnect() was called */
-// 		retval = -ENODEV;
-// 		goto exit;
-// 	}
-
-	/* do a blocking bulk write to send data to the device */
-/*	retval = usb_bulk_msg(dev->udev,
-			      usb_sndbulkpipe(dev->udev, dev->bulk_out_endpointAddr),
-			      buffer,
-			      2,
-			      &bytes_transferred, 10000);
-
-	CANMSG("Requested: %ld : retval %d, transferred %d bytes\n",address,retval,bytes_transferred);
-	if ((retval)||(bytes_transferred!=2)){
-		retval = -EFAULT;
-		goto exit;
-	}
-*/
-	/* do a blocking bulk read to get data from the device */
-// 	retval = usb_bulk_msg(dev->udev,
-// 			      usb_rcvbulkpipe(dev->udev, dev->bulk_in_endpointAddr),
-// 			      dev->bulk_in_buffer,
-// 			      dev->bulk_in_size,
-// 			      &bytes_transferred, 10000);
-
-	/* if the read was successful, copy the data to userspace */
-/*	CANMSG("Received %d bytes : %u:%X\n",bytes_transferred,(dev->bulk_in_buffer[0] & 0x7F),dev->bulk_in_buffer[1]);
-	if (!retval) {
-		if (bytes_transferred!=2)
-			retval = -EFAULT;
-		else
-			retval = dev->bulk_in_buffer[1];
-	}
-
-exit:
-	mutex_unlock(&dev->io_mutex);
-	return retval;*/
 }
 
 /* !!! Don't change this function !!! */
diff --git a/embedded/app/usbcan/usb/usb_defs.h b/embedded/app/usbcan/usb/usb_defs.h
index 6eef8b0..17fed74 100644
--- a/embedded/app/usbcan/usb/usb_defs.h
+++ b/embedded/app/usbcan/usb/usb_defs.h
@@ -20,6 +20,10 @@
     #define CODE
   #endif
 
+	#ifdef USB_MAX_PACKET
+		#undef USB_MAX_PACKET
+		#define USB_MAX_PACKET 16
+	#endif
 /*****************************************************/
 /*** Static data structures for device descriptors ***/
 /*****************************************************/
diff --git a/embedded/app/usbcan/usb_vend.c b/embedded/app/usbcan/usb_vend.c
index 8ee992c..0fec9b3 100644
--- a/embedded/app/usbcan/usb_vend.c
+++ b/embedded/app/usbcan/usb_vend.c
@@ -1,19 +1,21 @@
+#define CAN_DEBUG
+
 #include <stdio.h>
 #include <system_def.h>
 #include <hal_intr.h>
-#include "./can/ul_usb1.h"
 #include "./can/can.h"
-#include "./can/can_sysdep.h"
+#include "./can/can_sysless.h"
 #include "./can/main.h"
 #include "./can/devcommon.h"
 #include "./usb/usb_vend.h"
-//#include "./can/ul_usb1.h"
+#include "./can/ul_usb1.h"
 #include <endian.h>
 #if __BYTE_ORDER == __BIG_ENDIAN
   #include <byteswap.h>
 #endif
 
 extern struct canuser_t *canuser;
+extern uint8_t vendor_ret;
 
 int set_ext_mask_complete_fnc(struct usb_ep_t *ep, int status){
   int dest_chip;
@@ -100,25 +102,24 @@ int usbcan_vendor(usb_device_t *udev)
   // wIndex, wValue, bRequest, wLength
   int dest_chip;
   struct ul_usb1_chip_data *chip_data;
-  uint8_t ret;
 
   dest_chip=(udev->request.wIndex);
   if ((dest_chip>=MAX_TOT_CHIPS)||(dest_chip<0))
-  	return 0; // Should look like ok (0) or stall (-1)?
+  	return -1; // Should look like ok (0) or stall (-1)?
   if (!chips_p[dest_chip])
-  	return 0; // Should look like ok (0) or stall (-1)?
+  	return -1; // Should look like ok (0) or stall (-1)?
 
   switch ( udev->request.bRequest) {
 		case USBCAN_VENDOR_EXT_MASK_SET:
 			udev->ep0.complete_fnc=set_ext_mask_complete_fnc;
 			return 1;
 		case USBCAN_VENDOR_EXT_MASK_STATUS:
-			ret=-1;
+			vendor_ret=-1;
 			if ((chip_data=((struct ul_usb1_chip_data*)(chips_p[dest_chip]->chip_data)))==NULL)
-				usb_send_control_data(udev,&ret,1);
+				usb_send_control_data(udev,&vendor_ret,1);
 			else{
-				ret=(chip_data->flags & UL_USB1_CHIP_MASK_SET)?1:0;
-				usb_send_control_data(udev,&ret,1);
+				vendor_ret=(chip_data->flags & UL_USB1_CHIP_MASK_SET)?1:0;
+				usb_send_control_data(udev,&vendor_ret,1);
 			}
 			chip_data->flags &= ~UL_USB1_CHIP_MASK_SET;
 			return 1;
@@ -127,12 +128,12 @@ int usbcan_vendor(usb_device_t *udev)
 			udev->ep0.complete_fnc=set_baud_rate_complete_fnc;
 			return 1;
 		case USBCAN_VENDOR_BAUD_RATE_STATUS:
-			ret=-1;
+			vendor_ret=-1;
 			if ((chip_data=((struct ul_usb1_chip_data*)(chips_p[dest_chip]->chip_data)))==NULL)
-				usb_send_control_data(udev,&ret,1);
+				usb_send_control_data(udev,&vendor_ret,1);
 			else{
-				ret=(chip_data->flags & UL_USB1_CHIP_BAUD_SET)?1:0;
-				usb_send_control_data(udev,&ret,1);
+				vendor_ret=(chip_data->flags & UL_USB1_CHIP_BAUD_SET)?1:0;
+				usb_send_control_data(udev,&vendor_ret,1);
 			}
 			chip_data->flags &= ~UL_USB1_CHIP_BAUD_SET;
 			return 1;
@@ -140,11 +141,10 @@ int usbcan_vendor(usb_device_t *udev)
 		case USBCAN_VENDOR_SET_BTREGS:
 			{
 				uint16_t value=udev->request.wValue;
-				ret=1;
+				vendor_ret=1;
 				if (chips_p[dest_chip]->chipspecops->set_btregs(chips_p[dest_chip],value&0xFF,(value>>8)&0xFF)<0)
-					ret=0;
-				usb_send_control_data(udev,&ret,1);
-				return 1;
+					vendor_ret=0;
+				usb_send_control_data(udev,&vendor_ret,1);
 			}
 			return 1;
 
@@ -152,26 +152,28 @@ int usbcan_vendor(usb_device_t *udev)
 			{
 				struct canque_edge_t *qedge;
 				struct canque_slot_t *slot;
-				ret=0;
+				vendor_ret=0;
 				if (canque_get_inslot(canuser->qends, &qedge, &slot, 0)>=0){
 					canque_abort_inslot(canuser->qends, qedge, slot);
-					ret=1;
+					DEBUGMSG("USBCAN_VENDOR_CHECK_TX_STAT - Free slot found\r\n");
+					vendor_ret=1;
 				}
-				usb_send_control_data(udev,&ret,1);
+				DEBUGMSG("USBCAN_VENDOR_CHECK_TX_STAT - Sending %d\r\n",vendor_ret);
+				usb_send_control_data(udev,&vendor_ret,1);
 				return 1;
 			}
 
 		case USBCAN_VENDOR_START_CHIP:
-			ret=1;
+			vendor_ret=1;
 			if (chips_p[dest_chip]->chipspecops->start_chip(chips_p[dest_chip])<0)
-				ret=0;
-			usb_send_control_data(udev,&ret,1);
+				vendor_ret=0;
+			usb_send_control_data(udev,&vendor_ret,1);
 			return 1;
 		case USBCAN_VENDOR_STOP_CHIP:
-			ret=1;
+			vendor_ret=1;
 			if (chips_p[dest_chip]->chipspecops->stop_chip(chips_p[dest_chip])<0)
-				ret=0;
-			usb_send_control_data(udev,&ret,1);
+				vendor_ret=0;
+			usb_send_control_data(udev,&vendor_ret,1);
 			return 1;
   }
 
diff --git a/embedded/libs4c/usb/base/usb/usb_spec.h b/embedded/libs4c/usb/base/usb/usb_spec.h
index 7d6f87b..841a2a4 100644
--- a/embedded/libs4c/usb/base/usb/usb_spec.h
+++ b/embedded/libs4c/usb/base/usb/usb_spec.h
@@ -29,7 +29,7 @@
     uint16_t wLength;
   } USB_DEVICE_REQUEST;
 
-/****************************************************************************************/ 
+/****************************************************************************************/
 /*** definitions for USB tranfer standard packets described in USB secif. - chapter 9 ***/
 /****************************************************************************************/
   #define DEVICE_ADDRESS_MASK   0x7F
@@ -102,7 +102,7 @@
     uint8_t  iSerialNumber;
     uint8_t  bNumConfigurations;
   } PACKED;
-  typedef struct _tag_usb_device_descriptor 
+  typedef struct _tag_usb_device_descriptor
     USB_DEVICE_DESCRIPTOR, *PUSB_DEVICE_DESCRIPTOR;
 
 /* *** DEVICE_QUALIFIER *** (spec. 9.6.2) */
@@ -203,7 +203,7 @@
   } PACKED;
   typedef struct _tag_usb_string_descriptor
     USB_STRING_DESCRIPTOR, *PUSB_STRING_DESCRIPTOR;
-  
+
 
 /*******************************************/
 /*** USB_IF - Defined USB device classes ***/
@@ -224,5 +224,5 @@
   /* define application specific device class subclasses */
   #define USB_APPL_SUBCLASS_FIRMWARE_UPDATE     0x01
   #define USB_APPL_SUBCLASS_IRDA_USB_BRIDGE     0x02
-                                                           
+
 #endif
diff --git a/lincan/include/can_sysdep.h b/lincan/include/can_sysdep.h
index 5a3b86c..5d57f5c 100644
--- a/lincan/include/can_sysdep.h
+++ b/lincan/include/can_sysdep.h
@@ -1,5 +1,5 @@
-/* can_sysdep.h - hides differences between individual Linux kernel 
- *                versions and RT extensions 
+/* can_sysdep.h - hides differences between individual Linux kernel
+ *                versions and RT extensions
  * Linux CAN-bus device driver.
  * Written by Pavel Pisa - OCERA team member
  * email:pisa@cmp.felk.cvut.cz
@@ -205,7 +205,7 @@
 
 /*
  * terrible hack to test rtl_file private_data concept, ugh !!!
- * this would result in crash on architectures,  where 
+ * this would result in crash on architectures,  where
  * sizeof(int) < sizeof(void *)
  */
 #define can_set_rtl_file_private_data(fptr, p) do{ fptr->f_minor=(long)(p); } while(0)
@@ -223,4 +223,20 @@ extern can_spinlock_t can_irq_manipulation_lock;
 
 #endif /*CAN_WITH_RTL*/
 
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,4))
+	#include <linux/kthread.h>
+	#define	can_kthread_create	kthread_create
+	#define can_kthread_run	kthread_run
+	#define can_kthread_bind	kthread_bind
+	#define can_kthread_stop	kthread_stop
+	#define can_kthread_should_stop	kthread_should_stop
+#else
+	#define	can_kthread_create
+	#define can_kthread_run
+	#define can_kthread_bind
+	#define can_kthread_stop
+	#define can_kthread_should_stop
+#endif
+
+
 #endif /*_CAN_SYSDEP_H*/
diff --git a/lincan/include/usbcan.h b/lincan/include/usbcan.h
index 0f1e39c..b01a627 100644
--- a/lincan/include/usbcan.h
+++ b/lincan/include/usbcan.h
@@ -66,7 +66,7 @@ int usbcan_irq_handler(int irq, struct canchip_t *chip);
 int usbcan_init(void);
 void usbcan_exit(void);
 
-void usbcan_read_kthread(kthread_t *kthread);
+int usbcan_read_kthread(void *data);
 int usbcan_chip_queue_status(struct canchip_t *chip);
 
 #ifdef CONFIG_OC_LINCAN_DETAILED_ERRORS
@@ -115,6 +115,23 @@ int usbcan_chip_queue_status(struct canchip_t *chip);
 
 #endif /*CONFIG_OC_LINCAN_DETAILED_ERRORS*/
 
+struct usbcan_message {
+	struct urb	*u;
+	u8	msg[16];
+	spinlock_t		acc;		/* access lock */
+	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
+
 /* Structure to hold all of our device specific stuff */
 struct usbcan_usb {
 	struct usb_device	*udev;			/* the usb device for this device */
@@ -133,10 +150,13 @@ struct usbcan_usb {
 	int			open_count;		/* count the number of openers */
 	spinlock_t		err_lock;		/* lock for errors */
 	struct mutex		io_mutex;		/* synchronize I/O with disconnect */
-	struct urb		*rcv;
-	u8	rcv_msg[16];
+	struct urb		*rx;
+	struct urb		*tx;
+	u8	rx_msg[16];
 	u8	tx_msg[16];
-	kthread_t  rcvthread;                      /* usb receive kernel thread  */
+
+	struct task_struct *comthread;                      /* usb communication kernel thread  */
+
 
 	struct candevice_t *candev;
 	long flags;
diff --git a/lincan/src/main.c b/lincan/src/main.c
index 0e9f2a5..f69009b 100644
--- a/lincan/src/main.c
+++ b/lincan/src/main.c
@@ -591,29 +591,15 @@ void cleanup_usbdev(struct candevice_t *dev)
 	}
 #endif
 
-	for(i=0;i<MAX_TOT_MSGOBJS;i++) {
-		if(!objects_p[i]) continue;
-		if(objects_p[i]->hostchip->hostdevice != dev) continue;
-		//canqueue_ends_done_chip(objects_p[i]->qends);
-		//can_checked_free(objects_p[i]->qends);
-		//can_checked_free(objects_p[i]);
-		objects_p[i]=NULL;
-	}
-
 	for(i=0;i<MAX_TOT_CHIPS;i++){
 		if(!chips_p[i]) continue;
 		if(chips_p[i]->hostdevice != dev) continue;
-		//can_checked_free(chips_p[i]->chipspecops);
-		//can_checked_free(chips_p[i]);
 		chips_p[i]=NULL;
 	}
 
 	hardware_p->candevice[dev->candev_idx]=NULL;
 	hardware_p->nr_boards--;
-	//kfree(hw[dev->candev_idx]);
 	hw[dev->candev_idx]=NULL;
-	//can_checked_free(dev->hwspecops);
-	//can_checked_free(dev);
 
 	candevice_done(dev);
 	can_checked_free(dev);
diff --git a/lincan/src/usbcan.c b/lincan/src/usbcan.c
index 91807f7..b356373 100644
--- a/lincan/src/usbcan.c
+++ b/lincan/src/usbcan.c
@@ -54,8 +54,7 @@ int usbcan_request_io(struct candevice_t *candev)
 	}
 
 	/* start kernel thread */
-	dev->rcvthread.arg = dev;
-	start_kthread(usbcan_read_kthread, &dev->rcvthread);
+	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;
@@ -77,13 +76,12 @@ 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 */
-	if (dev->rcv){
-		usb_kill_urb(dev->rcv);
-		usb_free_urb(dev->rcv);
-	}
-	stop_kthread(&dev->rcvthread);
+	can_kthread_stop(dev->comthread);
+
 	return 0;
 }
 
@@ -277,6 +275,9 @@ int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned  l
 
 	u8 usbbuf[16];
 
+	if (!dev)
+		return -ENODEV;
+
 	*(uint32_t *)(usbbuf)=cpu_to_le32(mask);
 	*(uint32_t *)(usbbuf+4)=cpu_to_le32(code);
 
@@ -284,7 +285,7 @@ int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned  l
 		usb_sndctrlpipe(dev->udev, dev->ctl_out_endpointAddr),
 		USBCAN_VENDOR_EXT_MASK_SET,
 		USB_TYPE_VENDOR,
-		0, chip->chip_idx,
+		cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
 		&usbbuf, 16,
 		10000);
 	if (retval<0)
@@ -294,7 +295,7 @@ int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned  l
 		usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
 		USBCAN_VENDOR_EXT_MASK_STATUS,
 		USB_TYPE_VENDOR,
-		0, chip->chip_idx,
+		cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
 		&usbbuf, 16,
 		10000);
 
@@ -330,6 +331,9 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw,
 
 	u8 usbbuf[16];
 
+	if (!dev)
+		return -ENODEV;
+
 	*(int32_t *)(usbbuf)=cpu_to_le32(rate);
 	*(int32_t *)(usbbuf+4)=cpu_to_le32(sjw);
 	*(int32_t *)(usbbuf+8)=cpu_to_le32(sampl_pt);
@@ -339,7 +343,7 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw,
 		usb_sndctrlpipe(dev->udev, dev->ctl_out_endpointAddr),
 		USBCAN_VENDOR_BAUD_RATE_SET,
 		USB_TYPE_VENDOR,
-		0, chip->chip_idx,
+		cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
 		&usbbuf, 16,
 		10000);
 	if (retval<0)
@@ -349,7 +353,7 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw,
 		usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
 		USBCAN_VENDOR_BAUD_RATE_STATUS,
 		USB_TYPE_VENDOR,
-		0, chip->chip_idx,
+		cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
 		usbbuf, 16,
 		10000);
 
@@ -399,6 +403,8 @@ int usbcan_pre_write_config(struct canchip_t *chip, struct msgobj_t *obj,
 	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) {
@@ -444,6 +450,9 @@ int usbcan_send_msg(struct canchip_t *chip, struct msgobj_t *obj,
 	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),
@@ -458,7 +467,23 @@ int usbcan_send_msg(struct canchip_t *chip, struct msgobj_t *obj,
 		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;
 }
 
@@ -473,7 +498,10 @@ int usbcan_send_msg(struct canchip_t *chip, struct msgobj_t *obj,
  */
 int usbcan_check_tx_stat(struct canchip_t *chip)
 {
-	if (test_bit(USBCAN_TX_PENDING,&((struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev)->flags))
+	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
+	if (!dev)
+		return 0;
+	if (test_bit(USBCAN_TX_PENDING,&dev->flags))
 		return 1;
 	return 0;
 }
@@ -494,11 +522,14 @@ int usbcan_set_btregs(struct canchip_t *chip, unsigned short btr0,
 	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
 	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),
 	USBCAN_VENDOR_SET_BTREGS,
 	USB_TYPE_VENDOR,
-	cpu_to_le16(value), chip->chip_idx,
+	cpu_to_le16(value), cpu_to_le16(chip->chip_idx),
 	dev->ctl_in_buffer, dev->ctl_in_size,
 	10000);
 
@@ -521,11 +552,14 @@ int usbcan_start_chip(struct canchip_t *chip)
 	int retval;
 	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
 
+	if (!dev)
+		return -ENODEV;
+
 	retval = usb_control_msg(dev->udev,
 	usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
 	USBCAN_VENDOR_START_CHIP,
 	USB_TYPE_VENDOR,
-	0, chip->chip_idx,
+	cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
 	dev->ctl_in_buffer, dev->ctl_in_size,
 	10000);
 
@@ -547,23 +581,29 @@ int usbcan_start_chip(struct canchip_t *chip)
  */
 int usbcan_chip_queue_status(struct canchip_t *chip)
 {
-	int retval;
+	int retval,i;
 	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
 
+	if (!dev)
+		return -ENODEV;
 	retval = usb_control_msg(dev->udev,
 	usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
 	USBCAN_VENDOR_CHECK_TX_STAT,
 	USB_TYPE_VENDOR,
-	0, chip->chip_idx,
+	cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
 	dev->ctl_in_buffer, dev->ctl_in_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)
 			return 0;
 		if(dev->ctl_in_buffer[0]==0)
 			return 1;
 	}
+	CANMSG("Chip_queue_status error: %d\n",retval);
 	return -ENODEV;
 }
 
@@ -579,11 +619,14 @@ int usbcan_stop_chip(struct canchip_t *chip)
 	int retval;
 	struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev;
 
+	if (!dev)
+		return -ENODEV;
+
 	retval = usb_control_msg(dev->udev,
 	usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr),
 	USBCAN_VENDOR_STOP_CHIP,
 	USB_TYPE_VENDOR,
-	0, chip->chip_idx,
+	cpu_to_le16(0), cpu_to_le16(chip->chip_idx),
 	dev->ctl_in_buffer, dev->ctl_in_size,
 	10000);
 
@@ -848,7 +891,7 @@ int usbcan_irq_handler(int irq, struct canchip_t *chip)
  */
 int usbcan_wakeup_tx(struct canchip_t *chip, struct msgobj_t *obj)
 {
-
+	CANMSG("Trying to send message\n");
 	can_preempt_disable();
 
 	can_msgobj_set_fl(obj,TX_PENDING);
@@ -962,6 +1005,30 @@ int usbcan_init_chip_data(struct candevice_t *candev, int chipnr)
 
 
 /* --------------------------------------------------------------------------------------------------- */
+static int usbcan_sleep_thread(struct usbcan_usb *dev)
+{
+	int	rc = 0;
+
+	/* Wait until a signal arrives or we are woken up */
+	for (;;) {
+		try_to_freeze();
+		set_current_state(TASK_INTERRUPTIBLE);
+		if (signal_pending(current)) {
+			rc = -EINTR;
+			break;
+		}
+		if (
+			can_kthread_should_stop() ||
+			test_bit(USBCAN_DATA_READ,&dev->flags) ||
+			test_bit(USBCAN_TERMINATE,&dev->flags) ||
+			test_bit(USBCAN_ERROR,&dev->flags)
+		)
+			break;
+		schedule();
+	}
+	__set_current_state(TASK_RUNNING);
+	return rc;
+}
 
 static void usbcan_rcv(struct urb *urb)
 {
@@ -972,7 +1039,8 @@ static void usbcan_rcv(struct urb *urb)
 	case 0:
 		/* success */
 		set_bit(USBCAN_DATA_READ,&dev->flags);
-		wake_up(&dev->rcvthread.queue);
+		CANMSG("Message received\n");
+		wake_up_process(dev->comthread);
 		return;
 	case -ECONNRESET:
 	case -ENOENT:
@@ -980,10 +1048,10 @@ static void usbcan_rcv(struct urb *urb)
 		/* 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(&dev->rcvthread.queue);
+		wake_up_process(dev->comthread);
 		return;
 	default:
-		CANMSG("%s - nonzero urb status received: %d\n", __FUNCTION__, urb->status);
+// 		CANMSG("%s - nonzero urb status received: %d\n", __FUNCTION__, urb->status);
 		break;
 	}
 
@@ -992,54 +1060,50 @@ static void usbcan_rcv(struct urb *urb)
 		CANMSG("%s - usb_submit_urb failed with result %d\n",
 		     __FUNCTION__, retval);
 		set_bit(USBCAN_ERROR,&dev->flags);
-		wake_up(&dev->rcvthread.queue);
+		wake_up_process(dev->comthread);
 	}
 }
 
-void usbcan_read_kthread(kthread_t *kthread)
+int usbcan_read_kthread(void *data)
 {
 	int retval;
-	struct usbcan_usb *dev=(struct usbcan_usb *)kthread->arg;
+ 	struct usbcan_usb *dev=(struct usbcan_usb *)data;
 	struct msgobj_t *obj;
 
-  /* setup the thread environment */
-  init_kthread(kthread, "usbcan");
-
   /* this is normal work to do */
   CANMSG ("usbcan thread started!\n");
 
-	dev->rcv = usb_alloc_urb(0, GFP_KERNEL);
-	if (!dev->rcv){
+	dev->rx = usb_alloc_urb(0, GFP_KERNEL);
+	if (!dev->rx){
 		CANMSG("Error allocating usb urb\n");
 		goto error;
 	}
-	dev->rcv->dev = dev->udev;
-	usb_fill_bulk_urb(dev->rcv, dev->udev,
+	dev->rx->dev = dev->udev;
+	usb_fill_bulk_urb(dev->rx, dev->udev,
 			 usb_rcvbulkpipe(dev->udev, dev->bulk_in_endpointAddr),
-			 &dev->rcv_msg, 16,
+			 &dev->rx_msg, 16,
 			 usbcan_rcv, dev);
 
   /* an endless loop in which we are doing our work */
   for(;;)
   {
-		retval=usb_submit_urb(dev->rcv, GFP_KERNEL);
-		if (retval){
-			CANMSG("URB error %d\n",retval);
-			break;
-		}
-		/* fall asleep */
-		wait_event_interruptible(kthread->queue,
-			test_bit(USBCAN_DATA_READ,&dev->flags)
-			|| test_bit(USBCAN_TERMINATE,&dev->flags)
-			|| test_bit(USBCAN_ERROR,&dev->flags)
-		);
+		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();
 
 		/* here we are back from sleep because we caught a signal. */
-		if (kthread->terminate)
+		if (can_kthread_should_stop())
 		{
 			/* we received a request to terminate ourself */
 			break;
@@ -1054,22 +1118,23 @@ void usbcan_read_kthread(kthread_t *kthread)
 			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->rcv_msg[0]])&&
-					(dev->candev->chip[dev->rcv_msg[0]]->flags & CHIP_CONFIGURED)
+				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->rcv_msg[0]]->msgobj[0];
+					obj=dev->candev->chip[dev->rx_msg[0]]->msgobj[0];
 
-					len=*(uint8_t *)(dev->rcv_msg+1);
+					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->rcv_msg+2));
-					obj->rx_msg.id=le32_to_cpu((*(uint32_t *)(dev->rcv_msg+4)));
+					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->rcv_msg+8,i=0; i < len; ptr++,i++) {
+					for(ptr=dev->rx_msg+8,i=0; i < len; ptr++,i++) {
 						obj->rx_msg.data[i]=*ptr;
 					}
 
@@ -1077,14 +1142,22 @@ void usbcan_read_kthread(kthread_t *kthread)
 					can_filltimestamp(&obj->rx_msg.timestamp);
 					canque_filter_msg2edges(obj->qends, &obj->rx_msg);
 				}
+				else
+					CANMSG("Destination chip not found\n");
 			}
     }
   }
   /* here we go only in case of termination of the thread */
+	if (dev->rx){
+		usb_kill_urb(dev->rx);
+		usb_free_urb(dev->rx);
+	}
+  return 0;
 error:
   /* cleanup the thread, leave */
   CANMSG ("kernel thread terminated!\n");
-  exit_kthread(kthread);
+  return -ENOMEM;
+//   exit_kthread(kthread);
 
   /* returning from the thread here calls the exit functions */
 }
@@ -1107,6 +1180,7 @@ static int usbcan_probe(struct usb_interface *interface, const struct usb_device
 	memset(dev, 0, sizeof(struct usbcan_usb));
 
 	sema_init(&dev->limit_sem, WRITES_IN_FLIGHT);
+	mutex_init(&dev->io_mutex);
 	spin_lock_init(&dev->err_lock);
 	init_usb_anchor(&dev->submitted);
 
@@ -1139,31 +1213,15 @@ static int usbcan_probe(struct usb_interface *interface, const struct usb_device
 				dev->bulk_out_endpointAddr = endpoint->bEndpointAddress;
 		}
 
-		if (!dev->ctl_in_endpointAddr &&
-		    usb_endpoint_xfer_control(endpoint) &&
-		    usb_endpoint_dir_in(endpoint)) {
-			/* we found a bulk in endpoint */
-			buffer_size = le16_to_cpu(endpoint->wMaxPacketSize);
-			dev->ctl_in_size = buffer_size;
-			dev->ctl_in_endpointAddr = endpoint->bEndpointAddress;
-			dev->ctl_in_buffer = can_checked_malloc(buffer_size);
-			if (!dev->ctl_in_buffer) {
-				err("Could not allocate bulk_in_buffer");
-				goto error;
-			}
-		}
-
-		if (!dev->ctl_out_endpointAddr &&
-		    usb_endpoint_xfer_control(endpoint) &&
-		    usb_endpoint_dir_out(endpoint)) {
-			/* we found a bulk out endpoint */
-				dev->ctl_out_endpointAddr = endpoint->bEndpointAddress;
-		}
 	}
 	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;
 
 	/* save our data pointer in this interface device */
 	usb_set_intfdata(interface, dev);
@@ -1207,14 +1265,17 @@ static void usbcan_disconnect(struct usb_interface *interface)
 	//usb_kill_anchored_urbs(&dev->submitted);
 
 	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 (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);
 
 	info("USB Skeleton now disconnected");