udp.c

来自「ADS下的bios工程」· C语言 代码 · 共 112 行

#include <bios/netdev.h>
#include <bios/checksum.h>

#undef DEBUG_S3C4_UDP
#ifdef DEBUG_S3C4_UDP
	#define DEBUG_UDP(fmt, args...) printf("%s-%s()[%d]: " fmt, __FILE__, __FUNCTION__, __LINE__, args)
#else
	#define DEBUG_UDP(fmt, args...)
#endif

extern int ip_send(struct netdev *nd, int protocol, u32 from, u32 to, struct buflist *data);
extern int ip_recv(struct netdev *nd, int protocol, u32 from, u32 to, u8 *buffer);

int udp_send(struct netdev *nd, struct sin *from, struct sin *to, struct buflist *data);
int udp_recv(struct netdev *nd, struct sin *from, struct sin *to,void *buffer, int size);

static unsigned short
udp_check(struct udphdr *hdr, struct sin *from, struct sin *to, struct buflist *data, int size)
{
	static struct buflist *blp;
	int check;

	check = checksum(&from->sin_addr, sizeof(from->sin_addr), 0x11 + size);
        DEBUG_UDP("checksum(%x)\n",check);
	check = checksum(&to->sin_addr, sizeof(to->sin_addr), check);
        DEBUG_UDP("checksum(%x)\n",check);

	for (blp = data; blp; blp = blp->next)
		check = checksum(blp->data, blp->size, check);
        DEBUG_UDP("checksum(%x)\n",check);

	check = ~check;

	if (check == 0)
		check = -1;

	return htons(check);
}

int udp_send(struct netdev *nd, struct sin *from, struct sin *to, struct buflist *data)
{
	static struct buflist bl, *blp;
	static struct udphdr udp;
	int size = 0;

	bl.data = &udp;
	bl.size = sizeof(udp);
	bl.next = data;

	for (blp = &bl; blp; blp = blp->next)
		size += blp->size;

	udp.udp_source = from->sin_port;
	udp.udp_dest   = to->sin_port;
	udp.udp_length = htons(size);
	udp.udp_check  = 0;
	udp.udp_check  = udp_check(&udp, from, to, &bl, size);

        DEBUG_UDP("from(%x),to(%x),&UDP_Pkt(%x)\n",from->sin_addr, to->sin_addr, &bl);
	return ip_send(nd, 0x11, from->sin_addr, to->sin_addr, &bl);
}

int udp_recv(struct netdev *nd, struct sin *from, struct sin *to,
	     void *buffer, int size)
{
	static u8 udp_buffer[1564];
	struct udphdr *udp;
	int bytes;

        DEBUG_UDP("\n%s", "->");
	bytes = ip_recv(nd, ETH_P_UDP, from->sin_addr, to->sin_addr, udp_buffer);
        DEBUG_UDP("BYTES(%d)\n", bytes);

	udp = (struct udphdr *)udp_buffer; /* 2001-2-24 */

	if (bytes) {
		if (bytes < sizeof(struct udphdr)) {
			DEBUG_UDP("INVALID length(%d)\n", bytes);
			return 0;
		}

		if (bytes < htons(udp->udp_length)) {
			DEBUG_UDP("WRONG length (%d)", bytes, htons(udp->udp_length));
			DEBUG_UDP("\thtons(udp->udp_length)(%d)", htons(udp->udp_length));
			DEBUG_UDP("\tudp->udp_length(%d)\n", udp->udp_length);
			return 0;
		}

		if (to->sin_port != 0) {
			if (to->sin_port != udp->udp_dest) {
				DEBUG_UDP("WRONG dest port(%x)\n",to->sin_port);
				return 0;
			}
		} else
			to->sin_port = udp->udp_dest;

		if (from->sin_port != 0) {
			if (from->sin_port != udp->udp_source) {
				DEBUG_UDP("WRONG source port(%x)\n",from->sin_port);
				return 0;
			}
		} else
			from->sin_port = udp->udp_source;

		bytes -= sizeof(*udp);
		memcpy(buffer, udp_buffer + sizeof(*udp), bytes);
	}

	return bytes;
}