driver_prism54.c

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

/* set attachments */
static int prism54_set_generic_elem(const char *ifname, void *priv,
				    const u8 *elem, size_t elem_len)
{
	struct prism54_driver_data *drv = priv;
	pimdev_hdr *hdr;
	char *pos;
	struct obj_attachment_hdr *attach;
	size_t blen = sizeof(*hdr) + sizeof(*attach) + elem_len;
	hdr = os_zalloc(blen);
	if (hdr == NULL) {
		printf("%s: memory low\n", __func__);
		return -1;
	}
	hdr->op = htonl(PIMOP_SET);
	hdr->oid = htonl(DOT11_OID_ATTACHMENT);
	attach = (struct obj_attachment_hdr *)&hdr[1];
	attach->type = DOT11_PKT_BEACON;
	attach->id = -1;
	attach->size = host_to_le16((short)elem_len);
	pos = ((char*) attach) + sizeof(*attach);
	if (elem)
		memcpy(pos, elem, elem_len);
	send(drv->pim_sock, hdr, blen, 0);
	attach->type = DOT11_PKT_PROBE_RESP;
	send(drv->pim_sock, hdr, blen, 0);
	free(hdr);
	return 0;
}


/* tell the card to auth the sta */
static void prism54_handle_probe(struct prism54_driver_data *drv,
				 void *buf, size_t len)
{
	struct obj_mlmeex *mlme;
	pimdev_hdr *hdr;
	struct sta_info *sta;
	hdr = (pimdev_hdr *)buf;
	mlme = (struct obj_mlmeex *) &hdr[1];
	sta = ap_get_sta(drv->hapd, (u8 *) &mlme->address[0]);
	if (sta != NULL) {
		if (sta->flags & (WLAN_STA_AUTH | WLAN_STA_ASSOC))
			return;
	}
	if (len < sizeof(*mlme)) {
		printf("bad probe packet\n");
		return;
	}
	mlme->state = htons(DOT11_STATE_AUTHING);
	mlme->code = 0;
	hdr->op = htonl(PIMOP_SET);
	hdr->oid = htonl(DOT11_OID_AUTHENTICATEEX);
	mlme->size = 0;
	send(drv->pim_sock, hdr, sizeof(*hdr)+sizeof(*mlme), 0);
}


static void prism54_handle_deauth(struct prism54_driver_data *drv,
				  void *buf, size_t len)
{
	struct obj_mlme *mlme;
	pimdev_hdr *hdr;
	struct sta_info *sta;
	char *mac_id;

	hdr = (pimdev_hdr *) buf;
	mlme = (struct obj_mlme *) &hdr[1];
	sta = ap_get_sta(drv->hapd, (u8 *) &mlme->address[0]);
	mac_id = mac_id_get(drv, mlme->id);
	if (sta == NULL || mac_id == NULL)
		return;
	memcpy(&mlme->address[0], mac_id, ETH_ALEN);
	sta->flags &= ~(WLAN_STA_AUTH | WLAN_STA_ASSOC);
	wpa_auth_sm_event(sta->wpa_sm, WPA_DEAUTH);
	sta->acct_terminate_cause = RADIUS_ACCT_TERMINATE_CAUSE_USER_REQUEST;
	ieee802_1x_notify_port_enabled(sta->eapol_sm, 0);
	ap_free_sta(drv->hapd, sta);
}


static void prism54_handle_disassoc(struct prism54_driver_data *drv,
				    void *buf, size_t len)
{
	struct obj_mlme *mlme;
	pimdev_hdr *hdr;
	struct sta_info *sta;
	char *mac_id;

	hdr = (pimdev_hdr *) buf;
	mlme = (struct obj_mlme *) &hdr[1];
	mac_id = mac_id_get(drv, mlme->id);
	if (mac_id == NULL)
		return;
	memcpy(&mlme->address[0], mac_id, ETH_ALEN);
	sta = ap_get_sta(drv->hapd, (u8 *) &mlme->address[0]);
	if (sta == NULL) {
		return;
	}
	sta->flags &= ~WLAN_STA_ASSOC;
	wpa_auth_sm_event(sta->wpa_sm, WPA_DISASSOC);
	sta->acct_terminate_cause = RADIUS_ACCT_TERMINATE_CAUSE_USER_REQUEST;
	ieee802_1x_notify_port_enabled(sta->eapol_sm, 0);
	accounting_sta_stop(drv->hapd, sta);
	ieee802_1x_free_station(sta);
}


/* to auth it, just allow it now, later for os/sk */
static void prism54_handle_auth(struct prism54_driver_data *drv,
				void *buf, size_t len)
{
	struct obj_mlmeex *mlme;
	pimdev_hdr *hdr;
	struct sta_info *sta;
	int resp;

	hdr = (pimdev_hdr *) buf;
	mlme = (struct obj_mlmeex *) &hdr[1];
	if (len < sizeof(*mlme)) {
		printf("bad auth packet\n");
		return;
	}

	if (mlme->state == htons(DOT11_STATE_AUTHING)) {
		sta = ap_sta_add(drv->hapd, (u8 *) &mlme->address[0]);
		if (drv->hapd->tkip_countermeasures) {
			resp = WLAN_REASON_MICHAEL_MIC_FAILURE;
			goto fail;
		}
		mac_id_refresh(drv, mlme->id, &mlme->address[0]);
		if (!sta) {
			resp = WLAN_STATUS_UNSPECIFIED_FAILURE;
			goto fail;
		}
		sta->flags &= ~WLAN_STA_PREAUTH;
		
		ieee802_1x_notify_pre_auth(sta->eapol_sm, 0);
		sta->flags |= WLAN_STA_AUTH;
		wpa_auth_sm_event(sta->wpa_sm, WPA_AUTH);
		mlme->code = 0;
		mlme->state=htons(DOT11_STATE_AUTH);
		hdr->op = htonl(PIMOP_SET);
		hdr->oid = htonl(DOT11_OID_AUTHENTICATEEX);
		mlme->size = 0;
		sta->timeout_next = STA_NULLFUNC;
		send(drv->pim_sock, hdr, sizeof(*hdr) + sizeof(*mlme), 0);
	}
	return;

fail:
	printf("auth fail: %x\n", resp);
	mlme->code = host_to_le16(resp);
	mlme->size = 0;
	if (sta)
		sta->flags &= ~(WLAN_STA_AUTH | WLAN_STA_ASSOC);
	hdr->oid = htonl(DOT11_OID_DEAUTHENTICATEEX);
	hdr->op = htonl(PIMOP_SET);
	send(drv->pim_sock, hdr, sizeof(*hdr)+sizeof(*mlme), 0);
}


/* do the wpa thing */
static void prism54_handle_assoc(struct prism54_driver_data *drv,
				 void *buf, size_t len)
{
	pimdev_hdr *hdr;
	struct obj_mlmeex *mlme;
	struct ieee802_11_elems elems;
	struct sta_info *sta;
	u8 *wpa_ie;
	u8 *cb;
	int ieofs = 0;
	size_t wpa_ie_len;
	int resp, new_assoc;
	char *mac_id;

	resp = 0;
	hdr = (pimdev_hdr *) buf;
	mlme = (struct obj_mlmeex *) &hdr[1];
	switch (ntohl(hdr->oid)) {
		case DOT11_OID_ASSOCIATE:
		case DOT11_OID_REASSOCIATE:
			mlme->size = 0;
		default:
			break;
	}
	if ((mlme->state == (int) htonl(DOT11_STATE_ASSOCING)) ||
	    (mlme->state == (int) htonl(DOT11_STATE_REASSOCING))) {
		if (len < sizeof(pimdev_hdr) + sizeof(struct obj_mlme)) {
			printf("bad assoc packet\n");
			return;
		}
		mac_id = mac_id_get(drv, mlme->id);
		if (mac_id == NULL)
			return;
		memcpy(&mlme->address[0], mac_id, ETH_ALEN);
		sta = ap_get_sta(drv->hapd, (u8 *) &mlme->address[0]);
		if (sta == NULL) {
			printf("cannot get sta\n");
			return;
		}
		cb = (u8 *) &mlme->data[0];
		if (hdr->oid == htonl(DOT11_OID_ASSOCIATEEX)) {
			ieofs = 4;
		} else if (hdr->oid == htonl(DOT11_OID_REASSOCIATEEX)) {
			ieofs = 10;
		}
		if (le_to_host16(mlme->size) <= ieofs) {
			printf("attach too small\n");
			resp = WLAN_STATUS_UNSPECIFIED_FAILURE;
			goto fail;
		}
		if (ieee802_11_parse_elems(cb + ieofs,
					   le_to_host16(mlme->size) - ieofs,
					   &elems, 1) == ParseFailed) {
			printf("STA " MACSTR " sent invalid association "
			       "request\n", MAC2STR(sta->addr));
			resp = WLAN_STATUS_UNSPECIFIED_FAILURE;
			goto fail;
		}
		if ((drv->hapd->conf->wpa & WPA_PROTO_RSN) &&
		    elems.rsn_ie) {
			wpa_ie = elems.rsn_ie;
			wpa_ie_len = elems.rsn_ie_len;
		} else if ((drv->hapd->conf->wpa & WPA_PROTO_WPA) &&
			   elems.wpa_ie) {
			wpa_ie = elems.wpa_ie;
			wpa_ie_len = elems.wpa_ie_len;
		} else {
			wpa_ie = NULL;
			wpa_ie_len = 0;
		}
		if (drv->hapd->conf->wpa && wpa_ie == NULL) {
			printf("STA " MACSTR ": No WPA/RSN IE in association "
			       "request\n", MAC2STR(sta->addr));
			resp = WLAN_STATUS_INVALID_IE;
			goto fail;
		}
		if (drv->hapd->conf->wpa) {
			int res;
			wpa_ie -= 2;
			wpa_ie_len += 2;
			if (sta->wpa_sm == NULL)
				sta->wpa_sm = wpa_auth_sta_init(
					drv->hapd->wpa_auth, sta->addr);
			if (sta->wpa_sm == NULL) {
				printf("Failed to initialize WPA state "
				       "machine\n");
				resp = WLAN_STATUS_UNSPECIFIED_FAILURE;
				goto fail;
			}
			res = wpa_validate_wpa_ie(drv->hapd->wpa_auth,
						  sta->wpa_sm,
						  wpa_ie, wpa_ie_len,
						  NULL, 0);
			if (res == WPA_INVALID_GROUP)
				resp = WLAN_STATUS_GROUP_CIPHER_NOT_VALID;
			else if (res == WPA_INVALID_PAIRWISE)
				resp = WLAN_STATUS_PAIRWISE_CIPHER_NOT_VALID;
			else if (res == WPA_INVALID_AKMP)
				resp = WLAN_STATUS_AKMP_NOT_VALID;
			else if (res == WPA_ALLOC_FAIL)
				resp = WLAN_STATUS_UNSPECIFIED_FAILURE;
			else if (res != WPA_IE_OK)
				resp = WLAN_STATUS_INVALID_IE;
			if (resp != WLAN_STATUS_SUCCESS)
				goto fail;
		}
		hdr->oid = (hdr->oid == htonl(DOT11_OID_ASSOCIATEEX)) ?
			htonl(DOT11_OID_ASSOCIATEEX) :
			htonl(DOT11_OID_REASSOCIATEEX);
		hdr->op = htonl(PIMOP_SET);
		mlme->code = 0;
		mlme->state = htons(DOT11_STATE_ASSOC);
		mlme->size = 0;
		send(drv->pim_sock, hdr, sizeof(*hdr) + sizeof(*mlme), 0);
		return;
	} else if (mlme->state==htons(DOT11_STATE_ASSOC)) {
		if (len < sizeof(pimdev_hdr) + sizeof(struct obj_mlme)) {
			printf("bad assoc packet\n");
			return;
		}
		mac_id = mac_id_get(drv, mlme->id);
		if (mac_id == NULL)
			return;
		memcpy(&mlme->address[0], mac_id, ETH_ALEN);
		sta = ap_get_sta(drv->hapd, (u8 *) &mlme->address[0]);
		if (sta == NULL) {
			printf("cannot get sta\n");
			return;
		}
		new_assoc = (sta->flags & WLAN_STA_ASSOC) == 0;
		sta->flags |= WLAN_STA_AUTH | WLAN_STA_ASSOC;
		wpa_auth_sm_event(sta->wpa_sm, WPA_ASSOC);
		hostapd_new_assoc_sta(drv->hapd, sta, !new_assoc);
		ieee802_1x_notify_port_enabled(sta->eapol_sm, 1);
		sta->timeout_next = STA_NULLFUNC;
		return;
	}
	return;

fail:
	printf("Prism54: assoc fail: %x\n", resp);
	mlme->code = host_to_le16(resp);
	mlme->size = 0;
	mlme->state = htons(DOT11_STATE_ASSOCING);
	hdr->oid = htonl(DOT11_OID_DISASSOCIATEEX);
	hdr->op = htonl(PIMOP_SET);
	sta->flags &= ~(WLAN_STA_AUTH | WLAN_STA_ASSOC);
	send(drv->pim_sock, hdr, sizeof(*hdr) + sizeof(*mlme), 0);
}


static void handle_pim(int sock, void *eloop_ctx, void *sock_ctx)
{
	struct prism54_driver_data *drv = eloop_ctx;
	int len;
	pimdev_hdr *hdr;

	hdr = malloc(PIM_BUF_SIZE);
	if (hdr == NULL)
		return;
	len = recv(sock, hdr, PIM_BUF_SIZE, 0);
	if (len < 0) {
		perror("recv");
		free(hdr);
		return;
	}
	if (len < 8) {
		printf("handle_pim: too short (%d)\n", len);
		free(hdr);
		return;
	}

	if (hdr->op != (int) htonl(PIMOP_TRAP)) {
		free(hdr);
		return;
	}
	switch (ntohl(hdr->oid)) {
		case DOT11_OID_PROBE:
			prism54_handle_probe(drv, hdr, len);
			break;
		case DOT11_OID_DEAUTHENTICATEEX:
		case DOT11_OID_DEAUTHENTICATE:
			prism54_handle_deauth(drv, hdr, len);
			break;
		case DOT11_OID_DISASSOCIATEEX:
		case DOT11_OID_DISASSOCIATE:
			prism54_handle_disassoc(drv, hdr, len);
			break;
		case DOT11_OID_AUTHENTICATEEX:
		case DOT11_OID_AUTHENTICATE:
			prism54_handle_auth(drv, hdr, len);
			break;
		case DOT11_OID_ASSOCIATEEX:
		case DOT11_OID_REASSOCIATEEX:
		case DOT11_OID_ASSOCIATE:
		case DOT11_OID_REASSOCIATE:
			prism54_handle_assoc(drv, hdr, len);
		default:
			break;
	}

	free(hdr);
}


static void handle_802_3(int sock, void *eloop_ctx, void *sock_ctx)
{
	struct hostapd_data *hapd = (struct hostapd_data *) eloop_ctx;
	int len;
	ieee802_3_hdr *hdr;

	hdr = malloc(PIM_BUF_SIZE);
	if (hdr == NULL)
		return;
	len = recv(sock, hdr, PIM_BUF_SIZE, 0);
	if (len < 0) {
		perror("recv");
		free(hdr);
		return;
	}
        if (len < 14) {
                wpa_printf(MSG_MSGDUMP, "handle_802_3: too short (%d)", len);
		free(hdr);
                return;
        }
        if (hdr->type == htons(ETH_P_PAE)) {
                ieee802_1x_receive(hapd, (u8 *) &hdr->sa[0], (u8 *) &hdr[1],
				   len - sizeof(*hdr));
        }
	free(hdr);
}


static int prism54_init_sockets(struct prism54_driver_data *drv)
{
	struct hostapd_data *hapd = drv->hapd;
	struct ifreq ifr;
	struct sockaddr_ll addr;

	drv->sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_PAE));
	if (drv->sock < 0) {
		perror("socket[PF_PACKET,SOCK_RAW]");
		return -1;
	}

	if (eloop_register_read_sock(drv->sock, handle_802_3, drv->hapd, NULL))
	{
		printf("Could not register read socket\n");
		return -1;
	}

        memset(&ifr, 0, sizeof(ifr));
	if (hapd->conf->bridge[0] != '\0') {
		printf("opening bridge: %s\n", hapd->conf->bridge);
		os_strlcpy(ifr.ifr_name, hapd->conf->bridge,
			   sizeof(ifr.ifr_name));
	} else {
		os_strlcpy(ifr.ifr_name, drv->iface, sizeof(ifr.ifr_name));
	}
        if (ioctl(drv->sock, SIOCGIFINDEX, &ifr) != 0) {
		perror("ioctl(SIOCGIFINDEX)");
		return -1;
        }

	memset(&addr, 0, sizeof(addr));
	addr.sll_family = AF_PACKET;
	addr.sll_ifindex = ifr.ifr_ifindex;
	addr.sll_protocol = htons(ETH_P_PAE);
	wpa_printf(MSG_DEBUG, "Opening raw packet socket for ifindex %d",
		   addr.sll_ifindex);

	if (bind(drv->sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		perror("bind");
		return -1;
	}

        memset(&ifr, 0, sizeof(ifr));
        os_strlcpy(ifr.ifr_name, drv->iface, sizeof(ifr.ifr_name));
        if (ioctl(drv->sock, SIOCGIFHWADDR, &ifr) != 0) {
		perror("ioctl(SIOCGIFHWADDR)");
		return -1;
        }

	if (ifr.ifr_hwaddr.sa_family != ARPHRD_ETHER) {
		printf("Invalid HW-addr family 0x%04x\n",
		       ifr.ifr_hwaddr.sa_family);
		return -1;
	}
	memcpy(drv->hapd->own_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);

	drv->pim_sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
	if (drv->pim_sock < 0) {
		perror("socket[PF_PACKET,SOCK_RAW]");
		return -1;
	}

	if (eloop_register_read_sock(drv->pim_sock, handle_pim, drv, NULL)) {
		printf("Could not register read socket\n");
		return -1;
	}

        memset(&ifr, 0, sizeof(ifr));
        snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "%sap", drv->iface);
        if (ioctl(drv->pim_sock, SIOCGIFINDEX, &ifr) != 0) {
		perror("ioctl(SIOCGIFINDEX)");
		return -1;
        }

	memset(&addr, 0, sizeof(addr));
	addr.sll_family = AF_PACKET;
	addr.sll_ifindex = ifr.ifr_ifindex;
	addr.sll_protocol = htons(ETH_P_ALL);
	wpa_printf(MSG_DEBUG, "Opening raw packet socket for ifindex %d",
		   addr.sll_ifindex);

	if (bind(drv->pim_sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		perror("bind");
		return -1;
	}

	return 0;
}


static void * prism54_driver_init(struct hostapd_data *hapd)
{
	struct prism54_driver_data *drv;

	drv = os_zalloc(sizeof(struct prism54_driver_data));
	if (drv == NULL) {
		printf("Could not allocate memory for hostapd Prism54 driver "
		       "data\n");
		return NULL;
	}

	drv->hapd = hapd;
	drv->pim_sock = drv->sock = -1;
	memcpy(drv->iface, hapd->conf->iface, sizeof(drv->iface));

	if (prism54_init_sockets(drv)) {
		free(drv);
		return NULL;
	}
	prism54_init_1x(drv);
	/* must clean previous elems */
	prism54_set_generic_elem(drv->iface, drv, NULL, 0);

	return drv;
}


static void prism54_driver_deinit(void *priv)
{
	struct prism54_driver_data *drv = priv;

	if (drv->pim_sock >= 0)
		close(drv->pim_sock);

	if (drv->sock >= 0)
		close(drv->sock);
	
	free(drv);
}


const struct wpa_driver_ops wpa_driver_prism54_ops = {
	.name = "prism54",
	.init = prism54_driver_init,
	.deinit = prism54_driver_deinit,
	/* .set_ieee8021x = prism54_init_1x, */
	.set_privacy = prism54_set_privacy_invoked,
	.set_encryption = prism54_set_encryption,
	.get_seqnum = prism54_get_seqnum,
	.flush = prism54_flush,
	.set_generic_elem = prism54_set_generic_elem,
	.send_eapol = prism54_send_eapol,
	.sta_set_flags = prism54_sta_set_flags,
	.sta_deauth = prism54_sta_deauth,
	.sta_disassoc = prism54_sta_disassoc,
	.set_ssid = prism54_ioctl_setiwessid,
	.get_inact_sec = prism54_get_inact_sec,
};