Adding canbus read method

Change-Id: I438d885516e9cbe499d333ff3065c5e9d6d4d99a
Signed-off-by: Romain Forlot <romain.forlot@iot.bzh>

2 files changed, 69 insertions, 19 deletions

diff --git a/src/can-utils.cpp b/src/can-utils.cpp
index 16e18b4b..7dfe2286 100644
--- a/src/can-utils.cpp
+++ b/src/can-utils.cpp
@@ -23,25 +23,25 @@
 *
 *********************************************************************************/
 
-CanBus_c::CanBus_c(afb_binding_interface *itf, const std:string& dev_name)
+CanBus_t::CanBus_t(afb_binding_interface *itf, const std:string& dev_name)
     : interface{itf}, deviceName{dev_name}
 {
 }
 
-int CanBus_c::open()
+int CanBus_t::open()
 {
 	const int canfd_on = 1;
 	struct ifreq ifr;
 	struct timeval timeout = {1, 0};
 
-	DEBUG(interface, "open_can_dev: CAN Handler socket : %d", socket);
+	DEBUG(interface_, "open_can_dev: CAN Handler socket : %d", can_socket_);
 	if (can_socket_ >= 0)
 		return 0;
 
 	can_socket_ = ::socket(PF_CAN, SOCK_RAW, CAN_RAW);
 	if (socket < 0)
 	{
-		ERROR(interface, "open_can_dev: socket could not be created");
+		ERROR(interface_, "open_can_dev: socket could not be created");
 	}
 	else
 	{
@@ -50,25 +50,25 @@ int CanBus_c::open()
 		/* try to switch the socket into CAN_FD mode */
 		if (::setsockopt(can_socket_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on)) < 0)
 		{
-			NOTICE(interface, "open_can_dev: Can not switch into CAN Extended frame format.");
-			is_fdmode_on = false;
+			NOTICE(interface_, "open_can_dev: Can not switch into CAN Extended frame format.");
+			is_fdmode_on_ = false;
 		} else {
-			is_fdmode_on = true;
+			is_fdmode_on_ = true;
 		}
 
 		/* Attempts to open a socket to CAN bus */
 		::strcpy(ifr.ifr_name, device);
 		if(ioctl(can_socket_, SIOCGIFINDEX, &ifr) < 0)
-			ERROR(interface, "open_can_dev: ioctl failed");
+			ERROR(interface_, "open_can_dev: ioctl failed");
 		else
 		{
 			txAddress.can_family = AF_CAN;
 			txAddress.can_ifindex = ifr.ifr_ifindex;
 
 			/* And bind it to txAddress */
-			if (::bind(can_socket_, (struct sockaddr *)&txAddress, sizeof(txAddress)) < 0)
+			if (::bind(can_socket_, (struct sockaddr *)&txAddress_, sizeof(txAddress_)) < 0)
 			{
-				ERROR(interface, "open_can_dev: bind failed");
+				ERROR(interface_, "open_can_dev: bind failed");
 			}
 			else
 			{
@@ -81,23 +81,70 @@ int CanBus_c::open()
 	return -1;
 }
 
-int CanBus_c::close()
+int CanBus_t::close()
 {
 	::close(can_socket_);
 	can_socket_ = -1;
 }
 
-void CanBus_c::start_threads()
+
+canfd_frame CanBus_t::can_read()
+{
+	ssize_t nbytes;
+	int maxdlen;
+	canfd_frame canfd_frame;
+
+	/* Test that socket is really opened */
+	if (can_socket_ < 0)
+	{
+		ERROR(interface_, "read_can: Socket unavailable. Closing thread.");
+		is_running_ = false;
+	}
+
+	nbytes = ::read(can_socket_, &canfd_frame, CANFD_MTU);
+
+	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");
+			::memset(&canfd_frame, 0, sizeof(canfd_frame));
+			break;
+	}
+	
+	return canfd_frame;
+}
+
+void CanBus_t::start_threads()
 {
 	th_reading_ = std::thread(can_reader, interface, socket, can_message_q_);
+	is_running_ = true;
+
 	th_decoding_ = std::thread(can_decoder, interface, can_message_q, can_message_q_);
 	th_pushing_ = std::thread(can_event_push, interface, can_message_q_);
 }
 
 /*
+ * Return is_running_ bool
+ */
+bool CanBus_t::is_running()
+{
+	return is_running_;
+}
+
+/*
  * Send a can message from a CanMessage_c object.
  */
-int CanBus_c::send_can_message(CanMessage_c can_msg)
+int CanBus_t::send_can_message(CanMessage_c &can_msg)
 {
 	int nbytes;
 	canfd_frame *f;
@@ -130,7 +177,7 @@ int CanBus_c::send_can_message(CanMessage_c can_msg)
  * 
  * Return the next queue element or NULL if queue is empty.
  */
-CanMessage_c* CanBus_c::next_can_message()
+CanMessage_c* CanBus_t::next_can_message()
 {
 	if(! can_message_q_.empty())
 	{
@@ -142,7 +189,7 @@ CanMessage_c* CanBus_c::next_can_message()
 	return nullptr;
 }
 
-void CanBus_c::insert_new_can_message(CanMessage_c *can_msg)
+void CanBus_t::insert_new_can_message(CanMessage_c &can_msg)
 {
 	can_message_q_.push(can_msg);
 }
@@ -153,7 +200,7 @@ void CanBus_c::insert_new_can_message(CanMessage_c *can_msg)
  * 
  * Return the next queue element or NULL if queue is empty.
  */
-openxc_VehicleMessage* CanBus_c::next_vehicle_message()
+openxc_VehicleMessage* CanBus_t::next_vehicle_message()
 {
 	if(! vehicle_message_q_.empty())
 	{
@@ -165,7 +212,7 @@ openxc_VehicleMessage* CanBus_c::next_vehicle_message()
 	return nullptr;
 }
 
-void CanBus_c::insert_new_vehicle_message(openxc_VehicleMessage *v_msg)
+void CanBus_t::insert_new_vehicle_message(openxc_VehicleMessage *v_msg)
 {
 	vehicle_message_q_.push(v_msg);
 }
diff --git a/src/can-utils.h b/src/can-utils.h
index 04303e32..3a6070ad 100644
--- a/src/can-utils.h
+++ b/src/can-utils.h
@@ -94,9 +94,10 @@ class CanBus_c {
 
 		int can_socket_;
 		bool is_fdmode_on_;
-		struct sockaddr_can txAddress;
+		struct sockaddr_can txAddress_;
 
 		std::thread th_reading_;
+		bool is_running_;
 		std::thread th_decoding_;
 		std::thread th_pushing_;
 
@@ -108,10 +109,12 @@ class CanBus_c {
 		int close();
 
 		void start_threads();
+		bool is_running();
+
 		int send_can_message(CanMessage_c can_msg);
 
 		CanMessage_c* next_can_message();
-		void insert_new_can_message(CanMessage_c *can_msg);
+		void insert_new_can_message(CanMessage_c &can_msg);
 
 		openxc_VehicleMessage* next_vehicle_message();
 		void insert_new_vehicle_message(openxc_VehicleMessage *v_msg);