net: em_canid: Ematch rule to match CAN frames according to their identifiers (f057bbb6) · Commits · 戴 / test

include/linux/can.h

+3 −0

Original line number	Diff line number	Diff line
		@@ -38,6 +38,9 @@
		*/
		typedef __u32 canid_t;

		#define CAN_SFF_ID_BITS 11
		#define CAN_EFF_ID_BITS 29

		/*
		* Controller Area Network Error Message Frame Mask structure
		*

include/linux/pkt_cls.h

+3 −2

Original line number	Diff line number	Diff line
		@@ -452,7 +452,8 @@ enum {
		#define TCF_EM_META 4
		#define TCF_EM_TEXT 5
		#define TCF_EM_VLAN 6
		#define TCF_EM_MAX 6
		#define TCF_EM_CANID 7
		#define TCF_EM_MAX 7

		enum {
		TCF_EM_PROG_TC

net/sched/Kconfig

+10 −0

Original line number	Diff line number	Diff line
		@@ -507,6 +507,16 @@ config NET_EMATCH_TEXT
		To compile this code as a module, choose M here: the
		module will be called em_text.

		config NET_EMATCH_CANID
		tristate "CAN Identifier"
		depends on NET_EMATCH && CAN
		---help---
		Say Y here if you want to be able to classify CAN frames based
		on CAN Identifier.

		To compile this code as a module, choose M here: the
		module will be called em_canid.

		config NET_CLS_ACT
		bool "Actions"
		---help---

net/sched/Makefile

+1 −0

Original line number	Diff line number	Diff line
		@@ -55,3 +55,4 @@ obj-$(CONFIG_NET_EMATCH_NBYTE) += em_nbyte.o
		obj-$(CONFIG_NET_EMATCH_U32) += em_u32.o
		obj-$(CONFIG_NET_EMATCH_META) += em_meta.o
		obj-$(CONFIG_NET_EMATCH_TEXT) += em_text.o
		obj-$(CONFIG_NET_EMATCH_CANID) += em_canid.o

net/sched/em_canid.c

0 → 100644

+240 −0

Original line number	Diff line number	Diff line
		/*
		* em_canid.c Ematch rule to match CAN frames according to their CAN IDs
		*
		* This program is free software; you can distribute it and/or
		* modify it under the terms of the GNU General Public License
		* as published by the Free Software Foundation; either version
		* 2 of the License, or (at your option) any later version.
		*
		* Idea: Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
		* Copyright: (c) 2011 Czech Technical University in Prague
		* (c) 2011 Volkswagen Group Research
		* Authors: Michal Sojka <sojkam1@fel.cvut.cz>
		* Pavel Pisa <pisa@cmp.felk.cvut.cz>
		* Rostislav Lisovy <lisovy@gmail.cz>
		* Funded by: Volkswagen Group Research
		*/

		#include <linux/slab.h>
		#include <linux/module.h>
		#include <linux/types.h>
		#include <linux/kernel.h>
		#include <linux/string.h>
		#include <linux/skbuff.h>
		#include <net/pkt_cls.h>
		#include <linux/can.h>

		#define EM_CAN_RULES_MAX 500

		struct canid_match {
		/* For each SFF CAN ID (11 bit) there is one record in this bitfield */
		DECLARE_BITMAP(match_sff, (1 << CAN_SFF_ID_BITS));

		int rules_count;
		int sff_rules_count;
		int eff_rules_count;

		/*
		* Raw rules copied from netlink message; Used for sending
		* information to userspace (when 'tc filter show' is invoked)
		* AND when matching EFF frames
		*/
		struct can_filter rules_raw[];
		};

		/**
		* em_canid_get_id() - Extracts Can ID out of the sk_buff structure.
		*/
		static canid_t em_canid_get_id(struct sk_buff *skb)
		{
		/* CAN ID is stored within the data field */
		struct can_frame cf = (struct can_frame )skb->data;

		return cf->can_id;
		}

		static void em_canid_sff_match_add(struct canid_match *cm, u32 can_id,
		u32 can_mask)
		{
		int i;

		/*
		* Limit can_mask and can_id to SFF range to
		* protect against write after end of array
		*/
		can_mask &= CAN_SFF_MASK;
		can_id &= can_mask;

		/* Single frame */
		if (can_mask == CAN_SFF_MASK) {
		set_bit(can_id, cm->match_sff);
		return;
		}

		/* All frames */
		if (can_mask == 0) {
		bitmap_fill(cm->match_sff, (1 << CAN_SFF_ID_BITS));
		return;
		}

		/*
		* Individual frame filter.
		* Add record (set bit to 1) for each ID that
		* conforms particular rule
		*/
		for (i = 0; i < (1 << CAN_SFF_ID_BITS); i++) {
		if ((i & can_mask) == can_id)
		set_bit(i, cm->match_sff);
		}
		}

		static inline struct canid_match em_canid_priv(struct tcf_ematch m)
		{
		return (struct canid_match *)m->data;
		}

		static int em_canid_match(struct sk_buff skb, struct tcf_ematch m,
		struct tcf_pkt_info *info)
		{
		struct canid_match *cm = em_canid_priv(m);
		canid_t can_id;
		int match = 0;
		int i;
		const struct can_filter *lp;

		can_id = em_canid_get_id(skb);

		if (can_id & CAN_EFF_FLAG) {
		for (i = 0, lp = cm->rules_raw;
		i < cm->eff_rules_count; i++, lp++) {
		if (!(((lp->can_id ^ can_id) & lp->can_mask))) {
		match = 1;
		break;
		}
		}
		} else { /* SFF */
		can_id &= CAN_SFF_MASK;
		match = (test_bit(can_id, cm->match_sff) ? 1 : 0);
		}

		return match;
		}

		static int em_canid_change(struct tcf_proto tp, void data, int len,
		struct tcf_ematch *m)
		{
		struct can_filter conf = data; / Array with rules */
		struct canid_match *cm;
		struct canid_match cm_old = (struct canid_match )m->data;
		int i;

		if (!len)
		return -EINVAL;

		if (len % sizeof(struct can_filter))
		return -EINVAL;

		if (len > sizeof(struct can_filter) * EM_CAN_RULES_MAX)
		return -EINVAL;

		cm = kzalloc(sizeof(struct canid_match) + len, GFP_KERNEL);
		if (!cm)
		return -ENOMEM;

		cm->rules_count = len / sizeof(struct can_filter);

		/*
		* We need two for() loops for copying rules into two contiguous
		* areas in rules_raw to process all eff rules with a simple loop.
		* NB: The configuration interface supports sff and eff rules.
		* We do not support filters here that match for the same can_id
		* provided in a SFF and EFF frame (e.g. 0x123 / 0x80000123).
		* For this (unusual case) two filters have to be specified. The
		* SFF/EFF separation is done with the CAN_EFF_FLAG in the can_id.
		*/

		/* Fill rules_raw with EFF rules first */
		for (i = 0; i < cm->rules_count; i++) {
		if (conf[i].can_id & CAN_EFF_FLAG) {
		memcpy(cm->rules_raw + cm->eff_rules_count,
		&conf[i],
		sizeof(struct can_filter));

		cm->eff_rules_count++;
		}
		}

		/* append SFF frame rules */
		for (i = 0; i < cm->rules_count; i++) {
		if (!(conf[i].can_id & CAN_EFF_FLAG)) {
		memcpy(cm->rules_raw
		+ cm->eff_rules_count
		+ cm->sff_rules_count,
		&conf[i], sizeof(struct can_filter));

		cm->sff_rules_count++;

		em_canid_sff_match_add(cm,
		conf[i].can_id, conf[i].can_mask);
		}
		}

		m->datalen = sizeof(struct canid_match) + len;
		m->data = (unsigned long)cm;

		if (cm_old != NULL) {
		pr_err("canid: Configuring an existing ematch!\n");
		kfree(cm_old);
		}

		return 0;
		}

		static void em_canid_destroy(struct tcf_proto tp, struct tcf_ematch m)
		{
		struct canid_match *cm = em_canid_priv(m);

		kfree(cm);
		}

		static int em_canid_dump(struct sk_buff skb, struct tcf_ematch m)
		{
		struct canid_match *cm = em_canid_priv(m);

		/*
		* When configuring this ematch 'rules_count' is set not to exceed
		* 'rules_raw' array size
		*/
		if (nla_put_nohdr(skb, sizeof(struct can_filter) * cm->rules_count,
		&cm->rules_raw) < 0)
		return -EMSGSIZE;

		return 0;
		}

		static struct tcf_ematch_ops em_canid_ops = {
		.kind = TCF_EM_CANID,
		.change = em_canid_change,
		.match = em_canid_match,
		.destroy = em_canid_destroy,
		.dump = em_canid_dump,
		.owner = THIS_MODULE,
		.link = LIST_HEAD_INIT(em_canid_ops.link)
		};

		static int __init init_em_canid(void)
		{
		return tcf_em_register(&em_canid_ops);
		}

		static void __exit exit_em_canid(void)
		{
		tcf_em_unregister(&em_canid_ops);
		}

		MODULE_LICENSE("GPL");

		module_init(init_em_canid);
		module_exit(exit_em_canid);

		MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);

Admin message