Improve mutex lock logic.

[apps/low-level-can-service.git] / src / can_reader.cpp

diff --git a/src/can_reader.cpp b/src/can_reader.cpp
index 6403cfa..7ae981b 100644 (file)
--- a/src/can_reader.cpp
+++ b/src/can_reader.cpp
@@ -16,51 +16,23 @@
  * limitations under the License.
  */
 
-#include <linux/can.h>
-#include <linux/can/raw.h>
+#include "can_reader.hpp"
 
-#include <afb/afb-binding.h>
+#include "low-can-binding.hpp"
+#include "can-utils.hpp"
 
-#include "can-utils.h"
-
-void can_reader(CanBus_c *can_bus))
+void can_reader(can_bus_dev_t &can_bus_dev, can_bus_t& can_bus)
 {
-       ssize_t nbytes;
-       int maxdlen;
-       CanMessage_c can_message;
-       canfd_frame canfd_frame;
-
-       /* Test that socket is really opened */
-       if ( can_bus->socket < 0)
-       {
-               ERROR(interface, "read_can: Socket unavailable");
-               return -1;
-       }
+       can_message_t can_message;
 
-       while(true)
+       while(can_bus_dev.is_running())
        {
-               nbytes = read(can_bus->socket, &canfd_frame, CANFD_MTU);
+               can_message.convert_from_canfd_frame(can_bus_dev.read());
 
-               switch(nbytes)
                {
-                       case CANFD_MTU:
-                               DEBUG(interface, "read_can: Got an CAN FD frame with length %d", canfd_frame.len);
-                               maxdlen = CANFD_MAX_DLEN;
-                               break;
-                       case CAN_MTU:
-                               DEBUG(interface, "read_can: Got a legacy CAN frame with length %d", canfd_frame.len);
-                               maxdlen = CAN_MAX_DLEN;
-                               break;
-                       default:
-                               if (errno == ENETDOWN)
-                                       ERROR(interface, "read_can: %s interface down", device);
-                               
-                               ERROR(interface, "read_can: Error reading CAN bus");
-                               return -2;
+                       std::lock_guard<std::mutex> can_message_lock(can_message_mutex);
+                       can_bus.push_new_can_message(can_message);
                }
-
-               can_message.convert_from_canfd_frame(canfd_frame);
-
-               can_message_q.push(can_message);
+               new_can_message.notify_one();
        }
 }
\ No newline at end of file