encoding.c

Net-CDP-0.09 cdp相关源包

				DUPLICATE("COS for Untrusted Ports TLV");
			if (tlv_length != sizeof(uint8_t))
				INVALID2("COS for Untrusted Ports TLV length (not %d)",
					sizeof(uint8_t));
			{
				uint8_t untrusted_cos;
				if (!GRAB_UINT8(untrusted_cos))
					EOP("COS for Untrusted Ports TLV");
				NEW(packet->untrusted_cos,
					untrusted_cos, uint8_t);
			}
			break;
		case CDP_TYPE_MGMT_ADDRESS:
			if (packet->mgmt_addresses)
				DUPLICATE("Management Address TLV");
			if (!GRAB_UINT32(count))
				EOP("number of addresses in Management Address TLV");
			packet->mgmt_addresses = cdp_llist_new(
				(cdp_dup_fn_t)cdp_address_dup,
				(cdp_free_fn_t)cdp_address_free
			);
			for (i = 0; i < count; i++) {
				uint8_t protocol_type;
				uint8_t protocol_length;
				const void *protocol;
				uint16_t address_length;
				const void *address;
				if (!GRAB_UINT8(protocol_type))
					EOP2("protocol type for address %d in Management Address TLV", i);
				if (!GRAB_UINT8(protocol_length))
					EOP2("protocol length for address %d in Management Address TLV", i);
				protocol = data;
				if (!SKIP(protocol_length))
					EOP2("protocol for address %d in Management Address TLV", i);
				if (!GRAB_UINT16(address_length))
					EOP2("address length for address %d in Management Address TLV", i);
				address = data;
				if (!SKIP(address_length))
					EOP2("address value for address %d in Management Address TLV", i);
				cdp_llist_append(packet->mgmt_addresses,
					cdp_address_new(
						protocol_type, protocol_length,
						protocol, address_length,
						address
					)
				);
			}
			break;
		default:
			/*
			 * Ignore the TLV. If it's an error, it will most
			 * likely get picked up here (remaining length isn't
			 * long enough), or the next TLV will be invalid.
			 */
			if (!SKIP(tlv_length)) EOP("unknown TLV");
			break;
		}
	}
	
	return packet;

fail:
	cdp_packet_free(packet);
	
	return NULL;
}

/*
 * Decode enough of the buffer to determine the checksum. This is so
 * cdp_packet_update can do its stuff.
 */
uint16_t
cdp_decode_checksum(const void *data, size_t length) {
	if (length >= 2 + sizeof(uint16_t))
		return ntohs(*(uint16_t *)(((uint8_t *)data) + 2));
	else
		return 0;
}

#define PUSH(value, type, func) \
	((length >= sizeof(type)) && \
		( \
			*((type*)pos) = func(value), \
			length -= sizeof(type), \
			pos += sizeof(type), \
			1 \
		))
#define PUSH_UINT8(value) PUSH(value, uint8_t, )
#define PUSH_UINT16(value) PUSH(value, uint16_t, htons)
#define PUSH_UINT32(value) PUSH(value, uint32_t, htonl)
#define PUSH_BYTES(value, bytes) \
	((length >= (bytes)) && \
		( \
			memcpy(pos, value, (bytes) * sizeof(uint8_t)), \
			length -= (bytes), \
			pos += (bytes), \
			1 \
		))

#define START_TLV(type) \
	( \
		tlv = pos, \
		PUSH_UINT16(type) && PUSH_UINT16(0) \
	)
#define END_TLV \
	( \
		*((uint16_t *)tlv + 1) = htons(pos - tlv) \
	)

size_t
cdp_encode(const struct cdp_packet *packet, void *data, size_t length) {
	uint8_t *pos;
	void *checksum_pos;
	uint8_t *tlv;
	
	pos = data;
	
	PUSH_UINT8(packet->version);
	if (!PUSH_UINT8(packet->ttl))
		return 0;

	/*
	 * Save the current position, then leave enough space for the
	 * checksum.
	 */
	checksum_pos = pos;
	if (!PUSH_UINT16(0))
		return 0;
	
	if (packet->device_id && !(
		START_TLV(CDP_TYPE_DEVICE_ID) &&
		PUSH_BYTES(packet->device_id, strlen(packet->device_id))
	))
		return 0;
	END_TLV;
	
	if (packet->addresses) {
		cdp_llist_iter_t iter;
		if (!(
			START_TLV(CDP_TYPE_ADDRESS) &&
			PUSH_UINT32(cdp_llist_count(packet->addresses))
		))
			return 0;
		for (
			iter = cdp_llist_iter(packet->addresses);
			iter;
			iter = cdp_llist_next(iter)
		) {
			struct cdp_address *address =
				(struct cdp_address *)cdp_llist_get(iter);
			if (!(
				PUSH_UINT8(address->protocol_type) &&
				PUSH_UINT8(address->protocol_length) &&
				PUSH_BYTES(address->protocol,
					address->protocol_length) &&
				PUSH_UINT16(address->address_length) &&
				PUSH_BYTES(address->address,
					address->address_length)
			))
				return 0;
		}
		END_TLV;
	}
	
	if (packet->port_id && !(
		START_TLV(CDP_TYPE_PORT_ID) &&
		PUSH_BYTES(packet->port_id, strlen(packet->port_id))
	))
		return 0;
	END_TLV;
	
	if (packet->capabilities && !(
		START_TLV(CDP_TYPE_CAPABILITIES) &&
		PUSH_UINT32(*packet->capabilities)
	))
		return 0;
	END_TLV;
	
	if (packet->ios_version && !(
		START_TLV(CDP_TYPE_IOS_VERSION) &&
		PUSH_BYTES(packet->ios_version, strlen(packet->ios_version))
	))
		return 0;
	END_TLV;
	
	if (packet->platform && !(
		START_TLV(CDP_TYPE_PLATFORM) &&
		PUSH_BYTES(packet->platform, strlen(packet->platform))
	))
		return 0;
	END_TLV;
	
	if (packet->ip_prefixes) {
		cdp_llist_iter_t iter;
		if (!START_TLV(CDP_TYPE_IP_PREFIX))
			return 0;
		for (
			iter = cdp_llist_iter(packet->ip_prefixes);
			iter;
			iter = cdp_llist_next(iter)
		) {
			struct cdp_ip_prefix *ip_prefix =
				(struct cdp_ip_prefix *)cdp_llist_get(iter);
			if (!(
				PUSH_UINT32(ip_prefix->network.s_addr) &&
				PUSH_UINT8(ip_prefix->length)
			))
				return 0;
		}
		END_TLV;
	}
	
	if (packet->vtp_mgmt_domain && !(
		START_TLV(CDP_TYPE_VTP_MGMT_DOMAIN) &&
		PUSH_BYTES(packet->vtp_mgmt_domain,
			strlen(packet->vtp_mgmt_domain))
	))
		return 0;
	END_TLV;
	
	if (packet->native_vlan && !(
		START_TLV(CDP_TYPE_NATIVE_VLAN) &&
		PUSH_UINT16(*packet->native_vlan)
	))
		return 0;
	END_TLV;
	
	if (packet->duplex && !(
		START_TLV(CDP_TYPE_DUPLEX) &&
		PUSH_UINT8(*packet->duplex)
	))
		return 0;
	END_TLV;
	
	if (packet->appliance && !(
		START_TLV(CDP_TYPE_APPLIANCE_REPLY) &&
		PUSH_UINT8(packet->appliance->id) &&
		PUSH_UINT16(packet->appliance->vlan)
	))
		return 0;
	END_TLV;
	
	if (packet->appliance_query && !(
		START_TLV(CDP_TYPE_APPLIANCE_QUERY) &&
		PUSH_UINT8(packet->appliance_query->id) &&
		PUSH_UINT16(packet->appliance_query->vlan)
	))
		return 0;
	END_TLV;
	
	if (packet->power_consumption && !(
		START_TLV(CDP_TYPE_POWER_CONSUMPTION) &&
		PUSH_UINT16(*packet->power_consumption)
	))
		return 0;
	END_TLV;
	
	if (packet->mtu && !(
		START_TLV(CDP_TYPE_MTU) &&
		PUSH_UINT32(*packet->mtu)
	))
		return 0;
	END_TLV;
	
	if (packet->extended_trust && !(
		START_TLV(CDP_TYPE_EXTENDED_TRUST) &&
		PUSH_UINT8(*packet->extended_trust)
	))
		return 0;
	END_TLV;
	
	if (packet->untrusted_cos && !(
		START_TLV(CDP_TYPE_UNTRUSTED_COS) &&
		PUSH_UINT8(*packet->untrusted_cos)
	))
		return 0;
	END_TLV;
	
	if (packet->mgmt_addresses) {
		cdp_llist_iter_t iter;
		if (!(
			START_TLV(CDP_TYPE_MGMT_ADDRESS) &&
			PUSH_UINT32(cdp_llist_count(packet->mgmt_addresses))
		))
			return 0;
		for (
			iter = cdp_llist_iter(packet->mgmt_addresses);
			iter;
			iter = cdp_llist_next(iter)
		) {
			struct cdp_address *address =
				(struct cdp_address *)cdp_llist_get(iter);
			if (!(
				PUSH_UINT8(address->protocol_type) &&
				PUSH_UINT8(address->protocol_length) &&
				PUSH_BYTES(address->protocol, address->protocol_length) &&
				PUSH_UINT16(address->address_length) &&
				PUSH_BYTES(address->address, address->address_length)
			))
				return 0;
		}
		END_TLV;
	}
	
	*(uint16_t *)checksum_pos = cdp_checksum(data, VOIDP_DIFF(pos, data));

	return VOIDP_DIFF(pos, data);
}