udp.c

增加了tftp功能的vivi代码

#include <string.h>
#include "net.h"

u8 phony_header[50]={192,168,1,10,192,168,1,190,0,17,0,0};

struct UDP_packet{
u16 source_port;
u16 destination_port;
u16 udp_length;
u16 check_sum;
u8 UDP_data[600];
};

u16 tftp_server_port;
u16 temp_port;

s32 unpack_UDP(u8 * packet)
{
 u8 * p;
 u16 packet_length;
 struct UDP_packet * upack;
 upack=(struct UDP_packet *)packet;
 phony_header[10]=(u8)(upack->udp_length);
 phony_header[11]=(u8)((upack->udp_length)>>8);
 //p=memcpy(&phony_header[12],upack,packet_length);
 p=phony_header;
 packet_length=shift16(upack->udp_length);
 if(0xffff!=check(p,12)+check(packet,packet_length)) return -2;
 temp_port=shift16(upack->source_port);
 if(temp_port!=tftp_server_port&&tftp_server_port!=69) return -2;
 return shift16(upack->udp_length);//检查端口是否正确

}

s32 pack_UDP(u8 * packet,s32 packet_length)
{
  struct UDP_packet * upack;
  int i;
  u8 *p;
  u16 check_sum;
  for(i=packet_length-1;i>=0;i--)
  {
	  packet[i+8]=packet[i];
  }
  upack=(struct UDP_packet *)packet;
  upack->source_port=0xCDAB;//??????????
  upack->destination_port=shift16(tftp_server_port);//???????????????????????
  upack->udp_length=shift16(packet_length+8);
  upack->check_sum=0;//???????
  phony_header[10]=(u8)(upack->udp_length);
  phony_header[11]=(u8)((upack->udp_length)>>8);
  memcpy(&phony_header[12],upack,packet_length+8);
  p=phony_header;
  check_sum=check(p,packet_length+20);
  upack->check_sum =shift16(check_sum);
  return shift16(upack->udp_length);
}


u16 check(u8 * addr,u16 count)
{
	 /* Compute Internet Checksum for "count" bytes
            * beginning at location "addr".
            */
            register long sum = 0;
			u16 check_sum;
            while( count > 1 ) {
            /* This is the inner loop */
            sum += shift16(* (unsigned short*) addr);
			addr++;
			addr++;
            count -= 2;
            }

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

            /* Fold 32-bit sum to 16 bits */
            while (sum>>16)
            sum = (sum & 0xffff) + (sum >> 16);
			check_sum=(u16)~sum;
			return check_sum;

}