/*
* Send traffic from a scheduler instance due by 'now'.
* Return a pointer to the head of the queue.
*/
static struct mbuf *
serve_sched(struct mq *q, struct dn_sch_inst *si, uint64_t now)
{
struct mq def_q;
struct dn_schk *s = si->sched;
struct mbuf *m = NULL;
int delay_line_idle = (si->dline.mq.head == NULL);
int done, bw;
if (q == NULL) {
q = &def_q;
q->head = NULL;
}
bw = s->link.bandwidth;
si->kflags &= ~DN_ACTIVE;
if (bw > 0)
si->credit += (now - si->sched_time) * bw;
else
si->credit = 0;
si->sched_time = now;
done = 0;
while (si->credit >= 0 && (m = s->fp->dequeue(si)) != NULL) {
uint64_t len_scaled;
done++;
len_scaled = (bw == 0) ? 0 : hz *
(m->m_pkthdr.len * 8 + extra_bits(m, s));
si->credit -= len_scaled;
/* Move packet in the delay line */
dn_tag_get(m)->output_time = dn_cfg.curr_time + s->link.delay ;
mq_append(&si->dline.mq, m);
}
/*
* If credit >= 0 the instance is idle, mark time.
* Otherwise put back in the heap, and adjust the output
* time of the last inserted packet, m, which was too early.
*/
if (si->credit >= 0) {
si->idle_time = now;
} else {
uint64_t t;
KASSERT (bw > 0, ("bw=0 and credit<0 ?"));
t = div64(bw - 1 - si->credit, bw);
if (m)
dn_tag_get(m)->output_time += t;
si->kflags |= DN_ACTIVE;
heap_insert(&dn_cfg.evheap, now + t, si);
}
if (delay_line_idle && done)
transmit_event(q, &si->dline, now);
return q->head;
}
/*
* Convert the additional MAC overheads/delays into an equivalent
* number of bits for the given data rate. The samples are
* in milliseconds so we need to divide by 1000.
*/
static uint64_t
extra_bits(struct mbuf *m, struct dn_schk *s)
{
int index;
uint64_t bits;
struct dn_profile *pf = s->profile;
if (!pf || pf->samples_no == 0)
return 0;
index = random() % pf->samples_no;
bits = div64((uint64_t)pf->samples[index] * s->link.bandwidth, 1000);
if (index >= pf->loss_level) {
struct dn_pkt_tag *dt = dn_tag_get(m);
if (dt)
dt->dn_dir = DIR_DROP;
}
return bits;
}
/*
* Fetch packets from the delay line which are due now. If there are
* leftover packets, reinsert the delay line in the heap.
* Runs under scheduler lock.
*/
static void
transmit_event(struct mq *q, struct delay_line *dline, uint64_t now)
{
struct mbuf *m;
struct dn_pkt_tag *pkt = NULL;
dline->oid.subtype = 0; /* not in heap */
while ((m = dline->mq.head) != NULL) {
pkt = dn_tag_get(m);
if (!DN_KEY_LEQ(pkt->output_time, now))
break;
dline->mq.head = m->m_nextpkt;
mq_append(q, m);
}
if (m != NULL) {
dline->oid.subtype = 1; /* in heap */
heap_insert(&dn_cfg.evheap, pkt->output_time, dline);
}
}
/*
* forward a chain of packets to the proper destination.
* This runs outside the dummynet lock.
*/
static void
dummynet_send(struct mbuf *m)
{
struct mbuf *n;
for (; m != NULL; m = n) {
struct ifnet *ifp = NULL; /* gcc 3.4.6 complains */
struct m_tag *tag;
int dst;
n = m->m_nextpkt;
m->m_nextpkt = NULL;
tag = m_tag_first(m);
if (tag == NULL) { /* should not happen */
dst = DIR_DROP;
} else {
struct dn_pkt_tag *pkt = dn_tag_get(m);
/* extract the dummynet info, rename the tag
* to carry reinject info.
*/
dst = pkt->dn_dir;
ifp = pkt->ifp;
tag->m_tag_cookie = MTAG_IPFW_RULE;
tag->m_tag_id = 0;
}
switch (dst) {
case DIR_OUT:
ip_output(m, NULL, NULL, IP_FORWARDING, NULL, NULL);
break ;
case DIR_IN :
netisr_dispatch(NETISR_IP, m);
break;
#ifdef INET6
case DIR_IN | PROTO_IPV6:
netisr_dispatch(NETISR_IPV6, m);
break;
case DIR_OUT | PROTO_IPV6:
ip6_output(m, NULL, NULL, IPV6_FORWARDING, NULL, NULL, NULL);
break;
#endif
case DIR_FWD | PROTO_IFB: /* DN_TO_IFB_FWD: */
if (bridge_dn_p != NULL)
((*bridge_dn_p)(m, ifp));
else
printf("dummynet: if_bridge not loaded\n");
break;
case DIR_IN | PROTO_LAYER2: /* DN_TO_ETH_DEMUX: */
/*
* The Ethernet code assumes the Ethernet header is
* contiguous in the first mbuf header.
* Insure this is true.
*/
if (m->m_len < ETHER_HDR_LEN &&
(m = m_pullup(m, ETHER_HDR_LEN)) == NULL) {
printf("dummynet/ether: pullup failed, "
"dropping packet\n");
break;
}
ether_demux(m->m_pkthdr.rcvif, m);
break;
case DIR_OUT | PROTO_LAYER2: /* N_TO_ETH_OUT: */
ether_output_frame(ifp, m);
break;
case DIR_DROP:
/* drop the packet after some time */
FREE_PKT(m);
break;
default:
printf("dummynet: bad switch %d!\n", dst);
FREE_PKT(m);
break;
}
}
}
/*
* Classify a packet to queue number using Jenkins hash function.
* Return: queue number
* the input of the hash are protocol no, perturbation, src IP, dst IP,
* src port, dst port,
*/
static inline int
fq_codel_classify_flow(struct mbuf *m, uint16_t fcount, struct fq_codel_si *si)
{
struct ip *ip;
struct tcphdr *th;
struct udphdr *uh;
uint8_t tuple[41];
uint16_t hash=0;
ip = (struct ip *)mtodo(m, dn_tag_get(m)->iphdr_off);
//#ifdef INET6
struct ip6_hdr *ip6;
int isip6;
isip6 = (ip->ip_v == 6);
if(isip6) {
ip6 = (struct ip6_hdr *)ip;
*((uint8_t *) &tuple[0]) = ip6->ip6_nxt;
*((uint32_t *) &tuple[1]) = si->perturbation;
memcpy(&tuple[5], ip6->ip6_src.s6_addr, 16);
memcpy(&tuple[21], ip6->ip6_dst.s6_addr, 16);
switch (ip6->ip6_nxt) {
case IPPROTO_TCP:
th = (struct tcphdr *)(ip6 + 1);
*((uint16_t *) &tuple[37]) = th->th_dport;
*((uint16_t *) &tuple[39]) = th->th_sport;
break;
case IPPROTO_UDP:
uh = (struct udphdr *)(ip6 + 1);
*((uint16_t *) &tuple[37]) = uh->uh_dport;
*((uint16_t *) &tuple[39]) = uh->uh_sport;
break;
default:
memset(&tuple[37], 0, 4);
}
hash = jenkins_hash(tuple, 41, HASHINIT) % fcount;
return hash;
}
//#endif
/* IPv4 */
*((uint8_t *) &tuple[0]) = ip->ip_p;
*((uint32_t *) &tuple[1]) = si->perturbation;
*((uint32_t *) &tuple[5]) = ip->ip_src.s_addr;
*((uint32_t *) &tuple[9]) = ip->ip_dst.s_addr;
switch (ip->ip_p) {
case IPPROTO_TCP:
th = (struct tcphdr *)(ip + 1);
*((uint16_t *) &tuple[13]) = th->th_dport;
*((uint16_t *) &tuple[15]) = th->th_sport;
break;
case IPPROTO_UDP:
uh = (struct udphdr *)(ip + 1);
*((uint16_t *) &tuple[13]) = uh->uh_dport;
*((uint16_t *) &tuple[15]) = uh->uh_sport;
break;
default:
memset(&tuple[13], 0, 4);
}
hash = jenkins_hash(tuple, 17, HASHINIT) % fcount;
return hash;
}
