C++ WLAN_FC_GET_TYPE函数代码示例

static void ccmp_aad_nonce(const struct ieee80211_hdr *hdr, const u8 *data,
			   u8 *aad, size_t *aad_len, u8 *nonce)
{
	u16 fc, stype, seq;
	int qos = 0, addr4 = 0;
	u8 *pos;

	nonce[0] = 0;

	fc = le_to_host16(hdr->frame_control);
	stype = WLAN_FC_GET_STYPE(fc);
	if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
	    (WLAN_FC_TODS | WLAN_FC_FROMDS))
		addr4 = 1;

	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA) {
		fc &= ~0x0070; /* Mask subtype bits */
		if (stype & 0x08) {
			const u8 *qc;
			qos = 1;
			fc &= ~WLAN_FC_ORDER;
			qc = (const u8 *) (hdr + 1);
			if (addr4)
				qc += ETH_ALEN;
			nonce[0] = qc[0] & 0x0f;
		}
	} else if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT)
		nonce[0] |= 0x10; /* Management */

	fc &= ~(WLAN_FC_RETRY | WLAN_FC_PWRMGT | WLAN_FC_MOREDATA);
	fc |= WLAN_FC_ISWEP;
	WPA_PUT_LE16(aad, fc);
	pos = aad + 2;
	os_memcpy(pos, hdr->addr1, 3 * ETH_ALEN);
	pos += 3 * ETH_ALEN;
	seq = le_to_host16(hdr->seq_ctrl);
	seq &= ~0xfff0; /* Mask Seq#; do not modify Frag# */
	WPA_PUT_LE16(pos, seq);
	pos += 2;

	os_memcpy(pos, hdr + 1, addr4 * ETH_ALEN + qos * 2);
	pos += addr4 * ETH_ALEN;
	if (qos) {
		pos[0] &= ~0x70;
		if (1 /* FIX: either device has SPP A-MSDU Capab = 0 */)
			pos[0] &= ~0x80;
		pos++;
		*pos++ = 0x00;
	}

	*aad_len = pos - aad;

	os_memcpy(nonce + 1, hdr->addr2, ETH_ALEN);
	nonce[7] = data[7]; /* PN5 */
	nonce[8] = data[6]; /* PN4 */
	nonce[9] = data[5]; /* PN3 */
	nonce[10] = data[4]; /* PN2 */
	nonce[11] = data[1]; /* PN1 */
	nonce[12] = data[0]; /* PN0 */
}

static void test_driver_mlme(struct test_driver_data *drv,
			     struct sockaddr_un *from, socklen_t fromlen,
			     u8 *data, size_t datalen)
{
	struct ieee80211_hdr *hdr;
	u16 fc;

	hdr = (struct ieee80211_hdr *) data;

	if (test_driver_get_cli(drv, from, fromlen) == NULL && datalen >= 16) {
		struct test_client_socket *cli;
		cli = os_zalloc(sizeof(*cli));
		if (cli == NULL)
			return;
		wpa_printf(MSG_DEBUG, "Adding client entry for " MACSTR,
			   MAC2STR(hdr->addr2));
		memcpy(cli->addr, hdr->addr2, ETH_ALEN);
		memcpy(&cli->un, from, sizeof(cli->un));
		cli->unlen = fromlen;
		cli->next = drv->cli;
		drv->cli = cli;
	}

	wpa_hexdump(MSG_MSGDUMP, "test_driver_mlme: received frame",
		    data, datalen);
	fc = le_to_host16(hdr->frame_control);
	if (WLAN_FC_GET_TYPE(fc) != WLAN_FC_TYPE_MGMT) {
		wpa_printf(MSG_ERROR, "%s: received non-mgmt frame",
			   __func__);
		return;
	}
	ieee802_11_mgmt(drv->hapd, data, datalen, WLAN_FC_GET_STYPE(fc), NULL);
}

/* Filters received packets. The function returns 1 if the packet should be
 * forwarded to ieee80211_rx(). If the packet should be ignored the function
 * returns 0. If an invalid packet is found the function returns -EINVAL.
 *
 * The function calls ieee80211_rx_mgt() directly.
 *
 * It has been based on ieee80211_rx_any.
 */
static int filter_rx(struct ieee80211_device *ieee,
	             const u8 *buffer, unsigned int length,
		     struct ieee80211_rx_stats *stats)
{
	struct ieee80211_hdr_4addr *hdr;
	u16 fc;

	if (ieee->iw_mode == IW_MODE_MONITOR)
		return 1;

	hdr = (struct ieee80211_hdr_4addr *)buffer;
	fc = le16_to_cpu(hdr->frame_ctl);
	if ((fc & IEEE80211_FCTL_VERS) != 0)
		return -EINVAL;

	switch (WLAN_FC_GET_TYPE(fc)) {
	case IEEE80211_FTYPE_MGMT:
		if (length < sizeof(struct ieee80211_hdr_3addr))
			return -EINVAL;
		ieee80211_rx_mgt(ieee, hdr, stats);
		return 0;
	case IEEE80211_FTYPE_CTL:
		return 0;
	case IEEE80211_FTYPE_DATA:
		/* Ignore invalid short buffers */
		if (length < sizeof(struct ieee80211_hdr_3addr))
			return -EINVAL;
		return is_data_packet_for_us(ieee, hdr);
	}

	return -EINVAL;
}

int ieee80211_get_hdrlen(u16 fc)
{
	int hdrlen = 24;

	switch (WLAN_FC_GET_TYPE(fc)) {
	case RTW_IEEE80211_FTYPE_DATA:
		if (fc & RTW_IEEE80211_STYPE_QOS_DATA)
			hdrlen += 2;
		if ((fc & RTW_IEEE80211_FCTL_FROMDS) && (fc & RTW_IEEE80211_FCTL_TODS))
			hdrlen += 6; /* Addr4 */
		break;
	case RTW_IEEE80211_FTYPE_CTL:
		switch (WLAN_FC_GET_STYPE(fc)) {
		case RTW_IEEE80211_STYPE_CTS:
		case RTW_IEEE80211_STYPE_ACK:
			hdrlen = 10;
			break;
		default:
			hdrlen = 16;
			break;
		}
		break;
	}

	return hdrlen;
}

static void handle_tx_callback(hostapd *hapd, char *buf, size_t len, int ok)
{
	struct ieee80211_hdr *hdr;
	u16 fc, type, stype;
	hdr = (struct ieee80211_hdr *) buf;
	fc = le_to_host16(hdr->frame_control);

	type = WLAN_FC_GET_TYPE(fc);
	stype = WLAN_FC_GET_STYPE(fc);

	switch (type) {
	case WLAN_FC_TYPE_MGMT:
		HOSTAPD_DEBUG(HOSTAPD_DEBUG_MINIMAL,
			      "MGMT (TX callback) %s\n", ok ? "ACK" : "fail");
		ieee802_11_mgmt_cb(hapd, buf, len, stype, ok);
		break;
	case WLAN_FC_TYPE_CTRL:
		HOSTAPD_DEBUG(HOSTAPD_DEBUG_MINIMAL,
			      "CTRL (TX callback) %s\n", ok ? "ACK" : "fail");
		break;
	case WLAN_FC_TYPE_DATA:
		HOSTAPD_DEBUG(HOSTAPD_DEBUG_MINIMAL,
			      "DATA (TX callback) %s\n", ok ? "ACK" : "fail");
		/* TODO: could replace TXExc counter hack with kernel driver
		 * in data polling for inactivity check with the new TX
		 * callback.. */
		/* handle_data_cb(hapd, buf, len, stype, ok); */
		break;
	default:
		printf("unknown TX callback frame type %d\n", type);
		break;
	}
}

static void atheros_raw_receive(void *ctx, const u8 *src_addr, const u8 *buf,
				size_t len)
{
	struct atheros_driver_data *drv = ctx;
	const struct ieee80211_mgmt *mgmt;
	u16 fc;
	union wpa_event_data event;

	/* Send Probe Request information to WPS processing */

	if (len < IEEE80211_HDRLEN + sizeof(mgmt->u.probe_req))
		return;
	mgmt = (const struct ieee80211_mgmt *) buf;

	fc = le_to_host16(mgmt->frame_control);
	if (WLAN_FC_GET_TYPE(fc) != WLAN_FC_TYPE_MGMT ||
	    WLAN_FC_GET_STYPE(fc) != WLAN_FC_STYPE_PROBE_REQ)
		return;

	os_memset(&event, 0, sizeof(event));
	event.rx_probe_req.sa = mgmt->sa;
	event.rx_probe_req.da = mgmt->da;
	event.rx_probe_req.bssid = mgmt->bssid;
	event.rx_probe_req.ie = mgmt->u.probe_req.variable;
	event.rx_probe_req.ie_len =
		len - (IEEE80211_HDRLEN + sizeof(mgmt->u.probe_req));
	wpa_supplicant_event(drv->hapd, EVENT_RX_PROBE_REQ, &event);
}

void hostap_dump_rx_header(const char *name, const struct hfa384x_rx_frame *rx)
{
	u16 status, fc;

	status = __le16_to_cpu(rx->status);

	printk(KERN_DEBUG "%s: RX status=0x%04x (port=%d, type=%d, "
	       "fcserr=%d) silence=%d signal=%d rate=%d rxflow=%d; "
	       "jiffies=%ld\n",
	       name, status, (status >> 8) & 0x07, status >> 13, status & 1,
	       rx->silence, rx->signal, rx->rate, rx->rxflow, jiffies);

	fc = __le16_to_cpu(rx->frame_control);
	printk(KERN_DEBUG "   FC=0x%04x (type=%d:%d) dur=0x%04x seq=0x%04x "
	       "data_len=%d%s%s\n",
	       fc, WLAN_FC_GET_TYPE(fc), WLAN_FC_GET_STYPE(fc),
	       __le16_to_cpu(rx->duration_id), __le16_to_cpu(rx->seq_ctrl),
	       __le16_to_cpu(rx->data_len),
	       fc & WLAN_FC_TODS ? " [ToDS]" : "",
	       fc & WLAN_FC_FROMDS ? " [FromDS]" : "");

	printk(KERN_DEBUG "   A1=" MACSTR " A2=" MACSTR " A3=" MACSTR " A4="
	       MACSTR "\n",
	       MAC2STR(rx->addr1), MAC2STR(rx->addr2), MAC2STR(rx->addr3),
	       MAC2STR(rx->addr4));

	printk(KERN_DEBUG "   dst=" MACSTR " src=" MACSTR " len=%d\n",
	       MAC2STR(rx->dst_addr), MAC2STR(rx->src_addr),
	       __be16_to_cpu(rx->len));
}

void hostap_dump_tx_header(const char *name, const struct hfa384x_tx_frame *tx)
{
	u16 fc;

	printk(KERN_DEBUG "%s: TX status=0x%04x retry_count=%d tx_rate=%d "
	       "tx_control=0x%04x; jiffies=%ld\n",
	       name, __le16_to_cpu(tx->status), tx->retry_count, tx->tx_rate,
	       __le16_to_cpu(tx->tx_control), jiffies);

	fc = __le16_to_cpu(tx->frame_control);
	printk(KERN_DEBUG "   FC=0x%04x (type=%d:%d) dur=0x%04x seq=0x%04x "
	       "data_len=%d%s%s\n",
	       fc, WLAN_FC_GET_TYPE(fc), WLAN_FC_GET_STYPE(fc),
	       __le16_to_cpu(tx->duration_id), __le16_to_cpu(tx->seq_ctrl),
	       __le16_to_cpu(tx->data_len),
	       fc & WLAN_FC_TODS ? " [ToDS]" : "",
	       fc & WLAN_FC_FROMDS ? " [FromDS]" : "");

	printk(KERN_DEBUG "   A1=" MACSTR " A2=" MACSTR " A3=" MACSTR " A4="
	       MACSTR "\n",
	       MAC2STR(tx->addr1), MAC2STR(tx->addr2), MAC2STR(tx->addr3),
	       MAC2STR(tx->addr4));

	printk(KERN_DEBUG "   dst=" MACSTR " src=" MACSTR " len=%d\n",
	       MAC2STR(tx->dst_addr), MAC2STR(tx->src_addr),
	       __be16_to_cpu(tx->len));
}

static void handle_frame(struct wpa_driver_nl80211_data *drv,
			 u8 *buf, size_t len, int datarate, int ssi_signal)
{
	struct ieee80211_hdr *hdr;
	u16 fc;
	union wpa_event_data event;

	hdr = (struct ieee80211_hdr *) buf;
	fc = le_to_host16(hdr->frame_control);

	switch (WLAN_FC_GET_TYPE(fc)) {
	case WLAN_FC_TYPE_MGMT:
		os_memset(&event, 0, sizeof(event));
		event.rx_mgmt.frame = buf;
		event.rx_mgmt.frame_len = len;
		event.rx_mgmt.datarate = datarate;
		event.rx_mgmt.ssi_signal = ssi_signal;
		wpa_supplicant_event(drv->ctx, EVENT_RX_MGMT, &event);
		break;
	case WLAN_FC_TYPE_CTRL:
		/* can only get here with PS-Poll frames */
		wpa_printf(MSG_DEBUG, "CTRL");
		from_unknown_sta(drv, buf, len);
		break;
	case WLAN_FC_TYPE_DATA:
		from_unknown_sta(drv, buf, len);
		break;
	}
}

static void madwifi_raw_receive(void *ctx, const u8 *src_addr, const u8 *buf,
				size_t len)
{
	struct madwifi_driver_data *drv = ctx;
	const struct ieee80211_mgmt *mgmt;
	const u8 *end, *ie;
	u16 fc;
	size_t ie_len;

	/* Send Probe Request information to WPS processing */

	if (len < IEEE80211_HDRLEN + sizeof(mgmt->u.probe_req))
		return;
	mgmt = (const struct ieee80211_mgmt *) buf;

	fc = le_to_host16(mgmt->frame_control);
	if (WLAN_FC_GET_TYPE(fc) != WLAN_FC_TYPE_MGMT ||
	    WLAN_FC_GET_STYPE(fc) != WLAN_FC_STYPE_PROBE_REQ)
		return;

	end = buf + len;
	ie = mgmt->u.probe_req.variable;
	ie_len = len - (IEEE80211_HDRLEN + sizeof(mgmt->u.probe_req));

	hostapd_wps_probe_req_rx(drv->hapd, mgmt->sa, ie, ie_len);
}

/* Returns 1 if the data packet is for us and 0 otherwise. */
static int is_data_packet_for_us(struct ieee80211_device *ieee,
	                         struct ieee80211_hdr_4addr *hdr)
{
	struct net_device *netdev = ieee->dev;
	u16 fc = le16_to_cpu(hdr->frame_ctl);

	ZD_ASSERT(WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA);

	switch (ieee->iw_mode) {
	case IW_MODE_ADHOC:
		if ((fc & (IEEE80211_FCTL_TODS|IEEE80211_FCTL_FROMDS)) != 0 ||
		    memcmp(hdr->addr3, ieee->bssid, ETH_ALEN) != 0)
			return 0;
		break;
	case IW_MODE_AUTO:
	case IW_MODE_INFRA:
		if ((fc & (IEEE80211_FCTL_TODS|IEEE80211_FCTL_FROMDS)) !=
		    IEEE80211_FCTL_FROMDS ||
		    memcmp(hdr->addr2, ieee->bssid, ETH_ALEN) != 0)
			return 0;
		break;
	default:
		ZD_ASSERT(ieee->iw_mode != IW_MODE_MONITOR);
		return 0;
	}

	return memcmp(hdr->addr1, netdev->dev_addr, ETH_ALEN) == 0 ||
	       (is_multicast_ether_addr(hdr->addr1) &&
		memcmp(hdr->addr3, netdev->dev_addr, ETH_ALEN) != 0) ||
	       (netdev->flags & IFF_PROMISC);
}

static void handle_frame(struct hostap_driver_data *drv, u8 *buf, size_t len)
{
	struct ieee80211_hdr *hdr;
	u16 fc, type, stype;
	size_t data_len = len;
	int ver;
	union wpa_event_data event;

	/* PSPOLL is only 16 bytes, but driver does not (at least yet) pass
	 * these to user space */
	if (len < 24) {
		wpa_printf(MSG_MSGDUMP, "handle_frame: too short (%lu)",
			   (unsigned long) len);
		return;
	}

	hdr = (struct ieee80211_hdr *) buf;
	fc = le_to_host16(hdr->frame_control);
	type = WLAN_FC_GET_TYPE(fc);
	stype = WLAN_FC_GET_STYPE(fc);

	if (type != WLAN_FC_TYPE_MGMT || stype != WLAN_FC_STYPE_BEACON) {
		wpa_hexdump(MSG_MSGDUMP, "Received management frame",
			    buf, len);
	}

	ver = fc & WLAN_FC_PVER;

	/* protocol version 2 is reserved for indicating ACKed frame (TX
	 * callbacks), and version 1 for indicating failed frame (no ACK, TX
	 * callbacks) */
	if (ver == 1 || ver == 2) {
		handle_tx_callback(drv, buf, data_len, ver == 2 ? 1 : 0);
		return;
	} else if (ver != 0) {
		printf("unknown protocol version %d\n", ver);
		return;
	}

	switch (type) {
	case WLAN_FC_TYPE_MGMT:
		os_memset(&event, 0, sizeof(event));
		event.rx_mgmt.frame = buf;
		event.rx_mgmt.frame_len = data_len;
		wpa_supplicant_event(drv->hapd, EVENT_RX_MGMT, &event);
		break;
	case WLAN_FC_TYPE_CTRL:
		wpa_printf(MSG_DEBUG, "CTRL");
		break;
	case WLAN_FC_TYPE_DATA:
		wpa_printf(MSG_DEBUG, "DATA");
		handle_data(drv, buf, data_len, stype);
		break;
	default:
		wpa_printf(MSG_DEBUG, "unknown frame type %d", type);
		break;
	}
}

static int hostapd_mgmt_rx(struct hostapd_data *hapd, struct rx_mgmt *rx_mgmt)
{
	struct hostapd_iface *iface = hapd->iface;
	const struct ieee80211_hdr *hdr;
	const u8 *bssid;
	struct hostapd_frame_info fi;
	int ret;

	hdr = (const struct ieee80211_hdr *) rx_mgmt->frame;
	bssid = get_hdr_bssid(hdr, rx_mgmt->frame_len);
	if (bssid == NULL)
		return 0;

	hapd = get_hapd_bssid(iface, bssid);
	if (hapd == NULL) {
		u16 fc;
		fc = le_to_host16(hdr->frame_control);

		/*
		 * Drop frames to unknown BSSIDs except for Beacon frames which
		 * could be used to update neighbor information.
		 */
		if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT &&
		    WLAN_FC_GET_STYPE(fc) == WLAN_FC_STYPE_BEACON)
			hapd = iface->bss[0];
		else
			return 0;
	}

	os_memset(&fi, 0, sizeof(fi));
	fi.datarate = rx_mgmt->datarate;
	fi.ssi_signal = rx_mgmt->ssi_signal;

	if (hapd == HAPD_BROADCAST) {
		size_t i;
		ret = 0;
		for (i = 0; i < iface->num_bss; i++) {
			if (ieee802_11_mgmt(iface->bss[i], rx_mgmt->frame,
					    rx_mgmt->frame_len, &fi) > 0)
				ret = 1;
		}
	} else
		ret = ieee802_11_mgmt(hapd, rx_mgmt->frame, rx_mgmt->frame_len,
				      &fi);

	random_add_randomness(&fi, sizeof(fi));

	return ret;
}

static void rtl871x_handle_tx_callback(struct hostapd_data *hapd, u8 *buf, size_t len,
			       int ok)
{
#if 0
	struct ieee80211_hdr *hdr;
	u16 fc, type, stype;
	struct sta_info *sta;

	//printf("%s\n", __func__);

	hdr = (struct ieee80211_hdr *) buf;
	fc = le_to_host16(hdr->frame_control);

	type = WLAN_FC_GET_TYPE(fc);
	stype = WLAN_FC_GET_STYPE(fc);

	switch (type) {
	case WLAN_FC_TYPE_MGMT:
		//printf("MGMT (TX callback) %s\n",
		//	   ok ? "ACK" : "fail");
		ieee802_11_mgmt_cb(hapd, buf, len, stype, ok);
		break;
	case WLAN_FC_TYPE_CTRL:
		printf("CTRL (TX callback) %s\n",
			   ok ? "ACK" : "fail");
		break;
	case WLAN_FC_TYPE_DATA:
		printf("DATA (TX callback) %s\n",
			   ok ? "ACK" : "fail");
		sta = ap_get_sta(hapd, hdr->addr1);
		if (sta && sta->flags & WLAN_STA_PENDING_POLL) {
			wpa_printf(MSG_DEBUG, "STA " MACSTR
				   " %s pending activity poll",
				   MAC2STR(sta->addr),
				   ok ? "ACKed" : "did not ACK");
			if (ok)
				sta->flags &= ~WLAN_STA_PENDING_POLL;
		}
		if (sta)
			ieee802_1x_tx_status(hapd, sta, buf, len, ok);
		break;
	default:
		printf("unknown TX callback frame type %d\n", type);
		break;
	}
#endif	
}	

static void cs_set_control(struct zd_mac *mac, struct zd_ctrlset *cs,
	                   struct ieee80211_hdr_4addr *header)
{
	struct ieee80211softmac_device *softmac = ieee80211_priv(mac->netdev);
	unsigned int tx_length = le16_to_cpu(cs->tx_length);
	u16 fctl = le16_to_cpu(header->frame_ctl);
	u16 ftype = WLAN_FC_GET_TYPE(fctl);
	u16 stype = WLAN_FC_GET_STYPE(fctl);

	/*
	 * CONTROL TODO:
	 * - if backoff needed, enable bit 0
	 * - if burst (backoff not needed) disable bit 0
	 */

	cs->control = 0;

	/* First fragment */
	if (WLAN_GET_SEQ_FRAG(le16_to_cpu(header->seq_ctl)) == 0)
		cs->control |= ZD_CS_NEED_RANDOM_BACKOFF;

	/* Multicast */
	if (is_multicast_ether_addr(header->addr1))
		cs->control |= ZD_CS_MULTICAST;

	/* PS-POLL */
	if (stype == IEEE80211_STYPE_PSPOLL)
		cs->control |= ZD_CS_PS_POLL_FRAME;

	/* Unicast data frames over the threshold should have RTS */
	if (!is_multicast_ether_addr(header->addr1) &&
	    	ftype != IEEE80211_FTYPE_MGMT &&
		    tx_length > zd_netdev_ieee80211(mac->netdev)->rts)
		cs->control |= ZD_CS_RTS;

	/* Use CTS-to-self protection if required */
	if (ZD_CS_TYPE(cs->modulation) == ZD_CS_OFDM &&
			ieee80211softmac_protection_needed(softmac)) {
		/* FIXME: avoid sending RTS *and* self-CTS, is that correct? */
		cs->control &= ~ZD_CS_RTS;
		cs->control |= ZD_CS_SELF_CTS;
	}

	/* FIXME: Management frame? */
}