em_canid.c 5.3 KB

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