agentadv.c

mobile ip 在linux下的一种实现

 * @from:
 * @addr:
 * 
 * Check incoming ICMP packet. 
 *
 * Returns:
 *   -1: error in receiving the message,
 *    0: router solicitation received,
 */
int check_icmp_sol(int s, struct sockaddr_ll *from, struct in_addr *dstaddr,
		   struct in_addr *srcaddr)
{
	int len;
	unsigned int fromlen;
	unsigned char buf[MAX_ADV_MSG];
	struct agent_sol *asol;
	struct iphdr *ip;
	int i;

	/* process the packet - only reply to agent sol. messages */

	fromlen = sizeof(struct sockaddr_ll);

	len = recvfrom(s, buf, sizeof(buf), 0, (struct sockaddr *) from,
		       &fromlen);
	if (len < 0) {
		DEBUG(DEBUG_FLAG, "handle_icmp_sol - recvfrom: %s\n",
		      strerror(errno));
		return -1;
	}

	ip = (struct iphdr *) buf;
	asol = (struct agent_sol *) (buf + sizeof(struct iphdr));

	/* quick checks to skip most of the ICMP messages quickly */
	if (len < sizeof(struct iphdr) + sizeof(struct agent_sol)) return -1;
	if (asol->type != ICMP_ROUTERSOLICIT)
		return -1;
	if (asol->reserved != 0) return -1;

	srcaddr->s_addr = ip->saddr;
	DEBUG(DEBUG_FLAG, "ICMP from %s ", inet_ntoa(*srcaddr));
	dstaddr->s_addr = ip->daddr;
	DEBUG(DEBUG_FLAG, "to %s, len=%i", inet_ntoa(*dstaddr), len);
	if (len != ntohs(ip->tot_len)) {
		DEBUG(DEBUG_FLAG, " => %i", ntohs(ip->tot_len));
		if (ntohs(ip->tot_len) > len) {
			DEBUG(DEBUG_FLAG, "  message underflow!");
			return -1;
		}
		len = ntohs(ip->tot_len);
		if (len < sizeof(struct iphdr) + sizeof(struct agent_sol)) {
			DEBUG(DEBUG_FLAG, "  too short!\n");
			return -1;
		}
	}
	DEBUG(DEBUG_FLAG, "\n");
	DEBUG(DEBUG_FLAG, "\tsll_ifindex=%i sll_hatype=0x%04x sll_pkttype=%i "
	      "sll_halen=%i sll_addr=",
	      from->sll_ifindex, ntohs(from->sll_hatype),
	      from->sll_pkttype, from->sll_halen);
	for (i = 0; i < sizeof(from->sll_addr); i++) {
		if (i > 0)
			DEBUG(DEBUG_FLAG, ":");
		DEBUG(DEBUG_FLAG, "%02x", from->sll_addr[i]);
	}
	DEBUG(DEBUG_FLAG, "\n");

	if (ip_checksum((unsigned char *) asol,
			len - sizeof(struct iphdr)) != 0) {
		DEBUG(DEBUG_FLAG, "\tICMP checksum error\n");
		DEBUG_HEXDUMP(DEBUG_FLAG, "Packet dump", buf, len);
		return -1;
	}

	if (len < sizeof(struct iphdr) + sizeof(struct agent_sol)) {
		DEBUG(DEBUG_FLAG, "Packet underflow - "
			"type=%i, code=%i - len=%i, expected %i\n",
		      asol->type, asol->code, len,
		      sizeof(struct iphdr) + sizeof(struct agent_sol));
		DEBUG_HEXDUMP(DEBUG_FLAG, "Packet dump", buf, len);
		return -1;
	}

	if (asol->type != 10 || asol->code != 0 || asol->reserved != 0) {
		DEBUG(DEBUG_FLAG,
		      "\tunknown ICMP - not to us (type=%i, code=%i, "
		      "reserved=%i)\n",
		      asol->type, asol->code, ntohl(asol->reserved));
		DEBUG_HEXDUMP(DEBUG_FLAG, "Packet dump", buf, len);
		return -1;
	}

	return 0;
}


/**
 * handle_icmp_sol:
 * @s:
 *
 * Get next ICMP message and reply if it is a router solicitation
 *
 * Returns:
 *   -1: error in receiving the message,
 *    0: router solicitation received and replied (agentadv sent),
 */
int handle_icmp_sol(int s)
{
	struct sockaddr_ll from;
	struct in_addr dstaddr, srcaddr;
	int r;

	r = check_icmp_sol(s, &from, &dstaddr, &srcaddr);
	if (r)
		return r;

	/* ICMP Router Solicitation Message */
	DEBUG(DEBUG_FLAG, "\tagent solicitation => reply with agent adv\n");
	send_agent_advertisement(s, (struct sockaddr *) &from, &srcaddr,
				 from.sll_ifindex);
	return 0;
}

#endif /* DYN_TARGET_LINUX */


/**
 * handle_icmp_adv:
 * @s: file descriptor of the ICMP socket for agent advertisements
 * @msg: pointer to the start of the data buffer
 * @len: maximum length of the buffer
 * @ext: pointer for the parsed agent advertisement
 *
 * Get next ICMP message and parse agent advertisement into @ext. The
 * extensions that are not included in the advertisement are set to %NULL.
 * The contents of @ext is unspecified if function fails (i.e., returned value
 * is not 1).
 *
 * Returns:
 *   1 an agent advertisement was received and there was enough buffer space
 *     for the message,
 *   0 failed - unspecified reason,
 *   -1 failed - Network is down
 */
int handle_icmp_adv(int s, char *msg, int maxlen, struct adv_extensions *ext)
{
	struct in_addr in;
	int len, used;
	socklen_t fromlen;
	struct agent_adv_dynamics *tmp_ext;
	struct in_addr addr;
#ifdef DYN_TARGET_LINUX
	unsigned char *tmp;
#endif

	assert(msg != NULL && ext != NULL);

	memset(ext, 0, sizeof(*ext));

	/* process the packet - only accept agent adv. messages */

	fromlen = sizeof(ext->from);
	len = recvfrom(s, msg, maxlen, 0, (struct sockaddr *) &ext->from,
		       &fromlen);
	if (len < 0) {
		DEBUG(DEBUG_FLAG, "handle_icmp_adv - recvfrom: %s\n",
		      strerror(errno));
		if (errno == ENETDOWN)
			return -1;
		return 0;
	}

#ifdef PACKET_DUMP
	DEBUG_HEXDUMP(DEBUG_FLAG, "Received agentadv", msg, len);
#endif

#ifdef DYN_TARGET_LINUX
	if (len < sizeof(struct iphdr) + 
	    sizeof(struct router_adv) + sizeof(struct agent_adv_ext))
		return 0;
#else
	if (len < sizeof(struct ethhdr) + sizeof(struct iphdr) + 
	    sizeof(struct router_adv) + sizeof(struct agent_adv_ext))
		return 0;
#endif

	if (user_space_filter != AGENTADV_FILTER_NONE && !ok_buf(msg))
		return 0;

	ext->start = msg;
	ext->len = len;

#ifdef DYN_TARGET_LINUX
	ext->ip = (struct iphdr *) ext->start;

	/* Packet socket seems to give some extra bytes in certain situations.
	 * Find out the correct length from the IP header. */
	tmp = (unsigned char *) &ext->ip->tot_len;
	used = *tmp * 256 + *(tmp + 1);
	if (used > ext->len) {
		DEBUG(DEBUG_FLAG, "Dropping truncated packet (%i < %i)\n",
		      ext->len, used);
		return 0;
	} else if (used < ext->len) {
		DEBUG(DEBUG_FLAG, "Fixed packet length: %i => %i\n", ext->len,
		      used);
		ext->len = used;
	}

	DEBUG(DEBUG_FLAG, "handle_icmp_adv: ifindex=%i hatype=0x%04x "
	      "pkttype=%i halen=%i addr=",
	      ext->from.sll_ifindex, ntohs(ext->from.sll_hatype),
	      ext->from.sll_pkttype, ext->from.sll_halen);
	for (used = 0; used < sizeof(ext->from.sll_addr); used++) {
		if (used > 0)
			DEBUG(DEBUG_FLAG, ":");
		DEBUG(DEBUG_FLAG, "%02x", ext->from.sll_addr[used]);
	}
	DEBUG(DEBUG_FLAG, "\n");

	used = sizeof(struct iphdr);
#else
	ext->eth = (struct ethhdr *) ext->start;
	DEBUG(DEBUG_FLAG, "handle_icmp_adv: HW-dest   = %s\n",
	      ether_hwtoa(ext->eth->h_dest));
	DEBUG(DEBUG_FLAG, "handle_icmp_adv: HW-source = %s\n",
	      ether_hwtoa(ext->eth->h_source));
	ext->ip = (struct iphdr *) (ext->eth + 1);

	used = sizeof(struct iphdr) + sizeof(struct ethhdr);
#endif

	ext->radv = (struct router_adv *) (ext->start + used);
	used += sizeof(struct router_adv);

	if (ext->radv->code != 0 && ext->radv->code != 16)
		return 0;

	UNALIGNED_(&in.s_addr, &ext->ip->saddr);
	DEBUG(DEBUG_FLAG, "ICMP from %s, len=%i\n", inet_ntoa(in),
	      ext->len);

	if (ext->radv->entry_size != 2) {
		DEBUG(DEBUG_FLAG, "\tinvalid entry_size %i\n",
		      ext->radv->entry_size);
		return 0;
	}

	if (ip_checksum((unsigned char *) ext->radv,
			ext->len -
#ifndef DYN_TARGET_LINUX
			sizeof(struct ethhdr) -
#endif
			sizeof(struct iphdr)) != 0) {
		DEBUG(DEBUG_FLAG, "\tICMP checksum error\n");
		return 0;
	}


	used += ext->radv->num_addr * sizeof(struct router_adv_router);
	ext->ext = (struct agent_adv_ext *) (ext->start + used);

	if (used + 2 > len || used + 2 + ext->ext->length > len) {
		DEBUG(DEBUG_FLAG, "\tmessage overflow - dropped\n");
		return 0;
	}

	if (ext->ext->type != AGENT_ADVERTISEMENT_EXTENSION_TYPE ||
	    ext->ext->length < 6) {
		DEBUG(DEBUG_FLAG, "\tunknown first extension type=%i, "
		      "length=%i\n", ext->ext->type, ext->ext->length);
		return 0;
	}

	tmp_ext = (struct agent_adv_dynamics *) ext->ext;
	for (;;) {
		if (tmp_ext->type == 0)
			used++; /* zero padding */
		else {
			used += 2 + tmp_ext->length;
		}
		if (used == ext->len) break;
		if (used + 1 > ext->len) {
			DEBUG(DEBUG_FLAG, "\textension overflow\n");
			return 0;
		}

		tmp_ext = (struct agent_adv_dynamics *) (ext->start + used);
		if (tmp_ext->type == 0) continue;
		if (used + 2 > ext->len ||
		    used + 2 + tmp_ext->length > ext->len) {
			DEBUG(DEBUG_FLAG, "\textension overflow - dropped\n");
			return 0;
		}
		if (ext->own_ext == NULL && tmp_ext->type == VENDOR_EXT_TYPE2
		    && tmp_ext->length >= MIN_AGENT_ADV_DYNAMICS_LEN &&
		    ntohl(tmp_ext->vendor_id) == VENDOR_ID_DYNAMICS &&
		    ntohs(tmp_ext->sub_type) == VENDOR_EXT_DYNAMICS_AGENTADV) {
			ext->own_ext = tmp_ext;
			DEBUG(DEBUG_FLAG, " * Dynamics agentadv ext\n");
			continue;
		}
		if (ext->pubkey_hash == NULL &&
		    tmp_ext->type == VENDOR_EXT_TYPE2
		    && tmp_ext->length >= sizeof(struct msg_key) &&
		    ntohl(tmp_ext->vendor_id) == VENDOR_ID_DYNAMICS &&
		    ntohs(tmp_ext->sub_type) ==
		    VENDOR_EXT_DYNAMICS_PUBKEY_HASH) {
			ext->pubkey_hash = (struct msg_key *) tmp_ext;
			DEBUG(DEBUG_FLAG, " * pubkey hash ext\n");
			continue;
		}

		if (ext->fa_nai == NULL && tmp_ext->type == VENDOR_EXT_TYPE2
		    && tmp_ext->length >= sizeof(struct fa_nai_ext) &&
		    ntohl(tmp_ext->vendor_id) == VENDOR_ID_DYNAMICS &&
		    ntohs(tmp_ext->sub_type) == VENDOR_EXT_DYNAMICS_FA_NAI) {
			unsigned char *c;
			int i;
			ext->fa_nai = (struct fa_nai_ext *) tmp_ext;
			DEBUG(DEBUG_FLAG, " * FA NAI ext: ");
			c = MSG_NAI_DATA(ext->fa_nai);
			for (i = 0; i < GET_NAI_LEN(ext->fa_nai); i++) {
				if (*c < 32 || *c > 126)
					DEBUG(DEBUG_FLAG, "<%i>", *c);
				else
					DEBUG(DEBUG_FLAG, "%c", *c);
				c++;
			}
			DEBUG(DEBUG_FLAG, "\n");
			continue;
		}

		if (ext->prefix == NULL &&
		    tmp_ext->type == PREFIX_LENGTHS_EXTENSION_TYPE) {
			ext->prefix = (struct prefix_len_ext *) tmp_ext;
			DEBUG(DEBUG_FLAG, " * prefix-lengths ext\n");
			continue;
		}

		if (ext->challenge == NULL &&
		    tmp_ext->type == AGENT_ADV_CHALLENGE_EXT) {
			int j;
			unsigned char *c;
			ext->challenge = (struct challenge_ext *) tmp_ext;
			DEBUG(DEBUG_FLAG, " * challenge ext: ");
			c = MSG_CHALLENGE_EXT_DATA(ext->challenge);
			for (j = 0; j < GET_CHALLENGE_LEN(ext->challenge); j++)
				DEBUG(DEBUG_FLAG, "%02X", *c++);
			DEBUG(DEBUG_FLAG, "\n");
			continue;
		}

		if (tmp_ext->type <= 127) {
			DEBUG(DEBUG_FLAG, "\tunknown extension (type %i) - "
			      "dropping advertisement\n", tmp_ext->type);
			return 0;
		} else {
			DEBUG(DEBUG_FLAG, "\tunknown extension (type %i) - "
			      "ignoring it, but handling the rest of the "
			      "advertisement\n", tmp_ext->type);
		}
	}

	/* ICMP Router Advertisement Message */
	UNALIGNED_(&addr.s_addr, &ext->ip->daddr);
	DEBUG(DEBUG_FLAG, "\tagent adv (sent to %s)\n", inet_ntoa(addr));

	ext->coaddrs = (ext->ext->length - 6) / 4;

#ifndef DYN_TARGET_LINUX
	assert(ext->eth);
#endif
	assert(ext->ip && ext->radv && ext->ext);

	return 1;
}