From: Rostislav Lisovy Date: Fri, 5 Aug 2011 15:50:39 +0000 (+0200) Subject: Seems to work correctly. Is able of getting valid Netlink message from userspace... X-Git-Url: https://rtime.felk.cvut.cz/gitweb/lisovros/linux_canprio.git/commitdiff_plain/1771abd29589f4b4599a5014587d5d5078b81ef2 Seems to work correctly. Is able of getting valid Netlink message from userspace (i.e. Can ID) --- diff --git a/net/sched/Makefile b/net/sched/Makefile index f14e71bfa58..d64988dcc22 100644 --- a/net/sched/Makefile +++ b/net/sched/Makefile @@ -38,6 +38,7 @@ obj-$(CONFIG_NET_CLS_RSVP) += cls_rsvp.o obj-$(CONFIG_NET_CLS_TCINDEX) += cls_tcindex.o obj-$(CONFIG_NET_CLS_RSVP6) += cls_rsvp6.o obj-$(CONFIG_NET_CLS_BASIC) += cls_basic.o +obj-$(CONFIG_NET_CLS_BASIC) += cls_canprio.o #CANPRIO obj-$(CONFIG_NET_CLS_FLOW) += cls_flow.o obj-$(CONFIG_NET_CLS_CGROUP) += cls_cgroup.o obj-$(CONFIG_NET_EMATCH) += ematch.o diff --git a/net/sched/cls_canprio.c b/net/sched/cls_canprio.c new file mode 100644 index 00000000000..9c3b9943a39 --- /dev/null +++ b/net/sched/cls_canprio.c @@ -0,0 +1,323 @@ +/* cls_canprio.c -- Canprio classifier. + * Makes decisions accoring to Can IDs. + * + * Implementation notes; + * parameter of functions named "base" is pointer to some parent element + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/* Definition of Netlink messages */ +enum { + TCA_CANPRIO_A_UNSPEC, + TCA_CANPRIO_CLASSID, + TCA_CANPRIO_MATCH, + TCA_CANPRIO_XY, + __TCA_CANPRIO_MAX, +}; +#define TCA_CANPRIO_MAX (__TCA_CANPRIO_MAX - 1) + +static const struct nla_policy canprio_policy[TCA_CANPRIO_MAX + 1] = { + [TCA_CANPRIO_CLASSID] = { .type = NLA_U32 }, + [TCA_CANPRIO_MATCH] = { .type = NLA_U32 }, //{ .type = NLA_NESTED }, + [TCA_CANPRIO_XY] = { .type = NLA_U32 }, +}; + +struct canprio_head { + u32 hgenerator; + struct list_head flist; +}; + +struct canprio_filter { + u32 handle; + u32 match; // Matching CAN ID. Will be sth like list os lists + struct tcf_result res; + struct list_head link; +}; + +/* + * ---------------------------------------------------------------------------- + */ + +/* + * Extracts Can ID ot ouf the sk_buff structure. + */ +static u32 canprio_get_id(struct sk_buff *skb) +{ + /* Can ID is inside of data field */ + struct can_frame *cf = (struct can_frame *)skb->data; + //canid_t can_id = cf->can_id; + + return (u32)cf->can_id; +} + +/* + * Performs the classification. Iterates over all instances of filter + * checking for Can ID match. + * + * @skb: Socket buffer + * @tp: + * @res: Is used for setting Class ID as a result of classification + * + * Returns value relevant for policing. Used return values: + * TC_POLICE_OK if succesfully classified (without regard to policing rules) + * TC_POLICE_UNSPEC if no matching filter was found + */ +static int canprio_classify(struct sk_buff *skb, struct tcf_proto *tp, + struct tcf_result *res) +{ + struct canprio_head *head = (struct canprio_head *)tp->root; + struct canprio_filter *f; + + printk(" canprio_classify invoked\n"); + + list_for_each_entry(f, &head->flist, link) { + if (canprio_get_id(skb) == 123) { // Should match value given by user + *res = f->res; + return TC_POLICE_OK; + } + } + + return TC_POLICE_UNSPEC; +} + +/* + * Configure filter + */ +static int canprio_set_parms(struct tcf_proto *tp, struct canprio_filter *f, + unsigned long base, struct nlattr **tb, + struct nlattr *est) +{ + //int err; + + if (tb[TCA_CANPRIO_CLASSID]) { + f->res.classid = nla_get_u32(tb[TCA_U32_CLASSID]); + tcf_bind_filter(tp, &f->res, base); + } + + return 0; +} + +/* + * Looks up a filter element by its handle and returns the internal filter ID + */ +static unsigned long canprio_get(struct tcf_proto *tp, u32 handle) +{ + unsigned long l = 0UL; + struct canprio_head *head = (struct canprio_head *) tp->root; + struct canprio_filter *f; + + if (head == NULL) + return 0UL; + + list_for_each_entry(f, &head->flist, link) + if (f->handle == handle) + l = (unsigned long) f; + + return l; +} + +/* + * Is invoked when a filter element previously referenced + * with get() is no longer used + */ +static void canprio_put(struct tcf_proto *tp, unsigned long f) +{ +} + + +/* + * Called for changing properties of an existing filter or after addition + * of a new filter to a class (by calling bind_tcf which binds an instance + * of a filter to the class). + * + * @tp: Structure representing instance of a filter. + * Part of a linked list of all filters. + * @base: + * @handle: + * @tca: Messages passed through the Netlink from userspace. + * @arg: ??? FIXME + */ +static int canprio_change(struct tcf_proto *tp, unsigned long base, u32 handle, + struct nlattr **tca, unsigned long *arg) +{ + int err; + struct canprio_head *head = (struct canprio_head *)tp->root; + struct canprio_filter *f = (struct canprio_filter *)*arg; + struct nlattr *tb[TCA_CANPRIO_MAX + 1]; + + printk(" canprio_change invoked\n"); + + if (tca[TCA_OPTIONS] == NULL) + return -EINVAL; + + err = nla_parse_nested(tb, TCA_CANPRIO_MAX, tca[TCA_OPTIONS], canprio_policy); + if (err < 0) + return err; + + //Change existing filter (?) + if (f != NULL) { + if (handle && f->handle != handle) + return -EINVAL; + return canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]); + } + + //Create new filter (?) + err = -ENOBUFS; + f = kzalloc(sizeof(*f), GFP_KERNEL); + if (f == NULL) + goto errout; + + + //FIXME why? + err = -EINVAL; + if (handle) + f->handle = handle; + else { + //FIXME wat? + unsigned int i = 0x80000000; + do { + if (++head->hgenerator == 0x7FFFFFFF) + head->hgenerator = 1; + } while (--i > 0 && canprio_get(tp, head->hgenerator)); + + if (i <= 0) { + pr_err("Insufficient number of handles\n"); + goto errout; + } + + f->handle = head->hgenerator; + } + + + //Configure newly created filter + err = canprio_set_parms(tp, f, base, tb, tca[TCA_RATE]); + if (err < 0) + goto errout; + + + //Parse arguments + if (tb[TCA_CANPRIO_MATCH] == NULL) + return -EINVAL; + + //f->match = nla_data(tb[TCA_CANPRIO_MATCH]); + f->match = nla_get_u32(tb[TCA_CANPRIO_MATCH]); + printk(" match = %d\n", f->match); + + //Add newly created filter to list of all filters + tcf_tree_lock(tp); + list_add(&f->link, &head->flist); + tcf_tree_unlock(tp); + *arg = (unsigned long) f; + + return 0; + +errout: + if (*arg == 0UL && f) //FIXME why 0UL? + kfree(f); + + return err; +} + + +static void canprio_delete_filter(struct tcf_proto *tp, struct canprio_filter *f) +{ + tcf_unbind_filter(tp, &f->res); + //tcf_exts_destroy(tp, &f->exts); + //tcf_em_tree_destroy(tp, &f->ematches); + kfree(f); +} + +/* + * Remove whole filter. + */ +static void canprio_destroy(struct tcf_proto *tp) +{ + struct canprio_head *head = tp->root; + struct canprio_filter *f, *n; + + list_for_each_entry_safe(f, n, &head->flist, link) { + list_del(&f->link); + canprio_delete_filter(tp, f); + } + kfree(head); +} + +/* + * Delete one instance of a filter. + */ +static int canprio_delete(struct tcf_proto *tp, unsigned long arg) +{ + struct canprio_head *head = (struct canprio_head *)tp->root; + struct canprio_filter *t; + struct canprio_filter *f = (struct canprio_filter *)arg; + + list_for_each_entry(t, &head->flist, link) + if (t == f) { + tcf_tree_lock(tp); + list_del(&t->link); + tcf_tree_unlock(tp); + canprio_delete_filter(tp, t); + return 0; + } + + return -ENOENT; +} + +//FIXME copied from cls_basic, not sure if ok; +static int canprio_init(struct tcf_proto *tp) +{ + struct canprio_head *head; + + printk(" canprio_init invoked\n"); + + head = kzalloc(sizeof(*head), GFP_KERNEL); + if (head == NULL) + return -ENOBUFS; + INIT_LIST_HEAD(&head->flist); + tp->root = head; + return 0; +} + + +static struct tcf_proto_ops cls_canprio_ops __read_mostly = { + .kind = "canprio", + .classify = canprio_classify, + .init = canprio_init, + .destroy = canprio_destroy, + .get = canprio_get, + .put = canprio_put, + .change = canprio_change, + .delete = canprio_delete, + .owner = THIS_MODULE, +}; + +static int __init init_canprio(void) +{ + printk("Canprio loaded\n"); + return register_tcf_proto_ops(&cls_canprio_ops); +} + +static void __exit exit_canprio(void) +{ + printk("Canprio removed\n"); + unregister_tcf_proto_ops(&cls_canprio_ops); +} + +module_init(init_canprio); +module_exit(exit_canprio); +MODULE_LICENSE("GPL"); +