vlan.h

/*
 * Copyright (C) 2020, 2022-2024 Kernkonzept GmbH.
 * Author(s): Jan Klötzke <jan.kloetzke@kernkonzept.com>
 *
 * License: see LICENSE.spdx (in this directory or the directories above)
 */
#pragma once
 
#include <l4/cxx/minmax>
#include <l4/l4virtio/server/virtio>
#include <l4/sys/types.h>
#include <string.h>
 
#include "virtio_net.h"
#include "virtio_net_buffer.h"
 
namespace {
 
const l4_uint16_t VLAN_ID_NATIVE = 0xffffU; 
const l4_uint16_t VLAN_ID_TRUNK = 0xfffeU;  
 
inline bool vlan_valid_id(l4_uint16_t id)
{
  return id > 0U && id < 0xfffU;
}
 
}

class Virtio_vlan_mangle
{
  l4_uint16_t _tci;
  l4_uint8_t _mac_remaining;
  l4_int8_t _tag_remaining;
 
  constexpr Virtio_vlan_mangle(l4_uint16_t tci, l4_int8_t tag_remaining)
  : _tci{tci}, _mac_remaining{12}, _tag_remaining{tag_remaining}
  {}
 
public:
  Virtio_vlan_mangle()
  : _tci{0}, _mac_remaining{0}, _tag_remaining{0}
  {}

  static constexpr Virtio_vlan_mangle add(l4_uint16_t tci)
  {
    return Virtio_vlan_mangle(tci, 4);
  }

  static constexpr Virtio_vlan_mangle remove()
  {
    return Virtio_vlan_mangle(0xffffU, -4);
  }

  l4_uint32_t copy_pkt(Buffer &dst, Buffer &src)
  {
    l4_uint32_t ret;
 
    if (L4_LIKELY(_tci == 0))
      {
        // pass through (no tag or keep tag)
        ret = src.copy_to(&dst);
      }
    else if (_mac_remaining)
      {
        // copy initial MAC addresses
        ret = src.copy_to(&dst, _mac_remaining);
        _mac_remaining -= ret;
      }
    else if (_tag_remaining > 0)
      {
        // add VLAN tag
        l4_uint8_t tag[4] = {
          0x81, 0x00,
          static_cast<l4_uint8_t>(_tci >> 8),
          static_cast<l4_uint8_t>(_tci & 0xffU)
        };
 
        ret = cxx::min(static_cast<l4_uint32_t>(_tag_remaining), dst.left);
        memcpy(dst.pos, &tag[4 - _tag_remaining], ret);
        dst.skip(ret);
        _tag_remaining -= (int)ret;
      }
    else if (_tag_remaining < 0)
      {
        // remove VLAN tag
        _tag_remaining += static_cast<int>(src.skip(-_tag_remaining));
        ret = 0;
      }
    else
      ret = src.copy_to(&dst);
 
    return ret;
  }

  void rewrite_hdr(Virtio_net::Hdr *hdr)
  {
    if (L4_UNLIKELY(_tci != 0 && hdr->flags.need_csum()))
      {
        if (_tci == 0xffffU)
          hdr->csum_start -= 4U;
        else
          hdr->csum_start += 4U;
      }
  }
};