Update rawsock code
This commit is contained in:
Joseph C. Lehner
parent
5d3933ce29
commit
3b1cdc7931
2 changed files with 124 additions and 154 deletions
269
rawsock.c
269
rawsock.c
|
|
@ -1,181 +1,142 @@
|
|||
#include <sys/types.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <net/bpf.h>
|
||||
#include <net/if.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <pcap.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
stat = ioctl(fd, SIOCGIFMTU, ifr);
|
||||
if (stat < 0) {
|
||||
perror("ioctl(SIOCGIFMTU)");
|
||||
stat = -1;
|
||||
} else {
|
||||
stat = 0;
|
||||
}
|
||||
|
||||
close(fd);
|
||||
return stat;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
strncpy(ifr.ifr_name, interface, IFNAMSIZ - 1);
|
||||
|
||||
if (ioctl(sock->fd, BIOCSETIF, &ifr) != 0) {
|
||||
perror("ioctl(BIOCSETIF)");
|
||||
return -1;
|
||||
}
|
||||
|
||||
val = 1;
|
||||
|
||||
if (ioctl(sock->fd, BIOCIMMEDIATE, &val) != 0) {
|
||||
perror("ioctl(BIOCIMMEDIATE)");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (ioctl(sock->fd, BIOCGBLEN, &val) != 0) {
|
||||
perror("ioctl(BIOCGBLEN)");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (get_mtu(&ifr) != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (init_filter(sock, protocol, ifr.ifr_mtu) != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
sock->timeout.tv_sec = 0;
|
||||
sock->timeout.tv_usec = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ethsock_close(struct ethsock *sock)
|
||||
{
|
||||
close(sock->fd);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
|
||||
{
|
||||
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;
|
||||
fd_set fds;
|
||||
char errbuf[PCAP_ERRBUF_SIZE];
|
||||
struct bpf_program fp;
|
||||
struct rawsock *sock;
|
||||
int err;
|
||||
|
||||
sock = malloc(sizeof(struct rawsock));
|
||||
if (!sock) {
|
||||
perror("malloc");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
errbuf[0] = '\0';
|
||||
|
||||
sock->pcap = pcap_open_live(interface, BUFSIZ, 1, 1, errbuf);
|
||||
if (!sock->pcap) {
|
||||
fprintf(stderr, "pcap_open_live: %s\n", errbuf);
|
||||
goto cleanup_malloc;
|
||||
}
|
||||
|
||||
if (*errbuf) {
|
||||
fprintf(stderr, "Warning: %s.\n", errbuf);
|
||||
}
|
||||
|
||||
if (pcap_datalink(sock->pcap) != DLT_EN10MB) {
|
||||
fprintf(stderr, "Interface %s not supported.\n", interface);
|
||||
goto cleanup_pcap;
|
||||
}
|
||||
|
||||
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 ((err = pcap_setfilter(sock->pcap, &fp))) {
|
||||
pcap_perror(sock->pcap, "pcap_setfilter");
|
||||
goto cleanup_pcap;
|
||||
}
|
||||
|
||||
sock->fd = pcap_get_selectable_fd(sock->pcap);
|
||||
if (sock->fd == -1) {
|
||||
fprintf(stderr, "No selectable file descriptor available.\n");
|
||||
goto cleanup_pcap;
|
||||
}
|
||||
|
||||
return sock;
|
||||
|
||||
cleanup_pcap:
|
||||
pcap_close(sock->pcap);
|
||||
cleanup_malloc:
|
||||
free(sock);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int rawsock_recv(struct rawsock *sock, uint8_t **buffer, unsigned *size)
|
||||
{
|
||||
struct pcap_pkthdr* hdr;
|
||||
int status;
|
||||
fd_set fds;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
ethsock_close(&sock);
|
||||
return len > 0 ? 0 : 1;
|
||||
#endif
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
9
rawsock.h
Normal file
9
rawsock.h
Normal file
|
|
@ -0,0 +1,9 @@
|
|||
#include <stdint.h>
|
||||
|
||||
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);
|
||||
Loading…
Add table
Reference in a new issue