ifq.c revision 1.41 
1/*	$OpenBSD: ifq.c,v 1.41 2020/07/07 00:00:03 dlg Exp $ */
2
3/*
4 * Copyright (c) 2015 David Gwynne <dlg@openbsd.org>
5 *
6 * Permission to use, copy, modify, and distribute this software for any
7 * purpose with or without fee is hereby granted, provided that the above
8 * copyright notice and this permission notice appear in all copies.
9 *
10 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
11 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
12 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
13 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
14 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
15 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
16 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
17 */
18
19#include "bpfilter.h"
20#include "kstat.h"
21
22#include <sys/param.h>
23#include <sys/systm.h>
24#include <sys/socket.h>
25#include <sys/mbuf.h>
26#include <sys/proc.h>
27#include <sys/sysctl.h>
28
29#include <net/if.h>
30#include <net/if_var.h>
31
32#if NBPFILTER > 0
33#include <net/bpf.h>
34#endif
35
36#if NKSTAT > 0
37#include <sys/kstat.h>
38#endif
39
40/*
41 * priq glue
42 */
43unsigned int	 priq_idx(unsigned int, const struct mbuf *);
44struct mbuf	*priq_enq(struct ifqueue *, struct mbuf *);
45struct mbuf	*priq_deq_begin(struct ifqueue *, void **);
46void		 priq_deq_commit(struct ifqueue *, struct mbuf *, void *);
47void		 priq_purge(struct ifqueue *, struct mbuf_list *);
48
49void		*priq_alloc(unsigned int, void *);
50void		 priq_free(unsigned int, void *);
51
52const struct ifq_ops priq_ops = {
53	priq_idx,
54	priq_enq,
55	priq_deq_begin,
56	priq_deq_commit,
57	priq_purge,
58	priq_alloc,
59	priq_free,
60};
61
62const struct ifq_ops * const ifq_priq_ops = &priq_ops;
63
64/*
65 * priq internal structures
66 */
67
68struct priq {
69	struct mbuf_list	 pq_lists[IFQ_NQUEUES];
70};
71
72/*
73 * ifqueue serialiser
74 */
75
76void	ifq_start_task(void *);
77void	ifq_restart_task(void *);
78void	ifq_barrier_task(void *);
79void	ifq_bundle_task(void *);
80
81static inline void
82ifq_run_start(struct ifqueue *ifq)
83{
84	ifq_serialize(ifq, &ifq->ifq_start);
85}
86
87void
88ifq_serialize(struct ifqueue *ifq, struct task *t)
89{
90	struct task work;
91
92	if (ISSET(t->t_flags, TASK_ONQUEUE))
93		return;
94
95	mtx_enter(&ifq->ifq_task_mtx);
96	if (!ISSET(t->t_flags, TASK_ONQUEUE)) {
97		SET(t->t_flags, TASK_ONQUEUE);
98		TAILQ_INSERT_TAIL(&ifq->ifq_task_list, t, t_entry);
99	}
100
101	if (ifq->ifq_serializer == NULL) {
102		ifq->ifq_serializer = curcpu();
103
104		while ((t = TAILQ_FIRST(&ifq->ifq_task_list)) != NULL) {
105			TAILQ_REMOVE(&ifq->ifq_task_list, t, t_entry);
106			CLR(t->t_flags, TASK_ONQUEUE);
107			work = *t; /* copy to caller to avoid races */
108
109			mtx_leave(&ifq->ifq_task_mtx);
110
111			(*work.t_func)(work.t_arg);
112
113			mtx_enter(&ifq->ifq_task_mtx);
114		}
115
116		ifq->ifq_serializer = NULL;
117	}
118	mtx_leave(&ifq->ifq_task_mtx);
119}
120
121int
122ifq_is_serialized(struct ifqueue *ifq)
123{
124	return (ifq->ifq_serializer == curcpu());
125}
126
127void
128ifq_start(struct ifqueue *ifq)
129{
130	if (ifq_len(ifq) >= min(ifq->ifq_if->if_txmit, ifq->ifq_maxlen)) {
131		task_del(ifq->ifq_softnet, &ifq->ifq_bundle);
132		ifq_run_start(ifq);
133	} else
134		task_add(ifq->ifq_softnet, &ifq->ifq_bundle);
135}
136
137void
138ifq_start_task(void *p)
139{
140	struct ifqueue *ifq = p;
141	struct ifnet *ifp = ifq->ifq_if;
142
143	if (!ISSET(ifp->if_flags, IFF_RUNNING) ||
144	    ifq_empty(ifq) || ifq_is_oactive(ifq))
145		return;
146
147	ifp->if_qstart(ifq);
148}
149
150void
151ifq_restart_task(void *p)
152{
153	struct ifqueue *ifq = p;
154	struct ifnet *ifp = ifq->ifq_if;
155
156	ifq_clr_oactive(ifq);
157	ifp->if_qstart(ifq);
158}
159
160void
161ifq_bundle_task(void *p)
162{
163	struct ifqueue *ifq = p;
164
165	ifq_run_start(ifq);
166}
167
168void
169ifq_barrier(struct ifqueue *ifq)
170{
171	struct cond c = COND_INITIALIZER();
172	struct task t = TASK_INITIALIZER(ifq_barrier_task, &c);
173
174	task_del(ifq->ifq_softnet, &ifq->ifq_bundle);
175
176	if (ifq->ifq_serializer == NULL)
177		return;
178
179	ifq_serialize(ifq, &t);
180
181	cond_wait(&c, "ifqbar");
182}
183
184void
185ifq_barrier_task(void *p)
186{
187	struct cond *c = p;
188
189	cond_signal(c);
190}
191
192/*
193 * ifqueue mbuf queue API
194 */
195
196#if NKSTAT > 0
197struct ifq_kstat_data {
198	struct kstat_kv kd_packets;
199	struct kstat_kv kd_bytes;
200	struct kstat_kv kd_qdrops;
201	struct kstat_kv kd_errors;
202	struct kstat_kv kd_qlen;
203	struct kstat_kv kd_maxqlen;
204	struct kstat_kv kd_oactive;
205};
206
207static const struct ifq_kstat_data ifq_kstat_tpl = {
208	KSTAT_KV_UNIT_INITIALIZER("packets",
209	    KSTAT_KV_T_COUNTER64, KSTAT_KV_U_PACKETS),
210	KSTAT_KV_UNIT_INITIALIZER("bytes",
211	    KSTAT_KV_T_COUNTER64, KSTAT_KV_U_BYTES),
212	KSTAT_KV_UNIT_INITIALIZER("qdrops",
213	    KSTAT_KV_T_COUNTER64, KSTAT_KV_U_PACKETS),
214	KSTAT_KV_UNIT_INITIALIZER("errors",
215	    KSTAT_KV_T_COUNTER64, KSTAT_KV_U_PACKETS),
216	KSTAT_KV_UNIT_INITIALIZER("qlen",
217	    KSTAT_KV_T_UINT32, KSTAT_KV_U_PACKETS),
218	KSTAT_KV_UNIT_INITIALIZER("maxqlen",
219	    KSTAT_KV_T_UINT32, KSTAT_KV_U_PACKETS),
220	KSTAT_KV_INITIALIZER("oactive", KSTAT_KV_T_BOOL),
221};
222
223int
224ifq_kstat_copy(struct kstat *ks, void *dst)
225{
226	struct ifqueue *ifq = ks->ks_softc;
227	struct ifq_kstat_data *kd = dst;
228
229	*kd = ifq_kstat_tpl;
230	kstat_kv_u64(&kd->kd_packets) = ifq->ifq_packets;
231	kstat_kv_u64(&kd->kd_bytes) = ifq->ifq_bytes;
232	kstat_kv_u64(&kd->kd_qdrops) = ifq->ifq_qdrops;
233	kstat_kv_u64(&kd->kd_errors) = ifq->ifq_errors;
234	kstat_kv_u32(&kd->kd_qlen) = ifq->ifq_len;
235	kstat_kv_u32(&kd->kd_maxqlen) = ifq->ifq_maxlen;
236	kstat_kv_bool(&kd->kd_oactive) = ifq->ifq_oactive;
237
238	return (0);
239}
240#endif
241
242void
243ifq_init(struct ifqueue *ifq, struct ifnet *ifp, unsigned int idx)
244{
245	ifq->ifq_if = ifp;
246	ifq->ifq_softnet = net_tq(ifp->if_index); /* + idx */
247	ifq->ifq_softc = NULL;
248
249	mtx_init(&ifq->ifq_mtx, IPL_NET);
250
251	/* default to priq */
252	ifq->ifq_ops = &priq_ops;
253	ifq->ifq_q = priq_ops.ifqop_alloc(idx, NULL);
254
255	ml_init(&ifq->ifq_free);
256	ifq->ifq_len = 0;
257
258	ifq->ifq_packets = 0;
259	ifq->ifq_bytes = 0;
260	ifq->ifq_qdrops = 0;
261	ifq->ifq_errors = 0;
262	ifq->ifq_mcasts = 0;
263
264	mtx_init(&ifq->ifq_task_mtx, IPL_NET);
265	TAILQ_INIT(&ifq->ifq_task_list);
266	ifq->ifq_serializer = NULL;
267	task_set(&ifq->ifq_bundle, ifq_bundle_task, ifq);
268
269	task_set(&ifq->ifq_start, ifq_start_task, ifq);
270	task_set(&ifq->ifq_restart, ifq_restart_task, ifq);
271
272	if (ifq->ifq_maxlen == 0)
273		ifq_set_maxlen(ifq, IFQ_MAXLEN);
274
275	ifq->ifq_idx = idx;
276
277#if NKSTAT > 0
278	/* XXX xname vs driver name and unit */
279	ifq->ifq_kstat = kstat_create(ifp->if_xname, 0,
280	    "txq", ifq->ifq_idx, KSTAT_T_KV, 0);
281	KASSERT(ifq->ifq_kstat != NULL);
282	kstat_set_mutex(ifq->ifq_kstat, &ifq->ifq_mtx);
283	ifq->ifq_kstat->ks_softc = ifq;
284	ifq->ifq_kstat->ks_datalen = sizeof(ifq_kstat_tpl);
285	ifq->ifq_kstat->ks_copy = ifq_kstat_copy;
286	kstat_install(ifq->ifq_kstat);
287#endif
288}
289
290void
291ifq_attach(struct ifqueue *ifq, const struct ifq_ops *newops, void *opsarg)
292{
293	struct mbuf_list ml = MBUF_LIST_INITIALIZER();
294	struct mbuf_list free_ml = MBUF_LIST_INITIALIZER();
295	struct mbuf *m;
296	const struct ifq_ops *oldops;
297	void *newq, *oldq;
298
299	newq = newops->ifqop_alloc(ifq->ifq_idx, opsarg);
300
301	mtx_enter(&ifq->ifq_mtx);
302	ifq->ifq_ops->ifqop_purge(ifq, &ml);
303	ifq->ifq_len = 0;
304
305	oldops = ifq->ifq_ops;
306	oldq = ifq->ifq_q;
307
308	ifq->ifq_ops = newops;
309	ifq->ifq_q = newq;
310
311	while ((m = ml_dequeue(&ml)) != NULL) {
312		m = ifq->ifq_ops->ifqop_enq(ifq, m);
313		if (m != NULL) {
314			ifq->ifq_qdrops++;
315			ml_enqueue(&free_ml, m);
316		} else
317			ifq->ifq_len++;
318	}
319	mtx_leave(&ifq->ifq_mtx);
320
321	oldops->ifqop_free(ifq->ifq_idx, oldq);
322
323	ml_purge(&free_ml);
324}
325
326void
327ifq_destroy(struct ifqueue *ifq)
328{
329	struct mbuf_list ml = MBUF_LIST_INITIALIZER();
330
331#if NKSTAT > 0
332	kstat_destroy(ifq->ifq_kstat);
333#endif
334
335	NET_ASSERT_UNLOCKED();
336	if (!task_del(ifq->ifq_softnet, &ifq->ifq_bundle))
337		taskq_barrier(ifq->ifq_softnet);
338
339	/* don't need to lock because this is the last use of the ifq */
340
341	ifq->ifq_ops->ifqop_purge(ifq, &ml);
342	ifq->ifq_ops->ifqop_free(ifq->ifq_idx, ifq->ifq_q);
343
344	ml_purge(&ml);
345}
346
347void
348ifq_add_data(struct ifqueue *ifq, struct if_data *data)
349{
350	mtx_enter(&ifq->ifq_mtx);
351	data->ifi_opackets += ifq->ifq_packets;
352	data->ifi_obytes += ifq->ifq_bytes;
353	data->ifi_oqdrops += ifq->ifq_qdrops;
354	data->ifi_omcasts += ifq->ifq_mcasts;
355	/* ifp->if_data.ifi_oerrors */
356	mtx_leave(&ifq->ifq_mtx);
357}
358
359int
360ifq_enqueue(struct ifqueue *ifq, struct mbuf *m)
361{
362	struct mbuf *dm;
363
364	mtx_enter(&ifq->ifq_mtx);
365	dm = ifq->ifq_ops->ifqop_enq(ifq, m);
366	if (dm != m) {
367		ifq->ifq_packets++;
368		ifq->ifq_bytes += m->m_pkthdr.len;
369		if (ISSET(m->m_flags, M_MCAST))
370			ifq->ifq_mcasts++;
371	}
372
373	if (dm == NULL)
374		ifq->ifq_len++;
375	else
376		ifq->ifq_qdrops++;
377	mtx_leave(&ifq->ifq_mtx);
378
379	if (dm != NULL)
380		m_freem(dm);
381
382	return (dm == m ? ENOBUFS : 0);
383}
384
385static inline void
386ifq_deq_enter(struct ifqueue *ifq)
387{
388	mtx_enter(&ifq->ifq_mtx);
389}
390
391static inline void
392ifq_deq_leave(struct ifqueue *ifq)
393{
394	struct mbuf_list ml;
395
396	ml = ifq->ifq_free;
397	ml_init(&ifq->ifq_free);
398
399	mtx_leave(&ifq->ifq_mtx);
400
401	if (!ml_empty(&ml))
402		ml_purge(&ml);
403}
404
405struct mbuf *
406ifq_deq_begin(struct ifqueue *ifq)
407{
408	struct mbuf *m = NULL;
409	void *cookie;
410
411	ifq_deq_enter(ifq);
412	if (ifq->ifq_len == 0 ||
413	    (m = ifq->ifq_ops->ifqop_deq_begin(ifq, &cookie)) == NULL) {
414		ifq_deq_leave(ifq);
415		return (NULL);
416	}
417
418	m->m_pkthdr.ph_cookie = cookie;
419
420	return (m);
421}
422
423void
424ifq_deq_commit(struct ifqueue *ifq, struct mbuf *m)
425{
426	void *cookie;
427
428	KASSERT(m != NULL);
429	cookie = m->m_pkthdr.ph_cookie;
430
431	ifq->ifq_ops->ifqop_deq_commit(ifq, m, cookie);
432	ifq->ifq_len--;
433	ifq_deq_leave(ifq);
434}
435
436void
437ifq_deq_rollback(struct ifqueue *ifq, struct mbuf *m)
438{
439	KASSERT(m != NULL);
440
441	ifq_deq_leave(ifq);
442}
443
444struct mbuf *
445ifq_dequeue(struct ifqueue *ifq)
446{
447	struct mbuf *m;
448
449	m = ifq_deq_begin(ifq);
450	if (m == NULL)
451		return (NULL);
452
453	ifq_deq_commit(ifq, m);
454
455	return (m);
456}
457
458int
459ifq_deq_sleep(struct ifqueue *ifq, struct mbuf **mp, int nbio, int priority,
460    const char *wmesg, volatile unsigned int *sleeping,
461    volatile unsigned int *alive)
462{
463	struct mbuf *m;
464	void *cookie;
465	int error = 0;
466
467	ifq_deq_enter(ifq);
468	if (ifq->ifq_len == 0 && nbio)
469		error = EWOULDBLOCK;
470	else {
471		for (;;) {
472			m = ifq->ifq_ops->ifqop_deq_begin(ifq, &cookie);
473			if (m != NULL) {
474				ifq->ifq_ops->ifqop_deq_commit(ifq, m, cookie);
475				ifq->ifq_len--;
476				*mp = m;
477				break;
478			}
479
480			(*sleeping)++;
481			error = msleep_nsec(ifq, &ifq->ifq_mtx,
482			    priority, wmesg, INFSLP);
483			(*sleeping)--;
484			if (error != 0)
485				break;
486			if (!(*alive)) {
487				error = ENXIO;
488				break;
489			}
490		}
491	}
492	ifq_deq_leave(ifq);
493
494	return (error);
495}
496
497int
498ifq_hdatalen(struct ifqueue *ifq)
499{
500	struct mbuf *m;
501	int len = 0;
502
503	m = ifq_deq_begin(ifq);
504	if (m != NULL) {
505		len = m->m_pkthdr.len;
506		ifq_deq_rollback(ifq, m);
507	}
508
509	return (len);
510}
511
512unsigned int
513ifq_purge(struct ifqueue *ifq)
514{
515	struct mbuf_list ml = MBUF_LIST_INITIALIZER();
516	unsigned int rv;
517
518	mtx_enter(&ifq->ifq_mtx);
519	ifq->ifq_ops->ifqop_purge(ifq, &ml);
520	rv = ifq->ifq_len;
521	ifq->ifq_len = 0;
522	ifq->ifq_qdrops += rv;
523	mtx_leave(&ifq->ifq_mtx);
524
525	KASSERT(rv == ml_len(&ml));
526
527	ml_purge(&ml);
528
529	return (rv);
530}
531
532void *
533ifq_q_enter(struct ifqueue *ifq, const struct ifq_ops *ops)
534{
535	mtx_enter(&ifq->ifq_mtx);
536	if (ifq->ifq_ops == ops)
537		return (ifq->ifq_q);
538
539	mtx_leave(&ifq->ifq_mtx);
540
541	return (NULL);
542}
543
544void
545ifq_q_leave(struct ifqueue *ifq, void *q)
546{
547	KASSERT(q == ifq->ifq_q);
548	mtx_leave(&ifq->ifq_mtx);
549}
550
551void
552ifq_mfreem(struct ifqueue *ifq, struct mbuf *m)
553{
554	MUTEX_ASSERT_LOCKED(&ifq->ifq_mtx);
555
556	ifq->ifq_len--;
557	ifq->ifq_qdrops++;
558	ml_enqueue(&ifq->ifq_free, m);
559}
560
561void
562ifq_mfreeml(struct ifqueue *ifq, struct mbuf_list *ml)
563{
564	MUTEX_ASSERT_LOCKED(&ifq->ifq_mtx);
565
566	ifq->ifq_len -= ml_len(ml);
567	ifq->ifq_qdrops += ml_len(ml);
568	ml_enlist(&ifq->ifq_free, ml);
569}
570
571/*
572 * ifiq
573 */
574
575#if NKSTAT > 0
576struct ifiq_kstat_data {
577	struct kstat_kv kd_packets;
578	struct kstat_kv kd_bytes;
579	struct kstat_kv kd_qdrops;
580	struct kstat_kv kd_errors;
581	struct kstat_kv kd_qlen;
582};
583
584static const struct ifiq_kstat_data ifiq_kstat_tpl = {
585	KSTAT_KV_UNIT_INITIALIZER("packets",
586	    KSTAT_KV_T_COUNTER64, KSTAT_KV_U_PACKETS),
587	KSTAT_KV_UNIT_INITIALIZER("bytes",
588	    KSTAT_KV_T_COUNTER64, KSTAT_KV_U_BYTES),
589	KSTAT_KV_UNIT_INITIALIZER("qdrops",
590	    KSTAT_KV_T_COUNTER64, KSTAT_KV_U_PACKETS),
591	KSTAT_KV_UNIT_INITIALIZER("errors",
592	    KSTAT_KV_T_COUNTER64, KSTAT_KV_U_PACKETS),
593	KSTAT_KV_UNIT_INITIALIZER("qlen",
594	    KSTAT_KV_T_UINT32, KSTAT_KV_U_PACKETS),
595};
596
597int
598ifiq_kstat_copy(struct kstat *ks, void *dst)
599{
600	struct ifiqueue *ifiq = ks->ks_softc;
601	struct ifiq_kstat_data *kd = dst;
602
603	*kd = ifiq_kstat_tpl;
604	kstat_kv_u64(&kd->kd_packets) = ifiq->ifiq_packets;
605	kstat_kv_u64(&kd->kd_bytes) = ifiq->ifiq_bytes;
606	kstat_kv_u64(&kd->kd_qdrops) = ifiq->ifiq_qdrops;
607	kstat_kv_u64(&kd->kd_errors) = ifiq->ifiq_errors;
608	kstat_kv_u32(&kd->kd_qlen) = ml_len(&ifiq->ifiq_ml);
609
610	return (0);
611}
612#endif
613
614static void	ifiq_process(void *);
615
616void
617ifiq_init(struct ifiqueue *ifiq, struct ifnet *ifp, unsigned int idx)
618{
619	ifiq->ifiq_if = ifp;
620	ifiq->ifiq_softnet = net_tq(ifp->if_index); /* + idx */
621	ifiq->ifiq_softc = NULL;
622
623	mtx_init(&ifiq->ifiq_mtx, IPL_NET);
624	ml_init(&ifiq->ifiq_ml);
625	task_set(&ifiq->ifiq_task, ifiq_process, ifiq);
626	ifiq->ifiq_pressure = 0;
627
628	ifiq->ifiq_packets = 0;
629	ifiq->ifiq_bytes = 0;
630	ifiq->ifiq_qdrops = 0;
631	ifiq->ifiq_errors = 0;
632
633	ifiq->ifiq_idx = idx;
634
635#if NKSTAT > 0
636	/* XXX xname vs driver name and unit */
637	ifiq->ifiq_kstat = kstat_create(ifp->if_xname, 0,
638	    "rxq", ifiq->ifiq_idx, KSTAT_T_KV, 0);
639	KASSERT(ifiq->ifiq_kstat != NULL);
640	kstat_set_mutex(ifiq->ifiq_kstat, &ifiq->ifiq_mtx);
641	ifiq->ifiq_kstat->ks_softc = ifiq;
642	ifiq->ifiq_kstat->ks_datalen = sizeof(ifiq_kstat_tpl);
643	ifiq->ifiq_kstat->ks_copy = ifiq_kstat_copy;
644	kstat_install(ifiq->ifiq_kstat);
645#endif
646}
647
648void
649ifiq_destroy(struct ifiqueue *ifiq)
650{
651#if NKSTAT > 0
652	kstat_destroy(ifiq->ifiq_kstat);
653#endif
654
655	NET_ASSERT_UNLOCKED();
656	if (!task_del(ifiq->ifiq_softnet, &ifiq->ifiq_task))
657		taskq_barrier(ifiq->ifiq_softnet);
658
659	/* don't need to lock because this is the last use of the ifiq */
660	ml_purge(&ifiq->ifiq_ml);
661}
662
663unsigned int ifiq_maxlen_drop = 2048 * 5;
664unsigned int ifiq_maxlen_return = 2048 * 3;
665
666int
667ifiq_input(struct ifiqueue *ifiq, struct mbuf_list *ml)
668{
669	struct ifnet *ifp = ifiq->ifiq_if;
670	struct mbuf *m;
671	uint64_t packets;
672	uint64_t bytes = 0;
673	unsigned int len;
674#if NBPFILTER > 0
675	caddr_t if_bpf;
676#endif
677
678	if (ml_empty(ml))
679		return (0);
680
681	MBUF_LIST_FOREACH(ml, m) {
682		m->m_pkthdr.ph_ifidx = ifp->if_index;
683		m->m_pkthdr.ph_rtableid = ifp->if_rdomain;
684		bytes += m->m_pkthdr.len;
685	}
686	packets = ml_len(ml);
687
688#if NBPFILTER > 0
689	if_bpf = ifp->if_bpf;
690	if (if_bpf) {
691		struct mbuf_list ml0 = *ml;
692
693		ml_init(ml);
694
695		while ((m = ml_dequeue(&ml0)) != NULL) {
696			if (bpf_mtap_ether(if_bpf, m, BPF_DIRECTION_IN))
697				m_freem(m);
698			else
699				ml_enqueue(ml, m);
700		}
701
702		if (ml_empty(ml)) {
703			mtx_enter(&ifiq->ifiq_mtx);
704			ifiq->ifiq_packets += packets;
705			ifiq->ifiq_bytes += bytes;
706			mtx_leave(&ifiq->ifiq_mtx);
707
708			return (0);
709		}
710	}
711#endif
712
713	mtx_enter(&ifiq->ifiq_mtx);
714	ifiq->ifiq_packets += packets;
715	ifiq->ifiq_bytes += bytes;
716
717	len = ml_len(&ifiq->ifiq_ml);
718	if (len > ifiq_maxlen_drop)
719		ifiq->ifiq_qdrops += ml_len(ml);
720	else
721		ml_enlist(&ifiq->ifiq_ml, ml);
722	mtx_leave(&ifiq->ifiq_mtx);
723
724	if (ml_empty(ml))
725		task_add(ifiq->ifiq_softnet, &ifiq->ifiq_task);
726	else
727		ml_purge(ml);
728
729	return (len > ifiq_maxlen_return);
730}
731
732void
733ifiq_add_data(struct ifiqueue *ifiq, struct if_data *data)
734{
735	mtx_enter(&ifiq->ifiq_mtx);
736	data->ifi_ipackets += ifiq->ifiq_packets;
737	data->ifi_ibytes += ifiq->ifiq_bytes;
738	data->ifi_iqdrops += ifiq->ifiq_qdrops;
739	mtx_leave(&ifiq->ifiq_mtx);
740}
741
742int
743ifiq_enqueue(struct ifiqueue *ifiq, struct mbuf *m)
744{
745	mtx_enter(&ifiq->ifiq_mtx);
746	ml_enqueue(&ifiq->ifiq_ml, m);
747	mtx_leave(&ifiq->ifiq_mtx);
748
749	task_add(ifiq->ifiq_softnet, &ifiq->ifiq_task);
750
751	return (0);
752}
753
754static void
755ifiq_process(void *arg)
756{
757	struct ifiqueue *ifiq = arg;
758	struct mbuf_list ml;
759
760	if (ifiq_empty(ifiq))
761		return;
762
763	mtx_enter(&ifiq->ifiq_mtx);
764	ml = ifiq->ifiq_ml;
765	ml_init(&ifiq->ifiq_ml);
766	mtx_leave(&ifiq->ifiq_mtx);
767
768	if_input_process(ifiq->ifiq_if, &ml);
769}
770
771int
772net_ifiq_sysctl(int *name, u_int namelen, void *oldp, size_t *oldlenp,
773    void *newp, size_t newlen)
774{
775	int error = EOPNOTSUPP;
776/* pressure is disabled for 6.6-release */
777#if 0
778	int val;
779
780	if (namelen != 1)
781		return (EISDIR);
782
783	switch (name[0]) {
784	case NET_LINK_IFRXQ_PRESSURE_RETURN:
785		val = ifiq_pressure_return;
786		error = sysctl_int(oldp, oldlenp, newp, newlen, &val);
787		if (error != 0)
788			return (error);
789		if (val < 1 || val > ifiq_pressure_drop)
790			return (EINVAL);
791		ifiq_pressure_return = val;
792		break;
793	case NET_LINK_IFRXQ_PRESSURE_DROP:
794		val = ifiq_pressure_drop;
795		error = sysctl_int(oldp, oldlenp, newp, newlen, &val);
796		if (error != 0)
797			return (error);
798		if (ifiq_pressure_return > val)
799			return (EINVAL);
800		ifiq_pressure_drop = val;
801		break;
802	default:
803		error = EOPNOTSUPP;
804		break;
805	}
806#endif
807
808	return (error);
809}
810
811/*
812 * priq implementation
813 */
814
815unsigned int
816priq_idx(unsigned int nqueues, const struct mbuf *m)
817{
818	unsigned int flow = 0;
819
820	if (ISSET(m->m_pkthdr.csum_flags, M_FLOWID))
821		flow = m->m_pkthdr.ph_flowid;
822
823	return (flow % nqueues);
824}
825
826void *
827priq_alloc(unsigned int idx, void *null)
828{
829	struct priq *pq;
830	int i;
831
832	pq = malloc(sizeof(struct priq), M_DEVBUF, M_WAITOK);
833	for (i = 0; i < IFQ_NQUEUES; i++)
834		ml_init(&pq->pq_lists[i]);
835	return (pq);
836}
837
838void
839priq_free(unsigned int idx, void *pq)
840{
841	free(pq, M_DEVBUF, sizeof(struct priq));
842}
843
844struct mbuf *
845priq_enq(struct ifqueue *ifq, struct mbuf *m)
846{
847	struct priq *pq;
848	struct mbuf_list *pl;
849	struct mbuf *n = NULL;
850	unsigned int prio;
851
852	pq = ifq->ifq_q;
853	KASSERT(m->m_pkthdr.pf.prio <= IFQ_MAXPRIO);
854
855	/* Find a lower priority queue to drop from */
856	if (ifq_len(ifq) >= ifq->ifq_maxlen) {
857		for (prio = 0; prio < m->m_pkthdr.pf.prio; prio++) {
858			pl = &pq->pq_lists[prio];
859			if (ml_len(pl) > 0) {
860				n = ml_dequeue(pl);
861				goto enqueue;
862			}
863		}
864		/*
865		 * There's no lower priority queue that we can
866		 * drop from so don't enqueue this one.
867		 */
868		return (m);
869	}
870
871 enqueue:
872	pl = &pq->pq_lists[m->m_pkthdr.pf.prio];
873	ml_enqueue(pl, m);
874
875	return (n);
876}
877
878struct mbuf *
879priq_deq_begin(struct ifqueue *ifq, void **cookiep)
880{
881	struct priq *pq = ifq->ifq_q;
882	struct mbuf_list *pl;
883	unsigned int prio = nitems(pq->pq_lists);
884	struct mbuf *m;
885
886	do {
887		pl = &pq->pq_lists[--prio];
888		m = MBUF_LIST_FIRST(pl);
889		if (m != NULL) {
890			*cookiep = pl;
891			return (m);
892		}
893	} while (prio > 0);
894
895	return (NULL);
896}
897
898void
899priq_deq_commit(struct ifqueue *ifq, struct mbuf *m, void *cookie)
900{
901	struct mbuf_list *pl = cookie;
902
903	KASSERT(MBUF_LIST_FIRST(pl) == m);
904
905	ml_dequeue(pl);
906}
907
908void
909priq_purge(struct ifqueue *ifq, struct mbuf_list *ml)
910{
911	struct priq *pq = ifq->ifq_q;
912	struct mbuf_list *pl;
913	unsigned int prio = nitems(pq->pq_lists);
914
915	do {
916		pl = &pq->pq_lists[--prio];
917		ml_enlist(ml, pl);
918	} while (prio > 0);
919}
920