static int raw_enable_errfilter(struct net_device *dev, struct sock *sk,
can_err_mask_t err_mask)
{
int err = 0;
if (err_mask)
err = can_rx_register(dev, 0, err_mask | CAN_ERR_FLAG,
raw_rcv, sk, "raw");
return err;
}
static void raw_add_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_register(dev, filter[i].can_id, filter[i].can_mask,
raw_rcv, sk, IDENT);
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;
}
static int raw_bind(struct socket *sock, struct sockaddr *uaddr, int len)
{
struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
struct sock *sk = sock->sk;
struct raw_opt *ro = raw_sk(sk);
struct net_device *dev;
DBG("socket %p to device %d\n", sock, addr->can_ifindex);
if (len < sizeof(*addr))
return -EINVAL;
if (ro->bound) {
#ifdef CAN_RAW_SUPPORT_REBIND
/* remove current bindings / notifier */
if (ro->ifindex) {
dev = dev_get_by_index(ro->ifindex);
if (!dev) {
DBG("could not find device %d\n",
addr->can_ifindex);
return -ENODEV;
}
if (!(dev->flags & IFF_UP)) {
sk->sk_err = ENETDOWN;
if (!sock_flag(sk, SOCK_DEAD))
sk->sk_error_report(sk);
goto out;
}
can_dev_unregister(dev, raw_notifier, sk);
} else
dev = NULL;
/* unregister current filters for this device */
raw_remove_filters(dev, sk);
if (dev)
dev_put(dev);
ro->bound = 0;
#else
return -EINVAL;
#endif
}
if (addr->can_ifindex) {
dev = dev_get_by_index(addr->can_ifindex);
if (!dev) {
DBG("could not find device %d\n", addr->can_ifindex);
return -ENODEV;
}
if (!(dev->flags & IFF_UP)) {
sk->sk_err = ENETDOWN;
if (!sock_flag(sk, SOCK_DEAD))
sk->sk_error_report(sk);
goto out;
}
can_dev_register(dev, raw_notifier, sk);
} else
dev = NULL;
ro->ifindex = addr->can_ifindex;
/* filters set by default/setsockopt */
raw_add_filters(dev, sk);
/* error frame filter set by setsockopt */
if (ro->err_mask)
can_rx_register(dev, 0, ro->err_mask | CAN_ERR_FLAG,
raw_rcv, sk, IDENT);
ro->bound = 1;
out:
if (dev)
dev_put(dev);
return 0;
}