/*
|
* Copyright 2017, The Android Open Source Project
|
*
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
* you may not use this file except in compliance with the License.
|
* You may obtain a copy of the License at
|
*
|
* http://www.apache.org/licenses/LICENSE-2.0
|
*
|
* Unless required by applicable law or agreed to in writing, software
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
* See the License for the specific language governing permissions and
|
* limitations under the License.
|
*/
|
|
#include "socket.h"
|
|
#include "message.h"
|
#include "utils.h"
|
|
#include <errno.h>
|
#include <linux/if_packet.h>
|
#include <netinet/ip.h>
|
#include <netinet/udp.h>
|
#include <string.h>
|
#include <sys/socket.h>
|
#include <sys/types.h>
|
#include <sys/uio.h>
|
#include <unistd.h>
|
|
// Combine the checksum of |buffer| with |size| bytes with |checksum|. This is
|
// used for checksum calculations for IP and UDP.
|
static uint32_t addChecksum(const uint8_t* buffer,
|
size_t size,
|
uint32_t checksum) {
|
const uint16_t* data = reinterpret_cast<const uint16_t*>(buffer);
|
while (size > 1) {
|
checksum += *data++;
|
size -= 2;
|
}
|
if (size > 0) {
|
// Odd size, add the last byte
|
checksum += *reinterpret_cast<const uint8_t*>(data);
|
}
|
// msw is the most significant word, the upper 16 bits of the checksum
|
for (uint32_t msw = checksum >> 16; msw != 0; msw = checksum >> 16) {
|
checksum = (checksum & 0xFFFF) + msw;
|
}
|
return checksum;
|
}
|
|
// Convenienct template function for checksum calculation
|
template<typename T>
|
static uint32_t addChecksum(const T& data, uint32_t checksum) {
|
return addChecksum(reinterpret_cast<const uint8_t*>(&data), sizeof(T), checksum);
|
}
|
|
// Finalize the IP or UDP |checksum| by inverting and truncating it.
|
static uint32_t finishChecksum(uint32_t checksum) {
|
return ~checksum & 0xFFFF;
|
}
|
|
Socket::Socket() : mSocketFd(-1) {
|
}
|
|
Socket::~Socket() {
|
if (mSocketFd != -1) {
|
::close(mSocketFd);
|
mSocketFd = -1;
|
}
|
}
|
|
|
Result Socket::open(int domain, int type, int protocol) {
|
if (mSocketFd != -1) {
|
return Result::error("Socket already open");
|
}
|
mSocketFd = ::socket(domain, type, protocol);
|
if (mSocketFd == -1) {
|
return Result::error("Failed to open socket: %s", strerror(errno));
|
}
|
return Result::success();
|
}
|
|
Result Socket::bind(const void* sockaddr, size_t sockaddrLength) {
|
if (mSocketFd == -1) {
|
return Result::error("Socket not open");
|
}
|
|
int status = ::bind(mSocketFd,
|
reinterpret_cast<const struct sockaddr*>(sockaddr),
|
sockaddrLength);
|
if (status != 0) {
|
return Result::error("Unable to bind raw socket: %s", strerror(errno));
|
}
|
|
return Result::success();
|
}
|
|
Result Socket::bindIp(in_addr_t address, uint16_t port) {
|
struct sockaddr_in sockaddr;
|
memset(&sockaddr, 0, sizeof(sockaddr));
|
sockaddr.sin_family = AF_INET;
|
sockaddr.sin_port = htons(port);
|
sockaddr.sin_addr.s_addr = address;
|
|
return bind(&sockaddr, sizeof(sockaddr));
|
}
|
|
Result Socket::bindRaw(unsigned int interfaceIndex) {
|
struct sockaddr_ll sockaddr;
|
memset(&sockaddr, 0, sizeof(sockaddr));
|
sockaddr.sll_family = AF_PACKET;
|
sockaddr.sll_protocol = htons(ETH_P_IP);
|
sockaddr.sll_ifindex = interfaceIndex;
|
|
return bind(&sockaddr, sizeof(sockaddr));
|
}
|
|
Result Socket::sendOnInterface(unsigned int interfaceIndex,
|
in_addr_t destinationAddress,
|
uint16_t destinationPort,
|
const Message& message) {
|
if (mSocketFd == -1) {
|
return Result::error("Socket not open");
|
}
|
|
char controlData[CMSG_SPACE(sizeof(struct in_pktinfo))] = { 0 };
|
struct sockaddr_in addr;
|
memset(&addr, 0, sizeof(addr));
|
addr.sin_family = AF_INET;
|
addr.sin_port = htons(destinationPort);
|
addr.sin_addr.s_addr = destinationAddress;
|
|
struct msghdr header;
|
memset(&header, 0, sizeof(header));
|
struct iovec iov;
|
// The struct member is non-const since it's used for receiving but it's
|
// safe to cast away const for sending.
|
iov.iov_base = const_cast<uint8_t*>(message.data());
|
iov.iov_len = message.size();
|
header.msg_name = &addr;
|
header.msg_namelen = sizeof(addr);
|
header.msg_iov = &iov;
|
header.msg_iovlen = 1;
|
header.msg_control = &controlData;
|
header.msg_controllen = sizeof(controlData);
|
|
struct cmsghdr* controlHeader = CMSG_FIRSTHDR(&header);
|
controlHeader->cmsg_level = IPPROTO_IP;
|
controlHeader->cmsg_type = IP_PKTINFO;
|
controlHeader->cmsg_len = CMSG_LEN(sizeof(struct in_pktinfo));
|
auto packetInfo =
|
reinterpret_cast<struct in_pktinfo*>(CMSG_DATA(controlHeader));
|
memset(packetInfo, 0, sizeof(*packetInfo));
|
packetInfo->ipi_ifindex = interfaceIndex;
|
|
ssize_t status = ::sendmsg(mSocketFd, &header, 0);
|
if (status <= 0) {
|
return Result::error("Failed to send packet: %s", strerror(errno));
|
}
|
return Result::success();
|
}
|
|
Result Socket::sendRawUdp(in_addr_t source,
|
uint16_t sourcePort,
|
in_addr_t destination,
|
uint16_t destinationPort,
|
unsigned int interfaceIndex,
|
const Message& message) {
|
struct iphdr ip;
|
struct udphdr udp;
|
|
ip.version = IPVERSION;
|
ip.ihl = sizeof(ip) >> 2;
|
ip.tos = 0;
|
ip.tot_len = htons(sizeof(ip) + sizeof(udp) + message.size());
|
ip.id = 0;
|
ip.frag_off = 0;
|
ip.ttl = IPDEFTTL;
|
ip.protocol = IPPROTO_UDP;
|
ip.check = 0;
|
ip.saddr = source;
|
ip.daddr = destination;
|
ip.check = finishChecksum(addChecksum(ip, 0));
|
|
udp.source = htons(sourcePort);
|
udp.dest = htons(destinationPort);
|
udp.len = htons(sizeof(udp) + message.size());
|
udp.check = 0;
|
|
uint32_t udpChecksum = 0;
|
udpChecksum = addChecksum(ip.saddr, udpChecksum);
|
udpChecksum = addChecksum(ip.daddr, udpChecksum);
|
udpChecksum = addChecksum(htons(IPPROTO_UDP), udpChecksum);
|
udpChecksum = addChecksum(udp.len, udpChecksum);
|
udpChecksum = addChecksum(udp, udpChecksum);
|
udpChecksum = addChecksum(message.data(), message.size(), udpChecksum);
|
udp.check = finishChecksum(udpChecksum);
|
|
struct iovec iov[3];
|
|
iov[0].iov_base = static_cast<void*>(&ip);
|
iov[0].iov_len = sizeof(ip);
|
iov[1].iov_base = static_cast<void*>(&udp);
|
iov[1].iov_len = sizeof(udp);
|
// sendmsg requires these to be non-const but for sending won't modify them
|
iov[2].iov_base = static_cast<void*>(const_cast<uint8_t*>(message.data()));
|
iov[2].iov_len = message.size();
|
|
struct sockaddr_ll dest;
|
memset(&dest, 0, sizeof(dest));
|
dest.sll_family = AF_PACKET;
|
dest.sll_protocol = htons(ETH_P_IP);
|
dest.sll_ifindex = interfaceIndex;
|
dest.sll_halen = ETH_ALEN;
|
memset(dest.sll_addr, 0xFF, ETH_ALEN);
|
|
struct msghdr header;
|
memset(&header, 0, sizeof(header));
|
header.msg_name = &dest;
|
header.msg_namelen = sizeof(dest);
|
header.msg_iov = iov;
|
header.msg_iovlen = sizeof(iov) / sizeof(iov[0]);
|
|
ssize_t res = ::sendmsg(mSocketFd, &header, 0);
|
if (res == -1) {
|
return Result::error("Failed to send message: %s", strerror(errno));
|
}
|
return Result::success();
|
}
|
|
Result Socket::receiveFromInterface(Message* message,
|
unsigned int* interfaceIndex) {
|
char controlData[CMSG_SPACE(sizeof(struct in_pktinfo))];
|
struct msghdr header;
|
memset(&header, 0, sizeof(header));
|
struct iovec iov;
|
iov.iov_base = message->data();
|
iov.iov_len = message->capacity();
|
header.msg_iov = &iov;
|
header.msg_iovlen = 1;
|
header.msg_control = &controlData;
|
header.msg_controllen = sizeof(controlData);
|
|
ssize_t bytesRead = ::recvmsg(mSocketFd, &header, 0);
|
if (bytesRead < 0) {
|
return Result::error("Error receiving on socket: %s", strerror(errno));
|
}
|
message->setSize(static_cast<size_t>(bytesRead));
|
if (header.msg_controllen >= sizeof(struct cmsghdr)) {
|
for (struct cmsghdr* ctrl = CMSG_FIRSTHDR(&header);
|
ctrl;
|
ctrl = CMSG_NXTHDR(&header, ctrl)) {
|
if (ctrl->cmsg_level == SOL_IP &&
|
ctrl->cmsg_type == IP_PKTINFO) {
|
auto packetInfo =
|
reinterpret_cast<struct in_pktinfo*>(CMSG_DATA(ctrl));
|
*interfaceIndex = packetInfo->ipi_ifindex;
|
}
|
}
|
}
|
return Result::success();
|
}
|
|
Result Socket::receiveRawUdp(uint16_t expectedPort,
|
Message* message,
|
bool* isValid) {
|
struct iphdr ip;
|
struct udphdr udp;
|
|
struct iovec iov[3];
|
iov[0].iov_base = &ip;
|
iov[0].iov_len = sizeof(ip);
|
iov[1].iov_base = &udp;
|
iov[1].iov_len = sizeof(udp);
|
iov[2].iov_base = message->data();
|
iov[2].iov_len = message->capacity();
|
|
ssize_t bytesRead = ::readv(mSocketFd, iov, 3);
|
if (bytesRead < 0) {
|
return Result::error("Unable to read from socket: %s", strerror(errno));
|
}
|
if (static_cast<size_t>(bytesRead) < sizeof(ip) + sizeof(udp)) {
|
// Not enough bytes to even cover IP and UDP headers
|
*isValid = false;
|
return Result::success();
|
}
|
*isValid = ip.version == IPVERSION &&
|
ip.ihl == (sizeof(ip) >> 2) &&
|
ip.protocol == IPPROTO_UDP &&
|
udp.dest == htons(expectedPort);
|
|
message->setSize(bytesRead - sizeof(ip) - sizeof(udp));
|
return Result::success();
|
}
|
|
Result Socket::enableOption(int level, int optionName) {
|
if (mSocketFd == -1) {
|
return Result::error("Socket not open");
|
}
|
|
int enabled = 1;
|
int status = ::setsockopt(mSocketFd,
|
level,
|
optionName,
|
&enabled,
|
sizeof(enabled));
|
if (status == -1) {
|
return Result::error("Failed to set socket option: %s",
|
strerror(errno));
|
}
|
return Result::success();
|
}
|
