2018-08-17 21:49:58 +02:00
|
|
|
#include <llarp/endian.h>
|
2018-08-21 15:02:05 +02:00
|
|
|
#include <algorithm>
|
2018-08-23 20:48:41 +02:00
|
|
|
#include <llarp/ip.hpp>
|
|
|
|
#include "llarp/buffer.hpp"
|
2018-08-22 17:52:10 +02:00
|
|
|
#include "mem.hpp"
|
2018-09-19 01:44:23 +02:00
|
|
|
#ifndef _WIN32
|
|
|
|
#include <netinet/in.h>
|
|
|
|
#endif
|
2018-09-18 13:08:47 +02:00
|
|
|
#include <llarp/endian.h>
|
2018-09-16 14:06:19 +02:00
|
|
|
#include <map>
|
2018-10-10 14:06:28 +02:00
|
|
|
#include <algorithm>
|
2018-08-17 21:49:58 +02:00
|
|
|
|
|
|
|
namespace llarp
|
|
|
|
{
|
|
|
|
namespace net
|
|
|
|
{
|
2018-08-20 21:12:12 +02:00
|
|
|
bool
|
|
|
|
IPv4Packet::Load(llarp_buffer_t pkt)
|
2018-08-17 21:49:58 +02:00
|
|
|
{
|
2018-10-09 19:49:20 +02:00
|
|
|
sz = std::min(pkt.sz, sizeof(buf));
|
2018-08-22 17:52:10 +02:00
|
|
|
memcpy(buf, pkt.base, sz);
|
2018-08-20 21:12:12 +02:00
|
|
|
return true;
|
2018-08-17 21:49:58 +02:00
|
|
|
}
|
2018-08-20 21:12:12 +02:00
|
|
|
|
2018-08-22 17:52:10 +02:00
|
|
|
llarp_buffer_t
|
|
|
|
IPv4Packet::Buffer()
|
|
|
|
{
|
|
|
|
return llarp::InitBuffer(buf, sz);
|
|
|
|
}
|
|
|
|
|
2018-10-09 19:02:49 +02:00
|
|
|
static uint32_t
|
2018-10-10 17:14:45 +02:00
|
|
|
ipchksum_pseudoIPv4(nuint32_t src_ip, nuint32_t dst_ip, uint8_t proto,
|
2018-10-09 19:09:45 +02:00
|
|
|
uint16_t innerlen)
|
2018-10-09 19:02:49 +02:00
|
|
|
{
|
2018-10-09 23:56:20 +02:00
|
|
|
#define IPCS(x) ((uint32_t)(x & 0xFFFF) + (uint32_t)(x >> 16))
|
2018-10-10 17:14:45 +02:00
|
|
|
uint32_t sum = IPCS(src_ip.n) + IPCS(dst_ip.n) + (uint32_t)proto
|
2018-10-09 23:56:20 +02:00
|
|
|
+ (uint32_t)htons(innerlen);
|
2018-10-09 19:09:45 +02:00
|
|
|
#undef IPCS
|
2018-10-09 19:02:49 +02:00
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
|
2018-09-16 14:06:19 +02:00
|
|
|
static uint16_t
|
2018-09-18 13:08:47 +02:00
|
|
|
ipchksum(const byte_t *buf, size_t sz, uint32_t sum = 0)
|
2018-09-16 14:06:19 +02:00
|
|
|
{
|
|
|
|
while(sz > 1)
|
2018-08-20 21:12:12 +02:00
|
|
|
{
|
2018-09-16 14:06:19 +02:00
|
|
|
sum += *(const uint16_t *)buf;
|
|
|
|
sz -= sizeof(uint16_t);
|
|
|
|
buf += sizeof(uint16_t);
|
2018-08-20 21:12:12 +02:00
|
|
|
}
|
2018-09-16 14:06:19 +02:00
|
|
|
if(sz > 0)
|
|
|
|
sum += *(const byte_t *)buf;
|
2018-08-20 21:12:12 +02:00
|
|
|
|
|
|
|
while(sum >> 16)
|
|
|
|
sum = (sum & 0xffff) + (sum >> 16);
|
|
|
|
|
2018-09-16 14:06:19 +02:00
|
|
|
return ~sum;
|
|
|
|
}
|
|
|
|
|
2018-10-09 19:02:49 +02:00
|
|
|
static uint16_t
|
2018-10-10 17:14:45 +02:00
|
|
|
deltachksum(uint16_t old_sum, huint32_t old_src_ip, huint32_t old_dst_ip,
|
|
|
|
huint32_t new_src_ip, huint32_t new_dst_ip)
|
2018-10-09 19:02:49 +02:00
|
|
|
{
|
2018-10-10 17:14:45 +02:00
|
|
|
#define ADDIPCS(x) ((uint32_t)(x.h & 0xFFFF) + (uint32_t)(x.h >> 16))
|
|
|
|
#define SUBIPCS(x) ((uint32_t)((~x.h) & 0xFFFF) + (uint32_t)((~x.h) >> 16))
|
2018-10-10 03:59:52 +02:00
|
|
|
|
2018-10-10 17:14:45 +02:00
|
|
|
uint32_t sum = ntohs(old_sum) + ADDIPCS(old_src_ip) + ADDIPCS(old_dst_ip)
|
|
|
|
+ SUBIPCS(new_src_ip) + SUBIPCS(new_dst_ip);
|
2018-10-10 03:29:44 +02:00
|
|
|
|
2018-10-09 23:56:20 +02:00
|
|
|
#undef ADDIPCS
|
|
|
|
#undef SUBIPCS
|
2018-10-10 03:29:44 +02:00
|
|
|
|
2018-10-09 19:02:49 +02:00
|
|
|
while(sum >> 16)
|
|
|
|
sum = (sum & 0xffff) + (sum >> 16);
|
2018-10-10 03:29:44 +02:00
|
|
|
|
|
|
|
return htons(sum);
|
2018-10-09 19:02:49 +02:00
|
|
|
}
|
|
|
|
|
2018-09-18 13:08:47 +02:00
|
|
|
static std::map<
|
|
|
|
byte_t, std::function< void(const ip_header *, byte_t *, size_t) > >
|
2018-10-09 19:09:45 +02:00
|
|
|
protoDstCheckSummer = {
|
2018-10-10 04:38:59 +02:00
|
|
|
// {RFC3022} says that IPv4 hdr isn't included in ICMP checksum calc
|
|
|
|
// and that we don't need to modify it
|
2018-10-09 19:09:45 +02:00
|
|
|
{// TCP
|
|
|
|
6,
|
|
|
|
[](const ip_header *hdr, byte_t *pkt, size_t sz) {
|
2018-10-10 14:06:28 +02:00
|
|
|
auto hlen = size_t(hdr->ihl * 4);
|
|
|
|
|
2018-10-09 19:09:45 +02:00
|
|
|
uint16_t *check = (uint16_t *)(pkt + hlen + 16);
|
2018-10-10 14:06:28 +02:00
|
|
|
|
2018-10-10 17:14:45 +02:00
|
|
|
*check = deltachksum(*check, huint32_t{0}, huint32_t{0},
|
|
|
|
xntohl(nuint32_t{hdr->saddr}),
|
|
|
|
xntohl(nuint32_t{hdr->daddr}));
|
2018-10-09 19:09:45 +02:00
|
|
|
}},
|
2018-10-10 04:38:59 +02:00
|
|
|
{// UDP
|
|
|
|
17,
|
|
|
|
[](const ip_header *hdr, byte_t *pkt, size_t sz) {
|
2018-10-10 14:06:28 +02:00
|
|
|
auto hlen = size_t(hdr->ihl * 4);
|
|
|
|
|
2018-10-10 19:51:37 +02:00
|
|
|
uint16_t *check = (uint16_t *)(pkt + hlen + 6);
|
2018-10-10 04:38:59 +02:00
|
|
|
if(*check != 0xFFff)
|
|
|
|
{
|
|
|
|
if(*check == 0x0000)
|
|
|
|
return; // don't change zero
|
|
|
|
|
2018-10-10 17:14:45 +02:00
|
|
|
*check = deltachksum(*check, huint32_t{0}, huint32_t{0},
|
|
|
|
xntohl(nuint32_t{hdr->saddr}),
|
|
|
|
xntohl(nuint32_t{hdr->daddr}));
|
2018-10-10 04:38:59 +02:00
|
|
|
if(*check == 0x0000)
|
|
|
|
*check = 0xFFff;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
// such checksum can mean 2 things: 0x0000 or 0xFFff
|
|
|
|
// we can only know by looking at data :<
|
|
|
|
if(hlen > sz)
|
|
|
|
return; // malformed, bail out
|
|
|
|
|
2018-10-10 14:06:28 +02:00
|
|
|
auto oldcs = *check;
|
|
|
|
|
|
|
|
*check = 0; // zero checksum before calculation
|
|
|
|
|
2018-10-10 17:14:45 +02:00
|
|
|
auto cs =
|
|
|
|
ipchksum(pkt + hlen, sz - hlen,
|
|
|
|
ipchksum_pseudoIPv4(nuint32_t{0}, nuint32_t{0},
|
|
|
|
17, sz - hlen));
|
2018-10-10 04:38:59 +02:00
|
|
|
|
2018-10-10 17:14:45 +02:00
|
|
|
auto mod_cs = deltachksum(cs, huint32_t{0}, huint32_t{0},
|
|
|
|
xntohl(nuint32_t{hdr->saddr}),
|
|
|
|
xntohl(nuint32_t{hdr->daddr}));
|
2018-10-10 04:38:59 +02:00
|
|
|
|
|
|
|
if(cs != 0x0000 && cs != 0xFFff)
|
|
|
|
{
|
|
|
|
// packet was bad - sabotage new checksum
|
2018-10-10 14:06:28 +02:00
|
|
|
mod_cs += cs - oldcs;
|
2018-10-10 04:38:59 +02:00
|
|
|
}
|
|
|
|
// 0x0000 is reserved for no checksum
|
|
|
|
if(mod_cs == 0x0000)
|
|
|
|
mod_cs = 0xFFff;
|
|
|
|
// put it in
|
|
|
|
*check = mod_cs;
|
|
|
|
}
|
|
|
|
}},
|
|
|
|
|
2018-10-09 19:02:49 +02:00
|
|
|
};
|
2018-09-16 14:06:19 +02:00
|
|
|
void
|
2018-10-09 16:09:03 +02:00
|
|
|
IPv4Packet::UpdateChecksumsOnDst()
|
2018-09-16 14:06:19 +02:00
|
|
|
{
|
2018-10-09 19:02:49 +02:00
|
|
|
auto hdr = Header();
|
|
|
|
|
2018-10-09 16:09:03 +02:00
|
|
|
// IPv4 checksum
|
2018-10-10 17:14:45 +02:00
|
|
|
hdr->check = deltachksum(hdr->check, huint32_t{0}, huint32_t{0},
|
|
|
|
xntohl(nuint32_t{hdr->saddr}),
|
|
|
|
xntohl(nuint32_t{hdr->daddr}));
|
2018-10-09 16:09:03 +02:00
|
|
|
|
|
|
|
// L4 checksum
|
2018-09-17 20:59:12 +02:00
|
|
|
auto proto = hdr->protocol;
|
2018-10-09 19:02:49 +02:00
|
|
|
auto itr = protoDstCheckSummer.find(proto);
|
|
|
|
if(itr != protoDstCheckSummer.end())
|
2018-09-16 14:06:19 +02:00
|
|
|
{
|
2018-09-18 13:08:47 +02:00
|
|
|
itr->second(hdr, buf, sz);
|
2018-09-16 14:06:19 +02:00
|
|
|
}
|
2018-08-20 21:12:12 +02:00
|
|
|
}
|
2018-10-09 16:09:03 +02:00
|
|
|
|
2018-10-09 19:02:49 +02:00
|
|
|
static std::map<
|
2018-10-09 19:09:45 +02:00
|
|
|
byte_t, std::function< void(const ip_header *, byte_t *, size_t) > >
|
|
|
|
protoSrcCheckSummer = {
|
|
|
|
{// TCP
|
|
|
|
6,
|
|
|
|
[](const ip_header *hdr, byte_t *pkt, size_t sz) {
|
2018-10-10 04:38:59 +02:00
|
|
|
auto hlen = size_t(hdr->ihl * 4);
|
|
|
|
|
2018-10-09 19:09:45 +02:00
|
|
|
uint16_t *check = (uint16_t *)(pkt + hlen + 16);
|
2018-10-10 14:06:28 +02:00
|
|
|
|
2018-10-10 17:14:45 +02:00
|
|
|
*check = deltachksum(*check, xntohl(nuint32_t{hdr->saddr}),
|
|
|
|
xntohl(nuint32_t{hdr->daddr}), huint32_t{0},
|
|
|
|
huint32_t{0});
|
2018-10-09 19:09:45 +02:00
|
|
|
}},
|
2018-10-10 04:38:59 +02:00
|
|
|
{// UDP
|
|
|
|
17,
|
|
|
|
[](const ip_header *hdr, byte_t *pkt, size_t sz) {
|
2018-10-10 14:06:28 +02:00
|
|
|
auto hlen = size_t(hdr->ihl * 4);
|
|
|
|
|
2018-10-10 19:51:37 +02:00
|
|
|
uint16_t *check = (uint16_t *)(pkt + hlen + 6);
|
2018-10-10 04:38:59 +02:00
|
|
|
if(*check != 0xFFff)
|
|
|
|
{
|
|
|
|
if(*check == 0x0000)
|
|
|
|
return; // don't change zero
|
|
|
|
|
2018-10-10 17:14:45 +02:00
|
|
|
*check = deltachksum(*check, xntohl(nuint32_t{hdr->saddr}),
|
|
|
|
xntohl(nuint32_t{hdr->daddr}),
|
|
|
|
huint32_t{0}, huint32_t{0});
|
2018-10-10 04:38:59 +02:00
|
|
|
if(*check == 0x0000)
|
|
|
|
*check = 0xFFff;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
// such checksum can mean 2 things: 0x0000 or 0xFFff
|
|
|
|
// we can only know by looking at data :<
|
|
|
|
if(hlen > sz)
|
|
|
|
return; // malformed, bail out
|
|
|
|
|
2018-10-10 14:06:28 +02:00
|
|
|
auto oldcs = *check;
|
|
|
|
|
|
|
|
*check = 0; // zero checksum before calculation
|
|
|
|
|
2018-10-10 17:14:45 +02:00
|
|
|
auto cs = ipchksum(
|
|
|
|
pkt + hlen, sz - hlen,
|
|
|
|
ipchksum_pseudoIPv4(nuint32_t{hdr->saddr},
|
|
|
|
nuint32_t{hdr->daddr}, 17, sz - hlen));
|
2018-10-10 04:38:59 +02:00
|
|
|
|
2018-10-10 17:14:45 +02:00
|
|
|
auto mod_cs = deltachksum(cs, xntohl(nuint32_t{hdr->saddr}),
|
|
|
|
xntohl(nuint32_t{hdr->daddr}),
|
|
|
|
huint32_t{0}, huint32_t{0});
|
2018-10-10 04:38:59 +02:00
|
|
|
|
|
|
|
if(cs != 0x0000 && cs != 0xFFff)
|
|
|
|
{
|
|
|
|
// packet was bad - sabotage new checksum
|
2018-10-10 14:06:28 +02:00
|
|
|
mod_cs += cs - oldcs;
|
2018-10-10 04:38:59 +02:00
|
|
|
}
|
|
|
|
// 0x0000 is reserved for no checksum
|
|
|
|
if(mod_cs == 0x0000)
|
|
|
|
mod_cs = 0xFFff;
|
|
|
|
// put it in
|
|
|
|
*check = mod_cs;
|
|
|
|
}
|
|
|
|
}},
|
2018-10-09 19:02:49 +02:00
|
|
|
};
|
2018-10-09 16:09:03 +02:00
|
|
|
void
|
|
|
|
IPv4Packet::UpdateChecksumsOnSrc()
|
|
|
|
{
|
2018-10-09 19:02:49 +02:00
|
|
|
auto hdr = Header();
|
|
|
|
|
|
|
|
// L4
|
|
|
|
auto proto = hdr->protocol;
|
|
|
|
auto itr = protoSrcCheckSummer.find(proto);
|
|
|
|
if(itr != protoSrcCheckSummer.end())
|
|
|
|
{
|
|
|
|
itr->second(hdr, buf, sz);
|
|
|
|
}
|
|
|
|
|
2018-10-09 16:09:03 +02:00
|
|
|
// IPv4
|
2018-10-10 17:14:45 +02:00
|
|
|
hdr->check = deltachksum(hdr->check, xntohl(nuint32_t{hdr->saddr}),
|
|
|
|
xntohl(nuint32_t{hdr->daddr}), huint32_t{0},
|
|
|
|
huint32_t{0});
|
2018-10-09 16:09:03 +02:00
|
|
|
}
|
2018-08-17 21:49:58 +02:00
|
|
|
} // namespace net
|
|
|
|
} // namespace llarp
|