util.c

mobile ip 在linux下的一种实现

int ether_atohw(const char *hwtxt, unsigned char *addr)
{
	int n;
	unsigned int _addr[6];
	n = sscanf(hwtxt, "%02x:%02x:%02x:%02x:%02x:%02x",
		   &_addr[0], &_addr[1], &_addr[2], &_addr[3], &_addr[4],
		   &_addr[5]);
	if (n != 6) {
		DEBUG(DEBUG_FLAG, "ether_atohw: invalid addr '%s' (n=%i)\n",
		      hwtxt, n);
		return -1;
	}
	for (n = 0; n < 6; n++)
		addr[n] = _addr[n] & 0xff;
	return 0;
}


char* reply_code_str(__u8 code)
{
	switch (code) {
	case REGREP_ACCEPTED:
		return "registration accepted";
	case REGREP_ACCEPTED_NO_SB:
		return "registration accepted, but simultaneous mobility "
			"binding unsupported";
	case REGREP_REASON_UNSPEC_FA:
		return "reason unspecified (FA)";
	case REGREP_ADMIN_PROHIBITED_FA:
		return "administratively prohibited (FA)";
	case REGREP_NO_RESOURCES_FA:
		return "insufficient resources (FA)";
	case REGREP_MN_FAILED_AUTH_FA:
		return "mobile node failed authentication (FA)";
	case REGREP_HA_FAILED_AUTH_FA:
		return "home agent failed authentication (FA)";
	case REGREP_LONG_LIFETIME_FA:
		return "requested Lifetime too long (FA)";
	case REGREP_BAD_REQUEST_FA:
		return "poorly formed Request (FA)";
	case REGREP_BAD_REPLY_FA:
		return "poorly formed Reply (FA)";
	case REGREP_ENCAP_UNAVAIL_FA:
		return "requested encapsulation unavailable (FA)";
	case REGREP_VJ_UNAVAIL_FA:
		return "requested Van Jacobson compression unavailable (FA)";
	case REGREP_INVALID_CAREOF_FA:
		return "invalid care-of address (FA)";
	case REGREP_REGISTRATION_TIMEOUT_FA:
		return "registration timeout (FA)";
	case REGREP_DELIVERY_STYLE_NOT_SUPPORTED_FA:
		return "delivery style not supported (FA)";
	case REGREP_HN_UNCREACHABLE_FA:
		return "home network unreachable (FA)";
	case REGREP_HA_HOST_UNCREACHABLE_FA:
		return "home agent host unreachable (FA)";
	case REGREP_HA_PORT_UNCREACHABLE_FA:
		return "home agent port unreachable (FA)";
	case REGREP_HA_UNREACHABLE_FA:
		return "home agent unreachable (FA)";
	case REGREP_NONZERO_HOMEADDR_REQD_FA:
		return "non-zero home address required (FA)";
	case REGREP_MISSING_NAI_FA:
		return "missing network access identifier (FA)";
	case REGREP_MISSING_HOME_AGENT_FA:
		return "missing home agent (FA)";
	case REGREP_MISSING_HOMEADDR_FA:
		return "missing home address (FA)";
	case REGREP_UNKNOWN_CHALLENGE_FA:
		return "unknown challenge (FA)";
	case REGREP_MISSING_CHALLENGE_FA:
		return "missing challenge (FA)";
	case REGREP_STALE_CHALLENGE_FA:
		return "stale challenge (FA)";
	case REGREP_MISSING_MN_FA_FA:
		return "missing MN-FA key material from AAA extension";
	case REGREP_UNSUPP_VENDOR_ID_MN_FA:
		return "unsupported vendor extension (MN->FA)";
	case REGREP_UNSUPP_VENDOR_ID_HA_FA:
		return "unsupported vendor extension (HA->FA)";
	case REGREP_REASON_UNSPEC_HA:
		return "reason unspecified (HA)";
	case REGREP_ADMIN_PROHIBITED_HA:
		return "administratively prohibited (HA)";
	case REGREP_NO_RESOURCES_HA:
		return "insufficient resources (HA)";
	case REGREP_MN_FAILED_AUTH_HA:
		return "mobile node failed authentication (HA)";
	case REGREP_FA_FAILED_AUTH_HA:
		return "foreign agent failed authentication (HA)";
	case REGREP_ID_MISMATCH_HA:
		return "registration Identification mismatch (HA)";
	case REGREP_BAD_REQUEST_HA:
		return "poorly formed Request (HA)";
	case REGREP_TOO_MANY_SIMULTANEOUS:
		return "too many simultaneous mobility bindings (HA)";
	case REGREP_UNKNOWN_HA_HA:
		return "unknown home agent address (HA)";
	case REGREP_REVERSE_TUNNEL_UNAVAIL_FA:
		return "requested reverse tunnel unavailable (FA)";
	case REGREP_REVERSE_TUNNEL_MANDATORY_FA:
		return "reverse tunnel is mandatory and 'T' bit not set (FA)";
	case REGREP_REVERSE_TUNNEL_MN_TOO_DISTANT_FA:
		return "mobile node too distant (FA)";
	case REGREP_REVERSE_TUNNEL_UNAVAIL_HA:
		return "requested reverse tunnel unavailable (HA)";
	case REGREP_REVERSE_TUNNEL_MANDATORY_HA:
		return "reverse tunnel is mandatory and 'T' bit not set (HA)";
	case REGREP_ENCAP_UNAVAIL_HA:
		return "requested encapsulation unavailable (HA)";
	case REGREP_UNSUPP_VENDOR_ID_MN_HA:
		return "unsupported vendor extension (MN->HA)";
	case REGREP_UNSUPP_VENDOR_ID_FA_HA:
		return "unsupported vendor extension (FA->HA)";
	}

	return "UNKNOWN CODE";
}


unsigned int usec_passed(struct timeval *tv, struct timeval *now)
{
	struct timeval t;
	unsigned long diff;
	
	if (now == NULL) {
		gettimeofday(&t, NULL);
		diff = (t.tv_sec - tv->tv_sec) * 1000000 + 
			t.tv_usec - tv->tv_usec;
	}
	else {
		diff = (now->tv_sec - tv->tv_sec) * 1000000 + 
			now->tv_usec - tv->tv_usec;
	}

	return diff;
}


unsigned int sec_passed(struct timeval *tv)
{
	struct timeval now;
	gettimeofday(&now, NULL);
	return (now.tv_sec - tv->tv_sec);
}

/* returns 1, 0, -1 when tv1 is greater, equal or less than tv2 */
int cmp_timeval(struct timeval *tv1, struct timeval *tv2)
{
	if (tv1->tv_sec < tv2->tv_sec) return -1;
	if (tv1->tv_sec > tv2->tv_sec) return 1;
	if (tv1->tv_usec < tv2->tv_usec) return -1;
	if (tv1->tv_usec > tv2->tv_usec) return 1;
	return 0;
}


void add_usecs(struct timeval *tv, unsigned int usecs)
{
	tv->tv_usec += usecs;
	tv->tv_sec += tv->tv_usec / 1000000;
	tv->tv_usec = tv->tv_usec % 1000000;
}

void set_usecs(struct timeval *tv, unsigned int const usecs)
{
	assert(tv != NULL);
	tv->tv_sec = (unsigned long int) usecs / 1000000;
	tv->tv_usec = (unsigned long int) usecs % 1000000;
}


#define MAX_REPORT_PAYLOAD 100
void report_discarded_msg(const char *msg, int len, struct sockaddr_in *from,
			  const char *reason)
{
	char payload[MAX_REPORT_PAYLOAD * 3 + 100], *pos;
	int i, max;

	syslog(LOG_INFO, "discarded message from %s:%i, len=%i, reason: %s",
	       inet_ntoa(from->sin_addr), ntohs(from->sin_port), len, reason);
	dynamics_strlcpy(payload, "   payload:", sizeof(payload));
	if (len > MAX_REPORT_PAYLOAD)
		max = MAX_REPORT_PAYLOAD;
	else
		max = len;
	pos = payload;
	while (*pos != '\0') pos++;
	for (i = 0; i < max; i++) {
		snprintf(pos, 4, " %02x", msg[i]);
		pos += 3;
	}
	if (len > max)
		dynamics_strlcpy(pos, " ...",
				 sizeof(payload) - (pos - payload));
	syslog(LOG_INFO, "%s", payload);
}

int get_hex_digit(const char c)
{
        if (c >= '0' && c <= '9') return (c - '0');
        if (c >= 'a' && c <= 'f') return (c - 'a' + 10);
        if (c >= 'A' && c <= 'F') return (c - 'A' + 10);
        return -1;
}

/**
 * dynamics_strlcpy:
 * @dst: destination
 * @src: source
 * @size: maximum size of @dst
 *
 * Copy up to size-1 characters from @src to @dst and always NUL terminate
 * @dst string (as long as @size is not 0).
 *
 * Returns strlen(src); if this is >= @size, truncation occurred
 */
size_t dynamics_strlcpy(char *dst, const char *src, size_t size)
{
	char *d = dst;
	const char *s = src;
	size_t n;

	if (size == 0)
		return strlen(src);

	n = size - 1;
	while (n > 0) {
		if ((*d++ = *s++) == '\0')
			break;
		n--;
	}

	if (n == 0) {
		/* truncate and search the end of src */
		*d = '\0';
		while (*s != '\0')
			s++;
	}

	return (s - src);
}


/**
 * ip_checksum:
 * @data: Data to calculated checksum for
 * @len: Length of data
 * 
 * Internet checksum calculation (based on RFC 1071)
 * 
 * Returns: Checksum
 */
int ip_checksum(unsigned char *data, int len)
{
	/* Compute Internet Checksum for "count" bytes
	 *         beginning at location "addr".
	 */
	register long sum = 0;
	__u16 *addr = (__u16 *) data;
	__u16 res;

	assert(data != NULL && len >= 0);

	/* add the 16-bit words */
        while (len > 1) {
		sum += *addr++;
		len -= 2;
	}

	/* add left-over byte, if any */
	if (len > 0)
		sum += *((unsigned char *) addr);

	/* fold 32-bit sum to 16 bits */
	while (sum >> 16)
		sum = (sum & 0xffff) + (sum >> 16);

	res = ~sum;
	return res;
}


/**
 * udp_checksum:
 * @ip: pointer to the IP header of the packet
 * @udp: pointer to the UDP header of the packet
 *
 * Calculate UDP checksum according to RFC 768.
 * Example use: udp->check = 0; udp->check = udp_checksum(ip, udp);
 *
 * Returns: Calculated checksum in network byte order
 */
__u16 udp_checksum(struct iphdr *ip, struct udphdr *udp)
{
	unsigned long sum = 0;
	__u16 *pos;
	int left;

	/* add pseudo header */
	sum += (ip->saddr >> 16) + (ip->saddr & 0xffff);
	sum += (ip->daddr >> 16) + (ip->daddr & 0xffff);
	sum += htons(ip->protocol);
	sum += udp->len;

	left = ntohs(udp->len);
	pos = (__u16 *) udp;
	while (left > 1) {
		sum += *pos++;
		left -= 2;
	}
	if (left)
		sum += htons(*((__u8 *) pos) << 8);

	/* fold 32-bit sum to 16 bits */
	while (sum >> 16)
		sum = (sum & 0xffff) + (sum >> 16);

	return (~sum & 0xffff);
}