X-Git-Url: http://rtime.felk.cvut.cz/gitweb/lincan.git/blobdiff_plain/4cf24de229090b1ab6279570a564d224e13dd706..786c7d54e8d820e89997e507c29ea716c0d55fd9:/lincan/src/sja1000.c

diff --git a/lincan/src/sja1000.c b/lincan/src/sja1000.c
index e1da4c1..0955c76 100644
--- a/lincan/src/sja1000.c
+++ b/lincan/src/sja1000.c
@@ -210,22 +210,22 @@ int sja1000_pre_read_config(struct chip_t *chip, struct msgobj_t *obj)
 // TODO: this would be best sja1000_irq_read_handler(chip);
 // now just duplicate the code.
 	do {
-		id=(can_read_reg(chip, SJARXID1)<<8) + can_read_reg(chip, 
-							SJARXID0);
-		fifo->buf_rx_entry[fifo->head].length = (id>>8) & 0x0f;
-		fifo->buf_rx_entry[fifo->head].id = id>>5;
-		fifo->buf_rx_entry[fifo->head].flags = id&ID0_RTR ?
-								MSG_RTR : 0;
-		fifo->buf_rx_entry[fifo->head].timestamp = 0;
-		fifo->buf_rx_entry[fifo->head].cob = 0;
-		for (i=0; i<fifo->buf_rx_entry[fifo->head].length; i++) {
-			fifo->buf_rx_entry[fifo->head].data[i] = 
-					can_read_reg(chip,SJARXDAT0 + i);
-		}
-		fifo->head++;
-		if (fifo->head == MAX_BUF_LENGTH -1)
-			fifo->head = 0;
+		id = can_read_reg(chip, SJARXID0) | (can_read_reg(chip, SJARXID1)<<8);
+		fifo->rx_writep->length = id & 0x0f;
+		fifo->rx_writep->flags = id&ID0_RTR ? MSG_RTR : 0;
+		fifo->rx_writep->timestamp = 0;
+		fifo->rx_writep->cob = 0;
+		fifo->rx_writep->id = id>>5;
+
+		for (i=0; i<fifo->rx_writep->length; i++)
+			fifo->rx_writep->data[i]=can_read_reg(chip, SJARXDAT0 + i);
+
+		fifo->rx_writep++;
+		if (fifo->rx_writep >= fifo->buf_rx_entry + MAX_BUF_LENGTH)
+			fifo->rx_writep = fifo->buf_rx_entry;
+
 		can_write_reg(chip, CMR_RRB, SJACMR);
+
 	} while (can_read_reg(chip, SJASR) & SR_RBS);
 
 // enable interrupts