<commit_before>package com.cisco.trex.stateless;

import static org.pcap4j.util.ByteArrays.BYTE_SIZE_IN_BYTES;

import com.cisco.trex.stateless.exception.ServiceModeRequiredException;
import com.cisco.trex.stateless.model.Ipv6Node;
import com.cisco.trex.stateless.model.PortStatus;
import com.cisco.trex.stateless.model.StreamMode;
import com.cisco.trex.stateless.model.StreamModeRate;
import com.cisco.trex.stateless.model.StreamRxStats;
import com.cisco.trex.stateless.model.StreamVM;
import com.cisco.trex.stateless.model.TRexClientResult;
import com.google.common.net.InetAddresses;
import java.net.Inet6Address;
import java.net.UnknownHostException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Optional;
import java.util.function.Predicate;
import java.util.stream.Collectors;
import java.util.stream.Stream;
import org.apache.commons.lang3.StringUtils;
import org.pcap4j.packet.EthernetPacket;
import org.pcap4j.packet.IcmpV6CommonPacket;
import org.pcap4j.packet.IcmpV6CommonPacket.IpV6NeighborDiscoveryOption;
import org.pcap4j.packet.IcmpV6EchoReplyPacket;
import org.pcap4j.packet.IcmpV6EchoRequestPacket;
import org.pcap4j.packet.IcmpV6NeighborAdvertisementPacket;
import org.pcap4j.packet.IcmpV6NeighborAdvertisementPacket.IcmpV6NeighborAdvertisementHeader;
import org.pcap4j.packet.IcmpV6NeighborSolicitationPacket;
import org.pcap4j.packet.IpV6NeighborDiscoverySourceLinkLayerAddressOption;
import org.pcap4j.packet.IpV6NeighborDiscoveryTargetLinkLayerAddressOption;
import org.pcap4j.packet.IpV6Packet;
import org.pcap4j.packet.IpV6SimpleFlowLabel;
import org.pcap4j.packet.IpV6SimpleTrafficClass;
import org.pcap4j.packet.Packet;
import org.pcap4j.packet.namednumber.EtherType;
import org.pcap4j.packet.namednumber.IcmpV6Code;
import org.pcap4j.packet.namednumber.IcmpV6Type;
import org.pcap4j.packet.namednumber.IpNumber;
import org.pcap4j.packet.namednumber.IpVersion;
import org.pcap4j.util.ByteArrays;
import org.pcap4j.util.MacAddress;

public class IPv6NeighborDiscoveryService {

  private TRexClient tRexClient;

  public IPv6NeighborDiscoveryService(TRexClient tRexClient) {
    this.tRexClient = tRexClient;
  }

  public Map<String, Ipv6Node> scan(int portIdx, int timeDuration, String dstIP, String srcIP)
      throws ServiceModeRequiredException {
    String broadcastIP = "fc00:e968:6179::de52:7100";

    long endTimeSec = System.currentTimeMillis() + timeDuration / 2 * 1000;
    TRexClientResult<PortStatus> portStatusResult = tRexClient.getPortStatus(portIdx);
    PortStatus portStatus = portStatusResult.get();

    if (!portStatus.getService()) {
      throw new ServiceModeRequiredException();
    }

    String srcMac = portStatus.getAttr().getLayerConiguration().getL2Configuration().getSrc();

    Packet pingPkt =
        buildICMPV6EchoReq(
            srcIP,
            srcMac,
            multicastMacFromIPv6(broadcastIP).toString(),
            expandIPv6Address(broadcastIP));
    tRexClient.startStreamsIntermediate(portIdx, Collections.singletonList(buildStream(pingPkt)));

    List<com.cisco.trex.stateless.model.Stream> nsNaStreams = new ArrayList<>();
    Predicate<EthernetPacket> ipV6NSPktFilter =
        etherPkt ->
            etherPkt.contains(IcmpV6NeighborSolicitationPacket.class)
                || etherPkt.contains(IcmpV6NeighborAdvertisementPacket.class);

    while (endTimeSec > System.currentTimeMillis()) {
      tRexClient
          .getRxQueue(portIdx, ipV6NSPktFilter)
          .forEach(
              pkt -> {
                IpV6Packet ipV6Packet = pkt.get(IpV6Packet.class);
                String nodeIp = ipV6Packet.getHeader().getSrcAddr().toString().substring(1);
                String nodeMac = getLinkLayerAddress(ipV6Packet);

                nsNaStreams.add(buildStream(buildICMPV6NSPkt(srcMac, nodeMac, nodeIp, srcIP)));
                nsNaStreams.add(buildStream(buildICMPV6NAPkt(srcMac, nodeMac, nodeIp, srcIP)));
              });
    }

    tRexClient.startStreamsIntermediate(portIdx, nsNaStreams);

    List<EthernetPacket> icmpNAReplies = new ArrayList<>();

    Predicate<EthernetPacket> ipV6NAPktFilter =
        etherPkt -> etherPkt.contains(IcmpV6NeighborAdvertisementPacket.class);
    endTimeSec = System.currentTimeMillis() + timeDuration / 2 * 1000;
    while (endTimeSec > System.currentTimeMillis()) {
      icmpNAReplies.addAll(tRexClient.getRxQueue(portIdx, ipV6NAPktFilter));
    }
    tRexClient.removeRxQueue(portIdx);
    return icmpNAReplies
        .stream()
        .map(this::toIpv6Node)
        .distinct()
        .filter(
            ipv6Node -> {
              if (dstIP != null) {
                return InetAddresses.forString(dstIP)
                    .equals(InetAddresses.forString(ipv6Node.getIp()));
              }
              return true;
            })
        .collect(Collectors.toMap(Ipv6Node::getIp, node -> node));
  }

  private Ipv6Node toIpv6Node(EthernetPacket ethernetPacket) {
    IcmpV6NeighborAdvertisementHeader icmpV6NaHdr =
        ethernetPacket.get(IcmpV6NeighborAdvertisementPacket.class).getHeader();
    boolean isRouter = icmpV6NaHdr.getRouterFlag();
    String nodeIp = icmpV6NaHdr.getTargetAddress().toString().substring(1);
    String nodeMac = ethernetPacket.getHeader().getSrcAddr().toString();
    return new Ipv6Node(nodeMac, nodeIp, isRouter);
  }

  public EthernetPacket sendIcmpV6Echo(
      int portIdx, String dstIp, int icmpId, int icmpSeq, int timeOut) {
    PortStatus portStatus = tRexClient.getPortStatus(portIdx).get();
    String srcMac = portStatus.getAttr().getLayerConiguration().getL2Configuration().getSrc();
    return sendIcmpV6Echo(portIdx, srcMac, dstIp, icmpId, icmpSeq, timeOut);
  }

  public EthernetPacket sendIcmpV6Echo(
      int portIdx, String srcMac, String dstIp, int icmpId, int icmpSeq, int timeOut) {
    Map<String, EthernetPacket> stringEthernetPacketMap =
        sendNSandIcmpV6Req(portIdx, timeOut, srcMac, dstIp);

    Optional<Map.Entry<String, EthernetPacket>> icmpMulticastResponse =
        stringEthernetPacketMap.entrySet().stream().findFirst();

    EthernetPacket icmpUnicastReply = null;

    if (icmpMulticastResponse.isPresent()) {
      EthernetPacket etherPkt = icmpMulticastResponse.get().getValue();
      String nodeMac = etherPkt.getHeader().getSrcAddr().toString();
      Packet pingPkt = buildICMPV6EchoReq(null, srcMac, nodeMac, dstIp, icmpId, icmpSeq);
      tRexClient.startStreamsIntermediate(portIdx, Arrays.asList(buildStream(pingPkt)));
      long endTimeSec = System.currentTimeMillis() + timeOut * 1000 / 2;

      while (endTimeSec > System.currentTimeMillis()) {
        List<EthernetPacket> rxQueue =
            tRexClient.getRxQueue(portIdx, pkt -> pkt.contains(IcmpV6EchoReplyPacket.class));
        if (rxQueue.size() > 0) {
          icmpUnicastReply = rxQueue.get(0);
        }
      }
    }

    tRexClient.removeRxQueue(portIdx);
    if (tRexClient.getPortStatus(portIdx).get().getState().equals("TX")) {
      tRexClient.stopTraffic(portIdx);
    }
    tRexClient.removeAllStreams(portIdx);
    return icmpUnicastReply;
  }

  public EthernetPacket sendNeighborSolicitation(int portIdx, int timeout, String dstIp) {
    PortStatus portStatus = tRexClient.getPortStatus(portIdx).get();
    String srcMac = portStatus.getAttr().getLayerConiguration().getL2Configuration().getSrc();
    return sendNeighborSolicitation(portIdx, timeout, srcMac, dstIp);
  }

  public EthernetPacket sendNeighborSolicitation(
      int portIdx, int timeout, String srcMac, String dstIp) {
    long endTs = System.currentTimeMillis() + timeout * 1000;

    Packet icmpv6NSPkt =
        buildICMPV6NSPkt(srcMac, multicastMacFromIPv6(dstIp).toString(), dstIp, null);

    tRexClient.startStreamsIntermediate(portIdx, Arrays.asList(buildStream(icmpv6NSPkt)));

    Predicate<EthernetPacket> ipV6NAPktFilter =
        etherPkt -> {
          if (!etherPkt.contains(IcmpV6NeighborAdvertisementPacket.class)) {
            return false;
          }
          IcmpV6NeighborAdvertisementHeader icmpV6NaHdr =
              etherPkt.get(IcmpV6NeighborAdvertisementPacket.class).getHeader();

          String nodeIp = icmpV6NaHdr.getTargetAddress().toString().substring(1);

          IpV6Packet.IpV6Header ipV6Header = etherPkt.get(IpV6Packet.class).getHeader();
          String dstAddr = ipV6Header.getDstAddr().toString().substring(1);

          try {
            Inet6Address dstIPv6Addr = (Inet6Address) Inet6Address.getByName(dstAddr);
            Inet6Address srcIPv6Addr =
                (Inet6Address) Inet6Address.getByName(generateIPv6AddrFromMAC(srcMac));

            Inet6Address nodeIpv6 = (Inet6Address) Inet6Address.getByName(nodeIp);
            Inet6Address targetIpv6inNS = (Inet6Address) Inet6Address.getByName(dstIp);
            return icmpV6NaHdr.getSolicitedFlag()
                && nodeIpv6.equals(targetIpv6inNS)
                && dstIPv6Addr.equals(srcIPv6Addr);
          } catch (UnknownHostException ignored) {
            // Do nothing
          }
          return false;
        };

    EthernetPacket na = null;
    while (endTs > System.currentTimeMillis() && na == null) {
      List<EthernetPacket> pkts = tRexClient.getRxQueue(portIdx, ipV6NAPktFilter);
      if (!pkts.isEmpty()) {
        na = pkts.get(0);
      }
    }
    tRexClient.removeRxQueue(portIdx);
    if (tRexClient.getPortStatus(portIdx).get().getState().equals("TX")) {
      tRexClient.stopTraffic(portIdx);
    }
    tRexClient.removeAllStreams(portIdx);
    return na;
  }

  private Map<String, EthernetPacket> sendNSandIcmpV6Req(
      int portIdx, int timeDuration, String srcMac, String dstIp) {
    long endTs = System.currentTimeMillis() + timeDuration * 1000;
    TRexClientResult<PortStatus> portStatusResult = tRexClient.getPortStatus(portIdx);

    Packet pingPkt = buildICMPV6EchoReq(null, srcMac, null, dstIp);
    Packet icmpv6NSPkt =
        buildICMPV6NSPkt(srcMac, multicastMacFromIPv6(dstIp).toString(), dstIp, null);

    List<com.cisco.trex.stateless.model.Stream> stlStreams =
        Stream.of(buildStream(pingPkt), buildStream(icmpv6NSPkt)).collect(Collectors.toList());

    tRexClient.startStreamsIntermediate(portIdx, stlStreams);

    Map<String, EthernetPacket> naIncomingRequests = new HashMap<>();

    Predicate<EthernetPacket> ipV6NAPktFilter =
        etherPkt -> {
          if (!etherPkt.contains(IcmpV6NeighborAdvertisementPacket.class)) {
            return false;
          }
          IcmpV6NeighborAdvertisementHeader icmpV6NaHdr =
              etherPkt.get(IcmpV6NeighborAdvertisementPacket.class).getHeader();

          String nodeIp = icmpV6NaHdr.getTargetAddress().toString().substring(1);

          IpV6Packet.IpV6Header ipV6Header = etherPkt.get(IpV6Packet.class).getHeader();
          String dstAddr = ipV6Header.getDstAddr().toString().substring(1);

          try {
            Inet6Address dstIPv6Addr = (Inet6Address) Inet6Address.getByName(dstAddr);
            Inet6Address srcIPv6Addr =
                (Inet6Address) Inet6Address.getByName(generateIPv6AddrFromMAC(srcMac));
            return !naIncomingRequests.containsKey(nodeIp) && dstIPv6Addr.equals(srcIPv6Addr);
          } catch (UnknownHostException ignored) {
            // Do nothing
          }
          return false;
        };

    while (endTs > System.currentTimeMillis()) {
      tRexClient
          .getRxQueue(portIdx, ipV6NAPktFilter)
          .forEach(
              pkt -> {
                IcmpV6NeighborAdvertisementHeader icmpV6NaHdr =
                    pkt.get(IcmpV6NeighborAdvertisementPacket.class).getHeader();
                String nodeIp = icmpV6NaHdr.getTargetAddress().toString().substring(1);
                naIncomingRequests.put(nodeIp, pkt);
              });
    }
    tRexClient.removeRxQueue(portIdx);
    return naIncomingRequests;
  }

  private com.cisco.trex.stateless.model.Stream buildStream(Packet pkt) {
    int streamId = (int) (Math.random() * 1000);
    return new com.cisco.trex.stateless.model.Stream(
        streamId,
        true,
        3,
        0.0,
        new StreamMode(
            2,
            2,
            5,
            1.0,
            new StreamModeRate(StreamModeRate.Type.percentage, 100.0),
            StreamMode.Type.single_burst),
        -1,
        pkt,
        new StreamRxStats(false, false, true, streamId),
        new StreamVM("", Collections.emptyList()),
        true,
        false,
        null);
  }

  private Packet buildICMPV6NSPkt(String srcMac, String dstMac, String dstIp, String srcIp) {
    EthernetPacket.Builder ethBuilder = new EthernetPacket.Builder();
    try {

      IpV6NeighborDiscoverySourceLinkLayerAddressOption sourceLLAddr =
          new IpV6NeighborDiscoverySourceLinkLayerAddressOption.Builder()
              .correctLengthAtBuild(true)
              .linkLayerAddress(hexStringToByteArray(srcMac.replace(":", "")))
              .build();

      IcmpV6NeighborSolicitationPacket.Builder ipv6NSBuilder =
          new IcmpV6NeighborSolicitationPacket.Builder();
      ipv6NSBuilder
          .options(Arrays.asList(sourceLLAddr))
          .targetAddress((Inet6Address) Inet6Address.getByName(dstIp));

      final String specifiedSrcIP = srcIp != null ? srcIp : generateIPv6AddrFromMAC(srcMac);

      IcmpV6CommonPacket.Builder icmpCommonPktBuilder = new IcmpV6CommonPacket.Builder();
      icmpCommonPktBuilder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr((Inet6Address) Inet6Address.getByName(dstIp))
          .type(IcmpV6Type.NEIGHBOR_SOLICITATION)
          .code(IcmpV6Code.NO_CODE)
          .correctChecksumAtBuild(true)
          .payloadBuilder(ipv6NSBuilder);

      IpV6Packet.Builder ipV6Builder = new IpV6Packet.Builder();
      ipV6Builder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr((Inet6Address) Inet6Address.getByName(dstIp))
          .version(IpVersion.IPV6)
          .hopLimit((byte) -1)
          .trafficClass(IpV6SimpleTrafficClass.newInstance((byte) 0))
          .flowLabel(IpV6SimpleFlowLabel.newInstance(0))
          .nextHeader(IpNumber.ICMPV6)
          .payloadBuilder(icmpCommonPktBuilder)
          .correctLengthAtBuild(true);

      ethBuilder
          .type(EtherType.IPV6)
          .srcAddr(MacAddress.getByName(srcMac))
          .dstAddr(MacAddress.getByName(dstMac))
          .payloadBuilder(ipV6Builder)
          .paddingAtBuild(true);
    } catch (UnknownHostException ignored) {
      // Do nothing
    }
    return ethBuilder.build();
  }

  /**
   * IPv6 Neighbor Discovery Source Link Layer Address header
   *
   * <p>0 1 2 3 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
   * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Type | Length | Link-Layer
   * Address ... +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   */
  private static String getLinkLayerAddress(IpV6Packet pkt) {
    final int TYPE_OFFSET = 0;
    final int TYPE_SIZE = BYTE_SIZE_IN_BYTES;
    final int LENGTH_OFFSET = TYPE_OFFSET + TYPE_SIZE;
    final int LENGTH_SIZE = BYTE_SIZE_IN_BYTES;
    final int LINK_LAYER_ADDRESS_OFFSET = LENGTH_OFFSET + LENGTH_SIZE;
    final int LINK_LAYER_ADDRESS_LENGTH = 6; // MAC address

    IcmpV6NeighborSolicitationPacket nsPkt = pkt.get(IcmpV6NeighborSolicitationPacket.class);

    IpV6NeighborDiscoveryOption linkLayerAddressOption = nsPkt.getHeader().getOptions().get(0);

    byte[] linkLayerAddress =
        ByteArrays.getSubArray(
            linkLayerAddressOption.getRawData(),
            LINK_LAYER_ADDRESS_OFFSET,
            LINK_LAYER_ADDRESS_LENGTH);

    return ByteArrays.toHexString(linkLayerAddress, ":");
  }

  private Packet buildICMPV6NAPkt(String srcMac, String dstMac, String dstIp, String srcIP) {
    final String specifiedSrcIP = srcIP != null ? srcIP : generateIPv6AddrFromMAC(srcMac);

    EthernetPacket.Builder ethBuilder = new EthernetPacket.Builder();
    try {

      IpV6NeighborDiscoveryTargetLinkLayerAddressOption tLLAddr =
          new IpV6NeighborDiscoveryTargetLinkLayerAddressOption.Builder()
              .correctLengthAtBuild(true)
              .linkLayerAddress(hexStringToByteArray(dstMac.replace(":", "")))
              .build();

      IcmpV6NeighborAdvertisementPacket.Builder ipv6NABuilder =
          new IcmpV6NeighborAdvertisementPacket.Builder();
      ipv6NABuilder
          .routerFlag(false)
          .options(Arrays.asList(tLLAddr))
          .solicitedFlag(true)
          .overrideFlag(true)
          .targetAddress((Inet6Address) Inet6Address.getByName(specifiedSrcIP));

      IcmpV6CommonPacket.Builder icmpCommonPktBuilder = new IcmpV6CommonPacket.Builder();
      icmpCommonPktBuilder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr((Inet6Address) Inet6Address.getByName(dstIp))
          .type(IcmpV6Type.NEIGHBOR_ADVERTISEMENT)
          .code(IcmpV6Code.NO_CODE)
          .correctChecksumAtBuild(true)
          .payloadBuilder(ipv6NABuilder);

      IpV6Packet.Builder ipV6Builder = new IpV6Packet.Builder();
      ipV6Builder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr((Inet6Address) Inet6Address.getByName(dstIp))
          .version(IpVersion.IPV6)
          .trafficClass(IpV6SimpleTrafficClass.newInstance((byte) 0))
          .flowLabel(IpV6SimpleFlowLabel.newInstance(0))
          .nextHeader(IpNumber.ICMPV6)
          .hopLimit((byte) 1)
          .payloadBuilder(icmpCommonPktBuilder)
          .correctLengthAtBuild(true);

      ethBuilder
          .type(EtherType.IPV6)
          .srcAddr(MacAddress.getByName(srcMac))
          .dstAddr(MacAddress.getByName("33:33:00:00:00:01"))
          .payloadBuilder(ipV6Builder)
          .paddingAtBuild(true);
    } catch (UnknownHostException ignored) {
      // Do nothing
    }

    return ethBuilder.build();
  }

  public static byte[] hexStringToByteArray(String s) {
    int len = s.length();
    byte[] data = new byte[len / 2];
    for (int i = 0; i < len; i += 2) {
      data[i / 2] =
          (byte) ((Character.digit(s.charAt(i), 16) << 4) + Character.digit(s.charAt(i + 1), 16));
    }
    return data;
  }

  public static EthernetPacket buildICMPV6EchoReq(
      String srcIp,
      String srcMacString,
      String dstMacString,
      String dstIp,
      int icmpId,
      int icmpSeq) {
    /*
     *
     * mld_pkt = (Ether(src = self.src_mac, dst = self.dst_mld_mac) / IPv6(src = self.src_ip, dst =
     * self.dst_mld_ip, hlim = 1) / IPv6ExtHdrHopByHop(options = [RouterAlert(), PadN()]) /
     * ICMPv6MLReportV2() / MLDv2Addr(type = 4, len = 0, multicast_addr = 'fc00:db20:35b:7399::5')) ping_pkt =
     * (Ether(src = self.src_mac, dst = dst_mac) / IPv6(src = self.src_ip, dst = self.dst_ip, hlim =
     * 1) / ICMPv6EchoRequest()) return [self.vlan.embed(mld_pkt), self.vlan.embed(ping_pkt)]
     */

    final String specifiedSrcIP = srcIp != null ? srcIp : generateIPv6AddrFromMAC(srcMacString);

    IcmpV6EchoRequestPacket.Builder icmpV6ERBuilder = new IcmpV6EchoRequestPacket.Builder();
    icmpV6ERBuilder.identifier((short) icmpId).sequenceNumber((short) icmpSeq);

    IcmpV6CommonPacket.Builder icmpCommonPktBuilder = new IcmpV6CommonPacket.Builder();
    try {
      icmpCommonPktBuilder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr(
              (Inet6Address) Inet6Address.getByName(dstIp != null ? dstIp : "fd00:c2b6:b24b:be67:2827:688d:e6a1:6a3b"))
          .type(IcmpV6Type.ECHO_REQUEST)
          .code(IcmpV6Code.NO_CODE)
          .correctChecksumAtBuild(true)
          .payloadBuilder(icmpV6ERBuilder);
      IpV6Packet.Builder ipV6Builder = new IpV6Packet.Builder();
      ipV6Builder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr(
              (Inet6Address) Inet6Address.getByName(dstIp != null ? dstIp : "fd00:c2b6:b24b:be67:2827:688d:e6a1:6a3b"))
          .version(IpVersion.IPV6)
          .trafficClass(IpV6SimpleTrafficClass.newInstance((byte) 0))
          .flowLabel(IpV6SimpleFlowLabel.newInstance(0))
          .nextHeader(IpNumber.ICMPV6)
          .hopLimit((byte) 64)
          .payloadBuilder(icmpCommonPktBuilder)
          .correctLengthAtBuild(true);

      MacAddress dstMac;
      if (dstMacString != null) {
        dstMac = MacAddress.getByName(dstMacString);
      } else {
        dstMac =
            dstIp == null ? MacAddress.getByName("33:33:00:00:00:01") : multicastMacFromIPv6(dstIp);
      }

      EthernetPacket.Builder ethBuilder = new EthernetPacket.Builder();
      ethBuilder
          .type(EtherType.IPV6)
          .srcAddr(MacAddress.getByName(srcMacString))
          .dstAddr(dstMac)
          .payloadBuilder(ipV6Builder)
          .paddingAtBuild(true);

      return ethBuilder.build();
    } catch (UnknownHostException ignore) {
      // Do nothing
    }
    return null;
  }

  public static EthernetPacket buildICMPV6EchoReq(
      String srcIp, String srcMacString, String dstMacString, String dstIp) {
    return buildICMPV6EchoReq(srcIp, srcMacString, dstMacString, dstIp, 0, 0);
  }

  private static MacAddress multicastMacFromIPv6(String ipV6) {
    String expandedIPv6 = expandIPv6Address(ipV6);
    List<Long> ipv6Octets =
        Arrays.stream(expandedIPv6.split(":"))
            .map(octet -> Long.parseLong(octet, 16))
            .collect(Collectors.toList());

    int lastIdx = ipv6Octets.size() - 1;
    int preLastIdx = ipv6Octets.size() - 2;
    String macAddressStr =
        String.format(
            "33:33:%02x:%02x:%02x:%02x",
            divMod(ipv6Octets.get(preLastIdx), 256)[0],
            divMod(ipv6Octets.get(preLastIdx), 256)[1],
            divMod(ipv6Octets.get(lastIdx), 256)[0],
            divMod(ipv6Octets.get(lastIdx), 256)[1]);
    return MacAddress.getByName(macAddressStr);
  }

  private static long[] divMod(long a, long b) {
    long[] result = new long[2];

    result[1] = a % b;
    result[0] = (a - result[1]) / b;

    return result;
  }

  private static String expandIPv6Address(String shortAddress) {
    String[] addressArray = shortAddress.split(":");
    if (shortAddress.startsWith(":")) {
      addressArray[0] = "0";
    } else if (shortAddress.endsWith(":")) {
      addressArray[addressArray.length - 1] = "0";
    }

    for (int i = 0; i < addressArray.length; i++) {
      if (addressArray[i] == null || addressArray[i].isEmpty()) {
        StringBuilder sb = new StringBuilder();

        int leftSize = i + 1;
        String[] left = new String[i + 1];
        System.arraycopy(addressArray, 0, left, 0, leftSize);

        sb.append(Arrays.stream(left).collect(Collectors.joining(":")));

        String[] expanded =
            Stream.generate(() -> "0").limit(9 - addressArray.length).toArray(String[]::new);
        sb.append(Arrays.stream(expanded).collect(Collectors.joining(":")));
        sb.append(":");

        int rightSize = addressArray.length - i - 1;
        String[] right = new String[rightSize];
        System.arraycopy(addressArray, i + 1, right, 0, rightSize);
        sb.append(Arrays.stream(right).collect(Collectors.joining(":")));

        return sb.toString();
      }
    }
    return Arrays.stream(addressArray).collect(Collectors.joining(":"));
  }

  static String generateIPv6AddrFromMAC(String mac) {
    String prefix = "fe80";
    List<Integer> macOctets =
        Arrays.stream(mac.split(":"))
            .map(octet -> Integer.parseInt(octet, 16))
            .collect(Collectors.toList());
    // insert ff fe in the middle
    macOctets.add(3, 0xfe);
    macOctets.add(3, 0xff);

    // invert second bind
    macOctets.set(0, macOctets.get(0) ^ 0b1 << 1);
    List<String> strOctets = new ArrayList<>();
    for (int i = 0; i < macOctets.size(); i += 2) {
      strOctets.add(
          String.format(
              "%s%s",
              StringUtils.leftPad(Integer.toHexString(macOctets.get(i)), 2, "0"),
              StringUtils.leftPad(Integer.toHexString(macOctets.get(i + 1)), 2, "0")));
    }
    return String.format("%s::%s", prefix, String.join(":", strOctets));
  }
}
<commit_msg>add service mode check<commit_after>package com.cisco.trex.stateless;

import static org.pcap4j.util.ByteArrays.BYTE_SIZE_IN_BYTES;

import com.cisco.trex.stateless.exception.ServiceModeRequiredException;
import com.cisco.trex.stateless.model.Ipv6Node;
import com.cisco.trex.stateless.model.PortStatus;
import com.cisco.trex.stateless.model.StreamMode;
import com.cisco.trex.stateless.model.StreamModeRate;
import com.cisco.trex.stateless.model.StreamRxStats;
import com.cisco.trex.stateless.model.StreamVM;
import com.cisco.trex.stateless.model.TRexClientResult;
import com.google.common.net.InetAddresses;
import java.net.Inet6Address;
import java.net.UnknownHostException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Optional;
import java.util.function.Predicate;
import java.util.stream.Collectors;
import java.util.stream.Stream;
import org.apache.commons.lang3.StringUtils;
import org.pcap4j.packet.EthernetPacket;
import org.pcap4j.packet.IcmpV6CommonPacket;
import org.pcap4j.packet.IcmpV6CommonPacket.IpV6NeighborDiscoveryOption;
import org.pcap4j.packet.IcmpV6EchoReplyPacket;
import org.pcap4j.packet.IcmpV6EchoRequestPacket;
import org.pcap4j.packet.IcmpV6NeighborAdvertisementPacket;
import org.pcap4j.packet.IcmpV6NeighborAdvertisementPacket.IcmpV6NeighborAdvertisementHeader;
import org.pcap4j.packet.IcmpV6NeighborSolicitationPacket;
import org.pcap4j.packet.IpV6NeighborDiscoverySourceLinkLayerAddressOption;
import org.pcap4j.packet.IpV6NeighborDiscoveryTargetLinkLayerAddressOption;
import org.pcap4j.packet.IpV6Packet;
import org.pcap4j.packet.IpV6SimpleFlowLabel;
import org.pcap4j.packet.IpV6SimpleTrafficClass;
import org.pcap4j.packet.Packet;
import org.pcap4j.packet.namednumber.EtherType;
import org.pcap4j.packet.namednumber.IcmpV6Code;
import org.pcap4j.packet.namednumber.IcmpV6Type;
import org.pcap4j.packet.namednumber.IpNumber;
import org.pcap4j.packet.namednumber.IpVersion;
import org.pcap4j.util.ByteArrays;
import org.pcap4j.util.MacAddress;

public class IPv6NeighborDiscoveryService {

  private TRexClient tRexClient;

  public IPv6NeighborDiscoveryService(TRexClient tRexClient) {
    this.tRexClient = tRexClient;
  }

  public Map<String, Ipv6Node> scan(int portIdx, int timeDuration, String dstIP, String srcIP)
      throws ServiceModeRequiredException {
    String broadcastIP = "fc00:e968:6179::de52:7100";

    long endTimeSec = System.currentTimeMillis() + timeDuration / 2 * 1000;
    TRexClientResult<PortStatus> portStatusResult = tRexClient.getPortStatus(portIdx);
    PortStatus portStatus = portStatusResult.get();

    if (!portStatus.getService()) {
      throw new ServiceModeRequiredException();
    }

    String srcMac = portStatus.getAttr().getLayerConiguration().getL2Configuration().getSrc();

    Packet pingPkt =
        buildICMPV6EchoReq(
            srcIP,
            srcMac,
            multicastMacFromIPv6(broadcastIP).toString(),
            expandIPv6Address(broadcastIP));
    tRexClient.startStreamsIntermediate(portIdx, Collections.singletonList(buildStream(pingPkt)));

    List<com.cisco.trex.stateless.model.Stream> nsNaStreams = new ArrayList<>();
    Predicate<EthernetPacket> ipV6NSPktFilter =
        etherPkt ->
            etherPkt.contains(IcmpV6NeighborSolicitationPacket.class)
                || etherPkt.contains(IcmpV6NeighborAdvertisementPacket.class);

    while (endTimeSec > System.currentTimeMillis()) {
      tRexClient
          .getRxQueue(portIdx, ipV6NSPktFilter)
          .forEach(
              pkt -> {
                IpV6Packet ipV6Packet = pkt.get(IpV6Packet.class);
                String nodeIp = ipV6Packet.getHeader().getSrcAddr().toString().substring(1);
                String nodeMac = getLinkLayerAddress(ipV6Packet);

                nsNaStreams.add(buildStream(buildICMPV6NSPkt(srcMac, nodeMac, nodeIp, srcIP)));
                nsNaStreams.add(buildStream(buildICMPV6NAPkt(srcMac, nodeMac, nodeIp, srcIP)));
              });
    }

    tRexClient.startStreamsIntermediate(portIdx, nsNaStreams);

    List<EthernetPacket> icmpNAReplies = new ArrayList<>();

    Predicate<EthernetPacket> ipV6NAPktFilter =
        etherPkt -> etherPkt.contains(IcmpV6NeighborAdvertisementPacket.class);
    endTimeSec = System.currentTimeMillis() + timeDuration / 2 * 1000;
    while (endTimeSec > System.currentTimeMillis()) {
      icmpNAReplies.addAll(tRexClient.getRxQueue(portIdx, ipV6NAPktFilter));
    }
    tRexClient.removeRxQueue(portIdx);
    return icmpNAReplies
        .stream()
        .map(this::toIpv6Node)
        .distinct()
        .filter(
            ipv6Node -> {
              if (dstIP != null) {
                return InetAddresses.forString(dstIP)
                    .equals(InetAddresses.forString(ipv6Node.getIp()));
              }
              return true;
            })
        .collect(Collectors.toMap(Ipv6Node::getIp, node -> node));
  }

  private Ipv6Node toIpv6Node(EthernetPacket ethernetPacket) {
    IcmpV6NeighborAdvertisementHeader icmpV6NaHdr =
        ethernetPacket.get(IcmpV6NeighborAdvertisementPacket.class).getHeader();
    boolean isRouter = icmpV6NaHdr.getRouterFlag();
    String nodeIp = icmpV6NaHdr.getTargetAddress().toString().substring(1);
    String nodeMac = ethernetPacket.getHeader().getSrcAddr().toString();
    return new Ipv6Node(nodeMac, nodeIp, isRouter);
  }

  public EthernetPacket sendIcmpV6Echo(
      int portIdx, String dstIp, int icmpId, int icmpSeq, int timeOut) {
    PortStatus portStatus = tRexClient.getPortStatus(portIdx).get();
    String srcMac = portStatus.getAttr().getLayerConiguration().getL2Configuration().getSrc();
    return sendIcmpV6Echo(portIdx, srcMac, dstIp, icmpId, icmpSeq, timeOut);
  }

  public EthernetPacket sendIcmpV6Echo(
      int portIdx, String srcMac, String dstIp, int icmpId, int icmpSeq, int timeOut) {
    Map<String, EthernetPacket> stringEthernetPacketMap =
        sendNSandIcmpV6Req(portIdx, timeOut, srcMac, dstIp);

    Optional<Map.Entry<String, EthernetPacket>> icmpMulticastResponse =
        stringEthernetPacketMap.entrySet().stream().findFirst();

    EthernetPacket icmpUnicastReply = null;

    if (icmpMulticastResponse.isPresent()) {
      EthernetPacket etherPkt = icmpMulticastResponse.get().getValue();
      String nodeMac = etherPkt.getHeader().getSrcAddr().toString();
      Packet pingPkt = buildICMPV6EchoReq(null, srcMac, nodeMac, dstIp, icmpId, icmpSeq);
      tRexClient.startStreamsIntermediate(portIdx, Arrays.asList(buildStream(pingPkt)));
      long endTimeSec = System.currentTimeMillis() + timeOut * 1000 / 2;

      while (endTimeSec > System.currentTimeMillis()) {
        List<EthernetPacket> rxQueue =
            tRexClient.getRxQueue(portIdx, pkt -> pkt.contains(IcmpV6EchoReplyPacket.class));
        if (rxQueue.size() > 0) {
          icmpUnicastReply = rxQueue.get(0);
        }
      }
    }

    tRexClient.removeRxQueue(portIdx);
    if (tRexClient.getPortStatus(portIdx).get().getState().equals("TX")) {
      tRexClient.stopTraffic(portIdx);
    }
    tRexClient.removeAllStreams(portIdx);
    return icmpUnicastReply;
  }

  public EthernetPacket sendNeighborSolicitation(int portIdx, int timeout, String dstIp)
      throws ServiceModeRequiredException {
    PortStatus portStatus = tRexClient.getPortStatus(portIdx).get();
    if (!portStatus.getService()) {
      throw new ServiceModeRequiredException();
    }
    String srcMac = portStatus.getAttr().getLayerConiguration().getL2Configuration().getSrc();
    return sendNeighborSolicitation(portIdx, timeout, srcMac, dstIp);
  }

  public EthernetPacket sendNeighborSolicitation(
      int portIdx, int timeout, String srcMac, String dstIp) {
    long endTs = System.currentTimeMillis() + timeout * 1000;

    Packet icmpv6NSPkt =
        buildICMPV6NSPkt(srcMac, multicastMacFromIPv6(dstIp).toString(), dstIp, null);

    tRexClient.startStreamsIntermediate(portIdx, Arrays.asList(buildStream(icmpv6NSPkt)));

    Predicate<EthernetPacket> ipV6NAPktFilter =
        etherPkt -> {
          if (!etherPkt.contains(IcmpV6NeighborAdvertisementPacket.class)) {
            return false;
          }
          IcmpV6NeighborAdvertisementHeader icmpV6NaHdr =
              etherPkt.get(IcmpV6NeighborAdvertisementPacket.class).getHeader();

          String nodeIp = icmpV6NaHdr.getTargetAddress().toString().substring(1);

          IpV6Packet.IpV6Header ipV6Header = etherPkt.get(IpV6Packet.class).getHeader();
          String dstAddr = ipV6Header.getDstAddr().toString().substring(1);

          try {
            Inet6Address dstIPv6Addr = (Inet6Address) Inet6Address.getByName(dstAddr);
            Inet6Address srcIPv6Addr =
                (Inet6Address) Inet6Address.getByName(generateIPv6AddrFromMAC(srcMac));

            Inet6Address nodeIpv6 = (Inet6Address) Inet6Address.getByName(nodeIp);
            Inet6Address targetIpv6inNS = (Inet6Address) Inet6Address.getByName(dstIp);
            return icmpV6NaHdr.getSolicitedFlag()
                && nodeIpv6.equals(targetIpv6inNS)
                && dstIPv6Addr.equals(srcIPv6Addr);
          } catch (UnknownHostException ignored) {
            // Do nothing
          }
          return false;
        };

    EthernetPacket na = null;
    while (endTs > System.currentTimeMillis() && na == null) {
      List<EthernetPacket> pkts = tRexClient.getRxQueue(portIdx, ipV6NAPktFilter);
      if (!pkts.isEmpty()) {
        na = pkts.get(0);
      }
    }
    tRexClient.removeRxQueue(portIdx);
    if (tRexClient.getPortStatus(portIdx).get().getState().equals("TX")) {
      tRexClient.stopTraffic(portIdx);
    }
    tRexClient.removeAllStreams(portIdx);
    return na;
  }

  private Map<String, EthernetPacket> sendNSandIcmpV6Req(
      int portIdx, int timeDuration, String srcMac, String dstIp) {
    long endTs = System.currentTimeMillis() + timeDuration * 1000;
    TRexClientResult<PortStatus> portStatusResult = tRexClient.getPortStatus(portIdx);

    Packet pingPkt = buildICMPV6EchoReq(null, srcMac, null, dstIp);
    Packet icmpv6NSPkt =
        buildICMPV6NSPkt(srcMac, multicastMacFromIPv6(dstIp).toString(), dstIp, null);

    List<com.cisco.trex.stateless.model.Stream> stlStreams =
        Stream.of(buildStream(pingPkt), buildStream(icmpv6NSPkt)).collect(Collectors.toList());

    tRexClient.startStreamsIntermediate(portIdx, stlStreams);

    Map<String, EthernetPacket> naIncomingRequests = new HashMap<>();

    Predicate<EthernetPacket> ipV6NAPktFilter =
        etherPkt -> {
          if (!etherPkt.contains(IcmpV6NeighborAdvertisementPacket.class)) {
            return false;
          }
          IcmpV6NeighborAdvertisementHeader icmpV6NaHdr =
              etherPkt.get(IcmpV6NeighborAdvertisementPacket.class).getHeader();

          String nodeIp = icmpV6NaHdr.getTargetAddress().toString().substring(1);

          IpV6Packet.IpV6Header ipV6Header = etherPkt.get(IpV6Packet.class).getHeader();
          String dstAddr = ipV6Header.getDstAddr().toString().substring(1);

          try {
            Inet6Address dstIPv6Addr = (Inet6Address) Inet6Address.getByName(dstAddr);
            Inet6Address srcIPv6Addr =
                (Inet6Address) Inet6Address.getByName(generateIPv6AddrFromMAC(srcMac));
            return !naIncomingRequests.containsKey(nodeIp) && dstIPv6Addr.equals(srcIPv6Addr);
          } catch (UnknownHostException ignored) {
            // Do nothing
          }
          return false;
        };

    while (endTs > System.currentTimeMillis()) {
      tRexClient
          .getRxQueue(portIdx, ipV6NAPktFilter)
          .forEach(
              pkt -> {
                IcmpV6NeighborAdvertisementHeader icmpV6NaHdr =
                    pkt.get(IcmpV6NeighborAdvertisementPacket.class).getHeader();
                String nodeIp = icmpV6NaHdr.getTargetAddress().toString().substring(1);
                naIncomingRequests.put(nodeIp, pkt);
              });
    }
    tRexClient.removeRxQueue(portIdx);
    return naIncomingRequests;
  }

  private com.cisco.trex.stateless.model.Stream buildStream(Packet pkt) {
    int streamId = (int) (Math.random() * 1000);
    return new com.cisco.trex.stateless.model.Stream(
        streamId,
        true,
        3,
        0.0,
        new StreamMode(
            2,
            2,
            5,
            1.0,
            new StreamModeRate(StreamModeRate.Type.percentage, 100.0),
            StreamMode.Type.single_burst),
        -1,
        pkt,
        new StreamRxStats(false, false, true, streamId),
        new StreamVM("", Collections.emptyList()),
        true,
        false,
        null);
  }

  private Packet buildICMPV6NSPkt(String srcMac, String dstMac, String dstIp, String srcIp) {
    EthernetPacket.Builder ethBuilder = new EthernetPacket.Builder();
    try {

      IpV6NeighborDiscoverySourceLinkLayerAddressOption sourceLLAddr =
          new IpV6NeighborDiscoverySourceLinkLayerAddressOption.Builder()
              .correctLengthAtBuild(true)
              .linkLayerAddress(hexStringToByteArray(srcMac.replace(":", "")))
              .build();

      IcmpV6NeighborSolicitationPacket.Builder ipv6NSBuilder =
          new IcmpV6NeighborSolicitationPacket.Builder();
      ipv6NSBuilder
          .options(Arrays.asList(sourceLLAddr))
          .targetAddress((Inet6Address) Inet6Address.getByName(dstIp));

      final String specifiedSrcIP = srcIp != null ? srcIp : generateIPv6AddrFromMAC(srcMac);

      IcmpV6CommonPacket.Builder icmpCommonPktBuilder = new IcmpV6CommonPacket.Builder();
      icmpCommonPktBuilder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr((Inet6Address) Inet6Address.getByName(dstIp))
          .type(IcmpV6Type.NEIGHBOR_SOLICITATION)
          .code(IcmpV6Code.NO_CODE)
          .correctChecksumAtBuild(true)
          .payloadBuilder(ipv6NSBuilder);

      IpV6Packet.Builder ipV6Builder = new IpV6Packet.Builder();
      ipV6Builder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr((Inet6Address) Inet6Address.getByName(dstIp))
          .version(IpVersion.IPV6)
          .hopLimit((byte) -1)
          .trafficClass(IpV6SimpleTrafficClass.newInstance((byte) 0))
          .flowLabel(IpV6SimpleFlowLabel.newInstance(0))
          .nextHeader(IpNumber.ICMPV6)
          .payloadBuilder(icmpCommonPktBuilder)
          .correctLengthAtBuild(true);

      ethBuilder
          .type(EtherType.IPV6)
          .srcAddr(MacAddress.getByName(srcMac))
          .dstAddr(MacAddress.getByName(dstMac))
          .payloadBuilder(ipV6Builder)
          .paddingAtBuild(true);
    } catch (UnknownHostException ignored) {
      // Do nothing
    }
    return ethBuilder.build();
  }

  /**
   * IPv6 Neighbor Discovery Source Link Layer Address header
   *
   * <p>0 1 2 3 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
   * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ | Type | Length | Link-Layer
   * Address ... +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   */
  private static String getLinkLayerAddress(IpV6Packet pkt) {
    final int TYPE_OFFSET = 0;
    final int TYPE_SIZE = BYTE_SIZE_IN_BYTES;
    final int LENGTH_OFFSET = TYPE_OFFSET + TYPE_SIZE;
    final int LENGTH_SIZE = BYTE_SIZE_IN_BYTES;
    final int LINK_LAYER_ADDRESS_OFFSET = LENGTH_OFFSET + LENGTH_SIZE;
    final int LINK_LAYER_ADDRESS_LENGTH = 6; // MAC address

    IcmpV6NeighborSolicitationPacket nsPkt = pkt.get(IcmpV6NeighborSolicitationPacket.class);

    IpV6NeighborDiscoveryOption linkLayerAddressOption = nsPkt.getHeader().getOptions().get(0);

    byte[] linkLayerAddress =
        ByteArrays.getSubArray(
            linkLayerAddressOption.getRawData(),
            LINK_LAYER_ADDRESS_OFFSET,
            LINK_LAYER_ADDRESS_LENGTH);

    return ByteArrays.toHexString(linkLayerAddress, ":");
  }

  private Packet buildICMPV6NAPkt(String srcMac, String dstMac, String dstIp, String srcIP) {
    final String specifiedSrcIP = srcIP != null ? srcIP : generateIPv6AddrFromMAC(srcMac);

    EthernetPacket.Builder ethBuilder = new EthernetPacket.Builder();
    try {

      IpV6NeighborDiscoveryTargetLinkLayerAddressOption tLLAddr =
          new IpV6NeighborDiscoveryTargetLinkLayerAddressOption.Builder()
              .correctLengthAtBuild(true)
              .linkLayerAddress(hexStringToByteArray(dstMac.replace(":", "")))
              .build();

      IcmpV6NeighborAdvertisementPacket.Builder ipv6NABuilder =
          new IcmpV6NeighborAdvertisementPacket.Builder();
      ipv6NABuilder
          .routerFlag(false)
          .options(Arrays.asList(tLLAddr))
          .solicitedFlag(true)
          .overrideFlag(true)
          .targetAddress((Inet6Address) Inet6Address.getByName(specifiedSrcIP));

      IcmpV6CommonPacket.Builder icmpCommonPktBuilder = new IcmpV6CommonPacket.Builder();
      icmpCommonPktBuilder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr((Inet6Address) Inet6Address.getByName(dstIp))
          .type(IcmpV6Type.NEIGHBOR_ADVERTISEMENT)
          .code(IcmpV6Code.NO_CODE)
          .correctChecksumAtBuild(true)
          .payloadBuilder(ipv6NABuilder);

      IpV6Packet.Builder ipV6Builder = new IpV6Packet.Builder();
      ipV6Builder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr((Inet6Address) Inet6Address.getByName(dstIp))
          .version(IpVersion.IPV6)
          .trafficClass(IpV6SimpleTrafficClass.newInstance((byte) 0))
          .flowLabel(IpV6SimpleFlowLabel.newInstance(0))
          .nextHeader(IpNumber.ICMPV6)
          .hopLimit((byte) 1)
          .payloadBuilder(icmpCommonPktBuilder)
          .correctLengthAtBuild(true);

      ethBuilder
          .type(EtherType.IPV6)
          .srcAddr(MacAddress.getByName(srcMac))
          .dstAddr(MacAddress.getByName("33:33:00:00:00:01"))
          .payloadBuilder(ipV6Builder)
          .paddingAtBuild(true);
    } catch (UnknownHostException ignored) {
      // Do nothing
    }

    return ethBuilder.build();
  }

  public static byte[] hexStringToByteArray(String s) {
    int len = s.length();
    byte[] data = new byte[len / 2];
    for (int i = 0; i < len; i += 2) {
      data[i / 2] =
          (byte) ((Character.digit(s.charAt(i), 16) << 4) + Character.digit(s.charAt(i + 1), 16));
    }
    return data;
  }

  public static EthernetPacket buildICMPV6EchoReq(
      String srcIp,
      String srcMacString,
      String dstMacString,
      String dstIp,
      int icmpId,
      int icmpSeq) {
    /*
     *
     * mld_pkt = (Ether(src = self.src_mac, dst = self.dst_mld_mac) / IPv6(src = self.src_ip, dst =
     * self.dst_mld_ip, hlim = 1) / IPv6ExtHdrHopByHop(options = [RouterAlert(), PadN()]) /
     * ICMPv6MLReportV2() / MLDv2Addr(type = 4, len = 0, multicast_addr = 'fc00:db20:35b:7399::5')) ping_pkt =
     * (Ether(src = self.src_mac, dst = dst_mac) / IPv6(src = self.src_ip, dst = self.dst_ip, hlim =
     * 1) / ICMPv6EchoRequest()) return [self.vlan.embed(mld_pkt), self.vlan.embed(ping_pkt)]
     */

    final String specifiedSrcIP = srcIp != null ? srcIp : generateIPv6AddrFromMAC(srcMacString);

    IcmpV6EchoRequestPacket.Builder icmpV6ERBuilder = new IcmpV6EchoRequestPacket.Builder();
    icmpV6ERBuilder.identifier((short) icmpId).sequenceNumber((short) icmpSeq);

    IcmpV6CommonPacket.Builder icmpCommonPktBuilder = new IcmpV6CommonPacket.Builder();
    try {
      icmpCommonPktBuilder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr(
              (Inet6Address) Inet6Address.getByName(dstIp != null ? dstIp : "fd00:c2b6:b24b:be67:2827:688d:e6a1:6a3b"))
          .type(IcmpV6Type.ECHO_REQUEST)
          .code(IcmpV6Code.NO_CODE)
          .correctChecksumAtBuild(true)
          .payloadBuilder(icmpV6ERBuilder);
      IpV6Packet.Builder ipV6Builder = new IpV6Packet.Builder();
      ipV6Builder
          .srcAddr((Inet6Address) Inet6Address.getByName(specifiedSrcIP))
          .dstAddr(
              (Inet6Address) Inet6Address.getByName(dstIp != null ? dstIp : "fd00:c2b6:b24b:be67:2827:688d:e6a1:6a3b"))
          .version(IpVersion.IPV6)
          .trafficClass(IpV6SimpleTrafficClass.newInstance((byte) 0))
          .flowLabel(IpV6SimpleFlowLabel.newInstance(0))
          .nextHeader(IpNumber.ICMPV6)
          .hopLimit((byte) 64)
          .payloadBuilder(icmpCommonPktBuilder)
          .correctLengthAtBuild(true);

      MacAddress dstMac;
      if (dstMacString != null) {
        dstMac = MacAddress.getByName(dstMacString);
      } else {
        dstMac =
            dstIp == null ? MacAddress.getByName("33:33:00:00:00:01") : multicastMacFromIPv6(dstIp);
      }

      EthernetPacket.Builder ethBuilder = new EthernetPacket.Builder();
      ethBuilder
          .type(EtherType.IPV6)
          .srcAddr(MacAddress.getByName(srcMacString))
          .dstAddr(dstMac)
          .payloadBuilder(ipV6Builder)
          .paddingAtBuild(true);

      return ethBuilder.build();
    } catch (UnknownHostException ignore) {
      // Do nothing
    }
    return null;
  }

  public static EthernetPacket buildICMPV6EchoReq(
      String srcIp, String srcMacString, String dstMacString, String dstIp) {
    return buildICMPV6EchoReq(srcIp, srcMacString, dstMacString, dstIp, 0, 0);
  }

  private static MacAddress multicastMacFromIPv6(String ipV6) {
    String expandedIPv6 = expandIPv6Address(ipV6);
    List<Long> ipv6Octets =
        Arrays.stream(expandedIPv6.split(":"))
            .map(octet -> Long.parseLong(octet, 16))
            .collect(Collectors.toList());

    int lastIdx = ipv6Octets.size() - 1;
    int preLastIdx = ipv6Octets.size() - 2;
    String macAddressStr =
        String.format(
            "33:33:%02x:%02x:%02x:%02x",
            divMod(ipv6Octets.get(preLastIdx), 256)[0],
            divMod(ipv6Octets.get(preLastIdx), 256)[1],
            divMod(ipv6Octets.get(lastIdx), 256)[0],
            divMod(ipv6Octets.get(lastIdx), 256)[1]);
    return MacAddress.getByName(macAddressStr);
  }

  private static long[] divMod(long a, long b) {
    long[] result = new long[2];

    result[1] = a % b;
    result[0] = (a - result[1]) / b;

    return result;
  }

  private static String expandIPv6Address(String shortAddress) {
    String[] addressArray = shortAddress.split(":");
    if (shortAddress.startsWith(":")) {
      addressArray[0] = "0";
    } else if (shortAddress.endsWith(":")) {
      addressArray[addressArray.length - 1] = "0";
    }

    for (int i = 0; i < addressArray.length; i++) {
      if (addressArray[i] == null || addressArray[i].isEmpty()) {
        StringBuilder sb = new StringBuilder();

        int leftSize = i + 1;
        String[] left = new String[i + 1];
        System.arraycopy(addressArray, 0, left, 0, leftSize);

        sb.append(Arrays.stream(left).collect(Collectors.joining(":")));

        String[] expanded =
            Stream.generate(() -> "0").limit(9 - addressArray.length).toArray(String[]::new);
        sb.append(Arrays.stream(expanded).collect(Collectors.joining(":")));
        sb.append(":");

        int rightSize = addressArray.length - i - 1;
        String[] right = new String[rightSize];
        System.arraycopy(addressArray, i + 1, right, 0, rightSize);
        sb.append(Arrays.stream(right).collect(Collectors.joining(":")));

        return sb.toString();
      }
    }
    return Arrays.stream(addressArray).collect(Collectors.joining(":"));
  }

  static String generateIPv6AddrFromMAC(String mac) {
    String prefix = "fe80";
    List<Integer> macOctets =
        Arrays.stream(mac.split(":"))
            .map(octet -> Integer.parseInt(octet, 16))
            .collect(Collectors.toList());
    // insert ff fe in the middle
    macOctets.add(3, 0xfe);
    macOctets.add(3, 0xff);

    // invert second bind
    macOctets.set(0, macOctets.get(0) ^ 0b1 << 1);
    List<String> strOctets = new ArrayList<>();
    for (int i = 0; i < macOctets.size(); i += 2) {
      strOctets.add(
          String.format(
              "%s%s",
              StringUtils.leftPad(Integer.toHexString(macOctets.get(i)), 2, "0"),
              StringUtils.leftPad(Integer.toHexString(macOctets.get(i + 1)), 2, "0")));
    }
    return String.format("%s::%s", prefix, String.join(":", strOctets));
  }
}

{'fraction_non_alphanumeric': 0.11346457176803162, 'fraction_numerical': 0.022574103283692155, 'mean_word_length': 3.142735913102512, 'pattern_counts': {'":': 28, '<': 67, '<?xml version=': 0, '>': 89, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4232036', 'n_tokens_mistral': 16540, 'n_tokens_neox': 15773, 'n_words': 2472}