^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1) // SPDX-License-Identifier: GPL-2.0-or-later
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 2) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 3) * em_canid.c Ematch rule to match CAN frames according to their CAN IDs
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 4) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 5) * Idea: Oliver Hartkopp <oliver.hartkopp@volkswagen.de>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 6) * Copyright: (c) 2011 Czech Technical University in Prague
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 7) * (c) 2011 Volkswagen Group Research
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 8) * Authors: Michal Sojka <sojkam1@fel.cvut.cz>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 9) * Pavel Pisa <pisa@cmp.felk.cvut.cz>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 10) * Rostislav Lisovy <lisovy@gmail.cz>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 11) * Funded by: Volkswagen Group Research
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 12) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 13)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 14) #include <linux/slab.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 15) #include <linux/module.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 16) #include <linux/types.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 17) #include <linux/kernel.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 18) #include <linux/string.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 19) #include <linux/skbuff.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 20) #include <net/pkt_cls.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 21) #include <linux/can.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 22)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 23) #define EM_CAN_RULES_MAX 500
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 24)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 25) struct canid_match {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 26) /* For each SFF CAN ID (11 bit) there is one record in this bitfield */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 27) DECLARE_BITMAP(match_sff, (1 << CAN_SFF_ID_BITS));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 28)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 29) int rules_count;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 30) int sff_rules_count;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 31) int eff_rules_count;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 32)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 33) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 34) * Raw rules copied from netlink message; Used for sending
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 35) * information to userspace (when 'tc filter show' is invoked)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 36) * AND when matching EFF frames
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 37) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 38) struct can_filter rules_raw[];
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 39) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 40)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 41) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 42) * em_canid_get_id() - Extracts Can ID out of the sk_buff structure.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 43) * @skb: buffer to extract Can ID from
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 44) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 45) static canid_t em_canid_get_id(struct sk_buff *skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 46) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 47) /* CAN ID is stored within the data field */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 48) struct can_frame *cf = (struct can_frame *)skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 49)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 50) return cf->can_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 51) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 52)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 53) static void em_canid_sff_match_add(struct canid_match *cm, u32 can_id,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 54) u32 can_mask)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 55) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 56) int i;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 57)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 58) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 59) * Limit can_mask and can_id to SFF range to
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 60) * protect against write after end of array
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 61) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 62) can_mask &= CAN_SFF_MASK;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 63) can_id &= can_mask;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 64)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 65) /* Single frame */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 66) if (can_mask == CAN_SFF_MASK) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 67) set_bit(can_id, cm->match_sff);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 68) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 69) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 70)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 71) /* All frames */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 72) if (can_mask == 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 73) bitmap_fill(cm->match_sff, (1 << CAN_SFF_ID_BITS));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 74) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 75) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 76)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 77) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 78) * Individual frame filter.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 79) * Add record (set bit to 1) for each ID that
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 80) * conforms particular rule
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 81) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 82) for (i = 0; i < (1 << CAN_SFF_ID_BITS); i++) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 83) if ((i & can_mask) == can_id)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 84) set_bit(i, cm->match_sff);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 85) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 86) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 87)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 88) static inline struct canid_match *em_canid_priv(struct tcf_ematch *m)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 89) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 90) return (struct canid_match *)m->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 91) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 92)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 93) static int em_canid_match(struct sk_buff *skb, struct tcf_ematch *m,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 94) struct tcf_pkt_info *info)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 95) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 96) struct canid_match *cm = em_canid_priv(m);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 97) canid_t can_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 98) int match = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 99) int i;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100) const struct can_filter *lp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102) can_id = em_canid_get_id(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104) if (can_id & CAN_EFF_FLAG) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) for (i = 0, lp = cm->rules_raw;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106) i < cm->eff_rules_count; i++, lp++) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107) if (!(((lp->can_id ^ can_id) & lp->can_mask))) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) match = 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112) } else { /* SFF */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113) can_id &= CAN_SFF_MASK;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) match = (test_bit(can_id, cm->match_sff) ? 1 : 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117) return match;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120) static int em_canid_change(struct net *net, void *data, int len,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) struct tcf_ematch *m)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123) struct can_filter *conf = data; /* Array with rules */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124) struct canid_match *cm;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) int i;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) if (!len)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130) if (len % sizeof(struct can_filter))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) if (len > sizeof(struct can_filter) * EM_CAN_RULES_MAX)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) cm = kzalloc(sizeof(struct canid_match) + len, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137) if (!cm)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) return -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140) cm->rules_count = len / sizeof(struct can_filter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143) * We need two for() loops for copying rules into two contiguous
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144) * areas in rules_raw to process all eff rules with a simple loop.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) * NB: The configuration interface supports sff and eff rules.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146) * We do not support filters here that match for the same can_id
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147) * provided in a SFF and EFF frame (e.g. 0x123 / 0x80000123).
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148) * For this (unusual case) two filters have to be specified. The
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) * SFF/EFF separation is done with the CAN_EFF_FLAG in the can_id.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) /* Fill rules_raw with EFF rules first */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153) for (i = 0; i < cm->rules_count; i++) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154) if (conf[i].can_id & CAN_EFF_FLAG) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) memcpy(cm->rules_raw + cm->eff_rules_count,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) &conf[i],
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157) sizeof(struct can_filter));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159) cm->eff_rules_count++;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163) /* append SFF frame rules */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164) for (i = 0; i < cm->rules_count; i++) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165) if (!(conf[i].can_id & CAN_EFF_FLAG)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) memcpy(cm->rules_raw
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167) + cm->eff_rules_count
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168) + cm->sff_rules_count,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169) &conf[i], sizeof(struct can_filter));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171) cm->sff_rules_count++;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) em_canid_sff_match_add(cm,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174) conf[i].can_id, conf[i].can_mask);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178) m->datalen = sizeof(struct canid_match) + len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179) m->data = (unsigned long)cm;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183) static void em_canid_destroy(struct tcf_ematch *m)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185) struct canid_match *cm = em_canid_priv(m);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187) kfree(cm);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190) static int em_canid_dump(struct sk_buff *skb, struct tcf_ematch *m)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192) struct canid_match *cm = em_canid_priv(m);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195) * When configuring this ematch 'rules_count' is set not to exceed
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) * 'rules_raw' array size
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198) if (nla_put_nohdr(skb, sizeof(struct can_filter) * cm->rules_count,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199) &cm->rules_raw) < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) return -EMSGSIZE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 202) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 203) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 204)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 205) static struct tcf_ematch_ops em_canid_ops = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 206) .kind = TCF_EM_CANID,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 207) .change = em_canid_change,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 208) .match = em_canid_match,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 209) .destroy = em_canid_destroy,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 210) .dump = em_canid_dump,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 211) .owner = THIS_MODULE,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 212) .link = LIST_HEAD_INIT(em_canid_ops.link)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 213) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 214)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 215) static int __init init_em_canid(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 216) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 217) return tcf_em_register(&em_canid_ops);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 218) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 219)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 220) static void __exit exit_em_canid(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 221) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 222) tcf_em_unregister(&em_canid_ops);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 223) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 224)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 225) MODULE_LICENSE("GPL");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 226)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 227) module_init(init_em_canid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 228) module_exit(exit_em_canid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 229)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 230) MODULE_ALIAS_TCF_EMATCH(TCF_EM_CANID);