ieee80211_output.c

madwifi上的atheros无线网卡驱动源码

		llc = (struct llc *) skb_push(skb, LLC_SNAPFRAMELEN);
		llc->llc_dsap = llc->llc_ssap = LLC_SNAP_LSAP;
		llc->llc_control = LLC_UI;
		llc->llc_snap.org_code[0] = 0;
		llc->llc_snap.org_code[1] = 0;
		llc->llc_snap.org_code[2] = 0;
		llc->llc_snap.ether_type = eh.ether_type;

		eh_inter = (struct ether_header *) skb_push(skb, sizeof(struct ether_header));
		memcpy(eh_inter, &eh, sizeof(struct ether_header) - sizeof eh.ether_type);
		eh_inter->ether_type = htons(payload);

		/* overall ff encap header */
		/* XXX: the offset of 2, below, should be computed. but... it will not
		 *      practically ever change.
		 */
		ffhdr = (struct athl2p_tunnel_hdr *) skb_push(skb, sizeof(struct athl2p_tunnel_hdr) + 2);
		memset(ffhdr, 0, sizeof(struct athl2p_tunnel_hdr) + 2);

		/*
		 * add second skb tunnel hdrs
		 */

		payload = skb2->len + LLC_SNAPFRAMELEN;

		llc = (struct llc *) skb_push(skb2, LLC_SNAPFRAMELEN);
		if (llc == NULL) {
			IEEE80211_DPRINTF(vap, IEEE80211_MSG_SUPG,
				"%s: failed to push llc for 2nd skb (%p)\n",
				__func__, skb);
			return NULL;
		}
		llc->llc_dsap = llc->llc_ssap = LLC_SNAP_LSAP;
		llc->llc_control = LLC_UI;
		llc->llc_snap.org_code[0] = 0;
		llc->llc_snap.org_code[1] = 0;
		llc->llc_snap.org_code[2] = 0;
		llc->llc_snap.ether_type = eh.ether_type;

		eh_inter = (struct ether_header *) skb_push(skb2, sizeof(struct ether_header));
		if (eh_inter == NULL) {
			IEEE80211_DPRINTF(vap, IEEE80211_MSG_SUPG,
				"%s: failed to push eth hdr for 2nd skb (%p)\n",
				__func__, skb);
			return NULL;
		}

		memcpy(eh_inter, &eh2, sizeof(struct ether_header) - sizeof eh.ether_type);
		eh_inter->ether_type = htons(payload);

		/* variable length pad */
		skb_push(skb2, roundup(padded_len, 4) - padded_len);
	}
#endif

	llc = (struct llc *) skb_push(skb, LLC_SNAPFRAMELEN);
	llc->llc_dsap = llc->llc_ssap = LLC_SNAP_LSAP;
	llc->llc_control = LLC_UI;
#ifndef ATH_SUPERG_FF
	llc->llc_snap.org_code[0] = 0;
	llc->llc_snap.org_code[1] = 0;
	llc->llc_snap.org_code[2] = 0;
	llc->llc_snap.ether_type = eh.ether_type;
#else /* ATH_SUPERG_FF */
	if (isff) {
		llc->llc_snap.org_code[0] = ATH_SNAP_ORGCODE_0;
		llc->llc_snap.org_code[1] = ATH_SNAP_ORGCODE_1;
		llc->llc_snap.org_code[2] = ATH_SNAP_ORGCODE_2;
		llc->llc_snap.ether_type = htons(ATH_ETH_TYPE);
	} else {
		llc->llc_snap.org_code[0] = 0;
		llc->llc_snap.org_code[1] = 0;
		llc->llc_snap.org_code[2] = 0;
		llc->llc_snap.ether_type = eh.ether_type;
	}
#endif /* ATH_SUPERG_FF */
	datalen = skb->len;			/* NB: w/o 802.11 header */
	
	wh = (struct ieee80211_frame *) skb_push(skb, hdrsize);
	wh->i_fc[0] = IEEE80211_FC0_VERSION_0 | IEEE80211_FC0_TYPE_DATA;
	wh->i_dur = 0;
	if (use4addr) {
		wh->i_fc[1] = IEEE80211_FC1_DIR_DSTODS;
		IEEE80211_ADDR_COPY(wh->i_addr1, ni->ni_macaddr);
		IEEE80211_ADDR_COPY(wh->i_addr2, vap->iv_myaddr);
		IEEE80211_ADDR_COPY(wh->i_addr3, eh.ether_dhost);
		IEEE80211_ADDR_COPY(WH4(wh)->i_addr4, eh.ether_shost);
	} else {
		switch (vap->iv_opmode) {
		case IEEE80211_M_IBSS:
		case IEEE80211_M_AHDEMO:
			wh->i_fc[1] = IEEE80211_FC1_DIR_NODS;
			IEEE80211_ADDR_COPY(wh->i_addr1, eh.ether_dhost);
			IEEE80211_ADDR_COPY(wh->i_addr2, eh.ether_shost);
			/*
			 * NB: always use the bssid from iv_bss as the
			 *     neighbor's may be stale after an ibss merge
			 */
			IEEE80211_ADDR_COPY(wh->i_addr3, vap->iv_bss->ni_bssid);
			break;
		case IEEE80211_M_STA:
			wh->i_fc[1] = IEEE80211_FC1_DIR_TODS;
			IEEE80211_ADDR_COPY(wh->i_addr1, ni->ni_bssid);
			IEEE80211_ADDR_COPY(wh->i_addr2, eh.ether_shost);
			IEEE80211_ADDR_COPY(wh->i_addr3, eh.ether_dhost);
			break;
		case IEEE80211_M_HOSTAP:
			wh->i_fc[1] = IEEE80211_FC1_DIR_FROMDS;
			IEEE80211_ADDR_COPY(wh->i_addr1, eh.ether_dhost);
			IEEE80211_ADDR_COPY(wh->i_addr2, ni->ni_bssid);
			IEEE80211_ADDR_COPY(wh->i_addr3, eh.ether_shost);
			if (M_PWR_SAV_GET(skb)) {
				if (IEEE80211_NODE_SAVEQ_QLEN(ni)) {
					wh->i_fc[1] |= IEEE80211_FC1_MORE_DATA;
					M_PWR_SAV_CLR(skb);
				}
			}
			break;
		case IEEE80211_M_WDS:
			wh->i_fc[1] = IEEE80211_FC1_DIR_DSTODS;
			IEEE80211_ADDR_COPY(wh->i_addr1, ni->ni_macaddr);
			IEEE80211_ADDR_COPY(wh->i_addr2, vap->iv_myaddr);
			IEEE80211_ADDR_COPY(wh->i_addr3, eh.ether_dhost);
			IEEE80211_ADDR_COPY(WH4(wh)->i_addr4, eh.ether_shost);
			break;
		case IEEE80211_M_MONITOR:
			goto bad;
		}
	}
	if (IEEE80211_VAP_IS_SLEEPING(vap))
		wh->i_fc[1] |= IEEE80211_FC1_PWR_MGT;
	if (addqos) {
		struct ieee80211_qosframe *qwh =
			(struct ieee80211_qosframe *) wh;
		u_int8_t *qos;
		int tid;
		
		qos = &qwh->i_qos[0];
		if (use4addr)
			qos += IEEE80211_ADDR_LEN;
		/* map from access class/queue to 11e header priority value */
		tid = WME_AC_TO_TID(skb->priority);
		qos[0] = tid & IEEE80211_QOS_TID;
		if (ic->ic_wme.wme_wmeChanParams.cap_wmeParams[skb->priority].wmep_noackPolicy)
			qos[0] |= (1 << IEEE80211_QOS_ACKPOLICY_S) & IEEE80211_QOS_ACKPOLICY;
		qos[1] = 0;
		qwh->i_fc[0] |= IEEE80211_FC0_SUBTYPE_QOS;

		*(__le16 *)&wh->i_seq[0] =
			htole16(ni->ni_txseqs[tid] << IEEE80211_SEQ_SEQ_SHIFT);
		ni->ni_txseqs[tid]++;
	} else {
		*(__le16 *)wh->i_seq =
			htole16(ni->ni_txseqs[0] << IEEE80211_SEQ_SEQ_SHIFT);
		ni->ni_txseqs[0]++;
	}

	/* Is transmit fragmentation needed? */
	if (skb->len > vap->iv_fragthreshold &&
	    !IEEE80211_IS_MULTICAST(wh->i_addr1)) {
		int pktlen, skbcnt, tailsize, ciphdrsize;
		struct ieee80211_cipher *cip;

		pktlen = skb->len;
		ciphdrsize = 0;
		tailsize = IEEE80211_CRC_LEN;

		if (key != NULL) {
			cip = (struct ieee80211_cipher *) key->wk_cipher;
			ciphdrsize = cip->ic_header;
			tailsize += (cip->ic_trailer + cip->ic_miclen);
		}
		
		pdusize = vap->iv_fragthreshold - (hdrsize_nopad + ciphdrsize);
		fragcnt = *framecnt = 
			((pktlen - (hdrsize_nopad + ciphdrsize)) / pdusize) +
			(((pktlen - (hdrsize_nopad + ciphdrsize)) % 
				pdusize == 0) ? 0 : 1);

		/*
		 * Allocate sk_buff for each subsequent fragment; First fragment
		 * reuses input skb.
		 */
		for (skbcnt = 1; skbcnt < fragcnt; skbcnt++) {
			tskb = dev_alloc_skb(hdrsize + ciphdrsize + pdusize + tailsize);
			if (tskb == NULL)
				break;
			
			tskb->next = framelist;
			framelist = tskb;
		}

		if (skbcnt != fragcnt)
			goto bad;
	}
	else
		*framecnt = fragcnt;

	if (key != NULL) {
		/*
		 * IEEE 802.1X: send EAPOL frames always in the clear.
		 * WPA/WPA2: encrypt EAPOL keys when pairwise keys are set.
		 */
		if (eh.ether_type != __constant_htons(ETHERTYPE_PAE) ||
		    ((vap->iv_flags & IEEE80211_F_WPA) &&
		    (vap->iv_opmode == IEEE80211_M_STA ?
		    !KEY_UNDEFINED(*key) : !KEY_UNDEFINED(ni->ni_ucastkey)))) {
			int force_swmic = (fragcnt > 1) ? 1 : 0;

			wh->i_fc[1] |= IEEE80211_FC1_PROT;

			if (!ieee80211_crypto_enmic(vap, key, skb, force_swmic)) {
				IEEE80211_NOTE_MAC(vap, IEEE80211_MSG_OUTPUT,
					eh.ether_dhost,
					"%s", "enmic failed, discard frame");
				vap->iv_stats.is_crypto_enmicfail++;
				goto bad;
			}
		}
	}

	if (fragcnt > 1) {
		int fragnum, offset, pdulen;
		void *pdu;

		fragnum = 0;
		wh = twh = (struct ieee80211_frame *) skb->data;

		/*
		** Setup WLAN headers as fragment headers
		*/
		wh->i_fc[1] |= IEEE80211_FC1_MORE_FRAG;

		*(__le16 *)&wh->i_seq[0] |= 
			htole16((fragnum & IEEE80211_SEQ_FRAG_MASK) <<
				IEEE80211_SEQ_FRAG_SHIFT);
		fragnum++;

		offset = hdrsize + pdusize;
		datalen = (skb->len - hdrsize) - pdusize;

		IEEE80211_NODE_STAT(ni, tx_data);
		IEEE80211_NODE_STAT_ADD(ni, tx_bytes, pdusize);

		for (tskb = framelist; tskb != NULL; tskb = tskb->next) {
			/*
			 * Copy WLAN header into each frag header skb
			 */
			twh = (struct ieee80211_frame *) skb_put(tskb, hdrsize);
			memcpy((void *) twh, (void *) wh, hdrsize);

			*(__le16 *)&twh->i_seq[0] |=
				htole16((fragnum & IEEE80211_SEQ_FRAG_MASK) <<
					IEEE80211_SEQ_FRAG_SHIFT);
			fragnum++;

			if (pdusize <= datalen)
				pdulen = pdusize;
			else
				pdulen = datalen;

			/*
			 * Copy fragment payload from input skb.
			 * Doing copies isn't intuitive from 
			 * a performance perspective, however,
			 * for this case, it is believed to be 
			 * more efficient than cloning skbs.
			 */
			pdu = skb_put(tskb, pdulen);
			memcpy(pdu, (void *) skb->data+offset, pdulen);

			offset += pdusize;
			datalen -= pdusize;

			IEEE80211_NODE_STAT(ni, tx_data);
			IEEE80211_NODE_STAT_ADD(ni, tx_bytes, pdulen);
		}

		twh->i_fc[1] &= ~IEEE80211_FC1_MORE_FRAG;
		skb_trim(skb, hdrsize + pdusize);
		skb->next = framelist;
	} else {
		IEEE80211_NODE_STAT(ni, tx_data);
		IEEE80211_NODE_STAT_ADD(ni, tx_bytes, datalen);

#ifdef ATH_SUPERG_FF
		/* Account for a second skb in the same packet when FF is on */ 
		if (skb->next) {
			datalen = skb->next->len;	
			IEEE80211_NODE_STAT(ni, tx_data);
			IEEE80211_NODE_STAT_ADD(ni, tx_bytes, datalen);
		}
#endif
	}

	return skb;
bad:
	if (framelist != NULL) {
		struct sk_buff *temp;

		tskb = framelist;
		while (tskb) {
			temp = tskb->next;
			tskb->next = NULL;
			dev_kfree_skb(tskb);
			tskb = temp;
		}
	}

	if (skb != NULL) {
#ifdef ATH_SUPERG_FF
		/* FFXXX: rather specific to ff case of only 2 skbs chained */
		if (skb->next) {
			dev_kfree_skb(skb->next);
			skb->next = NULL;
		}
#endif
		dev_kfree_skb(skb);
	}
	return NULL;
#undef WH4
}
EXPORT_SYMBOL(ieee80211_encap);
#undef KEY_UNDEFINED

/*
 * Add a supported rates element id to a frame.
 */
u_int8_t *
ieee80211_add_rates(u_int8_t *frm, const struct ieee80211_rateset *rs)
{
	int nrates;

	*frm++ = IEEE80211_ELEMID_RATES;
	nrates = rs->rs_nrates;
	if (nrates > IEEE80211_RATE_SIZE)
		nrates = IEEE80211_RATE_SIZE;
	*frm++ = nrates;
	memcpy(frm, rs->rs_rates, nrates);
	return frm + nrates;
}

/*
 * Add an extended supported rates element id to a frame.
 */
u_int8_t *
ieee80211_add_xrates(u_int8_t *frm, const struct ieee80211_rateset *rs)
{
	/*
	 * Add an extended supported rates element if operating in 11g mode.
	 */
	if (rs->rs_nrates > IEEE80211_RATE_SIZE) {
		int nrates = rs->rs_nrates - IEEE80211_RATE_SIZE;
		*frm++ = IEEE80211_ELEMID_XRATES;
		*frm++ = nrates;
		memcpy(frm, rs->rs_rates + IEEE80211_RATE_SIZE, nrates);
		frm += nrates;
	}
	return frm;
}

/*
 * Add an ssid elemet to a frame.
 */
static u_int8_t *
ieee80211_add_ssid(u_int8_t *frm, const u_int8_t *ssid, u_int len)
{
	*frm++ = IEEE80211_ELEMID_SSID;
	*frm++ = len;
	memcpy(frm, ssid, len);
	return frm + len;
}

/*
 * Add an erp element to a frame.
 */
u_int8_t *
ieee80211_add_erp(u_int8_t *frm, struct ieee80211com *ic)
{
	u_int8_t erp;

	*frm++ = IEEE80211_ELEMID_ERP;
	*frm++ = 1;
	erp = 0;
	if (ic->ic_nonerpsta != 0)
		erp |= IEEE80211_ERP_NON_ERP_PRESENT;
	if (ic->ic_flags & IEEE80211_F_USEPROT)
		erp |= IEEE80211_ERP_USE_PROTECTION;
	if (ic->ic_flags & IEEE80211_F_USEBARKER)
		erp |= IEEE80211_ERP_LONG_PREAMBLE;
	*frm++ = erp;
	return frm;
}

/*
 * Add a country information element to a frame.
 */
u_int8_t *
ieee80211_add_country(u_int8_t *frm, struct ieee80211com *ic)
{
	/* add country code */
	memcpy(frm, (u_int8_t *)&ic->ic_country_ie,
		ic->ic_country_ie.country_len + 2);
	frm +=  ic->ic_country_ie.country_len + 2;
	return frm;
}

static u_int8_t *
ieee80211_setup_wpa_ie(struct ieee80211vap *vap, u_int8_t *ie)
{
#define	WPA_OUI_BYTES		0x00, 0x50, 0xf2
#define	ADDSHORT(frm, v) do {			\
	frm[0] = (v) & 0xff;			\
	frm[1] = (v) >> 8;			\
	frm += 2;				\
} while (0)
#define	ADDSELECTOR(frm, sel) do {		\
	memcpy(frm, sel, 4);			\
	frm += 4;				\
} while (0)
	static const u_int8_t oui[4] = { WPA_OUI_BYTES, WPA_OUI_TYPE };
	static const u_int8_t cipher_suite[][4] = {
		{ WPA_OUI_BYTES, WPA_CSE_WEP40 },	/* NB: 40-bit */
		{ WPA_OUI_BYTES, WPA_CSE_TKIP },
		{ 0x00, 0x00, 0x00, 0x00 },		/* XXX WRAP */
		{ WPA_OUI_BYTES, WPA_CSE_CCMP },
		{ 0x00, 0x00, 0x00, 0x00 },		/* XXX CKIP */
		{ WPA_OUI_BYTES, WPA_CSE_NULL },
	};
	static const u_int8_t wep104_suite[4] =
		{ WPA_OUI_BYTES, WPA_CSE_WEP104 };
	static const u_int8_t key_mgt_unspec[4] =
		{ WPA_OUI_BYTES, WPA_ASE_8021X_UNSPEC };
	static const u_int8_t key_mgt_psk[4] =
		{ WPA_OUI_BYTES, WPA_ASE_8021X_PSK };
	const struct ieee80211_rsnparms *rsn = &vap->iv_bss->ni_rsn;
	u_int8_t *frm = ie;
	u_int8_t *selcnt;

	*frm++ = IEEE80211_ELEMID_VENDOR;
	*frm++ = 0;				/* length filled in below */
	memcpy(frm, oui, sizeof(oui));		/* WPA OUI */
	frm += sizeof(oui);
	ADDSHORT(frm, WPA_VERSION);

	/* XXX filter out CKIP */

	/* multicast cipher */
	if (rsn->rsn_mcastcipher == IEEE80211_CIPHER_WEP &&
	    rsn->rsn_mcastkeylen >= 13)
		ADDSELECTOR(frm, wep104_suite);
	else
		ADDSELECTOR(frm, cipher_suite[rsn->rsn_mcastcipher]);