kernel: Make SMP support default (and non-optional).
[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,
108 int (*enqueue)(struct ifaltq *, struct mbuf *, struct altq_pktattr *),
d2c71fa0 109 struct mbuf *(*dequeue)(struct ifaltq *, struct mbuf *, int),
4d723e5a
JS
110 int (*request)(struct ifaltq *, int, void *),
111 void *clfier,
112 void *(*classify)(struct ifaltq *, struct mbuf *,
113 struct altq_pktattr *))
114{
115 if (!ifq_is_ready(ifq))
116 return ENXIO;
117
118 ifq->altq_type = type;
119 ifq->altq_disc = discipline;
120 ifq->altq_enqueue = enqueue;
121 ifq->altq_dequeue = dequeue;
122 ifq->altq_request = request;
123 ifq->altq_clfier = clfier;
124 ifq->altq_classify = classify;
125 ifq->altq_flags &= (ALTQF_CANTCHANGE|ALTQF_ENABLED);
126 return 0;
127}
128
9db4b353
SZ
129static int
130altq_detach_locked(struct ifaltq *ifq)
4d723e5a
JS
131{
132 if (!ifq_is_ready(ifq))
133 return ENXIO;
134 if (ifq_is_enabled(ifq))
135 return EBUSY;
136 if (!ifq_is_attached(ifq))
137 return (0);
138
b2f93efe 139 ifq_set_classic(ifq);
4d723e5a
JS
140 ifq->altq_type = ALTQT_NONE;
141 ifq->altq_disc = NULL;
4d723e5a
JS
142 ifq->altq_clfier = NULL;
143 ifq->altq_classify = NULL;
144 ifq->altq_flags &= ALTQF_CANTCHANGE;
145 return 0;
146}
147
148int
9db4b353
SZ
149altq_detach(struct ifaltq *ifq)
150{
151 int error;
152
153 ALTQ_LOCK(ifq);
154 error = altq_detach_locked(ifq);
155 ALTQ_UNLOCK(ifq);
156 return error;
157}
158
159static int
160altq_enable_locked(struct ifaltq *ifq)
4d723e5a 161{
4d723e5a
JS
162 if (!ifq_is_ready(ifq))
163 return ENXIO;
164 if (ifq_is_enabled(ifq))
165 return 0;
166
9db4b353 167 ifq_purge_locked(ifq);
4d723e5a 168 KKASSERT(ifq->ifq_len == 0);
9db4b353 169
4d723e5a
JS
170 ifq->altq_flags |= ALTQF_ENABLED;
171 if (ifq->altq_clfier != NULL)
172 ifq->altq_flags |= ALTQF_CLASSIFY;
4d723e5a
JS
173 return 0;
174}
175
176int
9db4b353
SZ
177altq_enable(struct ifaltq *ifq)
178{
179 int error;
180
181 ALTQ_LOCK(ifq);
182 error = altq_enable_locked(ifq);
183 ALTQ_UNLOCK(ifq);
184 return error;
185}
186
187static int
188altq_disable_locked(struct ifaltq *ifq)
4d723e5a 189{
4d723e5a
JS
190 if (!ifq_is_enabled(ifq))
191 return 0;
192
9db4b353 193 ifq_purge_locked(ifq);
4d723e5a
JS
194 KKASSERT(ifq->ifq_len == 0);
195 ifq->altq_flags &= ~(ALTQF_ENABLED|ALTQF_CLASSIFY);
4d723e5a
JS
196 return 0;
197}
198
9db4b353
SZ
199int
200altq_disable(struct ifaltq *ifq)
201{
202 int error;
203
204 ALTQ_LOCK(ifq);
205 error = altq_disable_locked(ifq);
206 ALTQ_UNLOCK(ifq);
207 return error;
208}
209
4d723e5a
JS
210/*
211 * internal representation of token bucket parameters
212 * rate: byte_per_unittime << 32
213 * (((bits_per_sec) / 8) << 32) / machclk_freq
214 * depth: byte << 32
215 *
216 */
217#define TBR_SHIFT 32
218#define TBR_SCALE(x) ((int64_t)(x) << TBR_SHIFT)
219#define TBR_UNSCALE(x) ((x) >> TBR_SHIFT)
220
221struct mbuf *
d2c71fa0 222tbr_dequeue(struct ifaltq *ifq, struct mbuf *mpolled, int op)
4d723e5a
JS
223{
224 struct tb_regulator *tbr;
225 struct mbuf *m;
226 int64_t interval;
227 uint64_t now;
228
0b31d406 229 crit_enter();
4d723e5a
JS
230 tbr = ifq->altq_tbr;
231 if (op == ALTDQ_REMOVE && tbr->tbr_lastop == ALTDQ_POLL) {
232 /* if this is a remove after poll, bypass tbr check */
233 } else {
234 /* update token only when it is negative */
235 if (tbr->tbr_token <= 0) {
236 now = read_machclk();
237 interval = now - tbr->tbr_last;
238 if (interval >= tbr->tbr_filluptime)
239 tbr->tbr_token = tbr->tbr_depth;
240 else {
241 tbr->tbr_token += interval * tbr->tbr_rate;
242 if (tbr->tbr_token > tbr->tbr_depth)
243 tbr->tbr_token = tbr->tbr_depth;
244 }
245 tbr->tbr_last = now;
246 }
247 /* if token is still negative, don't allow dequeue */
e9cb6d99 248 if (tbr->tbr_token <= 0) {
0b31d406 249 crit_exit();
4d723e5a 250 return (NULL);
e9cb6d99 251 }
4d723e5a
JS
252 }
253
d2c71fa0
MD
254 if (ifq_is_enabled(ifq)) {
255 m = (*ifq->altq_dequeue)(ifq, mpolled, op);
256 } else if (op == ALTDQ_POLL) {
4d723e5a 257 IF_POLL(ifq, m);
d2c71fa0 258 } else {
4d723e5a 259 IF_DEQUEUE(ifq, m);
d2c71fa0
MD
260 KKASSERT(mpolled == NULL || mpolled == m);
261 }
4d723e5a
JS
262
263 if (m != NULL && op == ALTDQ_REMOVE)
264 tbr->tbr_token -= TBR_SCALE(m_pktlen(m));
265 tbr->tbr_lastop = op;
0b31d406 266 crit_exit();
4d723e5a
JS
267 return (m);
268}
269
270/*
271 * set a token bucket regulator.
272 * if the specified rate is zero, the token bucket regulator is deleted.
273 */
9db4b353
SZ
274static int
275tbr_set_locked(struct ifaltq *ifq, struct tb_profile *profile)
4d723e5a
JS
276{
277 struct tb_regulator *tbr, *otbr;
278
279 if (machclk_freq == 0)
280 init_machclk();
281 if (machclk_freq == 0) {
9db4b353 282 kprintf("%s: no cpu clock available!\n", __func__);
4d723e5a
JS
283 return (ENXIO);
284 }
285
286 if (profile->rate == 0) {
287 /* delete this tbr */
288 if ((tbr = ifq->altq_tbr) == NULL)
289 return (ENOENT);
290 ifq->altq_tbr = NULL;
efda3bd0 291 kfree(tbr, M_ALTQ);
4d723e5a
JS
292 return (0);
293 }
294
efda3bd0 295 tbr = kmalloc(sizeof(*tbr), M_ALTQ, M_WAITOK | M_ZERO);
4d723e5a
JS
296 tbr->tbr_rate = TBR_SCALE(profile->rate / 8) / machclk_freq;
297 tbr->tbr_depth = TBR_SCALE(profile->depth);
298 if (tbr->tbr_rate > 0)
299 tbr->tbr_filluptime = tbr->tbr_depth / tbr->tbr_rate;
300 else
301 tbr->tbr_filluptime = 0xffffffffffffffffLL;
302 tbr->tbr_token = tbr->tbr_depth;
303 tbr->tbr_last = read_machclk();
304 tbr->tbr_lastop = ALTDQ_REMOVE;
305
306 otbr = ifq->altq_tbr;
307 ifq->altq_tbr = tbr; /* set the new tbr */
308
309 if (otbr != NULL)
efda3bd0 310 kfree(otbr, M_ALTQ);
4d723e5a
JS
311 else if (tbr_timer == 0) {
312 callout_reset(&tbr_callout, 1, tbr_timeout, NULL);
313 tbr_timer = 1;
314 }
315 return (0);
316}
317
9db4b353
SZ
318int
319tbr_set(struct ifaltq *ifq, struct tb_profile *profile)
320{
321 int error;
322
323 ALTQ_LOCK(ifq);
324 error = tbr_set_locked(ifq, profile);
325 ALTQ_UNLOCK(ifq);
326 return error;
327}
328
4d723e5a
JS
329/*
330 * tbr_timeout goes through the interface list, and kicks the drivers
331 * if necessary.
332 */
333static void
334tbr_timeout(void *arg)
335{
336 struct ifnet *ifp;
0b31d406 337 int active;
4d723e5a
JS
338
339 active = 0;
0b31d406 340 crit_enter();
4d723e5a
JS
341 for (ifp = TAILQ_FIRST(&ifnet); ifp; ifp = TAILQ_NEXT(ifp, if_list)) {
342 if (ifp->if_snd.altq_tbr == NULL)
343 continue;
344 active++;
78195a76 345 if (!ifq_is_empty(&ifp->if_snd) && ifp->if_start != NULL) {
a3dd34d2 346 ifnet_serialize_tx(ifp);
4d723e5a 347 (*ifp->if_start)(ifp);
a3dd34d2 348 ifnet_deserialize_tx(ifp);
78195a76 349 }
4d723e5a 350 }
0b31d406 351 crit_exit();
4d723e5a
JS
352 if (active > 0)
353 callout_reset(&tbr_callout, 1, tbr_timeout, NULL);
354 else
355 tbr_timer = 0; /* don't need tbr_timer anymore */
356}
357
358/*
359 * get token bucket regulator profile
360 */
361int
362tbr_get(struct ifaltq *ifq, struct tb_profile *profile)
363{
364 struct tb_regulator *tbr;
365
366 if ((tbr = ifq->altq_tbr) == NULL) {
367 profile->rate = 0;
368 profile->depth = 0;
369 } else {
370 profile->rate =
371 (u_int)TBR_UNSCALE(tbr->tbr_rate * 8 * machclk_freq);
372 profile->depth = (u_int)TBR_UNSCALE(tbr->tbr_depth);
373 }
374 return (0);
375}
376
377/*
378 * attach a discipline to the interface. if one already exists, it is
379 * overridden.
380 */
381int
382altq_pfattach(struct pf_altq *a)
383{
9db4b353 384 struct ifaltq *ifq;
4d723e5a 385 struct ifnet *ifp;
9db4b353
SZ
386 int error;
387
388 if (a->scheduler == ALTQT_NONE)
389 return 0;
390
391 if (a->altq_disc == NULL)
392 return EINVAL;
393
394 ifp = ifunit(a->ifname);
395 if (ifp == NULL)
396 return EINVAL;
397 ifq = &ifp->if_snd;
398
399 ALTQ_LOCK(ifq);
4d723e5a
JS
400
401 switch (a->scheduler) {
4d723e5a
JS
402#ifdef ALTQ_CBQ
403 case ALTQT_CBQ:
9db4b353 404 error = cbq_pfattach(a, ifq);
4d723e5a
JS
405 break;
406#endif
407#ifdef ALTQ_PRIQ
408 case ALTQT_PRIQ:
9db4b353 409 error = priq_pfattach(a, ifq);
4d723e5a
JS
410 break;
411#endif
412#ifdef ALTQ_HFSC
413 case ALTQT_HFSC:
9db4b353 414 error = hfsc_pfattach(a, ifq);
4d723e5a 415 break;
5950bf01
MD
416#endif
417#ifdef ALTQ_FAIRQ
418 case ALTQT_FAIRQ:
9db4b353 419 error = fairq_pfattach(a, ifq);
5950bf01 420 break;
4d723e5a
JS
421#endif
422 default:
423 error = ENXIO;
9db4b353 424 goto back;
4d723e5a
JS
425 }
426
4d723e5a 427 /* if the state is running, enable altq */
9db4b353
SZ
428 if (error == 0 && pfaltq_running && ifq->altq_type != ALTQT_NONE &&
429 !ifq_is_enabled(ifq))
430 error = altq_enable_locked(ifq);
4d723e5a
JS
431
432 /* if altq is already enabled, reset set tokenbucket regulator */
9db4b353
SZ
433 if (error == 0 && ifq_is_enabled(ifq)) {
434 struct tb_profile tb;
435
4d723e5a
JS
436 tb.rate = a->ifbandwidth;
437 tb.depth = a->tbrsize;
9db4b353 438 error = tbr_set_locked(ifq, &tb);
4d723e5a 439 }
9db4b353
SZ
440back:
441 ALTQ_UNLOCK(ifq);
4d723e5a
JS
442 return (error);
443}
444
445/*
446 * detach a discipline from the interface.
447 * it is possible that the discipline was already overridden by another
448 * discipline.
449 */
450int
451altq_pfdetach(struct pf_altq *a)
452{
453 struct ifnet *ifp;
9db4b353 454 struct ifaltq *ifq;
0b31d406 455 int error = 0;
4d723e5a 456
9db4b353
SZ
457 ifp = ifunit(a->ifname);
458 if (ifp == NULL)
4d723e5a 459 return (EINVAL);
9db4b353 460 ifq = &ifp->if_snd;
4d723e5a
JS
461
462 /* if this discipline is no longer referenced, just return */
9db4b353 463 if (a->altq_disc == NULL)
4d723e5a
JS
464 return (0);
465
9db4b353
SZ
466 ALTQ_LOCK(ifq);
467
468 if (a->altq_disc != ifq->altq_disc)
469 goto back;
470
471 if (ifq_is_enabled(ifq))
472 error = altq_disable_locked(ifq);
4d723e5a 473 if (error == 0)
9db4b353 474 error = altq_detach_locked(ifq);
4d723e5a 475
9db4b353
SZ
476back:
477 ALTQ_UNLOCK(ifq);
4d723e5a
JS
478 return (error);
479}
480
481/*
482 * add a discipline or a queue
483 */
484int
485altq_add(struct pf_altq *a)
486{
487 int error = 0;
488
489 if (a->qname[0] != 0)
490 return (altq_add_queue(a));
491
492 if (machclk_freq == 0)
493 init_machclk();
494 if (machclk_freq == 0)
495 panic("altq_add: no cpu clock");
496
497 switch (a->scheduler) {
498#ifdef ALTQ_CBQ
499 case ALTQT_CBQ:
500 error = cbq_add_altq(a);
501 break;
502#endif
503#ifdef ALTQ_PRIQ
504 case ALTQT_PRIQ:
505 error = priq_add_altq(a);
506 break;
507#endif
508#ifdef ALTQ_HFSC
509 case ALTQT_HFSC:
510 error = hfsc_add_altq(a);
511 break;
5950bf01
MD
512#endif
513#ifdef ALTQ_FAIRQ
514 case ALTQT_FAIRQ:
515 error = fairq_add_altq(a);
516 break;
4d723e5a
JS
517#endif
518 default:
519 error = ENXIO;
520 }
521
522 return (error);
523}
524
525/*
526 * remove a discipline or a queue
527 */
528int
529altq_remove(struct pf_altq *a)
530{
531 int error = 0;
532
533 if (a->qname[0] != 0)
534 return (altq_remove_queue(a));
535
536 switch (a->scheduler) {
537#ifdef ALTQ_CBQ
538 case ALTQT_CBQ:
539 error = cbq_remove_altq(a);
540 break;
541#endif
542#ifdef ALTQ_PRIQ
543 case ALTQT_PRIQ:
544 error = priq_remove_altq(a);
545 break;
546#endif
547#ifdef ALTQ_HFSC
548 case ALTQT_HFSC:
549 error = hfsc_remove_altq(a);
550 break;
5950bf01
MD
551#endif
552#ifdef ALTQ_FAIRQ
553 case ALTQT_FAIRQ:
554 error = fairq_remove_altq(a);
555 break;
4d723e5a
JS
556#endif
557 default:
558 error = ENXIO;
559 }
560
561 return (error);
562}
563
564/*
565 * add a queue to the discipline
566 */
567int
568altq_add_queue(struct pf_altq *a)
569{
570 int error = 0;
571
572 switch (a->scheduler) {
573#ifdef ALTQ_CBQ
574 case ALTQT_CBQ:
575 error = cbq_add_queue(a);
576 break;
577#endif
578#ifdef ALTQ_PRIQ
579 case ALTQT_PRIQ:
580 error = priq_add_queue(a);
581 break;
582#endif
583#ifdef ALTQ_HFSC
584 case ALTQT_HFSC:
585 error = hfsc_add_queue(a);
586 break;
5950bf01
MD
587#endif
588#ifdef ALTQ_FAIRQ
589 case ALTQT_FAIRQ:
590 error = fairq_add_queue(a);
591 break;
4d723e5a
JS
592#endif
593 default:
594 error = ENXIO;
595 }
596
597 return (error);
598}
599
600/*
601 * remove a queue from the discipline
602 */
603int
604altq_remove_queue(struct pf_altq *a)
605{
606 int error = 0;
607
608 switch (a->scheduler) {
609#ifdef ALTQ_CBQ
610 case ALTQT_CBQ:
611 error = cbq_remove_queue(a);
612 break;
613#endif
614#ifdef ALTQ_PRIQ
615 case ALTQT_PRIQ:
616 error = priq_remove_queue(a);
617 break;
618#endif
619#ifdef ALTQ_HFSC
620 case ALTQT_HFSC:
621 error = hfsc_remove_queue(a);
622 break;
5950bf01
MD
623#endif
624#ifdef ALTQ_FAIRQ
625 case ALTQT_FAIRQ:
626 error = fairq_remove_queue(a);
627 break;
4d723e5a
JS
628#endif
629 default:
630 error = ENXIO;
631 }
632
633 return (error);
634}
635
636/*
637 * get queue statistics
638 */
639int
640altq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes)
641{
642 int error = 0;
643
644 switch (a->scheduler) {
645#ifdef ALTQ_CBQ
646 case ALTQT_CBQ:
647 error = cbq_getqstats(a, ubuf, nbytes);
648 break;
649#endif
650#ifdef ALTQ_PRIQ
651 case ALTQT_PRIQ:
652 error = priq_getqstats(a, ubuf, nbytes);
653 break;
654#endif
655#ifdef ALTQ_HFSC
656 case ALTQT_HFSC:
657 error = hfsc_getqstats(a, ubuf, nbytes);
658 break;
5950bf01
MD
659#endif
660#ifdef ALTQ_FAIRQ
661 case ALTQT_FAIRQ:
662 error = fairq_getqstats(a, ubuf, nbytes);
663 break;
4d723e5a
JS
664#endif
665 default:
666 error = ENXIO;
667 }
668
669 return (error);
670}
671
672/*
673 * read and write diffserv field in IPv4 or IPv6 header
674 */
675uint8_t
676read_dsfield(struct mbuf *m, struct altq_pktattr *pktattr)
677{
678 struct mbuf *m0;
679 uint8_t ds_field = 0;
680
681 if (pktattr == NULL ||
682 (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
683 return ((uint8_t)0);
684
685 /* verify that pattr_hdr is within the mbuf data */
686 for (m0 = m; m0 != NULL; m0 = m0->m_next) {
687 if ((pktattr->pattr_hdr >= m0->m_data) &&
688 (pktattr->pattr_hdr < m0->m_data + m0->m_len))
689 break;
690 }
691 if (m0 == NULL) {
692 /* ick, pattr_hdr is stale */
693 pktattr->pattr_af = AF_UNSPEC;
694#ifdef ALTQ_DEBUG
4b1cf444 695 kprintf("read_dsfield: can't locate header!\n");
4d723e5a
JS
696#endif
697 return ((uint8_t)0);
698 }
699
700 if (pktattr->pattr_af == AF_INET) {
701 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
702
703 if (ip->ip_v != 4)
704 return ((uint8_t)0); /* version mismatch! */
705 ds_field = ip->ip_tos;
706 }
707#ifdef INET6
708 else if (pktattr->pattr_af == AF_INET6) {
709 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
710 uint32_t flowlabel;
711
712 flowlabel = ntohl(ip6->ip6_flow);
713 if ((flowlabel >> 28) != 6)
714 return ((uint8_t)0); /* version mismatch! */
715 ds_field = (flowlabel >> 20) & 0xff;
716 }
717#endif
718 return (ds_field);
719}
720
721void
722write_dsfield(struct mbuf *m, struct altq_pktattr *pktattr, uint8_t dsfield)
723{
724 struct mbuf *m0;
725
726 if (pktattr == NULL ||
727 (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
728 return;
729
730 /* verify that pattr_hdr is within the mbuf data */
731 for (m0 = m; m0 != NULL; m0 = m0->m_next) {
732 if ((pktattr->pattr_hdr >= m0->m_data) &&
733 (pktattr->pattr_hdr < m0->m_data + m0->m_len))
734 break;
735 }
736 if (m0 == NULL) {
737 /* ick, pattr_hdr is stale */
738 pktattr->pattr_af = AF_UNSPEC;
739#ifdef ALTQ_DEBUG
4b1cf444 740 kprintf("write_dsfield: can't locate header!\n");
4d723e5a
JS
741#endif
742 return;
743 }
744
745 if (pktattr->pattr_af == AF_INET) {
746 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
747 uint8_t old;
748 int32_t sum;
749
750 if (ip->ip_v != 4)
751 return; /* version mismatch! */
752 old = ip->ip_tos;
753 dsfield |= old & 3; /* leave CU bits */
754 if (old == dsfield)
755 return;
756 ip->ip_tos = dsfield;
757 /*
758 * update checksum (from RFC1624)
759 * HC' = ~(~HC + ~m + m')
760 */
761 sum = ~ntohs(ip->ip_sum) & 0xffff;
762 sum += 0xff00 + (~old & 0xff) + dsfield;
763 sum = (sum >> 16) + (sum & 0xffff);
764 sum += (sum >> 16); /* add carry */
765
766 ip->ip_sum = htons(~sum & 0xffff);
767 }
768#ifdef INET6
769 else if (pktattr->pattr_af == AF_INET6) {
770 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
771 uint32_t flowlabel;
772
773 flowlabel = ntohl(ip6->ip6_flow);
774 if ((flowlabel >> 28) != 6)
775 return; /* version mismatch! */
776 flowlabel = (flowlabel & 0xf03fffff) | (dsfield << 20);
777 ip6->ip6_flow = htonl(flowlabel);
778 }
779#endif
780}
781
782/*
783 * high resolution clock support taking advantage of a machine dependent
784 * high resolution time counter (e.g., timestamp counter of intel pentium).
785 * we assume
786 * - 64-bit-long monotonically-increasing counter
787 * - frequency range is 100M-4GHz (CPU speed)
788 */
789/* if pcc is not available or disabled, emulate 256MHz using microtime() */
790#define MACHCLK_SHIFT 8
791
792int machclk_usepcc;
870b0161 793uint64_t machclk_freq = 0;
4d723e5a
JS
794uint32_t machclk_per_tick = 0;
795
796void
797init_machclk(void)
798{
799 callout_init(&tbr_callout);
800
801 machclk_usepcc = 1;
802
803#if !defined(__i386__) || defined(ALTQ_NOPCC)
804 machclk_usepcc = 0;
1918fc5c 805#elif defined(__DragonFly__)
4d723e5a
JS
806 machclk_usepcc = 0;
807#elif defined(__i386__)
808 /* check if TSC is available */
809 if (machclk_usepcc == 1 && (cpu_feature & CPUID_TSC) == 0)
810 machclk_usepcc = 0;
811#endif
812
813 if (machclk_usepcc == 0) {
814 /* emulate 256MHz using microtime() */
870b0161 815 machclk_freq = 1000000LLU << MACHCLK_SHIFT;
4d723e5a
JS
816 machclk_per_tick = machclk_freq / hz;
817#ifdef ALTQ_DEBUG
0e6594a8 818 kprintf("altq: emulate %juHz cpu clock\n", (uintmax_t)machclk_freq);
4d723e5a
JS
819#endif
820 return;
821 }
822
823 /*
824 * if the clock frequency (of Pentium TSC or Alpha PCC) is
825 * accessible, just use it.
826 */
94132b9c 827#ifdef _RDTSC_SUPPORTED_
870b0161
MD
828 if (cpu_feature & CPUID_TSC)
829 machclk_freq = (uint64_t)tsc_frequency;
4d723e5a
JS
830#endif
831
832 /*
833 * if we don't know the clock frequency, measure it.
834 */
835 if (machclk_freq == 0) {
836 static int wait;
837 struct timeval tv_start, tv_end;
838 uint64_t start, end, diff;
839 int timo;
840
841 microtime(&tv_start);
842 start = read_machclk();
843 timo = hz; /* 1 sec */
844 tsleep(&wait, PCATCH, "init_machclk", timo);
845 microtime(&tv_end);
846 end = read_machclk();
847 diff = (uint64_t)(tv_end.tv_sec - tv_start.tv_sec) * 1000000
848 + tv_end.tv_usec - tv_start.tv_usec;
849 if (diff != 0)
870b0161 850 machclk_freq = (end - start) * 1000000 / diff;
4d723e5a
JS
851 }
852
853 machclk_per_tick = machclk_freq / hz;
854
855#ifdef ALTQ_DEBUG
0e6594a8 856 kprintf("altq: CPU clock: %juHz\n", (uintmax_t)machclk_freq);
4d723e5a
JS
857#endif
858}
859
860uint64_t
861read_machclk(void)
862{
863 uint64_t val;
864
865 if (machclk_usepcc) {
870b0161 866#ifdef _RDTSC_SUPPORTED_
4d723e5a
JS
867 val = rdtsc();
868#else
869 panic("read_machclk");
870#endif
871 } else {
872 struct timeval tv;
873
874 microtime(&tv);
875 val = (((uint64_t)(tv.tv_sec - boottime.tv_sec) * 1000000
876 + tv.tv_usec) << MACHCLK_SHIFT);
877 }
878 return (val);
879}
70224baa 880