diff options
author | Tom Smeding <tom.smeding@gmail.com> | 2018-07-23 15:48:44 +0200 |
---|---|---|
committer | Tom Smeding <tom.smeding@gmail.com> | 2018-07-23 15:48:44 +0200 |
commit | 5722c47aa3402f1458da9eec332153a454a540b7 (patch) | |
tree | f1bfaa8c89dc39c6b9ae0bf7ff54eb524b1468eb /icmp.c |
Initial version
Working ping back and forth with specified data, and data
arrives at the other party.
Currently, the server uses nflog to get the pings, which:
1. requires iptables settings to work;
2. buffers and collects messages for a second.
Both are suboptimal, and I believe raw sockets can improve
on this.
Diffstat (limited to 'icmp.c')
-rw-r--r-- | icmp.c | 128 |
1 files changed, 128 insertions, 0 deletions
@@ -0,0 +1,128 @@ +#include <stdlib.h> +#include <stddef.h> +#include <string.h> +#include <assert.h> +#include <sys/socket.h> +#include <netinet/in.h> +#include <netinet/ip_icmp.h> +#include <arpa/inet.h> +#include "icmp.h" +#include "util.h" + + +#define PROT_ICMP 1 + + +static void make_sockaddr(struct sockaddr_in *dst, const char *ip_address) { + memset(dst, 0, sizeof *dst); + dst->sin_family = AF_INET; + dst->sin_port = htons(0); + dst->sin_addr.s_addr = inet_addr(ip_address); +} + +static uint16_t compute_checksum(const void *buf_, size_t buflen) { + const uint8_t *buf_bytes = (const uint8_t*)buf_; + const uint16_t *buf = (const uint16_t*)buf_; + + uint32_t res = 0; + for (size_t i = 0; i < buflen / 2; i++) { + res += buf[i]; + res = (res + (res >> 16)) & 0xffff; + } + + if (buflen % 2 == 1) { + res += (uint16_t)buf_bytes[buflen - 1]; + res = (res + (res >> 16)) & 0xffff; + } + + return res; +} + +int icmp_open_socket(void) { + return socket(PF_INET, SOCK_DGRAM, PROT_ICMP); +} + +struct icmp_reply icmp_communicate(int sock, const char *ip_address, int seqnum, const void *data_, size_t length) { + static uint8_t buffer[ICMP_MAX_PAYLOAD_LENGTH]; + + const uint8_t *data = (const uint8_t*)data_; + + assert(length <= ICMP_MAX_PAYLOAD_LENGTH); + + struct icmp_reply errret = (struct icmp_reply){.data = NULL, .length = 0, .seqnum = 0}; + + struct sockaddr_in addr; + make_sockaddr(&addr, ip_address); + + struct icmp_echo msg; + memset(&msg, 0, sizeof msg); + msg.type = ICMP_ECHO; + msg.code = 0; + msg.seqnum = seqnum; + + memcpy(msg.payload, data, length); + + if (sendto(sock, &msg, ICMP_PAYLOAD_OFFSET + length, 0, (struct sockaddr*)&addr, sizeof addr) < 0) { + return errret; + } + + struct msghdr replyhdr; + memset(&replyhdr, 0, sizeof replyhdr); + + struct iovec iov1; + memset(&iov1, 0, sizeof iov1); + iov1.iov_base = &msg; + iov1.iov_len = sizeof msg; + + replyhdr.msg_name = &addr; + replyhdr.msg_namelen = sizeof addr; + replyhdr.msg_iov = &iov1; + replyhdr.msg_iovlen = 1; + + ssize_t nr = recvmsg(sock, &replyhdr, 0); + if (nr < 0) { + return errret; + } + + size_t payloadlen = nr - ICMP_PAYLOAD_OFFSET; + assert(payloadlen <= MAX_DATAGRAM_SIZE); + + memcpy(buffer, msg.payload, payloadlen); + + return (struct icmp_reply){ + .data = buffer, + .length = payloadlen, + .seqnum = msg.seqnum + }; +} + +int icmp_send_echo_reply(const char *ip_address, int id, int seqnum, const void *data_, size_t length) { + const uint8_t *data = (const uint8_t*)data_; + + int sock = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP); + int zero = 0; + assert(setsockopt(sock, IPPROTO_IP, IP_HDRINCL, &zero, sizeof zero) >= 0); + + struct sockaddr_in addr; + make_sockaddr(&addr, ip_address); + + struct icmp_echo msg; + memset(&msg, 0, sizeof msg); + msg.type = ICMP_ECHOREPLY; + msg.code = 0; + msg.id = id; + msg.seqnum = seqnum; + msg.checksum = 0; + + memcpy(msg.payload, data, length); + + size_t total_length = ICMP_PAYLOAD_OFFSET + length; + + msg.checksum = ~compute_checksum(&msg, total_length); + + if (sendto(sock, &msg, ICMP_PAYLOAD_OFFSET + length, 0, (struct sockaddr*)&addr, sizeof addr) < 0) { + return -1; + } + + return 0; +} |