From: Pekka Enberg <penberg@cs.helsinki.fi>
Date: Thu, 30 Oct 2008 17:04:50 +0000 (+0200)
Subject: Staging: w35und: make functions local to mds.c static
X-Git-Url: https://git.stricted.de/?a=commitdiff_plain;h=6261ab3a63e17009424a620ed2b6e30966ac72f4;p=GitHub%2Fmoto-9609%2Fandroid_kernel_motorola_exynos9610.git

Staging: w35und: make functions local to mds.c static

The functions have no external references so make them static after shuffling
the code around a bit to avoid forward declarations.

Acked-by: Pavel Machek <pavel@suse.cz>
Signed-off-by: Pekka Enberg <penberg@cs.helsinki.fi>
---

diff --git a/drivers/staging/winbond/mds.c b/drivers/staging/winbond/mds.c
index df59b04da749..1bfdbc4827ff 100644
--- a/drivers/staging/winbond/mds.c
+++ b/drivers/staging/winbond/mds.c
@@ -7,19 +7,6 @@
 #include "wbhal_f.h"
 #include "wblinux_f.h"
 
-void
-Mds_reset_descriptor(struct wbsoft_priv * adapter)
-{
-	PMDS pMds = &adapter->Mds;
-
-	pMds->TxPause = 0;
-	atomic_set(&pMds->TxThreadCount, 0);
-	pMds->TxFillIndex = 0;
-	pMds->TxDesIndex = 0;
-	pMds->ScanTxPause = 0;
-	memset(pMds->TxOwner, 0, ((MAX_USB_TX_BUFFER_NUMBER + 3) & ~0x03));
-}
-
 unsigned char
 Mds_initial(struct wbsoft_priv * adapter)
 {
@@ -40,203 +27,303 @@ Mds_Destroy(struct wbsoft_priv * adapter)
 	vRxTimerStop(adapter);
 }
 
-void
-Mds_Tx(struct wbsoft_priv * adapter)
+static void Mds_DurationSet(struct wbsoft_priv *adapter,  PDESCRIPTOR pDes,  u8 *buffer)
 {
-	phw_data_t	pHwData = &adapter->sHwData;
-	PMDS		pMds = &adapter->Mds;
-	DESCRIPTOR	TxDes;
-	PDESCRIPTOR	pTxDes = &TxDes;
-	u8		*XmitBufAddress;
-	u16		XmitBufSize, PacketSize, stmp, CurrentSize, FragmentThreshold;
-	u8		FillIndex, TxDesIndex, FragmentCount, FillCount;
-	unsigned char	BufferFilled = false, MICAdd = 0;
-
+	PT00_DESCRIPTOR	pT00;
+	PT01_DESCRIPTOR	pT01;
+	u16	Duration, NextBodyLen, OffsetSize;
+	u8	Rate, i;
+	unsigned char	CTS_on = false, RTS_on = false;
+	PT00_DESCRIPTOR pNextT00;
+	u16 BodyLen = 0;
+	unsigned char boGroupAddr = false;
 
-	if (pMds->TxPause)
-		return;
-	if (!hal_driver_init_OK(pHwData))
-		return;
+	OffsetSize = pDes->FragmentThreshold + 32 + 3;
+	OffsetSize &= ~0x03;
+	Rate = pDes->TxRate >> 1;
+	if (!Rate)
+		Rate = 1;
 
-	//Only one thread can be run here
-	if (!atomic_inc_return(&pMds->TxThreadCount) == 1)
-		goto cleanup;
+	pT00 = (PT00_DESCRIPTOR)buffer;
+	pT01 = (PT01_DESCRIPTOR)(buffer+4);
+	pNextT00 = (PT00_DESCRIPTOR)(buffer+OffsetSize);
 
-	// Start to fill the data
-	do {
-		FillIndex = pMds->TxFillIndex;
-		if (pMds->TxOwner[FillIndex]) { // Is owned by software 0:Yes 1:No
-#ifdef _PE_TX_DUMP_
-			WBDEBUG(("[Mds_Tx] Tx Owner is H/W.\n"));
-#endif
-			break;
-		}
+	if( buffer[ DOT_11_DA_OFFSET+8 ] & 0x1 ) // +8 for USB hdr
+		boGroupAddr = true;
 
-		XmitBufAddress = pMds->pTxBuffer + (MAX_USB_TX_BUFFER * FillIndex); //Get buffer
-		XmitBufSize = 0;
-		FillCount = 0;
-		do {
-			PacketSize = adapter->sMlmeFrame.len;
-			if (!PacketSize)
-				break;
+	//========================================
+	// Set RTS/CTS mechanism
+	//========================================
+	if (!boGroupAddr)
+	{
+		//NOTE : If the protection mode is enabled and the MSDU will be fragmented,
+		//		 the tx rates of MPDUs will all be DSSS rates. So it will not use
+		//		 CTS-to-self in this case. CTS-To-self will only be used when without
+		//		 fragmentation. -- 20050112
+		BodyLen = (u16)pT00->T00_frame_length;	//include 802.11 header
+		BodyLen += 4;	//CRC
 
-			//For Check the buffer resource
-			FragmentThreshold = CURRENT_FRAGMENT_THRESHOLD;
-			//931130.5.b
-			FragmentCount = PacketSize/FragmentThreshold + 1;
-			stmp = PacketSize + FragmentCount*32 + 8;//931130.5.c 8:MIC
-			if ((XmitBufSize + stmp) >= MAX_USB_TX_BUFFER) {
-				printk("[Mds_Tx] Excess max tx buffer.\n");
-				break; // buffer is not enough
+		if( BodyLen >= CURRENT_RTS_THRESHOLD )
+			RTS_on = true; // Using RTS
+		else
+		{
+			if( pT01->T01_modulation_type ) // Is using OFDM
+			{
+				if( CURRENT_PROTECT_MECHANISM ) // Is using protect
+					CTS_on = true; // Using CTS
 			}
+		}
+	}
 
+	if( RTS_on || CTS_on )
+	{
+		if( pT01->T01_modulation_type) // Is using OFDM
+		{
+			//CTS duration
+			// 2 SIFS + DATA transmit time + 1 ACK
+			// ACK Rate : 24 Mega bps
+			// ACK frame length = 14 bytes
+			Duration = 2*DEFAULT_SIFSTIME +
+					   2*PREAMBLE_PLUS_SIGNAL_PLUS_SIGNALEXTENSION +
+					   ((BodyLen*8 + 22 + Rate*4 - 1)/(Rate*4))*Tsym +
+					   ((112 + 22 + 95)/96)*Tsym;
+		}
+		else	//DSSS
+		{
+			//CTS duration
+			// 2 SIFS + DATA transmit time + 1 ACK
+			// Rate : ?? Mega bps
+			// ACK frame length = 14 bytes
+			if( pT01->T01_plcp_header_length ) //long preamble
+				Duration = LONG_PREAMBLE_PLUS_PLCPHEADER_TIME*2;
+			else
+				Duration = SHORT_PREAMBLE_PLUS_PLCPHEADER_TIME*2;
 
-			//
-			// Start transmitting
-			//
-			BufferFilled = true;
+			Duration += ( ((BodyLen + 14)*8 + Rate-1) / Rate +
+						DEFAULT_SIFSTIME*2 );
+		}
 
-			/* Leaves first u8 intact */
-			memset((u8 *)pTxDes + 1, 0, sizeof(DESCRIPTOR) - 1);
+		if( RTS_on )
+		{
+			if( pT01->T01_modulation_type ) // Is using OFDM
+			{
+				//CTS + 1 SIFS + CTS duration
+				//CTS Rate : 24 Mega bps
+				//CTS frame length = 14 bytes
+				Duration += (DEFAULT_SIFSTIME +
+								PREAMBLE_PLUS_SIGNAL_PLUS_SIGNALEXTENSION +
+								((112 + 22 + 95)/96)*Tsym);
+			}
+			else
+			{
+				//CTS + 1 SIFS + CTS duration
+				//CTS Rate : ?? Mega bps
+				//CTS frame length = 14 bytes
+				if( pT01->T01_plcp_header_length ) //long preamble
+					Duration += LONG_PREAMBLE_PLUS_PLCPHEADER_TIME;
+				else
+					Duration += SHORT_PREAMBLE_PLUS_PLCPHEADER_TIME;
 
-			TxDesIndex = pMds->TxDesIndex;//Get the current ID
-			pTxDes->Descriptor_ID = TxDesIndex;
-			pMds->TxDesFrom[ TxDesIndex ] = 2;//Storing the information of source comming from
-			pMds->TxDesIndex++;
-			pMds->TxDesIndex %= MAX_USB_TX_DESCRIPTOR;
+				Duration += ( ((112 + Rate-1) / Rate) + DEFAULT_SIFSTIME );
+			}
+		}
 
-			MLME_GetNextPacket( adapter, pTxDes );
+		// Set the value into USB descriptor
+		pT01->T01_add_rts = RTS_on ? 1 : 0;
+		pT01->T01_add_cts = CTS_on ? 1 : 0;
+		pT01->T01_rts_cts_duration = Duration;
+	}
 
-			// Copy header. 8byte USB + 24byte 802.11Hdr. Set TxRate, Preamble type
-			Mds_HeaderCopy( adapter, pTxDes, XmitBufAddress );
+	//=====================================
+	// Fill the more fragment descriptor
+	//=====================================
+	if( boGroupAddr )
+		Duration = 0;
+	else
+	{
+		for( i=pDes->FragmentCount-1; i>0; i-- )
+		{
+			NextBodyLen = (u16)pNextT00->T00_frame_length;
+			NextBodyLen += 4;	//CRC
 
-			// For speed up Key setting
-			if (pTxDes->EapFix) {
-#ifdef _PE_TX_DUMP_
-				WBDEBUG(("35: EPA 4th frame detected. Size = %d\n", PacketSize));
-#endif
-				pHwData->IsKeyPreSet = 1;
+			if( pT01->T01_modulation_type )
+			{
+				//OFDM
+				// data transmit time + 3 SIFS + 2 ACK
+				// Rate : ??Mega bps
+				// ACK frame length = 14 bytes, tx rate = 24M
+				Duration = PREAMBLE_PLUS_SIGNAL_PLUS_SIGNALEXTENSION * 3;
+				Duration += (((NextBodyLen*8 + 22 + Rate*4 - 1)/(Rate*4)) * Tsym +
+							(((2*14)*8 + 22 + 95)/96)*Tsym +
+							DEFAULT_SIFSTIME*3);
 			}
+			else
+			{
+				//DSSS
+				// data transmit time + 2 ACK + 3 SIFS
+				// Rate : ??Mega bps
+				// ACK frame length = 14 bytes
+				//TODO :
+				if( pT01->T01_plcp_header_length ) //long preamble
+					Duration = LONG_PREAMBLE_PLUS_PLCPHEADER_TIME*3;
+				else
+					Duration = SHORT_PREAMBLE_PLUS_PLCPHEADER_TIME*3;
 
-			// Copy (fragment) frame body, and set USB, 802.11 hdr flag
-			CurrentSize = Mds_BodyCopy(adapter, pTxDes, XmitBufAddress);
-
-			// Set RTS/CTS and Normal duration field into buffer
-			Mds_DurationSet(adapter, pTxDes, XmitBufAddress);
+				Duration += ( ((NextBodyLen + (2*14))*8 + Rate-1) / Rate +
+							DEFAULT_SIFSTIME*3 );
+			}
 
-			//
-			// Calculation MIC from buffer which maybe fragment, then fill into temporary address 8 byte
-			// 931130.5.e
-			if (MICAdd)
-				Mds_MicFill( adapter, pTxDes, XmitBufAddress );
+			((u16 *)buffer)[5] = cpu_to_le16(Duration);// 4 USHOR for skip 8B USB, 2USHORT=FC + Duration
 
-			//Shift to the next address
-			XmitBufSize += CurrentSize;
-			XmitBufAddress += CurrentSize;
+			//----20061009 add by anson's endian
+			pNextT00->value = cpu_to_le32(pNextT00->value);
+			pT01->value = cpu_to_le32( pT01->value );
+			//----end 20061009 add by anson's endian
 
-#ifdef _IBSS_BEACON_SEQ_STICK_
-			if ((XmitBufAddress[ DOT_11_DA_OFFSET+8 ] & 0xfc) != MAC_SUBTYPE_MNGMNT_PROBE_REQUEST) // +8 for USB hdr
-#endif
-				pMds->TxToggle = true;
+			buffer += OffsetSize;
+			pT01 = (PT01_DESCRIPTOR)(buffer+4);
+			if (i != 1)	//The last fragment will not have the next fragment
+				pNextT00 = (PT00_DESCRIPTOR)(buffer+OffsetSize);
+		}
 
-			// Get packet to transmit completed, 1:TESTSTA 2:MLME 3: Ndis data
-			MLME_SendComplete(adapter, 0, true);
+		//=====================================
+		// Fill the last fragment descriptor
+		//=====================================
+		if( pT01->T01_modulation_type )
+		{
+			//OFDM
+			// 1 SIFS + 1 ACK
+			// Rate : 24 Mega bps
+			// ACK frame length = 14 bytes
+			Duration = PREAMBLE_PLUS_SIGNAL_PLUS_SIGNALEXTENSION;
+			//The Tx rate of ACK use 24M
+			Duration += (((112 + 22 + 95)/96)*Tsym + DEFAULT_SIFSTIME );
+		}
+		else
+		{
+			// DSSS
+			// 1 ACK + 1 SIFS
+			// Rate : ?? Mega bps
+			// ACK frame length = 14 bytes(112 bits)
+			if( pT01->T01_plcp_header_length ) //long preamble
+				Duration = LONG_PREAMBLE_PLUS_PLCPHEADER_TIME;
+			else
+				Duration = SHORT_PREAMBLE_PLUS_PLCPHEADER_TIME;
 
-			// Software TSC count 20060214
-			pMds->TxTsc++;
-			if (pMds->TxTsc == 0)
-				pMds->TxTsc_2++;
+			Duration += ( (112 + Rate-1)/Rate +	DEFAULT_SIFSTIME );
+		}
+	}
 
-			FillCount++; // 20060928
-		} while (HAL_USB_MODE_BURST(pHwData)); // End of multiple MSDU copy loop. false = single true = multiple sending
+	((u16 *)buffer)[5] = cpu_to_le16(Duration);// 4 USHOR for skip 8B USB, 2USHORT=FC + Duration
+	pT00->value = cpu_to_le32(pT00->value);
+	pT01->value = cpu_to_le32(pT01->value);
+	//--end 20061009 add
 
-		// Move to the next one, if necessary
-		if (BufferFilled) {
-			// size setting
-			pMds->TxBufferSize[ FillIndex ] = XmitBufSize;
+}
 
-			// 20060928 set Tx count
-			pMds->TxCountInBuffer[FillIndex] = FillCount;
+// The function return the 4n size of usb pk
+static u16 Mds_BodyCopy(struct wbsoft_priv *adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
+{
+	PT00_DESCRIPTOR	pT00;
+	PMDS	pMds = &adapter->Mds;
+	u8	*buffer;
+	u8	*src_buffer;
+	u8	*pctmp;
+	u16	Size = 0;
+	u16	SizeLeft, CopySize, CopyLeft, stmp;
+	u8	buf_index, FragmentCount = 0;
 
-			// Set owner flag
-			pMds->TxOwner[FillIndex] = 1;
 
-			pMds->TxFillIndex++;
-			pMds->TxFillIndex %= MAX_USB_TX_BUFFER_NUMBER;
-			BufferFilled = false;
+	// Copy fragment body
+	buffer = TargetBuffer; // shift 8B usb + 24B 802.11
+	SizeLeft = pDes->buffer_total_size;
+	buf_index = pDes->buffer_start_index;
+
+	pT00 = (PT00_DESCRIPTOR)buffer;
+	while (SizeLeft) {
+		pT00 = (PT00_DESCRIPTOR)buffer;
+		CopySize = SizeLeft;
+		if (SizeLeft > pDes->FragmentThreshold) {
+			CopySize = pDes->FragmentThreshold;
+			pT00->T00_frame_length = 24 + CopySize;//Set USB length
 		} else
-			break;
+			pT00->T00_frame_length = 24 + SizeLeft;//Set USB length
 
-		if (!PacketSize) // No more pk for transmitting
-			break;
+		SizeLeft -= CopySize;
 
-	} while(true);
+		// 1 Byte operation
+		pctmp = (u8 *)( buffer + 8 + DOT_11_SEQUENCE_OFFSET );
+		*pctmp &= 0xf0;
+		*pctmp |= FragmentCount;//931130.5.m
+		if( !FragmentCount )
+			pT00->T00_first_mpdu = 1;
 
-	//
-	// Start to send by lower module
-	//
-	if (!pHwData->IsKeyPreSet)
-		Wb35Tx_start(adapter);
+		buffer += 32; // 8B usb + 24B 802.11 header
+		Size += 32;
 
- cleanup:
-	atomic_dec(&pMds->TxThreadCount);
-}
+		// Copy into buffer
+		stmp = CopySize + 3;
+		stmp &= ~0x03;//4n Alignment
+		Size += stmp;// Current 4n offset of mpdu
 
-void
-Mds_SendComplete(struct wbsoft_priv * adapter, PT02_DESCRIPTOR pT02)
-{
-	PMDS	pMds = &adapter->Mds;
-	phw_data_t	pHwData = &adapter->sHwData;
-	u8	PacketId = (u8)pT02->T02_Tx_PktID;
-	unsigned char	SendOK = true;
-	u8	RetryCount, TxRate;
+		while (CopySize) {
+			// Copy body
+			src_buffer = pDes->buffer_address[buf_index];
+			CopyLeft = CopySize;
+			if (CopySize >= pDes->buffer_size[buf_index]) {
+				CopyLeft = pDes->buffer_size[buf_index];
 
-	if (pT02->T02_IgnoreResult) // Don't care the result
-		return;
-	if (pT02->T02_IsLastMpdu) {
-		//TODO: DTO -- get the retry count and fragment count
-		// Tx rate
-		TxRate = pMds->TxRate[ PacketId ][ 0 ];
-		RetryCount = (u8)pT02->T02_MPDU_Cnt;
-		if (pT02->value & FLAG_ERROR_TX_MASK) {
-			SendOK = false;
+				// Get the next buffer of descriptor
+				buf_index++;
+				buf_index %= MAX_DESCRIPTOR_BUFFER_INDEX;
+			} else {
+				u8	*pctmp = pDes->buffer_address[buf_index];
+				pctmp += CopySize;
+				pDes->buffer_address[buf_index] = pctmp;
+				pDes->buffer_size[buf_index] -= CopySize;
+			}
 
-			if (pT02->T02_transmit_abort || pT02->T02_out_of_MaxTxMSDULiftTime) {
-				//retry error
-				pHwData->dto_tx_retry_count += (RetryCount+1);
-				//[for tx debug]
-				if (RetryCount<7)
-					pHwData->tx_retry_count[RetryCount] += RetryCount;
-				else
-					pHwData->tx_retry_count[7] += RetryCount;
-				#ifdef _PE_STATE_DUMP_
-				WBDEBUG(("dto_tx_retry_count =%d\n", pHwData->dto_tx_retry_count));
-				#endif
-				MTO_SetTxCount(adapter, TxRate, RetryCount);
+			memcpy(buffer, src_buffer, CopyLeft);
+			buffer += CopyLeft;
+			CopySize -= CopyLeft;
+		}
+
+		// 931130.5.n
+		if (pMds->MicAdd) {
+			if (!SizeLeft) {
+				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - pMds->MicAdd;
+				pMds->MicWriteSize[ pMds->MicWriteIndex ] = pMds->MicAdd;
+				pMds->MicAdd = 0;
 			}
-			pHwData->dto_tx_frag_count += (RetryCount+1);
+			else if( SizeLeft < 8 ) //931130.5.p
+			{
+				pMds->MicAdd = SizeLeft;
+				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - ( 8 - SizeLeft );
+				pMds->MicWriteSize[ pMds->MicWriteIndex ] = 8 - SizeLeft;
+				pMds->MicWriteIndex++;
+			}
+		}
 
-			//[for tx debug]
-			if (pT02->T02_transmit_abort_due_to_TBTT)
-				pHwData->tx_TBTT_start_count++;
-			if (pT02->T02_transmit_without_encryption_due_to_wep_on_false)
-				pHwData->tx_WepOn_false_count++;
-			if (pT02->T02_discard_due_to_null_wep_key)
-				pHwData->tx_Null_key_count++;
-		} else {
-			if (pT02->T02_effective_transmission_rate)
-				pHwData->tx_ETR_count++;
-			MTO_SetTxCount(adapter, TxRate, RetryCount);
+		// Does it need to generate the new header for next mpdu?
+		if (SizeLeft) {
+			buffer = TargetBuffer + Size; // Get the next 4n start address
+			memcpy( buffer, TargetBuffer, 32 );//Copy 8B USB +24B 802.11
+			pT00 = (PT00_DESCRIPTOR)buffer;
+			pT00->T00_first_mpdu = 0;
 		}
 
-		// Clear send result buffer
-		pMds->TxResult[ PacketId ] = 0;
-	} else
-		pMds->TxResult[ PacketId ] |= ((u16)(pT02->value & 0x0ffff));
+		FragmentCount++;
+	}
+
+	pT00->T00_last_mpdu = 1;
+	pT00->T00_IsLastMpdu = 1;
+	buffer = (u8 *)pT00 + 8; // +8 for USB hdr
+	buffer[1] &= ~0x04; // Clear more frag bit of 802.11 frame control
+	pDes->FragmentCount = FragmentCount; // Update the correct fragment number
+	return Size;
 }
 
-void
-Mds_HeaderCopy(struct wbsoft_priv * adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
+static void Mds_HeaderCopy(struct wbsoft_priv * adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
 {
 	PMDS	pMds = &adapter->Mds;
 	u8	*src_buffer = pDes->buffer_address[0];//931130.5.g
@@ -331,301 +418,197 @@ Mds_HeaderCopy(struct wbsoft_priv * adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
 
 }
 
-// The function return the 4n size of usb pk
-u16
-Mds_BodyCopy(struct wbsoft_priv * adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
+void
+Mds_Tx(struct wbsoft_priv * adapter)
 {
-	PT00_DESCRIPTOR	pT00;
-	PMDS	pMds = &adapter->Mds;
-	u8	*buffer;
-	u8	*src_buffer;
-	u8	*pctmp;
-	u16	Size = 0;
-	u16	SizeLeft, CopySize, CopyLeft, stmp;
-	u8	buf_index, FragmentCount = 0;
+	phw_data_t	pHwData = &adapter->sHwData;
+	PMDS		pMds = &adapter->Mds;
+	DESCRIPTOR	TxDes;
+	PDESCRIPTOR	pTxDes = &TxDes;
+	u8		*XmitBufAddress;
+	u16		XmitBufSize, PacketSize, stmp, CurrentSize, FragmentThreshold;
+	u8		FillIndex, TxDesIndex, FragmentCount, FillCount;
+	unsigned char	BufferFilled = false, MICAdd = 0;
 
 
-	// Copy fragment body
-	buffer = TargetBuffer; // shift 8B usb + 24B 802.11
-	SizeLeft = pDes->buffer_total_size;
-	buf_index = pDes->buffer_start_index;
+	if (pMds->TxPause)
+		return;
+	if (!hal_driver_init_OK(pHwData))
+		return;
 
-	pT00 = (PT00_DESCRIPTOR)buffer;
-	while (SizeLeft) {
-		pT00 = (PT00_DESCRIPTOR)buffer;
-		CopySize = SizeLeft;
-		if (SizeLeft > pDes->FragmentThreshold) {
-			CopySize = pDes->FragmentThreshold;
-			pT00->T00_frame_length = 24 + CopySize;//Set USB length
-		} else
-			pT00->T00_frame_length = 24 + SizeLeft;//Set USB length
+	//Only one thread can be run here
+	if (!atomic_inc_return(&pMds->TxThreadCount) == 1)
+		goto cleanup;
 
-		SizeLeft -= CopySize;
+	// Start to fill the data
+	do {
+		FillIndex = pMds->TxFillIndex;
+		if (pMds->TxOwner[FillIndex]) { // Is owned by software 0:Yes 1:No
+#ifdef _PE_TX_DUMP_
+			WBDEBUG(("[Mds_Tx] Tx Owner is H/W.\n"));
+#endif
+			break;
+		}
 
-		// 1 Byte operation
-		pctmp = (u8 *)( buffer + 8 + DOT_11_SEQUENCE_OFFSET );
-		*pctmp &= 0xf0;
-		*pctmp |= FragmentCount;//931130.5.m
-		if( !FragmentCount )
-			pT00->T00_first_mpdu = 1;
+		XmitBufAddress = pMds->pTxBuffer + (MAX_USB_TX_BUFFER * FillIndex); //Get buffer
+		XmitBufSize = 0;
+		FillCount = 0;
+		do {
+			PacketSize = adapter->sMlmeFrame.len;
+			if (!PacketSize)
+				break;
 
-		buffer += 32; // 8B usb + 24B 802.11 header
-		Size += 32;
+			//For Check the buffer resource
+			FragmentThreshold = CURRENT_FRAGMENT_THRESHOLD;
+			//931130.5.b
+			FragmentCount = PacketSize/FragmentThreshold + 1;
+			stmp = PacketSize + FragmentCount*32 + 8;//931130.5.c 8:MIC
+			if ((XmitBufSize + stmp) >= MAX_USB_TX_BUFFER) {
+				printk("[Mds_Tx] Excess max tx buffer.\n");
+				break; // buffer is not enough
+			}
 
-		// Copy into buffer
-		stmp = CopySize + 3;
-		stmp &= ~0x03;//4n Alignment
-		Size += stmp;// Current 4n offset of mpdu
 
-		while (CopySize) {
-			// Copy body
-			src_buffer = pDes->buffer_address[buf_index];
-			CopyLeft = CopySize;
-			if (CopySize >= pDes->buffer_size[buf_index]) {
-				CopyLeft = pDes->buffer_size[buf_index];
+			//
+			// Start transmitting
+			//
+			BufferFilled = true;
 
-				// Get the next buffer of descriptor
-				buf_index++;
-				buf_index %= MAX_DESCRIPTOR_BUFFER_INDEX;
-			} else {
-				u8	*pctmp = pDes->buffer_address[buf_index];
-				pctmp += CopySize;
-				pDes->buffer_address[buf_index] = pctmp;
-				pDes->buffer_size[buf_index] -= CopySize;
-			}
-
-			memcpy(buffer, src_buffer, CopyLeft);
-			buffer += CopyLeft;
-			CopySize -= CopyLeft;
-		}
+			/* Leaves first u8 intact */
+			memset((u8 *)pTxDes + 1, 0, sizeof(DESCRIPTOR) - 1);
 
-		// 931130.5.n
-		if (pMds->MicAdd) {
-			if (!SizeLeft) {
-				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - pMds->MicAdd;
-				pMds->MicWriteSize[ pMds->MicWriteIndex ] = pMds->MicAdd;
-				pMds->MicAdd = 0;
-			}
-			else if( SizeLeft < 8 ) //931130.5.p
-			{
-				pMds->MicAdd = SizeLeft;
-				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - ( 8 - SizeLeft );
-				pMds->MicWriteSize[ pMds->MicWriteIndex ] = 8 - SizeLeft;
-				pMds->MicWriteIndex++;
-			}
-		}
+			TxDesIndex = pMds->TxDesIndex;//Get the current ID
+			pTxDes->Descriptor_ID = TxDesIndex;
+			pMds->TxDesFrom[ TxDesIndex ] = 2;//Storing the information of source comming from
+			pMds->TxDesIndex++;
+			pMds->TxDesIndex %= MAX_USB_TX_DESCRIPTOR;
 
-		// Does it need to generate the new header for next mpdu?
-		if (SizeLeft) {
-			buffer = TargetBuffer + Size; // Get the next 4n start address
-			memcpy( buffer, TargetBuffer, 32 );//Copy 8B USB +24B 802.11
-			pT00 = (PT00_DESCRIPTOR)buffer;
-			pT00->T00_first_mpdu = 0;
-		}
+			MLME_GetNextPacket( adapter, pTxDes );
 
-		FragmentCount++;
-	}
+			// Copy header. 8byte USB + 24byte 802.11Hdr. Set TxRate, Preamble type
+			Mds_HeaderCopy( adapter, pTxDes, XmitBufAddress );
 
-	pT00->T00_last_mpdu = 1;
-	pT00->T00_IsLastMpdu = 1;
-	buffer = (u8 *)pT00 + 8; // +8 for USB hdr
-	buffer[1] &= ~0x04; // Clear more frag bit of 802.11 frame control
-	pDes->FragmentCount = FragmentCount; // Update the correct fragment number
-	return Size;
-}
+			// For speed up Key setting
+			if (pTxDes->EapFix) {
+#ifdef _PE_TX_DUMP_
+				WBDEBUG(("35: EPA 4th frame detected. Size = %d\n", PacketSize));
+#endif
+				pHwData->IsKeyPreSet = 1;
+			}
 
+			// Copy (fragment) frame body, and set USB, 802.11 hdr flag
+			CurrentSize = Mds_BodyCopy(adapter, pTxDes, XmitBufAddress);
 
-void
-Mds_DurationSet(  struct wbsoft_priv * adapter,  PDESCRIPTOR pDes,  u8 *buffer )
-{
-	PT00_DESCRIPTOR	pT00;
-	PT01_DESCRIPTOR	pT01;
-	u16	Duration, NextBodyLen, OffsetSize;
-	u8	Rate, i;
-	unsigned char	CTS_on = false, RTS_on = false;
-	PT00_DESCRIPTOR pNextT00;
-	u16 BodyLen = 0;
-	unsigned char boGroupAddr = false;
+			// Set RTS/CTS and Normal duration field into buffer
+			Mds_DurationSet(adapter, pTxDes, XmitBufAddress);
 
-	OffsetSize = pDes->FragmentThreshold + 32 + 3;
-	OffsetSize &= ~0x03;
-	Rate = pDes->TxRate >> 1;
-	if (!Rate)
-		Rate = 1;
+			//
+			// Calculation MIC from buffer which maybe fragment, then fill into temporary address 8 byte
+			// 931130.5.e
+			if (MICAdd)
+				Mds_MicFill( adapter, pTxDes, XmitBufAddress );
 
-	pT00 = (PT00_DESCRIPTOR)buffer;
-	pT01 = (PT01_DESCRIPTOR)(buffer+4);
-	pNextT00 = (PT00_DESCRIPTOR)(buffer+OffsetSize);
+			//Shift to the next address
+			XmitBufSize += CurrentSize;
+			XmitBufAddress += CurrentSize;
 
-	if( buffer[ DOT_11_DA_OFFSET+8 ] & 0x1 ) // +8 for USB hdr
-		boGroupAddr = true;
+#ifdef _IBSS_BEACON_SEQ_STICK_
+			if ((XmitBufAddress[ DOT_11_DA_OFFSET+8 ] & 0xfc) != MAC_SUBTYPE_MNGMNT_PROBE_REQUEST) // +8 for USB hdr
+#endif
+				pMds->TxToggle = true;
 
-	//========================================
-	// Set RTS/CTS mechanism
-	//========================================
-	if (!boGroupAddr)
-	{
-		//NOTE : If the protection mode is enabled and the MSDU will be fragmented,
-		//		 the tx rates of MPDUs will all be DSSS rates. So it will not use
-		//		 CTS-to-self in this case. CTS-To-self will only be used when without
-		//		 fragmentation. -- 20050112
-		BodyLen = (u16)pT00->T00_frame_length;	//include 802.11 header
-		BodyLen += 4;	//CRC
+			// Get packet to transmit completed, 1:TESTSTA 2:MLME 3: Ndis data
+			MLME_SendComplete(adapter, 0, true);
 
-		if( BodyLen >= CURRENT_RTS_THRESHOLD )
-			RTS_on = true; // Using RTS
-		else
-		{
-			if( pT01->T01_modulation_type ) // Is using OFDM
-			{
-				if( CURRENT_PROTECT_MECHANISM ) // Is using protect
-					CTS_on = true; // Using CTS
-			}
-		}
-	}
+			// Software TSC count 20060214
+			pMds->TxTsc++;
+			if (pMds->TxTsc == 0)
+				pMds->TxTsc_2++;
 
-	if( RTS_on || CTS_on )
-	{
-		if( pT01->T01_modulation_type) // Is using OFDM
-		{
-			//CTS duration
-			// 2 SIFS + DATA transmit time + 1 ACK
-			// ACK Rate : 24 Mega bps
-			// ACK frame length = 14 bytes
-			Duration = 2*DEFAULT_SIFSTIME +
-					   2*PREAMBLE_PLUS_SIGNAL_PLUS_SIGNALEXTENSION +
-					   ((BodyLen*8 + 22 + Rate*4 - 1)/(Rate*4))*Tsym +
-					   ((112 + 22 + 95)/96)*Tsym;
-		}
-		else	//DSSS
-		{
-			//CTS duration
-			// 2 SIFS + DATA transmit time + 1 ACK
-			// Rate : ?? Mega bps
-			// ACK frame length = 14 bytes
-			if( pT01->T01_plcp_header_length ) //long preamble
-				Duration = LONG_PREAMBLE_PLUS_PLCPHEADER_TIME*2;
-			else
-				Duration = SHORT_PREAMBLE_PLUS_PLCPHEADER_TIME*2;
+			FillCount++; // 20060928
+		} while (HAL_USB_MODE_BURST(pHwData)); // End of multiple MSDU copy loop. false = single true = multiple sending
 
-			Duration += ( ((BodyLen + 14)*8 + Rate-1) / Rate +
-						DEFAULT_SIFSTIME*2 );
-		}
+		// Move to the next one, if necessary
+		if (BufferFilled) {
+			// size setting
+			pMds->TxBufferSize[ FillIndex ] = XmitBufSize;
 
-		if( RTS_on )
-		{
-			if( pT01->T01_modulation_type ) // Is using OFDM
-			{
-				//CTS + 1 SIFS + CTS duration
-				//CTS Rate : 24 Mega bps
-				//CTS frame length = 14 bytes
-				Duration += (DEFAULT_SIFSTIME +
-								PREAMBLE_PLUS_SIGNAL_PLUS_SIGNALEXTENSION +
-								((112 + 22 + 95)/96)*Tsym);
-			}
-			else
-			{
-				//CTS + 1 SIFS + CTS duration
-				//CTS Rate : ?? Mega bps
-				//CTS frame length = 14 bytes
-				if( pT01->T01_plcp_header_length ) //long preamble
-					Duration += LONG_PREAMBLE_PLUS_PLCPHEADER_TIME;
-				else
-					Duration += SHORT_PREAMBLE_PLUS_PLCPHEADER_TIME;
+			// 20060928 set Tx count
+			pMds->TxCountInBuffer[FillIndex] = FillCount;
 
-				Duration += ( ((112 + Rate-1) / Rate) + DEFAULT_SIFSTIME );
-			}
-		}
+			// Set owner flag
+			pMds->TxOwner[FillIndex] = 1;
 
-		// Set the value into USB descriptor
-		pT01->T01_add_rts = RTS_on ? 1 : 0;
-		pT01->T01_add_cts = CTS_on ? 1 : 0;
-		pT01->T01_rts_cts_duration = Duration;
-	}
+			pMds->TxFillIndex++;
+			pMds->TxFillIndex %= MAX_USB_TX_BUFFER_NUMBER;
+			BufferFilled = false;
+		} else
+			break;
 
-	//=====================================
-	// Fill the more fragment descriptor
-	//=====================================
-	if( boGroupAddr )
-		Duration = 0;
-	else
-	{
-		for( i=pDes->FragmentCount-1; i>0; i-- )
-		{
-			NextBodyLen = (u16)pNextT00->T00_frame_length;
-			NextBodyLen += 4;	//CRC
+		if (!PacketSize) // No more pk for transmitting
+			break;
 
-			if( pT01->T01_modulation_type )
-			{
-				//OFDM
-				// data transmit time + 3 SIFS + 2 ACK
-				// Rate : ??Mega bps
-				// ACK frame length = 14 bytes, tx rate = 24M
-				Duration = PREAMBLE_PLUS_SIGNAL_PLUS_SIGNALEXTENSION * 3;
-				Duration += (((NextBodyLen*8 + 22 + Rate*4 - 1)/(Rate*4)) * Tsym +
-							(((2*14)*8 + 22 + 95)/96)*Tsym +
-							DEFAULT_SIFSTIME*3);
-			}
-			else
-			{
-				//DSSS
-				// data transmit time + 2 ACK + 3 SIFS
-				// Rate : ??Mega bps
-				// ACK frame length = 14 bytes
-				//TODO :
-				if( pT01->T01_plcp_header_length ) //long preamble
-					Duration = LONG_PREAMBLE_PLUS_PLCPHEADER_TIME*3;
-				else
-					Duration = SHORT_PREAMBLE_PLUS_PLCPHEADER_TIME*3;
+	} while(true);
 
-				Duration += ( ((NextBodyLen + (2*14))*8 + Rate-1) / Rate +
-							DEFAULT_SIFSTIME*3 );
-			}
+	//
+	// Start to send by lower module
+	//
+	if (!pHwData->IsKeyPreSet)
+		Wb35Tx_start(adapter);
 
-			((u16 *)buffer)[5] = cpu_to_le16(Duration);// 4 USHOR for skip 8B USB, 2USHORT=FC + Duration
+ cleanup:
+	atomic_dec(&pMds->TxThreadCount);
+}
 
-			//----20061009 add by anson's endian
-			pNextT00->value = cpu_to_le32(pNextT00->value);
-			pT01->value = cpu_to_le32( pT01->value );
-			//----end 20061009 add by anson's endian
+void
+Mds_SendComplete(struct wbsoft_priv * adapter, PT02_DESCRIPTOR pT02)
+{
+	PMDS	pMds = &adapter->Mds;
+	phw_data_t	pHwData = &adapter->sHwData;
+	u8	PacketId = (u8)pT02->T02_Tx_PktID;
+	unsigned char	SendOK = true;
+	u8	RetryCount, TxRate;
 
-			buffer += OffsetSize;
-			pT01 = (PT01_DESCRIPTOR)(buffer+4);
-			if (i != 1)	//The last fragment will not have the next fragment
-				pNextT00 = (PT00_DESCRIPTOR)(buffer+OffsetSize);
-		}
+	if (pT02->T02_IgnoreResult) // Don't care the result
+		return;
+	if (pT02->T02_IsLastMpdu) {
+		//TODO: DTO -- get the retry count and fragment count
+		// Tx rate
+		TxRate = pMds->TxRate[ PacketId ][ 0 ];
+		RetryCount = (u8)pT02->T02_MPDU_Cnt;
+		if (pT02->value & FLAG_ERROR_TX_MASK) {
+			SendOK = false;
 
-		//=====================================
-		// Fill the last fragment descriptor
-		//=====================================
-		if( pT01->T01_modulation_type )
-		{
-			//OFDM
-			// 1 SIFS + 1 ACK
-			// Rate : 24 Mega bps
-			// ACK frame length = 14 bytes
-			Duration = PREAMBLE_PLUS_SIGNAL_PLUS_SIGNALEXTENSION;
-			//The Tx rate of ACK use 24M
-			Duration += (((112 + 22 + 95)/96)*Tsym + DEFAULT_SIFSTIME );
-		}
-		else
-		{
-			// DSSS
-			// 1 ACK + 1 SIFS
-			// Rate : ?? Mega bps
-			// ACK frame length = 14 bytes(112 bits)
-			if( pT01->T01_plcp_header_length ) //long preamble
-				Duration = LONG_PREAMBLE_PLUS_PLCPHEADER_TIME;
-			else
-				Duration = SHORT_PREAMBLE_PLUS_PLCPHEADER_TIME;
+			if (pT02->T02_transmit_abort || pT02->T02_out_of_MaxTxMSDULiftTime) {
+				//retry error
+				pHwData->dto_tx_retry_count += (RetryCount+1);
+				//[for tx debug]
+				if (RetryCount<7)
+					pHwData->tx_retry_count[RetryCount] += RetryCount;
+				else
+					pHwData->tx_retry_count[7] += RetryCount;
+				#ifdef _PE_STATE_DUMP_
+				WBDEBUG(("dto_tx_retry_count =%d\n", pHwData->dto_tx_retry_count));
+				#endif
+				MTO_SetTxCount(adapter, TxRate, RetryCount);
+			}
+			pHwData->dto_tx_frag_count += (RetryCount+1);
 
-			Duration += ( (112 + Rate-1)/Rate +	DEFAULT_SIFSTIME );
+			//[for tx debug]
+			if (pT02->T02_transmit_abort_due_to_TBTT)
+				pHwData->tx_TBTT_start_count++;
+			if (pT02->T02_transmit_without_encryption_due_to_wep_on_false)
+				pHwData->tx_WepOn_false_count++;
+			if (pT02->T02_discard_due_to_null_wep_key)
+				pHwData->tx_Null_key_count++;
+		} else {
+			if (pT02->T02_effective_transmission_rate)
+				pHwData->tx_ETR_count++;
+			MTO_SetTxCount(adapter, TxRate, RetryCount);
 		}
-	}
-
-	((u16 *)buffer)[5] = cpu_to_le16(Duration);// 4 USHOR for skip 8B USB, 2USHORT=FC + Duration
-	pT00->value = cpu_to_le32(pT00->value);
-	pT01->value = cpu_to_le32(pT01->value);
-	//--end 20061009 add
 
+		// Clear send result buffer
+		pMds->TxResult[ PacketId ] = 0;
+	} else
+		pMds->TxResult[ PacketId ] |= ((u16)(pT02->value & 0x0ffff));
 }
diff --git a/drivers/staging/winbond/mds_f.h b/drivers/staging/winbond/mds_f.h
index a3724ac6798d..8109eba09add 100644
--- a/drivers/staging/winbond/mds_f.h
+++ b/drivers/staging/winbond/mds_f.h
@@ -7,12 +7,8 @@
 unsigned char Mds_initial(  struct wbsoft_priv *adapter );
 void Mds_Destroy(  struct wbsoft_priv *adapter );
 void Mds_Tx(  struct wbsoft_priv *adapter );
-void Mds_HeaderCopy(  struct wbsoft_priv *adapter,  PDESCRIPTOR pDes,  u8 *TargetBuffer );
-u16 Mds_BodyCopy(  struct wbsoft_priv *adapter,  PDESCRIPTOR pDes,  u8 *TargetBuffer );
-void Mds_DurationSet(  struct wbsoft_priv *adapter,  PDESCRIPTOR pDes,  u8 *TargetBuffer );
 void Mds_SendComplete(  struct wbsoft_priv *adapter,  PT02_DESCRIPTOR pT02 );
 void Mds_MpduProcess(  struct wbsoft_priv *adapter,  PDESCRIPTOR pRxDes );
-void Mds_reset_descriptor(  struct wbsoft_priv *adapter );
 extern void DataDmp(u8 *pdata, u32 len, u32 offset);