From b756a7c3a9bae10966c6eab5a365e0a4f4b92cd1 Mon Sep 17 00:00:00 2001
From: xucheng5 <xucheng5@xiaomi.com>
Date: Thu, 17 Aug 2023 15:27:28 +0800
Subject: [PATCH] socketcan : support error frame filter

add error mask for CAN_RAW_ERR_FILTER

Signed-off-by: xucheng5 <xucheng5@xiaomi.com>
---
 net/can/can.h            |  3 +++
 net/can/can_getsockopt.c | 12 ++++++++++++
 net/can/can_recvmsg.c    | 10 ++++++++++
 net/can/can_setsockopt.c |  8 ++++++++
 4 files changed, 33 insertions(+)

diff --git a/net/can/can.h b/net/can/can.h
index 7c86473729..46de920dc8 100644
--- a/net/can/can.h
+++ b/net/can/can.h
@@ -106,6 +106,9 @@ struct can_conn_s
 #  endif
   struct can_filter filters[CONFIG_NET_CAN_RAW_FILTER_MAX];
   int32_t filter_count;
+#  ifdef CONFIG_NET_CAN_ERRORS
+  can_err_mask_t err_mask;
+#  endif
 #  ifdef CONFIG_NET_CAN_RAW_TX_DEADLINE
   int32_t tx_deadline;
 #  endif
diff --git a/net/can/can_getsockopt.c b/net/can/can_getsockopt.c
index 744c5b4c1f..a979c5098d 100644
--- a/net/can/can_getsockopt.c
+++ b/net/can/can_getsockopt.c
@@ -141,6 +141,18 @@ int can_getsockopt(FAR struct socket *psock, int level, int option,
         break;
 
       case CAN_RAW_ERR_FILTER:
+#ifdef CONFIG_NET_CAN_ERRORS
+        if (*value_len < sizeof(can_err_mask_t))
+          {
+            return -EINVAL;
+          }
+        else
+          {
+            FAR can_err_mask_t *mask = (FAR can_err_mask_t *)value;
+            *mask = conn->err_mask;
+            *value_len = sizeof(can_err_mask_t);
+          }
+#endif
         break;
 
       case CAN_RAW_LOOPBACK:
diff --git a/net/can/can_recvmsg.c b/net/can/can_recvmsg.c
index f9cc7f272a..7477734489 100644
--- a/net/can/can_recvmsg.c
+++ b/net/can/can_recvmsg.c
@@ -301,6 +301,16 @@ static inline int can_readahead(struct can_recvfrom_s *pstate)
 static int can_recv_filter(struct can_conn_s *conn, canid_t id)
 {
   uint32_t i;
+
+#ifdef CONFIG_NET_CAN_ERRORS
+  /* error message frame */
+
+  if (id & CAN_ERR_FLAG)
+    {
+      return id & conn->err_mask ? 1 : 0;
+    }
+#endif
+
   for (i = 0; i < conn->filter_count; i++)
     {
       if (conn->filters[i].can_id & CAN_INV_FILTER)
diff --git a/net/can/can_setsockopt.c b/net/can/can_setsockopt.c
index 8716dcf491..2b1ce233ac 100644
--- a/net/can/can_setsockopt.c
+++ b/net/can/can_setsockopt.c
@@ -153,6 +153,14 @@ int can_setsockopt(FAR struct socket *psock, int level, int option,
         break;
 
       case CAN_RAW_ERR_FILTER:
+#ifdef CONFIG_NET_CAN_ERRORS
+        if (value_len != sizeof(can_err_mask_t))
+          {
+            return -EINVAL;
+          }
+
+        conn->err_mask = *(FAR can_err_mask_t *)value & CAN_ERR_MASK;
+#endif
         break;
 
       case CAN_RAW_LOOPBACK: