thirdparty/libcsp/src/drivers/can/can_socketcan.c

@@ -19,42 +19,46 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */
 
 /* SocketCAN driver */
-#include <csp/csp.h>
 #include <csp/drivers/can_socketcan.h>
-#include <csp/interfaces/csp_if_can.h>
+
+#include <stdint.h>
+#include <stdio.h>
+
+#include <stdlib.h>
+#include <unistd.h>
+#include <time.h>
+#include <string.h>
 #include <errno.h>
+
+#include <pthread.h>
+#include <semaphore.h>
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <sys/queue.h>
+#include <sys/uio.h>
+#include <sys/time.h>
+#include <net/if.h>
+
 #include <linux/can.h>
 #include <linux/can/raw.h>
 #include <linux/socket.h>
-#include <net/if.h>
-#include <pthread.h>
-#include <semaphore.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/ioctl.h>
-#include <sys/queue.h>
-#include <sys/socket.h>
-#include <sys/time.h>
-#include <sys/types.h>
-#include <sys/uio.h>
-#include <time.h>
-#include <unistd.h>
+
+#include <csp/csp.h>
+#include <csp/interfaces/csp_if_can.h>
 
 #ifdef CSP_HAVE_LIBSOCKETCAN
 #include <libsocketcan.h>
 #endif
 
-#define CAN_RX_TASK_PRIO 80
-
 static struct can_socketcan_s {
 	int socket;
 	csp_iface_t interface;
 } socketcan[1] = {
 	{
-        .interface =
-            {
+		.interface = {
 			.name = "CAN",
 			.nexthop = csp_can_tx,
 			.mtu = CSP_CAN_MTU,
@@ -63,7 +67,8 @@ static struct can_socketcan_s {
 	},
 };
 
-static void *socketcan_rx_thread(void *parameters) {
+static void * socketcan_rx_thread(void * parameters)
+{
 	struct can_frame frame;
 	int nbytes;
 
@@ -98,41 +103,21 @@ static void *socketcan_rx_thread(void *parameters) {
 	pthread_exit(NULL);
 }
 
-static int create_receive_thread() {
-  pthread_t rx_thread;
-  pthread_attr_t attributes;
-  if (pthread_attr_init(&attributes) != 0) {
-    return 1;
-  }
-  if (pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED) != 0) {
-    return 1;
-  }
-  if (pthread_attr_setschedpolicy(&attributes, SCHED_FIFO) != 0) {
-    return 1;
-  }
-  struct sched_param schedule_params;
-  schedule_params.__sched_priority = CAN_RX_TASK_PRIO;
-  if (pthread_attr_setschedparam(&attributes, &schedule_params) != 0) {
-    return 1;
-  }
-  if (pthread_create(&rx_thread, NULL, socketcan_rx_thread, NULL) != 0) {
-    csp_log_error("pthread_create: %s", strerror(errno));
-    return 1;
-  }
-  return 0;
-}
 
-int csp_can_tx_frame(csp_iface_t *interface, uint32_t id, const uint8_t *data, uint8_t dlc) {
+int csp_can_tx_frame(csp_iface_t *interface, uint32_t id, const uint8_t * data, uint8_t dlc)
+{
 	struct can_frame frame;
 	int i, tries = 0;
 	memset(&frame, 0, sizeof(frame));
-  if (dlc > 8) return -1;
+	if (dlc > 8)
+		return -1;
 
 	/* Copy identifier */
 	frame.can_id = id | CAN_EFF_FLAG;
 
 	/* Copy data to frame */
-  for (i = 0; i < dlc; i++) frame.data[i] = data[i];
+	for (i = 0; i < dlc; i++)
+		frame.data[i] = data[i];
 
 	/* Set DLC */
 	frame.can_dlc = dlc;
@@ -151,9 +136,11 @@ int csp_can_tx_frame(csp_iface_t *interface, uint32_t id, const uint8_t *data, u
 	return 0;
 }
 
-csp_iface_t *csp_can_socketcan_init(const char *ifc, int bitrate, int promisc) {
+csp_iface_t * csp_can_socketcan_init(const char * ifc, int bitrate, int promisc)
+{
 	struct ifreq ifr;
 	struct sockaddr_can addr;
+	pthread_t rx_thread;
 
 	//printf("-I-: Initiating CAN interface %s\n", ifc);
 
@@ -190,6 +177,7 @@ csp_iface_t *csp_can_socketcan_init(const char *ifc, int bitrate, int promisc) {
 
 	/* Set filter mode */
 	if (promisc == 0) {
+
 		struct can_filter filter;
 		filter.can_id = CFP_MAKE_DST(csp_get_address());
 		filter.can_mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1);
@@ -198,9 +186,12 @@ csp_iface_t *csp_can_socketcan_init(const char *ifc, int bitrate, int promisc) {
 			csp_log_error("setsockopt: %s", strerror(errno));
 			return NULL;
 		}
+
 	}
 
-  if (create_receive_thread() != 0) {
+	/* Create receive thread */
+	if (pthread_create(&rx_thread, NULL, socketcan_rx_thread, NULL) != 0) {
+		csp_log_error("pthread_create: %s", strerror(errno));
 		return NULL;
 	}