static Iface_DEFUN incomingFromUpperDistributorIf(struct Message* msg,
struct Iface* upperDistributorIf)
{
struct TUNAdapter_pvt* ud =
Identity_containerOf(upperDistributorIf, struct TUNAdapter_pvt, pub.upperDistributorIf);
Assert_true(msg->length >= RouteHeader_SIZE + DataHeader_SIZE);
struct RouteHeader* hdr = (struct RouteHeader*) msg->bytes;
struct DataHeader* dh = (struct DataHeader*) &hdr[1];
enum ContentType type = DataHeader_getContentType(dh);
Assert_true(type <= ContentType_IP6_RAW);
// Shift ip address into destination slot.
Bits_memmove(hdr->ip6 + DataHeader_SIZE - 16, hdr->ip6, 16);
// put my address as destination.
Bits_memcpy(&hdr->ip6[DataHeader_SIZE], ud->myIp6, 16);
Message_shift(msg, Headers_IP6Header_SIZE - DataHeader_SIZE - RouteHeader_SIZE, NULL);
struct Headers_IP6Header* ip6 = (struct Headers_IP6Header*) msg->bytes;
Bits_memset(ip6, 0, Headers_IP6Header_SIZE - 32);
Headers_setIpVersion(ip6);
ip6->payloadLength_be = Endian_bigEndianToHost16(msg->length - Headers_IP6Header_SIZE);
ip6->nextHeader = type;
ip6->hopLimit = 42;
TUNMessageType_push(msg, Ethernet_TYPE_IP6, NULL);
return sendToTunIf(msg, ud);
}
static void alignMessage(struct Message* msg, uint32_t alignmentBytes)
{
if (isAligned(msg->bytes, alignmentBytes)) { return; }
uint8_t* bytes = msg->bytes;
int length = msg->length;
do {
Message_push8(msg, 0, NULL);
} while (!isAligned(msg->bytes, alignmentBytes));
Bits_memmove(msg->bytes, bytes, length);
msg->length = length;
}
static uint8_t sendMessage(struct Message* message, struct Interface* ethIf)
{
struct ETHInterface* context = Identity_cast((struct ETHInterface*) ethIf);
struct sockaddr_ll addr;
Bits_memcpyConst(&addr, &context->addrBase, sizeof(struct sockaddr_ll));
Message_pop(message, addr.sll_addr, 8);
/* Cut down on the noise
uint8_t buff[sizeof(addr) * 2 + 1] = {0};
Hex_encode(buff, sizeof(buff), (uint8_t*)&addr, sizeof(addr));
Log_debug(context->logger, "Sending ethernet frame to [%s]", buff);
*/
// Check if we will have to pad the message and pad if necessary.
int pad = 0;
for (int length = message->length; length+2 < MIN_PACKET_SIZE; length += 8) {
pad++;
}
if (pad > 0) {
int length = message->length;
Message_shift(message, pad*8);
Bits_memset(message->bytes, 0, pad*8);
Bits_memmove(message->bytes, &message->bytes[pad*8], length);
}
Assert_true(pad < 8);
uint16_t padAndId_be = Endian_hostToBigEndian16((context->id << 3) | pad);
Message_push(message, &padAndId_be, 2);
if (sendto(context->socket,
message->bytes,
message->length,
0,
(struct sockaddr*) &addr,
sizeof(struct sockaddr_ll)) < 0)
{
switch (errno) {
case EMSGSIZE:
return Error_OVERSIZE_MESSAGE;
case ENOBUFS:
case EAGAIN:
return Error_LINK_LIMIT_EXCEEDED;
default:;
Log_info(context->logger, "Got error sending to socket [%s]", strerror(errno));
}
}
return 0;
}
static Iface_DEFUN incomingFromTunIf(struct Message* msg, struct Iface* tunIf)
{
struct TUNAdapter_pvt* ud = Identity_containerOf(tunIf, struct TUNAdapter_pvt, pub.tunIf);
uint16_t ethertype = TUNMessageType_pop(msg, NULL);
int version = Headers_getIpVersion(msg->bytes);
if ((ethertype == Ethernet_TYPE_IP4 && version != 4)
|| (ethertype == Ethernet_TYPE_IP6 && version != 6))
{
Log_debug(ud->log, "DROP packet because ip version [%d] "
"doesn't match ethertype [%u].", version, Endian_bigEndianToHost16(ethertype));
return NULL;
}
if (ethertype == Ethernet_TYPE_IP4) {
return Iface_next(&ud->pub.ipTunnelIf, msg);
}
if (ethertype != Ethernet_TYPE_IP6) {
Log_debug(ud->log, "DROP packet unknown ethertype [%u]",
Endian_bigEndianToHost16(ethertype));
return NULL;
}
if (msg->length < Headers_IP6Header_SIZE) {
Log_debug(ud->log, "DROP runt");
return NULL;
}
struct Headers_IP6Header* header = (struct Headers_IP6Header*) msg->bytes;
if (!AddressCalc_validAddress(header->destinationAddr)) {
return Iface_next(&ud->pub.ipTunnelIf, msg);
}
if (Bits_memcmp(header->sourceAddr, ud->myIp6, 16)) {
if (Defined(Log_DEBUG)) {
uint8_t expectedSource[40];
AddrTools_printIp(expectedSource, ud->myIp6);
uint8_t packetSource[40];
AddrTools_printIp(packetSource, header->sourceAddr);
Log_debug(ud->log,
"DROP packet from [%s] because all messages must have source address [%s]",
packetSource, expectedSource);
}
return NULL;
}
if (!Bits_memcmp(header->destinationAddr, ud->myIp6, 16)) {
// I'm Gonna Sit Right Down and Write Myself a Letter
TUNMessageType_push(msg, ethertype, NULL);
return Iface_next(tunIf, msg);
}
if (!Bits_memcmp(header->destinationAddr, "\xfc\0\0\0\0\0\0\0\0\0\0\0\0\0\0\x01", 16)) {
return Iface_next(&ud->pub.magicIf, msg);
}
// first move the dest addr to the right place.
Bits_memmove(header->destinationAddr - DataHeader_SIZE, header->destinationAddr, 16);
Message_shift(msg, DataHeader_SIZE + RouteHeader_SIZE - Headers_IP6Header_SIZE, NULL);
struct RouteHeader* rh = (struct RouteHeader*) msg->bytes;
struct DataHeader* dh = (struct DataHeader*) &rh[1];
Bits_memset(dh, 0, DataHeader_SIZE);
DataHeader_setContentType(dh, header->nextHeader);
DataHeader_setVersion(dh, DataHeader_CURRENT_VERSION);
// Other than the ipv6 addr at the end, everything is zeros right down the line.
Bits_memset(rh, 0, RouteHeader_SIZE - 16);
return Iface_next(&ud->pub.upperDistributorIf, msg);
}
