| @@ -5,6 +5,7 @@ · 4 unmodified lines |
| 5 | 5 | * Copyright (c) 2010 John Fastabend <john.r.fastabend@intel.com> |
| 6 | 6 | */ |
| 7 | 7 | |
| 8 | +#include <linux/ethtool_netlink.h> |
| 8 | 9 | #include <linux/types.h> |
| 9 | 10 | #include <linux/slab.h> |
| 10 | 11 | #include <linux/kernel.h> |
| @@ -27,6 +28,7 @@ struct mqprio_sched { · 16 unmodified lines |
| 27 | 28 | u32 flags; |
| 28 | 29 | u64 min_rate[TC_QOPT_MAX_QUEUE]; |
| 29 | 30 | u64 max_rate[TC_QOPT_MAX_QUEUE]; |
| 31 | + u32 fp[TC_QOPT_MAX_QUEUE]; |
| 30 | 32 | }; |
| 31 | 33 | |
| 32 | 34 | static int mqprio_enable_offload(struct Qdisc *sch, |
| @@ -63,6 +65,8 @@ static int mqprio_enable_offload(struct Qdisc *sch, · 30 unmodified lines |
| 63 | 65 | return -EINVAL; |
| 64 | 66 | } |
| 65 | 67 | |
| 68 | + mqprio_fp_to_offload(priv->fp, &mqprio); |
| 69 | + |
| 66 | 70 | err = dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_MQPRIO, |
| 67 | 71 | &mqprio); |
| 68 | 72 | if (err) |
| @@ -145,13 +149,95 @@ static int mqprio_parse_opt(struct net_device *dev, struct tc_mqprio_qopt *qopt, · 76 unmodified lines |
| 145 | 149 | return 0; |
| 146 | 150 | } |
| 147 | 151 | |
| 152 | +static const struct |
| 153 | +nla_policy mqprio_tc_entry_policy[TCA_MQPRIO_TC_ENTRY_MAX + 1] = { |
| 154 | + [TCA_MQPRIO_TC_ENTRY_INDEX] = NLA_POLICY_MAX(NLA_U32, |
| 155 | + TC_QOPT_MAX_QUEUE), |
| 156 | + [TCA_MQPRIO_TC_ENTRY_FP] = NLA_POLICY_RANGE(NLA_U32, |
| 157 | + TC_FP_EXPRESS, |
| 158 | + TC_FP_PREEMPTIBLE), |
| 159 | +}; |
| 160 | + |
| 148 | 161 | static const struct nla_policy mqprio_policy[TCA_MQPRIO_MAX + 1] = { |
| 149 | 162 | [TCA_MQPRIO_MODE] = { .len = sizeof(u16) }, |
| 150 | 163 | [TCA_MQPRIO_SHAPER] = { .len = sizeof(u16) }, |
| 151 | 164 | [TCA_MQPRIO_MIN_RATE64] = { .type = NLA_NESTED }, |
| 152 | 165 | [TCA_MQPRIO_MAX_RATE64] = { .type = NLA_NESTED }, |
| 166 | + [TCA_MQPRIO_TC_ENTRY] = { .type = NLA_NESTED }, |
| 153 | 167 | }; |
| 154 | 168 | |
| 169 | +static int mqprio_parse_tc_entry(u32 fp[TC_QOPT_MAX_QUEUE], |
| 170 | + struct nlattr *opt, |
| 171 | + unsigned long *seen_tcs, |
| 172 | + struct netlink_ext_ack *extack) |
| 173 | +{ |
| 174 | + struct nlattr *tb[TCA_MQPRIO_TC_ENTRY_MAX + 1]; |
| 175 | + int err, tc; |
| 176 | + |
| 177 | + err = nla_parse_nested(tb, TCA_MQPRIO_TC_ENTRY_MAX, opt, |
| 178 | + mqprio_tc_entry_policy, extack); |
| 179 | + if (err < 0) |
| 180 | + return err; |
| 181 | + |
| 182 | + if (NL_REQ_ATTR_CHECK(extack, opt, tb, TCA_MQPRIO_TC_ENTRY_INDEX)) { |
| 183 | + NL_SET_ERR_MSG(extack, "TC entry index missing"); |
| 184 | + return -EINVAL; |
| 185 | + } |
| 186 | + |
| 187 | + tc = nla_get_u32(tb[TCA_MQPRIO_TC_ENTRY_INDEX]); |
| 188 | + if (*seen_tcs & BIT(tc)) { |
| 189 | + NL_SET_ERR_MSG_ATTR(extack, tb[TCA_MQPRIO_TC_ENTRY_INDEX], |
| 190 | + "Duplicate tc entry"); |
| 191 | + return -EINVAL; |
| 192 | + } |
| 193 | + |
| 194 | + *seen_tcs |= BIT(tc); |
| 195 | + |
| 196 | + if (tb[TCA_MQPRIO_TC_ENTRY_FP]) |
| 197 | + fp[tc] = nla_get_u32(tb[TCA_MQPRIO_TC_ENTRY_FP]); |
| 198 | + |
| 199 | + return 0; |
| 200 | +} |
| 201 | + |
| 202 | +static int mqprio_parse_tc_entries(struct Qdisc *sch, struct nlattr *nlattr_opt, |
| 203 | + int nlattr_opt_len, |
| 204 | + struct netlink_ext_ack *extack) |
| 205 | +{ |
| 206 | + struct mqprio_sched *priv = qdisc_priv(sch); |
| 207 | + struct net_device *dev = qdisc_dev(sch); |
| 208 | + bool have_preemption = false; |
| 209 | + unsigned long seen_tcs = 0; |
| 210 | + u32 fp[TC_QOPT_MAX_QUEUE]; |
| 211 | + struct nlattr *n; |
| 212 | + int tc, rem; |
| 213 | + int err = 0; |
| 214 | + |
| 215 | + for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++) |
| 216 | + fp[tc] = priv->fp[tc]; |
| 217 | + |
| 218 | + nla_for_each_attr(n, nlattr_opt, nlattr_opt_len, rem) { |
| 219 | + if (nla_type(n) != TCA_MQPRIO_TC_ENTRY) |
| 220 | + continue; |
| 221 | + |
| 222 | + err = mqprio_parse_tc_entry(fp, n, &seen_tcs, extack); |
| 223 | + if (err) |
| 224 | + goto out; |
| 225 | + } |
| 226 | + |
| 227 | + for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++) { |
| 228 | + priv->fp[tc] = fp[tc]; |
| 229 | + if (fp[tc] == TC_FP_PREEMPTIBLE) |
| 230 | + have_preemption = true; |
| 231 | + } |
| 232 | + |
| 233 | + if (have_preemption && !ethtool_dev_mm_supported(dev)) { |
| 234 | + NL_SET_ERR_MSG(extack, "Device does not support preemption"); |
| 235 | + return -EOPNOTSUPP; |
| 236 | + } |
| 237 | +out: |
| 238 | + return err; |
| 239 | +} |
| 240 | + |
| 155 | 241 | /* Parse the other netlink attributes that represent the payload of |
| 156 | 242 | * TCA_OPTIONS, which are appended right after struct tc_mqprio_qopt. |
| 157 | 243 | */ |
| @@ -234,6 +320,13 @@ static int mqprio_parse_nlattr(struct Qdisc *sch, struct tc_mqprio_qopt *qopt, · 76 unmodified lines |
| 234 | 320 | priv->flags |= TC_MQPRIO_F_MAX_RATE; |
| 235 | 321 | } |
| 236 | 322 | |
| 323 | + if (tb[TCA_MQPRIO_TC_ENTRY]) { |
| 324 | + err = mqprio_parse_tc_entries(sch, nlattr_opt, nlattr_opt_len, |
| 325 | + extack); |
| 326 | + if (err) |
| 327 | + return err; |
| 328 | + } |
| 329 | + |
| 237 | 330 | return 0; |
| 238 | 331 | } |
| 239 | 332 | |
| @@ -247,7 +340,7 @@ static int mqprio_init(struct Qdisc *sch, struct nlattr *opt, · 7 unmodified lines |
| 247 | 340 | int i, err = -EOPNOTSUPP; |
| 248 | 341 | struct tc_mqprio_qopt *qopt = NULL; |
| 249 | 342 | struct tc_mqprio_caps caps; |
| 250 | | − int len; |
| 343 | + int len, tc; |
| 251 | 344 | |
| 252 | 345 | BUILD_BUG_ON(TC_MAX_QUEUE != TC_QOPT_MAX_QUEUE); |
| 253 | 346 | BUILD_BUG_ON(TC_BITMASK != TC_QOPT_BITMASK); |
| @@ -265,6 +358,9 @@ static int mqprio_init(struct Qdisc *sch, struct nlattr *opt, · 11 unmodified lines |
| 265 | 358 | if (!opt || nla_len(opt) < sizeof(*qopt)) |
| 266 | 359 | return -EINVAL; |
| 267 | 360 | |
| 361 | + for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++) |
| 362 | + priv->fp[tc] = TC_FP_EXPRESS; |
| 363 | + |
| 268 | 364 | qdisc_offload_query_caps(dev, TC_SETUP_QDISC_MQPRIO, |
| 269 | 365 | &caps, sizeof(caps)); |
| 270 | 366 | |
| @@ -415,6 +511,33 @@ nla_put_failure: · 144 unmodified lines |
| 415 | 511 | return -1; |
| 416 | 512 | } |
| 417 | 513 | |
| 514 | +static int mqprio_dump_tc_entries(struct mqprio_sched *priv, |
| 515 | + struct sk_buff *skb) |
| 516 | +{ |
| 517 | + struct nlattr *n; |
| 518 | + int tc; |
| 519 | + |
| 520 | + for (tc = 0; tc < TC_QOPT_MAX_QUEUE; tc++) { |
| 521 | + n = nla_nest_start(skb, TCA_MQPRIO_TC_ENTRY); |
| 522 | + if (!n) |
| 523 | + return -EMSGSIZE; |
| 524 | + |
| 525 | + if (nla_put_u32(skb, TCA_MQPRIO_TC_ENTRY_INDEX, tc)) |
| 526 | + goto nla_put_failure; |
| 527 | + |
| 528 | + if (nla_put_u32(skb, TCA_MQPRIO_TC_ENTRY_FP, priv->fp[tc])) |
| 529 | + goto nla_put_failure; |
| 530 | + |
| 531 | + nla_nest_end(skb, n); |
| 532 | + } |
| 533 | + |
| 534 | + return 0; |
| 535 | + |
| 536 | +nla_put_failure: |
| 537 | + nla_nest_cancel(skb, n); |
| 538 | + return -EMSGSIZE; |
| 539 | +} |
| 540 | + |
| 418 | 541 | static int mqprio_dump(struct Qdisc *sch, struct sk_buff *skb) |
| 419 | 542 | { |
| 420 | 543 | struct net_device *dev = qdisc_dev(sch); |
| @@ -465,6 +588,9 @@ static int mqprio_dump(struct Qdisc *sch, struct sk_buff *skb) · 44 unmodified lines |
| 465 | 588 | (dump_rates(priv, &opt, skb) != 0)) |
| 466 | 589 | goto nla_put_failure; |
| 467 | 590 | |
| 591 | + if (mqprio_dump_tc_entries(priv, skb)) |
| 592 | + goto nla_put_failure; |
| 593 | + |
| 468 | 594 | return nla_nest_end(skb, nla); |
| 469 | 595 | nla_put_failure: |
| 470 | 596 | nlmsg_trim(skb, nla); |