int tcf_police(struct sk_buff *skb, struct tcf_police *p)
{
psched_time_t now;
long toks;
long ptoks = 0;
spin_lock(&p->lock);
p->stats.bytes += skb->len;
p->stats.packets++;
#ifdef CONFIG_NET_ESTIMATOR
if (p->ewma_rate && p->stats.bps >= p->ewma_rate) {
p->stats.overlimits++;
spin_unlock(&p->lock);
return p->action;
}
#endif
if (skb->len <= p->mtu) {
if (p->R_tab == NULL) {
spin_unlock(&p->lock);
return p->result;
}
PSCHED_GET_TIME(now);
toks = PSCHED_TDIFF_SAFE(now, p->t_c, p->burst, 0);
if (p->P_tab) {
ptoks = toks + p->ptoks;
if (ptoks > (long)L2T_P(p, p->mtu))
ptoks = (long)L2T_P(p, p->mtu);
ptoks -= L2T_P(p, skb->len);
}
toks += p->toks;
if (toks > (long)p->burst)
toks = p->burst;
toks -= L2T(p, skb->len);
if ((toks|ptoks) >= 0) {
p->t_c = now;
p->toks = toks;
p->ptoks = ptoks;
spin_unlock(&p->lock);
return p->result;
}
}
p->stats.overlimits++;
spin_unlock(&p->lock);
return p->action;
}
static void tbf_reset(struct Qdisc* sch)
{
struct tbf_sched_data *q = qdisc_priv(sch);
qdisc_reset(q->qdisc);
sch->q.qlen = 0;
PSCHED_GET_TIME(q->t_c);
q->tokens = q->buffer;
q->ptokens = q->mtu;
sch->flags &= ~TCQ_F_THROTTLED;
del_timer(&q->wd_timer);
}
static void
tbf_reset(struct Qdisc* sch)
{
struct tbf_sched_data *q = (struct tbf_sched_data *)sch->data;
skb_queue_purge(&sch->q);
sch->stats.backlog = 0;
PSCHED_GET_TIME(q->t_c);
q->tokens = q->buffer;
q->ptokens = q->mtu;
del_timer(&q->wd_timer);
}
static int tbf_init(struct Qdisc* sch, struct rtattr *opt)
{
struct tbf_sched_data *q = qdisc_priv(sch);
if (opt == NULL)
return -EINVAL;
PSCHED_GET_TIME(q->t_c);
init_timer(&q->wd_timer);
q->wd_timer.function = tbf_watchdog;
q->wd_timer.data = (unsigned long)sch;
q->qdisc = &noop_qdisc;
return tbf_change(sch, opt);
}
static int tbf_init(struct Qdisc* sch, struct rtattr *opt)
{
struct tbf_sched_data *q = (struct tbf_sched_data *)sch->data;
struct rtattr *tb[TCA_TBF_PTAB];
struct tc_tbf_qopt *qopt;
MOD_INC_USE_COUNT;
if (opt == NULL ||
rtattr_parse(tb, TCA_TBF_PTAB, RTA_DATA(opt), RTA_PAYLOAD(opt)) ||
tb[TCA_TBF_PARMS-1] == NULL ||
RTA_PAYLOAD(tb[TCA_TBF_PARMS-1]) < sizeof(*qopt)) {
MOD_DEC_USE_COUNT;
return -EINVAL;
}
qopt = RTA_DATA(tb[TCA_TBF_PARMS-1]);
q->R_tab = qdisc_get_rtab(&qopt->rate, tb[TCA_TBF_RTAB-1]);
if (q->R_tab == NULL) {
MOD_DEC_USE_COUNT;
return -EINVAL;
}
if (qopt->peakrate.rate) {
q->P_tab = qdisc_get_rtab(&qopt->rate, tb[TCA_TBF_PTAB-1]);
if (q->P_tab == NULL) {
MOD_DEC_USE_COUNT;
qdisc_put_rtab(q->R_tab);
return -EINVAL;
}
}
PSCHED_GET_TIME(q->t_c);
init_timer(&q->wd_timer);
q->wd_timer.function = tbf_watchdog;
q->wd_timer.data = (unsigned long)sch;
q->limit = qopt->limit;
q->mtu = qopt->mtu;
if (q->mtu == 0)
q->mtu = psched_mtu(sch->dev);
q->buffer = qopt->buffer;
q->tokens = q->buffer;
q->ptokens = q->mtu;
return 0;
}
static struct sk_buff *netem_dequeue(struct Qdisc *sch)
{
struct netem_sched_data *q = qdisc_priv(sch);
struct sk_buff *skb;
skb = q->qdisc->dequeue(q->qdisc);
if (skb) {
const struct netem_skb_cb *cb
= (const struct netem_skb_cb *)skb->cb;
psched_time_t now;
/* if more time remaining? */
PSCHED_GET_TIME(now);
if (PSCHED_TLESS(cb->time_to_send, now)) {
pr_debug("netem_dequeue: return skb=%p\n", skb);
sch->q.qlen--;
sch->flags &= ~TCQ_F_THROTTLED;
return skb;
} else {
psched_tdiff_t delay = PSCHED_TDIFF(cb->time_to_send, now);
if (q->qdisc->ops->requeue(skb, q->qdisc) != NET_XMIT_SUCCESS) {
qdisc_tree_decrease_qlen(q->qdisc, 1);
sch->qstats.drops++;
printk(KERN_ERR "netem: queue discpline %s could not requeue\n",
q->qdisc->ops->id);
}
mod_timer(&q->timer, jiffies + PSCHED_US2JIFFIE(delay));
sch->flags |= TCQ_F_THROTTLED;
}
}
return NULL;
}
static void netem_watchdog(unsigned long arg)
{
struct Qdisc *sch = (struct Qdisc *)arg;
struct netem_sched_data *q = qdisc_priv(sch);
struct net_device *dev = sch->dev;
struct sk_buff *skb;
psched_time_t now;
pr_debug("netem_watchdog: fired @%lu\n", jiffies);
spin_lock_bh(&dev->queue_lock);
PSCHED_GET_TIME(now);
while ((skb = skb_peek(&q->delayed)) != NULL) {
const struct netem_skb_cb *cb
= (const struct netem_skb_cb *)skb->cb;
long delay
= PSCHED_US2JIFFIE(PSCHED_TDIFF(cb->time_to_send, now));
pr_debug("netem_watchdog: skb %p@%lu %ld\n",
skb, jiffies, delay);
/* if more time remaining? */
if (delay > 0) {
mod_timer(&q->timer, jiffies + delay);
break;
}
__skb_unlink(skb, &q->delayed);
if (q->qdisc->enqueue(skb, q->qdisc)) {
sch->q.qlen--;
sch->qstats.drops++;
}
}
qdisc_run(dev);
spin_unlock_bh(&dev->queue_lock);
}
/* Put skb in the private delayed queue. */
static int delay_skb(struct Qdisc *sch, struct sk_buff *skb)
{
struct netem_sched_data *q = qdisc_priv(sch);
struct netem_skb_cb *cb = (struct netem_skb_cb *)skb->cb;
psched_tdiff_t td;
psched_time_t now;
PSCHED_GET_TIME(now);
td = tabledist(q->latency, q->jitter, &q->delay_cor, q->delay_dist);
PSCHED_TADD2(now, td, cb->time_to_send);
/* Always queue at tail to keep packets in order */
if (likely(q->delayed.qlen < q->limit)) {
__skb_queue_tail(&q->delayed, skb);
if (!timer_pending(&q->timer)) {
q->timer.expires = jiffies + PSCHED_US2JIFFIE(td);
add_timer(&q->timer);
}
return NET_XMIT_SUCCESS;
}
kfree_skb(skb);
return NET_XMIT_DROP;
}
static struct sk_buff *tbf_dequeue(struct Qdisc* sch)
{
struct tbf_sched_data *q = (struct tbf_sched_data *)sch->data;
struct sk_buff *skb;
skb = q->qdisc->dequeue(q->qdisc);
if (skb) {
psched_time_t now;
long toks;
long ptoks = 0;
unsigned int len = skb->len;
PSCHED_GET_TIME(now);
toks = PSCHED_TDIFF_SAFE(now, q->t_c, q->buffer, 0);
if (q->P_tab) {
ptoks = toks + q->ptokens;
if (ptoks > (long)q->mtu)
ptoks = q->mtu;
ptoks -= L2T_P(q, len);
}
toks += q->tokens;
if (toks > (long)q->buffer)
toks = q->buffer;
toks -= L2T(q, len);
if ((toks|ptoks) >= 0) {
q->t_c = now;
q->tokens = toks;
q->ptokens = ptoks;
sch->stats.backlog -= len;
sch->q.qlen--;
sch->flags &= ~TCQ_F_THROTTLED;
return skb;
}
if (!netif_queue_stopped(sch->dev)) {
long delay = PSCHED_US2JIFFIE(max_t(long, -toks, -ptoks));
if (delay == 0)
delay = 1;
mod_timer(&q->wd_timer, jiffies+delay);
}
/* Maybe we have a shorter packet in the queue,
which can be sent now. It sounds cool,
but, however, this is wrong in principle.
We MUST NOT reorder packets under these circumstances.
Really, if we split the flow into independent
subflows, it would be a very good solution.
This is the main idea of all FQ algorithms
(cf. CSZ, HPFQ, HFSC)
*/
if (q->qdisc->ops->requeue(skb, q->qdisc) != NET_XMIT_SUCCESS) {
/* When requeue fails skb is dropped */
sch->q.qlen--;
sch->stats.backlog -= len;
sch->stats.drops++;
}
sch->flags |= TCQ_F_THROTTLED;
sch->stats.overlimits++;
}
struct tcf_police * tcf_police_locate(struct rtattr *rta, struct rtattr *est)
{
unsigned h;
struct tcf_police *p;
struct rtattr *tb[TCA_POLICE_MAX];
struct tc_police *parm;
if (rtattr_parse(tb, TCA_POLICE_MAX, RTA_DATA(rta), RTA_PAYLOAD(rta)) < 0)
return NULL;
if (tb[TCA_POLICE_TBF-1] == NULL)
return NULL;
parm = RTA_DATA(tb[TCA_POLICE_TBF-1]);
if (parm->index && (p = tcf_police_lookup(parm->index)) != NULL) {
p->refcnt++;
return p;
}
p = kmalloc(sizeof(*p), GFP_KERNEL);
if (p == NULL)
return NULL;
memset(p, 0, sizeof(*p));
p->refcnt = 1;
spin_lock_init(&p->lock);
p->stats.lock = &p->lock;
if (parm->rate.rate) {
if ((p->R_tab = qdisc_get_rtab(&parm->rate, tb[TCA_POLICE_RATE-1])) == NULL)
goto failure;
if (parm->peakrate.rate &&
(p->P_tab = qdisc_get_rtab(&parm->peakrate, tb[TCA_POLICE_PEAKRATE-1])) == NULL)
goto failure;
}
if (tb[TCA_POLICE_RESULT-1])
p->result = *(int*)RTA_DATA(tb[TCA_POLICE_RESULT-1]);
#ifdef CONFIG_NET_ESTIMATOR
if (tb[TCA_POLICE_AVRATE-1])
p->ewma_rate = *(u32*)RTA_DATA(tb[TCA_POLICE_AVRATE-1]);
#endif
p->toks = p->burst = parm->burst;
p->mtu = parm->mtu;
if (p->mtu == 0) {
p->mtu = ~0;
if (p->R_tab)
p->mtu = 255<<p->R_tab->rate.cell_log;
}
if (p->P_tab)
p->ptoks = L2T_P(p, p->mtu);
PSCHED_GET_TIME(p->t_c);
p->index = parm->index ? : tcf_police_new_index();
p->action = parm->action;
#ifdef CONFIG_NET_ESTIMATOR
if (est)
qdisc_new_estimator(&p->stats, est);
#endif
h = tcf_police_hash(p->index);
write_lock_bh(&police_lock);
p->next = tcf_police_ht[h];
tcf_police_ht[h] = p;
write_unlock_bh(&police_lock);
return p;
failure:
if (p->R_tab)
qdisc_put_rtab(p->R_tab);
kfree(p);
return NULL;
}
static struct sk_buff *
tbf_dequeue(struct Qdisc* sch)
{
struct tbf_sched_data *q = (struct tbf_sched_data *)sch->data;
struct sk_buff *skb;
skb = __skb_dequeue(&sch->q);
if (skb) {
psched_time_t now;
long toks;
long ptoks = 0;
PSCHED_GET_TIME(now);
toks = PSCHED_TDIFF_SAFE(now, q->t_c, q->buffer, 0);
if (q->P_tab) {
ptoks = toks + q->ptokens;
if (ptoks > (long)q->mtu)
ptoks = q->mtu;
ptoks -= L2T_P(q, skb->len);
}
toks += q->tokens;
if (toks > (long)q->buffer)
toks = q->buffer;
toks -= L2T(q, skb->len);
if ((toks|ptoks) >= 0) {
q->t_c = now;
q->tokens = toks;
q->ptokens = ptoks;
sch->stats.backlog -= skb->len;
return skb;
}
if (!sch->dev->tbusy) {
long delay = PSCHED_US2JIFFIE(max(-toks, -ptoks));
if (delay == 0)
delay = 1;
del_timer(&q->wd_timer);
q->wd_timer.expires = jiffies + delay;
add_timer(&q->wd_timer);
}
/* Maybe we have a shorter packet in the queue,
which can be sent now. It sounds cool,
but, however, this is wrong in principle.
We MUST NOT reorder packets under these circumstances.
Really, if we split the flow into independent
subflows, it would be a very good solution.
This is the main idea of all FQ algorithms
(cf. CSZ, HPFQ, HFCS)
*/
__skb_queue_head(&sch->q, skb);
sch->stats.overlimits++;
}
return NULL;
}
/*
* Insert one skb into qdisc.
* Note: parent depends on return value to account for queue length.
* NET_XMIT_DROP: queue length didn't change.
* NET_XMIT_SUCCESS: one skb was queued.
*/
static int netem_enqueue(struct sk_buff *skb, struct Qdisc *sch)
{
struct netem_sched_data *q = qdisc_priv(sch);
/* We don't fill cb now as skb_unshare() may invalidate it */
struct netem_skb_cb *cb;
struct sk_buff *skb2;
int ret;
int count = 1;
pr_debug("netem_enqueue skb=%p\n", skb);
/* Random duplication */
if (q->duplicate && q->duplicate >= get_crandom(&q->dup_cor))
++count;
/* Random packet drop 0 => none, ~0 => all */
if (q->loss && q->loss >= get_crandom(&q->loss_cor))
--count;
if (count == 0) {
sch->qstats.drops++;
kfree_skb(skb);
return NET_XMIT_BYPASS;
}
skb_orphan(skb);
/*
* If we need to duplicate packet, then re-insert at top of the
* qdisc tree, since parent queuer expects that only one
* skb will be queued.
*/
if (count > 1 && (skb2 = skb_clone(skb, GFP_ATOMIC)) != NULL) {
struct Qdisc *rootq = sch->dev->qdisc;
u32 dupsave = q->duplicate; /* prevent duplicating a dup... */
q->duplicate = 0;
rootq->enqueue(skb2, rootq);
q->duplicate = dupsave;
}
/*
* Randomized packet corruption.
* Make copy if needed since we are modifying
* If packet is going to be hardware checksummed, then
* do it now in software before we mangle it.
*/
if (q->corrupt && q->corrupt >= get_crandom(&q->corrupt_cor)) {
if (!(skb = skb_unshare(skb, GFP_ATOMIC))
|| (skb->ip_summed == CHECKSUM_PARTIAL
&& skb_checksum_help(skb))) {
sch->qstats.drops++;
return NET_XMIT_DROP;
}
skb->data[net_random() % skb_headlen(skb)] ^= 1<<(net_random() % 8);
}
cb = (struct netem_skb_cb *)skb->cb;
if (q->gap == 0 /* not doing reordering */
|| q->counter < q->gap /* inside last reordering gap */
|| q->reorder < get_crandom(&q->reorder_cor)) {
psched_time_t now;
psched_tdiff_t delay;
delay = tabledist(q->latency, q->jitter,
&q->delay_cor, q->delay_dist);
PSCHED_GET_TIME(now);
PSCHED_TADD2(now, delay, cb->time_to_send);
++q->counter;
ret = q->qdisc->enqueue(skb, q->qdisc);
} else {
/*
* Do re-ordering by putting one out of N packets at the front
* of the queue.
*/
PSCHED_GET_TIME(cb->time_to_send);
q->counter = 0;
ret = q->qdisc->ops->requeue(skb, q->qdisc);
}
if (likely(ret == NET_XMIT_SUCCESS)) {
sch->q.qlen++;
sch->bstats.bytes += skb->len;
sch->bstats.packets++;
} else
sch->qstats.drops++;
pr_debug("netem: enqueue ret %d\n", ret);
return ret;
}
