//
// Embedded 802.11 wireless network interface
// MAC layer rate control functions
// Copyright 2005 Digi International
//
#include "mac.h"

// Length (in usecs) of a MAC frame of bytes at rate (in 500kbps units)
// not including SIFS and PLCP preamble/header
#define	LENGTH(bytes, rate)		((16*(bytes)+(rate)-1)/(rate))
	
// Length (in usecs) of SIFS and PLCP preamble/header.
#define	PRE_LEN(rate)			(USE_SHORTPRE(rate) ? 106 : 202)

// Duration (in usecs) of an OFDM frame at rate (in 500kbps units)
// including SIFS and PLCP preamble/header
#define	OFDM_DUR(bytes, rate)	(36 + 4*((4*(bytes)+(rate)+10)/(rate)))
			
// Information on each supported rate
typedef struct {
	uint8	bps;		// Bit rate in 500kbps units
	uint8	ofdmCode;	// OFDM rate code, 0 if not OFDM
	uint16	ackLen;		// Duration of ACK or CTS in usecs
} RateInfo;

// Must be ordered by modulation type, then by increasing bit rate
static const RateInfo rateInfo[] = {
	{ 2,   0,   LENGTH   (ACK_SIZE, 2)   },
	{ 4,   0,   LENGTH   (ACK_SIZE, 4)   },
	{ 11,  0,   LENGTH   (ACK_SIZE, 11)  },
	{ 22,  0,   LENGTH   (ACK_SIZE, 22)  },
	{ 12,  0xb, OFDM_DUR (ACK_SIZE, 12)  },
	{ 18,  0xf, OFDM_DUR (ACK_SIZE, 18)  },
	{ 24,  0xa, OFDM_DUR (ACK_SIZE, 24)  },
	{ 36,  0xe, OFDM_DUR (ACK_SIZE, 36)  },
	{ 48,  0x9, OFDM_DUR (ACK_SIZE, 48)  },
	{ 72,  0xd, OFDM_DUR (ACK_SIZE, 72)  },
	{ 96,  0x8, OFDM_DUR (ACK_SIZE, 96)  },
	{ 108, 0xc, OFDM_DUR (ACK_SIZE, 108) },
};

#define	RATE_NUM	((int)(sizeof rateInfo/sizeof rateInfo[0]))

// Supported rates info elements
static const uint8 ratesA[]  = { ELEM_SUPRATES, 8, RATE_BASIC+12, 18, RATE_BASIC+24, 36, RATE_BASIC+48, 72, 96, 108 };
static const uint8 ratesB[]  = { ELEM_SUPRATES, 4, RATE_BASIC+2, RATE_BASIC+4, 11, 22 };
static const uint8 ratesG[]  = { ELEM_SUPRATES, 8, RATE_BASIC+2, RATE_BASIC+4, 11, 22, 12, 18, 24, 36 };
static const uint8 ratesGx[] = { ELEM_EXTSUPRATES, 4, 48, 72, 96, 108 };

// Following rate values are indices into rateInfo table
int rateTx;		// Directed frame rate
int rateBC;		// Broadcast or protection frame rate
int rateMin;	// Minimum rate

// Rates allowed in BSS
static int rateCount;
static uint8 rates[RATE_NUM];

// Map each rate to lower or equal BSS basic rate
uint8 ratesBasic[RATE_NUM];

// Map each rate to rate of ACK or CTS
static uint8 ratesACK[RATE_NUM];

// Adaptive rate control info
int rateTxCount;				// Transmitted frames
int rateErrorCount;				// Transmitted frames with errors

static int rateIdx;				// Current rate, index into rates[]
static int success;				// Successful intervals
static int successThreshold;	// Successful intervals needed to increase rate
static BOOL recovery;			// TRUE if interval follows rate increase

// Min/max successful intervals to increase rate
#define	THRESH_MIN		1
#define	THRESH_MAX		10

int wlnMacGetTxCurrentTRateX2(void)
{
    return rateInfo[rateTx].bps;
}

int wlnMacGetTxCurrentOfdmCode(void)
{
    return rateInfo[rateTx].ofdmCode;
}


//
// Get the duration (in usecs) of an ACK or CTS frame at rate
//
int GetAckDuration (int rate)
{
	int dur;

	// Get corresponding ACK/CTS rate
	rate = ratesACK[rate];
	dur = rateInfo[rate].ackLen;

	// Add PSK/CCK preamble time
	if (rateInfo[rate].ofdmCode == 0)
		dur += PRE_LEN(rate);

	return dur;
}

//
// Get the duration (in usecs) of a data frame and following ACK.
// SetPLCP() must have previously been called on the frame.
//
int GetDuration (MacBuffer *buf)
{
	int rate = buf->tx_rate;
	int dur;
	
	// Frame duration
	if (rateInfo[rate].ofdmCode)
		dur = OFDM_DUR(buf->length, rateInfo[rate].bps);
	else
		// PLCP length field previously set
		dur = PRE_LEN(rate) + NTOH16(buf->plcpHdr.pskcck.length);
		
	// Add ACK duration
	return dur + GetAckDuration (rate);
}

//
// Set the frame and PLCP headers in a frame.
// Buffer has its length field set.
// @param buf Frame to modify
// @param rate Transmit rate index
//
int lowRate =0, highRate=0;
extern int tryToTakeAverageVoltage;
void SetPLCP (MacBuffer *buf, int rate)
{
	int length = buf->length;
#if AIROHA_PWR_CALIBRATION	
	epStatus *pSta = rootEndPoint[0];
	
	if(tryToTakeAverageVoltage)
	{
		if (!CHAN_5G (pSta->macStatus.channel))
		{
		    if (rateInfo[rate].ofdmCode)
		    {
    			buf->tx_rate = 4;   // OFDM, lowest is 6 mbps
    			rate = 4;
    		}
    		else
    		{
    			buf->tx_rate = 0;   //CCK, lowest is 1 mbps
    			rate = 0;
    		}
    		
		}
		else
		{
			buf->tx_rate = 4;  // OFDM, lowest is 6 mbps
			rate = 4;
			
		}
		lowRate++;
	}
	else
	{
		buf->tx_rate = rate;
		highRate++;
	}
	
#else
	buf->tx_rate = rate;
#endif	
	
	buf->frameHdr.hdr32 = 0;
	
	// Length in words, excluding FCS, rounded up
	buf->frameHdr.tx.length = 
		(sizeof(FrameHeader) + sizeof(PlcpHeader) + length - FCS_SIZE + 3) / 4;
	
#if 0
		buf->frameHdr.tx.modType = MOD_OFDM;
		buf->plcpHdr.plcp32 = 0;
		buf->plcpHdr.ofdm.rate = rateInfo[9].ofdmCode;
		buf->plcpHdr.ofdm.length = length;
		// Swap bytes of OFDM PLCP header
		buf->plcpHdr.plcp32 = SWAP32 (buf->plcpHdr.plcp32);
#else
	// OFDM rates
	if (rateInfo[rate].ofdmCode)
	{
		buf->frameHdr.tx.modType = MOD_OFDM;
		buf->plcpHdr.plcp32 = 0;
		buf->plcpHdr.ofdm.rate = rateInfo[rate].ofdmCode;
		buf->plcpHdr.ofdm.length = length;

		// Swap bytes of OFDM PLCP header
		buf->plcpHdr.plcp32 = SWAP32 (buf->plcpHdr.plcp32);
	}
	
	// PSK/CCK rates.
	else
	{
		buf->frameHdr.tx.modType = MOD_PSKCCK;
		buf->plcpHdr.pskcck.signal = 5*rateInfo[rate].bps;
		buf->plcpHdr.pskcck.service = SERVICE_LOCKED;
		
		// Convert length from bytes to microseconds.
		// Use a switch to avoid a long divide operation.
		switch (rateInfo[rate].bps)
		{
			case 2:
				length *= 8;
				break;
			case 4:
				length *= 4;
				break;
			case 11:
				length = (16*length+10)/11;
				break;
			case 22:
				length = (8*length+10)/11;

				// Set length extension bit if needed
				if ((11*length)/8 > buf->length)
					buf->plcpHdr.pskcck.service |= SERVICE_LENEXT;
				break;
		}
			
		buf->plcpHdr.pskcck.length = HTON16 (length);
	}
#endif	
	// Swap bytes of frame header
	buf->frameHdr.hdr32 = SWAP32 (buf->frameHdr.hdr32);
}

//
// Check if we can handle the supported rates of a BSS.
// @param bssRates Rates supported by the BSS
// @param len Number of rates
// @param info Scan info to update with rates
// @return TRUE if supported, FALSE if a basic rate not supported
//
BOOL CheckRates (uint8 *bssRates, int len, wln_scan_bss *info)
{
	int i, j;

	// For each rate in BSS
	for (i = 0; i < len; i++)
	{
		// Check if we support the rate
		for (j = 0; j < RATE_NUM; j++)
			if ((bssRates[i] & ~RATE_BASIC) == rateInfo[j].bps)
				break;

		if (j < RATE_NUM)
		{
			// Rate is supported
			info->rates |= 1 << j;
			if (bssRates[i] & RATE_BASIC)
				info->rates_basic |= 1 << j;
				
			// Find max rate
			if (info->tx_rate < 5*rateInfo[j].bps)
				info->tx_rate = 5*rateInfo[j].bps;
		}
		
		// Basic rate not supported, return failure
		else if (bssRates[i] & RATE_BASIC)
			return FALSE;
	}
	
	return TRUE;
}

//
// Store supported rates elements into a buffer
// @param bp Pointer into buffer
// @param elem Element to store: ELEM_SUPRATES, ELEM_EXTSUPRATES, or 0 for both
// @param channel Channel number
// @return Updated buffer pointer
//
uint8 *SetRates (uint8 *bp, int elem, int channel)
{
	const uint8 *sr;
	
	// Select rate set
	if (CHAN_5G (channel))
		sr = ratesA;
	else if (macParams.options & WLN_OPT_BONLY)
		sr = ratesB;
	else
		sr = ratesG;

	// Store up to 8 supported rates
	if (elem != ELEM_EXTSUPRATES)
	{
		memcpy (bp, sr, sr[1]+2);
		bp += bp[1] + 2;
	}
	
	// Store remaining extended supported rates
	if (elem != ELEM_SUPRATES && sr == ratesG)
	{
		memcpy (bp, ratesGx, ratesGx[1]+2);
		bp += bp[1] + 2;
	}

	return bp;
}

//
// Initialize rate tables
// @param sp BSS scan information
// @param init TRUE to init adaptive rate control
// @return TRUE if supported, FALSE if error
//
BOOL InitRates (wln_scan_bss *sp, BOOL init)
{
	uint16 oper  = sp->rates;		// Operation rate mask
	uint16 basic = sp->rates_basic;	// Basic rate mask
	uint16 ack;						// ACK/CTS rate mask
	uint16 rateACK;					// Lower or equal ACK/CTS rate
	int i;
	epStatus *pSta = rootEndPoint[0];

	// Limit rate set to 802.11a or b rates
	if (CHAN_5G (pSta->macStatus.channel))
		oper &= RATE_MASK_A;
	else if (macParams.options & WLN_OPT_BONLY)
		oper &= RATE_MASK_B;

#if ns7520
	// Special case for channel 14:
	// hardware is only certified for 1 and 2 mbps
	if (pSta->macStatus.channel == 14)
		oper &= RATE_MASK_PSK;
#endif

	basic &= oper;

	// Use basic rates for ACK/CTS frames.
	// If no basic rate for a modulation type, use mandatory rates.
	// Ensure minimum rates for each modulation are included.
	ack = basic;
	if ((ack & RATE_MASK_A) == 0)
		ack |= RATE_MASK_BASIC & RATE_MASK_A;
	if ((ack & RATE_MASK_B) == 0)
		ack |= RATE_MASK_BASIC & RATE_MASK_B;
	ack |= (1 << RATE_MIN_A) | (1 << RATE_MIN_B);

	// Use basic rates for broadcast and protection frames.
	// Limit to 802.11b rates if protection enabled.
	if (sp->erp_info & ERP_USEPROTECT)
		basic &= RATE_MASK_B;

	// If OFDM rates in use, don't use CCK rates (5.5 and 11 mbps)
	// for directed frames because these overlap and have longer preambles
	if (oper & RATE_MASK_OFDM)
		oper &= ~RATE_MASK_CCK;

	// Find minimum basic rate
	if (CHAN_5G (pSta->macStatus.channel))
		rateBC = RATE_MIN_A;
	else
		rateBC = RATE_MIN_B;

	for (i = 0; i < RATE_NUM; i++)
		if (basic & (1 << i))
		{
			rateBC = i;
			break;
		}

	// Build rate tables
	rateCount = 0;
	rateACK = 0;
	for (i = 0; i < RATE_NUM; i++)
	{
		uint16 mask = 1 << i;

		// Rate allowed by parameters
		if (5*rateInfo[i].bps <= macParams.tx_rate)
		{
			// Rate allowed by BSS
			if (oper & mask)
				rates[rateCount++] = i;

			// Find lower or equal basic rate
			if (basic & mask)
				rateBC = i;
			ratesBasic[i] = rateBC;
		}

		// Find lower or equal ACK/CTS rate
		if (ack & mask)
			rateACK = i;
		ratesACK[i] = rateACK;
	}

	
	if (rateCount == 0)
		return FALSE;
		
	// Set directed transmit rate
	if (macParams.options & WLN_OPT_FIXEDRATE)
		rateIdx = rateCount - 1;

	else if (init || rateIdx >= rateCount)
	{
		rateIdx = rateCount/2;
	
		// Init adaptive rate info
		success = 0;
		successThreshold = THRESH_MIN;
		recovery = FALSE;
		rateTxCount = rateErrorCount = 0;
	}

	rateMin = rates[0];
	rateTx = rates[rateIdx];
	pSta->macStatus.tx_rate = 5*rateInfo[rateTx].bps;
	
	// Init CTS-to-self frame if protection enabled
	if (sp->erp_info & ERP_USEPROTECT)
		SetPLCP (&ctsFrame, rateBC);
	
	return TRUE;
}

//
// Reduce the transmit rate of a frame by steps
//
void ReduceRate (MacBuffer *buf, int steps)
{
	int i, dur;
	
	// No change if using fixed rate
	if (macParams.options & WLN_OPT_FIXEDRATE)
		return;
		
	// Limit rate index to valid range.
	// Use minimum for last retry.
	i = rateIdx - steps;
	if (i >= rateCount)
		i = rateCount - 1;
	if (i < 0 || steps >= SHORT_RETRY_LIMIT-1)
		i = 0;
		
	// No change needed
	if (rates[i] >= buf->tx_rate)
		return;

	// Subtract ACK at old rate from duration
	dur = NTOH16 (buf->macHdr.duration) - GetAckDuration (buf->tx_rate);

	// Set new rate
	SetPLCP (buf, rates[i]);

	// Add ACK at new rate to duration
	dur += GetAckDuration (buf->tx_rate);
	buf->macHdr.duration = HTON16 (dur);
}

//
// This is an implementation of the Adaptive Multi Rate Retry (AMRR)
// algorithm described at http://www.inria.fr/rrrt/rr-5208.html
//

// Have enough frames to adjust rate
#define is_enough()		(rateTxCount >= 10)

// Error rate is low enough to increase rate
#define is_success()	(8*rateErrorCount < rateTxCount)

// Error rate is high enough to decrease rate
#define is_failure()	(4*rateErrorCount > rateTxCount)
//
// Update the current transmit rate.
// Called periodically from MacTimer().
//
void UpdateRate (epStatus *pSta)
{
	// epStatus *pSta = rootEndPoint[0];
	// Using fixed rate, or not enough frames
	if ((macParams.options & WLN_OPT_FIXEDRATE) || !is_enough())
		return;

/*
 * Set this flag to use have the code try more aggressively to recover
 * from transmit errors.  Clear this flag to more closely follow rr-5208,
 * which will result in lower performance.
 */
#define WANT_AGGRESSIVE_ERROR_RECOVERY	(1)
#if WANT_AGGRESSIVE_ERROR_RECOVERY	
    rateErrorCount /= 2;
#endif
	// Try to increase rate
	if (is_success()) 
	{
		success++;
		if (success >= successThreshold && rateIdx < rateCount-1) 
		{
#define WANT_RATE_CHANGE_OUTPUT   (0)
#if WANT_RATE_CHANGE_OUTPUT
            printf("Increasing rate from %d to %d.\n", rateInfo[rateTx].bps/2, rateInfo[rateTx+1].bps/2);
#endif
			recovery = TRUE;
			success = 0;
			rateIdx++;
		}
		else
			recovery = FALSE;
	}
	
	// Decrease rate
	else if (is_failure()) 
	{
		if (rateIdx > 0) 
		{
			if (recovery) 
			{
				// Errors resulted from a rate increase.
				// Double successful intervals needed to increase next time.
				successThreshold *= 2;
				if (successThreshold > THRESH_MAX)
					successThreshold = THRESH_MAX;
			} 
			else
				successThreshold = THRESH_MIN;
#if WANT_RATE_CHANGE_OUTPUT
                printf("*** Reducing rate from %d to %d.\n", rateInfo[rateTx].bps/2, rateInfo[rateTx-1].bps/2);
#endif
			rateIdx--;
		}
		
		success = 0;
		recovery = FALSE;
	}
	
	// Set new transmit rate
	rateTx = rates[rateIdx];
	pSta->macStatus.tx_rate = 5*rateInfo[rateTx].bps;
	//DPRINTF ("rate=%d tx=%d err=%d\n", pSta->macStatus.tx_rate, rateTxCount, rateErrorCount);
	
	// Reset counters
	rateTxCount = rateErrorCount = 0;
}
