can: introduce new raw socket option to join the given CAN filters

[mirror_ubuntu-bionic-kernel.git] / net / can / raw.c

diff --git a/net/can/raw.c b/net/can/raw.c
index 0c8d537b59b8d0b083ac0b855113f045f9042a9a..31b9748cbb4ec6c1caa9ddc9e3a80609077a81eb 100644 (file)
--- a/net/can/raw.c
+++ b/net/can/raw.c
@@ -77,6 +77,7 @@ MODULE_ALIAS("can-proto-1");
 struct uniqframe {
        ktime_t tstamp;
        const struct sk_buff *skb;
+       unsigned int join_rx_count;
 };
 
 struct raw_sock {
@@ -87,6 +88,7 @@ struct raw_sock {
        int loopback;
        int recv_own_msgs;
        int fd_frames;
+       int join_filters;
        int count;                 /* number of active filters */
        struct can_filter dfilter; /* default/single filter */
        struct can_filter *filter; /* pointer to filter(s) */
@@ -132,10 +134,21 @@ static void raw_rcv(struct sk_buff *oskb, void *data)
        /* eliminate multiple filter matches for the same skb */
        if (this_cpu_ptr(ro->uniq)->skb == oskb &&
            ktime_equal(this_cpu_ptr(ro->uniq)->tstamp, oskb->tstamp)) {
-               return;
+               if (ro->join_filters) {
+                       this_cpu_inc(ro->uniq->join_rx_count);
+                       /* drop frame until all enabled filters matched */
+                       if (this_cpu_ptr(ro->uniq)->join_rx_count < ro->count)
+                               return;
+               } else {
+                       return;
+               }
        } else {
                this_cpu_ptr(ro->uniq)->skb = oskb;
                this_cpu_ptr(ro->uniq)->tstamp = oskb->tstamp;
+               this_cpu_ptr(ro->uniq)->join_rx_count = 1;
+               /* drop first frame to check all enabled filters? */
+               if (ro->join_filters && ro->count > 1)
+                       return;
        }
 
        /* clone the given skb to be able to enqueue it into the rcv queue */
@@ -311,6 +324,7 @@ static int raw_init(struct sock *sk)
        ro->loopback         = 1;
        ro->recv_own_msgs    = 0;
        ro->fd_frames        = 0;
+       ro->join_filters     = 0;
 
        /* alloc_percpu provides zero'ed memory */
        ro->uniq = alloc_percpu(struct uniqframe);
@@ -604,6 +618,15 @@ static int raw_setsockopt(struct socket *sock, int level, int optname,
 
                break;
 
+       case CAN_RAW_JOIN_FILTERS:
+               if (optlen != sizeof(ro->join_filters))
+                       return -EINVAL;
+
+               if (copy_from_user(&ro->join_filters, optval, optlen))
+                       return -EFAULT;
+
+               break;
+
        default:
                return -ENOPROTOOPT;
        }
@@ -668,6 +691,12 @@ static int raw_getsockopt(struct socket *sock, int level, int optname,
                val = &ro->fd_frames;
                break;
 
+       case CAN_RAW_JOIN_FILTERS:
+               if (len > sizeof(int))
+                       len = sizeof(int);
+               val = &ro->join_filters;
+               break;
+
        default:
                return -ENOPROTOOPT;
        }