X-Git-Url: http://rtime.felk.cvut.cz/gitweb/lincan.git/blobdiff_plain/3129fcd8094edcb910b708463fdf1a234f11fb55..c759ac4ffabb78a8863d9bced695980077eeb319:/lincan/src/can_queue.c

diff --git a/lincan/src/can_queue.c b/lincan/src/can_queue.c
index 62d8e19..d90b9c5 100644
--- a/lincan/src/can_queue.c
+++ b/lincan/src/can_queue.c
@@ -3,32 +3,37 @@
  * New CAN queues by Pavel Pisa - OCERA team member
  * email:pisa@cmp.felk.cvut.cz
  * This software is released under the GPL-License.
- * Version lincan-0.2  9 Jul 2003
+ * Version lincan-0.3  17 Jun 2004
  */
 
-#define __NO_VERSION__
-#include <linux/module.h>
-#include <linux/version.h>
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-#include <linux/malloc.h>
-#else
-#include <linux/slab.h>
-#endif
-#include <linux/wait.h>
 #include "../include/can.h"
+#include "../include/can_sysdep.h"
 #include "../include/can_queue.h"
 
+/* 
+ * Modifies Tx message processing 
+ *  0 .. local message processing disabled
+ *  1 .. local messages disabled by default but can be enabled by canque_set_filt
+ *  2 .. local messages enabled by default, can be disabled by canque_set_filt
+ */
+extern int processlocal;
+
+atomic_t edge_num_cnt;
+
+//#define CAN_DEBUG
+#undef CAN_DEBUG
+
 #ifdef CAN_DEBUG
-	#define DEBUGQUE(fmt,args...) printk(KERN_ERR "can_queue (debug): " fmt,\
+	#define DEBUGQUE(fmt,args...) can_printk(KERN_ERR "can_queue (debug): " fmt,\
 	##args)
-	
-  atomic_t edge_num_cnt;
+
 #else
 	#define DEBUGQUE(fmt,args...)
 #endif
 
 #define CANQUE_ROUNDROB 1
 
+
 /**
  * canque_fifo_flush_slots - free all ready slots from the FIFO
  * @fifo: pointer to the FIFO structure
@@ -43,36 +48,35 @@
 int canque_fifo_flush_slots(struct canque_fifo_t *fifo)
 {
 	int ret;
-	unsigned long flags;
+	can_spin_irqflags_t flags;
 	struct canque_slot_t *slot;
-	spin_lock_irqsave(&fifo->fifo_lock, flags);
+	can_spin_lock_irqsave(&fifo->fifo_lock, flags);
 	slot=fifo->head;
-	*fifo->tail=fifo->flist;
-	fifo->flist=slot;
-	fifo->head=NULL;
-	fifo->tail=&fifo->head;
-	ret=canque_fifo_test_and_set_fl(fifo,EMPTY);
-	spin_unlock_irqrestore(&fifo->fifo_lock, flags);
+	if(slot){
+		*fifo->tail=fifo->flist;
+		fifo->flist=slot;
+		fifo->head=NULL;
+		fifo->tail=&fifo->head;
+	}
+	canque_fifo_clear_fl(fifo,FULL);
+	ret=canque_fifo_test_and_set_fl(fifo,EMPTY)?0:1;
+	can_spin_unlock_irqrestore(&fifo->fifo_lock, flags);
 	return ret;
 }
 
 
 /**
- * canque_fifo_init_slots - initialize one CAN FIFO
+ * canque_fifo_init_slots - initializes slot chain of one CAN FIFO
  * @fifo: pointer to the FIFO structure
- * @slotsnr: number of requested slots
  *
  * Return Value: The negative value indicates, that there is no memory
  *	to allocate space for the requested number of the slots.
  */
-int canque_fifo_init_slots(struct canque_fifo_t *fifo, int slotsnr)
+int canque_fifo_init_slots(struct canque_fifo_t *fifo)
 {
-	int size;
 	struct canque_slot_t *slot;
-	if(!slotsnr) slotsnr=MAX_BUF_LENGTH;
-	size=sizeof(struct canque_slot_t)*slotsnr;
-	fifo->entry=kmalloc(size,GFP_KERNEL);
-	if(!fifo->entry) return -1;
+	int slotsnr=fifo->slotsnr;
+	if(!fifo->entry || !slotsnr) return -1;
 	slot=fifo->entry;
 	fifo->flist=slot;
 	while(--slotsnr){
@@ -86,18 +90,6 @@ int canque_fifo_init_slots(struct canque_fifo_t *fifo, int slotsnr)
 	return 1;
 }
 
-/**
- * canque_fifo_done - frees slots allocated for CAN FIFO
- * @fifo: pointer to the FIFO structure
- */
-int canque_fifo_done(struct canque_fifo_t *fifo)
-{
-	if(fifo->entry)
-		kfree(fifo->entry);
-	fifo->entry=NULL;
-	return 1;
-}
-
 /* atomic_dec_and_test(&qedge->edge_used);
  void atomic_inc(&qedge->edge_used);
  list_add_tail(struct list_head *new, struct list_head *head)
@@ -105,6 +97,11 @@ int canque_fifo_done(struct canque_fifo_t *fifo)
  list_entry(ptr, type, member)
 */
 
+void __canque_edge_decref(struct canque_edge_t *edge)
+{
+	__canque_edge_decref_body(edge);
+}
+
 /**
  * canque_get_inslot - finds one outgoing edge and allocates slot from it
  * @qends: ends structure belonging to calling communication object
@@ -121,15 +118,11 @@ int canque_get_inslot(struct canque_ends_t *qends,
 	struct canque_edge_t **qedgep, struct canque_slot_t **slotp, int cmd)
 {
 	int ret=-2;
-	unsigned long flags;
 	struct canque_edge_t *edge;
 	
-	spin_lock_irqsave(&qends->ends_lock, flags);
-	if(!list_empty(&qends->inlist)){
-		edge=list_entry(qends->inlist.next,struct canque_edge_t,inpeers);
-		if(!canque_fifo_test_fl(&edge->fifo,BLOCK)&&!canque_fifo_test_fl(&edge->fifo,DEAD)){
-			atomic_inc(&edge->edge_used);
-			spin_unlock_irqrestore(&qends->ends_lock, flags);
+	edge=canque_first_inedge(qends);
+	if(edge){
+		if(!canque_fifo_test_fl(&edge->fifo,BLOCK)){
 			ret=canque_fifo_get_inslot(&edge->fifo, slotp, cmd);
 			if(ret>0){
 				*qedgep=edge;
@@ -137,12 +130,9 @@ int canque_get_inslot(struct canque_ends_t *qends,
 				return ret;
 
 			}
-			spin_lock_irqsave(&qends->ends_lock, flags);
-			if(atomic_dec_and_test(&edge->edge_used))
-			  	canque_notify_bothends(edge,CANQUEUE_NOTIFY_NOUSR);
 		}
+		canque_edge_decref(edge);
 	}
-	spin_unlock_irqrestore(&qends->ends_lock, flags);
 	*qedgep=NULL;
 	DEBUGQUE("canque_get_inslot cmd=%d failed\n",cmd);
 	return ret;
@@ -170,14 +160,10 @@ int canque_get_inslot4id(struct canque_ends_t *qends,
 	int cmd, unsigned long id, int prio)
 {
 	int ret=-2;
-	unsigned long flags;
 	struct canque_edge_t *edge, *bestedge=NULL;
-	struct list_head *entry;
 	
-	spin_lock_irqsave(&qends->ends_lock, flags);
-	list_for_each(entry,&qends->inlist){
-		edge=list_entry(entry,struct canque_edge_t,inpeers);
-		if(canque_fifo_test_fl(&edge->fifo,BLOCK)||canque_fifo_test_fl(&edge->fifo,DEAD))
+	canque_for_each_inedge(qends, edge){
+		if(canque_fifo_test_fl(&edge->fifo,BLOCK))
 			continue;
 		if((id^edge->filtid)&edge->filtmask)
 			continue;
@@ -186,7 +172,9 @@ int canque_get_inslot4id(struct canque_ends_t *qends,
 				if (!edge->filtmask) continue;
 			} else {
 				if(edge->filtmask){
+					canque_edge_decref(bestedge);
 					bestedge=edge;
+					canque_edge_incref(bestedge);
 					continue;
 				}
 			}
@@ -195,23 +183,20 @@ int canque_get_inslot4id(struct canque_ends_t *qends,
 			} else {
 				if(bestedge->edge_prio<=prio) continue;
 			}
+			canque_edge_decref(bestedge);
 		}
 		bestedge=edge;
+		canque_edge_incref(bestedge);
 	}
 	if((edge=bestedge)!=NULL){
-		atomic_inc(&edge->edge_used);
-		spin_unlock_irqrestore(&qends->ends_lock, flags);
 		ret=canque_fifo_get_inslot(&edge->fifo, slotp, cmd);
 		if(ret>0){
 			*qedgep=edge;
 			DEBUGQUE("canque_get_inslot4id cmd=%d id=%ld prio=%d found edge %d\n",cmd,id,prio,edge->edge_num);
 			return ret;
 		}
-		spin_lock_irqsave(&qends->ends_lock, flags);
-		if(atomic_dec_and_test(&edge->edge_used))
-			canque_notify_bothends(edge,CANQUEUE_NOTIFY_NOUSR);
+		canque_edge_decref(bestedge);
 	}
-	spin_unlock_irqrestore(&qends->ends_lock, flags);
 	*qedgep=NULL;
 	DEBUGQUE("canque_get_inslot4id cmd=%d id=%ld prio=%d failed\n",cmd,id,prio);
 	return ret;
@@ -233,16 +218,12 @@ int canque_put_inslot(struct canque_ends_t *qends,
 	struct canque_edge_t *qedge, struct canque_slot_t *slot)
 {
 	int ret;
-	unsigned long flags;
 	ret=canque_fifo_put_inslot(&qedge->fifo,slot);
 	if(ret) {
 		canque_activate_edge(qends,qedge);
 		canque_notify_outends(qedge,CANQUEUE_NOTIFY_PROC);
 	}
-	spin_lock_irqsave(&qends->ends_lock, flags);
-	if(atomic_dec_and_test(&qedge->edge_used))
-  		canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
-	spin_unlock_irqrestore(&qends->ends_lock, flags);
+	canque_edge_decref(qedge);
 	DEBUGQUE("canque_put_inslot for edge %d returned %d\n",qedge->edge_num,ret);
 	return ret;
 }
@@ -261,15 +242,11 @@ int canque_abort_inslot(struct canque_ends_t *qends,
 	struct canque_edge_t *qedge, struct canque_slot_t *slot)
 {
 	int ret;
-	unsigned long flags;
 	ret=canque_fifo_abort_inslot(&qedge->fifo,slot);
 	if(ret) {
 		canque_notify_outends(qedge,CANQUEUE_NOTIFY_SPACE);
 	}
-	spin_lock_irqsave(&qends->ends_lock, flags);
-	if(atomic_dec_and_test(&qedge->edge_used))
-  		canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
-	spin_unlock_irqrestore(&qends->ends_lock, flags);
+	canque_edge_decref(qedge);
 	DEBUGQUE("canque_abort_inslot for edge %d returned %d\n",qedge->edge_num,ret);
 	return ret;
 }
@@ -287,22 +264,19 @@ int canque_filter_msg2edges(struct canque_ends_t *qends, struct canmsg_t *msg)
 {
 	int destnr=0;
 	int ret;
-	unsigned long flags;
+	unsigned long msgid;
 	struct canque_edge_t *edge;
-	struct list_head *entry;
 	struct canque_slot_t *slot;
 	
-	DEBUGQUE("canque_filter_msg2edges for msg ID %ld\n",msg->id);
-	spin_lock_irqsave(&qends->ends_lock, flags);
-	list_for_each(entry,&qends->inlist){
-		edge=list_entry(entry,struct canque_edge_t,inpeers);
-		if(canque_fifo_test_fl(&edge->fifo,BLOCK)||canque_fifo_test_fl(&edge->fifo,DEAD))
+	DEBUGQUE("canque_filter_msg2edges for msg ID 0x%08lx and flags 0x%02x\n",
+			msg->id, msg->flags);
+	msgid = canque_filtid2internal(msg->id, msg->flags);
+
+	canque_for_each_inedge(qends, edge) {
+		if(canque_fifo_test_fl(&edge->fifo,BLOCK))
 			continue;
-		/* FIXME: the next comparison should be outside of ends lock */
-		if((msg->id^edge->filtid)&edge->filtmask)
+		if((msgid^edge->filtid)&edge->filtmask)
 			continue;
-		atomic_inc(&edge->edge_used);
-		spin_unlock_irqrestore(&qends->ends_lock, flags);
 		ret=canque_fifo_get_inslot(&edge->fifo, &slot, 0);
 		if(ret>0){
 			slot->msg=*msg;
@@ -314,11 +288,7 @@ int canque_filter_msg2edges(struct canque_ends_t *qends, struct canmsg_t *msg)
 			}
 
 		}
-		spin_lock_irqsave(&qends->ends_lock, flags);
-		if(atomic_dec_and_test(&edge->edge_used))
-			canque_notify_bothends(edge,CANQUEUE_NOTIFY_NOUSR);
 	}
-	spin_unlock_irqrestore(&qends->ends_lock, flags);
 	DEBUGQUE("canque_filter_msg2edges sent msg ID %ld to %d edges\n",msg->id,destnr);
 	return destnr;
 }
@@ -338,22 +308,50 @@ int canque_filter_msg2edges(struct canque_ends_t *qends, struct canmsg_t *msg)
 int canque_test_outslot(struct canque_ends_t *qends,
 	struct canque_edge_t **qedgep, struct canque_slot_t **slotp)
 {
-	unsigned long flags;
+	can_spin_irqflags_t flags;
 	int prio;
 	struct canque_edge_t *edge;
+	int ret;
 	
-	spin_lock_irqsave(&qends->ends_lock, flags);
+	can_spin_lock_irqsave(&qends->ends_lock, flags);
 	for(prio=CANQUEUE_PRIO_NR;--prio>=0;){
-		if(!list_empty(&qends->active[prio])){
-			edge=list_entry(qends->active[prio].next,struct canque_edge_t,outpeers);
-			atomic_inc(&edge->edge_used);
-			spin_unlock_irqrestore(&qends->ends_lock, flags);
-			*qedgep=edge;
-			DEBUGQUE("canque_test_outslot found edge %d\n",edge->edge_num);
-			return canque_fifo_test_outslot(&edge->fifo, slotp);
+		while(!list_empty(&qends->active[prio])){
+			edge=list_entry(qends->active[prio].next,struct canque_edge_t,activepeers);
+			if(!canque_fifo_test_fl(&edge->fifo,DEAD)) {
+				/* The first test on unlocked FIFO */
+				if(canque_fifo_test_fl(&edge->fifo,EMPTY)) {
+					can_spin_lock(&edge->fifo.fifo_lock);
+					/* Test has to be repeated to ensure that EMPTY
+					   state has not been nagated when locking FIFO */
+					if(canque_fifo_test_fl(&edge->fifo,EMPTY)) {
+						canque_fifo_set_fl(&edge->fifo,INACTIVE);
+						list_del(&edge->activepeers);
+						list_add(&edge->activepeers,&qends->idle);
+						can_spin_unlock(&edge->fifo.fifo_lock);
+						continue;
+					}
+					can_spin_unlock(&edge->fifo.fifo_lock);
+				}
+				canque_edge_incref(edge);
+				can_spin_unlock_irqrestore(&qends->ends_lock, flags);
+				*qedgep=edge;
+				DEBUGQUE("canque_test_outslot found edge %d\n",edge->edge_num);
+				ret=canque_fifo_test_outslot(&edge->fifo, slotp);
+				if(ret>=0)
+					return ret;
+
+				canque_edge_decref(edge);
+				can_spin_lock_irqsave(&qends->ends_lock, flags);
+			} else {
+				can_spin_lock(&edge->fifo.fifo_lock);
+				canque_fifo_set_fl(&edge->fifo,INACTIVE);
+				list_del(&edge->activepeers);
+				list_add(&edge->activepeers,&qends->idle);
+				can_spin_unlock(&edge->fifo.fifo_lock);
+			}
 		}
 	}
-	spin_unlock_irqrestore(&qends->ends_lock, flags);
+	can_spin_unlock_irqrestore(&qends->ends_lock, flags);
 	*qedgep=NULL;
 	DEBUGQUE("canque_test_outslot no ready slot\n");
 	return -1;
@@ -374,31 +372,28 @@ int canque_free_outslot(struct canque_ends_t *qends,
 	struct canque_edge_t *qedge, struct canque_slot_t *slot)
 {
 	int ret;
-	unsigned long flags;
+	can_spin_irqflags_t flags;
 	ret=canque_fifo_free_outslot(&qedge->fifo, slot);
 	if(ret&CAN_FIFOF_EMPTY){
 	  	canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
 	}
 	if(ret&CAN_FIFOF_FULL)
 	  	canque_notify_inends(qedge,CANQUEUE_NOTIFY_SPACE);
-	spin_lock_irqsave(&qends->ends_lock, flags);
-	if((ret&CAN_FIFOF_EMPTY) || CANQUE_ROUNDROB){
-		spin_lock(&qedge->fifo.fifo_lock);
+	can_spin_lock_irqsave(&qends->ends_lock, flags);
+	if((ret&CAN_FIFOF_EMPTY) || CANQUE_ROUNDROB ){
+		can_spin_lock(&qedge->fifo.fifo_lock);
 		if(canque_fifo_test_fl(&qedge->fifo,EMPTY)){
-			list_del(&qedge->outpeers);
-			list_add(&qedge->outpeers,&qends->idle);
-		}
-	    #if CANQUE_ROUNDROB
-	    	else{
-			list_del(&qedge->outpeers);
-			list_add_tail(&qedge->outpeers,&qends->active[qedge->edge_prio]);
+			canque_fifo_set_fl(&qedge->fifo,INACTIVE);
+			list_del(&qedge->activepeers);
+			list_add(&qedge->activepeers,&qends->idle);
+		} else{
+			list_del(&qedge->activepeers);
+			list_add_tail(&qedge->activepeers,&qends->active[qedge->edge_prio]);
 		}
-	    #endif /*CANQUE_ROUNDROB*/
-		spin_unlock(&qedge->fifo.fifo_lock);
+		can_spin_unlock(&qedge->fifo.fifo_lock);
 	}
-	if(atomic_dec_and_test(&qedge->edge_used))
-  		canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
-	spin_unlock_irqrestore(&qends->ends_lock, flags);
+	can_spin_unlock_irqrestore(&qends->ends_lock, flags);
+	canque_edge_decref(qedge);
 	DEBUGQUE("canque_free_outslot for edge %d returned %d\n",qedge->edge_num,ret);
 	return ret;
 }
@@ -417,12 +412,8 @@ int canque_again_outslot(struct canque_ends_t *qends,
 	struct canque_edge_t *qedge, struct canque_slot_t *slot)
 {
 	int ret;
-	unsigned long flags;
 	ret=canque_fifo_again_outslot(&qedge->fifo, slot);
-	spin_lock_irqsave(&qends->ends_lock, flags);
-	if(atomic_dec_and_test(&qedge->edge_used))
-  		canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
-	spin_unlock_irqrestore(&qends->ends_lock, flags);
+	canque_edge_decref(qedge);
 	DEBUGQUE("canque_again_outslot for edge %d returned %d\n",qedge->edge_num,ret);
 	return ret;
 }
@@ -432,33 +423,37 @@ int canque_again_outslot(struct canque_ends_t *qends,
  * @qedge: pointer to the edge
  * @filtid: ID to set for the edge
  * @filtmask: mask used for ID match check
+ * @filtflags: required filer flags
  *
  * Return Value: Negative value is returned if edge is in the process of delete.
  */
 int canque_set_filt(struct canque_edge_t *qedge,
-	unsigned long filtid, unsigned long filtmask)
+	unsigned long filtid, unsigned long filtmask, int filtflags)
 {
 	int ret;
-	unsigned long flags;
+	can_spin_irqflags_t flags;
 
-	spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
-	atomic_inc(&qedge->edge_used);
-	qedge->filtid=filtid;
-	qedge->filtmask=filtmask;
+	can_spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
+	
+	if(!(filtflags&MSG_PROCESSLOCAL) && (processlocal<2))
+		filtflags |= MSG_LOCAL_MASK;
+	
+	qedge->filtid=canque_filtid2internal(filtid, filtflags);
+	qedge->filtmask=canque_filtid2internal(filtmask, filtflags>>MSG_FILT_MASK_SHIFT);
+	
 	if(canque_fifo_test_fl(&qedge->fifo,DEAD)) ret=-1;
 	else ret=canque_fifo_test_and_set_fl(&qedge->fifo,BLOCK)?1:0;
 
-	spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
+	can_spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
 	if(ret>=0){
 		canque_notify_bothends(qedge,CANQUEUE_NOTIFY_FILTCH);
 	}
-	spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
-	if(!ret)canque_fifo_clear_fl(&qedge->fifo,BLOCK);
-	if(atomic_dec_and_test(&qedge->edge_used))
-		canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
-	spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
+	can_spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
+	if(!ret) canque_fifo_clear_fl(&qedge->fifo,BLOCK);
+	can_spin_unlock_irqrestore(&qedge->fifo.fifo_lock,flags);
 	
-	DEBUGQUE("canque_set_filt for edge %d, ID %ld and mask %ld returned %d\n",qedge->edge_num,filtid,filtmask,ret);
+	DEBUGQUE("canque_set_filt for edge %d, ID %ld, mask %ld, flags %d returned %d\n",
+	          qedge->edge_num,filtid,filtmask,filtflags,ret);
 	return ret;
 }
 
@@ -475,23 +470,20 @@ int canque_set_filt(struct canque_edge_t *qedge,
 int canque_flush(struct canque_edge_t *qedge)
 {
 	int ret;
-	unsigned long flags;
+	can_spin_irqflags_t flags;
 
-	atomic_inc(&qedge->edge_used);
 	ret=canque_fifo_flush_slots(&qedge->fifo);
 	if(ret){
 		canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
 		canque_notify_inends(qedge,CANQUEUE_NOTIFY_SPACE);
-		spin_lock_irqsave(&qedge->outends->ends_lock, flags);
-		spin_lock(&qedge->fifo.fifo_lock);
+		can_spin_lock_irqsave(&qedge->outends->ends_lock, flags);
+		can_spin_lock(&qedge->fifo.fifo_lock);
 		if(canque_fifo_test_fl(&qedge->fifo,EMPTY)){
-			list_del(&qedge->outpeers);
-			list_add(&qedge->outpeers,&qedge->outends->idle);
+			list_del(&qedge->activepeers);
+			list_add(&qedge->activepeers,&qedge->outends->idle);
 		}
-		if(atomic_dec_and_test(&qedge->edge_used))
-			canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
-		spin_unlock(&qedge->fifo.fifo_lock);
-		spin_unlock_irqrestore(&qedge->outends->ends_lock, flags);
+		can_spin_unlock(&qedge->fifo.fifo_lock);
+		can_spin_unlock_irqrestore(&qedge->outends->ends_lock, flags);
 	}
 	DEBUGQUE("canque_flush for edge %d returned %d\n",qedge->edge_num,ret);
 	return ret;
@@ -506,165 +498,18 @@ int canque_flush(struct canque_edge_t *qedge)
 int canqueue_ends_init_gen(struct canque_ends_t *qends)
 {
 	int i;
+	qends->ends_flags=0;
 	for(i=CANQUEUE_PRIO_NR;--i>=0;){
 		INIT_LIST_HEAD(&qends->active[i]);
 	}
 	INIT_LIST_HEAD(&qends->idle);
 	INIT_LIST_HEAD(&qends->inlist);
-	spin_lock_init(&qends->ends_lock);
+	INIT_LIST_HEAD(&qends->outlist);
+	can_spin_lock_init(&qends->ends_lock);
 	return 0;
 }
 
 
-/**
- * canqueue_notify_kern - notification callback handler for Linux userspace clients
- * @qends: pointer to the callback side ends structure
- * @qedge: edge which invoked notification 
- * @what: notification type
- */
-void canqueue_notify_kern(struct canque_ends_t *qends, struct canque_edge_t *qedge, int what)
-{
-	DEBUGQUE("canqueue_notify_kern for edge %d and event %d\n",qedge->edge_num,what);
-	switch(what){
-		case CANQUEUE_NOTIFY_EMPTY:
-			wake_up_interruptible(&qends->endinfo.fileinfo.emptyq);
-			break;
-		case CANQUEUE_NOTIFY_SPACE:
-			wake_up_interruptible(&qends->endinfo.fileinfo.writeq);
-			break;
-		case CANQUEUE_NOTIFY_PROC:
-			wake_up_interruptible(&qends->endinfo.fileinfo.readq);
-			break;
-		case CANQUEUE_NOTIFY_NOUSR:
-			wake_up_interruptible(&qends->endinfo.fileinfo.readq);
-			wake_up_interruptible(&qends->endinfo.fileinfo.writeq);
-			wake_up_interruptible(&qends->endinfo.fileinfo.emptyq);
-			break;
-		case CANQUEUE_NOTIFY_DEAD:
-			if(atomic_read(&qedge->edge_used)>0)
-				atomic_dec(&qedge->edge_used);
-			break;
-		case CANQUEUE_NOTIFY_ATACH:
-			atomic_inc(&qedge->edge_used);
-			break;
-	}
-}
-
-/**
- * canqueue_ends_init_kern - Linux userspace clients specific ends initialization
- * @qends: pointer to the callback side ends structure
- */
-int canqueue_ends_init_kern(struct canque_ends_t *qends)
-{
-	canqueue_ends_init_gen(qends);
-	qends->context=NULL;
-	init_waitqueue_head(&qends->endinfo.fileinfo.readq);
-	init_waitqueue_head(&qends->endinfo.fileinfo.writeq);
-	init_waitqueue_head(&qends->endinfo.fileinfo.emptyq);
-	qends->notify=canqueue_notify_kern;
-	DEBUGQUE("canqueue_ends_init_kern\n");
-	return 0;
-}
-
-
-/**
- * canque_get_inslot4id_wait_kern - find or wait for best outgoing edge and slot for given ID
- * @qends: ends structure belonging to calling communication object
- * @qedgep: place to store pointer to found edge
- * @slotp: place to store pointer to  allocated slot
- * @cmd: command type for slot
- * @id: communication ID of message to send into edge
- * @prio: optional priority of message
- *
- * Same as canque_get_inslot4id(), except, that it waits for free slot
- * in case, that queue is full. Function is specific for Linux userspace clients.
- * Return Value: If there is no usable edge negative value is returned.
- */
-int canque_get_inslot4id_wait_kern(struct canque_ends_t *qends,
-	struct canque_edge_t **qedgep, struct canque_slot_t **slotp,
-	int cmd, unsigned long id, int prio)
-{
-	int ret=-1;
-	DEBUGQUE("canque_get_inslot4id_wait_kern for cmd %d, id %ld, prio %d\n",cmd,id,prio);
-	wait_event_interruptible((qends->endinfo.fileinfo.writeq), 
-		(ret=canque_get_inslot4id(qends,qedgep,slotp,cmd,id,prio))!=-1);
-	return ret;
-}
-
-/**
- * canque_get_outslot_wait_kern - receive or wait for ready slot for given ends
- * @qends: ends structure belonging to calling communication object
- * @qedgep: place to store pointer to found edge
- * @slotp: place to store pointer to received slot
- *
- * The same as canque_test_outslot(), except it waits in the case, that there is
- * no ready slot for given ends. Function is specific for Linux userspace clients.
- * Return Value: Negative value informs, that there is no ready output
- *	slot for given ends. Positive value is equal to the command
- *	slot has been allocated by the input side.
- */
-int canque_get_outslot_wait_kern(struct canque_ends_t *qends,
-	struct canque_edge_t **qedgep, struct canque_slot_t **slotp)
-{
-	int ret=-1;
-	DEBUGQUE("canque_get_outslot_wait_kern\n");
-	wait_event_interruptible((qends->endinfo.fileinfo.readq), 
-		(ret=canque_test_outslot(qends,qedgep,slotp))!=-1);
-	return ret;
-}
-
-/**
- * canque_sync_wait_kern - wait for all slots processing
- * @qends: ends structure belonging to calling communication object
- * @qedge: pointer to edge
- *
- * Functions waits for ends transition into empty state.
- * Return Value: Positive value indicates, that edge empty state has been reached.
- *	Negative or zero value informs about interrupted wait or other problem.
- */
-int canque_sync_wait_kern(struct canque_ends_t *qends, struct canque_edge_t *qedge)
-{
-	int ret=-1;
-	DEBUGQUE("canque_sync_wait_kern\n");
-	wait_event_interruptible((qends->endinfo.fileinfo.emptyq), 
-		(ret=canque_fifo_test_fl(&qedge->fifo,EMPTY)?1:0));
-	return ret;
-}
-
-
-/**
- * canque_new_edge_kern - allocate new edge structure in the Linux kernel context
- * @slotsnr: required number of slots in the newly allocated edge structure
- *
- * Return Value: Returns pointer to allocated slot structure or %NULL if
- *	there is not enough memory to process operation.
- */
-struct canque_edge_t *canque_new_edge_kern(int slotsnr)
-{
-	struct canque_edge_t *qedge;
-	qedge = (struct canque_edge_t *)kmalloc(sizeof(struct canque_edge_t), GFP_KERNEL);
-	if(qedge == NULL) return NULL;
-
-	memset(qedge,0,sizeof(struct canque_edge_t));
-	spin_lock_init(&qedge->fifo.fifo_lock);
-	if(canque_fifo_init_slots(&qedge->fifo, slotsnr)<0){
-		kfree(qedge);
-		DEBUGQUE("canque_new_edge_kern failed\n");
-		return NULL;
-	}
-	atomic_set(&qedge->edge_used,0);
-	qedge->filtid = 0;
-	qedge->filtmask = 0;
-	qedge->edge_prio = 0;
-    #ifdef CAN_DEBUG
-	/* not exactly clean, but enough for debugging */
-	atomic_inc(&edge_num_cnt);
-	qedge->edge_num=atomic_read(&edge_num_cnt);
-    #endif /* CAN_DEBUG */
-	DEBUGQUE("canque_new_edge_kern %d\n",qedge->edge_num);
-	return qedge;
-}
-
 /**
  * canqueue_connect_edge - connect edge between two communication entities
  * @qedge: pointer to edge
@@ -675,26 +520,23 @@ struct canque_edge_t *canque_new_edge_kern(int slotsnr)
  */
 int canqueue_connect_edge(struct canque_edge_t *qedge, struct canque_ends_t *inends, struct canque_ends_t *outends)
 {
-	unsigned long flags;
+	can_spin_irqflags_t flags;
 	if(qedge == NULL) return -1;
 	DEBUGQUE("canqueue_connect_edge %d\n",qedge->edge_num);
-	atomic_inc(&qedge->edge_used);
-	spin_lock_irqsave(&inends->ends_lock, flags);
-	spin_lock(&outends->ends_lock);
-	spin_lock(&qedge->fifo.fifo_lock);
+	canque_edge_incref(qedge);
+	flags=canque_edge_lock_both_ends(inends, outends);
+	can_spin_lock(&qedge->fifo.fifo_lock);
 	qedge->inends=inends;
 	list_add(&qedge->inpeers,&inends->inlist);
 	qedge->outends=outends;
-	list_add(&qedge->outpeers,&outends->idle);
-	spin_unlock(&qedge->fifo.fifo_lock);
-	spin_unlock(&outends->ends_lock);
-	spin_unlock_irqrestore(&inends->ends_lock, flags);
-	canque_notify_bothends(qedge, CANQUEUE_NOTIFY_ATACH);
-	
-	spin_lock_irqsave(&qedge->fifo.fifo_lock, flags);
-	if(atomic_dec_and_test(&qedge->edge_used))
-	  	canque_notify_bothends(qedge,CANQUEUE_NOTIFY_NOUSR);
-	spin_unlock_irqrestore(&qedge->fifo.fifo_lock, flags);
+	list_add(&qedge->outpeers,&outends->outlist);
+	list_add(&qedge->activepeers,&outends->idle);
+	can_spin_unlock(&qedge->fifo.fifo_lock);
+	canque_edge_unlock_both_ends(inends, outends, flags);
+	canque_notify_bothends(qedge, CANQUEUE_NOTIFY_ATTACH);
+
+	if(canque_fifo_test_and_set_fl(&qedge->fifo, READY))
+		canque_edge_decref(qedge);
 	return 0;
 }
 
@@ -702,22 +544,32 @@ int canqueue_connect_edge(struct canque_edge_t *qedge, struct canque_ends_t *ine
  * canqueue_disconnect_edge - disconnect edge from communicating entities
  * @qedge: pointer to edge
  *
- * Return Value: Negative value means, that edge is used and cannot
- *	be disconnected. Operation has to be delayed.
+ * Return Value: Negative value means, that edge is used by somebody
+ *	other and cannot be disconnected. Operation has to be delayed.
  */
 int canqueue_disconnect_edge(struct canque_edge_t *qedge)
 {
 	int ret;
-	unsigned long flags;
+	can_spin_irqflags_t flags;
 	struct canque_ends_t *inends, *outends;
 
 	inends=qedge->inends;
-	if(inends) spin_lock_irqsave(&inends->ends_lock,flags);
 	outends=qedge->outends;
-	if(outends) spin_lock(&outends->ends_lock);
-	spin_lock(&qedge->fifo.fifo_lock);
+
+	if(inends && outends) {
+		flags=canque_edge_lock_both_ends(inends, outends);
+	} else {
+		DEBUGQUE("canqueue_disconnect_edge called with not fully connected edge");
+		if(inends) can_spin_lock_irqsave(&inends->ends_lock,flags);
+		if(outends) can_spin_lock(&outends->ends_lock);
+		flags=0;
+	}
+	
+	can_spin_lock(&qedge->fifo.fifo_lock);
 	if(atomic_read(&qedge->edge_used)==0) {
 		if(qedge->outends){
+			list_del(&qedge->activepeers);
+			mb(); /* memory barrier for list_empty use in canque_dead_func */
 			list_del(&qedge->outpeers);
 			qedge->outends=NULL;
 		}
@@ -727,120 +579,172 @@ int canqueue_disconnect_edge(struct canque_edge_t *qedge)
 		}
 		ret=1;
 	} else ret=-1;
-	spin_unlock(&qedge->fifo.fifo_lock);
-	if(outends) spin_unlock(&outends->ends_lock);
-	if(inends) spin_unlock_irqrestore(&inends->ends_lock,flags);
+	can_spin_unlock(&qedge->fifo.fifo_lock);
+
+	if(inends && outends) {
+		canque_edge_unlock_both_ends(inends, outends, flags);
+	} else {
+		if(outends) can_spin_unlock(&outends->ends_lock);
+		if(inends) can_spin_unlock_irqrestore(&inends->ends_lock,flags);
+	}
+
 	DEBUGQUE("canqueue_disconnect_edge %d returned %d\n",qedge->edge_num,ret);
 	return ret;
 }
 
+
 /**
- * canqueue_disconnect_edge_kern - disconnect edge from communicating entities with wait
- * @qends: ends structure belonging to calling communication object
- * @qedge: pointer to edge
- *
- * Same as canqueue_disconnect_edge(), but tries to wait for state with zero
- * use counter.
- * Return Value: Negative value means, that edge is used and cannot
- *	be disconnected yet. Operation has to be delayed.
+ * canqueue_block_inlist - block slot allocation of all outgoing edges of specified ends  
+ * @qends: pointer to ends structure
  */
-int canqueue_disconnect_edge_kern(struct canque_ends_t *qends, struct canque_edge_t *qedge)
+void canqueue_block_inlist(struct canque_ends_t *qends)
 {
-	canque_fifo_set_fl(&qedge->fifo,BLOCK);
-	DEBUGQUE("canqueue_disconnect_edge_kern %d called\n",qedge->edge_num);
-	if(!canque_fifo_test_and_set_fl(&qedge->fifo,DEAD)){
-		canque_notify_bothends(qedge, CANQUEUE_NOTIFY_DEAD);
-		if(atomic_read(&qedge->edge_used)>0)
-			atomic_dec(&qedge->edge_used);
-		DEBUGQUE("canqueue_disconnect_edge_kern %d waiting\n",qedge->edge_num);
-		wait_event_interruptible((qends->endinfo.fileinfo.emptyq), 
-			(canqueue_disconnect_edge(qedge)>=0));
-		return 0;
-	} else {
-		DEBUGQUE("canqueue_disconnect_edge_kern failed\n");
-		return -1;
+	struct canque_edge_t *edge;
+
+        canque_for_each_inedge(qends, edge) {
+		canque_fifo_set_fl(&edge->fifo,BLOCK);
 	}
 }
 
 
-int canqueue_disconnect_list_kern(struct canque_ends_t *qends, struct list_head *list)
+/**
+ * canqueue_block_outlist - block slot allocation of all incoming edges of specified ends  
+ * @qends: pointer to ends structure
+ */
+void canqueue_block_outlist(struct canque_ends_t *qends)
 {
 	struct canque_edge_t *edge;
-	unsigned long flags;
-	for(;;){
-		spin_lock_irqsave(&qends->ends_lock,flags);
-		if(list_empty(list)){
-			spin_unlock_irqrestore(&qends->ends_lock,flags);
-			return 0;
-		}
-		if(list == &qends->inlist)
-			edge=list_entry(list->next,struct canque_edge_t,inpeers);
-		else
-			edge=list_entry(list->next,struct canque_edge_t,outpeers);
-		atomic_inc(&edge->edge_used);
-		spin_unlock_irqrestore(&qends->ends_lock,flags);
-		if(canqueue_disconnect_edge_kern(qends, edge)>=0) {
-			/* Free edge memory */
-			canque_fifo_done(&edge->fifo);
-			kfree(edge);
-		}else{
-			DEBUGQUE("canqueue_disconnect_list_kern in troubles\n");
-			DEBUGQUE("the edge %d has usage count %d and flags %ld\n",edge->edge_num,atomic_read(&edge->edge_used),edge->fifo.fifo_flags);
-			return -1;
+
+        canque_for_each_outedge(qends, edge) {
+		canque_fifo_set_fl(&edge->fifo,BLOCK);
+	}
+}
+
+
+/**
+ * canqueue_ends_kill_inlist - sends request to die to all outgoing edges
+ * @qends: pointer to ends structure
+ * @send_rest: select, whether already allocated slots should be processed
+ *	by FIFO output side
+ *
+ * Return Value: Non-zero value means, that not all edges could be immediately
+ *	disconnected and that ends structure memory release has to be delayed
+ */
+int canqueue_ends_kill_inlist(struct canque_ends_t *qends, int send_rest)
+{
+	struct canque_edge_t *edge;
+	
+	canque_for_each_inedge(qends, edge){
+		canque_notify_bothends(edge, CANQUEUE_NOTIFY_DEAD_WANTED);
+		if(send_rest){
+			canque_edge_incref(edge);
+			if(!canque_fifo_test_and_set_fl(&edge->fifo, FREEONEMPTY)){
+				if(!canque_fifo_test_fl(&edge->fifo, EMPTY))
+					continue;
+				if(!canque_fifo_test_and_clear_fl(&edge->fifo, FREEONEMPTY))
+					continue;
+			}
+			canque_edge_decref(edge);
 		}
 	}
+	return list_empty(&qends->inlist)?0:1;
 }
 
-void canqueue_block_list(struct canque_ends_t *qends, struct list_head *list)
+
+/**
+ * canqueue_ends_kill_outlist - sends request to die to all incoming edges
+ * @qends: pointer to ends structure
+ *
+ * Return Value: Non-zero value means, that not all edges could be immediately
+ *	disconnected and that ends structure memory release has to be delayed
+ */
+int canqueue_ends_kill_outlist(struct canque_ends_t *qends)
 {
 	struct canque_edge_t *edge;
-	struct list_head *entry;
 	
-	/* has to be called with qends->ends_lock already locked */
-	list_for_each(entry,&qends->inlist){
-		if(list == &qends->inlist)
-			edge=list_entry(list->next,struct canque_edge_t,inpeers);
-		else
-			edge=list_entry(list->next,struct canque_edge_t,outpeers);
-		canque_fifo_set_fl(&edge->fifo,BLOCK);
+	canque_for_each_outedge(qends, edge){
+		canque_notify_bothends(edge, CANQUEUE_NOTIFY_DEAD_WANTED);
 	}
+	return list_empty(&qends->outlist)?0:1;
 }
 
 
 /**
- * canqueue_ends_done_kern - finalizing of the ends structure for Linux kernel clients
+ * canqueue_ends_filt_conjuction - computes conjunction of incoming edges filters filters
  * @qends: pointer to ends structure
- * @sync: flag indicating, that user wants to wait for processing of all remaining
- *	messages
+ * @filt: pointer the filter structure filled by computed filters conjunction
  *
- * Return Value: Function should be designed such way to not fail.
+ * Return Value: Number of incoming edges
  */
-int canqueue_ends_done_kern(struct canque_ends_t *qends, int sync)
+int canqueue_ends_filt_conjuction(struct canque_ends_t *qends, struct canfilt_t *filt)
 {
-	unsigned long flags;
-	int i;
+	struct canque_edge_t *edge;
+	int cnt=0;
+	unsigned long filtid=0;
+	unsigned long filtmask=~0;
+	unsigned long local_only=canque_filtid2internal(0,MSG_LOCAL);
+
+	canque_for_each_inedge(qends, edge){
+		/* skip edges processing only local messages */
+		if(edge->filtid & edge->filtmask & local_only)
+			continue;
 
-	DEBUGQUE("canqueue_ends_done_kern\n");
-	spin_lock_irqsave(&qends->ends_lock,flags);
-	canqueue_block_list(qends, &qends->idle);
-	for(i=CANQUEUE_PRIO_NR;--i>=0;){
-		canqueue_block_list(qends, &qends->active[i]);
+		if(!cnt++)
+			filtid = edge->filtid;
+		else
+			filtmask &= ~(filtid ^ edge->filtid);
+	
+		filtmask &= edge->filtmask;
 	}
-	canqueue_block_list(qends, &qends->idle);
-	canqueue_block_list(qends, &qends->inlist);
-	spin_unlock_irqrestore(&qends->ends_lock,flags);
+	
+	filt->id = filtid & MSG_ID_MASK;
+	filt->mask = filtmask & MSG_ID_MASK;
+	filtid >>= 28;
+	filtmask >>= 28;
+	filt->flags = filtid & MSG_EXT;
+	if(filtmask & (MSG_EXT))
+		filt->flags |= MSG_EXT_MASK;
+	if(filtid & (MSG_RTR<<1))
+		filt->flags |= MSG_RTR<<1;
+	if(filtmask & (MSG_RTR<<1))
+		filt->flags |= MSG_RTR_MASK;
+	return cnt;
+}
 
-	for(i=CANQUEUE_PRIO_NR;--i>=0;){
-		canqueue_disconnect_list_kern(qends, &qends->active[i]);
-	}
-	canqueue_disconnect_list_kern(qends, &qends->idle);
-	canqueue_disconnect_list_kern(qends, &qends->inlist);
 
-	wake_up_interruptible(&qends->endinfo.fileinfo.readq);
-	wake_up_interruptible(&qends->endinfo.fileinfo.writeq);
-	wake_up_interruptible(&qends->endinfo.fileinfo.emptyq);
+/**
+ * canqueue_ends_flush_inlist - flushes all messages in incoming edges
+ * @qends: pointer to ends structure
+ *
+ * Return Value: Negative value informs about unsuccessful result
+ */
+int canqueue_ends_flush_inlist(struct canque_ends_t *qends)
+{
+	struct canque_edge_t *edge;
 	
+	canque_for_each_inedge(qends, edge){
+		canque_flush(edge);
+	}
+	return 0;
+}
 
+
+/**
+ * canqueue_ends_flush_outlist - flushes all messages in outgoing edges
+ * @qends: pointer to ends structure
+ *
+ * Return Value: Negative value informs about unsuccessful result
+ */
+int canqueue_ends_flush_outlist(struct canque_ends_t *qends)
+{
+	struct canque_edge_t *edge;
+	
+	canque_for_each_outedge(qends, edge){
+		canque_flush(edge);
+	}
 	return 0;
 }
 
+
+
+