forked from Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
230 lines
5.3 KiB
230 lines
5.3 KiB
// 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 <[email protected]> |
|
* Copyright: (c) 2011 Czech Technical University in Prague |
|
* (c) 2011 Volkswagen Group Research |
|
* Authors: Michal Sojka <[email protected]> |
|
* Pavel Pisa <[email protected]> |
|
* Rostislav Lisovy <[email protected]> |
|
* 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);
|
|
|