Alexander Duyck | 9265194 | 2008-09-12 16:29:34 -0700 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (c) 2008, Intel Corporation. |
| 3 | * |
| 4 | * This program is free software; you can redistribute it and/or modify it |
| 5 | * under the terms and conditions of the GNU General Public License, |
| 6 | * version 2, as published by the Free Software Foundation. |
| 7 | * |
| 8 | * This program is distributed in the hope it will be useful, but WITHOUT |
| 9 | * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
| 10 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for |
| 11 | * more details. |
| 12 | * |
| 13 | * You should have received a copy of the GNU General Public License along with |
| 14 | * this program; if not, write to the Free Software Foundation, Inc., 59 Temple |
| 15 | * Place - Suite 330, Boston, MA 02111-1307 USA. |
| 16 | * |
| 17 | * Author: Alexander Duyck <alexander.h.duyck@intel.com> |
| 18 | */ |
| 19 | |
| 20 | #include <linux/module.h> |
| 21 | #include <linux/types.h> |
| 22 | #include <linux/kernel.h> |
| 23 | #include <linux/string.h> |
| 24 | #include <linux/errno.h> |
| 25 | #include <linux/skbuff.h> |
| 26 | #include <net/netlink.h> |
| 27 | #include <net/pkt_sched.h> |
| 28 | |
| 29 | |
| 30 | struct multiq_sched_data { |
| 31 | u16 bands; |
| 32 | u16 max_bands; |
| 33 | u16 curband; |
| 34 | struct tcf_proto *filter_list; |
| 35 | struct Qdisc **queues; |
| 36 | }; |
| 37 | |
| 38 | |
| 39 | static struct Qdisc * |
| 40 | multiq_classify(struct sk_buff *skb, struct Qdisc *sch, int *qerr) |
| 41 | { |
| 42 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 43 | u32 band; |
| 44 | struct tcf_result res; |
| 45 | int err; |
| 46 | |
| 47 | *qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS; |
| 48 | err = tc_classify(skb, q->filter_list, &res); |
| 49 | #ifdef CONFIG_NET_CLS_ACT |
| 50 | switch (err) { |
| 51 | case TC_ACT_STOLEN: |
| 52 | case TC_ACT_QUEUED: |
| 53 | *qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN; |
| 54 | case TC_ACT_SHOT: |
| 55 | return NULL; |
| 56 | } |
| 57 | #endif |
| 58 | band = skb_get_queue_mapping(skb); |
| 59 | |
| 60 | if (band >= q->bands) |
| 61 | return q->queues[0]; |
| 62 | |
| 63 | return q->queues[band]; |
| 64 | } |
| 65 | |
| 66 | static int |
| 67 | multiq_enqueue(struct sk_buff *skb, struct Qdisc *sch) |
| 68 | { |
| 69 | struct Qdisc *qdisc; |
| 70 | int ret; |
| 71 | |
| 72 | qdisc = multiq_classify(skb, sch, &ret); |
| 73 | #ifdef CONFIG_NET_CLS_ACT |
| 74 | if (qdisc == NULL) { |
| 75 | |
| 76 | if (ret & __NET_XMIT_BYPASS) |
| 77 | sch->qstats.drops++; |
| 78 | kfree_skb(skb); |
| 79 | return ret; |
| 80 | } |
| 81 | #endif |
| 82 | |
| 83 | ret = qdisc_enqueue(skb, qdisc); |
| 84 | if (ret == NET_XMIT_SUCCESS) { |
| 85 | sch->bstats.bytes += qdisc_pkt_len(skb); |
| 86 | sch->bstats.packets++; |
| 87 | sch->q.qlen++; |
| 88 | return NET_XMIT_SUCCESS; |
| 89 | } |
| 90 | if (net_xmit_drop_count(ret)) |
| 91 | sch->qstats.drops++; |
| 92 | return ret; |
| 93 | } |
| 94 | |
| 95 | |
| 96 | static int |
| 97 | multiq_requeue(struct sk_buff *skb, struct Qdisc *sch) |
| 98 | { |
| 99 | struct Qdisc *qdisc; |
Alexander Duyck | a574420 | 2008-09-20 22:07:34 -0700 | [diff] [blame] | 100 | struct multiq_sched_data *q = qdisc_priv(sch); |
Alexander Duyck | 9265194 | 2008-09-12 16:29:34 -0700 | [diff] [blame] | 101 | int ret; |
| 102 | |
| 103 | qdisc = multiq_classify(skb, sch, &ret); |
| 104 | #ifdef CONFIG_NET_CLS_ACT |
| 105 | if (qdisc == NULL) { |
| 106 | if (ret & __NET_XMIT_BYPASS) |
| 107 | sch->qstats.drops++; |
| 108 | kfree_skb(skb); |
| 109 | return ret; |
| 110 | } |
| 111 | #endif |
| 112 | |
| 113 | ret = qdisc->ops->requeue(skb, qdisc); |
| 114 | if (ret == NET_XMIT_SUCCESS) { |
| 115 | sch->q.qlen++; |
| 116 | sch->qstats.requeues++; |
Alexander Duyck | a574420 | 2008-09-20 22:07:34 -0700 | [diff] [blame] | 117 | if (q->curband) |
| 118 | q->curband--; |
| 119 | else |
| 120 | q->curband = q->bands - 1; |
Alexander Duyck | 9265194 | 2008-09-12 16:29:34 -0700 | [diff] [blame] | 121 | return NET_XMIT_SUCCESS; |
| 122 | } |
| 123 | if (net_xmit_drop_count(ret)) |
| 124 | sch->qstats.drops++; |
| 125 | return ret; |
| 126 | } |
| 127 | |
| 128 | |
| 129 | static struct sk_buff *multiq_dequeue(struct Qdisc *sch) |
| 130 | { |
| 131 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 132 | struct Qdisc *qdisc; |
| 133 | struct sk_buff *skb; |
| 134 | int band; |
| 135 | |
| 136 | for (band = 0; band < q->bands; band++) { |
| 137 | /* cycle through bands to ensure fairness */ |
| 138 | q->curband++; |
| 139 | if (q->curband >= q->bands) |
| 140 | q->curband = 0; |
| 141 | |
| 142 | /* Check that target subqueue is available before |
| 143 | * pulling an skb to avoid excessive requeues |
| 144 | */ |
| 145 | if (!__netif_subqueue_stopped(qdisc_dev(sch), q->curband)) { |
| 146 | qdisc = q->queues[q->curband]; |
| 147 | skb = qdisc->dequeue(qdisc); |
| 148 | if (skb) { |
| 149 | sch->q.qlen--; |
| 150 | return skb; |
| 151 | } |
| 152 | } |
| 153 | } |
| 154 | return NULL; |
| 155 | |
| 156 | } |
| 157 | |
Jarek Poplawski | 8e3af97 | 2008-10-31 00:45:55 -0700 | [diff] [blame^] | 158 | static struct sk_buff *multiq_peek(struct Qdisc *sch) |
| 159 | { |
| 160 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 161 | unsigned int curband = q->curband; |
| 162 | struct Qdisc *qdisc; |
| 163 | struct sk_buff *skb; |
| 164 | int band; |
| 165 | |
| 166 | for (band = 0; band < q->bands; band++) { |
| 167 | /* cycle through bands to ensure fairness */ |
| 168 | curband++; |
| 169 | if (curband >= q->bands) |
| 170 | curband = 0; |
| 171 | |
| 172 | /* Check that target subqueue is available before |
| 173 | * pulling an skb to avoid excessive requeues |
| 174 | */ |
| 175 | if (!__netif_subqueue_stopped(qdisc_dev(sch), curband)) { |
| 176 | qdisc = q->queues[curband]; |
| 177 | skb = qdisc->ops->peek(qdisc); |
| 178 | if (skb) |
| 179 | return skb; |
| 180 | } |
| 181 | } |
| 182 | return NULL; |
| 183 | |
| 184 | } |
| 185 | |
Alexander Duyck | 9265194 | 2008-09-12 16:29:34 -0700 | [diff] [blame] | 186 | static unsigned int multiq_drop(struct Qdisc *sch) |
| 187 | { |
| 188 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 189 | int band; |
| 190 | unsigned int len; |
| 191 | struct Qdisc *qdisc; |
| 192 | |
| 193 | for (band = q->bands-1; band >= 0; band--) { |
| 194 | qdisc = q->queues[band]; |
| 195 | if (qdisc->ops->drop) { |
| 196 | len = qdisc->ops->drop(qdisc); |
| 197 | if (len != 0) { |
| 198 | sch->q.qlen--; |
| 199 | return len; |
| 200 | } |
| 201 | } |
| 202 | } |
| 203 | return 0; |
| 204 | } |
| 205 | |
| 206 | |
| 207 | static void |
| 208 | multiq_reset(struct Qdisc *sch) |
| 209 | { |
| 210 | u16 band; |
| 211 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 212 | |
| 213 | for (band = 0; band < q->bands; band++) |
| 214 | qdisc_reset(q->queues[band]); |
| 215 | sch->q.qlen = 0; |
| 216 | q->curband = 0; |
| 217 | } |
| 218 | |
| 219 | static void |
| 220 | multiq_destroy(struct Qdisc *sch) |
| 221 | { |
| 222 | int band; |
| 223 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 224 | |
| 225 | tcf_destroy_chain(&q->filter_list); |
| 226 | for (band = 0; band < q->bands; band++) |
| 227 | qdisc_destroy(q->queues[band]); |
| 228 | |
| 229 | kfree(q->queues); |
| 230 | } |
| 231 | |
| 232 | static int multiq_tune(struct Qdisc *sch, struct nlattr *opt) |
| 233 | { |
| 234 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 235 | struct tc_multiq_qopt *qopt; |
| 236 | int i; |
| 237 | |
| 238 | if (!netif_is_multiqueue(qdisc_dev(sch))) |
| 239 | return -EINVAL; |
| 240 | if (nla_len(opt) < sizeof(*qopt)) |
| 241 | return -EINVAL; |
| 242 | |
| 243 | qopt = nla_data(opt); |
| 244 | |
| 245 | qopt->bands = qdisc_dev(sch)->real_num_tx_queues; |
| 246 | |
| 247 | sch_tree_lock(sch); |
| 248 | q->bands = qopt->bands; |
| 249 | for (i = q->bands; i < q->max_bands; i++) { |
Alexander Duyck | f07d150 | 2008-09-12 17:57:23 -0700 | [diff] [blame] | 250 | if (q->queues[i] != &noop_qdisc) { |
| 251 | struct Qdisc *child = xchg(&q->queues[i], &noop_qdisc); |
Alexander Duyck | 9265194 | 2008-09-12 16:29:34 -0700 | [diff] [blame] | 252 | qdisc_tree_decrease_qlen(child, child->q.qlen); |
| 253 | qdisc_destroy(child); |
| 254 | } |
| 255 | } |
| 256 | |
| 257 | sch_tree_unlock(sch); |
| 258 | |
| 259 | for (i = 0; i < q->bands; i++) { |
| 260 | if (q->queues[i] == &noop_qdisc) { |
| 261 | struct Qdisc *child; |
| 262 | child = qdisc_create_dflt(qdisc_dev(sch), |
| 263 | sch->dev_queue, |
| 264 | &pfifo_qdisc_ops, |
| 265 | TC_H_MAKE(sch->handle, |
| 266 | i + 1)); |
| 267 | if (child) { |
| 268 | sch_tree_lock(sch); |
| 269 | child = xchg(&q->queues[i], child); |
| 270 | |
| 271 | if (child != &noop_qdisc) { |
| 272 | qdisc_tree_decrease_qlen(child, |
| 273 | child->q.qlen); |
| 274 | qdisc_destroy(child); |
| 275 | } |
| 276 | sch_tree_unlock(sch); |
| 277 | } |
| 278 | } |
| 279 | } |
| 280 | return 0; |
| 281 | } |
| 282 | |
| 283 | static int multiq_init(struct Qdisc *sch, struct nlattr *opt) |
| 284 | { |
| 285 | struct multiq_sched_data *q = qdisc_priv(sch); |
Alexander Duyck | f07d150 | 2008-09-12 17:57:23 -0700 | [diff] [blame] | 286 | int i, err; |
Alexander Duyck | 9265194 | 2008-09-12 16:29:34 -0700 | [diff] [blame] | 287 | |
| 288 | q->queues = NULL; |
| 289 | |
| 290 | if (opt == NULL) |
| 291 | return -EINVAL; |
| 292 | |
| 293 | q->max_bands = qdisc_dev(sch)->num_tx_queues; |
| 294 | |
| 295 | q->queues = kcalloc(q->max_bands, sizeof(struct Qdisc *), GFP_KERNEL); |
| 296 | if (!q->queues) |
| 297 | return -ENOBUFS; |
| 298 | for (i = 0; i < q->max_bands; i++) |
| 299 | q->queues[i] = &noop_qdisc; |
| 300 | |
Alexander Duyck | f07d150 | 2008-09-12 17:57:23 -0700 | [diff] [blame] | 301 | err = multiq_tune(sch,opt); |
| 302 | |
| 303 | if (err) |
| 304 | kfree(q->queues); |
| 305 | |
| 306 | return err; |
Alexander Duyck | 9265194 | 2008-09-12 16:29:34 -0700 | [diff] [blame] | 307 | } |
| 308 | |
| 309 | static int multiq_dump(struct Qdisc *sch, struct sk_buff *skb) |
| 310 | { |
| 311 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 312 | unsigned char *b = skb_tail_pointer(skb); |
| 313 | struct tc_multiq_qopt opt; |
| 314 | |
| 315 | opt.bands = q->bands; |
| 316 | opt.max_bands = q->max_bands; |
| 317 | |
| 318 | NLA_PUT(skb, TCA_OPTIONS, sizeof(opt), &opt); |
| 319 | |
| 320 | return skb->len; |
| 321 | |
| 322 | nla_put_failure: |
| 323 | nlmsg_trim(skb, b); |
| 324 | return -1; |
| 325 | } |
| 326 | |
| 327 | static int multiq_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new, |
| 328 | struct Qdisc **old) |
| 329 | { |
| 330 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 331 | unsigned long band = arg - 1; |
| 332 | |
| 333 | if (band >= q->bands) |
| 334 | return -EINVAL; |
| 335 | |
| 336 | if (new == NULL) |
| 337 | new = &noop_qdisc; |
| 338 | |
| 339 | sch_tree_lock(sch); |
| 340 | *old = q->queues[band]; |
| 341 | q->queues[band] = new; |
| 342 | qdisc_tree_decrease_qlen(*old, (*old)->q.qlen); |
| 343 | qdisc_reset(*old); |
| 344 | sch_tree_unlock(sch); |
| 345 | |
| 346 | return 0; |
| 347 | } |
| 348 | |
| 349 | static struct Qdisc * |
| 350 | multiq_leaf(struct Qdisc *sch, unsigned long arg) |
| 351 | { |
| 352 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 353 | unsigned long band = arg - 1; |
| 354 | |
| 355 | if (band >= q->bands) |
| 356 | return NULL; |
| 357 | |
| 358 | return q->queues[band]; |
| 359 | } |
| 360 | |
| 361 | static unsigned long multiq_get(struct Qdisc *sch, u32 classid) |
| 362 | { |
| 363 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 364 | unsigned long band = TC_H_MIN(classid); |
| 365 | |
| 366 | if (band - 1 >= q->bands) |
| 367 | return 0; |
| 368 | return band; |
| 369 | } |
| 370 | |
| 371 | static unsigned long multiq_bind(struct Qdisc *sch, unsigned long parent, |
| 372 | u32 classid) |
| 373 | { |
| 374 | return multiq_get(sch, classid); |
| 375 | } |
| 376 | |
| 377 | |
| 378 | static void multiq_put(struct Qdisc *q, unsigned long cl) |
| 379 | { |
| 380 | return; |
| 381 | } |
| 382 | |
| 383 | static int multiq_change(struct Qdisc *sch, u32 handle, u32 parent, |
| 384 | struct nlattr **tca, unsigned long *arg) |
| 385 | { |
| 386 | unsigned long cl = *arg; |
| 387 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 388 | |
| 389 | if (cl - 1 > q->bands) |
| 390 | return -ENOENT; |
| 391 | return 0; |
| 392 | } |
| 393 | |
| 394 | static int multiq_delete(struct Qdisc *sch, unsigned long cl) |
| 395 | { |
| 396 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 397 | if (cl - 1 > q->bands) |
| 398 | return -ENOENT; |
| 399 | return 0; |
| 400 | } |
| 401 | |
| 402 | |
| 403 | static int multiq_dump_class(struct Qdisc *sch, unsigned long cl, |
| 404 | struct sk_buff *skb, struct tcmsg *tcm) |
| 405 | { |
| 406 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 407 | |
| 408 | if (cl - 1 > q->bands) |
| 409 | return -ENOENT; |
| 410 | tcm->tcm_handle |= TC_H_MIN(cl); |
| 411 | if (q->queues[cl-1]) |
| 412 | tcm->tcm_info = q->queues[cl-1]->handle; |
| 413 | return 0; |
| 414 | } |
| 415 | |
| 416 | static int multiq_dump_class_stats(struct Qdisc *sch, unsigned long cl, |
| 417 | struct gnet_dump *d) |
| 418 | { |
| 419 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 420 | struct Qdisc *cl_q; |
| 421 | |
| 422 | cl_q = q->queues[cl - 1]; |
| 423 | if (gnet_stats_copy_basic(d, &cl_q->bstats) < 0 || |
| 424 | gnet_stats_copy_queue(d, &cl_q->qstats) < 0) |
| 425 | return -1; |
| 426 | |
| 427 | return 0; |
| 428 | } |
| 429 | |
| 430 | static void multiq_walk(struct Qdisc *sch, struct qdisc_walker *arg) |
| 431 | { |
| 432 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 433 | int band; |
| 434 | |
| 435 | if (arg->stop) |
| 436 | return; |
| 437 | |
| 438 | for (band = 0; band < q->bands; band++) { |
| 439 | if (arg->count < arg->skip) { |
| 440 | arg->count++; |
| 441 | continue; |
| 442 | } |
| 443 | if (arg->fn(sch, band+1, arg) < 0) { |
| 444 | arg->stop = 1; |
| 445 | break; |
| 446 | } |
| 447 | arg->count++; |
| 448 | } |
| 449 | } |
| 450 | |
| 451 | static struct tcf_proto **multiq_find_tcf(struct Qdisc *sch, unsigned long cl) |
| 452 | { |
| 453 | struct multiq_sched_data *q = qdisc_priv(sch); |
| 454 | |
| 455 | if (cl) |
| 456 | return NULL; |
| 457 | return &q->filter_list; |
| 458 | } |
| 459 | |
| 460 | static const struct Qdisc_class_ops multiq_class_ops = { |
| 461 | .graft = multiq_graft, |
| 462 | .leaf = multiq_leaf, |
| 463 | .get = multiq_get, |
| 464 | .put = multiq_put, |
| 465 | .change = multiq_change, |
| 466 | .delete = multiq_delete, |
| 467 | .walk = multiq_walk, |
| 468 | .tcf_chain = multiq_find_tcf, |
| 469 | .bind_tcf = multiq_bind, |
| 470 | .unbind_tcf = multiq_put, |
| 471 | .dump = multiq_dump_class, |
| 472 | .dump_stats = multiq_dump_class_stats, |
| 473 | }; |
| 474 | |
| 475 | static struct Qdisc_ops multiq_qdisc_ops __read_mostly = { |
| 476 | .next = NULL, |
| 477 | .cl_ops = &multiq_class_ops, |
| 478 | .id = "multiq", |
| 479 | .priv_size = sizeof(struct multiq_sched_data), |
| 480 | .enqueue = multiq_enqueue, |
| 481 | .dequeue = multiq_dequeue, |
Jarek Poplawski | 8e3af97 | 2008-10-31 00:45:55 -0700 | [diff] [blame^] | 482 | .peek = multiq_peek, |
Alexander Duyck | 9265194 | 2008-09-12 16:29:34 -0700 | [diff] [blame] | 483 | .requeue = multiq_requeue, |
| 484 | .drop = multiq_drop, |
| 485 | .init = multiq_init, |
| 486 | .reset = multiq_reset, |
| 487 | .destroy = multiq_destroy, |
| 488 | .change = multiq_tune, |
| 489 | .dump = multiq_dump, |
| 490 | .owner = THIS_MODULE, |
| 491 | }; |
| 492 | |
| 493 | static int __init multiq_module_init(void) |
| 494 | { |
| 495 | return register_qdisc(&multiq_qdisc_ops); |
| 496 | } |
| 497 | |
| 498 | static void __exit multiq_module_exit(void) |
| 499 | { |
| 500 | unregister_qdisc(&multiq_qdisc_ops); |
| 501 | } |
| 502 | |
| 503 | module_init(multiq_module_init) |
| 504 | module_exit(multiq_module_exit) |
| 505 | |
| 506 | MODULE_LICENSE("GPL"); |