Added support for local message processing and some cleanups.

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

diff --git a/lincan/src/i82527.c b/lincan/src/i82527.c
index c10db6b707d88602b2db92135d98501c8e179289..ec99430e58ba7222c45e4d739de93551fa1d6c3b 100644 (file)
--- a/lincan/src/i82527.c
+++ b/lincan/src/i82527.c
@@ -302,16 +302,16 @@ int i82527_pre_write_config(struct chip_t *chip, struct msgobj_t *obj,
                                                        struct canmsg_t *msg)
 {
        int i=0,id0=0,id1=0,id2=0,id3=0;
+       int len;
+       
+       len = msg->length;
+       if(len > CAN_MSG_LENGTH) len = CAN_MSG_LENGTH;
 
        canobj_write_reg(chip,obj,(RMPD_RES|TXRQ_RES|CPUU_SET|NEWD_RES),iMSGCTL1);
        canobj_write_reg(chip,obj,(MVAL_SET|TXIE_SET|RXIE_RES|INTPD_RES),iMSGCTL0);
-       if (extended) {
-               canobj_write_reg(chip,obj,(msg->length<<4)+(MCFG_DIR|MCFG_XTD),iMSGCFG);
-       }
-       else {
-               canobj_write_reg(chip,obj,(msg->length<<4)+MCFG_DIR,iMSGCFG);
-       }
-       if (extended) {
+
+       if (extended || (msg->flags&MSG_EXT)) {
+               canobj_write_reg(chip,obj,(len<<4)|(MCFG_DIR|MCFG_XTD),iMSGCFG);
                id0 = (unsigned char) (msg->id<<3);
                id1 = (unsigned char) (msg->id>>5);
                id2 = (unsigned char) (msg->id>>13);
@@ -322,13 +322,14 @@ int i82527_pre_write_config(struct chip_t *chip, struct msgobj_t *obj,
                canobj_write_reg(chip,obj,id3,iMSGID0);
        }
        else {
+               canobj_write_reg(chip,obj,(len<<4)|MCFG_DIR,iMSGCFG);
                id1 = (unsigned char) (msg->id<<5);
                id0 = (unsigned char) (msg->id>>3);
                canobj_write_reg(chip,obj,id1,iMSGID1);
                canobj_write_reg(chip,obj,id0,iMSGID0);
        }
        canobj_write_reg(chip,obj,0xfa,iMSGCTL1);
-       for (i=0; i<msg->length; i++) {
+       for (i=0; i<len; i++) {
                canobj_write_reg(chip,obj,msg->data[i],iMSGDAT0+i);
        }
 
@@ -409,6 +410,12 @@ inline void i82527_irq_write_handler(struct chip_t *chip, struct msgobj_t *obj)
        canobj_write_reg(chip,obj,(MVAL_RES|TXIE_RES|RXIE_RES|INTPD_RES),+iMSGCTL0);
 
        if(obj->tx_slot){
+               /* Do local transmitted message distribution if enabled */
+               if (processlocal){
+                       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;
        }
@@ -436,19 +443,20 @@ inline void i82527_irq_write_handler(struct chip_t *chip, struct msgobj_t *obj)
 inline void i82527_irq_read_handler(struct chip_t *chip, struct msgobj_t *obj,
                                    unsigned long message_id)
 {
-       int i=0, tmp=1 ;
+       int i=0, tmp=1, len;
        
        while (tmp) {
                canobj_write_reg(chip,obj,(RMPD_RES|TXRQ_RES|MLST_RES|NEWD_RES),iMSGCTL1);
                canobj_write_reg(chip,obj,(MVAL_SET|TXIE_RES|RXIE_SET|INTPD_RES),iMSGCTL0);
 
-               obj->rx_msg.length =(canobj_read_reg(chip,obj,iMSGCFG) & 0xf0) >> 4;
+               len = (canobj_read_reg(chip,obj,iMSGCFG) >> 4) & 0xf;
+               obj->rx_msg.length = len;
                obj->rx_msg.id = message_id;
+
+               if(len > CAN_MSG_LENGTH) len = CAN_MSG_LENGTH;
                for (i=0; i < obj->rx_msg.length; i++)
                        obj->rx_msg.data[i] = canobj_read_reg(chip,obj,iMSGDAT0+i);
 
-//FIXME: Add buffer overflow check, currently it's silently over written!
-
                canque_filter_msg2edges(obj->qends, &obj->rx_msg);
 
                if (!((tmp=canobj_read_reg(chip,obj,iMSGCTL1)) & NEWD_SET)) {