#include <stdlib.h>
#include "lib/tunnel_encaps.h"

static
int decode_encap(const void *p, size_t sub_tlv_len, struct tunnel_encap *encap, struct pool *pool)
{
  if (pool && !encap->encap) {
    encap->encap = mb_alloc(pool, sub_tlv_len);
    encap->encap_len = sub_tlv_len;
  }
  if (encap->encap_len < sub_tlv_len) {
    return -1;
  }
  memcpy(encap->encap, p, encap->encap_len);
  encap->encap_len = sub_tlv_len;
  encap->flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP;
  return 0;
}

static
int decode_color(const void *p, size_t sub_tlv_len, struct tunnel_encap *encap)
{
  if (sub_tlv_len != 8) {
    log(L_TRACE "WG: color len error %d", sub_tlv_len);
    return -1;
  }

  if (get_u16(p) != 0x030b) {
    log(L_TRACE "WG: color error %04x", get_u16(p));
    return -1;
  }

  encap->color = get_u32(p+4);
  encap->flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR;
  return 0;
}

static
int decode_udp_dest_port(const void *p, size_t sub_tlv_len, struct tunnel_encap *encap)
{
  if (sub_tlv_len != 2) {
    log(L_TRACE "WG: udp dest port len error %d", sub_tlv_len);
    return -1;
  }

  encap->udp_dest_port = get_u16(p);
  encap->flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT;
  return 0;
}

static
int decode_tunnel_ep(const void *p, size_t sub_tlv_len, struct tunnel_encap *encap)
{
  if (sub_tlv_len < 6) {
    log(L_TRACE "WG: tunnel ep len error");
    return -1;
  }

  encap->ep.asn = get_u32(p);
  u16 af = get_u16(p + 4);
  switch (af) {
  case NET_IP4:
    if (sub_tlv_len != 10) {
      log(L_TRACE "WG: IPv4 len error %d", sub_tlv_len);
      return -1;
    }
    encap->ep.ip = ipa_from_ip4(get_ip4(p + 6));
    encap->flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP;
    return 0;
  case NET_IP6:
    if (sub_tlv_len != 22) {
      log(L_TRACE "WG: IPv6 len error %d", sub_tlv_len);
      return -1;
    }
    encap->ep.ip = ipa_from_ip6(get_ip6(p + 6));
    encap->flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP;
    return 0;
  default:
    log(L_TRACE "WG: Address family error %d", af);
    return -1;
  }
}

static
int decode_sub_tlv(const u8 *p, size_t len, struct tunnel_encap *encap, struct pool *pool)
{
  if (len < 3) {
    log(L_TRACE "WG: sub_tlv len error %d", len);
    return -1;
  }

  const u8 *first = p;
  const u8 *last = p + len;
  int type = get_u8(p++);
  u16 sub_tlv_len = 0;

  log(L_TRACE "WG: sub tlv type %d", type);
  if (type >= 0 && type <= 127) {
    sub_tlv_len = get_u8(p);
    p++;
  } else if (type >= 128 && type <= 255) {
    sub_tlv_len = get_u16(p);
    p += 2;
  } else {
    log(L_TRACE "WG: sub_tlv type error %d", type);
    return -1;
  }

  log(L_TRACE "WG: sub tlv len %d", sub_tlv_len);
  if (p + sub_tlv_len > last) {
    log(L_TRACE "WG: sub_tlv value len error %d", sub_tlv_len);
    return -1;
  }

  int res = 0;

  switch (type) {
  case BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP:
    res = decode_encap(p, sub_tlv_len, encap, pool);
    break;
  case BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP:
    res = decode_tunnel_ep(p, sub_tlv_len, encap);
    break;
  case BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR:
    res = decode_color(p, sub_tlv_len, encap);
    break;
  case BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT:
    res = decode_udp_dest_port(p, sub_tlv_len, encap);
    break;
  default:
    /* Skip unsupported sub-TLV. */
    res = 0;
    break;
  }

  if (res < 0)
    return res;

  return p - first + sub_tlv_len;
}

int decode_tunnel_encap(const eattr *e, struct tunnel_encap *encap, struct pool *pool)
{
  const u8 *p = e->u.ptr->data;
  int len = e->u.ptr->length;

  if (len < 4) {
    log(L_TRACE "WG: tunnel_encap len error %d", len);
    return -1;
  }

  encap->type = get_u16(p);

  log(L_DEBUG "WG: tunnel type %d", encap->type);

  u16 value_length = get_u16(p + 2);

  log(L_TRACE "WG: tunnel encap value len %d", value_length);

  if (len < value_length + 4) {
    log(L_TRACE "WG: tunnel encap len error %d", value_length);
    return -1;
  }

  for (const u8 *cur = p + 4; cur < p + 4 + value_length;) {
    int res = decode_sub_tlv(cur, value_length, encap, pool);

    if (res < 0) {
      log(L_TRACE "WG: decode error %d", res);
      return res;
    }

    cur += res;
  }

  return 0;
}

struct {
  int type;
  const char *name;
} tunnel_types[] = {
  {0, "Reserved"},
  {1, "L2TPv3 over IP"},
  {2, "GRE"},
  {3, "Transmit tunnel endpoint"},
  {4, "IPsec in Tunnel-mode"},
  {5, "IP in IP tunnel with IPsec Transport Mode"},
  {6, "MPLS-in-IP tunnel with IPsec Transport Mode"},
  {7, "IP in IP"},
  {8, "VXLAN"},
  {9, "NVGRE"},
  {10, "MPLS"},
  {11, "MPLS in GRE"},
  {12, "VXLAN GPE"},
  {13, "MPLS in UDP"},
  {14, "IPv6 Tunnel"},
  {15, "SR TE Policy Type"},
  {16, "Bare"},
  {17, "SR Tunnel"},
};

const uint num_tunnel_types = sizeof(tunnel_types)/sizeof(tunnel_types[0]);

static const char *
lookup_name(int type)
{
  for (uint i = 0; i < num_tunnel_types; i++) {
    if (tunnel_types[i].type == type)
      return tunnel_types[i].name;
  }

  return "Unassigned";
}

int format_tunnel_encap(const eattr *a, byte *buf, uint size)
{
  byte *pos = buf;
  int l;
  struct tunnel_encap encap;
  memset(&encap, 0, sizeof(encap));
  encap.ep.ip = IPA_NONE;
  encap.encap_len = a->u.ptr->length;
  encap.encap = alloca(encap.encap_len); /* Guaranteed to be enough */

  if (decode_tunnel_encap(a, &encap, NULL) < 0) {
    l = bsnprintf(pos, size, "(invalid)");
    ADVANCE(pos, size, l);
    return pos - buf;
  }

  const char *name = lookup_name(encap.type);

  l = bsnprintf(pos, size, "type: %s(%d)", name, encap.type);
  if (l < 0)
    return pos - buf;
  ADVANCE(pos, size, l);

  if (encap.flags & FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP) {
    char tmp[20] = "";
    if (encap.ep.asn) {
      bsnprintf(tmp, sizeof(tmp), "[AS%d]:", encap.ep.asn);
    }
    l = bsnprintf(pos, size, " ep: %s%I", tmp, encap.ep.ip);
    if (l < 0)
      return pos - buf;
    ADVANCE(pos, size, l);
  }

  if (encap.flags & FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT) {
    l = bsnprintf(pos, size, " udp: %d", encap.udp_dest_port);
    if (l < 0)
      return pos - buf;
    ADVANCE(pos, size, l);
  }

  if (encap.flags & FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR) {
    l = bsnprintf(pos, size, " color: %d", encap.color);
    if (l < 0)
      return pos - buf;
    ADVANCE(pos, size, l);
  }

  if (encap.flags & FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP) {
    byte *data = encap.encap;

    l = bsnprintf(pos, size, " encap: ");
    if (l < 0)
      return pos - buf;
    ADVANCE(pos, size, l);

    for (uint i = 0; i < encap.encap_len; i++) {
      if (size < 4) {
	return pos - buf;
      }

      l = bsprintf(pos, "%02x ", data[i]);
      ADVANCE(pos, size, l);
    }
  }

  return pos - buf;
}