#include <linux/list.h>
#include <linux/spinlock.h>
#include <linux/rcupdate.h>
+#include <linux/rculist.h>
#include <linux/net.h>
#include <linux/netdevice.h>
#include <linux/if_arp.h>
#include <socketcan/can/version.h> /* for RCSID. Removed by mkpatch script */
RCSID("$Id$");
-#define CAN_GW_VERSION "20100218"
+#define CAN_GW_VERSION "20100222"
static __initdata const char banner[] =
KERN_INFO "can: netlink gateway (rev " CAN_GW_VERSION ")\n";
struct can_frame xor;
struct can_frame set;
} modframe;
+ struct {
+ u8 and;
+ u8 or;
+ u8 xor;
+ u8 set;
+ } modtype;
void (*modfunc[MAX_MODFUNCTIONS])(struct can_frame *cf,
struct can_can_gw *mod);
};
};
/* content of u32 gwjob.flags */
-#define CAN_TX_LOOPBACK 0x00000001
+#define CAN_TX_ECHO 0x00000001
+#define CAN_TX_SRC_TSTAMP 0x00000002
/* modification functions that are invoked in the hot path in gw_rcv */
void mod_and_id (struct can_frame *cf, struct can_can_gw *mod) {
*(u64 *)cf->data = *(u64 *)mod->modframe.set.data;
}
+static inline void canframecpy(struct can_frame *dst, struct can_frame *src)
+{
+ /*
+ * Copy the struct members separately to ensure that no uninitialized
+ * data are copied in the 3 bytes hole of the struct. This is needed
+ * to make easy compares of the data in the struct can_can_gw.
+ */
+
+ dst->can_id = src->can_id;
+ dst->can_dlc = src->can_dlc;
+ *(u64 *)dst->data = *(u64 *)src->data;
+}
/* the receive & process & send function */
static void gw_rcv(struct sk_buff *skb, void *data)
if (skb->sk == GW_SK_MAGIC)
return;
- if (!netif_running(gwj->dst_dev)) {
+ if (!(gwj->dst_dev->flags & IFF_UP)) {
gwj->dropped_frames++;
return;
}
/*
- * clone the given skb, which has not been done in can_rv()
+ * clone the given skb, which has not been done in can_rcv()
*
* When there is at least one modification function activated,
* we need to copy the skb as we want to modify skb->data.
while (modidx < MAX_MODFUNCTIONS && gwj->ccgw.modfunc[modidx])
(*gwj->ccgw.modfunc[modidx++])(cf, &gwj->ccgw);
+ /* clear the skb timestamp if not configured the other way */
+ if (!(gwj->flags & CAN_TX_SRC_TSTAMP))
+ nskb->tstamp.tv64 = 0;
+
/* send to netdevice */
- if (can_send(nskb, gwj->flags & CAN_TX_LOOPBACK))
+ if (can_send(nskb, gwj->flags & CAN_TX_ECHO))
gwj->dropped_frames++;
else
gwj->handled_frames++;
return NOTIFY_DONE;
}
-/*
- * Dump information about all ports, in response to GETROUTE
- */
-static int gw_dump_jobs(struct sk_buff *skb, struct netlink_callback *cb)
+static int gw_put_job(struct sk_buff *skb, struct gw_job *gwj)
{
- printk(KERN_INFO "%s (TODO)\n", __FUNCTION__);
-
- return 0;
-}
-
-static int gw_create_job(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
-{
-
- struct rtcanmsg *r;
- struct nlattr *tb[CGW_MAX+1];
- struct gw_job *gwj;
- u8 buf[CGW_MODATTR_LEN];
- int modidx = 0;
- int err = 0;
-
- printk(KERN_INFO "%s: len %d attrlen %d\n", __FUNCTION__,
- nlmsg_len(nlh), nlmsg_attrlen(nlh, sizeof(*r)));
+ struct {
+ struct can_frame cf;
+ u8 modtype;
+ } __attribute__((packed)) mb;
+
+ struct rtcanmsg *rtcan;
+ struct nlmsghdr *nlh = nlmsg_put(skb, 0, 0, 0, sizeof(*rtcan), 0);
+ if (!nlh)
+ return -EMSGSIZE;
+
+ rtcan = nlmsg_data(nlh);
+ rtcan->can_family = AF_CAN;
+ rtcan->src_ifindex = gwj->src_dev->ifindex;
+ rtcan->dst_ifindex = gwj->dst_dev->ifindex;
+ rtcan->can_txflags = 0;
+
+ if (gwj->flags & CAN_TX_ECHO)
+ rtcan->can_txflags |= CAN_GW_TXFLAGS_ECHO;
+
+ if (gwj->flags & CAN_TX_SRC_TSTAMP)
+ rtcan->can_txflags |= CAN_GW_TXFLAGS_SRC_TSTAMP;
+
+ /* check non default settings of attributes */
+ if (gwj->handled_frames) {
+ if (nla_put_u32(skb, CGW_HANDLED, gwj->handled_frames) < 0)
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
+ }
- if (nlmsg_len(nlh) < sizeof(*r))
- return -EINVAL;
+ if (gwj->dropped_frames) {
+ if (nla_put_u32(skb, CGW_DROPPED, gwj->dropped_frames) < 0)
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(u32));
+ }
- r = nlmsg_data(nlh);
- if (r->can_family != AF_CAN)
- return -EPFNOSUPPORT;
+ if (gwj->ccgw.filter.can_id || gwj->ccgw.filter.can_mask) {
+ if (nla_put(skb, CGW_FILTER, sizeof(struct can_filter),
+ &gwj->ccgw.filter) < 0)
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN +
+ NLA_ALIGN(sizeof(struct can_filter));
+ }
- gwj = kmem_cache_alloc(gw_cache, GFP_KERNEL);
- if (!gwj)
- return -ENOMEM;
+ if (gwj->ccgw.modtype.and) {
+ memcpy(&mb.cf, &gwj->ccgw.modframe.and, sizeof(mb.cf));
+ mb.modtype = gwj->ccgw.modtype.and;
+ if (nla_put(skb, CGW_MOD_AND, sizeof(mb), &mb) < 0)
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
+ }
- gwj->src_dev = dev_get_by_index(&init_net, r->src_ifindex);
- if (!gwj->src_dev) {
- err = -ENODEV;
- goto fail;
+ if (gwj->ccgw.modtype.or) {
+ memcpy(&mb.cf, &gwj->ccgw.modframe.or, sizeof(mb.cf));
+ mb.modtype = gwj->ccgw.modtype.or;
+ if (nla_put(skb, CGW_MOD_OR, sizeof(mb), &mb) < 0)
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
}
- /* for now the source device needs to be a CAN device */
- if (gwj->src_dev->type != ARPHRD_CAN) {
- err = -ENODEV;
- goto put_src_fail;
+ if (gwj->ccgw.modtype.xor) {
+ memcpy(&mb.cf, &gwj->ccgw.modframe.xor, sizeof(mb.cf));
+ mb.modtype = gwj->ccgw.modtype.xor;
+ if (nla_put(skb, CGW_MOD_XOR, sizeof(mb), &mb) < 0)
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
}
- gwj->dst_dev = dev_get_by_index(&init_net, r->dst_ifindex);
- if (!gwj->dst_dev) {
- err = -ENODEV;
- goto put_src_fail;
+ if (gwj->ccgw.modtype.set) {
+ memcpy(&mb.cf, &gwj->ccgw.modframe.set, sizeof(mb.cf));
+ mb.modtype = gwj->ccgw.modtype.set;
+ if (nla_put(skb, CGW_MOD_SET, sizeof(mb), &mb) < 0)
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(mb));
}
- /* for now the destination device needs to be a CAN device */
- if (gwj->dst_dev->type != ARPHRD_CAN) {
- err = -ENODEV;
- goto put_src_dst_fail;
+ return skb->len;
+
+cancel:
+ nlmsg_cancel(skb, nlh);
+ return -EMSGSIZE;
+}
+
+/* Dump information about all CAN gateway jobs, in response to RTM_GETROUTE */
+static int gw_dump_jobs(struct sk_buff *skb, struct netlink_callback *cb)
+{
+ struct gw_job *gwj = NULL;
+ struct hlist_node *n;
+ int idx = 0;
+ int ret = 0;
+
+ rcu_read_lock();
+ hlist_for_each_entry_rcu(gwj, n, &can_gw_list, list) {
+ if (idx >= cb->args[0]) {
+ ret = gw_put_job(skb, gwj);
+ if (ret > 0)
+ cb->args[0]++;
+ break;
+ }
+ idx++;
}
+ rcu_read_unlock();
- gwj->flags = 0;
+ return ret;
+}
- if (r->can_txflags & CAN_GW_TXFLAGS_LOOPBACK)
- gwj->flags |= CAN_TX_LOOPBACK;
+/* check for attributes / filters for the CAN->CAN gateway */
+static int can_can_parse_attr(struct nlmsghdr *nlh, struct can_can_gw *ccgw)
+{
+ struct nlattr *tb[CGW_MAX+1];
+ int modidx = 0;
+ int err = 0;
+
+ struct {
+ struct can_frame cf;
+ u8 modtype;
+ } __attribute__((packed)) mb;
- memset(&gwj->ccgw, 0, sizeof(gwj->ccgw));
+ BUILD_BUG_ON(sizeof(mb) != CGW_MODATTR_LEN);
- /* check for additional attributes / filters here */
+ memset(ccgw, 0, sizeof(*ccgw));
- err = nlmsg_parse(nlh, sizeof(*r), tb, CGW_MAX, NULL);
+ err = nlmsg_parse(nlh, sizeof(struct rtcanmsg), tb, CGW_MAX, NULL);
if (err < 0)
- goto put_src_dst_fail;
+ return err;
/* check for can_filter in attributes */
if (tb[CGW_FILTER] &&
nla_len(tb[CGW_FILTER]) == sizeof(struct can_filter))
- nla_memcpy(&gwj->ccgw.filter, tb[CGW_FILTER],
+ nla_memcpy(&ccgw->filter, tb[CGW_FILTER],
sizeof(struct can_filter));
/* check for AND/OR/XOR/SET modifications */
if (tb[CGW_MOD_AND] &&
nla_len(tb[CGW_MOD_AND]) == CGW_MODATTR_LEN) {
- nla_memcpy(&buf, tb[CGW_MOD_AND], CGW_MODATTR_LEN);
+ nla_memcpy(&mb, tb[CGW_MOD_AND], CGW_MODATTR_LEN);
- memcpy(&gwj->ccgw.modframe.and, &buf[1],
- sizeof(struct can_frame));
+ canframecpy(&ccgw->modframe.and, &mb.cf);
+ ccgw->modtype.and = mb.modtype;
- if (buf[0] & CGW_MOD_ID)
- gwj->ccgw.modfunc[modidx++] = mod_and_id;
+ if (mb.modtype & CGW_MOD_ID)
+ ccgw->modfunc[modidx++] = mod_and_id;
- if (buf[0] & CGW_MOD_DLC)
- gwj->ccgw.modfunc[modidx++] = mod_and_dlc;
+ if (mb.modtype & CGW_MOD_DLC)
+ ccgw->modfunc[modidx++] = mod_and_dlc;
- if (buf[0] & CGW_MOD_DATA)
- gwj->ccgw.modfunc[modidx++] = mod_and_data;
+ if (mb.modtype & CGW_MOD_DATA)
+ ccgw->modfunc[modidx++] = mod_and_data;
}
if (tb[CGW_MOD_OR] &&
nla_len(tb[CGW_MOD_OR]) == CGW_MODATTR_LEN) {
- nla_memcpy(&buf, tb[CGW_MOD_OR], CGW_MODATTR_LEN);
+ nla_memcpy(&mb, tb[CGW_MOD_OR], CGW_MODATTR_LEN);
- memcpy(&gwj->ccgw.modframe.or, &buf[1],
- sizeof(struct can_frame));
+ canframecpy(&ccgw->modframe.or, &mb.cf);
+ ccgw->modtype.or = mb.modtype;
- if (buf[0] & CGW_MOD_ID)
- gwj->ccgw.modfunc[modidx++] = mod_or_id;
+ if (mb.modtype & CGW_MOD_ID)
+ ccgw->modfunc[modidx++] = mod_or_id;
- if (buf[0] & CGW_MOD_DLC)
- gwj->ccgw.modfunc[modidx++] = mod_or_dlc;
+ if (mb.modtype & CGW_MOD_DLC)
+ ccgw->modfunc[modidx++] = mod_or_dlc;
- if (buf[0] & CGW_MOD_DATA)
- gwj->ccgw.modfunc[modidx++] = mod_or_data;
+ if (mb.modtype & CGW_MOD_DATA)
+ ccgw->modfunc[modidx++] = mod_or_data;
}
if (tb[CGW_MOD_XOR] &&
nla_len(tb[CGW_MOD_XOR]) == CGW_MODATTR_LEN) {
- nla_memcpy(&buf, tb[CGW_MOD_XOR], CGW_MODATTR_LEN);
+ nla_memcpy(&mb, tb[CGW_MOD_XOR], CGW_MODATTR_LEN);
- memcpy(&gwj->ccgw.modframe.xor, &buf[1],
- sizeof(struct can_frame));
+ canframecpy(&ccgw->modframe.xor, &mb.cf);
+ ccgw->modtype.xor = mb.modtype;
- if (buf[0] & CGW_MOD_ID)
- gwj->ccgw.modfunc[modidx++] = mod_xor_id;
+ if (mb.modtype & CGW_MOD_ID)
+ ccgw->modfunc[modidx++] = mod_xor_id;
- if (buf[0] & CGW_MOD_DLC)
- gwj->ccgw.modfunc[modidx++] = mod_xor_dlc;
+ if (mb.modtype & CGW_MOD_DLC)
+ ccgw->modfunc[modidx++] = mod_xor_dlc;
- if (buf[0] & CGW_MOD_DATA)
- gwj->ccgw.modfunc[modidx++] = mod_xor_data;
+ if (mb.modtype & CGW_MOD_DATA)
+ ccgw->modfunc[modidx++] = mod_xor_data;
}
if (tb[CGW_MOD_SET] &&
nla_len(tb[CGW_MOD_SET]) == CGW_MODATTR_LEN) {
- nla_memcpy(&buf, tb[CGW_MOD_SET], CGW_MODATTR_LEN);
+ nla_memcpy(&mb, tb[CGW_MOD_SET], CGW_MODATTR_LEN);
- memcpy(&gwj->ccgw.modframe.set, &buf[1],
- sizeof(struct can_frame));
+ canframecpy(&ccgw->modframe.set, &mb.cf);
+ ccgw->modtype.set = mb.modtype;
- if (buf[0] & CGW_MOD_ID)
- gwj->ccgw.modfunc[modidx++] = mod_set_id;
+ if (mb.modtype & CGW_MOD_ID)
+ ccgw->modfunc[modidx++] = mod_set_id;
- if (buf[0] & CGW_MOD_DLC)
- gwj->ccgw.modfunc[modidx++] = mod_set_dlc;
+ if (mb.modtype & CGW_MOD_DLC)
+ ccgw->modfunc[modidx++] = mod_set_dlc;
- if (buf[0] & CGW_MOD_DATA)
- gwj->ccgw.modfunc[modidx++] = mod_set_data;
+ if (mb.modtype & CGW_MOD_DATA)
+ ccgw->modfunc[modidx++] = mod_set_data;
}
+ return 0;
+}
+
+static int gw_create_job(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
+{
+ struct rtcanmsg *r;
+ struct gw_job *gwj;
+ int err = 0;
+
+ if (nlmsg_len(nlh) < sizeof(*r))
+ return -EINVAL;
+
+ r = nlmsg_data(nlh);
+ if (r->can_family != AF_CAN)
+ return -EPFNOSUPPORT;
+
+ gwj = kmem_cache_alloc(gw_cache, GFP_KERNEL);
+ if (!gwj)
+ return -ENOMEM;
+
+ gwj->src_dev = dev_get_by_index(&init_net, r->src_ifindex);
+ if (!gwj->src_dev) {
+ err = -ENODEV;
+ goto fail;
+ }
+
+ /* for now the source device needs to be a CAN device */
+ if (gwj->src_dev->type != ARPHRD_CAN) {
+ err = -ENODEV;
+ goto put_src_fail;
+ }
+
+ gwj->dst_dev = dev_get_by_index(&init_net, r->dst_ifindex);
+ if (!gwj->dst_dev) {
+ err = -ENODEV;
+ goto put_src_fail;
+ }
+
+ /* for now the destination device needs to be a CAN device */
+ if (gwj->dst_dev->type != ARPHRD_CAN) {
+ err = -ENODEV;
+ goto put_src_dst_fail;
+ }
+
+ gwj->handled_frames = 0;
+ gwj->dropped_frames = 0;
+ gwj->flags = 0;
+
+ if (r->can_txflags & CAN_GW_TXFLAGS_ECHO)
+ gwj->flags |= CAN_TX_ECHO;
+
+ if (r->can_txflags & CAN_GW_TXFLAGS_SRC_TSTAMP)
+ gwj->flags |= CAN_TX_SRC_TSTAMP;
+
+ err = can_can_parse_attr(nlh, &gwj->ccgw);
+ if (err < 0)
+ goto put_src_dst_fail;
+
spin_lock(&can_gw_list_lock);
err = can_gw_register_filter(gwj);
return err;
}
+static void gw_remove_all_jobs(void)
+{
+ struct gw_job *gwj = NULL;
+ struct hlist_node *n, *nx;
+
+ spin_lock(&can_gw_list_lock);
+
+ hlist_for_each_entry_safe(gwj, n, nx, &can_gw_list, list) {
+ hlist_del(&gwj->list);
+ can_gw_unregister_filter(gwj);
+ kfree(gwj);
+ }
+
+ spin_unlock(&can_gw_list_lock);
+}
+
static int gw_remove_job(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
{
- printk(KERN_INFO "%s (TODO)\n", __FUNCTION__);
+ struct gw_job *gwj = NULL;
+ struct hlist_node *n, *nx;
+ struct rtcanmsg *r;
+ struct can_can_gw ccgw;
+ u32 flags = 0;
+ int err = 0;
- return 0;
+ if (nlmsg_len(nlh) < sizeof(*r))
+ return -EINVAL;
+
+ r = nlmsg_data(nlh);
+ if (r->can_family != AF_CAN)
+ return -EPFNOSUPPORT;
+
+ /* if_index set to 0 => remove all entries */
+ if (!r->src_ifindex && !r->dst_ifindex) {
+ gw_remove_all_jobs();
+ return 0;
+ }
+
+ if (r->can_txflags & CAN_GW_TXFLAGS_ECHO)
+ flags |= CAN_TX_ECHO;
+
+ if (r->can_txflags & CAN_GW_TXFLAGS_SRC_TSTAMP)
+ flags |= CAN_TX_SRC_TSTAMP;
+
+ err = can_can_parse_attr(nlh, &ccgw);
+ if (err < 0)
+ return err;
+
+ err = -EINVAL;
+
+ spin_lock(&can_gw_list_lock);
+
+ /* remove only the first matching entry */
+ hlist_for_each_entry_safe(gwj, n, nx, &can_gw_list, list) {
+
+ if (gwj->dst_dev->ifindex != r->dst_ifindex)
+ continue;
+
+ if (gwj->src_dev->ifindex != r->src_ifindex)
+ continue;
+
+ if (gwj->flags != flags)
+ continue;
+
+ if (memcmp(&gwj->ccgw, &ccgw, sizeof(ccgw)))
+ continue;
+
+ hlist_del(&gwj->list);
+ can_gw_unregister_filter(gwj);
+ kfree(gwj);
+ err = 0;
+ break;
+ }
+
+ spin_unlock(&can_gw_list_lock);
+
+ return err;
}
static __init int gw_module_init(void)
static __exit void gw_module_exit(void)
{
- struct gw_job *gwj = NULL;
- struct hlist_node *n, *nx;
-
rtnl_unregister_all(PF_CAN);
unregister_netdevice_notifier(¬ifier);
- spin_lock(&can_gw_list_lock);
-
- hlist_for_each_entry_safe(gwj, n, nx, &can_gw_list, list) {
- hlist_del(&gwj->list);
- can_gw_unregister_filter(gwj);
- kfree(gwj);
- }
-
- spin_unlock(&can_gw_list_lock);
+ gw_remove_all_jobs();
rcu_barrier(); /* Wait for completion of call_rcu()'s */
projects / socketcan-devel.git / blobdiff