--- /dev/null
+/* 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 <linux/module.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/rtnetlink.h>
+#include <linux/skbuff.h>
+#include <net/netlink.h>
+#include <net/act_api.h>
+#include <net/pkt_cls.h>
+#include <linux/can.h>
+
+
+/* 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");
+
projects / lisovros / linux_canprio.git / commitdiff