static void raw_disable_filters(struct net_device *dev, struct sock *sk,
struct can_filter *filter, int count)
{
int i;
for (i = 0; i < count; i++)
can_rx_unregister(dev, filter[i].can_id, filter[i].can_mask,
raw_rcv, sk);
}
static void raw_remove_filters(struct net_device *dev, struct sock *sk)
{
struct raw_opt *ro = raw_sk(sk);
struct can_filter *filter = ro->filter;
int i;
for (i = 0; i < ro->count; i++) {
can_rx_unregister(dev, filter[i].can_id, filter[i].can_mask,
raw_rcv, sk);
DBG("filter can_id %08X, can_mask %08X%s, sk %p\n",
filter[i].can_id, filter[i].can_mask,
filter[i].can_id & CAN_INV_FILTER ? " (inv)" : "", sk);
}
}
static int raw_enable_filters(struct net_device *dev, struct sock *sk,
struct can_filter *filter, int count)
{
int err = 0;
int i;
for (i = 0; i < count; i++) {
err = can_rx_register(dev, filter[i].can_id,
filter[i].can_mask,
raw_rcv, sk, "raw");
if (err) {
/* clean up successfully registered filters */
while (--i >= 0)
can_rx_unregister(dev, filter[i].can_id,
filter[i].can_mask,
raw_rcv, sk);
break;
}
}
return err;
}
static int raw_setsockopt(struct socket *sock, int level, int optname,
char __user *optval, int optlen)
{
struct sock *sk = sock->sk;
struct raw_opt *ro = raw_sk(sk);
struct can_filter *filter = NULL; /* dyn. alloc'ed filters */
struct can_filter sfilter; /* single filter */
struct net_device *dev = NULL;
can_err_mask_t err_mask = 0;
int count = 0;
int err;
if (level != SOL_CAN_RAW)
return -EINVAL;
switch (optname) {
case CAN_RAW_FILTER:
if (optlen % sizeof(struct can_filter) != 0)
return -EINVAL;
count = optlen / sizeof(struct can_filter);
if (count > 1) {
/* filter does not fit into dfilter => alloc space */
filter = kmalloc(optlen, GFP_KERNEL);
if (!filter)
return -ENOMEM;
err = copy_from_user(filter, optval, optlen);
if (err) {
kfree(filter);
return err;
}
} else if (count == 1) {
err = copy_from_user(&sfilter, optval, optlen);
if (err)
return err;
}
if (ro->bound && ro->ifindex)
dev = dev_get_by_index(ro->ifindex);
/* remove current filters & unregister */
if (ro->bound)
raw_remove_filters(dev, sk);
if (ro->count > 1)
kfree(ro->filter);
if (count == 1) {
/* copy filter data for single filter */
ro->dfilter = sfilter;
filter = &ro->dfilter;
}
/* add new filters & register */
ro->filter = filter;
ro->count = count;
if (ro->bound)
raw_add_filters(dev, sk);
if (dev)
dev_put(dev);
break;
case CAN_RAW_ERR_FILTER:
if (optlen != sizeof(err_mask))
return -EINVAL;
err = copy_from_user(&err_mask, optval, optlen);
if (err)
return err;
err_mask &= CAN_ERR_MASK;
if (ro->bound && ro->ifindex)
dev = dev_get_by_index(ro->ifindex);
/* remove current error mask */
if (ro->err_mask && ro->bound)
can_rx_unregister(dev, 0, ro->err_mask | CAN_ERR_FLAG,
raw_rcv, sk);
/* add new error mask */
ro->err_mask = err_mask;
if (ro->err_mask && ro->bound)
can_rx_register(dev, 0, ro->err_mask | CAN_ERR_FLAG,
raw_rcv, sk, IDENT);
if (dev)
dev_put(dev);
break;
case CAN_RAW_LOOPBACK:
if (optlen != sizeof(ro->loopback))
return -EINVAL;
err = copy_from_user(&ro->loopback, optval, optlen);
if (err)
return err;
break;
case CAN_RAW_RECV_OWN_MSGS:
if (optlen != sizeof(ro->recv_own_msgs))
return -EINVAL;
err = copy_from_user(&ro->recv_own_msgs, optval, optlen);
if (err)
return err;
break;
default:
return -ENOPROTOOPT;
}
return 0;
}
