From: Joseph C. Lehner Date: Fri, 29 Jan 2016 11:16:05 +0000 (+0200) Subject: Update rawsock code X-Git-Tag: v0.9~87 X-Git-Url: https://git.librecmc.org/?a=commitdiff_plain;h=3b1cdc79312b5623bca7984605f7dc46bfa6c568;p=oweals%2Fnmrpflash.git Update rawsock code --- diff --git a/rawsock.c b/rawsock.c index 27888f6..500d3af 100644 --- a/rawsock.c +++ b/rawsock.c @@ -1,181 +1,142 @@ -#include -#include -#include -#include -#include #include -#include -#include +#include #include +#include -#ifndef MIN -#define MIN(a, b) ((a) < (b) ? (a) : (b)) -#endif - -struct ethsock +struct rawsock { - int fd; + pcap_t *pcap; struct timeval timeout; + int fd; }; -static int init_filter(struct ethsock *sock, int protocol, int mtu) +struct rawsock *rawsock_create(const char *interface) { - struct bpf_program prog; - struct bpf_insn insns[] = { - BPF_STMT(BPF_LD+BPF_H+BPF_ABS, 12), - BPF_JUMP(BPF_JMP+BPF_JEQ+BPF_K, protocol, 0, 1), - BPF_STMT(BPF_RET+BPF_K, mtu), - BPF_STMT(BPF_RET+BPF_K, 0) - }; - - prog.bf_insns = insns; - prog.bf_len = sizeof(insns) / sizeof(insns[0]); - - if (ioctl(sock->fd, BIOCSETFNR, &prog) != 0) { - perror("ioctl(BIOCSETFNR)"); - return -1; - } - - return 0; -} - -static int get_mtu(struct ifreq *ifr) -{ - int fd, stat; - - fd = socket(AF_INET, SOCK_DGRAM, 0); - if (fd < 0) { - perror("socket"); - return -1; - } + char errbuf[PCAP_ERRBUF_SIZE]; + struct bpf_program fp; + struct rawsock *sock; + int err; - stat = ioctl(fd, SIOCGIFMTU, ifr); - if (stat < 0) { - perror("ioctl(SIOCGIFMTU)"); - stat = -1; - } else { - stat = 0; + sock = malloc(sizeof(struct rawsock)); + if (!sock) { + perror("malloc"); + return NULL; } - close(fd); - return stat; -} + errbuf[0] = '\0'; -int ethsock_create(struct ethsock *sock, const char *interface, int protocol) -{ - struct ifreq ifr; - struct bpf_program bf; - int i, val; - char buf[12]; - - for (i = 0; i < 100; ++i) { - sprintf(buf, "/dev/bpf%d", i); - sock->fd = open(buf, O_RDWR); - if (sock->fd != -1) { - break; - } - } - - if (sock->fd == -1) { - fprintf(stderr, "Failed to open bpf device\n"); - return -1; + sock->pcap = pcap_open_live(interface, BUFSIZ, 1, 1, errbuf); + if (!sock->pcap) { + fprintf(stderr, "pcap_open_live: %s\n", errbuf); + goto cleanup_malloc; } - strncpy(ifr.ifr_name, interface, IFNAMSIZ - 1); - - if (ioctl(sock->fd, BIOCSETIF, &ifr) != 0) { - perror("ioctl(BIOCSETIF)"); - return -1; + if (*errbuf) { + fprintf(stderr, "Warning: %s.\n", errbuf); } - val = 1; - - if (ioctl(sock->fd, BIOCIMMEDIATE, &val) != 0) { - perror("ioctl(BIOCIMMEDIATE)"); - return -1; + if (pcap_datalink(sock->pcap) != DLT_EN10MB) { + fprintf(stderr, "Interface %s not supported.\n", interface); + goto cleanup_pcap; } - if (ioctl(sock->fd, BIOCGBLEN, &val) != 0) { - perror("ioctl(BIOCGBLEN)"); - return -1; + err = pcap_compile(sock->pcap, &fp, "ether proto 0x0912", 0, + PCAP_NETMASK_UNKNOWN); + if (err) { + pcap_perror(sock->pcap, "pcap_compile"); + goto cleanup_pcap; } - if (get_mtu(&ifr) != 0) { - return -1; + if ((err = pcap_setfilter(sock->pcap, &fp))) { + pcap_perror(sock->pcap, "pcap_setfilter"); + goto cleanup_pcap; } - if (init_filter(sock, protocol, ifr.ifr_mtu) != 0) { - return -1; + sock->fd = pcap_get_selectable_fd(sock->pcap); + if (sock->fd == -1) { + fprintf(stderr, "No selectable file descriptor available.\n"); + goto cleanup_pcap; } - sock->timeout.tv_sec = 0; - sock->timeout.tv_usec = 0; - - return 0; -} + return sock; -int ethsock_close(struct ethsock *sock) -{ - close(sock->fd); - return 0; +cleanup_pcap: + pcap_close(sock->pcap); +cleanup_malloc: + free(sock); + return NULL; } -int ethsock_set_timeout(struct ethsock *sock, unsigned msec) +int rawsock_recv(struct rawsock *sock, uint8_t **buffer, unsigned *size) { - sock->timeout.tv_sec = msec / 1000; - sock->timeout.tv_usec = (msec % 1000) * 1000; - return 0; -} - -ssize_t ethsock_read(struct ethsock *sock, void *buf, size_t size) -{ - struct bpf_hdr *bh; - ssize_t len; + struct pcap_pkthdr* hdr; + int status; fd_set fds; - int err; if (sock->timeout.tv_sec || sock->timeout.tv_usec) { FD_ZERO(&fds); FD_SET(sock->fd, &fds); - err = select(sock->fd + 1, &fds, NULL, NULL, &sock->timeout); - if (err == -1) { + status = select(sock->fd + 1, &fds, NULL, NULL, &sock->timeout); + if (status == -1) { perror("select"); return -1; - } else if (!err) { - return 0; + } else if (status == 0) { + return 1; } } - len = read(sock->fd, buf, size); - if (len < 0) { - perror("read"); + status = pcap_next_ex(sock->pcap, &hdr, (const u_char**)buffer); + switch (status) { + case 1: + status = 0; + *size = hdr->caplen; + break; + case 0: + status = 1; + break; + case -1: + pcap_perror(sock->pcap, "pcap_next_ex"); + status = -1; + break; + default: + fprintf(stderr, "pcap_next_ex: returned %d.\n", status); + status = -1; + break; } - bh = (struct bpf_hdr*)buf; - len = MIN(size, bh->bh_datalen); - memmove(buf, (char*)buf + bh->bh_hdrlen, len); - - return len; + return status; } -int main() +int rawsock_send(struct rawsock *sock, uint8_t *buffer, size_t size) { - struct ethsock sock; - ssize_t len; - char buf[1024]; - - if (ethsock_create(&sock, "en0", 0x0912) != 0) { - return 1; +#if defined(_WIN32) || defined(_WIN64) + if (pcap_sendpacket(sock->pcap, buffer, size) == 0) { + return 0; + } else { + pcap_perror(sock->pcap, "pcap_sendpacket"); + return -1; } - - ethsock_set_timeout(&sock, 1000); - - len = ethsock_read(&sock, buf, sizeof(buf)); - if (len > 0) { - write(1, buf, len); +#else + if (pcap_inject(sock->pcap, buffer, size) != size) { + return 0; + } else { + pcap_perror(sock->pcap, "pcap_inject"); + return -1; } +#endif +} - ethsock_close(&sock); - return len > 0 ? 0 : 1; +int rawsock_close(struct rawsock *sock) +{ + pcap_close(sock->pcap); + free(sock); + return 0; +} + +int rawsock_set_timeout(struct rawsock *sock, unsigned msec) +{ + sock->timeout.tv_sec = msec / 1000; + sock->timeout.tv_usec = (msec % 1000) * 1000; + return 0; } diff --git a/rawsock.h b/rawsock.h new file mode 100644 index 0000000..f0b7c08 --- /dev/null +++ b/rawsock.h @@ -0,0 +1,9 @@ +#include + +struct rawsock; + +struct rawsock *rawsock_create(const char *interface); +int rawsock_close(struct rawsock *sock); +int rawsock_send(struct rawsock *sock, uint8_t *buffer, size_t size); +int rawsock_recv(struct rawsock *sock, uint8_t **buffer, unsigned *size); +int rawsock_set_timeout(struct rawsock *sock, unsigned msec);