wps_upnp_ssdp.c

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

	int next_timeout_sec;
	int next_timeout_msec;

	wpa_printf(MSG_DEBUG, "WPS UPnP: M-SEARCH reply start (%d "
		   "outstanding)", sm->n_msearch_replies);
	if (sm->n_msearch_replies >= MAX_MSEARCH) {
		wpa_printf(MSG_INFO, "WPS UPnP: Too many outstanding "
			   "M-SEARCH replies");
		return;
	}

	a = os_zalloc(sizeof(*a));
	if (a == NULL)
		return;
	a->type = MSEARCH_REPLY;
	a->state = 0;
	a->sm = sm;
	os_memcpy(&a->client, client, sizeof(client));
	/* Wait time depending on MX value */
	next_timeout_msec = (1000 * mx * (os_random() & 0xFF)) >> 8;
	next_timeout_sec = next_timeout_msec / 1000;
	next_timeout_msec = next_timeout_msec % 1000;
	if (eloop_register_timeout(next_timeout_sec, next_timeout_msec,
				   msearchreply_state_machine_handler, sm,
				   a)) {
		/* No way to recover (from malloc failure) */
		goto fail;
	}
	/* Remember for future cleanup */
	if (sm->msearch_replies) {
		a->next = sm->msearch_replies;
		a->prev = a->next->prev;
		a->prev->next = a;
		a->next->prev = a;
	} else {
		sm->msearch_replies = a->next = a->prev = a;
	}
	sm->n_msearch_replies++;
	return;

fail:
	wpa_printf(MSG_INFO, "WPS UPnP: M-SEARCH reply failure!");
	eloop_cancel_timeout(msearchreply_state_machine_handler, sm, a);
	os_free(a);
}


/**
 * ssdp_parse_msearch - Process a received M-SEARCH
 * @sm: WPS UPnP state machine from upnp_wps_device_init()
 * @client: Client address
 * @data: NULL terminated M-SEARCH message
 *
 * Given that we have received a header w/ M-SEARCH, act upon it
 *
 * Format of M-SEARCH (case insensitive!):
 *
 * First line must be:
 *      M-SEARCH * HTTP/1.1
 * Other lines in arbitrary order:
 *      HOST:239.255.255.250:1900
 *      ST:<varies -- must match>
 *      MAN:"ssdp:discover"
 *      MX:<varies>
 *
 * It should be noted that when Microsoft Vista is still learning its IP
 * address, it sends out host lines like: HOST:[FF02::C]:1900
 */
static void ssdp_parse_msearch(struct upnp_wps_device_sm *sm,
			       struct sockaddr_in *client, const char *data)
{
	const char *start = data;
	const char *end;
	int got_host = 0;
	int got_st = 0, st_match = 0;
	int got_man = 0;
	int got_mx = 0;
	int mx = 0;

	/*
	 * Skip first line M-SEARCH * HTTP/1.1
	 * (perhaps we should check remainder of the line for syntax)
	 */
	data += line_length(data);

	/* Parse remaining lines */
	for (; *data != '\0'; data += line_length(data)) {
		end = data + line_length_stripped(data);
		if (token_eq(data, "host")) {
			/* The host line indicates who the packet
			 * is addressed to... but do we really care?
			 * Note that Microsoft sometimes does funny
			 * stuff with the HOST: line.
			 */
#if 0   /* could be */
			data += token_length(data);
			data += word_separation_length(data);
			if (*data != ':')
				goto bad;
			data++;
			data += word_separation_length(data);
			/* UPNP_MULTICAST_ADDRESS */
			if (!str_starts(data, "239.255.255.250"))
				goto bad;
			data += os_strlen("239.255.255.250");
			if (*data == ':') {
				if (!str_starts(data, ":1900"))
					goto bad;
			}
#endif  /* could be */
			got_host = 1;
			continue;
		} else if (token_eq(data, "st")) {
			/* There are a number of forms; we look
			 * for one that matches our case.
			 */
			got_st = 1;
			data += token_length(data);
			data += word_separation_length(data);
			if (*data != ':')
				continue;
			data++;
			data += word_separation_length(data);
			if (str_starts(data, "ssdp:all")) {
				st_match = 1;
				continue;
			}
			if (str_starts(data, "upnp:rootdevice")) {
				st_match = 1;
				continue;
			}
			if (str_starts(data, "uuid:")) {
				char uuid_string[80];
				data += os_strlen("uuid:");
				uuid_bin2str(sm->wps->uuid, uuid_string,
					     sizeof(uuid_string));
				if (str_starts(data, uuid_string))
					st_match = 1;
				continue;
			}
#if 0
			/* FIX: should we really reply to IGD string? */
			if (str_starts(data, "urn:schemas-upnp-org:device:"
				       "InternetGatewayDevice:1")) {
				st_match = 1;
				continue;
			}
#endif
			if (str_starts(data, "urn:schemas-wifialliance-org:"
				       "service:WFAWLANConfig:1")) {
				st_match = 1;
				continue;
			}
			if (str_starts(data, "urn:schemas-wifialliance-org:"
				       "device:WFADevice:1")) {
				st_match = 1;
				continue;
			}
			continue;
		} else if (token_eq(data, "man")) {
			data += token_length(data);
			data += word_separation_length(data);
			if (*data != ':')
				continue;
			data++;
			data += word_separation_length(data);
			if (!str_starts(data, "\"ssdp:discover\"")) {
				wpa_printf(MSG_DEBUG, "WPS UPnP: Unexpected "
					   "M-SEARCH man-field");
				goto bad;
			}
			got_man = 1;
			continue;
		} else if (token_eq(data, "mx")) {
			data += token_length(data);
			data += word_separation_length(data);
			if (*data != ':')
				continue;
			data++;
			data += word_separation_length(data);
			mx = atol(data);
			got_mx = 1;
			continue;
		}
		/* ignore anything else */
	}
	if (!got_host || !got_st || !got_man || !got_mx || mx < 0) {
		wpa_printf(MSG_DEBUG, "WPS UPnP: Invalid M-SEARCH: %d %d %d "
			   "%d mx=%d", got_host, got_st, got_man, got_mx, mx);
		goto bad;
	}
	if (!st_match) {
		wpa_printf(MSG_DEBUG, "WPS UPnP: Ignored M-SEARCH (no ST "
			   "match)");
		return;
	}
	if (mx > 120)
		mx = 120; /* UPnP-arch-DeviceArchitecture, 1.2.3 */
	msearchreply_state_machine_start(sm, client, mx);
	return;

bad:
	wpa_printf(MSG_INFO, "WPS UPnP: Failed to parse M-SEARCH");
	wpa_printf(MSG_MSGDUMP, "WPS UPnP: M-SEARCH data:\n%s", start);
}


/* Listening for (UDP) discovery (M-SEARCH) packets */

/**
 * ssdp_listener_stop - Stop SSDP listered
 * @sm: WPS UPnP state machine from upnp_wps_device_init()
 *
 * This function stops the SSDP listerner that was started by calling
 * ssdp_listener_start().
 */
void ssdp_listener_stop(struct upnp_wps_device_sm *sm)
{
	if (sm->ssdp_sd_registered) {
		eloop_unregister_sock(sm->ssdp_sd, EVENT_TYPE_READ);
		sm->ssdp_sd_registered = 0;
	}

	if (sm->ssdp_sd != -1) {
		close(sm->ssdp_sd);
		sm->ssdp_sd = -1;
	}

	eloop_cancel_timeout(msearchreply_state_machine_handler, sm,
			     ELOOP_ALL_CTX);
}


static void ssdp_listener_handler(int sd, void *eloop_ctx, void *sock_ctx)
{
	struct upnp_wps_device_sm *sm = sock_ctx;
	struct sockaddr_in addr; /* client address */
	socklen_t addr_len;
	int nread;
	char buf[MULTICAST_MAX_READ], *pos;

	addr_len = sizeof(addr);
	nread = recvfrom(sm->ssdp_sd, buf, sizeof(buf) - 1, 0,
			 (struct sockaddr *) &addr, &addr_len);
	if (nread <= 0)
		return;
	buf[nread] = '\0'; /* need null termination for algorithm */

	if (str_starts(buf, "NOTIFY ")) {
		/*
		 * Silently ignore NOTIFYs to avoid filling debug log with
		 * unwanted messages.
		 */
		return;
	}

	pos = os_strchr(buf, '\n');
	if (pos)
		*pos = '\0';
	wpa_printf(MSG_MSGDUMP, "WPS UPnP: Received SSDP packet from %s:%d: "
		   "%s", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port), buf);
	if (pos)
		*pos = '\n';

	/* Parse first line */
	if (os_strncasecmp(buf, "M-SEARCH", os_strlen("M-SEARCH")) == 0 &&
	    !isgraph(buf[strlen("M-SEARCH")])) {
		ssdp_parse_msearch(sm, &addr, buf);
		return;
	}

	/* Ignore anything else */
}


/**
 * ssdp_listener_start - Set up for receiving discovery (UDP) packets
 * @sm: WPS UPnP state machine from upnp_wps_device_init()
 * Returns: 0 on success, -1 on failure
 *
 * The SSDP listerner is stopped by calling ssdp_listener_stop().
 */
int ssdp_listener_start(struct upnp_wps_device_sm *sm)
{
	int sd = -1;
	struct sockaddr_in addr;
	struct ip_mreq mcast_addr;
	int on = 1;
	/* per UPnP spec, keep IP packet time to live (TTL) small */
	unsigned char ttl = 4;

	sm->ssdp_sd = sd = socket(AF_INET, SOCK_DGRAM, 0);
	if (sd < 0)
		goto fail;
	if (fcntl(sd, F_SETFL, O_NONBLOCK) != 0)
		goto fail;
	if (setsockopt(sd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)))
		goto fail;
	os_memset(&addr, 0, sizeof(addr));
	addr.sin_family = AF_INET;
	addr.sin_addr.s_addr = htonl(INADDR_ANY);
	addr.sin_port = htons(UPNP_MULTICAST_PORT);
	if (bind(sd, (struct sockaddr *) &addr, sizeof(addr)))
		goto fail;
	os_memset(&mcast_addr, 0, sizeof(mcast_addr));
	mcast_addr.imr_interface.s_addr = htonl(INADDR_ANY);
	mcast_addr.imr_multiaddr.s_addr = inet_addr(UPNP_MULTICAST_ADDRESS);
	if (setsockopt(sd, IPPROTO_IP, IP_ADD_MEMBERSHIP,
		       (char *) &mcast_addr, sizeof(mcast_addr)))
		goto fail;
	if (setsockopt(sd, IPPROTO_IP, IP_MULTICAST_TTL,
		       &ttl, sizeof(ttl)))
		goto fail;
	if (eloop_register_sock(sd, EVENT_TYPE_READ, ssdp_listener_handler,
				NULL, sm))
		goto fail;
	sm->ssdp_sd_registered = 1;
	return 0;

fail:
	/* Error */
	wpa_printf(MSG_ERROR, "WPS UPnP: ssdp_listener_start failed");
	ssdp_listener_stop(sm);
	return -1;
}


/**
 * add_ssdp_network - Add routing entry for SSDP
 * @net_if: Selected network interface name
 * Returns: 0 on success, -1 on failure
 *
 * This function assures that the multicast address will be properly
 * handled by Linux networking code (by a modification to routing tables).
 * This must be done per network interface. It really only needs to be done
 * once after booting up, but it does not hurt to call this more frequently
 * "to be safe".
 */
int add_ssdp_network(char *net_if)
{
#ifdef __linux__
	int ret = -1;
	int sock = -1;
	struct rtentry rt;
	struct sockaddr_in *sin;

	if (!net_if)
		goto fail;

	os_memset(&rt, 0, sizeof(rt));
	sock = socket(AF_INET, SOCK_DGRAM, 0);
	if (sock < 0)
		goto fail;

	rt.rt_dev = net_if;
	sin = (struct sockaddr_in *) &rt.rt_dst;
	sin->sin_family = AF_INET;
	sin->sin_port = 0;
	sin->sin_addr.s_addr = inet_addr(SSDP_TARGET);
	sin = (struct sockaddr_in *) &rt.rt_genmask;
	sin->sin_family = AF_INET;
	sin->sin_port = 0;
	sin->sin_addr.s_addr = inet_addr(SSDP_NETMASK);
	rt.rt_flags = RTF_UP;
	if (ioctl(sock, SIOCADDRT, &rt) < 0) {
		if (errno == EPERM) {
			wpa_printf(MSG_DEBUG, "add_ssdp_network: No "
				   "permissions to add routing table entry");
			/* Continue to allow testing as non-root */
		} else if (errno != EEXIST) {
			wpa_printf(MSG_INFO, "add_ssdp_network() ioctl errno "
				   "%d (%s)", errno, strerror(errno));
			goto fail;
		}
	}

	ret = 0;

fail:
	if (sock >= 0)
		close(sock);

	return ret;
#else /* __linux__ */
	return 0;
#endif /* __linux__ */
}


/**
 * ssdp_open_multicast - Open socket for sending multicast SSDP messages
 * @sm: WPS UPnP state machine from upnp_wps_device_init()
 * Returns: 0 on success, -1 on failure
 */
int ssdp_open_multicast(struct upnp_wps_device_sm *sm)
{
	int sd = -1;
	 /* per UPnP-arch-DeviceArchitecture, 1. Discovery, keep IP packet
	  * time to live (TTL) small */
	unsigned char ttl = 4;

	sm->multicast_sd = sd = socket(AF_INET, SOCK_DGRAM, 0);
	if (sd < 0)
		return -1;

#if 0   /* maybe ok if we sometimes block on writes */
	if (fcntl(sd, F_SETFL, O_NONBLOCK) != 0)
		return -1;
#endif

	if (setsockopt(sd, IPPROTO_IP, IP_MULTICAST_IF,
		       &sm->ip_addr, sizeof(sm->ip_addr)))
		return -1;
	if (setsockopt(sd, IPPROTO_IP, IP_MULTICAST_TTL,
		       &ttl, sizeof(ttl)))
		return -1;

#if 0   /* not needed, because we don't receive using multicast_sd */
	{
		struct ip_mreq mreq;
		mreq.imr_multiaddr.s_addr = inet_addr(UPNP_MULTICAST_ADDRESS);
		mreq.imr_interface.s_addr = sm->ip_addr;
		wpa_printf(MSG_DEBUG, "WPS UPnP: Multicast addr 0x%x if addr "
			   "0x%x",
			   mreq.imr_multiaddr.s_addr,
			   mreq.imr_interface.s_addr);
		if (setsockopt(sd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq,
				sizeof(mreq))) {
			wpa_printf(MSG_ERROR,
				   "WPS UPnP: setsockopt "
				   "IP_ADD_MEMBERSHIP errno %d (%s)",
				   errno, strerror(errno));
			return -1;
		}
	}
#endif  /* not needed */

	/*
	 * TODO: What about IP_MULTICAST_LOOP? It seems to be on by default?
	 * which aids debugging I suppose but isn't really necessary?
	 */

	return 0;
}