231 lines
6.9 KiB
C
231 lines
6.9 KiB
C
#include <string.h>
|
|
#include <assert.h>
|
|
|
|
#include "util.h"
|
|
#include "rlocs.h"
|
|
#include "packet.h"
|
|
|
|
// shamelessly copied from:
|
|
// http://www.roman10.net/how-to-calculate-iptcpudp-checksumpart-2-implementation/
|
|
unsigned short compute_checksum(unsigned short *addr, unsigned int count) {
|
|
|
|
unsigned long sum = 0;
|
|
|
|
while (count > 1) {
|
|
sum += * addr++;
|
|
count -= 2;
|
|
}
|
|
|
|
//if any bytes left, pad the bytes and add
|
|
if(count > 0) {
|
|
sum += ((*addr)&htons(0xFF00));
|
|
}
|
|
|
|
//Fold sum to 16 bits: add carrier to result
|
|
while (sum>>16) {
|
|
sum = (sum & 0xffff) + (sum >> 16);
|
|
}
|
|
|
|
//one's complement
|
|
sum = ~sum;
|
|
|
|
return ((unsigned short)sum);
|
|
|
|
}
|
|
|
|
void compute_ip_checksum(struct iphdr* pkt)
|
|
{
|
|
pkt->check = 0x0000;
|
|
pkt->check = compute_checksum( (unsigned short*) pkt, pkt->ihl * 4 );
|
|
}
|
|
|
|
|
|
int wrap_ipv4_packet(struct rlocs* reg, struct recv_pkt* pkt, struct rsp_data* out)
|
|
{
|
|
out->count = 3;
|
|
assert( out->count < MAX_IOVS );
|
|
|
|
unsigned char *scratch = &out->scratch[0];
|
|
|
|
// iovec 0: wrapping header
|
|
struct iphdr* wrap_hdr = (struct iphdr*) scratch;
|
|
unsigned int wrap_hdr_size = sizeof( struct iphdr );
|
|
scratch += wrap_hdr_size;
|
|
|
|
memset( wrap_hdr, 0, wrap_hdr_size );
|
|
|
|
wrap_hdr->version = 0x04;
|
|
wrap_hdr->ihl = wrap_hdr_size / 4;
|
|
wrap_hdr->ttl = IPDEFTTL;
|
|
wrap_hdr->protocol = IPPROTO_HIDE_EID;
|
|
wrap_hdr->frag_off = htons( 0x4000 ); // DF bit set
|
|
|
|
out->iovs[0].iov_base = wrap_hdr;
|
|
out->iovs[0].iov_len = wrap_hdr_size;
|
|
|
|
// TODO: id, still needs filling now.
|
|
|
|
// We need to know source and destination rlocs to construct the packet
|
|
struct rloc* s_rloc;
|
|
struct rloc* d_rloc;
|
|
struct in_addr tmp;
|
|
|
|
tmp.s_addr = pkt->hdr.ip.saddr;
|
|
if ( ( s_rloc = rloc_find_for_ipv4( reg, &tmp ) ) == NULL ) {
|
|
warn( "Couldn't find source rloc, dropping packet" );
|
|
// TODO: fallback behaviour here?
|
|
return 0;
|
|
}
|
|
|
|
tmp.s_addr = pkt->hdr.ip.daddr;
|
|
if ( ( d_rloc = rloc_find_for_ipv4( reg, &tmp ) ) == NULL ) {
|
|
warn( "Couldn't find destination rloc, dropping packet" );
|
|
// TODO: fallback behaviour here?
|
|
return 0;
|
|
}
|
|
|
|
wrap_hdr->saddr = s_rloc->addr.ip4.s_addr;
|
|
wrap_hdr->daddr = d_rloc->addr.ip4.s_addr;
|
|
|
|
// iovec 1: encrypted part.
|
|
// FIXME: Need to inspect the protocol field and gobble up the TCP/UDP/etc
|
|
// header as well, for decent anonymity. TCP/UDP ports are an obvious way
|
|
// to perform a correlation attack.
|
|
// RSA pubkey encryption with 4096-bit keys gobbles up at least 512 bytes
|
|
// of space, so we make sure to use it.
|
|
ssize_t enc_size;
|
|
size_t orig_data_size = ntohs( pkt->hdr.ip.tot_len );
|
|
size_t bytes_to_encrypt;
|
|
|
|
if ( orig_data_size > 512 ) {
|
|
bytes_to_encrypt = 512; // No point wasting bytes on padding
|
|
} else {
|
|
bytes_to_encrypt = orig_data_size;
|
|
}
|
|
|
|
off_t enc_max_len = IP_MAXPACKET - wrap_hdr_size - orig_data_size - bytes_to_encrypt;
|
|
|
|
// We use two bytes to store the size of the encrypted blob
|
|
unsigned short *pkt_enc_size = (unsigned short *) scratch;
|
|
scratch += 2;
|
|
|
|
enc_size = rlocs_encrypt(
|
|
reg, s_rloc, d_rloc,
|
|
(unsigned char *)&pkt->hdr, bytes_to_encrypt, scratch, enc_max_len - 2
|
|
);
|
|
|
|
if ( enc_size < 0 ) {
|
|
warn( "failed to encrypt, dropping packet" );
|
|
return 0;
|
|
}
|
|
|
|
*pkt_enc_size = htons( enc_size );
|
|
enc_size += 2;
|
|
scratch = (unsigned char*) pkt_enc_size;
|
|
|
|
warn( "Encrypted size: 2 + %zu", enc_size - 2);
|
|
|
|
out->iovs[1].iov_base = scratch;
|
|
out->iovs[1].iov_len = enc_size;
|
|
scratch += enc_size;
|
|
|
|
// iovec 2: unencrypted remains
|
|
if ( bytes_to_encrypt == orig_data_size ) {
|
|
out->count = 2;
|
|
out->iovs[2].iov_base = NULL;
|
|
out->iovs[2].iov_len = 0;
|
|
} else {
|
|
out->iovs[2].iov_base = (char *) pkt + bytes_to_encrypt;
|
|
out->iovs[2].iov_len = ntohs( pkt->hdr.ip.tot_len ) - bytes_to_encrypt;
|
|
}
|
|
|
|
wrap_hdr->tot_len = htons( wrap_hdr_size + enc_size + out->iovs[2].iov_len );
|
|
compute_ip_checksum( wrap_hdr );
|
|
|
|
info( "Finished wrapping IPv4 packet" );
|
|
|
|
return 1;
|
|
}
|
|
|
|
int wrap_ipv6_packet(struct rlocs *reg, struct recv_pkt* pkt, struct rsp_data* out)
|
|
{
|
|
warn( "STUB: wrap_ipv6_packet" );
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
int unwrap_ipv4_packet(struct rlocs* reg, struct recv_pkt* pkt, struct rsp_data* out)
|
|
{
|
|
out->count = 2;
|
|
assert( out->count < MAX_IOVS );
|
|
|
|
// first, check this is actually a hide-eid packet.
|
|
if ( pkt->hdr.ip.protocol != IPPROTO_HIDE_EID ) {
|
|
warn( "expected IP protocol %u, not %u", IPPROTO_HIDE_EID, pkt->hdr.ip.protocol );
|
|
return 0;
|
|
}
|
|
|
|
// We need to know source and destination rloc to decrypt the packet
|
|
struct rloc *s_rloc, *d_rloc;
|
|
struct in_addr tmp;
|
|
|
|
tmp.s_addr = pkt->hdr.ip.saddr;
|
|
if ( ( s_rloc = rloc_find_by_address( reg, &tmp, NULL ) ) == NULL ) {
|
|
warn( "Couldn't find rloc from source IP, dropping packet" );
|
|
// TODO: we should be able to specify we need it to have a private key
|
|
return 0;
|
|
}
|
|
|
|
tmp.s_addr = pkt->hdr.ip.daddr;
|
|
if ( ( d_rloc = rloc_find_by_address( reg, &tmp, NULL ) ) == NULL ) {
|
|
warn( "Couldn't find rloc from destination IP, dropping packet" );
|
|
// TODO: we should be able to specify we need it to have a private key
|
|
return 0;
|
|
}
|
|
|
|
uint16_t hdr_size = pkt->hdr.ip.ihl * 4;
|
|
uint16_t encrypted_size = ntohs( *((uint16_t*)pkt + ( hdr_size / 2 )) );
|
|
info( "encrypted_size: %u", encrypted_size );
|
|
|
|
// iovec 0: decrypted data. This should be an IP header.
|
|
unsigned char *encrypted_data = ((unsigned char *)pkt) + hdr_size + 2;
|
|
unsigned char *scratch = &out->scratch[0];
|
|
|
|
int decrypted_size = rlocs_decrypt(
|
|
reg, d_rloc, s_rloc,
|
|
encrypted_data, encrypted_size, scratch, IP_MAXPACKET
|
|
);
|
|
|
|
if ( decrypted_size < 0 ) {
|
|
warn( "Failed to decrypt packet!" );
|
|
return 0;
|
|
}
|
|
info( "decrypted_size: %u", decrypted_size );
|
|
|
|
out->iovs[0].iov_base = scratch;
|
|
out->iovs[0].iov_len = decrypted_size;
|
|
|
|
// iovec 1: never-encrypted part
|
|
out->iovs[1].iov_base = (unsigned char*) pkt + hdr_size + 2;
|
|
out->iovs[1].iov_len = ntohs( pkt->hdr.ip.tot_len ) - hdr_size - encrypted_size;
|
|
|
|
if ( out->iovs[0].iov_len + out->iovs[1].iov_len > IP_MAXPACKET ) {
|
|
warn( "Unwrapped packet is too large, dropping it" );
|
|
warn( "iovs[0] is %zu, iovs[1] is %zu", out->iovs[0].iov_len, out->iovs[1].iov_len );
|
|
warn( "hdr_size = %u, encrypted_size = %u, tot_len = %u", hdr_size, encrypted_size, ntohs( pkt->hdr.ip.tot_len ) );
|
|
return 0;
|
|
}
|
|
|
|
|
|
info( "Finished unwrapping IPv4 packet" );
|
|
return 1;
|
|
}
|
|
|
|
int unwrap_ipv6_packet(struct rlocs *reg, struct recv_pkt* pkt, struct rsp_data* out)
|
|
{
|
|
warn( "STUB: unwrap_ipv6_packet" );
|
|
|
|
return 0;
|
|
}
|