96 lines
2.8 KiB
C++
96 lines
2.8 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() {
|
|
VpnPacket packet(_vpn_mtu);
|
|
size_t nread = read_from_tun(packet);
|
|
if(nread == 0)
|
|
return;
|
|
|
|
// Recover VpnPeer -- or drop if new
|
|
const in6_addr& peer_inner_addr = packet.get_ipv6_header().dest;
|
|
std::shared_ptr<VpnPeer> peer = get_peer_for_ip(peer_inner_addr);
|
|
if(!peer) {
|
|
debugf("Dropping packet for destination %s -- unknown peer.\n",
|
|
format_address(peer_inner_addr.s6_addr));
|
|
return;
|
|
}
|
|
packet.set_peer(peer);
|
|
|
|
kdebugf("Transmitting %s -> %s, size %d\n",
|
|
format_address(packet.get_ipv6_header().source.s6_addr),
|
|
format_address(packet.get_ipv6_header().dest.s6_addr),
|
|
nread);
|
|
packet.prepare_for_sending();
|
|
peer->write(packet);
|
|
}
|
|
|
|
void UdpVpnServer::receive_from_udp() {
|
|
VpnPacket packet(_vpn_mtu);
|
|
sockaddr_in6 peer_addr;
|
|
size_t nread = read_from_udp(packet, peer_addr);
|
|
if(nread == 0)
|
|
return;
|
|
|
|
// Recover VpnPeer -- or create if new
|
|
const in6_addr& peer_inner_addr = packet.get_ipv6_header().source;
|
|
std::shared_ptr<VpnPeer> peer = get_peer_for_ip(peer_inner_addr);
|
|
if(!peer) {
|
|
peer = std::make_shared<VpnPeer>(this, peer_addr, peer_inner_addr);
|
|
_peers.insert({peer_inner_addr, peer});
|
|
|
|
debugf("Got new peer %s:%d -- %s\n",
|
|
format_address(peer_addr.sin6_addr.s6_addr),
|
|
htons(peer_addr.sin6_port),
|
|
format_address(peer_inner_addr.s6_addr));
|
|
}
|
|
packet.set_peer(peer);
|
|
|
|
// Reinject into tun
|
|
kdebugf("Receiving packet #%u of size %d from %s\n",
|
|
packet.get_seqno(),
|
|
nread,
|
|
format_address(packet.get_ipv6_header().source.s6_addr));
|
|
_tun_dev.write(packet.get_payload(), packet.get_payload_size());
|
|
}
|