developer | e181c88 | 2023-03-03 17:15:06 +0800 | [diff] [blame] | 1 | diff --git a/drivers/net/ethernet/mediatek/mtk_ppe.c b/drivers/net/ethernet/mediatek/mtk_ppe.c |
| 2 | index c2416b1..bc13a9b 100755 |
| 3 | --- a/drivers/net/ethernet/mediatek/mtk_ppe.c |
| 4 | +++ b/drivers/net/ethernet/mediatek/mtk_ppe.c |
| 5 | @@ -435,6 +435,17 @@ int mtk_foe_entry_set_qid(struct mtk_foe_entry *entry, int qid) |
| 6 | |
| 7 | return 0; |
| 8 | } |
| 9 | + |
| 10 | +int mtk_foe_entry_set_dscp(struct mtk_foe_entry *entry, int dscp) |
| 11 | +{ |
| 12 | + u32 *ib2 = mtk_foe_entry_ib2(entry); |
| 13 | + |
| 14 | + *ib2 &= ~MTK_FOE_IB2_DSCP; |
| 15 | + *ib2 |= FIELD_PREP(MTK_FOE_IB2_DSCP, dscp); |
| 16 | + |
| 17 | + return 0; |
| 18 | +} |
| 19 | + |
| 20 | static inline bool mtk_foe_entry_usable(struct mtk_foe_entry *entry) |
| 21 | { |
| 22 | return !(entry->ib1 & MTK_FOE_IB1_STATIC) && |
| 23 | diff --git a/drivers/net/ethernet/mediatek/mtk_ppe.h b/drivers/net/ethernet/mediatek/mtk_ppe.h |
| 24 | index e7ecbf7..df10040 100644 |
| 25 | --- a/drivers/net/ethernet/mediatek/mtk_ppe.h |
| 26 | +++ b/drivers/net/ethernet/mediatek/mtk_ppe.h |
| 27 | @@ -430,6 +430,7 @@ int mtk_foe_entry_set_pppoe(struct mtk_foe_entry *entry, int sid); |
| 28 | int mtk_foe_entry_set_wdma(struct mtk_foe_entry *entry, int wdma_idx, int txq, |
| 29 | int bss, int wcid); |
| 30 | int mtk_foe_entry_set_qid(struct mtk_foe_entry *entry, int qid); |
| 31 | +int mtk_foe_entry_set_dscp(struct mtk_foe_entry *entry, int dscp); |
| 32 | int mtk_foe_entry_commit(struct mtk_ppe *ppe, struct mtk_flow_entry *entry); |
| 33 | void mtk_foe_entry_clear(struct mtk_ppe *ppe, struct mtk_flow_entry *entry); |
| 34 | int mtk_foe_entry_idle_time(struct mtk_ppe *ppe, struct mtk_flow_entry *entry); |
| 35 | diff --git a/drivers/net/ethernet/mediatek/mtk_ppe_offload.c b/drivers/net/ethernet/mediatek/mtk_ppe_offload.c |
| 36 | index 23d2048..9bc0857 100644 |
| 37 | --- a/drivers/net/ethernet/mediatek/mtk_ppe_offload.c |
| 38 | +++ b/drivers/net/ethernet/mediatek/mtk_ppe_offload.c |
| 39 | @@ -246,6 +246,7 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f) |
| 40 | int wed_index = -1; |
| 41 | u16 addr_type = 0; |
| 42 | u8 l4proto = 0; |
| 43 | + u8 dscp = 0; |
| 44 | int err = 0; |
| 45 | int i; |
| 46 | |
| 47 | @@ -282,6 +283,15 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f) |
| 48 | return -EOPNOTSUPP; |
| 49 | } |
| 50 | |
| 51 | + if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_IP)) { |
| 52 | + struct flow_match_ip match; |
| 53 | + |
| 54 | + flow_rule_match_ip(rule, &match); |
| 55 | + dscp = match.key->tos; |
| 56 | + } else { |
| 57 | + return -EOPNOTSUPP; |
| 58 | + } |
| 59 | + |
| 60 | switch (addr_type) { |
| 61 | case 0: |
| 62 | offload_type = MTK_PPE_PKT_TYPE_BRIDGE; |
| 63 | @@ -441,6 +451,8 @@ mtk_flow_offload_replace(struct mtk_eth *eth, struct flow_cls_offload *f) |
| 64 | if (data.pppoe.num == 1) |
| 65 | mtk_foe_entry_set_pppoe(&foe, data.pppoe.sid); |
| 66 | |
| 67 | + mtk_foe_entry_set_dscp(&foe, dscp); |
| 68 | + |
| 69 | err = mtk_flow_set_output_device(eth, &foe, odev, f->flow->ct, data.eth.h_dest, |
| 70 | &wed_index); |
| 71 | if (err) |
| 72 | diff --git a/include/net/netfilter/nf_flow_table.h b/include/net/netfilter/nf_flow_table.h |
| 73 | index 55359dd..1a23c03 100644 |
| 74 | --- a/include/net/netfilter/nf_flow_table.h |
| 75 | +++ b/include/net/netfilter/nf_flow_table.h |
| 76 | @@ -36,6 +36,7 @@ struct nf_flow_key { |
| 77 | }; |
| 78 | struct flow_dissector_key_tcp tcp; |
| 79 | struct flow_dissector_key_ports tp; |
| 80 | + struct flow_dissector_key_ip ip; |
| 81 | } __aligned(BITS_PER_LONG / 8); /* Ensure that we can do comparisons as longs. */ |
| 82 | |
| 83 | struct nf_flow_match { |
| 84 | @@ -145,6 +146,7 @@ struct flow_offload_tuple { |
| 85 | u8 h_dest[ETH_ALEN]; |
| 86 | } out; |
| 87 | }; |
| 88 | + u8 tos; |
| 89 | }; |
| 90 | |
| 91 | struct flow_offload_tuple_rhash { |
| 92 | diff --git a/net/netfilter/nf_flow_table_offload.c b/net/netfilter/nf_flow_table_offload.c |
| 93 | index 61cc518..c1a5f64 100644 |
| 94 | --- a/net/netfilter/nf_flow_table_offload.c |
| 95 | +++ b/net/netfilter/nf_flow_table_offload.c |
| 96 | @@ -104,6 +104,7 @@ static int nf_flow_rule_match(struct nf_flow_match *match, |
| 97 | NF_FLOW_DISSECTOR(match, FLOW_DISSECTOR_KEY_IPV6_ADDRS, ipv6); |
| 98 | NF_FLOW_DISSECTOR(match, FLOW_DISSECTOR_KEY_TCP, tcp); |
| 99 | NF_FLOW_DISSECTOR(match, FLOW_DISSECTOR_KEY_PORTS, tp); |
| 100 | + NF_FLOW_DISSECTOR(match, FLOW_DISSECTOR_KEY_IP, ip); |
| 101 | |
| 102 | if (other_dst && other_dst->lwtstate) { |
| 103 | tun_info = lwt_tun_info(other_dst->lwtstate); |
| 104 | @@ -183,10 +184,14 @@ static int nf_flow_rule_match(struct nf_flow_match *match, |
| 105 | key->tp.dst = tuple->dst_port; |
| 106 | mask->tp.dst = 0xffff; |
| 107 | |
| 108 | + key->ip.tos = tuple->tos; |
| 109 | + mask->ip.tos = 0xff; |
| 110 | + |
| 111 | match->dissector.used_keys |= BIT(FLOW_DISSECTOR_KEY_META) | |
| 112 | BIT(FLOW_DISSECTOR_KEY_CONTROL) | |
| 113 | BIT(FLOW_DISSECTOR_KEY_BASIC) | |
| 114 | - BIT(FLOW_DISSECTOR_KEY_PORTS); |
| 115 | + BIT(FLOW_DISSECTOR_KEY_PORTS) | |
| 116 | + BIT(FLOW_DISSECTOR_KEY_IP); |
| 117 | return 0; |
| 118 | } |
| 119 | |
| 120 | diff --git a/net/netfilter/xt_FLOWOFFLOAD.c b/net/netfilter/xt_FLOWOFFLOAD.c |
| 121 | index 2d5c3cc..b231dd7 100644 |
| 122 | --- a/net/netfilter/xt_FLOWOFFLOAD.c |
| 123 | +++ b/net/netfilter/xt_FLOWOFFLOAD.c |
| 124 | @@ -49,6 +49,35 @@ static DEFINE_SPINLOCK(hooks_lock); |
| 125 | |
| 126 | struct xt_flowoffload_table flowtable[2]; |
| 127 | |
| 128 | +static int |
| 129 | +xt_flowoffload_dscp_init(struct sk_buff *skb, struct flow_offload *flow, |
| 130 | + enum ip_conntrack_dir dir) |
| 131 | +{ |
| 132 | + const struct flow_offload_tuple *flow_tuple = &flow->tuplehash[dir].tuple; |
| 133 | + struct iphdr *iph; |
| 134 | + struct ipv6hdr *ip6h; |
| 135 | + u32 offset = 0; |
| 136 | + u8 tos = 0; |
| 137 | + |
| 138 | + switch (flow_tuple->l3proto) { |
| 139 | + case NFPROTO_IPV4: |
| 140 | + iph = (struct iphdr *)(skb_network_header(skb) + offset); |
| 141 | + tos = iph->tos; |
| 142 | + break; |
| 143 | + case NFPROTO_IPV6: |
| 144 | + ip6h = (struct ipv6hdr *)(skb_network_header(skb) + offset); |
| 145 | + tos = ipv6_get_dsfield(ip6h); |
| 146 | + break; |
| 147 | + default: |
| 148 | + return -1; |
| 149 | + }; |
| 150 | + |
| 151 | + flow->tuplehash[dir].tuple.tos = tos; |
| 152 | + flow->tuplehash[!dir].tuple.tos = tos; |
| 153 | + |
| 154 | + return 0; |
| 155 | +} |
| 156 | + |
| 157 | static unsigned int |
| 158 | xt_flowoffload_net_hook(void *priv, struct sk_buff *skb, |
| 159 | const struct nf_hook_state *state) |
| 160 | @@ -599,6 +628,9 @@ flowoffload_tg(struct sk_buff *skb, const struct xt_action_param *par) |
| 161 | if (flow_offload_route_init(flow, &route) < 0) |
| 162 | goto err_flow_add; |
| 163 | |
| 164 | + if (xt_flowoffload_dscp_init(skb, flow, dir) < 0) |
| 165 | + goto err_flow_add; |
| 166 | + |
| 167 | if (tcph) { |
| 168 | ct->proto.tcp.seen[0].flags |= IP_CT_TCP_FLAG_BE_LIBERAL; |
| 169 | ct->proto.tcp.seen[1].flags |= IP_CT_TCP_FLAG_BE_LIBERAL; |