X-Git-Url: http://rtime.felk.cvut.cz/gitweb/lincan.git/blobdiff_plain/3e5b474f9cd113def3be02af0cf6d5401a7427c9..659b098309c2bca57ac60a35a1b9900d6f28e74d:/lincan/src/can_queue.c

diff --git a/lincan/src/can_queue.c b/lincan/src/can_queue.c
index 62d8e19..75853a9 100644
--- a/lincan/src/can_queue.c
+++ b/lincan/src/can_queue.c
@@ -18,6 +18,16 @@
 #include "../include/can.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;
+
+/*#define CAN_DEBUG*/
+
 #ifdef CAN_DEBUG
 	#define DEBUGQUE(fmt,args...) printk(KERN_ERR "can_queue (debug): " fmt,\
 	##args)
@@ -47,11 +57,14 @@ int canque_fifo_flush_slots(struct canque_fifo_t *fifo)
 	struct canque_slot_t *slot;
 	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);
+	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;
 	spin_unlock_irqrestore(&fifo->fifo_lock, flags);
 	return ret;
 }
@@ -288,18 +301,22 @@ 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);
+	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);
+
 	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))
 			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);
@@ -432,19 +449,24 @@ 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;
 
 	spin_lock_irqsave(&qedge->fifo.fifo_lock,flags);
-	atomic_inc(&qedge->edge_used);
-	qedge->filtid=filtid;
-	qedge->filtmask=filtmask;
+	
+	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;
 
@@ -454,11 +476,10 @@ int canque_set_filt(struct canque_edge_t *qedge,
 	}
 	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);
 	
-	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;
 }
 
@@ -477,7 +498,6 @@ int canque_flush(struct canque_edge_t *qedge)
 	int ret;
 	unsigned long flags;
 
-	atomic_inc(&qedge->edge_used);
 	ret=canque_fifo_flush_slots(&qedge->fifo);
 	if(ret){
 		canque_notify_inends(qedge,CANQUEUE_NOTIFY_EMPTY);
@@ -488,8 +508,6 @@ int canque_flush(struct canque_edge_t *qedge)
 			list_del(&qedge->outpeers);
 			list_add(&qedge->outpeers,&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);
 	}
@@ -654,7 +672,7 @@ struct canque_edge_t *canque_new_edge_kern(int slotsnr)
 	}
 	atomic_set(&qedge->edge_used,0);
 	qedge->filtid = 0;
-	qedge->filtmask = 0;
+	qedge->filtmask = canque_filtid2internal(0l, (processlocal<2)? MSG_LOCAL:0);
 	qedge->edge_prio = 0;
     #ifdef CAN_DEBUG
 	/* not exactly clean, but enough for debugging */