if: Multiple TX queue support step 3 of 3; map CPUID to subqueue
[dragonfly.git] / sys / net / altq / altq_subr.c
1 /*      $KAME: altq_subr.c,v 1.23 2004/04/20 16:10:06 itojun Exp $      */
2 /*      $DragonFly: src/sys/net/altq/altq_subr.c,v 1.12 2008/05/14 11:59:23 sephe Exp $ */
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>
47 #include <sys/thread2.h>
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 */
67 #include <machine/clock.h>              /* for tsc_frequency */
68 #include <machine/md_var.h>             /* for cpu_feature */
69 #include <machine/specialreg.h>         /* for CPUID_TSC */
70
71 /*
72  * internal function prototypes
73  */
74 static void     tbr_timeout(void *);
75 static int      altq_enable_locked(struct ifaltq *);
76 static int      altq_disable_locked(struct ifaltq *);
77 static int      altq_detach_locked(struct ifaltq *);
78 static int      tbr_set_locked(struct ifaltq *, struct tb_profile *);
79
80 int (*altq_input)(struct mbuf *, int) = NULL;
81 static int tbr_timer = 0;       /* token bucket regulator timer */
82 static struct callout tbr_callout;
83
84 int pfaltq_running;     /* keep track of running state */
85
86 MALLOC_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. */
93 void *
94 altq_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
106 int
107 altq_attach(struct ifaltq *ifq, int type, void *discipline,
108     altq_mapsubq_t mapsubq,
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 *))
112 {
113         if (!ifq_is_ready(ifq))
114                 return ENXIO;
115
116         ifq->altq_type     = type;
117         ifq->altq_disc     = discipline;
118         ifq->altq_clfier   = clfier;
119         ifq->altq_classify = classify;
120         ifq->altq_flags &= (ALTQF_CANTCHANGE|ALTQF_ENABLED);
121         ifq_set_methods(ifq, mapsubq, enqueue, dequeue, request);
122         return 0;
123 }
124
125 static int
126 altq_detach_locked(struct ifaltq *ifq)
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
135         ifq_set_classic(ifq);
136         ifq->altq_type     = ALTQT_NONE;
137         ifq->altq_disc     = NULL;
138         ifq->altq_clfier   = NULL;
139         ifq->altq_classify = NULL;
140         ifq->altq_flags &= ALTQF_CANTCHANGE;
141         return 0;
142 }
143
144 int
145 altq_detach(struct ifaltq *ifq)
146 {
147         int error;
148
149         ifq_lock_all(ifq);
150         error = altq_detach_locked(ifq);
151         ifq_unlock_all(ifq);
152         return error;
153 }
154
155 static int
156 altq_enable_locked(struct ifaltq *ifq)
157 {
158         if (!ifq_is_ready(ifq))
159                 return ENXIO;
160         if (ifq_is_enabled(ifq))
161                 return 0;
162
163         ifq_purge_all_locked(ifq);
164
165         ifq->altq_flags |= ALTQF_ENABLED;
166         if (ifq->altq_clfier != NULL)
167                 ifq->altq_flags |= ALTQF_CLASSIFY;
168         return 0;
169 }
170
171 int
172 altq_enable(struct ifaltq *ifq)
173 {
174         int error;
175
176         ifq_lock_all(ifq);
177         error = altq_enable_locked(ifq);
178         ifq_unlock_all(ifq);
179         return error;
180 }
181
182 static int
183 altq_disable_locked(struct ifaltq *ifq)
184 {
185         if (!ifq_is_enabled(ifq))
186                 return 0;
187
188         ifq_purge_all_locked(ifq);
189         ifq->altq_flags &= ~(ALTQF_ENABLED|ALTQF_CLASSIFY);
190         return 0;
191 }
192
193 int
194 altq_disable(struct ifaltq *ifq)
195 {
196         int error;
197
198         ifq_lock_all(ifq);
199         error = altq_disable_locked(ifq);
200         ifq_unlock_all(ifq);
201         return error;
202 }
203
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
215 struct mbuf *
216 tbr_dequeue(struct ifaltq_subque *ifsq, struct mbuf *mpolled, int op)
217 {
218         struct ifaltq *ifq = ifsq->ifsq_altq;
219         struct tb_regulator *tbr;
220         struct mbuf *m;
221         int64_t interval;
222         uint64_t now;
223
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
233         crit_enter();
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 */
252                 if (tbr->tbr_token <= 0) {
253                         crit_exit();
254                         return (NULL);
255                 }
256         }
257
258         if (ifq_is_enabled(ifq)) {
259                 m = (*ifsq->ifsq_dequeue)(ifsq, mpolled, op);
260         } else if (op == ALTDQ_POLL) {
261                 IF_POLL(ifsq, m);
262         } else {
263                 IF_DEQUEUE(ifsq, m);
264                 KKASSERT(mpolled == NULL || mpolled == m);
265         }
266
267         if (m != NULL && op == ALTDQ_REMOVE)
268                 tbr->tbr_token -= TBR_SCALE(m_pktlen(m));
269         tbr->tbr_lastop = op;
270         crit_exit();
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  */
278 static int
279 tbr_set_locked(struct ifaltq *ifq, struct tb_profile *profile)
280 {
281         struct tb_regulator *tbr, *otbr;
282
283         if (machclk_freq == 0)
284                 init_machclk();
285         if (machclk_freq == 0) {
286                 kprintf("%s: no cpu clock available!\n", __func__);
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;
295                 kfree(tbr, M_ALTQ);
296                 return (0);
297         }
298
299         tbr = kmalloc(sizeof(*tbr), M_ALTQ, M_WAITOK | M_ZERO);
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)
314                 kfree(otbr, M_ALTQ);
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
322 int
323 tbr_set(struct ifaltq *ifq, struct tb_profile *profile)
324 {
325         int error;
326
327         ifq_lock_all(ifq);
328         error = tbr_set_locked(ifq, profile);
329         ifq_unlock_all(ifq);
330         return error;
331 }
332
333 /*
334  * tbr_timeout goes through the interface list, and kicks the drivers
335  * if necessary.
336  */
337 static void
338 tbr_timeout(void *arg)
339 {
340         struct ifnet *ifp;
341         int active;
342
343         active = 0;
344         crit_enter();
345         for (ifp = TAILQ_FIRST(&ifnet); ifp; ifp = TAILQ_NEXT(ifp, if_list)) {
346                 struct ifaltq_subque *ifsq;
347
348                 if (ifp->if_snd.altq_tbr == NULL)
349                         continue;
350
351                 ifsq = &ifp->if_snd.altq_subq[ALTQ_SUBQ_INDEX_DEFAULT];
352                 active++;
353                 if (!ifsq_is_empty(ifsq) && ifp->if_start != NULL) {
354                         ifnet_serialize_tx(ifp, ifsq);
355                         (*ifp->if_start)(ifp, ifsq);
356                         ifnet_deserialize_tx(ifp, ifsq);
357                 }
358         }
359         crit_exit();
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  */
369 int
370 tbr_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  */
389 int
390 altq_pfattach(struct pf_altq *a)
391 {
392         struct ifaltq *ifq;
393         struct ifnet *ifp;
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
407         ifq_lock_all(ifq);
408
409         switch (a->scheduler) {
410 #ifdef ALTQ_CBQ
411         case ALTQT_CBQ:
412                 error = cbq_pfattach(a, ifq);
413                 break;
414 #endif
415 #ifdef ALTQ_PRIQ
416         case ALTQT_PRIQ:
417                 error = priq_pfattach(a, ifq);
418                 break;
419 #endif
420 #ifdef ALTQ_HFSC
421         case ALTQT_HFSC:
422                 error = hfsc_pfattach(a, ifq);
423                 break;
424 #endif
425 #ifdef ALTQ_FAIRQ
426         case ALTQT_FAIRQ:
427                 error = fairq_pfattach(a, ifq);
428                 break;
429 #endif
430         default:
431                 error = ENXIO;
432                 goto back;
433         }
434
435         /* if the state is running, enable altq */
436         if (error == 0 && pfaltq_running && ifq->altq_type != ALTQT_NONE &&
437             !ifq_is_enabled(ifq))
438                 error = altq_enable_locked(ifq);
439
440         /* if altq is already enabled, reset set tokenbucket regulator */
441         if (error == 0 && ifq_is_enabled(ifq)) {
442                 struct tb_profile tb;
443
444                 tb.rate = a->ifbandwidth;
445                 tb.depth = a->tbrsize;
446                 error = tbr_set_locked(ifq, &tb);
447         }
448 back:
449         ifq_unlock_all(ifq);
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  */
458 int
459 altq_pfdetach(struct pf_altq *a)
460 {
461         struct ifnet *ifp;
462         struct ifaltq *ifq;
463         int error = 0;
464
465         ifp = ifunit(a->ifname);
466         if (ifp == NULL)
467                 return (EINVAL);
468         ifq = &ifp->if_snd;
469
470         /* if this discipline is no longer referenced, just return */
471         if (a->altq_disc == NULL)
472                 return (0);
473
474         ifq_lock_all(ifq);
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);
481         if (error == 0)
482                 error = altq_detach_locked(ifq);
483
484 back:
485         ifq_unlock_all(ifq);
486         return (error);
487 }
488
489 /*
490  * add a discipline or a queue
491  */
492 int
493 altq_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
521 #ifdef ALTQ_FAIRQ
522         case ALTQT_FAIRQ:
523                 error = fairq_add_altq(a);
524                 break;
525 #endif
526         default:
527                 error = ENXIO;
528         }
529
530         return (error);
531 }
532
533 /*
534  * remove a discipline or a queue
535  */
536 int
537 altq_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
560 #ifdef ALTQ_FAIRQ
561         case ALTQT_FAIRQ:
562                 error = fairq_remove_altq(a);
563                 break;
564 #endif
565         default:
566                 error = ENXIO;
567         }
568
569         return (error);
570 }
571
572 /*
573  * add a queue to the discipline
574  */
575 int
576 altq_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
596 #ifdef ALTQ_FAIRQ
597         case ALTQT_FAIRQ:
598                 error = fairq_add_queue(a);
599                 break;
600 #endif
601         default:
602                 error = ENXIO;
603         }
604
605         return (error);
606 }
607
608 /*
609  * remove a queue from the discipline
610  */
611 int
612 altq_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
632 #ifdef ALTQ_FAIRQ
633         case ALTQT_FAIRQ:
634                 error = fairq_remove_queue(a);
635                 break;
636 #endif
637         default:
638                 error = ENXIO;
639         }
640
641         return (error);
642 }
643
644 /*
645  * get queue statistics
646  */
647 int
648 altq_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
668 #ifdef ALTQ_FAIRQ
669         case ALTQT_FAIRQ:
670                 error = fairq_getqstats(a, ubuf, nbytes);
671                 break;
672 #endif
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  */
683 uint8_t
684 read_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
703                 kprintf("read_dsfield: can't locate header!\n");
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
729 void
730 write_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
748                 kprintf("write_dsfield: can't locate header!\n");
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
800 int machclk_usepcc;
801 uint64_t machclk_freq = 0;
802 uint32_t machclk_per_tick = 0;
803
804 void
805 init_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;
813 #elif defined(__DragonFly__)
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() */
823                 machclk_freq = 1000000LLU << MACHCLK_SHIFT;
824                 machclk_per_tick = machclk_freq / hz;
825 #ifdef ALTQ_DEBUG
826                 kprintf("altq: emulate %juHz cpu clock\n", (uintmax_t)machclk_freq);
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          */
835 #ifdef _RDTSC_SUPPORTED_
836         if (cpu_feature & CPUID_TSC)
837                 machclk_freq = (uint64_t)tsc_frequency;
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)
858                         machclk_freq = (end - start) * 1000000 / diff;
859         }
860
861         machclk_per_tick = machclk_freq / hz;
862
863 #ifdef ALTQ_DEBUG
864         kprintf("altq: CPU clock: %juHz\n", (uintmax_t)machclk_freq);
865 #endif
866 }
867
868 uint64_t
869 read_machclk(void)
870 {
871         uint64_t val;
872
873         if (machclk_usepcc) {
874 #ifdef _RDTSC_SUPPORTED_
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 }
888