if: Multiple TX queue support step 3 of 3; map CPUID to subqueue
[dragonfly.git] / sys / net / altq / altq_subr.c
CommitLineData
4d723e5a 1/* $KAME: altq_subr.c,v 1.23 2004/04/20 16:10:06 itojun Exp $ */
9db4b353 2/* $DragonFly: src/sys/net/altq/altq_subr.c,v 1.12 2008/05/14 11:59:23 sephe Exp $ */
4d723e5a
JS
3
4/*
5 * Copyright (C) 1997-2003
6 * Sony Computer Science Laboratories Inc. All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 * 1. Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * 2. Redistributions in binary form must reproduce the above copyright
14 * notice, this list of conditions and the following disclaimer in the
15 * documentation and/or other materials provided with the distribution.
16 *
17 * THIS SOFTWARE IS PROVIDED BY SONY CSL AND CONTRIBUTORS ``AS IS'' AND
18 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 * ARE DISCLAIMED. IN NO EVENT SHALL SONY CSL OR CONTRIBUTORS BE LIABLE
21 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
23 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
24 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
26 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
27 * SUCH DAMAGE.
28 */
29
30#include "opt_altq.h"
31#include "opt_inet.h"
32#include "opt_inet6.h"
33
34#include <sys/param.h>
35#include <sys/malloc.h>
36#include <sys/mbuf.h>
37#include <sys/systm.h>
38#include <sys/proc.h>
39#include <sys/socket.h>
40#include <sys/socketvar.h>
41#include <sys/kernel.h>
42#include <sys/callout.h>
43#include <sys/errno.h>
44#include <sys/syslog.h>
45#include <sys/sysctl.h>
46#include <sys/queue.h>
0b31d406 47#include <sys/thread2.h>
4d723e5a
JS
48
49#include <net/if.h>
50#include <net/if_dl.h>
51#include <net/if_types.h>
52#include <net/ifq_var.h>
53
54#include <netinet/in.h>
55#include <netinet/in_systm.h>
56#include <netinet/ip.h>
57#ifdef INET6
58#include <netinet/ip6.h>
59#endif
60#include <netinet/tcp.h>
61#include <netinet/udp.h>
62
63#include <net/pf/pfvar.h>
64#include <net/altq/altq.h>
65
66/* machine dependent clock related includes */
870b0161 67#include <machine/clock.h> /* for tsc_frequency */
4d723e5a
JS
68#include <machine/md_var.h> /* for cpu_feature */
69#include <machine/specialreg.h> /* for CPUID_TSC */
4d723e5a
JS
70
71/*
72 * internal function prototypes
73 */
74static void tbr_timeout(void *);
9db4b353
SZ
75static int altq_enable_locked(struct ifaltq *);
76static int altq_disable_locked(struct ifaltq *);
77static int altq_detach_locked(struct ifaltq *);
78static int tbr_set_locked(struct ifaltq *, struct tb_profile *);
79
4d723e5a
JS
80int (*altq_input)(struct mbuf *, int) = NULL;
81static int tbr_timer = 0; /* token bucket regulator timer */
82static struct callout tbr_callout;
83
84int pfaltq_running; /* keep track of running state */
85
86MALLOC_DEFINE(M_ALTQ, "altq", "ALTQ structures");
87
88/*
89 * alternate queueing support routines
90 */
91
92/* look up the queue state by the interface name and the queueing type. */
93void *
94altq_lookup(const char *name, int type)
95{
96 struct ifnet *ifp;
97
98 if ((ifp = ifunit(name)) != NULL) {
99 if (type != ALTQT_NONE && ifp->if_snd.altq_type == type)
100 return (ifp->if_snd.altq_disc);
101 }
102
103 return (NULL);
104}
105
106int
107altq_attach(struct ifaltq *ifq, int type, void *discipline,
2cc2f639 108 altq_mapsubq_t mapsubq,
f0a26983
SZ
109 ifsq_enqueue_t enqueue, ifsq_dequeue_t dequeue, ifsq_request_t request,
110 void *clfier,
111 void *(*classify)(struct ifaltq *, struct mbuf *, struct altq_pktattr *))
4d723e5a
JS
112{
113 if (!ifq_is_ready(ifq))
114 return ENXIO;
115
116 ifq->altq_type = type;
117 ifq->altq_disc = discipline;
4d723e5a
JS
118 ifq->altq_clfier = clfier;
119 ifq->altq_classify = classify;
120 ifq->altq_flags &= (ALTQF_CANTCHANGE|ALTQF_ENABLED);
2cc2f639 121 ifq_set_methods(ifq, mapsubq, enqueue, dequeue, request);
4d723e5a
JS
122 return 0;
123}
124
9db4b353
SZ
125static int
126altq_detach_locked(struct ifaltq *ifq)
4d723e5a
JS
127{
128 if (!ifq_is_ready(ifq))
129 return ENXIO;
130 if (ifq_is_enabled(ifq))
131 return EBUSY;
132 if (!ifq_is_attached(ifq))
133 return (0);
134
b2f93efe 135 ifq_set_classic(ifq);
4d723e5a
JS
136 ifq->altq_type = ALTQT_NONE;
137 ifq->altq_disc = NULL;
4d723e5a
JS
138 ifq->altq_clfier = NULL;
139 ifq->altq_classify = NULL;
140 ifq->altq_flags &= ALTQF_CANTCHANGE;
141 return 0;
142}
143
144int
9db4b353
SZ
145altq_detach(struct ifaltq *ifq)
146{
147 int error;
148
f0a26983 149 ifq_lock_all(ifq);
9db4b353 150 error = altq_detach_locked(ifq);
f0a26983 151 ifq_unlock_all(ifq);
9db4b353
SZ
152 return error;
153}
154
155static int
156altq_enable_locked(struct ifaltq *ifq)
4d723e5a 157{
4d723e5a
JS
158 if (!ifq_is_ready(ifq))
159 return ENXIO;
160 if (ifq_is_enabled(ifq))
161 return 0;
162
9275f515 163 ifq_purge_all_locked(ifq);
9db4b353 164
4d723e5a
JS
165 ifq->altq_flags |= ALTQF_ENABLED;
166 if (ifq->altq_clfier != NULL)
167 ifq->altq_flags |= ALTQF_CLASSIFY;
4d723e5a
JS
168 return 0;
169}
170
171int
9db4b353
SZ
172altq_enable(struct ifaltq *ifq)
173{
174 int error;
175
f0a26983 176 ifq_lock_all(ifq);
9db4b353 177 error = altq_enable_locked(ifq);
f0a26983 178 ifq_unlock_all(ifq);
9db4b353
SZ
179 return error;
180}
181
182static int
183altq_disable_locked(struct ifaltq *ifq)
4d723e5a 184{
4d723e5a
JS
185 if (!ifq_is_enabled(ifq))
186 return 0;
187
9275f515 188 ifq_purge_all_locked(ifq);
4d723e5a 189 ifq->altq_flags &= ~(ALTQF_ENABLED|ALTQF_CLASSIFY);
4d723e5a
JS
190 return 0;
191}
192
9db4b353
SZ
193int
194altq_disable(struct ifaltq *ifq)
195{
196 int error;
197
f0a26983 198 ifq_lock_all(ifq);
9db4b353 199 error = altq_disable_locked(ifq);
f0a26983 200 ifq_unlock_all(ifq);
9db4b353
SZ
201 return error;
202}
203
4d723e5a
JS
204/*
205 * internal representation of token bucket parameters
206 * rate: byte_per_unittime << 32
207 * (((bits_per_sec) / 8) << 32) / machclk_freq
208 * depth: byte << 32
209 *
210 */
211#define TBR_SHIFT 32
212#define TBR_SCALE(x) ((int64_t)(x) << TBR_SHIFT)
213#define TBR_UNSCALE(x) ((x) >> TBR_SHIFT)
214
215struct mbuf *
f0a26983 216tbr_dequeue(struct ifaltq_subque *ifsq, struct mbuf *mpolled, int op)
4d723e5a 217{
f0a26983 218 struct ifaltq *ifq = ifsq->ifsq_altq;
4d723e5a
JS
219 struct tb_regulator *tbr;
220 struct mbuf *m;
221 int64_t interval;
222 uint64_t now;
223
f0a26983
SZ
224 if (ifsq_get_index(ifsq) != ALTQ_SUBQ_INDEX_DEFAULT) {
225 /*
226 * Race happened, the unrelated subqueue was
227 * picked during the packet scheduler transition.
228 */
229 ifsq_classic_request(ifsq, ALTRQ_PURGE, NULL);
230 return NULL;
231 }
232
0b31d406 233 crit_enter();
4d723e5a
JS
234 tbr = ifq->altq_tbr;
235 if (op == ALTDQ_REMOVE && tbr->tbr_lastop == ALTDQ_POLL) {
236 /* if this is a remove after poll, bypass tbr check */
237 } else {
238 /* update token only when it is negative */
239 if (tbr->tbr_token <= 0) {
240 now = read_machclk();
241 interval = now - tbr->tbr_last;
242 if (interval >= tbr->tbr_filluptime)
243 tbr->tbr_token = tbr->tbr_depth;
244 else {
245 tbr->tbr_token += interval * tbr->tbr_rate;
246 if (tbr->tbr_token > tbr->tbr_depth)
247 tbr->tbr_token = tbr->tbr_depth;
248 }
249 tbr->tbr_last = now;
250 }
251 /* if token is still negative, don't allow dequeue */
e9cb6d99 252 if (tbr->tbr_token <= 0) {
0b31d406 253 crit_exit();
4d723e5a 254 return (NULL);
e9cb6d99 255 }
4d723e5a
JS
256 }
257
d2c71fa0 258 if (ifq_is_enabled(ifq)) {
f0a26983 259 m = (*ifsq->ifsq_dequeue)(ifsq, mpolled, op);
d2c71fa0 260 } else if (op == ALTDQ_POLL) {
f0a26983 261 IF_POLL(ifsq, m);
d2c71fa0 262 } else {
f0a26983 263 IF_DEQUEUE(ifsq, m);
d2c71fa0
MD
264 KKASSERT(mpolled == NULL || mpolled == m);
265 }
4d723e5a
JS
266
267 if (m != NULL && op == ALTDQ_REMOVE)
268 tbr->tbr_token -= TBR_SCALE(m_pktlen(m));
269 tbr->tbr_lastop = op;
0b31d406 270 crit_exit();
4d723e5a
JS
271 return (m);
272}
273
274/*
275 * set a token bucket regulator.
276 * if the specified rate is zero, the token bucket regulator is deleted.
277 */
9db4b353
SZ
278static int
279tbr_set_locked(struct ifaltq *ifq, struct tb_profile *profile)
4d723e5a
JS
280{
281 struct tb_regulator *tbr, *otbr;
282
283 if (machclk_freq == 0)
284 init_machclk();
285 if (machclk_freq == 0) {
9db4b353 286 kprintf("%s: no cpu clock available!\n", __func__);
4d723e5a
JS
287 return (ENXIO);
288 }
289
290 if (profile->rate == 0) {
291 /* delete this tbr */
292 if ((tbr = ifq->altq_tbr) == NULL)
293 return (ENOENT);
294 ifq->altq_tbr = NULL;
efda3bd0 295 kfree(tbr, M_ALTQ);
4d723e5a
JS
296 return (0);
297 }
298
efda3bd0 299 tbr = kmalloc(sizeof(*tbr), M_ALTQ, M_WAITOK | M_ZERO);
4d723e5a
JS
300 tbr->tbr_rate = TBR_SCALE(profile->rate / 8) / machclk_freq;
301 tbr->tbr_depth = TBR_SCALE(profile->depth);
302 if (tbr->tbr_rate > 0)
303 tbr->tbr_filluptime = tbr->tbr_depth / tbr->tbr_rate;
304 else
305 tbr->tbr_filluptime = 0xffffffffffffffffLL;
306 tbr->tbr_token = tbr->tbr_depth;
307 tbr->tbr_last = read_machclk();
308 tbr->tbr_lastop = ALTDQ_REMOVE;
309
310 otbr = ifq->altq_tbr;
311 ifq->altq_tbr = tbr; /* set the new tbr */
312
313 if (otbr != NULL)
efda3bd0 314 kfree(otbr, M_ALTQ);
4d723e5a
JS
315 else if (tbr_timer == 0) {
316 callout_reset(&tbr_callout, 1, tbr_timeout, NULL);
317 tbr_timer = 1;
318 }
319 return (0);
320}
321
9db4b353
SZ
322int
323tbr_set(struct ifaltq *ifq, struct tb_profile *profile)
324{
325 int error;
326
f0a26983 327 ifq_lock_all(ifq);
9db4b353 328 error = tbr_set_locked(ifq, profile);
f0a26983 329 ifq_unlock_all(ifq);
9db4b353
SZ
330 return error;
331}
332
4d723e5a
JS
333/*
334 * tbr_timeout goes through the interface list, and kicks the drivers
335 * if necessary.
336 */
337static void
338tbr_timeout(void *arg)
339{
340 struct ifnet *ifp;
0b31d406 341 int active;
4d723e5a
JS
342
343 active = 0;
0b31d406 344 crit_enter();
4d723e5a 345 for (ifp = TAILQ_FIRST(&ifnet); ifp; ifp = TAILQ_NEXT(ifp, if_list)) {
3c4cd924 346 struct ifaltq_subque *ifsq;
f0a26983 347
4d723e5a
JS
348 if (ifp->if_snd.altq_tbr == NULL)
349 continue;
3c4cd924
SZ
350
351 ifsq = &ifp->if_snd.altq_subq[ALTQ_SUBQ_INDEX_DEFAULT];
4d723e5a 352 active++;
f0a26983 353 if (!ifsq_is_empty(ifsq) && ifp->if_start != NULL) {
3c4cd924 354 ifnet_serialize_tx(ifp, ifsq);
f0a26983 355 (*ifp->if_start)(ifp, ifsq);
3c4cd924 356 ifnet_deserialize_tx(ifp, ifsq);
78195a76 357 }
4d723e5a 358 }
0b31d406 359 crit_exit();
4d723e5a
JS
360 if (active > 0)
361 callout_reset(&tbr_callout, 1, tbr_timeout, NULL);
362 else
363 tbr_timer = 0; /* don't need tbr_timer anymore */
364}
365
366/*
367 * get token bucket regulator profile
368 */
369int
370tbr_get(struct ifaltq *ifq, struct tb_profile *profile)
371{
372 struct tb_regulator *tbr;
373
374 if ((tbr = ifq->altq_tbr) == NULL) {
375 profile->rate = 0;
376 profile->depth = 0;
377 } else {
378 profile->rate =
379 (u_int)TBR_UNSCALE(tbr->tbr_rate * 8 * machclk_freq);
380 profile->depth = (u_int)TBR_UNSCALE(tbr->tbr_depth);
381 }
382 return (0);
383}
384
385/*
386 * attach a discipline to the interface. if one already exists, it is
387 * overridden.
388 */
389int
390altq_pfattach(struct pf_altq *a)
391{
9db4b353 392 struct ifaltq *ifq;
4d723e5a 393 struct ifnet *ifp;
9db4b353
SZ
394 int error;
395
396 if (a->scheduler == ALTQT_NONE)
397 return 0;
398
399 if (a->altq_disc == NULL)
400 return EINVAL;
401
402 ifp = ifunit(a->ifname);
403 if (ifp == NULL)
404 return EINVAL;
405 ifq = &ifp->if_snd;
406
f0a26983 407 ifq_lock_all(ifq);
4d723e5a
JS
408
409 switch (a->scheduler) {
4d723e5a
JS
410#ifdef ALTQ_CBQ
411 case ALTQT_CBQ:
9db4b353 412 error = cbq_pfattach(a, ifq);
4d723e5a
JS
413 break;
414#endif
415#ifdef ALTQ_PRIQ
416 case ALTQT_PRIQ:
9db4b353 417 error = priq_pfattach(a, ifq);
4d723e5a
JS
418 break;
419#endif
420#ifdef ALTQ_HFSC
421 case ALTQT_HFSC:
9db4b353 422 error = hfsc_pfattach(a, ifq);
4d723e5a
JS
423 break;
424#endif
5950bf01
MD
425#ifdef ALTQ_FAIRQ
426 case ALTQT_FAIRQ:
9db4b353 427 error = fairq_pfattach(a, ifq);
5950bf01
MD
428 break;
429#endif
4d723e5a
JS
430 default:
431 error = ENXIO;
9db4b353 432 goto back;
4d723e5a
JS
433 }
434
4d723e5a 435 /* if the state is running, enable altq */
9db4b353
SZ
436 if (error == 0 && pfaltq_running && ifq->altq_type != ALTQT_NONE &&
437 !ifq_is_enabled(ifq))
438 error = altq_enable_locked(ifq);
4d723e5a
JS
439
440 /* if altq is already enabled, reset set tokenbucket regulator */
9db4b353
SZ
441 if (error == 0 && ifq_is_enabled(ifq)) {
442 struct tb_profile tb;
443
4d723e5a
JS
444 tb.rate = a->ifbandwidth;
445 tb.depth = a->tbrsize;
9db4b353 446 error = tbr_set_locked(ifq, &tb);
4d723e5a 447 }
9db4b353 448back:
f0a26983 449 ifq_unlock_all(ifq);
4d723e5a
JS
450 return (error);
451}
452
453/*
454 * detach a discipline from the interface.
455 * it is possible that the discipline was already overridden by another
456 * discipline.
457 */
458int
459altq_pfdetach(struct pf_altq *a)
460{
461 struct ifnet *ifp;
9db4b353 462 struct ifaltq *ifq;
0b31d406 463 int error = 0;
4d723e5a 464
9db4b353
SZ
465 ifp = ifunit(a->ifname);
466 if (ifp == NULL)
4d723e5a 467 return (EINVAL);
9db4b353 468 ifq = &ifp->if_snd;
4d723e5a
JS
469
470 /* if this discipline is no longer referenced, just return */
9db4b353 471 if (a->altq_disc == NULL)
4d723e5a
JS
472 return (0);
473
f0a26983 474 ifq_lock_all(ifq);
9db4b353
SZ
475
476 if (a->altq_disc != ifq->altq_disc)
477 goto back;
478
479 if (ifq_is_enabled(ifq))
480 error = altq_disable_locked(ifq);
4d723e5a 481 if (error == 0)
9db4b353 482 error = altq_detach_locked(ifq);
4d723e5a 483
9db4b353 484back:
f0a26983 485 ifq_unlock_all(ifq);
4d723e5a
JS
486 return (error);
487}
488
489/*
490 * add a discipline or a queue
491 */
492int
493altq_add(struct pf_altq *a)
494{
495 int error = 0;
496
497 if (a->qname[0] != 0)
498 return (altq_add_queue(a));
499
500 if (machclk_freq == 0)
501 init_machclk();
502 if (machclk_freq == 0)
503 panic("altq_add: no cpu clock");
504
505 switch (a->scheduler) {
506#ifdef ALTQ_CBQ
507 case ALTQT_CBQ:
508 error = cbq_add_altq(a);
509 break;
510#endif
511#ifdef ALTQ_PRIQ
512 case ALTQT_PRIQ:
513 error = priq_add_altq(a);
514 break;
515#endif
516#ifdef ALTQ_HFSC
517 case ALTQT_HFSC:
518 error = hfsc_add_altq(a);
519 break;
520#endif
5950bf01
MD
521#ifdef ALTQ_FAIRQ
522 case ALTQT_FAIRQ:
523 error = fairq_add_altq(a);
524 break;
525#endif
4d723e5a
JS
526 default:
527 error = ENXIO;
528 }
529
530 return (error);
531}
532
533/*
534 * remove a discipline or a queue
535 */
536int
537altq_remove(struct pf_altq *a)
538{
539 int error = 0;
540
541 if (a->qname[0] != 0)
542 return (altq_remove_queue(a));
543
544 switch (a->scheduler) {
545#ifdef ALTQ_CBQ
546 case ALTQT_CBQ:
547 error = cbq_remove_altq(a);
548 break;
549#endif
550#ifdef ALTQ_PRIQ
551 case ALTQT_PRIQ:
552 error = priq_remove_altq(a);
553 break;
554#endif
555#ifdef ALTQ_HFSC
556 case ALTQT_HFSC:
557 error = hfsc_remove_altq(a);
558 break;
559#endif
5950bf01
MD
560#ifdef ALTQ_FAIRQ
561 case ALTQT_FAIRQ:
562 error = fairq_remove_altq(a);
563 break;
564#endif
4d723e5a
JS
565 default:
566 error = ENXIO;
567 }
568
569 return (error);
570}
571
572/*
573 * add a queue to the discipline
574 */
575int
576altq_add_queue(struct pf_altq *a)
577{
578 int error = 0;
579
580 switch (a->scheduler) {
581#ifdef ALTQ_CBQ
582 case ALTQT_CBQ:
583 error = cbq_add_queue(a);
584 break;
585#endif
586#ifdef ALTQ_PRIQ
587 case ALTQT_PRIQ:
588 error = priq_add_queue(a);
589 break;
590#endif
591#ifdef ALTQ_HFSC
592 case ALTQT_HFSC:
593 error = hfsc_add_queue(a);
594 break;
595#endif
5950bf01
MD
596#ifdef ALTQ_FAIRQ
597 case ALTQT_FAIRQ:
598 error = fairq_add_queue(a);
599 break;
600#endif
4d723e5a
JS
601 default:
602 error = ENXIO;
603 }
604
605 return (error);
606}
607
608/*
609 * remove a queue from the discipline
610 */
611int
612altq_remove_queue(struct pf_altq *a)
613{
614 int error = 0;
615
616 switch (a->scheduler) {
617#ifdef ALTQ_CBQ
618 case ALTQT_CBQ:
619 error = cbq_remove_queue(a);
620 break;
621#endif
622#ifdef ALTQ_PRIQ
623 case ALTQT_PRIQ:
624 error = priq_remove_queue(a);
625 break;
626#endif
627#ifdef ALTQ_HFSC
628 case ALTQT_HFSC:
629 error = hfsc_remove_queue(a);
630 break;
631#endif
5950bf01
MD
632#ifdef ALTQ_FAIRQ
633 case ALTQT_FAIRQ:
634 error = fairq_remove_queue(a);
635 break;
636#endif
4d723e5a
JS
637 default:
638 error = ENXIO;
639 }
640
641 return (error);
642}
643
644/*
645 * get queue statistics
646 */
647int
648altq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes)
649{
650 int error = 0;
651
652 switch (a->scheduler) {
653#ifdef ALTQ_CBQ
654 case ALTQT_CBQ:
655 error = cbq_getqstats(a, ubuf, nbytes);
656 break;
657#endif
658#ifdef ALTQ_PRIQ
659 case ALTQT_PRIQ:
660 error = priq_getqstats(a, ubuf, nbytes);
661 break;
662#endif
663#ifdef ALTQ_HFSC
664 case ALTQT_HFSC:
665 error = hfsc_getqstats(a, ubuf, nbytes);
666 break;
667#endif
5950bf01
MD
668#ifdef ALTQ_FAIRQ
669 case ALTQT_FAIRQ:
670 error = fairq_getqstats(a, ubuf, nbytes);
671 break;
672#endif
4d723e5a
JS
673 default:
674 error = ENXIO;
675 }
676
677 return (error);
678}
679
680/*
681 * read and write diffserv field in IPv4 or IPv6 header
682 */
683uint8_t
684read_dsfield(struct mbuf *m, struct altq_pktattr *pktattr)
685{
686 struct mbuf *m0;
687 uint8_t ds_field = 0;
688
689 if (pktattr == NULL ||
690 (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
691 return ((uint8_t)0);
692
693 /* verify that pattr_hdr is within the mbuf data */
694 for (m0 = m; m0 != NULL; m0 = m0->m_next) {
695 if ((pktattr->pattr_hdr >= m0->m_data) &&
696 (pktattr->pattr_hdr < m0->m_data + m0->m_len))
697 break;
698 }
699 if (m0 == NULL) {
700 /* ick, pattr_hdr is stale */
701 pktattr->pattr_af = AF_UNSPEC;
702#ifdef ALTQ_DEBUG
4b1cf444 703 kprintf("read_dsfield: can't locate header!\n");
4d723e5a
JS
704#endif
705 return ((uint8_t)0);
706 }
707
708 if (pktattr->pattr_af == AF_INET) {
709 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
710
711 if (ip->ip_v != 4)
712 return ((uint8_t)0); /* version mismatch! */
713 ds_field = ip->ip_tos;
714 }
715#ifdef INET6
716 else if (pktattr->pattr_af == AF_INET6) {
717 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
718 uint32_t flowlabel;
719
720 flowlabel = ntohl(ip6->ip6_flow);
721 if ((flowlabel >> 28) != 6)
722 return ((uint8_t)0); /* version mismatch! */
723 ds_field = (flowlabel >> 20) & 0xff;
724 }
725#endif
726 return (ds_field);
727}
728
729void
730write_dsfield(struct mbuf *m, struct altq_pktattr *pktattr, uint8_t dsfield)
731{
732 struct mbuf *m0;
733
734 if (pktattr == NULL ||
735 (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
736 return;
737
738 /* verify that pattr_hdr is within the mbuf data */
739 for (m0 = m; m0 != NULL; m0 = m0->m_next) {
740 if ((pktattr->pattr_hdr >= m0->m_data) &&
741 (pktattr->pattr_hdr < m0->m_data + m0->m_len))
742 break;
743 }
744 if (m0 == NULL) {
745 /* ick, pattr_hdr is stale */
746 pktattr->pattr_af = AF_UNSPEC;
747#ifdef ALTQ_DEBUG
4b1cf444 748 kprintf("write_dsfield: can't locate header!\n");
4d723e5a
JS
749#endif
750 return;
751 }
752
753 if (pktattr->pattr_af == AF_INET) {
754 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
755 uint8_t old;
756 int32_t sum;
757
758 if (ip->ip_v != 4)
759 return; /* version mismatch! */
760 old = ip->ip_tos;
761 dsfield |= old & 3; /* leave CU bits */
762 if (old == dsfield)
763 return;
764 ip->ip_tos = dsfield;
765 /*
766 * update checksum (from RFC1624)
767 * HC' = ~(~HC + ~m + m')
768 */
769 sum = ~ntohs(ip->ip_sum) & 0xffff;
770 sum += 0xff00 + (~old & 0xff) + dsfield;
771 sum = (sum >> 16) + (sum & 0xffff);
772 sum += (sum >> 16); /* add carry */
773
774 ip->ip_sum = htons(~sum & 0xffff);
775 }
776#ifdef INET6
777 else if (pktattr->pattr_af == AF_INET6) {
778 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
779 uint32_t flowlabel;
780
781 flowlabel = ntohl(ip6->ip6_flow);
782 if ((flowlabel >> 28) != 6)
783 return; /* version mismatch! */
784 flowlabel = (flowlabel & 0xf03fffff) | (dsfield << 20);
785 ip6->ip6_flow = htonl(flowlabel);
786 }
787#endif
788}
789
790/*
791 * high resolution clock support taking advantage of a machine dependent
792 * high resolution time counter (e.g., timestamp counter of intel pentium).
793 * we assume
794 * - 64-bit-long monotonically-increasing counter
795 * - frequency range is 100M-4GHz (CPU speed)
796 */
797/* if pcc is not available or disabled, emulate 256MHz using microtime() */
798#define MACHCLK_SHIFT 8
799
800int machclk_usepcc;
870b0161 801uint64_t machclk_freq = 0;
4d723e5a
JS
802uint32_t machclk_per_tick = 0;
803
804void
805init_machclk(void)
806{
807 callout_init(&tbr_callout);
808
809 machclk_usepcc = 1;
810
811#if !defined(__i386__) || defined(ALTQ_NOPCC)
812 machclk_usepcc = 0;
1918fc5c 813#elif defined(__DragonFly__)
4d723e5a
JS
814 machclk_usepcc = 0;
815#elif defined(__i386__)
816 /* check if TSC is available */
817 if (machclk_usepcc == 1 && (cpu_feature & CPUID_TSC) == 0)
818 machclk_usepcc = 0;
819#endif
820
821 if (machclk_usepcc == 0) {
822 /* emulate 256MHz using microtime() */
870b0161 823 machclk_freq = 1000000LLU << MACHCLK_SHIFT;
4d723e5a
JS
824 machclk_per_tick = machclk_freq / hz;
825#ifdef ALTQ_DEBUG
0e6594a8 826 kprintf("altq: emulate %juHz cpu clock\n", (uintmax_t)machclk_freq);
4d723e5a
JS
827#endif
828 return;
829 }
830
831 /*
832 * if the clock frequency (of Pentium TSC or Alpha PCC) is
833 * accessible, just use it.
834 */
94132b9c 835#ifdef _RDTSC_SUPPORTED_
870b0161
MD
836 if (cpu_feature & CPUID_TSC)
837 machclk_freq = (uint64_t)tsc_frequency;
4d723e5a
JS
838#endif
839
840 /*
841 * if we don't know the clock frequency, measure it.
842 */
843 if (machclk_freq == 0) {
844 static int wait;
845 struct timeval tv_start, tv_end;
846 uint64_t start, end, diff;
847 int timo;
848
849 microtime(&tv_start);
850 start = read_machclk();
851 timo = hz; /* 1 sec */
852 tsleep(&wait, PCATCH, "init_machclk", timo);
853 microtime(&tv_end);
854 end = read_machclk();
855 diff = (uint64_t)(tv_end.tv_sec - tv_start.tv_sec) * 1000000
856 + tv_end.tv_usec - tv_start.tv_usec;
857 if (diff != 0)
870b0161 858 machclk_freq = (end - start) * 1000000 / diff;
4d723e5a
JS
859 }
860
861 machclk_per_tick = machclk_freq / hz;
862
863#ifdef ALTQ_DEBUG
0e6594a8 864 kprintf("altq: CPU clock: %juHz\n", (uintmax_t)machclk_freq);
4d723e5a
JS
865#endif
866}
867
868uint64_t
869read_machclk(void)
870{
871 uint64_t val;
872
873 if (machclk_usepcc) {
870b0161 874#ifdef _RDTSC_SUPPORTED_
4d723e5a
JS
875 val = rdtsc();
876#else
877 panic("read_machclk");
878#endif
879 } else {
880 struct timeval tv;
881
882 microtime(&tv);
883 val = (((uint64_t)(tv.tv_sec - boottime.tv_sec) * 1000000
884 + tv.tv_usec) << MACHCLK_SHIFT);
885 }
886 return (val);
887}
70224baa 888