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); }