Added support for local message processing and some cleanups.

[lincan.git] / lincan / src / can_queue.c

diff --git a/lincan/src/can_queue.c b/lincan/src/can_queue.c
index 62d8e19b49fa40e358684e006a4ce0f6e51fe97f..75853a961effa2bd10147e991660f952b5e5721a 100644 (file)
--- 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"
 
 #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)
 #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;
        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;
 }
        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;
        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;
        
        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 */
        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);
                        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
  * @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,
  *
  * 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);
 {
        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;
 
        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);
        }
        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);
        
        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;
 }
 
        return ret;
 }
 


@@ -477,7 +498,6 @@ int canque_flush(struct canque_edge_t *qedge)
        int ret;
        unsigned long flags;
 
        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);
        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);
                }
                        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);
        }
                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;
        }
        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 */
        qedge->edge_prio = 0;
     #ifdef CAN_DEBUG
        /* not exactly clean, but enough for debugging */