int ether2udp(buf_t *bp)
{
/* Transform an UDP packet read from raw Ethernet to normal UDP.
* Return true iff the packet is indeed UDP and has no errors.
*/
udp_io_hdr_t udpio;
if (bp->eth->eh_proto != htons(ETH_IP_PROTO)
|| bp->ip->ih_vers_ihl != 0x45
|| bp->ip->ih_proto != IPPROTO_UDP
|| oneC_sum(0, bp->ip, 20) != (u16_t) ~0
|| udp_cksum(bp->ip->ih_src, bp->ip->ih_dst, bp->udp) != (u16_t) ~0
) {
/* Not UDP/IP or checksums bad. */
return 0;
}
udpio.uih_src_addr= bp->ip->ih_src;
udpio.uih_dst_addr= bp->ip->ih_dst;
udpio.uih_src_port= bp->udp->uh_src_port;
udpio.uih_dst_port= bp->udp->uh_dst_port;
udpio.uih_ip_opt_len= 0;
udpio.uih_data_len= ntohs(bp->udp->uh_length) - sizeof(udp_hdr_t);
*bp->udpio= udpio;
return 1;
}
int
usnet_udp_input(usn_context_t *ctx, usn_iphdr_t *iph, int iplen)
{
usn_udphdr_t *udph;
uint16_t port;
unsigned char* payload;
int udplen;
DEBUG(ctx->log, "udp packet");
udph = (usn_udphdr_t *)((char*)iph + (iph->ip_hl << 2));
udplen = iplen - (iph->ip_hl << 2);
payload = (unsigned char*)usnet_ipv4_output_alone(ctx, iph->ip_id,
IPPROTO_UDP, iph->ip_dst, iph->ip_src, udplen);
if ( payload == 0 ) {
ERROR(ctx->log,"could not get buffer");
return -1;
}
//swap ports
port = udph->uh_dport;
udph->uh_dport = udph->uh_sport;
udph->uh_sport = port;
// check sum
udph->uh_sum = 0;
udph->uh_sum = udp_cksum((uint16_t*)udph, udplen, iph->ip_dst, iph->ip_src);
memcpy(payload, udph, udplen);
usnet_send_data(ctx);
return 0;
}
u_short get_udpsum
(
struct ip * pIph, /* IP header */
struct udphdr * pUdph /* UDP header */
)
{
struct ps_udph UdpPh; /* UDP pseudo-header */
bzero ( (char *)&UdpPh, sizeof (UdpPh));
UdpPh.srcip.s_addr = pIph->ip_src.s_addr;
UdpPh.dstip.s_addr = pIph->ip_dst.s_addr;
UdpPh.zero = 0;
UdpPh.prto = IPPROTO_UDP;
UdpPh.ulen = pUdph->uh_ulen;
pUdph->uh_sum = 0;
return (udp_cksum (&UdpPh, (char *)pUdph, ntohs (UdpPh.ulen)));
}
static inline int build_udp(ufw_sk *sk, const void *pl, int pls){
struct udphdr *udp;
int hlen;
udp = (struct udphdr *)(sk->sendbuf + pls);
if(!pls)pl = NULL;
if(pl == NULL)pls = 0;
hlen = sizeof(struct udphdr);
if(hlen + pls > IP_MAXPACKET){
errno = EMSGSIZE;
if(sk->opts & FATAL)die("payload");
return -1;
}
udp->source = sk->sport;
udp->dest = sk->dport;
udp->len = htons(hlen + pls);
if(pl != NULL)
memcpy(sk->sendbuf + hlen, pl, pls);
udp->check = udp_cksum(sk->sendbuf, hlen + pls);
return hlen + pls;
}
