/* SPDX-License-Identifier: GPL-3.0-only */
#include "htp_packet_bridge.h"

#include <string.h>

#define HTP_ETHERTYPE_IPV4 0x0800U
#define HTP_ETHERTYPE_ARP 0x0806U
#define HTP_ARP_PACKET_SIZE 28U
#define HTP_ARP_REQUEST 1U
#define HTP_ARP_REPLY 2U

static uint16_t read_u16(const uint8_t *value) {
    return (uint16_t)(((uint16_t)value[0] << 8U) | value[1]);
}

static void write_u16(uint8_t *value, uint16_t number) {
    value[0] = (uint8_t)(number >> 8U);
    value[1] = (uint8_t)(number & 0xffU);
}

static uint32_t read_u32(const uint8_t *value) {
    return ((uint32_t)value[0] << 24U) | ((uint32_t)value[1] << 16U) |
           ((uint32_t)value[2] << 8U) | (uint32_t)value[3];
}

static void write_u32(uint8_t *value, uint32_t number) {
    value[0] = (uint8_t)(number >> 24U);
    value[1] = (uint8_t)(number >> 16U);
    value[2] = (uint8_t)(number >> 8U);
    value[3] = (uint8_t)number;
}

static int valid_ipv4(const uint8_t *packet, size_t length) {
    if (!packet || length < 20U || (packet[0] >> 4U) != 4U) return 0;
    const size_t header_length = (size_t)(packet[0] & 0x0fU) * 4U;
    const size_t total_length = read_u16(packet + 2U);
    return header_length >= 20U && header_length <= length && total_length >= header_length && total_length <= length;
}

static void learn_peer(htp_packet_bridge *bridge, uint32_t ipv4_be, const uint8_t mac[HTP_MAC_SIZE]) {
    if (!bridge || !ipv4_be || !mac || (mac[0] & 1U)) return;
    bridge->sequence += 1U;
    htp_bridge_peer *target = NULL;
    for (size_t index = 0; index < HTP_BRIDGE_MAX_PEERS; index += 1U) {
        htp_bridge_peer *peer = &bridge->peers[index];
        if (peer->used && peer->ipv4_be == ipv4_be) {
            target = peer;
            break;
        }
        if (!peer->used && !target) target = peer;
    }
    if (!target) {
        target = &bridge->peers[0];
        for (size_t index = 1; index < HTP_BRIDGE_MAX_PEERS; index += 1U) {
            if (bridge->peers[index].sequence < target->sequence) target = &bridge->peers[index];
        }
    }
    target->used = 1;
    target->ipv4_be = ipv4_be;
    target->sequence = bridge->sequence;
    memcpy(target->mac, mac, HTP_MAC_SIZE);
    memcpy(bridge->default_peer_mac, mac, HTP_MAC_SIZE);
    bridge->has_default_peer = 1;
}

static const uint8_t *peer_mac(htp_packet_bridge *bridge, uint32_t ipv4_be) {
    for (size_t index = 0; index < HTP_BRIDGE_MAX_PEERS; index += 1U) {
        if (bridge->peers[index].used && bridge->peers[index].ipv4_be == ipv4_be) return bridge->peers[index].mac;
    }
    return bridge->has_default_peer ? bridge->default_peer_mac : NULL;
}

void htp_packet_bridge_init(
    htp_packet_bridge *bridge,
    uint32_t local_ipv4_be,
    const uint8_t local_mac[HTP_MAC_SIZE]) {
    if (!bridge) return;
    memset(bridge, 0, sizeof(*bridge));
    bridge->local_ipv4_be = local_ipv4_be;
    if (local_mac) memcpy(bridge->local_mac, local_mac, HTP_MAC_SIZE);
}

int htp_packet_bridge_ip_to_ethernet(
    htp_packet_bridge *bridge,
    const uint8_t *packet,
    size_t packet_length,
    uint8_t *frame,
    size_t frame_capacity) {
    if (!bridge || !frame || !valid_ipv4(packet, packet_length)) {
        return -1;
    }
    const size_t ipv4_length = read_u16(packet + 2U);
    if (frame_capacity < ipv4_length + HTP_ETHERNET_HEADER_SIZE) return -1;
    const uint8_t broadcast[HTP_MAC_SIZE] = {0xffU, 0xffU, 0xffU, 0xffU, 0xffU, 0xffU};
    const uint8_t *destination = peer_mac(bridge, read_u32(packet + 16U));
    memcpy(frame, destination ? destination : broadcast, HTP_MAC_SIZE);
    memcpy(frame + HTP_MAC_SIZE, bridge->local_mac, HTP_MAC_SIZE);
    write_u16(frame + 12U, HTP_ETHERTYPE_IPV4);
    memcpy(frame + HTP_ETHERNET_HEADER_SIZE, packet, ipv4_length);
    return (int)(ipv4_length + HTP_ETHERNET_HEADER_SIZE);
}

int htp_packet_bridge_build_arp_request(
    const htp_packet_bridge *bridge,
    uint32_t target_ipv4_be,
    uint8_t *frame,
    size_t frame_capacity) {
    if (!bridge || !target_ipv4_be || !frame ||
        frame_capacity < HTP_ETHERNET_HEADER_SIZE + HTP_ARP_PACKET_SIZE) {
        return -1;
    }

    memset(frame, 0, HTP_ETHERNET_HEADER_SIZE + HTP_ARP_PACKET_SIZE);
    memset(frame, 0xff, HTP_MAC_SIZE);
    memcpy(frame + HTP_MAC_SIZE, bridge->local_mac, HTP_MAC_SIZE);
    write_u16(frame + 12U, HTP_ETHERTYPE_ARP);

    uint8_t *request = frame + HTP_ETHERNET_HEADER_SIZE;
    write_u16(request, 1U);
    write_u16(request + 2U, HTP_ETHERTYPE_IPV4);
    request[4] = HTP_MAC_SIZE;
    request[5] = 4U;
    write_u16(request + 6U, HTP_ARP_REQUEST);
    memcpy(request + 8U, bridge->local_mac, HTP_MAC_SIZE);
    write_u32(request + 14U, bridge->local_ipv4_be);
    write_u32(request + 24U, target_ipv4_be);
    return (int)(HTP_ETHERNET_HEADER_SIZE + HTP_ARP_PACKET_SIZE);
}

int htp_packet_bridge_has_peer(
    const htp_packet_bridge *bridge,
    uint32_t ipv4_be) {
    if (!bridge || !ipv4_be) return 0;
    for (size_t index = 0; index < HTP_BRIDGE_MAX_PEERS; index += 1U) {
        if (bridge->peers[index].used && bridge->peers[index].ipv4_be == ipv4_be) return 1;
    }
    return 0;
}

static htp_bridge_result handle_arp(
    htp_packet_bridge *bridge,
    const uint8_t *frame,
    size_t frame_length,
    uint8_t *reply,
    size_t reply_capacity,
    size_t *reply_length) {
    if (frame_length < HTP_ETHERNET_HEADER_SIZE + HTP_ARP_PACKET_SIZE) return HTP_BRIDGE_ERROR;
    const uint8_t *arp = frame + HTP_ETHERNET_HEADER_SIZE;
    if (read_u16(arp) != 1U || read_u16(arp + 2U) != HTP_ETHERTYPE_IPV4 || arp[4] != 6U || arp[5] != 4U) {
        return HTP_BRIDGE_IGNORED;
    }
    const uint16_t operation = read_u16(arp + 6U);
    const uint8_t *sender_mac = arp + 8U;
    const uint32_t sender_ip = read_u32(arp + 14U);
    const uint32_t target_ip = read_u32(arp + 24U);
    learn_peer(bridge, sender_ip, sender_mac);
    if (operation != HTP_ARP_REQUEST || target_ip != bridge->local_ipv4_be) return HTP_BRIDGE_IGNORED;
    if (!reply || reply_capacity < HTP_ETHERNET_HEADER_SIZE + HTP_ARP_PACKET_SIZE) return HTP_BRIDGE_ERROR;

    memcpy(reply, sender_mac, HTP_MAC_SIZE);
    memcpy(reply + HTP_MAC_SIZE, bridge->local_mac, HTP_MAC_SIZE);
    write_u16(reply + 12U, HTP_ETHERTYPE_ARP);
    uint8_t *response = reply + HTP_ETHERNET_HEADER_SIZE;
    write_u16(response, 1U);
    write_u16(response + 2U, HTP_ETHERTYPE_IPV4);
    response[4] = 6U;
    response[5] = 4U;
    write_u16(response + 6U, HTP_ARP_REPLY);
    memcpy(response + 8U, bridge->local_mac, HTP_MAC_SIZE);
    write_u32(response + 14U, bridge->local_ipv4_be);
    memcpy(response + 18U, sender_mac, HTP_MAC_SIZE);
    write_u32(response + 24U, sender_ip);
    if (reply_length) *reply_length = HTP_ETHERNET_HEADER_SIZE + HTP_ARP_PACKET_SIZE;
    return HTP_BRIDGE_CONTROL_REPLY;
}

htp_bridge_result htp_packet_bridge_ethernet_to_ip(
    htp_packet_bridge *bridge,
    const uint8_t *frame,
    size_t frame_length,
    uint8_t *packet,
    size_t packet_capacity,
    size_t *packet_length,
    uint8_t *control_reply,
    size_t control_reply_capacity,
    size_t *control_reply_length) {
    if (packet_length) *packet_length = 0U;
    if (control_reply_length) *control_reply_length = 0U;
    if (!bridge || !frame || frame_length < HTP_ETHERNET_HEADER_SIZE) return HTP_BRIDGE_ERROR;
    const uint16_t ether_type = read_u16(frame + 12U);
    if (ether_type == HTP_ETHERTYPE_ARP) {
        return handle_arp(bridge, frame, frame_length, control_reply, control_reply_capacity, control_reply_length);
    }
    if (ether_type != HTP_ETHERTYPE_IPV4) return HTP_BRIDGE_IGNORED;
    const uint8_t *payload = frame + HTP_ETHERNET_HEADER_SIZE;
    const size_t payload_length = frame_length - HTP_ETHERNET_HEADER_SIZE;
    if (!valid_ipv4(payload, payload_length) || !packet) return HTP_BRIDGE_ERROR;
    const size_t ipv4_length = read_u16(payload + 2U);
    if (packet_capacity < ipv4_length) return HTP_BRIDGE_ERROR;
    learn_peer(bridge, read_u32(payload + 12U), frame + HTP_MAC_SIZE);
    memcpy(packet, payload, ipv4_length);
    if (packet_length) *packet_length = ipv4_length;
    return HTP_BRIDGE_IP_PACKET;
}
