X-Git-Url: http://rtime.felk.cvut.cz/gitweb/lincan.git/blobdiff_plain/7440c5c6ece06cdf6ea19270047cca22b324d73a..aa97a7ac78dbe529888662e3ffdc207b172c5a2c:/lincan/src/hcan2.c

diff --git a/lincan/src/hcan2.c b/lincan/src/hcan2.c
index a31490f..e960685 100644
--- a/lincan/src/hcan2.c
+++ b/lincan/src/hcan2.c
@@ -311,18 +311,19 @@ int hcan2_standard_mask(struct canchip_t *chip, unsigned short code, unsigned sh
 	else
 	    return -ENODEV;
 
-	chip->chip_data = (void*)0; /* reset mbox number */
+	chip->chip_data = (void*)0;	/* reset mbox number */
 
 
 	ctrl0 = ((code & 0x07ff) << 4);
 	lafm0 = ((mask & 0x07ff) << 4);
+	lafm0 |= 0x0003;		/* ignore Ext ID 17:16  */
 	
 	can_write_reg_w(chip, ctrl0, (int) obj->obj_base_addr + HCAN2_MB_CTRL0);
 	can_write_reg_w(chip, 0x0000, (int) obj->obj_base_addr + HCAN2_MB_CTRL1);
 	can_write_reg_w(chip, lafm0, (int) obj->obj_base_addr + HCAN2_MB_MASK);
 	can_write_reg_w(chip, 0xffff, (int) obj->obj_base_addr + HCAN2_MB_MASK + 2);
 
-	DEBUGMSG("Set standard_mask [id:0x%04x, m:0x%04x]\n", code, mask);
+	DEBUGMSG("MB%02d: Set standard_mask [id:0x%04x, m:0x%04x]\n", obj_idx, code, lafm0);
 	return 0;
 }
 
@@ -355,7 +356,7 @@ int hcan2_extended_mask(struct canchip_t *chip, unsigned long code, unsigned lon
 	can_write_reg_w(chip, lafm0, (int) obj->obj_base_addr + HCAN2_MB_MASK);
 	can_write_reg_w(chip, lafm1, (int) obj->obj_base_addr + HCAN2_MB_MASK + 2);
 	
-	DEBUGMSG("Set extended_mask [id:0x%08x, m:0x%08x]\n", (uint32_t)code, (uint32_t)mask);
+	DEBUGMSG("MB%02d: Set extended_mask [id:0x%08x, m:0x%08x]\n", obj_idx, (uint32_t)code, (uint32_t)mask);
     
         return 0;
 }
@@ -454,25 +455,24 @@ int hcan2_irq_handler(int irq, struct canchip_t *chip)
 		/* Received message */
 		if (irq_reg & HCAN2_IRR_DFRI)
 		{
-    			rxdf = (can_read_reg_w(chip, HCAN2_RXPR1) << 16) +
-			    can_read_reg_w(chip, HCAN2_RXPR0);
+			rxdf = (can_read_reg_w(chip, HCAN2_RXPR1) << 16) +
+				can_read_reg_w(chip, HCAN2_RXPR0);
 
-			DEBUGMSG("Received message [0x%08x]\n", rxdf);
+			while(rxdf) {
+				DEBUGMSG("Received message [0x%08x]\n", rxdf);
 
-			while (rxdf) {
 				/* find the message object */
-				for (idx = 0; (idx < chip->max_objects) && !(rxdf & (1<<idx)); idx++) { }
-				
-				/* realy i got one? */
-				if (idx < chip->max_objects) {
-					hcan2_irq_read_handler(chip, chip->msgobj[idx]);
-				
-					/* clear RXPR flag for this msgobj */
-					can_write_reg_w(chip, (1 << (idx % 16)), HCAN2_RXPR0 - 2*(idx / 16));
-					
-					rxdf = (can_read_reg_w(chip, HCAN2_RXPR1) << 16) +
+				for (idx = 0; (idx < chip->max_objects) && rxdf; idx++)
+					if ((rxdf & (1<<idx))) {
+						hcan2_irq_read_handler(chip, chip->msgobj[idx]);
+						/* RXPR flag for this msgobj is cleared during irq_read_handler*/
+						rxdf &= ~(1 << idx);
+					}
+
+
+				DEBUGMSG("Before reset flags [0x%08x]\n", rxdf);
+				rxdf = (can_read_reg_w(chip, HCAN2_RXPR1) << 16) +
 					can_read_reg_w(chip, HCAN2_RXPR0);
-				}
 			}
 		}
 
@@ -695,22 +695,22 @@ void hcan2_irq_read_handler(struct canchip_t *chip, struct msgobj_t *obj)
 	}
 	else
 	    obj->rx_msg.id = (ctrl0 & HCAN2_MBCT0_STDID)>>4;
-	    
+
 
 	if(len > CAN_MSG_LENGTH) len = CAN_MSG_LENGTH;
 	for (i = 0; i < len; i++)
 	{
 		/* rcanqueue_ends_filt_conjuctionead 16bit data - two data bytes*/
 		data = can_read_reg_w(chip, (int) obj->obj_base_addr + HCAN2_MB_DATA1 + i);
-		obj->rx_msg.data[i] = (data & 0xff00) >> 8;
-		if (++i < len) obj->rx_msg.data[i] = data & 0x00ff;
+		obj->rx_msg.data[i] = (data & 0xff00) >> 8;		// one data byte
+		if (++i < len) obj->rx_msg.data[i] = data & 0x00ff;	// second data byte
 	}
 
 	/* Computes correct offset address of register from MSGBOX_IDX and RTR flag
 	 * result is one of these:
 	 * HCAN2_RXPR1, HCAN2_RXPR0, HCAN2_RFPR1, HCAN2_RFPR0
 	 */
-	flag_addr = ((ctrl0 & HCAN2_MBCT0_RTR) << 3) + HCAN2_RXPR0 - ((obj->object - 1) / 16) * 2;
+	flag_addr = HCAN2_RXPR0 - (int)((obj->object - 1) / 16) * 2;
 		
 	/* Reset flag by writing 1 to its position */
 	can_write_reg_w(chip, (1 << ((obj->object - 1) % 16)), flag_addr);