diff options
| -rw-r--r-- | src/pdesc.c | 29 | ||||
| -rw-r--r-- | src/pdesc.h | 19 | ||||
| -rw-r--r-- | src/ppkt.c | 42 | ||||
| -rw-r--r-- | src/ppkt.h | 25 | ||||
| -rw-r--r-- | src/psock.c | 88 | ||||
| -rw-r--r-- | src/psock.h | 22 | ||||
| -rw-r--r-- | src/ptunnel.c | 4 |
7 files changed, 147 insertions, 82 deletions
diff --git a/src/pdesc.c b/src/pdesc.c index 82cc600..77f511e 100644 --- a/src/pdesc.c +++ b/src/pdesc.c @@ -1,12 +1,33 @@ #include "pdesc.h" +#include "ppkt.h" #include "psock.h" +#include <netinet/ip_icmp.h> -enum pdesc_retval pdesc_find_remote(struct psock * psock) +static void pdesc_init(struct psock * psock, struct pdesc * pdesc, uint16_t identifier) { - for (size_t i = 0; i < psock->remotes.used; ++i) - { + pdesc->state = PDESC_STATE_AUTH; + pdesc->peer = psock->current.peer; + pdesc->identifier = identifier; + pdesc->sequence = 0; +} + +struct pdesc * pdesc_find_remote(struct psock * psock) +{ + size_t i; + + if (psock->remotes.used == psock->remotes.max || ppkt_process_icmp(psock) != 0 || + psock->current.packet.icmphdr->type != ICMP_ECHOREPLY || ppkt_process_ppkt(psock) != 0) { + return NULL; } - return PDESC_REMOTE_FOUND; + for (i = 0; i < psock->remotes.used; ++i) { + if (psock->current.packet.icmphdr->un.echo.id == psock->remotes.descriptors[i].identifier) { + return &psock->remotes.descriptors[i]; + } + } + + pdesc_init(psock, &psock->remotes.descriptors[i], psock->current.packet.icmphdr->un.echo.id); + + return &psock->remotes.descriptors[i]; } diff --git a/src/pdesc.h b/src/pdesc.h index 8dde3cf..54fad4a 100644 --- a/src/pdesc.h +++ b/src/pdesc.h @@ -1,25 +1,20 @@ #ifndef PDESC_H #define PDESC_H +#include <netinet/in.h> #include <stdint.h> - struct psock; -struct pdesc -{ +enum pdesc_state { PDESC_STATE_AUTH }; + +struct pdesc { + enum pdesc_state state; + struct sockaddr_storage peer; uint16_t identifier; uint16_t sequence; }; -enum pdesc_retval -{ - PDESC_REMOTE_NEW, - PDESC_REMOTE_INVALID, - PDESC_REMOTE_FOUND -}; - - -enum pdesc_retval pdesc_find_remote(struct psock *); +struct pdesc * pdesc_find_remote(struct psock *); #endif @@ -1,6 +1,46 @@ #include "ppkt.h" +#include "psock.h" +#include <netinet/ip.h> +#include <netinet/ip_icmp.h> -void ppkt_process_icmp(struct psock * psock) +int ppkt_process_icmp(struct psock * psock) { + if (psock->current.peer.ss_family == AF_INET) { + psock->current.packet.icmphdr = (struct icmphdr *)(psock->current.packet.buffer + sizeof(struct iphdr)); + + psock->current.packet.icmphdr->checksum = ntohs(psock->current.packet.icmphdr->checksum); + psock->current.packet.icmphdr->un.echo.id = ntohs(psock->current.packet.icmphdr->un.echo.id); + psock->current.packet.icmphdr->un.echo.sequence = ntohs(psock->current.packet.icmphdr->un.echo.sequence); + } else { + return -1; + } + + return 0; +} + +int ppkt_process_ppkt(struct psock * psock) +{ + if (psock->current.peer.ss_family == AF_INET) { + if (psock->current.packet.used < sizeof(struct iphdr) + sizeof(*psock->current.packet.icmphdr)) { + return -1; + } + + psock->current.packet.ppkt = (struct ppkt *)(psock->current.packet.buffer + sizeof(struct iphdr) + + sizeof(*psock->current.packet.icmphdr)); + } else { + return -1; + } + + psock->current.packet.ppkt->ident = ntohl(psock->current.packet.ppkt->ident); + if (psock->current.packet.ppkt->ident != PTUNNEL_IDENT) { + return -1; + } + + psock->current.packet.ppkt->total_size = ntohs(psock->current.packet.ppkt->total_size); + if (psock->current.packet.ppkt->total_size > psock->current.packet.used) { + return -1; + } + + return 0; } @@ -3,22 +3,33 @@ #include <stdint.h> - #define PTUNNEL_IDENT 0xdeadc0de struct psock; -struct ppkt -{ - uint32_t ident; +struct ppkt_option_header { uint16_t type; - uint16_t data_size; + uint16_t size; +} __attribute__((__packed__)); + +struct ppkt_option_auth { + struct ppkt_option_header option; uint8_t data[0]; } __attribute__((__packed__)); +union ppkt_option { + struct ppkt_option_auth auth; +} __attribute__((__packed__)); +; + +struct ppkt { + uint32_t ident; + uint16_t total_size; + union ppkt_option current; +} __attribute__((__packed__)); -void ppkt_process_icmp(struct psock *); +int ppkt_process_icmp(struct psock *); -void ppkt_process_body(struct psock *); +int ppkt_process_ppkt(struct psock *); #endif diff --git a/src/psock.c b/src/psock.c index 78ecbff..f5575ab 100644 --- a/src/psock.c +++ b/src/psock.c @@ -2,7 +2,6 @@ #include "psock.h" #include <errno.h> -#include <netinet/in.h> #include <stdio.h> #include <stdlib.h> #include <string.h> @@ -10,7 +9,6 @@ #include <sys/socket.h> #include <unistd.h> - int psock_init(struct psock * psock, size_t max_descriptors, size_t packet_buffer_size) { struct epoll_event ev; @@ -18,44 +16,38 @@ int psock_init(struct psock * psock, size_t max_descriptors, size_t packet_buffe memset(psock, 0, sizeof(*psock)); psock->icmp_fd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP); - if (psock->icmp_fd < 0) - { + if (psock->icmp_fd < 0) { goto error; } psock->epoll_fd = epoll_create1(EPOLL_CLOEXEC); - if (psock->epoll_fd < 0) - { + if (psock->epoll_fd < 0) { goto error; } ev.events = EPOLLIN; ev.data.fd = psock->icmp_fd; - if (epoll_ctl(psock->epoll_fd, EPOLL_CTL_ADD, psock->icmp_fd, &ev) != 0) - { + if (epoll_ctl(psock->epoll_fd, EPOLL_CTL_ADD, psock->icmp_fd, &ev) != 0) { goto error; } - psock->packet.max = packet_buffer_size; - psock->packet.used = 0; - psock->packet.buffer = (uint8_t *)calloc(packet_buffer_size, sizeof(*psock->packet.buffer)); - if (psock->packet.buffer == NULL) - { + psock->current.packet.max = packet_buffer_size; + psock->current.packet.used = 0; + psock->current.packet.buffer = (uint8_t *)calloc(packet_buffer_size, sizeof(*psock->current.packet.buffer)); + if (psock->current.packet.buffer == NULL) { goto error; } psock->remotes.max = max_descriptors; psock->remotes.used = 0; psock->remotes.descriptors = (struct pdesc *)calloc(max_descriptors, sizeof(*psock->remotes.descriptors)); - if (psock->remotes.descriptors == NULL) - { + if (psock->remotes.descriptors == NULL) { goto error; } return 0; error: - if (errno != 0) - { + if (errno != 0) { perror("[FATAL] psock_init failed"); } psock_free(psock); @@ -78,24 +70,22 @@ void psock_free(struct psock * psock) static void psock_process_cmsg(struct msghdr * hdr) { - for (struct cmsghdr * cmsg = CMSG_FIRSTHDR(hdr); cmsg != NULL; cmsg = CMSG_NXTHDR(hdr, cmsg)) - { + for (struct cmsghdr * cmsg = CMSG_FIRSTHDR(hdr); cmsg != NULL; cmsg = CMSG_NXTHDR(hdr, cmsg)) { printf("CMSG TYPE/LEVEL/LEN: %d / %d / %zu\n", cmsg->cmsg_type, cmsg->cmsg_level, cmsg->cmsg_len); } } static int psock_recvmsg(struct psock * psock) { - struct sockaddr_storage peer; struct iovec iov; struct msghdr hdr = {}; ssize_t nread; - iov.iov_base = (void *)psock->packet.buffer; - iov.iov_len = psock->packet.max; + iov.iov_base = (void *)psock->current.packet.buffer; + iov.iov_len = psock->current.packet.max; - hdr.msg_name = &peer; - hdr.msg_namelen = sizeof(peer); + hdr.msg_name = &psock->current.peer; + hdr.msg_namelen = sizeof(psock->current.peer); hdr.msg_iov = &iov; hdr.msg_iovlen = 1; @@ -103,37 +93,43 @@ static int psock_recvmsg(struct psock * psock) nread = recvmsg(psock->icmp_fd, &hdr, 0); } while (nread == -1 && errno == EINTR); - if (nread >= 0) - { - psock->packet.used = nread; + if (nread >= 0) { + psock->current.packet.used = nread; psock_process_cmsg(&hdr); return 0; } else { - psock->packet.used = 0; + psock->current.packet.used = 0; return -1; } } -static int psock_sendmsg(struct psock * psock) +static int psock_sendmsg(struct psock * psock, uint8_t * buf, size_t siz) { - return -1; + struct iovec iov; + struct msghdr hdr = {}; + ssize_t n = siz, nwritten; + + iov.iov_base = buf; + iov.iov_len = siz; + + hdr.msg_name = &psock->current.peer; + hdr.msg_namelen = sizeof(psock->current.peer); + hdr.msg_iov = &iov; + hdr.msg_iovlen = 1; + + nwritten = sendmsg(psock->icmp_fd, &hdr, 0); + + return (nwritten == n ? 0 : nwritten); } static void psock_handle_events(struct psock * psock) { - if (psock_recvmsg(psock) == 0) - { - switch (pdesc_find_remote(psock)) - { - case PDESC_REMOTE_NEW: - fprintf(stderr, "Remote NEW.\n"); - break; - case PDESC_REMOTE_INVALID: - fprintf(stderr, "Remote INVALID\n"); - break; - case PDESC_REMOTE_FOUND: - fprintf(stderr, "Remote FOUND\n"); - break; + if (psock_recvmsg(psock) == 0) { + struct pdesc * remote = pdesc_find_remote(psock); + if (remote != NULL) { + printf("Remote descriptor ID: %u\n", remote->identifier); + } else { + fprintf(stderr, "Could not find/alloc a remote descriptor.\n"); } } } @@ -143,12 +139,10 @@ void psock_loop(struct psock * psock) const int max_events = 32; struct epoll_event events[max_events]; - while (1) - { + while (1) { int nready = epoll_wait(psock->epoll_fd, events, max_events, -1); - switch (nready) - { + switch (nready) { case -1: break; case 0: diff --git a/src/psock.h b/src/psock.h index 5faf24b..da2be7b 100644 --- a/src/psock.h +++ b/src/psock.h @@ -1,22 +1,29 @@ #ifndef PSOCK_H #define PSOCK_H 1 +#include <netinet/in.h> #include <stdint.h> #include <stdlib.h> - +struct icmphdr; struct pdesc; +struct ppkt; -struct psock -{ +struct psock { int epoll_fd; int icmp_fd; struct { - size_t used; - size_t max; - uint8_t * buffer; - } packet; + struct sockaddr_storage peer; + + struct { + size_t used; + size_t max; + uint8_t * buffer; + struct icmphdr * icmphdr; + struct ppkt * ppkt; + } packet; + } current; struct { size_t used; @@ -25,7 +32,6 @@ struct psock } remotes; }; - int psock_init(struct psock *, size_t, size_t); void psock_free(struct psock *); diff --git a/src/ptunnel.c b/src/ptunnel.c index df10db9..acf87d2 100644 --- a/src/ptunnel.c +++ b/src/ptunnel.c @@ -4,13 +4,11 @@ #include <stdio.h> #include <stdlib.h> - int main(void) { struct psock psock = {}; - if (psock_init(&psock, 16, 2048) != 0) - { + if (psock_init(&psock, 16, 1500) != 0) { return 1; } |