使用 UDP 方式傳送與接收
定義
// <linux/if_ether.h>
struct ethhdr {
unsigned char h_dest[ETH_ALEN]; /* destination eth addr */
unsigned char h_source[ETH_ALEN]; /* source ether addr */
__be16 h_proto; /* packet type ID field */
} __attribute__((packed));
// <linux/ip.h>
struct iphdr {
#if defined(__LITTLE_ENDIAN_BITFIELD)
__u8 ihl:4,
version:4;
#elif defined (__BIG_ENDIAN_BITFIELD)
__u8 version:4,
ihl:4;
#else
#error "Please fix <asm/byteorder.h>"
#endif
__u8 tos;
__be16 tot_len;
__be16 id;
__be16 frag_off;
__u8 ttl;
__u8 protocol;
__sum16 check;
__be32 saddr;
__be32 daddr;
/*The options start here. */
};
// <linux/udp.h>
struct udphdr {
__be16 source;
__be16 dest;
__be16 len;
__sum16 check;
};
//copy from <linux/netfilter_ipv4/ipt_NETLINK.h>
struct netlink_t {
unsigned int len;
unsigned int mark;
char iface[16]; // IFNAMSIZ = 16
};
static int group = 1; /* mcast groups */
UDP Server 範例
{
struct sockaddr_in address;
int sockfd;
char buffer[1024];
struct ip_mreq command;
int reuse = 1;
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0)
return;
if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (char *)&reuse, sizeof(reuse)) < 0) {
close(sockfd);
return;
}
memset(&address, 0, sizeof(address));
address.sin_family = AF_INET;
address.sin_addr.s_addr = htonl(INADDR_ANY);
address.sin_port = htons(10234);
if (bind(sockfd, (struct sockaddr *)&address, sizeof(struct sockaddr_in)) < 0)
return;
command.imr_multiaddr.s_addr = inet_addr("224.0.0.1"); // address.sin_addr;
command.imr_interface.s_addr = htonl(INADDR_ANY);
setsockopt(sockfd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &command, sizeof(command));
memset(buffer, '\0', sizeof(buffer));
while (recv(sockfd, buffer, 1024, 0)) {
syslog(1, "recv buffer=%s", buffer);
}
close(sockfd);
}
UDP Client 範例
{
struct sockaddr_in address;
int sockfd;
char *send_str = "sendData";
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
memset(&address, 0, sizeof(address));
address.sin_family = AF_INET;
address.sin_addr.s_addr = inet_addr("224.0.0.1");
address.sin_port = htons(10234);
sendto(sockfd, send_str, strlen(send_str), 0, &address, sizeof(struct sockaddr_in));
close(sockfd);
}
接收 NETLINK 後重送 UDP 封包 (DHCP, ARP)
void udp_resend()
{
struct sockaddr_nl src_addr;
struct netlink_t *nlhdr;
struct ethhdr *eth;
struct iphdr *ip;
struct udphdr *udp;
char *data;
int sock_fd, size, offset;
char buffer[2048], PseudoHeader[16];
memset(buffer, '\0', sizeof(buffer));
sock_fd = socket(PF_NETLINK, SOCK_RAW, NETLINK_USERSOCK); // NETLINK_USERSOCK , NETLINK_FIREWALL
memset(&src_addr, 0, sizeof(src_addr));
src_addr.nl_family = AF_NETLINK;
src_addr.nl_pid = getpid(); /* self pid */
src_addr.nl_groups = group; /* mcast groups */
bind(sock_fd, (struct sockaddr*)&src_addr,sizeof(src_addr));
while(1) {
if(recv(sock_fd, buffer, 2048, 0) > 0) {
nlhdr = (struct netlink_t *)buffer;
eth = (struct ethhdr *)&buffer[sizeof(struct netlink_t)];
ip = (struct iphdr *)&buffer[sizeof(struct netlink_t) + sizeof(struct ethhdr)];
udp = (struct udphdr *)&buffer[sizeof(struct netlink_t) + sizeof(struct ethhdr) + sizeof(struct iphdr)];
data = &buffer[sizeof(struct netlink_t) + sizeof(struct ethhdr) + sizeof(struct iphdr) + sizeof(struct udphdr)];
offset = sizeof(struct netlink_t) + sizeof(struct ethhdr);
size = nlhdr->len - offset;
/* check is dhcp Request packet */
if((eth->h_dest[0]&0xff)==0xff && (eth->h_dest[1]&0xff)==0xff &&
(eth->h_dest[2]&0xff)==0xff && (eth->h_dest[3]&0xff)==0xff &&
(eth->h_dest[4]&0xff)==0xff && (eth->h_dest[5]&0xff)==0xff
) {
if(htons(eth->h_proto) == 0x800) { /* Next is IP Header */
if(ip->protocol == 17) { /* Next is UDP Header */
if(htons(udp->source) == 68 && htons(udp->dest) == 67 && *data == 1) { /* is DHCP Request */
data[10] = 0x80; // set broadcast flag (長度應該是 2 Bytes 現在只設定第一個Byte, 另一個Byte保持原樣即可)
/* udp checksum */
memcpy(&PseudoHeader[0], &(ip->saddr), 4);
memcpy(&PseudoHeader[4], &(ip->daddr), 4);
PseudoHeader[8] = 0;
PseudoHeader[9] = ip->protocol;
memcpy(&PseudoHeader[10], &(udp->len), 2);
// 有改到IP HEADER才要重算CHECKSUM
udp->check = 0; // 一定要先設為0, 再重算checksum
udp->check = htons(checksum(udp, size-sizeof(struct iphdr), PseudoHeader, 12));
raw_packet(nlhdr->iface, &buffer[offset], size);
} else if(htons(udp->source) == 67 && htons(udp->dest) == 68 && *data == 2) { /* is DHCP Reply */
// 可轉送其他介面
raw_packet(nlhdr->iface, 9, &buffer[offset], size);
}
}
} else if(htons(eth->h_proto) == 0x806) { /* is ARP */
// 可轉送其他介面
raw_packet(nlhdr->iface, &buffer[offset], size);
}
}
}
}
close(sock_fd);
}
共用函式
void raw_packet(char *iface, char *data, int len) {
int fd;
struct ifreq ifr;
struct sockaddr_ll dest;
memset(&dest, 0, sizeof(dest));
// Create Socket
if((fd = socket(PF_PACKET, SOCK_DGRAM, htons(ETH_P_IP))) < 0) { // ETH_P_IP , ETH_P_ALL
perror("create socket fail");
return;
}
// get ifindex
memset ((void*)&ifr, 0, sizeof (ifr));
snprintf (ifr.ifr_name, sizeof (ifr.ifr_name), iface);
if(ioctl (fd, SIOCGIFINDEX, &ifr) < 0) {
close (fd);
perror("ioctl fail");
return;
}
// Bind
dest.sll_family = AF_PACKET;
dest.sll_protocol = htons(ETH_P_ARP); // ETH_P_IP , ETH_P_ALL , ETH_P_ARP
dest.sll_ifindex = ifr.ifr_ifindex; //IF_PORT_10BASET;
dest.sll_pkttype = PACKET_BROADCAST;
dest.sll_halen = 6; // ARPHRD_IEEE802
memset(dest.sll_addr, 0xff, 6);
if (bind(fd, (struct sockaddr *)&dest, sizeof(struct sockaddr_ll)) < 0) {
close(fd);
return;
}
sendto(fd, data, len, 0, (struct sockaddr *)&dest, sizeof(dest));
close(fd);
}
uint16_t checksum(void *addr, int count, void *addr2, int count2)
{
/* Compute Internet Checksum for "count" bytes
* beginning at location "addr".
*/
register int32_t sum = 0;
uint16_t *source = (uint16_t *) addr;
while (count > 1) {
/* This is the inner loop */
sum += *source++;
count -= 2;
}
/* Add left-over byte, if any */
if (count > 0) {
/* Make sure that the left-over byte is added correctly both
* with little and big endian hosts */
uint16_t tmp = 0;
*(uint8_t *) (&tmp) = * (uint8_t *) source;
sum += tmp;
}
/* addr2 */
if(addr2) {
source = (uint16_t *) addr2;
while (count2 > 1) {
/* This is the inner loop */
sum += *source++;
count2 -= 2;
}
}
/* Fold 32-bit sum to 16 bits */
while (sum >> 16)
sum = (sum & 0xffff) + (sum >> 16);
return ~sum;
}
沒有留言:
張貼留言