[deliverable/linux.git] / net / mac80211 / rc80211_pid_algo.c

/*
 * Copyright 2002-2005, Instant802 Networks, Inc.
 * Copyright 2005, Devicescape Software, Inc.
 * Copyright 2007, Mattias Nissler <mattias.nissler@gmx.de>
 * Copyright 2007-2008, Stefano Brivio <stefano.brivio@polimi.it>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */

#include <linux/netdevice.h>
#include <linux/types.h>
#include <linux/skbuff.h>
#include <linux/debugfs.h>
#include <net/mac80211.h>
#include "ieee80211_rate.h"

#include "rc80211_pid.h"


/* This is an implementation of a TX rate control algorithm that uses a PID
 * controller. Given a target failed frames rate, the controller decides about
 * TX rate changes to meet the target failed frames rate.
 *
 * The controller basically computes the following:
 *
 * adj = CP * err + CI * err_avg + CD * (err - last_err) * (1 + sharpening)
 *
 * where
 * 	adj	adjustment value that is used to switch TX rate (see below)
 * 	err	current error: target vs. current failed frames percentage
 * 	last_err	last error
 * 	err_avg	average (i.e. poor man's integral) of recent errors
 *	sharpening	non-zero when fast response is needed (i.e. right after
 *			association or no frames sent for a long time), heading
 * 			to zero over time
 * 	CP	Proportional coefficient
 * 	CI	Integral coefficient
 * 	CD	Derivative coefficient
 *
 * CP, CI, CD are subject to careful tuning.
 *
 * The integral component uses a exponential moving average approach instead of
 * an actual sliding window. The advantage is that we don't need to keep an
 * array of the last N error values and computation is easier.
 *
 * Once we have the adj value, we map it to a rate by means of a learning
 * algorithm. This algorithm keeps the state of the percentual failed frames
 * difference between rates. The behaviour of the lowest available rate is kept
 * as a reference value, and every time we switch between two rates, we compute
 * the difference between the failed frames each rate exhibited. By doing so,
 * we compare behaviours which different rates exhibited in adjacent timeslices,
 * thus the comparison is minimally affected by external conditions. This
 * difference gets propagated to the whole set of measurements, so that the
 * reference is always the same. Periodically, we normalize this set so that
 * recent events weigh the most. By comparing the adj value with this set, we
 * avoid pejorative switches to lower rates and allow for switches to higher
 * rates if they behaved well.
 *
 * Note that for the computations we use a fixed-point representation to avoid
 * floating point arithmetic. Hence, all values are shifted left by
 * RC_PID_ARITH_SHIFT.
 */

/* Adjust the rate while ensuring that we won't switch to a lower rate if it
 * exhibited a worse failed frames behaviour and we'll choose the highest rate
 * whose failed frames behaviour is not worse than the one of the original rate
 * target. While at it, check that the new rate is valid. */
static void rate_control_pid_adjust_rate(struct ieee80211_local *local,
					 struct sta_info *sta, int adj,
					 struct rc_pid_rateinfo *rinfo)
{
	struct ieee80211_sub_if_data *sdata;
	struct ieee80211_hw_mode *mode;
	int cur_sorted, new_sorted, probe, tmp, n_bitrates;
	int cur = sta->txrate;

	sdata = IEEE80211_DEV_TO_SUB_IF(sta->dev);

	mode = local->oper_hw_mode;
	n_bitrates = mode->num_rates;

	/* Map passed arguments to sorted values. */
	cur_sorted = rinfo[cur].rev_index;
	new_sorted = cur_sorted + adj;

	/* Check limits. */
	if (new_sorted < 0)
		new_sorted = rinfo[0].rev_index;
	else if (new_sorted >= n_bitrates)
		new_sorted = rinfo[n_bitrates - 1].rev_index;

	tmp = new_sorted;

	if (adj < 0) {
		/* Ensure that the rate decrease isn't disadvantageous. */
		for (probe = cur_sorted; probe >= new_sorted; probe--)
			if (rinfo[probe].diff <= rinfo[cur_sorted].diff &&
			    rate_supported(sta, mode, rinfo[probe].index))
				tmp = probe;
	} else {
		/* Look for rate increase with zero (or below) cost. */
		for (probe = new_sorted + 1; probe < n_bitrates; probe++)
			if (rinfo[probe].diff <= rinfo[new_sorted].diff &&
			    rate_supported(sta, mode, rinfo[probe].index))
				tmp = probe;
	}

	/* Fit the rate found to the nearest supported rate. */
	do {
		if (rate_supported(sta, mode, rinfo[tmp].index)) {
			sta->txrate = rinfo[tmp].index;
			break;
		}
		if (adj < 0)
			tmp--;
		else
			tmp++;
	} while (tmp < n_bitrates && tmp >= 0);

#ifdef CONFIG_MAC80211_DEBUGFS
	rate_control_pid_event_rate_change(
		&((struct rc_pid_sta_info *)sta->rate_ctrl_priv)->events,
		cur, mode->rates[cur].rate);
#endif
}

/* Normalize the failed frames per-rate differences. */
static void rate_control_pid_normalize(struct rc_pid_info *pinfo, int l)
{
	int i, norm_offset = pinfo->norm_offset;
	struct rc_pid_rateinfo *r = pinfo->rinfo;

	if (r[0].diff > norm_offset)
		r[0].diff -= norm_offset;
	else if (r[0].diff < -norm_offset)
		r[0].diff += norm_offset;
	for (i = 0; i < l - 1; i++)
		if (r[i + 1].diff > r[i].diff + norm_offset)
			r[i + 1].diff -= norm_offset;
		else if (r[i + 1].diff <= r[i].diff)
			r[i + 1].diff += norm_offset;
}

static void rate_control_pid_sample(struct rc_pid_info *pinfo,
				    struct ieee80211_local *local,
				    struct sta_info *sta)
{
	struct rc_pid_sta_info *spinfo = sta->rate_ctrl_priv;
	struct rc_pid_rateinfo *rinfo = pinfo->rinfo;
	struct ieee80211_hw_mode *mode;
	u32 pf;
	s32 err_avg;
	u32 err_prop;
	u32 err_int;
	u32 err_der;
	int adj, i, j, tmp;
	unsigned long period;

	mode = local->oper_hw_mode;
	spinfo = sta->rate_ctrl_priv;

	/* In case nothing happened during the previous control interval, turn
	 * the sharpening factor on. */
	period = (HZ * pinfo->sampling_period + 500) / 1000;
	if (!period)
		period = 1;
	if (jiffies - spinfo->last_sample > 2 * period)
		spinfo->sharp_cnt = pinfo->sharpen_duration;

	spinfo->last_sample = jiffies;

	/* This should never happen, but in case, we assume the old sample is
	 * still a good measurement and copy it. */
	if (unlikely(spinfo->tx_num_xmit == 0))
		pf = spinfo->last_pf;
	else {
		pf = spinfo->tx_num_failed * 100 / spinfo->tx_num_xmit;
		pf <<= RC_PID_ARITH_SHIFT;
	}

	spinfo->tx_num_xmit = 0;
	spinfo->tx_num_failed = 0;

	/* If we just switched rate, update the rate behaviour info. */
	if (pinfo->oldrate != sta->txrate) {

		i = rinfo[pinfo->oldrate].rev_index;
		j = rinfo[sta->txrate].rev_index;

		tmp = (pf - spinfo->last_pf);
		tmp = RC_PID_DO_ARITH_RIGHT_SHIFT(tmp, RC_PID_ARITH_SHIFT);

		rinfo[j].diff = rinfo[i].diff + tmp;
		pinfo->oldrate = sta->txrate;
	}
	rate_control_pid_normalize(pinfo, mode->num_rates);

	/* Compute the proportional, integral and derivative errors. */
	err_prop = (pinfo->target << RC_PID_ARITH_SHIFT) - pf;

	err_avg = spinfo->err_avg_sc >> pinfo->smoothing_shift;
	spinfo->err_avg_sc = spinfo->err_avg_sc - err_avg + err_prop;
	err_int = spinfo->err_avg_sc >> pinfo->smoothing_shift;

	err_der = (pf - spinfo->last_pf) *
		  (1 + pinfo->sharpen_factor * spinfo->sharp_cnt);
	spinfo->last_pf = pf;
	if (spinfo->sharp_cnt)
			spinfo->sharp_cnt--;

#ifdef CONFIG_MAC80211_DEBUGFS
	rate_control_pid_event_pf_sample(&spinfo->events, pf, err_prop, err_int,
					 err_der);
#endif

	/* Compute the controller output. */
	adj = (err_prop * pinfo->coeff_p + err_int * pinfo->coeff_i
	      + err_der * pinfo->coeff_d);
	adj = RC_PID_DO_ARITH_RIGHT_SHIFT(adj, 2 * RC_PID_ARITH_SHIFT);

	/* Change rate. */
	if (adj)
		rate_control_pid_adjust_rate(local, sta, adj, rinfo);
}

static void rate_control_pid_tx_status(void *priv, struct net_device *dev,
				       struct sk_buff *skb,
				       struct ieee80211_tx_status *status)
{
	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
	struct ieee80211_sub_if_data *sdata;
	struct rc_pid_info *pinfo = priv;
	struct sta_info *sta;
	struct rc_pid_sta_info *spinfo;
	unsigned long period;

	sta = sta_info_get(local, hdr->addr1);

	if (!sta)
		return;

	/* Don't update the state if we're not controlling the rate. */
	sdata = IEEE80211_DEV_TO_SUB_IF(sta->dev);
	if (sdata->bss && sdata->bss->force_unicast_rateidx > -1) {
		sta->txrate = sdata->bss->max_ratectrl_rateidx;
		return;
	}

	/* Ignore all frames that were sent with a different rate than the rate
	 * we currently advise mac80211 to use. */
	if (status->control.rate != &local->oper_hw_mode->rates[sta->txrate])
		goto ignore;

	spinfo = sta->rate_ctrl_priv;
	spinfo->tx_num_xmit++;

#ifdef CONFIG_MAC80211_DEBUGFS
	rate_control_pid_event_tx_status(&spinfo->events, status);
#endif

	/* We count frames that totally failed to be transmitted as two bad
	 * frames, those that made it out but had some retries as one good and
	 * one bad frame. */
	if (status->excessive_retries) {
		spinfo->tx_num_failed += 2;
		spinfo->tx_num_xmit++;
	} else if (status->retry_count) {
		spinfo->tx_num_failed++;
		spinfo->tx_num_xmit++;
	}

	if (status->excessive_retries) {
		sta->tx_retry_failed++;
		sta->tx_num_consecutive_failures++;
		sta->tx_num_mpdu_fail++;
	} else {
		sta->last_ack_rssi[0] = sta->last_ack_rssi[1];
		sta->last_ack_rssi[1] = sta->last_ack_rssi[2];
		sta->last_ack_rssi[2] = status->ack_signal;
		sta->tx_num_consecutive_failures = 0;
		sta->tx_num_mpdu_ok++;
	}
	sta->tx_retry_count += status->retry_count;
	sta->tx_num_mpdu_fail += status->retry_count;

	/* Update PID controller state. */
	period = (HZ * pinfo->sampling_period + 500) / 1000;
	if (!period)
		period = 1;
	if (time_after(jiffies, spinfo->last_sample + period))
		rate_control_pid_sample(pinfo, local, sta);

ignore:
	sta_info_put(sta);
}

static void rate_control_pid_get_rate(void *priv, struct net_device *dev,
				      struct ieee80211_hw_mode *mode,
				      struct sk_buff *skb,
				      struct rate_selection *sel)
{
	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
	struct ieee80211_sub_if_data *sdata;
	struct sta_info *sta;
	int rateidx;
	u16 fc;

	sta = sta_info_get(local, hdr->addr1);

	/* Send management frames and broadcast/multicast data using lowest
	 * rate. */
	fc = le16_to_cpu(hdr->frame_control);
	if ((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA ||
	    is_multicast_ether_addr(hdr->addr1) || !sta) {
		sel->rate = rate_lowest(local, mode, sta);
		if (sta)
			sta_info_put(sta);
		return;
	}

	/* If a forced rate is in effect, select it. */
	sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	if (sdata->bss && sdata->bss->force_unicast_rateidx > -1)
		sta->txrate = sdata->bss->force_unicast_rateidx;

	rateidx = sta->txrate;

	if (rateidx >= mode->num_rates)
		rateidx = mode->num_rates - 1;

	sta->last_txrate = rateidx;

	sta_info_put(sta);

	sel->rate = &mode->rates[rateidx];

#ifdef CONFIG_MAC80211_DEBUGFS
	rate_control_pid_event_tx_rate(
		&((struct rc_pid_sta_info *) sta->rate_ctrl_priv)->events,
		rateidx, mode->rates[rateidx].rate);
#endif
}

static void rate_control_pid_rate_init(void *priv, void *priv_sta,
					  struct ieee80211_local *local,
					  struct sta_info *sta)
{
	/* TODO: This routine should consider using RSSI from previous packets
	 * as we need to have IEEE 802.1X auth succeed immediately after assoc..
	 * Until that method is implemented, we will use the lowest supported
	 * rate as a workaround. */
	sta->txrate = rate_lowest_index(local, local->oper_hw_mode, sta);
}

static void *rate_control_pid_alloc(struct ieee80211_local *local)
{
	struct rc_pid_info *pinfo;
	struct rc_pid_rateinfo *rinfo;
	struct ieee80211_hw_mode *mode;
	int i, j, tmp;
	bool s;
#ifdef CONFIG_MAC80211_DEBUGFS
	struct rc_pid_debugfs_entries *de;
#endif

	pinfo = kmalloc(sizeof(*pinfo), GFP_ATOMIC);
	if (!pinfo)
		return NULL;

	/* We can safely assume that oper_hw_mode won't change unless we get
	 * reinitialized. */
	mode = local->oper_hw_mode;
	rinfo = kmalloc(sizeof(*rinfo) * mode->num_rates, GFP_ATOMIC);
	if (!rinfo) {
		kfree(pinfo);
		return NULL;
	}

	/* Sort the rates. This is optimized for the most common case (i.e.
	 * almost-sorted CCK+OFDM rates). Kind of bubble-sort with reversed
	 * mapping too. */
	for (i = 0; i < mode->num_rates; i++) {
		rinfo[i].index = i;
		rinfo[i].rev_index = i;
		if (pinfo->fast_start)
			rinfo[i].diff = 0;
		else
			rinfo[i].diff = i * pinfo->norm_offset;
	}
	for (i = 1; i < mode->num_rates; i++) {
		s = 0;
		for (j = 0; j < mode->num_rates - i; j++)
			if (unlikely(mode->rates[rinfo[j].index].rate >
				     mode->rates[rinfo[j + 1].index].rate)) {
				tmp = rinfo[j].index;
				rinfo[j].index = rinfo[j + 1].index;
				rinfo[j + 1].index = tmp;
				rinfo[rinfo[j].index].rev_index = j;
				rinfo[rinfo[j + 1].index].rev_index = j + 1;
				s = 1;
			}
		if (!s)
			break;
	}

	pinfo->target = RC_PID_TARGET_PF;
	pinfo->sampling_period = RC_PID_INTERVAL;
	pinfo->coeff_p = RC_PID_COEFF_P;
	pinfo->coeff_i = RC_PID_COEFF_I;
	pinfo->coeff_d = RC_PID_COEFF_D;
	pinfo->smoothing_shift = RC_PID_SMOOTHING_SHIFT;
	pinfo->sharpen_factor = RC_PID_SHARPENING_FACTOR;
	pinfo->sharpen_duration = RC_PID_SHARPENING_DURATION;
	pinfo->norm_offset = RC_PID_NORM_OFFSET;
	pinfo->fast_start = RC_PID_FAST_START;
	pinfo->rinfo = rinfo;
	pinfo->oldrate = 0;

#ifdef CONFIG_MAC80211_DEBUGFS
	de = &pinfo->dentries;
	de->dir = debugfs_create_dir("rc80211_pid",
				     local->hw.wiphy->debugfsdir);
	de->target = debugfs_create_u32("target_pf", S_IRUSR | S_IWUSR,
					de->dir, &pinfo->target);
	de->sampling_period = debugfs_create_u32("sampling_period",
						 S_IRUSR | S_IWUSR, de->dir,
						 &pinfo->sampling_period);
	de->coeff_p = debugfs_create_u32("coeff_p", S_IRUSR | S_IWUSR,
					 de->dir, &pinfo->coeff_p);
	de->coeff_i = debugfs_create_u32("coeff_i", S_IRUSR | S_IWUSR,
					 de->dir, &pinfo->coeff_i);
	de->coeff_d = debugfs_create_u32("coeff_d", S_IRUSR | S_IWUSR,
					 de->dir, &pinfo->coeff_d);
	de->smoothing_shift = debugfs_create_u32("smoothing_shift",
						 S_IRUSR | S_IWUSR, de->dir,
						 &pinfo->smoothing_shift);
	de->sharpen_factor = debugfs_create_u32("sharpen_factor",
					       S_IRUSR | S_IWUSR, de->dir,
					       &pinfo->sharpen_factor);
	de->sharpen_duration = debugfs_create_u32("sharpen_duration",
						  S_IRUSR | S_IWUSR, de->dir,
						  &pinfo->sharpen_duration);
	de->norm_offset = debugfs_create_u32("norm_offset",
					     S_IRUSR | S_IWUSR, de->dir,
					     &pinfo->norm_offset);
	de->fast_start = debugfs_create_bool("fast_start",
					     S_IRUSR | S_IWUSR, de->dir,
					     &pinfo->fast_start);
#endif

	return pinfo;
}

static void rate_control_pid_free(void *priv)
{
	struct rc_pid_info *pinfo = priv;
#ifdef CONFIG_MAC80211_DEBUGFS
	struct rc_pid_debugfs_entries *de = &pinfo->dentries;

	debugfs_remove(de->fast_start);
	debugfs_remove(de->norm_offset);
	debugfs_remove(de->sharpen_duration);
	debugfs_remove(de->sharpen_factor);
	debugfs_remove(de->smoothing_shift);
	debugfs_remove(de->coeff_d);
	debugfs_remove(de->coeff_i);
	debugfs_remove(de->coeff_p);
	debugfs_remove(de->sampling_period);
	debugfs_remove(de->target);
	debugfs_remove(de->dir);
#endif

	kfree(pinfo->rinfo);
	kfree(pinfo);
}

static void rate_control_pid_clear(void *priv)
{
}

static void *rate_control_pid_alloc_sta(void *priv, gfp_t gfp)
{
	struct rc_pid_sta_info *spinfo;

	spinfo = kzalloc(sizeof(*spinfo), gfp);
	if (spinfo == NULL)
		return NULL;

	spinfo->last_sample = jiffies;

#ifdef CONFIG_MAC80211_DEBUGFS
	spin_lock_init(&spinfo->events.lock);
	init_waitqueue_head(&spinfo->events.waitqueue);
#endif

	return spinfo;
}

static void rate_control_pid_free_sta(void *priv, void *priv_sta)
{
	struct rc_pid_sta_info *spinfo = priv_sta;
	kfree(spinfo);
}

static struct rate_control_ops mac80211_rcpid = {
	.name = "pid",
	.tx_status = rate_control_pid_tx_status,
	.get_rate = rate_control_pid_get_rate,
	.rate_init = rate_control_pid_rate_init,
	.clear = rate_control_pid_clear,
	.alloc = rate_control_pid_alloc,
	.free = rate_control_pid_free,
	.alloc_sta = rate_control_pid_alloc_sta,
	.free_sta = rate_control_pid_free_sta,
#ifdef CONFIG_MAC80211_DEBUGFS
	.add_sta_debugfs = rate_control_pid_add_sta_debugfs,
	.remove_sta_debugfs = rate_control_pid_remove_sta_debugfs,
#endif
};

MODULE_DESCRIPTION("PID controller based rate control algorithm");
MODULE_AUTHOR("Stefano Brivio");
MODULE_AUTHOR("Mattias Nissler");
MODULE_LICENSE("GPL");

int __init rc80211_pid_init(void)
{
	return ieee80211_rate_control_register(&mac80211_rcpid);
}

void rc80211_pid_exit(void)
{
	ieee80211_rate_control_unregister(&mac80211_rcpid);
}

#ifdef CONFIG_MAC80211_RC_PID_MODULE
module_init(rc80211_pid_init);
module_exit(rc80211_pid_exit);
#endif
Commit	Line	Data
ad018375 MN	1	/*
	2	* Copyright 2002-2005, Instant802 Networks, Inc.
	3	* Copyright 2005, Devicescape Software, Inc.
	4	* Copyright 2007, Mattias Nissler <mattias.nissler@gmx.de>
1d60ab05	5	* Copyright 2007-2008, Stefano Brivio <stefano.brivio@polimi.it>
ad018375 MN	6	*
	7	* This program is free software; you can redistribute it and/or modify
	8	* it under the terms of the GNU General Public License version 2 as
	9	* published by the Free Software Foundation.
	10	*/
	11
	12	#include <linux/netdevice.h>
	13	#include <linux/types.h>
	14	#include <linux/skbuff.h>
4b475898	15	#include <linux/debugfs.h>
ad018375 MN	16	#include <net/mac80211.h>
	17	#include "ieee80211_rate.h"
	18
12446c67 MN	19	#include "rc80211_pid.h"
12446c67 MN	20
ad018375 MN	21
	22	/* This is an implementation of a TX rate control algorithm that uses a PID
	23	* controller. Given a target failed frames rate, the controller decides about
	24	* TX rate changes to meet the target failed frames rate.
	25	*
	26	* The controller basically computes the following:
	27	*
1dc4d1e6	28	* adj = CP * err + CI * err_avg + CD * (err - last_err) * (1 + sharpening)
ad018375 MN	29	*
	30	* where
	31	* adj adjustment value that is used to switch TX rate (see below)
	32	* err current error: target vs. current failed frames percentage
	33	* last_err last error
	34	* err_avg average (i.e. poor man's integral) of recent errors
1dc4d1e6 SB	35	* sharpening non-zero when fast response is needed (i.e. right after
	36	* association or no frames sent for a long time), heading
	37	* to zero over time
ad018375 MN	38	* CP Proportional coefficient
	39	* CI Integral coefficient
	40	* CD Derivative coefficient
	41	*
	42	* CP, CI, CD are subject to careful tuning.
	43	*
	44	* The integral component uses a exponential moving average approach instead of
	45	* an actual sliding window. The advantage is that we don't need to keep an
	46	* array of the last N error values and computation is easier.
	47	*
90d501d6 SB	48	* Once we have the adj value, we map it to a rate by means of a learning
	49	* algorithm. This algorithm keeps the state of the percentual failed frames
	50	* difference between rates. The behaviour of the lowest available rate is kept
	51	* as a reference value, and every time we switch between two rates, we compute
	52	* the difference between the failed frames each rate exhibited. By doing so,
	53	* we compare behaviours which different rates exhibited in adjacent timeslices,
	54	* thus the comparison is minimally affected by external conditions. This
	55	* difference gets propagated to the whole set of measurements, so that the
	56	* reference is always the same. Periodically, we normalize this set so that
	57	* recent events weigh the most. By comparing the adj value with this set, we
	58	* avoid pejorative switches to lower rates and allow for switches to higher
	59	* rates if they behaved well.
ad018375 MN	60	*
	61	* Note that for the computations we use a fixed-point representation to avoid
	62	* floating point arithmetic. Hence, all values are shifted left by
	63	* RC_PID_ARITH_SHIFT.
	64	*/
	65
1d60ab05 SB	66	/* Adjust the rate while ensuring that we won't switch to a lower rate if it
	67	* exhibited a worse failed frames behaviour and we'll choose the highest rate
	68	* whose failed frames behaviour is not worse than the one of the original rate
	69	* target. While at it, check that the new rate is valid. */
ad018375	70	static void rate_control_pid_adjust_rate(struct ieee80211_local *local,
90d501d6 SB	71	struct sta_info *sta, int adj,
90d501d6 SB	72	struct rc_pid_rateinfo *rinfo)
ad018375 MN	73	{
	74	struct ieee80211_sub_if_data *sdata;
	75	struct ieee80211_hw_mode *mode;
1d60ab05 SB	76	int cur_sorted, new_sorted, probe, tmp, n_bitrates;
1d60ab05 SB	77	int cur = sta->txrate;
ad018375 MN	78
ad018375 MN	79	sdata = IEEE80211_DEV_TO_SUB_IF(sta->dev);
ad018375 MN	80
ad018375 MN	81	mode = local->oper_hw_mode;
1d60ab05	82	n_bitrates = mode->num_rates;
ad018375	83
1d60ab05 SB	84	/* Map passed arguments to sorted values. */
	85	cur_sorted = rinfo[cur].rev_index;
	86	new_sorted = cur_sorted + adj;
ad018375	87
1d60ab05 SB	88	/* Check limits. */
	89	if (new_sorted < 0)
	90	new_sorted = rinfo[0].rev_index;
	91	else if (new_sorted >= n_bitrates)
	92	new_sorted = rinfo[n_bitrates - 1].rev_index;
	93
	94	tmp = new_sorted;
ad018375	95
1d60ab05 SB	96	if (adj < 0) {
	97	/* Ensure that the rate decrease isn't disadvantageous. */
	98	for (probe = cur_sorted; probe >= new_sorted; probe--)
	99	if (rinfo[probe].diff <= rinfo[cur_sorted].diff &&
	100	rate_supported(sta, mode, rinfo[probe].index))
	101	tmp = probe;
	102	} else {
	103	/* Look for rate increase with zero (or below) cost. */
	104	for (probe = new_sorted + 1; probe < n_bitrates; probe++)
	105	if (rinfo[probe].diff <= rinfo[new_sorted].diff &&
	106	rate_supported(sta, mode, rinfo[probe].index))
	107	tmp = probe;
ad018375	108	}
12446c67	109
1d60ab05 SB	110	/* Fit the rate found to the nearest supported rate. */
	111	do {
	112	if (rate_supported(sta, mode, rinfo[tmp].index)) {
	113	sta->txrate = rinfo[tmp].index;
	114	break;
	115	}
	116	if (adj < 0)
	117	tmp--;
	118	else
	119	tmp++;
	120	} while (tmp < n_bitrates && tmp >= 0);
	121
12446c67 MN	122	#ifdef CONFIG_MAC80211_DEBUGFS
	123	rate_control_pid_event_rate_change(
	124	&((struct rc_pid_sta_info *)sta->rate_ctrl_priv)->events,
1d60ab05	125	cur, mode->rates[cur].rate);
12446c67	126	#endif
ad018375 MN	127	}
ad018375 MN	128
90d501d6	129	/* Normalize the failed frames per-rate differences. */
1946b74c	130	static void rate_control_pid_normalize(struct rc_pid_info *pinfo, int l)
90d501d6	131	{
1946b74c MN	132	int i, norm_offset = pinfo->norm_offset;
1946b74c MN	133	struct rc_pid_rateinfo *r = pinfo->rinfo;
90d501d6	134
1946b74c MN	135	if (r[0].diff > norm_offset)
	136	r[0].diff -= norm_offset;
	137	else if (r[0].diff < -norm_offset)
	138	r[0].diff += norm_offset;
90d501d6	139	for (i = 0; i < l - 1; i++)
1946b74c MN	140	if (r[i + 1].diff > r[i].diff + norm_offset)
1946b74c MN	141	r[i + 1].diff -= norm_offset;
90d501d6	142	else if (r[i + 1].diff <= r[i].diff)
1946b74c	143	r[i + 1].diff += norm_offset;
90d501d6 SB	144	}
90d501d6 SB	145
ad018375 MN	146	static void rate_control_pid_sample(struct rc_pid_info *pinfo,
	147	struct ieee80211_local *local,
	148	struct sta_info *sta)
	149	{
	150	struct rc_pid_sta_info *spinfo = sta->rate_ctrl_priv;
90d501d6 SB	151	struct rc_pid_rateinfo *rinfo = pinfo->rinfo;
90d501d6 SB	152	struct ieee80211_hw_mode *mode;
ad018375 MN	153	u32 pf;
ad018375 MN	154	s32 err_avg;
1946b74c MN	155	u32 err_prop;
	156	u32 err_int;
	157	u32 err_der;
90d501d6	158	int adj, i, j, tmp;
1946b74c	159	unsigned long period;
ad018375	160
90d501d6	161	mode = local->oper_hw_mode;
ad018375	162	spinfo = sta->rate_ctrl_priv;
1dc4d1e6 SB	163
	164	/* In case nothing happened during the previous control interval, turn
	165	* the sharpening factor on. */
1946b74c MN	166	period = (HZ * pinfo->sampling_period + 500) / 1000;
	167	if (!period)
	168	period = 1;
	169	if (jiffies - spinfo->last_sample > 2 * period)
	170	spinfo->sharp_cnt = pinfo->sharpen_duration;
1dc4d1e6	171
ad018375 MN	172	spinfo->last_sample = jiffies;
ad018375 MN	173
1dc4d1e6	174	/* This should never happen, but in case, we assume the old sample is
ad018375	175	* still a good measurement and copy it. */
1dc4d1e6	176	if (unlikely(spinfo->tx_num_xmit == 0))
ad018375 MN	177	pf = spinfo->last_pf;
	178	else {
	179	pf = spinfo->tx_num_failed * 100 / spinfo->tx_num_xmit;
	180	pf <<= RC_PID_ARITH_SHIFT;
ad018375 MN	181	}
ad018375 MN	182
1dc4d1e6 SB	183	spinfo->tx_num_xmit = 0;
	184	spinfo->tx_num_failed = 0;
	185
90d501d6 SB	186	/* If we just switched rate, update the rate behaviour info. */
	187	if (pinfo->oldrate != sta->txrate) {
	188
	189	i = rinfo[pinfo->oldrate].rev_index;
	190	j = rinfo[sta->txrate].rev_index;
	191
	192	tmp = (pf - spinfo->last_pf);
	193	tmp = RC_PID_DO_ARITH_RIGHT_SHIFT(tmp, RC_PID_ARITH_SHIFT);
	194
	195	rinfo[j].diff = rinfo[i].diff + tmp;
	196	pinfo->oldrate = sta->txrate;
	197	}
1946b74c	198	rate_control_pid_normalize(pinfo, mode->num_rates);
90d501d6	199
ad018375	200	/* Compute the proportional, integral and derivative errors. */
426706c0	201	err_prop = (pinfo->target << RC_PID_ARITH_SHIFT) - pf;
ad018375	202
1946b74c	203	err_avg = spinfo->err_avg_sc >> pinfo->smoothing_shift;
ad018375	204	spinfo->err_avg_sc = spinfo->err_avg_sc - err_avg + err_prop;
1946b74c	205	err_int = spinfo->err_avg_sc >> pinfo->smoothing_shift;
ad018375	206
1946b74c MN	207	err_der = (pf - spinfo->last_pf) *
1946b74c MN	208	(1 + pinfo->sharpen_factor * spinfo->sharp_cnt);
ad018375	209	spinfo->last_pf = pf;
1dc4d1e6 SB	210	if (spinfo->sharp_cnt)
1dc4d1e6 SB	211	spinfo->sharp_cnt--;
ad018375	212
12446c67 MN	213	#ifdef CONFIG_MAC80211_DEBUGFS
	214	rate_control_pid_event_pf_sample(&spinfo->events, pf, err_prop, err_int,
	215	err_der);
	216	#endif
	217
ad018375 MN	218	/* Compute the controller output. */
	219	adj = (err_prop * pinfo->coeff_p + err_int * pinfo->coeff_i
	220	+ err_der * pinfo->coeff_d);
90d501d6	221	adj = RC_PID_DO_ARITH_RIGHT_SHIFT(adj, 2 * RC_PID_ARITH_SHIFT);
ad018375 MN	222
	223	/* Change rate. */
	224	if (adj)
90d501d6	225	rate_control_pid_adjust_rate(local, sta, adj, rinfo);
ad018375 MN	226	}
	227
	228	static void rate_control_pid_tx_status(void priv, struct net_device dev,
	229	struct sk_buff *skb,
	230	struct ieee80211_tx_status *status)
	231	{
	232	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
	233	struct ieee80211_hdr hdr = (struct ieee80211_hdr ) skb->data;
df26e7ea	234	struct ieee80211_sub_if_data *sdata;
ad018375 MN	235	struct rc_pid_info *pinfo = priv;
	236	struct sta_info *sta;
	237	struct rc_pid_sta_info *spinfo;
1946b74c	238	unsigned long period;
ad018375 MN	239
	240	sta = sta_info_get(local, hdr->addr1);
	241
	242	if (!sta)
	243	return;
	244
df26e7ea AL	245	/* Don't update the state if we're not controlling the rate. */
	246	sdata = IEEE80211_DEV_TO_SUB_IF(sta->dev);
	247	if (sdata->bss && sdata->bss->force_unicast_rateidx > -1) {
	248	sta->txrate = sdata->bss->max_ratectrl_rateidx;
	249	return;
	250	}
	251
ad018375 MN	252	/* Ignore all frames that were sent with a different rate than the rate
	253	* we currently advise mac80211 to use. */
	254	if (status->control.rate != &local->oper_hw_mode->rates[sta->txrate])
13e05aa6	255	goto ignore;
ad018375 MN	256
	257	spinfo = sta->rate_ctrl_priv;
	258	spinfo->tx_num_xmit++;
	259
12446c67 MN	260	#ifdef CONFIG_MAC80211_DEBUGFS
	261	rate_control_pid_event_tx_status(&spinfo->events, status);
	262	#endif
	263
ad018375 MN	264	/* We count frames that totally failed to be transmitted as two bad
	265	* frames, those that made it out but had some retries as one good and
	266	* one bad frame. */
	267	if (status->excessive_retries) {
	268	spinfo->tx_num_failed += 2;
	269	spinfo->tx_num_xmit++;
	270	} else if (status->retry_count) {
	271	spinfo->tx_num_failed++;
	272	spinfo->tx_num_xmit++;
	273	}
	274
	275	if (status->excessive_retries) {
	276	sta->tx_retry_failed++;
	277	sta->tx_num_consecutive_failures++;
	278	sta->tx_num_mpdu_fail++;
	279	} else {
	280	sta->last_ack_rssi[0] = sta->last_ack_rssi[1];
	281	sta->last_ack_rssi[1] = sta->last_ack_rssi[2];
	282	sta->last_ack_rssi[2] = status->ack_signal;
	283	sta->tx_num_consecutive_failures = 0;
	284	sta->tx_num_mpdu_ok++;
	285	}
	286	sta->tx_retry_count += status->retry_count;
	287	sta->tx_num_mpdu_fail += status->retry_count;
	288
	289	/* Update PID controller state. */
1946b74c MN	290	period = (HZ * pinfo->sampling_period + 500) / 1000;
	291	if (!period)
	292	period = 1;
	293	if (time_after(jiffies, spinfo->last_sample + period))
ad018375 MN	294	rate_control_pid_sample(pinfo, local, sta);
ad018375 MN	295
13e05aa6	296	ignore:
ad018375 MN	297	sta_info_put(sta);
	298	}
	299
	300	static void rate_control_pid_get_rate(void priv, struct net_device dev,
	301	struct ieee80211_hw_mode *mode,
	302	struct sk_buff *skb,
	303	struct rate_selection *sel)
	304	{
	305	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
	306	struct ieee80211_hdr hdr = (struct ieee80211_hdr ) skb->data;
2bc454b0	307	struct ieee80211_sub_if_data *sdata;
ad018375 MN	308	struct sta_info *sta;
ad018375 MN	309	int rateidx;
2bc454b0	310	u16 fc;
ad018375 MN	311
	312	sta = sta_info_get(local, hdr->addr1);
	313
2bc454b0 MW	314	/* Send management frames and broadcast/multicast data using lowest
	315	* rate. */
	316	fc = le16_to_cpu(hdr->frame_control);
	317	if ((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA \|\|
	318	is_multicast_ether_addr(hdr->addr1) \|\| !sta) {
	319	sel->rate = rate_lowest(local, mode, sta);
	320	if (sta)
	321	sta_info_put(sta);
ad018375 MN	322	return;
	323	}
	324
2bc454b0 MW	325	/* If a forced rate is in effect, select it. */
	326	sdata = IEEE80211_DEV_TO_SUB_IF(dev);
	327	if (sdata->bss && sdata->bss->force_unicast_rateidx > -1)
	328	sta->txrate = sdata->bss->force_unicast_rateidx;
	329
ad018375 MN	330	rateidx = sta->txrate;
	331
	332	if (rateidx >= mode->num_rates)
	333	rateidx = mode->num_rates - 1;
	334
2bc454b0 MW	335	sta->last_txrate = rateidx;
2bc454b0 MW	336
ad018375 MN	337	sta_info_put(sta);
	338
	339	sel->rate = &mode->rates[rateidx];
12446c67 MN	340
	341	#ifdef CONFIG_MAC80211_DEBUGFS
	342	rate_control_pid_event_tx_rate(
	343	&((struct rc_pid_sta_info *) sta->rate_ctrl_priv)->events,
	344	rateidx, mode->rates[rateidx].rate);
	345	#endif
ad018375 MN	346	}
	347
	348	static void rate_control_pid_rate_init(void priv, void priv_sta,
	349	struct ieee80211_local *local,
	350	struct sta_info *sta)
	351	{
	352	/* TODO: This routine should consider using RSSI from previous packets
	353	* as we need to have IEEE 802.1X auth succeed immediately after assoc..
	354	* Until that method is implemented, we will use the lowest supported
	355	* rate as a workaround. */
	356	sta->txrate = rate_lowest_index(local, local->oper_hw_mode, sta);
	357	}
	358
	359	static void rate_control_pid_alloc(struct ieee80211_local local)
	360	{
	361	struct rc_pid_info *pinfo;
90d501d6 SB	362	struct rc_pid_rateinfo *rinfo;
	363	struct ieee80211_hw_mode *mode;
	364	int i, j, tmp;
	365	bool s;
1946b74c MN	366	#ifdef CONFIG_MAC80211_DEBUGFS
	367	struct rc_pid_debugfs_entries *de;
	368	#endif
ad018375 MN	369
ad018375 MN	370	pinfo = kmalloc(sizeof(*pinfo), GFP_ATOMIC);
90d501d6 SB	371	if (!pinfo)
	372	return NULL;
	373
	374	/* We can safely assume that oper_hw_mode won't change unless we get
	375	* reinitialized. */
	376	mode = local->oper_hw_mode;
	377	rinfo = kmalloc(sizeof(rinfo) mode->num_rates, GFP_ATOMIC);
	378	if (!rinfo) {
	379	kfree(pinfo);
	380	return NULL;
	381	}
	382
	383	/* Sort the rates. This is optimized for the most common case (i.e.
	384	* almost-sorted CCK+OFDM rates). Kind of bubble-sort with reversed
	385	* mapping too. */
	386	for (i = 0; i < mode->num_rates; i++) {
	387	rinfo[i].index = i;
	388	rinfo[i].rev_index = i;
1946b74c	389	if (pinfo->fast_start)
90d501d6 SB	390	rinfo[i].diff = 0;
90d501d6 SB	391	else
1946b74c	392	rinfo[i].diff = i * pinfo->norm_offset;
90d501d6 SB	393	}
	394	for (i = 1; i < mode->num_rates; i++) {
	395	s = 0;
	396	for (j = 0; j < mode->num_rates - i; j++)
	397	if (unlikely(mode->rates[rinfo[j].index].rate >
	398	mode->rates[rinfo[j + 1].index].rate)) {
	399	tmp = rinfo[j].index;
	400	rinfo[j].index = rinfo[j + 1].index;
	401	rinfo[j + 1].index = tmp;
	402	rinfo[rinfo[j].index].rev_index = j;
	403	rinfo[rinfo[j + 1].index].rev_index = j + 1;
	404	s = 1;
	405	}
	406	if (!s)
	407	break;
	408	}
ad018375 MN	409
ad018375 MN	410	pinfo->target = RC_PID_TARGET_PF;
1946b74c	411	pinfo->sampling_period = RC_PID_INTERVAL;
ad018375 MN	412	pinfo->coeff_p = RC_PID_COEFF_P;
	413	pinfo->coeff_i = RC_PID_COEFF_I;
	414	pinfo->coeff_d = RC_PID_COEFF_D;
1946b74c MN	415	pinfo->smoothing_shift = RC_PID_SMOOTHING_SHIFT;
	416	pinfo->sharpen_factor = RC_PID_SHARPENING_FACTOR;
	417	pinfo->sharpen_duration = RC_PID_SHARPENING_DURATION;
	418	pinfo->norm_offset = RC_PID_NORM_OFFSET;
	419	pinfo->fast_start = RC_PID_FAST_START;
90d501d6 SB	420	pinfo->rinfo = rinfo;
90d501d6 SB	421	pinfo->oldrate = 0;
ad018375	422
1946b74c MN	423	#ifdef CONFIG_MAC80211_DEBUGFS
	424	de = &pinfo->dentries;
	425	de->dir = debugfs_create_dir("rc80211_pid",
	426	local->hw.wiphy->debugfsdir);
	427	de->target = debugfs_create_u32("target_pf", S_IRUSR \| S_IWUSR,
	428	de->dir, &pinfo->target);
	429	de->sampling_period = debugfs_create_u32("sampling_period",
	430	S_IRUSR \| S_IWUSR, de->dir,
	431	&pinfo->sampling_period);
	432	de->coeff_p = debugfs_create_u32("coeff_p", S_IRUSR \| S_IWUSR,
	433	de->dir, &pinfo->coeff_p);
	434	de->coeff_i = debugfs_create_u32("coeff_i", S_IRUSR \| S_IWUSR,
	435	de->dir, &pinfo->coeff_i);
	436	de->coeff_d = debugfs_create_u32("coeff_d", S_IRUSR \| S_IWUSR,
	437	de->dir, &pinfo->coeff_d);
	438	de->smoothing_shift = debugfs_create_u32("smoothing_shift",
	439	S_IRUSR \| S_IWUSR, de->dir,
	440	&pinfo->smoothing_shift);
	441	de->sharpen_factor = debugfs_create_u32("sharpen_factor",
	442	S_IRUSR \| S_IWUSR, de->dir,
	443	&pinfo->sharpen_factor);
	444	de->sharpen_duration = debugfs_create_u32("sharpen_duration",
	445	S_IRUSR \| S_IWUSR, de->dir,
	446	&pinfo->sharpen_duration);
	447	de->norm_offset = debugfs_create_u32("norm_offset",
	448	S_IRUSR \| S_IWUSR, de->dir,
	449	&pinfo->norm_offset);
	450	de->fast_start = debugfs_create_bool("fast_start",
	451	S_IRUSR \| S_IWUSR, de->dir,
	452	&pinfo->fast_start);
	453	#endif
	454
ad018375 MN	455	return pinfo;
	456	}
	457
	458	static void rate_control_pid_free(void *priv)
	459	{
	460	struct rc_pid_info *pinfo = priv;
1946b74c MN	461	#ifdef CONFIG_MAC80211_DEBUGFS
	462	struct rc_pid_debugfs_entries *de = &pinfo->dentries;
	463
	464	debugfs_remove(de->fast_start);
	465	debugfs_remove(de->norm_offset);
	466	debugfs_remove(de->sharpen_duration);
	467	debugfs_remove(de->sharpen_factor);
	468	debugfs_remove(de->smoothing_shift);
	469	debugfs_remove(de->coeff_d);
	470	debugfs_remove(de->coeff_i);
	471	debugfs_remove(de->coeff_p);
	472	debugfs_remove(de->sampling_period);
	473	debugfs_remove(de->target);
	474	debugfs_remove(de->dir);
	475	#endif
	476
90d501d6	477	kfree(pinfo->rinfo);
ad018375 MN	478	kfree(pinfo);
	479	}
	480
	481	static void rate_control_pid_clear(void *priv)
	482	{
	483	}
	484
	485	static void rate_control_pid_alloc_sta(void priv, gfp_t gfp)
	486	{
	487	struct rc_pid_sta_info *spinfo;
	488
	489	spinfo = kzalloc(sizeof(*spinfo), gfp);
12446c67 MN	490	if (spinfo == NULL)
	491	return NULL;
	492
c09c7237 SB	493	spinfo->last_sample = jiffies;
c09c7237 SB	494
12446c67 MN	495	#ifdef CONFIG_MAC80211_DEBUGFS
	496	spin_lock_init(&spinfo->events.lock);
	497	init_waitqueue_head(&spinfo->events.waitqueue);
	498	#endif
ad018375 MN	499
	500	return spinfo;
	501	}
	502
	503	static void rate_control_pid_free_sta(void priv, void priv_sta)
	504	{
	505	struct rc_pid_sta_info *spinfo = priv_sta;
	506	kfree(spinfo);
	507	}
	508
4b475898	509	static struct rate_control_ops mac80211_rcpid = {
ad018375 MN	510	.name = "pid",
	511	.tx_status = rate_control_pid_tx_status,
	512	.get_rate = rate_control_pid_get_rate,
	513	.rate_init = rate_control_pid_rate_init,
	514	.clear = rate_control_pid_clear,
	515	.alloc = rate_control_pid_alloc,
	516	.free = rate_control_pid_free,
	517	.alloc_sta = rate_control_pid_alloc_sta,
	518	.free_sta = rate_control_pid_free_sta,
12446c67 MN	519	#ifdef CONFIG_MAC80211_DEBUGFS
	520	.add_sta_debugfs = rate_control_pid_add_sta_debugfs,
	521	.remove_sta_debugfs = rate_control_pid_remove_sta_debugfs,
	522	#endif
ad018375	523	};
4b475898 JB	524
	525	MODULE_DESCRIPTION("PID controller based rate control algorithm");
	526	MODULE_AUTHOR("Stefano Brivio");
	527	MODULE_AUTHOR("Mattias Nissler");
	528	MODULE_LICENSE("GPL");
	529
	530	int __init rc80211_pid_init(void)
	531	{
	532	return ieee80211_rate_control_register(&mac80211_rcpid);
	533	}
	534
f0b9205c	535	void rc80211_pid_exit(void)
4b475898 JB	536	{
	537	ieee80211_rate_control_unregister(&mac80211_rcpid);
	538	}
	539
	540	#ifdef CONFIG_MAC80211_RC_PID_MODULE
	541	module_init(rc80211_pid_init);
	542	module_exit(rc80211_pid_exit);
	543	#endif