105 lines
3.2 KiB
C++
105 lines
3.2 KiB
C++
#include <cstring>
|
|
|
|
#include "UdpVpnServer.hpp"
|
|
#include "ip_header.hpp"
|
|
|
|
UdpVpnServer::UdpVpnServer(in_port_t port) : UdpVpn() {
|
|
memset(&_bind_addr, 0, sizeof(_bind_addr));
|
|
bind(in6addr_any, port);
|
|
}
|
|
|
|
UdpVpnServer::UdpVpnServer(const struct in6_addr& bind_addr6, in_port_t port)
|
|
: UdpVpn()
|
|
{
|
|
memset(&_bind_addr, 0, sizeof(_bind_addr));
|
|
bind(bind_addr6, port);
|
|
}
|
|
|
|
void UdpVpnServer::bind(const struct in6_addr& bind_addr6, in_port_t port) {
|
|
int rc;
|
|
|
|
_bind_addr.sin6_family = AF_INET6;
|
|
_bind_addr.sin6_port = htons(port);
|
|
_bind_addr.sin6_addr = bind_addr6;
|
|
|
|
rc = ::bind(
|
|
_socket, (const struct sockaddr*)&_bind_addr, sizeof(_bind_addr));
|
|
if(rc < 0) {
|
|
throw UdpVpn::InitializationError("Cannot bind socket", errno, true);
|
|
}
|
|
|
|
debugf("> Listening on port %d\n", port);
|
|
}
|
|
|
|
|
|
std::shared_ptr<VpnPeer> UdpVpnServer::get_peer_for_ip(
|
|
const in6_addr& peer_addr)
|
|
{
|
|
auto peer_iter = _peers.find(peer_addr);
|
|
if(peer_iter == _peers.end()) // Unknown peer
|
|
return nullptr;
|
|
return peer_iter->second;
|
|
}
|
|
|
|
void UdpVpnServer::receive_from_tun() {
|
|
char buffer[1500];
|
|
size_t nread = read_from_tun(buffer, 1500);
|
|
if(nread == 0)
|
|
return;
|
|
|
|
// Parse inner packet header -- assume IPv6 for now [FIXME]
|
|
IPv6Header inner_header;
|
|
if(!parse_ipv6_header(buffer, nread, inner_header)) {
|
|
// Not a valid header -- ignore the packet
|
|
debugf("Ignoring outgoing packet with invalid header\n");
|
|
return;
|
|
}
|
|
|
|
// Recover VpnPeer -- or drop if new
|
|
std::shared_ptr<VpnPeer> peer = get_peer_for_ip(inner_header.dest);
|
|
if(!peer) {
|
|
debugf("Dropping packet for destination %s -- unknown peer.\n",
|
|
format_address(inner_header.dest.s6_addr));
|
|
return;
|
|
}
|
|
|
|
kdebugf("Transmitting %s -> %s, size %d\n",
|
|
format_address(inner_header.source.s6_addr),
|
|
format_address(inner_header.dest.s6_addr),
|
|
nread);
|
|
peer->write(buffer, nread);
|
|
}
|
|
|
|
void UdpVpnServer::receive_from_udp() {
|
|
char buffer[1500];
|
|
sockaddr_in6 peer_addr;
|
|
size_t nread = read_from_udp(buffer, 1500, peer_addr);
|
|
|
|
// Parse inner packet header -- assume IPv6 for now [FIXME]
|
|
IPv6Header inner_header;
|
|
if(!parse_ipv6_header(buffer, nread, inner_header)) {
|
|
// Not a valid header -- ignore the packet
|
|
debugf("Ignoring packet with invalid header\n");
|
|
return;
|
|
}
|
|
|
|
// Recover VpnPeer -- or create if new
|
|
std::shared_ptr<VpnPeer> peer = get_peer_for_ip(inner_header.source);
|
|
if(!peer) {
|
|
peer = std::make_shared<VpnPeer>(this, peer_addr, inner_header.source);
|
|
auto insert_result = _peers.insert({inner_header.source, peer});
|
|
|
|
debugf("Got new peer %s:%d -- %s [really inserted=%d]\n",
|
|
format_address(peer_addr.sin6_addr.s6_addr),
|
|
htons(peer_addr.sin6_port),
|
|
format_address(inner_header.source.s6_addr),
|
|
insert_result.second);
|
|
}
|
|
// VpnPeer* peer = (peer_iter->second);
|
|
// TODO -- pass the packet to `peer` for a cleaner flow
|
|
|
|
// Reinject into tun
|
|
kdebugf("Receiving packet of size %d from peer\n", nread);
|
|
_tun_dev.write(buffer, nread);
|
|
}
|