M7350/external/hostap/wlantest/rx_ip.c

/*
 * Received Data frame processing for IPv4 packets
 * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
 *
 * This software may be distributed under the terms of the BSD license.
 * See README for more details.
 */

#include "utils/includes.h"
#include <netinet/ip.h>
#include <netinet/ip_icmp.h>

#include "utils/common.h"
#include "wlantest.h"


static void ping_update(struct wlantest_sta *sta, int req, u32 src, u32 dst,
			u16 id, u16 seq)
{
	if (req) {
		sta->icmp_echo_req_src = src;
		sta->icmp_echo_req_dst = dst;
		sta->icmp_echo_req_id = id;
		sta->icmp_echo_req_seq = seq;
		return;
	}

	if (sta->icmp_echo_req_src == dst &&
	    sta->icmp_echo_req_dst == src &&
	    sta->icmp_echo_req_id == id &&
	    sta->icmp_echo_req_seq == seq) {
		sta->counters[WLANTEST_STA_COUNTER_PING_OK]++;
		if (sta->counters[WLANTEST_STA_COUNTER_ASSOCREQ_TX] == 0 &&
		    sta->counters[WLANTEST_STA_COUNTER_REASSOCREQ_TX] == 0)
			sta->counters[
				WLANTEST_STA_COUNTER_PING_OK_FIRST_ASSOC]++;
		wpa_printf(MSG_DEBUG, "ICMP echo (ping) match for STA " MACSTR,
			   MAC2STR(sta->addr));
	}
}


static void rx_data_icmp(struct wlantest *wt, const u8 *bssid,
			 const u8 *sta_addr, u32 dst, u32 src,
			 const u8 *data, size_t len, const u8 *peer_addr)
{
	struct in_addr addr;
	char buf[20];
	const struct icmphdr *hdr;
	u16 id, seq;
	struct wlantest_bss *bss;
	struct wlantest_sta *sta;

	hdr = (const struct icmphdr *) data;
	if (len < 4)
		return;

	/* TODO: check hdr->checksum */

	if (hdr->type != ICMP_ECHOREPLY && hdr->type != ICMP_ECHO)
		return;
	if (len < 8)
		return;

	id = ntohs(hdr->un.echo.id);
	seq = ntohs(hdr->un.echo.sequence);

	addr.s_addr = dst;
	snprintf(buf, sizeof(buf), "%s", inet_ntoa(addr));
	addr.s_addr = src;
	wpa_printf(MSG_DEBUG, "ICMP echo %s %s -> %s id=%04x seq=%u len=%u%s",
		   hdr->type == ICMP_ECHO ? "request" : "response",
		   inet_ntoa(addr), buf, id, seq, (unsigned) len - 8,
		   peer_addr ? " [DL]" : "");

	bss = bss_find(wt, bssid);
	if (bss == NULL) {
		wpa_printf(MSG_INFO, "No BSS " MACSTR " known for ICMP packet",
			   MAC2STR(bssid));
		return;
	}

	if (sta_addr == NULL)
		return; /* FromDS broadcast ping */

	sta = sta_find(bss, sta_addr);
	if (sta == NULL) {
		wpa_printf(MSG_INFO, "No STA " MACSTR " known for ICMP packet",
			   MAC2STR(sta_addr));
		return;
	}

	ping_update(sta, hdr->type == ICMP_ECHO, src, dst, id, seq);
	if (peer_addr && (sta = sta_find(bss, peer_addr)))
		ping_update(sta, hdr->type == ICMP_ECHO, src, dst, id, seq);
}


void rx_data_ip(struct wlantest *wt, const u8 *bssid, const u8 *sta_addr,
		const u8 *dst, const u8 *src, const u8 *data, size_t len,
		const u8 *peer_addr)
{
	const struct iphdr *ip;
	const u8 *payload;
	size_t plen;
	u16 frag_off, tot_len;

	ip = (const struct iphdr *) data;
	if (len < sizeof(*ip))
		return;
	if (ip->version != 4) {
		wpa_printf(MSG_DEBUG, "Unexpected IP protocol version %u in "
			   "IPv4 packet (bssid=" MACSTR " str=" MACSTR
			   " dst=" MACSTR ")", ip->version, MAC2STR(bssid),
			   MAC2STR(src), MAC2STR(dst));
		return;
	}
	if (ip->ihl * 4 < sizeof(*ip)) {
		wpa_printf(MSG_DEBUG, "Unexpected IP header length %u in "
			   "IPv4 packet (bssid=" MACSTR " str=" MACSTR
			   " dst=" MACSTR ")", ip->ihl, MAC2STR(bssid),
			   MAC2STR(src), MAC2STR(dst));
		return;
	}
	if (ip->ihl * 4 > len) {
		wpa_printf(MSG_DEBUG, "Truncated IP header (ihl=%u len=%u) in "
			   "IPv4 packet (bssid=" MACSTR " str=" MACSTR
			   " dst=" MACSTR ")", ip->ihl, (unsigned) len,
			   MAC2STR(bssid), MAC2STR(src), MAC2STR(dst));
		return;
	}

	/* TODO: check header checksum in ip->check */

	frag_off = be_to_host16(ip->frag_off);
	if (frag_off & 0x1fff) {
		wpa_printf(MSG_EXCESSIVE, "IP fragment reassembly not yet "
			   "supported");
		return;
	}

	tot_len = be_to_host16(ip->tot_len);
	if (tot_len > len)
		return;
	if (tot_len < len)
		len = tot_len;

	payload = data + 4 * ip->ihl;
	plen = len - 4 * ip->ihl;

	switch (ip->protocol) {
	case IPPROTO_ICMP:
		rx_data_icmp(wt, bssid, sta_addr, ip->daddr, ip->saddr,
			     payload, plen, peer_addr);
		break;
	}
}