cdp.c

Net-CDP-0.09 cdp相关源包

		cdp_llist_t *ports;
		cdp_llist_iter_t iter;
		struct cdp_address *address;

		if (!(ports = cdp_get_ports(errors)))
			goto fail;
		for (
			iter = cdp_llist_iter(ports);
			iter;
			iter = cdp_llist_next(iter)
		)
			if (!port || (
				strcmp((char *)cdp_llist_get(iter), port) == 0
			))
				break;
		if (!iter) {
			if (port)
				sprintf(errors, "Port %s not found", port);
			else
				strcpy(errors, "No available ports found");
			cdp_llist_free(ports);
			goto fail;
		}

		cdp->port = strdup((char *)cdp_llist_get(iter));
		cdp_llist_free(ports);

		/*
		 * I think we can reliably only grab the first address
		 * (unless we use that SIOCGIFCONF trick mentioned above).
		 */
		if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
			snprintf(errors, (CDP_ERRBUF_SIZE - 1) * sizeof(char),
				"Error opening socket: %s", strerror(errno));
			errors[CDP_ERRBUF_SIZE - 1] = '\0';
			goto fail;
		}
		memset(&ifrflags, 0, sizeof ifrflags);
		strncpy(ifrflags.ifr_name, cdp->port, sizeof ifrflags.ifr_name);
		if (ioctl(sock, SIOCGIFADDR, (char *)&ifrflags) < 0) {
			snprintf(errors, (CDP_ERRBUF_SIZE - 1) * sizeof(char),
				"SIOCGIFADDR error getting flags for interface %s: %s",
				cdp->port, strerror(errno));
			errors[CDP_ERRBUF_SIZE - 1] = '\0';
			close(sock);
			goto fail;
		}
		close(sock);

		address = NULL;
		switch (ifrflags.ifr_addr.sa_family) {
		case AF_INET:
			address = cdp_address_new(
				cdp_predefs[CDP_ADDR_PROTO_IPV4].protocol_type,
				cdp_predefs[CDP_ADDR_PROTO_IPV4].protocol_length,
				cdp_predefs[CDP_ADDR_PROTO_IPV4].protocol,
				sizeof(((struct sockaddr_in *)&ifrflags.ifr_addr)->sin_addr),
				&((struct sockaddr_in *)&ifrflags.ifr_addr)->sin_addr
			);
			break;
		case AF_INET6:
			address = cdp_address_new(
				cdp_predefs[CDP_ADDR_PROTO_IPV6].protocol_type,
				cdp_predefs[CDP_ADDR_PROTO_IPV6].protocol_length,
				cdp_predefs[CDP_ADDR_PROTO_IPV6].protocol,
				sizeof(((struct sockaddr_in6 *)&ifrflags.ifr_addr)->sin6_addr),
				&((struct sockaddr_in6 *)&ifrflags.ifr_addr)->sin6_addr
			);
			break;
		}
		if (address) {
			cdp->addresses = cdp_llist_new(
				(cdp_dup_fn_t)cdp_address_dup,
				(cdp_free_fn_t)cdp_address_free
			);
			cdp_llist_append(cdp->addresses, address);
		}
	}
	
	if (pcap_lookupnet(cdp->port, &net, &mask, pcap_errors) == -1) {
		strncpy(errors, pcap_errors, CDP_ERRBUF_SIZE - 1);
		errors[CDP_ERRBUF_SIZE - 1] = '\0';
		goto fail;
	}
	
	if (!(cdp->flags & CDP_DISABLE_RECV)) {
		if (!(cdp->pcap = pcap_open_live(cdp->port, BUFSIZ,
					cdp->flags & CDP_PROMISCUOUS, 0, pcap_errors))) {
			strncpy(errors, pcap_errors, CDP_ERRBUF_SIZE - 1);
			errors[CDP_ERRBUF_SIZE - 1] = '\0';
			goto fail;
		}
		if (!(cdp->flags & CDP_PROMISCUOUS)) {
			if (_cdp_multicast(cdp, 1) < 0) {
				snprintf(errors, (CDP_ERRBUF_SIZE - 1) * sizeof(char),
					"Could not enable multicast address for interface %s: %s",
					cdp->port, strerror(errno));
				errors[CDP_ERRBUF_SIZE - 1] = '\0';
				goto fail;
			}
		}

		if (pcap_compile(cdp->pcap, &filter, BPF_FILTER, 1, mask)) {
			strncpy(errors, pcap_geterr(cdp->pcap), CDP_ERRBUF_SIZE - 1);
			errors[CDP_ERRBUF_SIZE - 1] = '\0';
			goto fail_multi;
		}

		if (pcap_setfilter(cdp->pcap, &filter)) {
			strncpy(errors, pcap_geterr(cdp->pcap), CDP_ERRBUF_SIZE - 1);
			errors[CDP_ERRBUF_SIZE - 1] = '\0';
			pcap_freecode(&filter);
			goto fail_multi;
		}
		pcap_freecode(&filter);
	}
	
	if (!(cdp->flags & CDP_DISABLE_SEND)) {
		if (!(cdp->libnet = libnet_init(LIBNET_LINK, cdp->port, libnet_errors))) {
			strncpy(errors, libnet_errors, CDP_ERRBUF_SIZE - 1);
			errors[CDP_ERRBUF_SIZE - 1] = '\0';
			goto fail_multi;
		}

		/*
		 * Save the hardware address for when cdp_send is called.
		 */
		if (!(hwaddr = libnet_get_hwaddr(cdp->libnet))) {
			strncpy(errors, libnet_geterror(cdp->libnet), CDP_ERRBUF_SIZE - 1);
			errors[CDP_ERRBUF_SIZE - 1] = '\0';
			goto fail_multi;
		}
		memcpy(cdp->mac, hwaddr->ether_addr_octet, 6 * sizeof(uint8_t));
	}
	
#if HAVE_LINUX_ETHTOOL_H
	/*
	 * Grab the duplex mode of the interface now.
	 */
	_cdp_duplex(cdp);
#endif /* HAVE_ETHTOOL_H */
	
	FREE(libnet_errors);
	FREE(pcap_errors);
	return cdp;

fail_multi:
	if (!(cdp->flags & CDP_DISABLE_RECV))
		if (!(cdp->flags & CDP_PROMISCUOUS))
			_cdp_multicast(cdp, 0); /* Ignore errors */
	
fail:
	cdp_free(cdp);
	FREE(libnet_errors);
	FREE(pcap_errors);
	return NULL;
}

void
cdp_free(cdp_t *cdp) {
	if (cdp->port) {
		if (!(cdp->flags & CDP_DISABLE_RECV))
			if (!(cdp->flags & CDP_PROMISCUOUS))
				_cdp_multicast(cdp, 0); /* Ignore errors */
		FREE(cdp->port);
	}
	if (cdp->libnet) libnet_destroy(cdp->libnet);
	if (cdp->pcap) pcap_close(cdp->pcap);
	if (cdp->addresses) cdp_llist_free(cdp->addresses);
	if (cdp->duplex) FREE(cdp->duplex);
	FREE(cdp);
}

const char *
cdp_get_port(const cdp_t *cdp) {
	return cdp->port;
}

const cdp_llist_t *
cdp_get_addresses(const cdp_t *cdp) {
	return cdp->addresses;
}

const uint8_t *
cdp_get_duplex(const cdp_t *cdp) {
	return cdp->duplex;
}

int
cdp_get_fd(const cdp_t *cdp) {
	return cdp->flags & CDP_DISABLE_RECV ? -1 : pcap_fileno(cdp->pcap);
}

static void
timeval_subtract(struct timeval *result, const struct timeval *x,
		const struct timeval *y) {
	struct timeval yy = { y->tv_sec, y->tv_usec };
	
	if (x->tv_usec < yy.tv_usec) {
		int nsec = (yy.tv_usec - x->tv_usec) / 1000000.0 + 1;
		yy.tv_usec -= 1000000.0 * nsec;
		yy.tv_sec += nsec;
	}
	if (x->tv_usec - yy.tv_usec > 1000000.0) {
		int nsec = (x->tv_usec - yy.tv_usec) / 1000000.0;
		yy.tv_usec += 1000000.0 * nsec;
		yy.tv_sec -= nsec;
	}
	result->tv_sec = x->tv_sec - yy.tv_sec;
	result->tv_usec = x->tv_usec - yy.tv_usec;
}

struct cdp_packet *
cdp_recv(cdp_t *cdp, int flags, char *errors) {
	char *pcap_errors;
	struct cdp_packet *packet;
	
	if (cdp->flags & CDP_DISABLE_RECV) {
		sprintf(errors, "Can not receive with CDP_DISABLE_RECV set");
		return NULL;
	}
	
	pcap_errors = SALLOC(PCAP_ERRBUF_SIZE);
	packet = NULL;
	
#if HAVE_PCAP_SETNONBLOCK
	if (pcap_setnonblock(cdp->pcap, flags & CDP_RECV_NONBLOCK, pcap_errors)) {
		strncpy(errors, pcap_errors, (CDP_ERRBUF_SIZE - 1) * sizeof(char));
		errors[CDP_ERRBUF_SIZE - 1] = '\0';
		goto fail;
	}
#else /* ! HAVE_PCAP_SETNONBLOCK */
	{
		int fd, current;
		
		fd = pcap_fileno(cdp->pcap);
		if ((current = fcntl(fd, F_GETFL, 0)) == -1) {
			snprintf(errors, (CDP_ERRBUF_SIZE - 1) * sizeof(char),
					"Could not get socket flags: %s", strerror(errno));
			errors[CDP_ERRBUF_SIZE - 1] = '\0';
			goto fail;			
		}
		if (flags & CDP_RECV_NONBLOCK)
			current |= O_NONBLOCK;
		else
			current &= ~O_NONBLOCK;
		if (fcntl(fd, F_SETFL, current) == -1) {
			snprintf(errors, (CDP_ERRBUF_SIZE - 1) * sizeof(char),
					"Could not set socket flags: %s", strerror(errno));
			errors[CDP_ERRBUF_SIZE - 1] = '\0';
			goto fail;			
		}
	}
#endif /* ! HAVE_PCAP_SETNONBLOCK */
	
	do {
		int result;
		
		/*
		 * Use pcap_dispatch, not pcap_next, so that read errors can be
		 * detected in non-blocking mode.
		 */
		result = pcap_dispatch(
			cdp->pcap, 1, (pcap_handler)_cdp_callback,
			(uint8_t *)cdp
		);
		if (result < 0) {
			strncpy(errors, pcap_geterr(cdp->pcap), CDP_ERRBUF_SIZE);
			errors[CDP_ERRBUF_SIZE - 1] = '\0';
			goto fail;
		}
		if (result) {
			packet = cdp_decode(cdp->data, cdp->header->caplen, errors);
			if (!packet) {
				if (flags & CDP_RECV_DECODE_ERRORS)
					goto fail; /* errors is already set */
				errors[0] = '\0';
			}
		} else if (flags & CDP_RECV_NONBLOCK)
			goto fail;
	} while (!packet);
	
	FREE(pcap_errors);
	return packet;

fail:
	FREE(pcap_errors);
	return NULL;
}

int
cdp_send(cdp_t *cdp, const struct cdp_packet *packet, char *errors) {
	static uint8_t dst[6] = { 0x01, 0x00, 0x0c, 0xcc, 0xcc, 0xcc };
	static uint8_t oui[3] = { 0x00, 0x00, 0x0c };
	
	int result;
	
	if (cdp->flags & CDP_DISABLE_SEND) {
		sprintf(errors, "Can not send with CDP_DISABLE_SEND set");
		return -1;
	}

	if (libnet_build_data(
			(packet->packet_length ? packet->packet : NULL), packet->packet_length,
			cdp->libnet, 0
	) == -1)
		goto fail;
	if (libnet_build_802_2snap(
			0xaa, 0xaa,
			0x03, oui,
			0x2000,
			NULL, 0,
			cdp->libnet, 0
	) == -1)
		goto fail;
	/* length is 802.2 SNAP header + CDP's length */
	if (libnet_build_802_3(
			dst, cdp->mac,
			LIBNET_802_2SNAP_H + packet->packet_length,
			NULL, 0,
			cdp->libnet, 0
	) == -1)
		goto fail;
	if ((result = libnet_write(cdp->libnet)) == -1)
		goto fail;
		
	libnet_clear_packet(cdp->libnet);
	return result;

fail:
	libnet_clear_packet(cdp->libnet);
	strncpy(errors, libnet_geterror(cdp->libnet), CDP_ERRBUF_SIZE);
	errors[CDP_ERRBUF_SIZE - 1] = '\0';
	return -1;
}