WebSVN – HomeAutomation – Diff – /trunk/EmbeddedSoftware/AVR/avr-lib/drivers/can/stdcan.c

 #include "stdcan.h"
 #if defined(_AVRLIB_BIOS_)
 #include <bios.h>
 #define Can_Send BIOS_CanSend
 #else
 #include <drivers/can/mcp2515/mcp2515.h>
 #endif
 #include <avr/interrupt.h>
+#include <util/atomic.h>
 #if (STDCAN_TX_QUEUE_SIZE > 1)
 #error StdCan: Tx queue size longer than one msg not yet implemented.
 #endif
 /**
  * The receive queue.
  */
-StdCan_Msg_t RxQ[STDCAN_RX_QUEUE_SIZE];
+StdCan_Msg_t RxQ[STDCAN_RX_QUEUE_SIZE + 1];
 unsigned char RxQ_Rd_idx; /**< Receive queue read index. */
 unsigned char RxQ_Wr_idx; /**< Receive queue write index. */
-unsigned char RxQ_Len; /**< Number of messages in receive queue. */
 #if (STDCAN_TX_QUEUE_SIZE > 1)
 /**
  * The transmit queue.
  */
 StdCan_Msg_t TxQ[STDCAN_TX_QUEUE_SIZE];
 unsigned char TxQ_Rd_idx; /**< Transmit queue read index. */
 unsigned char TxQ_Wr_idx; /**< Transmit queue write index. */
-unsigned char TxQ_Len; /**< Number of messages in transmit queue. */
 #endif
 #if (STDCAN_FILTER)
 /**
  * The message acceptance filters.
 typedef struct {
     char Active; /**< True if this filter should be used. */
     unsigned long Id; /**< Match if id matches the message id in all bit locations that are not masked. */
     unsigned long Mask; /**< Each bit specifies if the corresponding id bit must match (mask[n] = 1) or is Don't Care (mask[n] = 0). */
 } StdCan_Filter_t;
 StdCan_Filter_t RxFilters[STDCAN_NUM_FILTERS];
 #endif
 /**
  * CAN message callback.
  * Callback for when a message is received from the lower
  * layer (BIOS or CAN driver).
  */
 void Can_Process(Can_Message_t* msg)
+{
-    unsigned char n;
+    /* What will the next write index be if the push goes well? */
-    /* Check if there is room on the queue. */
+    unsigned char Wr_next = RxQ_Wr_idx + 1;
-    if (RxQ_Len < STDCAN_RX_QUEUE_SIZE) {
+    if (Wr_next >= STDCAN_RX_QUEUE_SIZE + 1) Wr_next = 0;
+    /* Check if there is room on the queue. */
+    if (Wr_next != RxQ_Rd_idx) {
+        unsigned char n;
 #if (STDCAN_FILTER)
         /* Try to match each filter in turn. */
         for (n = 0; n < STDCAN_NUM_FILTERS; n++) {
             if (RxFilters[n].Active && !((msg->Id ^ RxFilters[n].Id) & RxFilters[n].Mask)) {
                 RxQ[RxQ_Wr_idx].Match = n;
                 break;
+            }
         }
         if (n == STDCAN_NUM_FILTERS) return; // No match found, discard message.
 #endif
         /* Copy the message from lower layer into the queue. */
         RxQ[RxQ_Wr_idx].Id     = msg->Id;
         RxQ[RxQ_Wr_idx].Length = msg->DataLength;
         //TODO: This should be guarded against invalid DataLength.
         for (n = 0; n < RxQ[RxQ_Wr_idx].Length; n++) {
             RxQ[RxQ_Wr_idx].Data[n] = msg->Data.bytes[n];
+        }
-        /* Increment write index and queue length. */
+        /* Update write index. */
-        if (++RxQ_Wr_idx >= STDCAN_RX_QUEUE_SIZE) RxQ_Wr_idx = 0;
-        RxQ_Len++;
+        RxQ_Wr_idx = Wr_next;
     } else {
         /* Overflow, just drop the new message. In the future, some
          * form of priority scheme could be used to drop another
          * message in the queue.
          */
     }
 }
 #ifdef MODULE_APPLICATION
 StdCan_Ret_t StdCan_Init(void)
+{
 #else
 StdCan_Ret_t StdCan_Init(Node_Desc_t* node_desc)
     StdCan_Ret_t retval;
     /* Reset all queue variables. */
     RxQ_Rd_idx = 0;
     RxQ_Wr_idx = 0;
-    RxQ_Len    = 0;
 #if (STDCAN_TX_QUEUE_SIZE > 1)
     TxQ_Rd_idx = 0;
     TxQ_Wr_idx = 0;
-    TxQ_Len    = 0;
 #endif
 #if defined(_AVRLIB_BIOS_)
     /* Initialize BIOS' interface for CAN. */
     BIOS_CanCallback = Can_Process;
     /* Try to send it. */
     Can_Send(&Startup);
 #endif
     return retval;
 }
 StdCan_Ret_t StdCan_Get(StdCan_Msg_t* msg)
+{
+    unsigned char Wr_idx;
-    unsigned char n;
+    unsigned char Rd_idx;
+    /* Read indices atomically into local copies. */
+    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+        Wr_idx = *(volatile unsigned char*)&RxQ_Wr_idx;
+        Rd_idx = *(volatile unsigned char*)&RxQ_Rd_idx;
-    cli();
+    }
     /* Check if there's a message waiting. */
-    if (RxQ_Len) {
+    if (Rd_idx != Wr_idx) {
+        unsigned char n;
         /* Copy message to user buffer. */
-        msg->Id     = RxQ[RxQ_Rd_idx].Id;
+        msg->Id     = RxQ[Rd_idx].Id;
-        msg->Length = RxQ[RxQ_Rd_idx].Length;
+        msg->Length = RxQ[Rd_idx].Length;
         //TODO: Consider a mempcy() of the entire message.
-        for (n = 0; n < RxQ[RxQ_Rd_idx].Length; n++) {
+        for (n = 0; n < RxQ[Rd_idx].Length; n++) {
-            msg->Data[n] = RxQ[RxQ_Rd_idx].Data[n];
+            msg->Data[n] = RxQ[Rd_idx].Data[n];
         }
-        /* Increment read index and decrease queue length. */
+        /* Update read index and store back atomically. */
-        if (++RxQ_Rd_idx >= STDCAN_RX_QUEUE_SIZE) RxQ_Rd_idx = 0;
+        if (++Rd_idx >= STDCAN_RX_QUEUE_SIZE + 1) Rd_idx = 0;
+        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+            *(volatile unsigned char*)&RxQ_Rd_idx = Rd_idx;
-        RxQ_Len--;
+        }
-        sei();
         return StdCan_Ret_OK;
     } else {
         /* Queue is empty. */
-        sei();
         return StdCan_Ret_Empty;
+    }
-}
-unsigned char StdCan_Get_Pending(void)
-{
-    return RxQ_Len;
+}
 StdCan_Ret_t StdCan_Put(StdCan_Msg_t* msg)
+{
     Can_Message_t Can_Msg;

Subversion Repositories HomeAutomation

(root)/trunk/EmbeddedSoftware/AVR/avr-lib/drivers/can/stdcan.c – Rev 1909 → 2029