diff options
| -rw-r--r-- | src/pdesc.c | 10 | ||||
| -rw-r--r-- | src/pdesc.h | 4 | ||||
| -rw-r--r-- | src/ppkt.c | 40 | ||||
| -rw-r--r-- | src/ppkt.h | 3 | ||||
| -rw-r--r-- | src/psock.c | 101 | ||||
| -rw-r--r-- | src/putils.c | 43 | ||||
| -rw-r--r-- | src/putils.h | 5 |
7 files changed, 173 insertions, 33 deletions
diff --git a/src/pdesc.c b/src/pdesc.c index 4ffd7f5..da07533 100644 --- a/src/pdesc.c +++ b/src/pdesc.c @@ -4,10 +4,10 @@ #include <netinet/ip_icmp.h> -static void pdesc_init(struct psock * sock, struct pdesc * desc, uint16_t identifier) +void pdesc_init(struct pdesc * desc, struct sockaddr_storage * sockaddr, uint16_t identifier) { desc->state = PDESC_STATE_AUTH; - desc->peer = sock->current.peer; + desc->peer = *sockaddr; desc->identifier = identifier; desc->sequence = 0; } @@ -31,7 +31,8 @@ enum pdesc_remote_errno pdesc_find_current_remote(struct psock * sock, struct pd } for (i = 0; i < sock->remotes.used; ++i) { - if (sock->current.packet.icmphdr->un.echo.id == sock->remotes.descriptors[i].identifier) { + if (sock->remotes.descriptors[i].state != PDESC_STATE_INVALID && + sock->current.packet.icmphdr->un.echo.id == sock->remotes.descriptors[i].identifier) { *desc = &sock->remotes.descriptors[i]; return REMOTE_EXISTS; } @@ -40,8 +41,9 @@ enum pdesc_remote_errno pdesc_find_current_remote(struct psock * sock, struct pd return REMOTE_MAX_DESCRIPTORS; } - pdesc_init(sock, &sock->remotes.descriptors[i], sock->current.packet.icmphdr->un.echo.id); + pdesc_init(&sock->remotes.descriptors[i], &sock->current.peer, sock->current.packet.icmphdr->un.echo.id); *desc = &sock->remotes.descriptors[i]; + sock->remotes.used++; return REMOTE_NOT_FOUND; } diff --git a/src/pdesc.h b/src/pdesc.h index dcba303..d81798e 100644 --- a/src/pdesc.h +++ b/src/pdesc.h @@ -15,7 +15,7 @@ enum pdesc_remote_errno { REMOTE_MAX_DESCRIPTORS, }; -enum pdesc_state { PDESC_STATE_AUTH, PDESC_STATE_DATA }; +enum pdesc_state { PDESC_STATE_INVALID = 0, PDESC_STATE_AUTH, PDESC_STATE_DATA }; struct pdesc { enum pdesc_state state; @@ -25,6 +25,8 @@ struct pdesc { uint16_t sequence; }; +void pdesc_init(struct pdesc *, struct sockaddr_storage *, uint16_t identifier); + enum pdesc_remote_errno pdesc_find_current_remote(struct psock *, struct pdesc ** const); #endif @@ -1,6 +1,7 @@ #include "pdesc.h" #include "ppkt.h" #include "psock.h" +#include "putils.h" #include <netinet/ip.h> #include <netinet/ip_icmp.h> @@ -68,7 +69,7 @@ int ppkt_process_ppkt(struct psock * sock) return 0; } -static size_t ppkt_prepare_ppkt(struct ppkt * pkt, enum ptype type, size_t additional_size) +static size_t ppkt_sizeof_pkt(struct ppkt * pkt, enum ptype type, size_t additional_size) { size_t subpkt_size; @@ -91,16 +92,41 @@ static size_t ppkt_prepare_ppkt(struct ppkt * pkt, enum ptype type, size_t addit return sizeof(*pkt) + subpkt_size + additional_size; } -void ppkt_prepare_auth_request(struct ppkt_buffer * const pkt_buf, uint8_t * const hash, size_t hash_siz) +static size_t ppkt_init_pkt(struct pdesc const * desc, + struct ppkt_buffer * pkt_buf, + enum ptype type, + size_t additional_size) { - size_t total_size = ppkt_prepare_ppkt(&pkt_buf->pkt, PTYPE_AUTH_REQUEST, hash_siz); + pkt_buf->icmphdr.un.echo.id = desc->identifier; + switch (type) { + case PTYPE_INVALID: + pkt_buf->icmphdr.type = 3; // Destination Unreachable + break; + case PTYPE_AUTH_REQUEST: + pkt_buf->icmphdr.type = 8; // Echo Request + break; + case PTYPE_AUTH_RESPONSE: + pkt_buf->icmphdr.type = 0; // Echo Reply + break; + } + pkt_buf->icmphdr.code = 0; + + return ppkt_sizeof_pkt(&pkt_buf->pkt, type, additional_size); +} + +static void ppkt_finalize_pkt(struct ppkt_buffer * const pkt_buf) +{ + pkt_buf->icmphdr.checksum = 0; + pkt_buf->icmphdr.checksum = icmp_checksum_iovec(pkt_buf->iovec, pkt_buf->iovec_used); +} + +void ppkt_prepare_auth_request(struct pdesc * desc, struct ppkt_buffer * pkt_buf, uint8_t * hash, size_t hash_siz) +{ + size_t total_size = ppkt_init_pkt(desc, pkt_buf, PTYPE_AUTH_REQUEST, hash_siz); pkt_buf->auth_request.magic = htonl(PTUNNEL_MAGIC); pkt_buf->auth_request.hash_siz = hash_siz; - pkt_buf->icmphdr.type = 8; - pkt_buf->icmphdr.code = 0; - pkt_buf->iovec[0].iov_base = &pkt_buf->icmphdr; pkt_buf->iovec[0].iov_len = sizeof(pkt_buf->icmphdr); @@ -114,4 +140,6 @@ void ppkt_prepare_auth_request(struct ppkt_buffer * const pkt_buf, uint8_t * con pkt_buf->iovec[3].iov_len = hash_siz; pkt_buf->iovec_used = 4; + + ppkt_finalize_pkt(pkt_buf); } @@ -18,7 +18,6 @@ enum ptype { struct psock; struct pdesc; -struct iovec; struct ppkt_auth_request { uint32_t magic; @@ -54,6 +53,6 @@ int ppkt_process_icmp(struct psock *); int ppkt_process_ppkt(struct psock *); -void ppkt_prepare_auth_request(struct ppkt_buffer *, uint8_t *, size_t); +void ppkt_prepare_auth_request(struct pdesc *, struct ppkt_buffer *, uint8_t *, size_t); #endif diff --git a/src/psock.c b/src/psock.c index 548be93..35747d1 100644 --- a/src/psock.c +++ b/src/psock.c @@ -70,7 +70,7 @@ error: return -1; } -static int psock_name_to_address(char const * address, struct sockaddr_storage * const sockaddr, size_t * sockaddr_size) +static int psock_name_to_address(char const * address, struct sockaddr_storage * sockaddr, size_t * sockaddr_size) { struct addrinfo hints = {}; struct addrinfo *result, *rp; @@ -120,9 +120,15 @@ int psock_add_server(struct psock * sock, char const * address) return -1; } - for (size_t addr_i = 0; sock->remotes.used < sock->remotes.max && addr_i < max_sockaddrs; - ++sock->remotes.used, ++addr_i) { - size_t desc_i = sock->remotes.used; + size_t desc_i = 0; + for (size_t addr_i = 0; desc_i < sock->remotes.max && addr_i < max_sockaddrs;) { + if (sock->remotes.descriptors[desc_i].state != PDESC_STATE_INVALID) { + desc_i++; + continue; + } else { + pdesc_init(&sock->remotes.descriptors[desc_i], &sockaddrs[addr_i], icmp_generate_identifier()); + sock->remotes.descriptors[desc_i].state = PDESC_STATE_AUTH; + } switch (sockaddrs[addr_i].ss_family) { case AF_INET: { @@ -146,7 +152,10 @@ int psock_add_server(struct psock * sock, char const * address) break; } } + logger_early(0, "Added remote: %s", sock->remotes.descriptors[desc_i].peer_str); + addr_i++; + sock->remotes.used++; } return 0; @@ -154,6 +163,9 @@ int psock_add_server(struct psock * sock, char const * address) void psock_free(struct psock * sock) { + free(sock->current.packet.buffer); + sock->current.packet.buffer = NULL; + free(sock->remotes.descriptors); sock->remotes.descriptors = NULL; sock->remotes.used = 0; @@ -216,35 +228,37 @@ static int psock_sendmsg(struct psock * sock, struct iovec * const iov, size_t i return nwritten; } -static void psock_handle_events(struct psock * sock) +static struct pdesc * psock_handle_events(struct psock * sock) { - if (psock_recvmsg(sock) == 0) { - struct pdesc * remote; + struct pdesc * remote = NULL; + if (psock_recvmsg(sock) == 0) { switch (pdesc_find_current_remote(sock, &remote)) { case REMOTE_EXISTS: - printf("Existing Remote with descriptor ID: %u\n", remote->identifier); + logger(0, "Existing Remote with descriptor ID: %u", remote->identifier); break; case REMOTE_NOT_FOUND: - printf("New Remote with descriptor ID: %u\n", remote->identifier); + logger(1, "New Remote with descriptor ID: %u", remote->identifier); break; case REMOTE_PACKET_INVALID: - fprintf(stderr, "Invalid packet received.\n"); + logger(1, "Invalid packet received."); break; case REMOTE_ICMP_ECHO_CLIENT: - fprintf(stderr, "Received ICMP echo, but I am a client.\n"); + logger(0, "Received ICMP echo, but I am a client."); break; case REMOTE_ICMP_REPLY_SERVER: - fprintf(stderr, "Received ICMP reply, but I am a server.\n"); + logger(0, "Received ICMP reply, but I am a server."); break; case REMOTE_MAX_DESCRIPTORS: - fprintf(stderr, "Max descriptors reached, sorry.\n"); + logger(1, "Max descriptors reached, sorry."); break; } } + + return remote; } -void psock_loop(struct psock * sock) +static void psock_loop_server(struct psock * sock) { const int max_events = 32; struct epoll_event events[max_events]; @@ -256,12 +270,6 @@ void psock_loop(struct psock * sock) case -1: break; case 0: - if (sock->local.is_client != 0) { - uint8_t b[3] = {0x41, 0x42, 0x43}; - struct ppkt_buffer pb; - ppkt_prepare_auth_request(&pb, b, 3); - psock_sendmsg(sock, pb.iovec, pb.iovec_used); - } continue; default: psock_handle_events(sock); @@ -269,3 +277,56 @@ void psock_loop(struct psock * sock) } } } + +static void psock_loop_client_event_timeout(struct psock * sock) +{ + for (size_t i = 0; i < sock->remotes.max; ++i) { + struct pdesc * const desc = &sock->remotes.descriptors[i]; + + switch (desc->state) { + case PDESC_STATE_INVALID: + break; + case PDESC_STATE_AUTH: { + logger(0, "Sending authentication request."); + + uint8_t b[3] = {0x41, 0x42, 0x43}; + struct ppkt_buffer pb; + ppkt_prepare_auth_request(desc, &pb, b, 3); + psock_sendmsg(sock, pb.iovec, pb.iovec_used); + break; + } + case PDESC_STATE_DATA: + break; + } + } +} + +static void psock_loop_client(struct psock * sock) +{ + const int max_events = 16; + struct epoll_event events[max_events]; + + while (1) { + int nready = epoll_wait(sock->epoll_fd, events, max_events, 1000); + + switch (nready) { + case -1: + break; + case 0: + psock_loop_client_event_timeout(sock); + break; + default: + psock_handle_events(sock); + break; + } + } +} + +void psock_loop(struct psock * sock) +{ + if (sock->local.is_client == 0) { + psock_loop_server(sock); + } else { + psock_loop_client(sock); + } +} diff --git a/src/putils.c b/src/putils.c index 263b3a7..3f8563d 100644 --- a/src/putils.c +++ b/src/putils.c @@ -11,6 +11,7 @@ #include <syslog.h> #include <sys/stat.h> #include <sys/types.h> +#include <time.h> #include <unistd.h> static char const * app_name = NULL; @@ -151,3 +152,45 @@ int parse_address(struct sockaddr_storage * out, char const * address) return 0; } + +static inline uint16_t get_n16bit(uint8_t const * cbuf) +{ + uint16_t r = ((uint16_t)cbuf[0]) | (((uint16_t)cbuf[1]) << 8); + return r; +} + +uint16_t icmp_checksum_iovec(struct iovec const * iovec, size_t iovec_size) +{ + uint32_t checksum = 0; + + for (size_t iov_i = 0; iov_i < iovec_size; ++iov_i) { + uint8_t const * buf = iovec[iov_i].iov_base; + size_t len = iovec[iov_i].iov_len; + + for (; len > 1; len -= 2) { + checksum += get_n16bit(buf); + buf += 2; + } + + if (len == 1) { + checksum += *buf; + } + } + + checksum = (checksum >> 16) + (checksum & 0xFFFF); + checksum += (checksum >> 16); + + return ~checksum; +} + +uint16_t icmp_generate_identifier(void) +{ + uint64_t current_time = time(NULL); + uint16_t identifier = 0; + + for (size_t i = 0; i < sizeof(current_time) / 2; ++i) { + identifier += ((uint16_t *)¤t_time)[i]; + } + + return identifier; +} diff --git a/src/putils.h b/src/putils.h index 895318a..06d034f 100644 --- a/src/putils.h +++ b/src/putils.h @@ -2,6 +2,8 @@ #define PUTILS_H 1 #include <stdarg.h> +#include <stdint.h> +#include <sys/socket.h> struct sockaddr_storage; @@ -25,4 +27,7 @@ __attribute__((format(printf, 2, 3))) void logger_early(int, char const *, ...); int parse_address(struct sockaddr_storage *, char const *); +uint16_t icmp_checksum_iovec(struct iovec const *, size_t); + +uint16_t icmp_generate_identifier(void); #endif |