driver_nl80211.c

最新的Host AP 新添加了许多pcmcia 的驱动

	NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, addr);
	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX,
		    if_nametoindex(ifname));

	return send_and_recv_msgs(drv, msg, NULL, NULL);
 nla_put_failure:
	return -ENOBUFS;
}


static int i802_set_country(void *priv, const char *country)
{
	struct i802_driver_data *drv = priv;
	struct nl_msg *msg;
	char alpha2[3];

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	genlmsg_put(msg, 0, 0, genl_family_get_id(drv->nl80211), 0,
		    0, NL80211_CMD_REQ_SET_REG, 0);

	alpha2[0] = country[0];
	alpha2[1] = country[1];
	alpha2[2] = '\0';
	NLA_PUT_STRING(msg, NL80211_ATTR_REG_ALPHA2, alpha2);

	return send_and_recv_msgs(drv, msg, NULL, NULL);
 nla_put_failure:
	return -ENOBUFS;
}


static void handle_unknown_sta(struct hostapd_data *hapd, u8 *ta)
{
	struct sta_info *sta;

	sta = ap_get_sta(hapd, ta);
	if (!sta || !(sta->flags & WLAN_STA_ASSOC)) {
		printf("Data/PS-poll frame from not associated STA "
		       MACSTR "\n", MAC2STR(ta));
		if (sta && (sta->flags & WLAN_STA_AUTH))
			hostapd_sta_disassoc(
				hapd, ta,
				WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
		else
			hostapd_sta_deauth(
				hapd, ta,
				WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
	}
}


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

	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:
		wpa_printf(MSG_DEBUG, "MGMT (TX callback) %s",
			   ok ? "ACK" : "fail");
		ieee802_11_mgmt_cb(hapd, buf, len, stype, ok);
		break;
	case WLAN_FC_TYPE_CTRL:
		wpa_printf(MSG_DEBUG, "CTRL (TX callback) %s",
			   ok ? "ACK" : "fail");
		break;
	case WLAN_FC_TYPE_DATA:
		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;
	}
}


static void handle_frame(struct hostapd_iface *iface, u8 *buf, size_t len,
			 struct hostapd_frame_info *hfi,
			 enum ieee80211_msg_type msg_type)
{
	struct ieee80211_hdr *hdr;
	u16 fc, type, stype;
	size_t data_len = len;
	struct hostapd_data *hapd = NULL;
	int broadcast_bssid = 0;
	size_t i;
	u8 *bssid;

	/*
	 * PS-Poll frames are 16 bytes. All other frames are
	 * 24 bytes or longer.
	 */
	if (len < 16)
		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);

	switch (type) {
	case WLAN_FC_TYPE_DATA:
		if (len < 24)
			return;
		switch (fc & (WLAN_FC_FROMDS | WLAN_FC_TODS)) {
		case WLAN_FC_TODS:
			bssid = hdr->addr1;
			break;
		case WLAN_FC_FROMDS:
			bssid = hdr->addr2;
			break;
		default:
			/* discard */
			return;
		}
		break;
	case WLAN_FC_TYPE_CTRL:
		/* discard non-ps-poll frames */
		if (stype != WLAN_FC_STYPE_PSPOLL)
			return;
		bssid = hdr->addr1;
		break;
	case WLAN_FC_TYPE_MGMT:
		bssid = hdr->addr3;
		break;
	default:
		/* discard */
		return;
	}

	/* find interface frame belongs to */
	for (i = 0; i < iface->num_bss; i++) {
		if (memcmp(bssid, iface->bss[i]->own_addr, ETH_ALEN) == 0) {
			hapd = iface->bss[i];
			break;
		}
	}

	if (hapd == NULL) {
		hapd = iface->bss[0];

		if (bssid[0] != 0xff || bssid[1] != 0xff ||
		    bssid[2] != 0xff || bssid[3] != 0xff ||
		    bssid[4] != 0xff || bssid[5] != 0xff) {
			/*
			 * Unknown BSSID - drop frame if this is not from
			 * passive scanning or a beacon (at least ProbeReq
			 * frames to other APs may be allowed through RX
			 * filtering in the wlan hw/driver)
			 */
			if ((type != WLAN_FC_TYPE_MGMT ||
			     stype != WLAN_FC_STYPE_BEACON))
				return;
		} else
			broadcast_bssid = 1;
	}

	switch (msg_type) {
	case ieee80211_msg_normal:
		/* continue processing */
		break;
	case ieee80211_msg_tx_callback_ack:
		handle_tx_callback(hapd, buf, data_len, 1);
		return;
	case ieee80211_msg_tx_callback_fail:
		handle_tx_callback(hapd, buf, data_len, 0);
		return;
	}

	switch (type) {
	case WLAN_FC_TYPE_MGMT:
		if (stype != WLAN_FC_STYPE_BEACON &&
		    stype != WLAN_FC_STYPE_PROBE_REQ)
			wpa_printf(MSG_MSGDUMP, "MGMT");
		if (broadcast_bssid) {
			for (i = 0; i < iface->num_bss; i++)
				ieee802_11_mgmt(iface->bss[i], buf, data_len,
						stype, hfi);
		} else
			ieee802_11_mgmt(hapd, buf, data_len, stype, hfi);
		break;
	case WLAN_FC_TYPE_CTRL:
		/* can only get here with PS-Poll frames */
		wpa_printf(MSG_DEBUG, "CTRL");
		handle_unknown_sta(hapd, hdr->addr2);
		break;
	case WLAN_FC_TYPE_DATA:
		handle_unknown_sta(hapd, hdr->addr2);
		break;
	}
}


static void handle_eapol(int sock, void *eloop_ctx, void *sock_ctx)
{
	struct i802_driver_data *drv = eloop_ctx;
	struct hostapd_data *hapd = drv->hapd;
	struct sockaddr_ll lladdr;
	unsigned char buf[3000];
	int len;
	socklen_t fromlen = sizeof(lladdr);

	len = recvfrom(sock, buf, sizeof(buf), 0,
		       (struct sockaddr *)&lladdr, &fromlen);
	if (len < 0) {
		perror("recv");
		return;
	}

	if (have_ifidx(drv, lladdr.sll_ifindex))
		ieee802_1x_receive(hapd, lladdr.sll_addr, buf, len);
}


static void handle_monitor_read(int sock, void *eloop_ctx, void *sock_ctx)
{
	struct i802_driver_data *drv = eloop_ctx;
	int len;
	unsigned char buf[3000];
	struct hostapd_data *hapd = drv->hapd;
	struct ieee80211_radiotap_iterator iter;
	int ret;
	struct hostapd_frame_info hfi;
	int injected = 0, failed = 0, msg_type, rxflags = 0;

	len = recv(sock, buf, sizeof(buf), 0);
	if (len < 0) {
		perror("recv");
		return;
	}

	if (ieee80211_radiotap_iterator_init(&iter, (void*)buf, len)) {
		printf("received invalid radiotap frame\n");
		return;
	}

	memset(&hfi, 0, sizeof(hfi));

	while (1) {
		ret = ieee80211_radiotap_iterator_next(&iter);
		if (ret == -ENOENT)
			break;
		if (ret) {
			printf("received invalid radiotap frame (%d)\n", ret);
			return;
		}
		switch (iter.this_arg_index) {
		case IEEE80211_RADIOTAP_FLAGS:
			if (*iter.this_arg & IEEE80211_RADIOTAP_F_FCS)
				len -= 4;
			break;
		case IEEE80211_RADIOTAP_RX_FLAGS:
			rxflags = 1;
			break;
		case IEEE80211_RADIOTAP_TX_FLAGS:
			injected = 1;
			failed = le_to_host16((*(uint16_t *) iter.this_arg)) &
					IEEE80211_RADIOTAP_F_TX_FAIL;
			break;
		case IEEE80211_RADIOTAP_DATA_RETRIES:
			break;
		case IEEE80211_RADIOTAP_CHANNEL:
			/* TODO convert from freq/flags to channel number
			hfi.channel = XXX;
			hfi.phytype = XXX;
			 */
			break;
		case IEEE80211_RADIOTAP_RATE:
			hfi.datarate = *iter.this_arg * 5;
			break;
		case IEEE80211_RADIOTAP_DB_ANTSIGNAL:
			hfi.ssi_signal = *iter.this_arg;
			break;
		}
	}

	if (rxflags && injected)
		return;

	if (!injected)
		msg_type = ieee80211_msg_normal;
	else if (failed)
		msg_type = ieee80211_msg_tx_callback_fail;
	else
		msg_type = ieee80211_msg_tx_callback_ack;

	handle_frame(hapd->iface, buf + iter.max_length,
		     len - iter.max_length, &hfi, msg_type);
}


/*
 * we post-process the filter code later and rewrite
 * this to the offset to the last instruction
 */
#define PASS	0xFF
#define FAIL	0xFE

static struct sock_filter msock_filter_insns[] = {
	/*
	 * do a little-endian load of the radiotap length field
	 */
	/* load lower byte into A */
	BPF_STMT(BPF_LD  | BPF_B | BPF_ABS, 2),
	/* put it into X (== index register) */
	BPF_STMT(BPF_MISC| BPF_TAX, 0),
	/* load upper byte into A */
	BPF_STMT(BPF_LD  | BPF_B | BPF_ABS, 3),
	/* left-shift it by 8 */
	BPF_STMT(BPF_ALU | BPF_LSH | BPF_K, 8),
	/* or with X */
	BPF_STMT(BPF_ALU | BPF_OR | BPF_X, 0),
	/* put result into X */
	BPF_STMT(BPF_MISC| BPF_TAX, 0),

	/*
	 * Allow management frames through, this also gives us those
	 * management frames that we sent ourselves with status
	 */
	/* load the lower byte of the IEEE 802.11 frame control field */
	BPF_STMT(BPF_LD  | BPF_B | BPF_IND, 0),
	/* mask off frame type and version */
	BPF_STMT(BPF_ALU | BPF_AND | BPF_K, 0xF),
	/* accept frame if it's both 0, fall through otherwise */
	BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0, PASS, 0),

	/*
	 * TODO: add a bit to radiotap RX flags that indicates
	 * that the sending station is not associated, then
	 * add a filter here that filters on our DA and that flag
	 * to allow us to deauth frames to that bad station.
	 *
	 * Not a regression -- we didn't do it before either.
	 */

#if 0
	/*
	 * drop non-data frames, WDS frames
	 */
	/* load the lower byte of the frame control field */
	BPF_STMT(BPF_LD   | BPF_B | BPF_IND, 0),
	/* mask off QoS bit */
	BPF_STMT(BPF_ALU  | BPF_AND | BPF_K, 0x0c),
	/* drop non-data frames */
	BPF_JUMP(BPF_JMP  | BPF_JEQ | BPF_K, 8, 0, FAIL),
	/* load the upper byte of the frame control field */
	BPF_STMT(BPF_LD   | BPF_B | BPF_IND, 0),
	/* mask off toDS/fromDS */
	BPF_STMT(BPF_ALU  | BPF_AND | BPF_K, 0x03),
	/* drop WDS frames */
	BPF_JUMP(BPF_JMP  | BPF_JEQ | BPF_K, 3, FAIL, 0),
#endif

	/*
	 * add header length to index
	 */
	/* load the lower byte of the frame control field */
	BPF_STMT(BPF_LD   | BPF_B | BPF_IND, 0),
	/* mask off QoS bit */
	BPF_STMT(BPF_ALU  | BPF_AND | BPF_K, 0x80),
	/* right shift it by 6 to give 0 or 2 */
	BPF_STMT(BPF_ALU  | BPF_RSH | BPF_K, 6),
	/* add data frame header length */
	BPF_STMT(BPF_ALU  | BPF_ADD | BPF_K, 24),
	/* add index, was start of 802.11 header */
	BPF_STMT(BPF_ALU  | BPF_ADD | BPF_X, 0),
	/* move to index, now start of LL header */
	BPF_STMT(BPF_MISC | BPF_TAX, 0),

	/*
	 * Accept empty data frames, we use those for
	 * polling activity.
	 */
	BPF_STMT(BPF_LD  | BPF_W | BPF_LEN, 0),
	BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_X, 0, PASS, 0),

	/*
	 * Accept EAPOL frames
	 */
	BPF_STMT(BPF_LD  | BPF_W | BPF_IND, 0),
	BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0xAAAA0300, 0, FAIL),
	BPF_STMT(BPF_LD  | BPF_W | BPF_IND, 4),
	BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 0x0000888E, PASS, FAIL),

	/* keep these last two statements or change the code below */
	/* return 0 == "DROP" */
	BPF_STMT(BPF_RET | BPF_K, 0),
	/* return ~0 == "keep all" */
	BPF_STMT(BPF_RET | BPF_K, ~0),
};

static struct sock_fprog msock_filter = {
	.len = sizeof(msock_filter_insns)/sizeof(msock_filter_insns[0]),
	.filter = msock_filter_insns,
};


static int add_monitor_filter(int s)
{
	int idx;

	/* rewrite all PASS/FAIL jump offsets */
	for (idx = 0; idx < msock_filter.len; idx++) {
		struct sock_filter *insn = &msock_filter_insns[idx];

		if (BPF_CLASS(insn->code) == BPF_JMP) {
			if (insn->code == (BPF_JMP|BPF_JA)) {
				if (insn->k == PASS)
					insn->k = msock_filter.len - idx - 2;
				else if (insn->k == FAIL)
					insn->k = msock_filter.len - idx - 3;
			}

			if (insn->jt == PASS)
				insn->jt = msock_filter.len - idx - 2;
			else if (insn->jt == FAIL)
				insn->jt = msock_filter.len - idx - 3;

			if (insn->jf == PASS)
				insn->jf = msock_filter.len - idx - 2;
			else if (insn->jf == FAIL)
				insn->jf = msock_filter.len - idx - 3;
		}
	}

	if (setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER,
		       &msock_filter, sizeof(msock_filter))) {
		perror("SO_ATTACH_FILTER");
		return -1;
	}

	return 0;
}


static int nl80211_create_monitor_interface(struct i802_driver_data *drv)
{
	char buf[IFNAMSIZ];
	struct sockaddr_ll ll;
	int optval;
	socklen_t optlen;

	snprintf(buf, IFNAMSIZ, "mon.%s", drv->iface);
	buf[IFNAMSIZ - 1] = '\0';

	drv->monitor_ifidx =
		nl80211_create_iface(drv, buf, NL80211_IFTYPE_MONITOR, NULL);

	if (drv->monitor_ifidx < 0)
		return -1;

	if (hostapd_set_iface_flags(drv, buf, 1))
		goto error;

	memset(&ll, 0, sizeof(ll));
	ll.sll_family = AF_PACKET;
	ll.sll_ifindex = drv->monitor_ifidx;
	drv->monitor_sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
	if (drv->monitor_sock < 0) {
		perror("socket[PF_PACKET,SOCK_RAW]");
		goto error;
	}

	if (add_monitor_filter(drv->monitor_sock)) {
		wpa_printf(MSG_INFO, "Failed to set socket filter for monitor "
			   "interface; do filtering in user space");
		/* This works, but will cost in performance. */
	}

	if (bind(drv->monitor_sock, (struct sockaddr *) &ll,
		 sizeof(ll)) < 0) {
		perror("monitor socket bind");
		goto error;
	}

	optlen = sizeof(optval);
	optval = 20;
	if (setsockopt
	    (drv->monitor_sock, SOL_SOCKET, SO_PRIORITY, &optval, optlen)) {
		perror("Failed to set socket priority");
		goto error;
	}

	if (eloop_register_read_sock(drv->monitor_sock, handle_monitor_read,
				     drv, NULL)) {
		printf("Could not register monitor read socket\n");
		goto error;
	}

	return 0;
 error:
	nl80211_remove_iface(drv, drv->monitor_ifidx);
	return -1;
}


static int nl80211_set_master_mode(struct i802_driver_data *drv,
				   const char *ifname)
{
	struct nl_msg *msg;
	int ret = -ENOBUFS;

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	genlmsg_put(msg, 0, 0, genl_family_get_id(drv->nl80211), 0,
		    0, NL80211_CMD_SET_INTERFACE, 0);
	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX,
		    if_nametoindex(ifname));
	NLA_PUT_U32(msg, NL80211_ATTR_IFTYPE, NL80211_IFTYPE_AP);

	ret = send_and_recv_msgs(drv, msg, NULL, NULL);
	if (!ret)
		return 0;
 nla_put_failure:
	wpa_printf(MSG_ERROR, "Failed to set interface %s to master "
		   "mode.", ifname);