bool transport_set(Packet* packet, uint16_t header_len, uint16_t tail_len) {
//check size
if(packet->start > header_len && (packet->end + tail_len) < packet->size) {
Ether* _ether = (Ether*)(packet->buffer + packet->start);
IP* _ip = (IP*)_ether->payload;
_ip->length = endian16(endian16(_ip->length) + header_len + tail_len);
packet->start -= header_len;
packet->end += tail_len;
Ether* ether = (Ether*)(packet->buffer + packet->start);
memmove(ether, _ether, ETHER_LEN + _ip->ihl * 4 /*Ether + IP Header Length*/);
return true;
} else if(packet->end + header_len + tail_len < packet->size) {
Ether* _ether = (Ether*)(packet->buffer + packet->start);
IP* _ip = (IP*)_ether->payload;
_ip->length = endian16(endian16(_ip->length) + header_len + tail_len);
memmove(_ip->body + header_len, _ip->body, _ip->length - _ip->ihl * 4 /* Body length*/);
return true;
} else {
printf("packet has not enough padding\n");
return false;
}
return true;
}
static bool arping_process(Packet* packet, void* context) {
ARPingContext* arping_context = context;
Ether* ether = (Ether*)(packet->buffer + packet->start);
if(endian16(ether->type) != ETHER_TYPE_ARP)
return true;
ARP* arp = (ARP*)ether->payload;
switch(endian16(arp->operation)) {
case 2: // Reply
;
uint64_t smac = endian48(arp->sha);
uint32_t sip = endian32(arp->spa);
if(arping_context->addr == sip) {
uint32_t time = timer_ns() - arping_context->time;
uint32_t ms = time / 1000;
uint32_t ns = time - ms * 1000;
printf("Reply from %d.%d.%d.%d [%02x:%02x:%02x:%02x:%02x:%02x] %d.%dms\n",
(sip >> 24) & 0xff,
(sip >> 16) & 0xff,
(sip >> 8) & 0xff,
(sip >> 0) & 0xff,
(smac >> 40) & 0xff,
(smac >> 32) & 0xff,
(smac >> 24) & 0xff,
(smac >> 16) & 0xff,
(smac >> 8) & 0xff,
(smac >> 0) & 0xff,
ms, ns);
}
break;
}
bool transport_unset(Packet* packet, uint16_t header_len, uint16_t tail_len) {
Ether* _ether = (Ether*)(packet->buffer + packet->start);
IP* _ip = (IP*)_ether->payload;
_ip->length = endian16(endian16(_ip->length) - header_len - tail_len);
memmove(_ether + header_len, _ether, ETHER_LEN + _ip->ihl * 4);
packet->start += header_len;
packet->end -= tail_len;
return true;
}
void ip_pack(Packet* packet, uint16_t ip_body_len) {
Ether* ether = (Ether*)(packet->buffer + packet->start);
IP* ip = (IP*)ether->payload;
ip->length = endian16(ip->ihl * 4 + ip_body_len);
ip->ttl = IP_TTL;
ip->checksum = 0;
ip->checksum = endian16(checksum(ip, ip->ihl * 4));
packet->end = packet->start + ETHER_LEN + ip->ihl * 4 + ip_body_len;
}
TSS_RESULT
get_trans_props(TSS_HCONTEXT tspContext, UINT32 *alg, UINT16 *enc)
{
TSS_RESULT result;
UINT32 algs[] = { TPM_ALG_MGF1, TPM_ALG_AES128, 0 }, a = 0;
UINT16 encs[] = { TPM_ES_SYM_OFB, TPM_ES_SYM_CNT, TPM_ES_SYM_CBC_PKCS5PAD, 0 }, e = 0;
BYTE *respData;
UINT32 respLen, tcsSubCap32;
UINT16 tcsSubCap16;
if (*alg)
goto check_es;
for (a = 0; algs[a]; a++) {
tcsSubCap32 = endian32(algs[a]);
if ((result = RPC_GetTPMCapability(tspContext, TPM_CAP_TRANS_ALG, sizeof(UINT32),
(BYTE *)&tcsSubCap32, &respLen, &respData)))
return result;
if (*(TSS_BOOL *)respData == TRUE) {
free(respData);
break;
}
free(respData);
}
if (!algs[a]) {
LogError("TPM reports no usable sym algorithms for transport session");
return TSPERR(TSS_E_INTERNAL_ERROR);
}
check_es:
if (*enc || algs[a] == TPM_ALG_MGF1)
goto done;
for (e = 0; encs[e]; e++) {
tcsSubCap16 = endian16(encs[e]);
if ((result = RPC_GetTPMCapability(tspContext, TPM_CAP_TRANS_ES, sizeof(UINT16),
(BYTE *)&tcsSubCap16, &respLen, &respData)))
return result;
if (*(TSS_BOOL *)respData == TRUE) {
free(respData);
break;
}
free(respData);
}
if (!encs[e]) {
LogError("TPM reports no usable sym modes for transport session");
return TSPERR(TSS_E_INTERNAL_ERROR);
}
*alg = algs[a];
*enc = encs[e];
done:
return TSS_SUCCESS;
}
bool tunnel_set(Packet* packet, uint16_t header_len, uint16_t tail_len) {
//check size
if((packet->start > (header_len + IP_LEN)) && (packet->end + tail_len) < packet->size) {
Ether* _ether = (Ether*)(packet->buffer + packet->start);
IP* _ip = (IP*)_ether->payload;
unsigned char* padding = NULL;
packet->start -= (IP_LEN + header_len);
padding = packet->buffer + packet->end;
for(int i = 0; i < tail_len; i++) {
padding[i] = i + 1;
}
packet->end += tail_len;
Ether* ether = (Ether*)(packet->buffer + packet->start);
IP* ip = (IP*)ether->payload;
ip->ihl = _ip->ihl;
ip->version = _ip->version;
ip->ecn = _ip->ecn;
ip->dscp = _ip->dscp;
ip->length = endian16(endian16(_ip->length) + IP_LEN + header_len + tail_len);
ip->id = _ip->id;
ip->flags_offset = _ip->flags_offset;
return true;
} else if(packet->end + IP_LEN + header_len + tail_len < packet->size) {
Ether* ether = (Ether*)(packet->buffer + packet->start);
IP* ip = (IP*)ether->payload;
memmove(ip->body + header_len, ip, ip->length);
ip->length = endian16(IP_LEN + endian16(ip->length) + header_len + tail_len);
packet->end += IP_LEN + header_len + tail_len;
return true;
} else {
printf("packet has not enough padding\n");
return false;
}
return true;
}
bool lifx_turn_on_blue(Actuator* actuator) {
lifx_set_power(actuator, 0xffff, 0xffffffff);
HSBK color;
for(int i = 0; i < 0xffff; i++) {
color.hue = endian16(i);
color.saturation = 0xffff;
color.brightness = 0xffff;
color.kelvin = 0x0dac;
lifx_set_color(actuator, &color, 0x0);
}
return true;
}
void process(NIC* ni) {
static uint32_t myip = 0xc0a86402; // 192.168.100.2
Packet* packet = nic_input(ni);
if(!packet)
return;
Ether* ether = (Ether*)(packet->buffer + packet->start);
if(endian16(ether->type) == ETHER_TYPE_ARP) {
// ARP response
ARP* arp = (ARP*)ether->payload;
if(endian16(arp->operation) == 1 && endian32(arp->tpa) == myip) {
ether->dmac = ether->smac;
ether->smac = endian48(ni->mac);
arp->operation = endian16(2);
arp->tha = arp->sha;
arp->tpa = arp->spa;
arp->sha = ether->smac;
arp->spa = endian32(myip);
nic_output(ni, packet);
packet = NULL;
}
} else if(endian16(ether->type) == ETHER_TYPE_IPv4) {
IP* ip = (IP*)ether->payload;
if(ip->protocol == IP_PROTOCOL_ICMP && endian32(ip->destination) == myip) {
// Echo reply
ICMP* icmp = (ICMP*)ip->body;
icmp->type = 0;
icmp->checksum = 0;
icmp->checksum = endian16(checksum(icmp, packet->end - packet->start - ETHER_LEN - IP_LEN));
ip->destination = ip->source;
ip->source = endian32(myip);
ip->ttl = endian8(64);
ip->checksum = 0;
ip->checksum = endian16(checksum(ip, ip->ihl * 4));
ether->dmac = ether->smac;
ether->smac = endian48(ni->mac);
nic_output(ni, packet);
packet = NULL;
} else if(ip->protocol == IP_PROTOCOL_UDP) {
UDP* udp = (UDP*)ip->body;
if(endian16(udp->destination) == 9000) {
reply_count = 2;
// Control packet
uint32_t idx = 0;
seq = read_u16(udp->body, &idx);
user_mac = endian48(ether->smac);
user_ip = endian32(ip->source);
user_port = endian16(udp->source);
uint8_t msg = read_u8(udp->body, &idx);
switch(msg) {
case 1: // MSG_CREATE
{
idx++; // read ctype
uint32_t clen = read_u32(udp->body, &idx);
char collection[clen + 1];
collection[clen] = '\0';
memcpy(collection, udp->body + idx, clen);
idx += clen;
uint64_t id = newID(collection);
memmove(udp->body + idx + 9, udp->body + idx, endian16(udp->length) - 8 - idx);
packet->end += 9;
udp->length = endian16(endian16(udp->length) + 9);
ip->length = endian16(endian16(ip->length) + 9);
write_u8(udp->body, 4, &idx);
write_u64(udp->body, id, &idx);
}
break;
case 2: // MSG_READ
reply_count = 1;
break;
case 3: // MSG_RETRIEVE
break;
case 4: // MSG_UPDATE
break;
case 5: // MSG_DELETE
break;
case 6: // MSG_HELLO
reply_count = 1;
break;
}
udp->source = endian16(9999);
udp->destination = endian16(9001);
udp->checksum = 0;
ip->destination = ip->source;
ip->source = endian32(myip);
ip->ttl = endian8(ip->ttl) - 1;
ip->checksum = 0;
ip->checksum = endian16(checksum(ip, ip->ihl * 4));
ether->dmac = ether->smac;
ether->smac = endian48(ni->mac);
nic_output_dup(ni, packet);
udp->destination = endian16(9002);
ip->checksum = 0;
ip->checksum = endian16(checksum(ip, ip->ihl * 4));
nic_output_dup(ni, packet);
udp->destination = endian16(9003);
ip->checksum = 0;
ip->checksum = endian16(checksum(ip, ip->ihl * 4));
nic_output(ni, packet);
packet = NULL;
} else if(endian16(udp->destination) == 9999) {
uint32_t idx = 0;
uint16_t seq2 = read_u16(udp->body, &idx);
if(seq == seq2 && --reply_count == 0) {
udp->checksum = 0;
udp->destination = endian16(user_port);
udp->source = endian16(9000);
ip->destination = endian32(user_ip);
ip->source = endian32(myip);
ip->ttl = endian8(64);
ip->checksum = 0;
ip->checksum = endian16(checksum(ip, ip->ihl * 4));
ether->dmac = endian48(user_mac);
ether->smac = endian48(ni->mac);
nic_output(ni, packet);
packet = NULL;
}
}
}
}
if(packet)
nic_free(packet);
}
void process(NetworkInterface* ni) {
Packet* packet = ni_input(ni);
if(!packet)
return;
if(arp_process(packet))
return;
Ether* ether = (Ether*)(packet->buffer + packet->start);
/*
if(endian16(ether->type) == ETHER_TYPE_ARP) {
// ARP response
ARP* arp = (ARP*)ether->payload;
if(endian16(arp->operation) == 1 && endian32(arp->tpa) == address) {
ether->dmac = ether->smac;
ether->smac = endian48(ni->mac);
arp->operation = endian16(2);
arp->tha = arp->sha;
arp->tpa = arp->spa;
arp->sha = ether->smac;
arp->spa = endian32(address);
printf("- Ethernet Header -\n");
printf("Dst Mac : %012lx Src Mac : %012lx\n Ether Type : %04hx\n",
endian48(ether->dmac), endian48(ether->smac), ether->type);
ni_output(ni, packet);
packet = NULL;
}
}*/
if(endian16(ether->type) == ETHER_TYPE_IPv4) {
IP* ip = (IP*)ether->payload;
if(ip->protocol == IP_PROTOCOL_ICMP && endian32(ip->destination) == address){
// Echo reply
ICMP* icmp = (ICMP*)ip->body;
// Performance check
// perf();
icmp->type = 0;
icmp->checksum = 0;
icmp->checksum = endian16(checksum(icmp, packet->end - packet->start - ETHER_LEN - IP_LEN));
ip->destination = ip->source;
ip->source = endian32(address);
ip->ttl = endian8(64);
ip->checksum = 0;
ip->checksum = endian16(checksum(ip, ip->ihl * 4));
ether->dmac = ether->smac;
ether->smac = endian48(ni->mac);
ni_output(ni, packet);
packet = NULL;
}
/*
} else if(ip->protocol == IP_PROTOCOL_TCP) {
TCP* tcp = (TCP*)ip->body;
printf("source=%u, destination=%u, sequence=%u, acknowledgement=%u\n",
endian16(tcp->source), endian16(tcp->destination), endian32(tcp->sequence), endian32(tcp->acknowledgement));
printf("offset=%d, ns=%d, cwr=%d, ece=%d, urg=%d, ack=%d, psh=%d, rst=%d, syn=%d, fin=%d\n",
tcp->offset, tcp->ns, tcp->cwr, tcp->ece, tcp->urg, tcp->ack, tcp->psh, tcp->rst, tcp->syn, tcp->fin);
printf("window=%d, checksum=%x, urgent=%d\n", endian16(tcp->window), endian16(tcp->checksum), endian16(tcp->urgent));
*/
}
if(packet)
ni_free(packet);
}
void process(NetworkInterface* ni) {
Packet* packet = ni_input(ni);
if(!packet)
return;
if(arp_process(packet))
return;
Ether* ether = (Ether*)(packet->buffer + packet->start);
if(endian16(ether->type) == ETHER_TYPE_IPv4) {
IP* ip = (IP*)ether->payload;
if(ip->protocol == IP_PROTOCOL_UDP)
{
UDP* udp = (UDP*)ip->body;
if(endian16(udp->destination) == SETKEY_PORT_NUM)
{
int orig_len = endian16(ip->length);
Parameter* parameter = (Parameter*)udp->body;
// ARP Request
if(arp_request(ni, parameter->dst_ip) == true);
//printf("ARP Request for destination IP\n");
int result = parse(parameter);
memcpy(&(udp->body), &result, sizeof(result));
#ifdef _DEBUG_
for(int i = 0; i < 64 /* Packet Minimum Size */ - ETHER_LEN /* 14 */ - IP_LEN /* 20 */ - UDP_LEN /* 8 */ - sizeof(result); i++)
udp->body[i + 4] = i;
#endif
uint16_t t = udp->destination;
udp->destination = udp->source;
udp->source = t;
udp->checksum = 0;
udp->length = endian16(64 - ETHER_LEN - IP_LEN);
uint32_t t2 = ip->destination;
ip->destination = ip->source;
ip->source = t2;
ip->ttl = 0x40;
ip->length = endian16(64 - ETHER_LEN);
ip->checksum = 0;
ip->checksum = endian16(checksum(ip, ip->ihl * 4));
uint64_t t3 = ether->dmac;
ether->dmac = ether->smac;
ether->smac = t3;
packet->end += (endian16(ip->length) - orig_len);
ni_output(ni, packet);
packet = NULL;
}
}
if(ip->protocol == IP_PROTOCOL_ESP){
int orig_len = endian16(ip->length);
#ifdef _DEBUG_
printf("Before Decryption - Packet : \n");
for(int i = 1; i < 1 + endian16(ip->length); i++)
{
printf("%02x ", ether->payload[i - 1]); // Packet - IP Header
if( i % 16 == 0 )
printf("\n");
}
printf("\n");
#endif
if(decrypt(ip) >= 0)
{
packet->end += (endian16(ip->length) - orig_len);
// ether->dmac = endian48(arp_get_mac(ni, endian32(ip->destination)));
// ether->smac = endian48(ni->mac);
#ifdef _DEBUG_
printf("- Ethernet Header After Decryption-\n");
printf("Dst Mac : %012lx Src Mac : %012lx\n Ether Type : %x\n",
endian48(ether->dmac), endian48(ether->smac), ether->type);
#endif
if(ip->protocol == IP_PROTOCOL_ICMP){
// Echo reply
ICMP* icmp = (ICMP*)ip->body;
icmp->type = 0;
icmp->checksum = 0;
icmp->checksum = endian16(checksum(icmp, packet->end - packet->start - ETHER_LEN - IP_LEN));
uint32_t temp = ip->destination;
ip->destination = ip->source;
ip->source = temp;
ip->ttl = endian8(64);
ip->checksum = 0;
ip->checksum = endian16(checksum(ip, ip->ihl * 4));
// ether->dmac = ether->smac;
// ether->smac = endian48(ni->mac);
// ni_output(ni, packet);
// packet = NULL;
}
// ni_output(ni1, packet);
// packet = NULL;
}
orig_len = endian16(ip->length);
#ifdef _DEBUG_
printf("Before Encryption - Packet : \n");
for(int i = 1; i < 1 + endian16(ip->length); i++)
{
printf("%02x ", ether->payload[i - 1]); // Packet - IP Header
if( i % 16 == 0 )
printf("\n");
}
printf("\n");
#endif
if(encrypt(ip) >= 0)
{
packet->end += (endian16(ip->length) - orig_len);
ether->dmac = endian48(arp_get_mac(ni, endian32(ip->destination)));
ether->smac = endian48(ni->mac);
#ifdef _DEBUG_
printf("- Ethernet Header After Encryption-\n");
printf("Dst Mac : %012lx Src Mac : %012lx\n Ether Type : %x\n",
endian48(ether->dmac), endian48(ether->smac), ether->type);
#endif
ni_output(ni, packet);
packet = NULL;
}
}
}
if(packet)
ni_free(packet);
}