mirror of
https://mirrors.bfsu.edu.cn/git/linux.git
synced 2024-12-14 22:44:27 +08:00
90ac5d0301
Simple fixes which require no deep knowledge of the code. Cc: Jamal Hadi Salim <jhs@mojatatu.com> Cc: Cong Wang <xiyou.wangcong@gmail.com> Cc: Jiri Pirko <jiri@resnulli.us> Signed-off-by: Andrew Lunn <andrew@lunn.ch> Signed-off-by: David S. Miller <davem@davemloft.net>
231 lines
5.3 KiB
C
231 lines
5.3 KiB
C
// SPDX-License-Identifier: GPL-2.0-or-later
|
|
/*
|
|
* em_canid.c Ematch rule to match CAN frames according to their CAN IDs
|
|
*
|
|
* 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.
|
|
* @skb: buffer to extract Can ID from
|
|
*/
|
|
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 net *net, void *data, int len,
|
|
struct tcf_ematch *m)
|
|
{
|
|
struct can_filter *conf = data; /* Array with rules */
|
|
struct canid_match *cm;
|
|
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;
|
|
return 0;
|
|
}
|
|
|
|
static void em_canid_destroy(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);
|