diff --git a/.env b/.env
index b97d789..c87236f 100644
--- a/.env
+++ b/.env
@@ -1 +1 @@
-FEITCSI_VERSION=1.2.0
\ No newline at end of file
+FEITCSI_VERSION=2.0.0
\ No newline at end of file
diff --git a/include/Arguments.h b/include/Arguments.h
index 1e28396..fca4a2f 100644
--- a/include/Arguments.h
+++ b/include/Arguments.h
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2024 Miroslav Hutar.
+ * Copyright (C) 2024-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -25,8 +25,11 @@
 #include <argp.h>
 #include "main.h"
 
+#define ETH_ALEN 6
+
 struct Args
 {
+    bool strict;
     bool verbose;
     uint16_t frequency;
     bool gui = false;
@@ -48,6 +51,16 @@ struct Args
     bool measure;
     std::string mode;
     std::string ltf;
+    uint32_t modeDelay;
+    bool ftm = false;
+    bool ftmResponder = false;
+    bool ftmAsap = false;
+    uint8_t ftmBurstExp;
+    uint8_t ftmPerBurst;
+    uint16_t ftmBurstPeriod;
+    uint8_t ftmBurstDuration;
+    uint8_t mac[ETH_ALEN];
+    uint8_t ftmTargetMac[ETH_ALEN];
     std::string inputFile;
     std::map<enum processor, bool> processors;
 };
@@ -85,13 +98,22 @@ private:
         {"coding", 'c', "CODING", 0, "Coding scheme [LDPC|BCC]"},
         {"tx-power", 't', "TXPOWER", 0, "TX power of antenna in dBm [1-22]"},
         {"antenna", 'a', "ANTENNA", 0, "Transmitting antenna 1, 2 or 12 for both"},
-        {"mode", 'i', "MODE", 0, "Mode of program[measure|inject|measureinject]"},
+        {"mode", 'i', "MODE", 0, "Mode of program[measure|inject|measureinject|ftm|ftmres|injectftmres|measureftm]"},
         {"inject-delay", 'd', "INJECTDELAY", 0, "Delay between frame injections is us"},
         {"inject-repeat", 'j', "INJECTREPEAT", 0, "How many times inject frame"},
         {"verbose", 'v', 0, OPTION_ARG_OPTIONAL, "Produce verbose output"},
         {"plot", 'p', 0, OPTION_ARG_OPTIONAL, "Plot CSI data"},
         {"gui", 'x', 0, OPTION_ARG_OPTIONAL, "Run application in GUI"},
         {"udp-socket", 'u', 0, OPTION_ARG_OPTIONAL, "Run application and listen to UDP"},
+        {"ftm-asap", 'b', 0, OPTION_ARG_OPTIONAL, "FTM asap mode"},
+        {"ftm-burst-exp", 'q', "FTMBURSTEXP", 0, "FTM burst exponent"},
+        {"ftm-per-burst", 'e', "FTMPERBURST", 0, "FTM samples per burst"},
+        {"ftm-burst-period", 'h', "FTMBURSTPERIOD", 0, "FTM burst period"},
+        {"ftm-burst-duration", 'k', "FTMBURTSDURATION", 0, "FTM burst duration"},
+        {"ftm-mac", 'n', "FTMMAC", 0, "FTM target MAC address xx:xx:xx:xx:xx:xx"},
+        {"mode-delay", 'y', "SWAPTIME", 0, "Delay in ms between inject and ftm responder or measure and ftm initiator when modes are injectftmres|measureftm"},
+        {"strict", 'z', 0, OPTION_ARG_OPTIONAL, "Strict mode: filter out values that do not contain a specific MCS"},
+        {"mac", '#', "MAC", 0, "Default NICs MAC will be change to providing MAC xx:xx:xx:xx:xx:xx"},
         {0}};
 };
 
diff --git a/include/Csi.h b/include/Csi.h
index 49cf154..d2145cf 100644
--- a/include/Csi.h
+++ b/include/Csi.h
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023-2024 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -27,12 +27,14 @@
 
 #define CSI_HEADER_LENGTH 272
 
-struct RawHeaderData
+struct __attribute__((__packed__)) RawHeaderData
 {
     uint32_t csiDataSize;
     uint32_t space4;
     uint32_t ftmClock;
-    uint8_t space12[34];
+    //uint8_t space12[34];
+    uint64_t timestamp;
+    uint8_t space20[26];
     uint8_t numRx;
     uint8_t numTx;
     uint8_t space48[4];
diff --git a/include/MainController.h b/include/MainController.h
index 19485f4..1175538 100644
--- a/include/MainController.h
+++ b/include/MainController.h
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023-2024 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -79,6 +79,10 @@ private:
 
     pthread_t injectPacketThread = 0;
 
+    pthread_t ftmThread = 0;
+
+    pthread_t ftmResponderThread = 0;
+
     PacketInjector packetInjector;
 
     WiFIController wifiController;
@@ -91,10 +95,17 @@ private:
 
     static void *injectPackets(void *arg);
 
+    static void *ftm(void *arg);
+
+    static void *ftmResponder(void *arg);
+
     bool measuring = false;
 
     bool injecting = false;
-    
+
+    bool ftmEnabled = false;
+
+    bool ftmResponderEnabled = false;
 };
 
 #endif
\ No newline at end of file
diff --git a/include/PacketInjector.h b/include/PacketInjector.h
index 912029f..3721de2 100644
--- a/include/PacketInjector.h
+++ b/include/PacketInjector.h
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023-2024 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -36,7 +36,6 @@ public:
     void injectVHT();
     void injectHE();
 
-
 private:
     void send(uint32_t rateNFlags);
     pcap_t *ppcap = nullptr;
diff --git a/include/WiFIController.h b/include/WiFIController.h
index b0e8b8c..7332798 100644
--- a/include/WiFIController.h
+++ b/include/WiFIController.h
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -64,28 +64,36 @@ class WiFIController : public Netlink
 public:
     void killNetworkProcesses();
     void getInterfaces();
+    void getInterfaceInfo(const char* ifname);
     void setTxPower();
-    void addMonitorDevice(const char *name, const nl80211_iftype type);
+    void addDevice(const char *name, const nl80211_iftype type);
     int setInterfaceUpDown(const char *ifName, bool up);
     void setFreq(uint16_t freq, const char *bw);
     void removeInterface(const char *name, int ifIndex = 0);
+    void createMonitorInteface();
+    void createApInteface();
     void getPhys();
+    int setApMode();
     static ChanMode getChanMode(const char *width);
     static uint16_t chanModeToWidth(ChanMode &chanMode);
     std::vector<InterfaceInfo> interfaces;
     std::vector<int64_t> phys;
+    InterfaceInfo currentInterfaceInfo = {};
 
 private:
     std::string macN2a(const unsigned char *arg);
     int freqToCHannel(int freq);
     int phyLookup(char *name);
+    std::string currentDeviceName;
 
     static int getCf1(const ChanMode *chanmode, unsigned long freq);
     static int processGetInterfacesHandler(nl_msg *msg, void *arg);
+    static int processGetInterfaceInfoHandler(nl_msg *msg, void *arg);
     static int processGetPhysHandler(nl_msg *msg, void *arg);
     static int processSetFreq(nl80211_state *state, nl_msg *msg, void *arg);
     static int setTxPowerHandler(nl80211_state *state, nl_msg *msg, void *arg);
-    static int processAddMonitorDeviceHandler(nl80211_state *state, nl_msg *msg, void *arg);
+    static int processaddDeviceHandler(nl80211_state *state, nl_msg *msg, void *arg);
+    static int setApModeHandler(nl80211_state *state, nl_msg *msg, void *arg);
 };
 
 #endif
\ No newline at end of file
diff --git a/include/WiFiCsiController.h b/include/WiFiCsiController.h
index c6aeccb..fbcf5f1 100644
--- a/include/WiFiCsiController.h
+++ b/include/WiFiCsiController.h
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023-2024 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -34,9 +34,10 @@ class WiFiCsiController : public Netlink
 public:
     void init();
     int listenToCsi();
-    void enableCsi(bool enable = true);
+    static void enableCsi(bool enable = true);
     inline static std::mutex csiQueueMutex;
     inline static std::queue<Csi*> csiQueue;
+    int64_t stopTime = 0;
 
     ~WiFiCsiController();
 
diff --git a/include/WiFiFtmController.h b/include/WiFiFtmController.h
new file mode 100644
index 0000000..7c398ee
--- /dev/null
+++ b/include/WiFiFtmController.h
@@ -0,0 +1,63 @@
+/*
+ * FeitCSI is the tool for extracting CSI information from supported intel NICs.
+ * Copyright (C) 2025 Miroslav Hutar.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef WIFI_FTM_CONTROLLER_H
+#define WIFI_FTM_CONTROLLER_H
+
+#include "Netlink.h"
+
+struct __attribute__((__packed__)) Ftm
+{
+    uint32_t burstIndex;
+    uint32_t numFtmrAttempts;
+    uint32_t numFtmrSuccesses;
+    uint32_t numBurstsExp;
+    uint8_t burstDuration;
+    uint8_t ftmsPerBurst;
+    uint8_t rssiAvg;
+    uint32_t rssiSpread;
+    uint64_t rttAvg;
+    uint64_t rttVariance;
+    uint64_t rttSpread;
+    uint64_t distAvg;
+    uint64_t distVariance;
+    uint64_t distSpread;
+    uint64_t timestamp;
+}; // size 79 bytes
+
+#define FTM_SIZE sizeof(Ftm)
+
+class WiFiFtmController : public Netlink
+{
+
+public:
+    void init();
+    int startInitiator();
+    int startResponder();
+    int setApMode();
+    uint64_t lastRttIsSuccess = false;
+
+private:
+
+    static int ftmHandler(nl80211_state *state, nl_msg *msg, void *arg);
+    static int ftmResponderHandler(nl80211_state *state, nl_msg *msg, void *arg);
+    static int setApModeHandler(nl80211_state *state, nl_msg *msg, void *arg);
+    static int processFtmHandler(nl_msg *msg, void *arg);
+};
+
+#endif
\ No newline at end of file
diff --git a/include/main.h b/include/main.h
index 9fe4a94..920143f 100644
--- a/include/main.h
+++ b/include/main.h
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023-2024 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -19,7 +19,8 @@
 #ifndef MAIN_H
 #define MAIN_H
 
-#define MONITOR_INTERFACE_NAME "mon0"
+#define MONITOR_INTERFACE_NAME "FeitCSImon"
+#define AP_INTERFACE_NAME "FeitCSIap"
 
 enum processor 
 {
diff --git a/src/Arguments.cpp b/src/Arguments.cpp
index 3c724b5..ec11b9f 100644
--- a/src/Arguments.cpp
+++ b/src/Arguments.cpp
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2024 Miroslav Hutar.
+ * Copyright (C) 2024-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -27,6 +27,7 @@ const char *argp_program_bug_address = "https://github.com/KuskoSoft/FeitCSI/iss
 void Arguments::init()
 {
     Arguments::arguments = {
+        .strict = false,
         .verbose = false,
         .frequency = 2412,
         .gui = false,
@@ -47,6 +48,15 @@ void Arguments::init()
         .measure = true,
         .mode = "measure",
         .ltf = "1xLTF+0.8",
+        .modeDelay = 3000,
+        .ftm = false,
+        .ftmResponder = false,
+        .ftmAsap = false,
+        .ftmBurstExp = 0,
+        .ftmPerBurst = 0,
+        .ftmBurstPeriod = 0,
+        .ftmBurstDuration = 0,
+        .mac = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55}
     };
 }
 
@@ -68,6 +78,9 @@ error_t Arguments::parse_opt(int key, char *arg, struct argp_state *state)
     case 'v':
         args->verbose = true;
         break;
+    case 'z':
+        args->strict = true;
+        break;
     case 'x':
         args->gui = true;
         break;
@@ -94,9 +107,30 @@ error_t Arguments::parse_opt(int key, char *arg, struct argp_state *state)
             args->measure = true;
             args->inject = true;
         }
+        else if (args->mode == "measureftm")
+        {
+            args->measure = true;
+            args->ftm = true;
+        }
+        else if (args->mode == "ftm")
+        {
+            args->measure = false;
+            args->ftm = true;
+        }
+        else if (args->mode == "ftmres")
+        {
+            args->measure = false;
+            args->ftmResponder = true;
+        }
+        else if (args->mode == "injectftmres")
+        {
+            args->measure = false;
+            args->ftmResponder = true;
+            args->inject = true;
+        }
         else
         {
-            argp_failure(state, 1, 0, "Bad mode. Possible values [measure|inject|measureinject]");
+            argp_failure(state, 1, 0, "Bad mode. Possible values [measure|inject|measureinject|ftm]");
             exit(ARGP_ERR_UNKNOWN);
         }
         break;
@@ -151,6 +185,17 @@ error_t Arguments::parse_opt(int key, char *arg, struct argp_state *state)
         }
         break;
     }
+    case 'y':
+    {
+        int modeDelay = std::atoi(arg);
+        if (modeDelay <= 0)
+        {
+            argp_failure(state, 1, 0, "Mode delay is not correct number");
+            exit(ARGP_ERR_UNKNOWN);
+        }
+        args->modeDelay = (uint32_t)modeDelay;
+        break;
+    }
     case 'g':
     {
         int gi = std::atoi(arg);
@@ -257,6 +302,74 @@ error_t Arguments::parse_opt(int key, char *arg, struct argp_state *state)
     case 'o':
         args->outputFile = arg;
         break;
+    case 'b':
+        args->ftmAsap = true;
+        break;
+    case 'q':
+    {
+        int f = std::atoi(arg);
+        if (f <= 0)
+        {
+            argp_failure(state, 1, 0, "FTM burst exponent is not correct");
+            exit(ARGP_ERR_UNKNOWN);
+        }
+        args->ftmBurstExp = (uint8_t)f;
+        break;
+    }
+    case 'e':
+    {
+        int f = std::atoi(arg);
+        if (f <= 0)
+        {
+            argp_failure(state, 1, 0, "FTM per burst is not correct");
+            exit(ARGP_ERR_UNKNOWN);
+        }
+        args->ftmPerBurst = (uint8_t)f;
+        break;
+    }
+    case 'h':
+    {
+        int f = std::atoi(arg);
+        if (f <= 0)
+        {
+            argp_failure(state, 1, 0, "FTM burst period is not correct");
+            exit(ARGP_ERR_UNKNOWN);
+        }
+        args->ftmBurstPeriod = (uint16_t)f;
+        break;
+    }
+    case 'k':
+    {
+        int f = std::atoi(arg);
+        if (f <= 0)
+        {
+            argp_failure(state, 1, 0, "FTM burst duration is not correct");
+            exit(ARGP_ERR_UNKNOWN);
+        }
+        args->ftmBurstDuration = (uint8_t)f;
+        break;
+    }
+    case '#':
+    {
+        int res = sscanf(arg, "%2hhx:%2hhx:%2hhx:%2hhx:%2hhx:%2hhx", &args->mac[0], &args->mac[1], &args->mac[2], &args->mac[3], &args->mac[4], &args->mac[5]);
+        if (res != ETH_ALEN)
+        {
+            argp_failure(state, 1, 0, "Bad mac address");
+            exit(ARGP_ERR_UNKNOWN);
+        }
+        break;
+    }
+    case 'n':
+    {
+        int res = sscanf(arg, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
+                         &args->ftmTargetMac[0], &args->ftmTargetMac[1], &args->ftmTargetMac[2], &args->ftmTargetMac[3], &args->ftmTargetMac[4], &args->ftmTargetMac[5]);
+        if (res != ETH_ALEN)
+        {
+            argp_failure(state, 1, 0, "FTM target mac address is not correct");
+            exit(ARGP_ERR_UNKNOWN);
+        }
+        break;
+    }
     case ARGP_KEY_ARG:
     case ARGP_KEY_END:
         if (args->frequency == 0 ||
diff --git a/src/Csi.cpp b/src/Csi.cpp
index 92dd751..d4c2969 100644
--- a/src/Csi.cpp
+++ b/src/Csi.cpp
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023-2024 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -58,6 +58,7 @@ void Csi::loadFromMemory(uint8_t *pHeader, uint8_t *pRawCsiData)
     memcpy(&this->rawHeaderData, pHeader, CSI_HEADER_LENGTH);
     this->rawCsiData = new uint8_t[this->rawHeaderData.csiDataSize];
     memcpy(this->rawCsiData, pRawCsiData, this->rawHeaderData.csiDataSize);
+    //this->rawHeaderData.timestamp = (uint64_t)std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
 
     this->processRawCsi();
 }
diff --git a/src/MainController.cpp b/src/MainController.cpp
index a82c3a6..29e30f5 100644
--- a/src/MainController.cpp
+++ b/src/MainController.cpp
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023-2024 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -21,6 +21,7 @@
 #include "Logger.h"
 #include "gui/MainWindow.h"
 #include "layout.h"
+#include "WiFiFtmController.h"
 #include <iostream>
 #include <chrono>
 #include <thread>
@@ -137,11 +138,11 @@ void MainController::runNoGui(bool detach)
         auto builder = Gtk::Builder::create_from_string(layout);
         builder->get_widget_derived("MainWindow", this->mainWindow);
         this->initPlots();
-        if (Arguments::arguments.measure)
+        if (Arguments::arguments.measure && !Arguments::arguments.ftm)
         {
             this->measureCsi();
         }
-        if (Arguments::arguments.inject)
+        if (Arguments::arguments.inject && !Arguments::arguments.ftmResponder)
         {
             this->injectPackets();
         }
@@ -149,17 +150,27 @@ void MainController::runNoGui(bool detach)
         return;
     }
 
-    if (Arguments::arguments.measure)
+    if (Arguments::arguments.measure && !Arguments::arguments.ftm)
     {
         this->measuring = true;
         pthread_create(&this->measureCsiThread, NULL, &MainController::measureCsi, NULL);
     }
-    if (Arguments::arguments.inject)
+    if (Arguments::arguments.inject && !Arguments::arguments.ftmResponder)
     {
         this->injecting = true;
         pthread_create(&this->injectPacketThread, NULL, &MainController::injectPackets, NULL);
     }
-    if (Arguments::arguments.measure)
+    if (Arguments::arguments.ftm)
+    {
+        this->ftmEnabled = true;
+        pthread_create(&this->ftmThread, NULL, &MainController::ftm, NULL);
+    }
+    if (Arguments::arguments.ftmResponder)
+    {
+        this->ftmResponderEnabled = true;
+        pthread_create(&this->ftmResponderThread, NULL, &MainController::ftmResponder, NULL);
+    }
+    if (Arguments::arguments.measure && !Arguments::arguments.ftm)
     {
         if (detach) {
             pthread_detach(this->measureCsiThread);
@@ -167,7 +178,7 @@ void MainController::runNoGui(bool detach)
             pthread_join(this->measureCsiThread, NULL);
         }
     }
-    if (Arguments::arguments.inject)
+    if (Arguments::arguments.inject && !Arguments::arguments.ftmResponder)
     {
         if (detach) {
             pthread_detach(this->injectPacketThread);
@@ -175,6 +186,28 @@ void MainController::runNoGui(bool detach)
             pthread_join(this->injectPacketThread, NULL);
         }
     }
+    if (Arguments::arguments.ftm)
+    {
+        if (detach)
+        {
+            pthread_detach(this->ftmThread);
+        }
+        else
+        {
+            pthread_join(this->ftmThread, NULL);
+        }
+    }
+    if (Arguments::arguments.ftmResponder)
+    {
+        if (detach)
+        {
+            pthread_detach(this->ftmResponderThread);
+        }
+        else
+        {
+            pthread_join(this->ftmResponderThread, NULL);
+        }
+    }
 }
 
 void MainController::measureCsi(bool stop)
@@ -195,8 +228,6 @@ void MainController::measureCsi(bool stop)
 
 void MainController::injectPackets(bool stop)
 {
-    this->wifiController.setTxPower();
-    
     if (stop)
     {
         this->injecting = false;
@@ -256,15 +287,10 @@ void MainController::initInterface()
             this->bkpInterfaces.push_back(interface);
             this->wifiController.removeInterface(NULL, interface.ifIndex);
         }
-        this->wifiController.addMonitorDevice(MONITOR_INTERFACE_NAME, NL80211_IFTYPE_MONITOR);
-        this->wifiController.setInterfaceUpDown(MONITOR_INTERFACE_NAME, true);
-        this->wifiController.setFreq(Arguments::arguments.frequency, Arguments::arguments.bandwidth.c_str());
-        if (!MainController::mainWindow)
-        {
-            std::this_thread::sleep_for(std::chrono::milliseconds(500)); //wait on init then set power and go next
-            this->wifiController.setTxPower();
-        }
-
+        this->wifiController.createMonitorInteface();
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        this->wifiController.createApInteface();
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
     }
     catch(const std::exception& e)
     {
@@ -285,6 +311,8 @@ void *MainController::measureCsi(void *arg)
 {
     try
     {
+        MainController::getInstance()->wifiController.setInterfaceUpDown(AP_INTERFACE_NAME, false);
+        MainController::getInstance()->wifiController.setInterfaceUpDown(MONITOR_INTERFACE_NAME, true);
         WiFiCsiController wcs;
         wcs.init();
         wcs.listenToCsi();
@@ -304,10 +332,164 @@ void *MainController::measureCsi(void *arg)
     return 0;
 }
 
+void *MainController::ftm(void *arg)
+{
+    bool firstRun = true;
+    try
+    {
+        MainController::getInstance()->wifiController.setInterfaceUpDown(AP_INTERFACE_NAME, true);
+        WiFiFtmController wft;
+        wft.init();
+        if (Arguments::arguments.measure)
+        {
+            uint64_t startFtmTime = 0;
+            while (1)
+            {
+                std::this_thread::sleep_for(std::chrono::microseconds(Arguments::arguments.injectDelay));
+                try
+                {
+                    wft.startInitiator();
+                }
+                catch(const std::exception& e)
+                {
+                    std::cerr << e.what() << '\n';
+                }
+                
+                
+                if (wft.lastRttIsSuccess && firstRun)
+                {
+                    firstRun = false;
+                    startFtmTime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+                }
+
+                uint64_t now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+                if ((startFtmTime + (Arguments::arguments.modeDelay / 2)) > now)
+                {
+                    continue;
+                }
+
+                if (!wft.lastRttIsSuccess && !firstRun)
+                {
+                    MainController::getInstance()->measureCsi(false);
+                    std::this_thread::sleep_for(std::chrono::milliseconds(Arguments::arguments.modeDelay));
+                    MainController::getInstance()->measureCsi(true);
+                    WiFiCsiController::enableCsi(false);
+                    firstRun = true;
+                    MainController::getInstance()->wifiController.setInterfaceUpDown(MONITOR_INTERFACE_NAME, false);
+                    MainController::getInstance()->wifiController.setInterfaceUpDown(AP_INTERFACE_NAME, true);
+                }
+            }
+        }
+        else
+        {
+            if (Arguments::arguments.injectRepeat)
+            {
+                for (uint32_t i = 0; i < Arguments::arguments.injectRepeat; i++)
+                {
+                    wft.startInitiator();
+                    std::this_thread::sleep_for(std::chrono::microseconds(Arguments::arguments.injectDelay));
+                }
+            }
+            else
+            {
+                while (true)
+                {
+                    wft.startInitiator();
+                    std::this_thread::sleep_for(std::chrono::microseconds(Arguments::arguments.injectDelay));
+                }
+            }
+        }
+    }
+    catch (const std::exception &e)
+    {
+        if (MainController::mainWindow)
+        {
+            MainController::mainWindow->fatalError(e.what());
+        }
+        else
+        {
+            Logger::log(error) << e.what() << '\n';
+        }
+    }
+
+    return 0;
+}
+
+void *MainController::ftmResponder(void *arg)
+{
+    WiFiFtmController wft;
+    wft.init();
+    try
+    {
+        if (Arguments::arguments.inject)
+        {
+            while (true)
+            {
+                MainController::getInstance()->injectPackets(false);
+                std::this_thread::sleep_for(std::chrono::milliseconds(Arguments::arguments.modeDelay));
+                MainController::getInstance()->injectPackets(true);
+                MainController::getInstance()->wifiController.setInterfaceUpDown(MONITOR_INTERFACE_NAME, false);
+
+                MainController::getInstance()->wifiController.setInterfaceUpDown(AP_INTERFACE_NAME, true);
+                MainController::getInstance()->wifiController.getInterfaceInfo(AP_INTERFACE_NAME);
+                while (MainController::getInstance()->wifiController.currentInterfaceInfo.freq == 0)
+                {
+                    try
+                    {
+                        wft.startResponder();
+                    }
+                    catch (const std::exception &e)
+                    {
+                        // Just ignore.. wait on network up
+                    }
+                    MainController::getInstance()->wifiController.getInterfaceInfo(AP_INTERFACE_NAME);
+                }
+                std::this_thread::sleep_for(std::chrono::milliseconds(Arguments::arguments.modeDelay));
+            }
+        } else
+        {
+            while(MainController::getInstance()->wifiController.currentInterfaceInfo.freq == 0) {
+                try
+                {
+                    wft.startResponder();
+                }
+                catch(const std::exception& e)
+                {
+                    //Just ignore.. wait on network up
+                }
+                MainController::getInstance()->wifiController.getInterfaceInfo(AP_INTERFACE_NAME);
+            }
+            if (Arguments::arguments.verbose)
+            {
+                Logger::log(info) << "FTM responder was started\n";
+            }
+        }
+        while (1)
+        {
+            sleep(1);
+        }
+    }
+    catch (const std::exception &e)
+    {
+        if (MainController::mainWindow)
+        {
+            MainController::mainWindow->fatalError(e.what());
+        }
+        else
+        {
+            Logger::log(error) << e.what() << '\n';
+        }
+    }
+
+    return 0;
+}
+
 void *MainController::injectPackets(void *arg)
 {
     try
     {
+        MainController::getInstance()->wifiController.setInterfaceUpDown(AP_INTERFACE_NAME, false);
+        MainController::getInstance()->wifiController.setInterfaceUpDown(MONITOR_INTERFACE_NAME, true);
         PacketInjector pi;
         if (Arguments::arguments.injectRepeat)
         {
@@ -355,13 +537,14 @@ void MainController::restoreState()
     }
 
     mainController->wifiController.removeInterface(MONITOR_INTERFACE_NAME);
+    mainController->wifiController.removeInterface(AP_INTERFACE_NAME);
     for (InterfaceInfo interface : mainController->bkpInterfaces)
     {
         if (Arguments::arguments.verbose)
         {
             Logger::log(info) << "Recovering interface " << interface.ifName << "\n";
         }
-        mainController->wifiController.addMonitorDevice(interface.ifName.c_str(), (nl80211_iftype)interface.ifType);
+        mainController->wifiController.addDevice(interface.ifName.c_str(), (nl80211_iftype)interface.ifType);
     }
     mainController->bkpInterfaces.clear();
     mainController->wifiController.interfaces.clear();
diff --git a/src/Netlink.cpp b/src/Netlink.cpp
index 95b0d78..826d76a 100644
--- a/src/Netlink.cpp
+++ b/src/Netlink.cpp
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -27,7 +27,7 @@
 
 void Netlink::init()
 {
-    /* int err =  */ this->nlInit(&this->nlstate);
+        /* int err =  */ this->nlInit(&this->nlstate);
     // TODO if error
 }
 
@@ -127,6 +127,7 @@ int Netlink::nlExecCommand(Cmd &cmd)
     nl_socket_set_cb(this->nlstate.nl_sock, s_cb);
 
     err = nl_send_auto_complete(this->nlstate.nl_sock, msg);
+    
     if (err < 0)
         goto out;
 
@@ -145,7 +146,6 @@ int Netlink::nlExecCommand(Cmd &cmd)
     {
         errMsg = "command failed: " + std::string(strerror(-err)) + std::to_string(err) + "\n";
     }
-
 out:
     nl_cb_put(cb);
     nl_cb_put(s_cb);
diff --git a/src/PacketInjector.cpp b/src/PacketInjector.cpp
index dc3877c..c62ec2a 100644
--- a/src/PacketInjector.cpp
+++ b/src/PacketInjector.cpp
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023-2024 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -175,7 +175,7 @@ void PacketInjector::send(uint32_t rateNFlags)
     char szErrbuf[500];
 
     if (ppcap == nullptr) {
-        ppcap = pcap_open_live("mon0", 800, 1, 20, szErrbuf);
+        ppcap = pcap_open_live(MONITOR_INTERFACE_NAME, 800, 1, 20, szErrbuf);
     }
 
     int r = pcap_inject(ppcap, sendBuffer, totalSize);
diff --git a/src/WiFIController.cpp b/src/WiFIController.cpp
index 0ae54a9..ac1dd98 100644
--- a/src/WiFIController.cpp
+++ b/src/WiFIController.cpp
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023-2024 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -107,20 +107,34 @@ void WiFIController::getInterfaces()
     this->nlExecCommand(cmd);
 }
 
+void WiFIController::getInterfaceInfo(const char *ifname)
+{
+    Cmd cmd{
+        .id = NL80211_CMD_GET_INTERFACE,
+        .idby = CIB_NETDEV,
+        .nlFlags = 0,
+        .device = if_nametoindex(ifname),
+        .handler = NULL,
+        .valid_handler = this->processGetInterfaceInfoHandler,
+    };
+
+    this->nlExecCommand(cmd);
+}
+
 void WiFIController::setTxPower()
 {
     Cmd cmd{
         .id = NL80211_CMD_SET_WIPHY,
         .idby = CIB_NETDEV,
         .nlFlags = 0,
-        .device = if_nametoindex(MONITOR_INTERFACE_NAME),
+        .device = if_nametoindex(this->currentDeviceName.c_str()),
         .handler = this->setTxPowerHandler,
     };
 
     this->nlExecCommand(cmd);
 }
 
-void WiFIController::addMonitorDevice(const char *name, const nl80211_iftype type)
+void WiFIController::addDevice(const char *name, const nl80211_iftype type)
 {
     if (this->phys.empty())
     {
@@ -135,7 +149,7 @@ void WiFIController::addMonitorDevice(const char *name, const nl80211_iftype typ
         .idby = CIB_PHY,
         .nlFlags = 0,
         .device = this->phys[0],
-        .handler = this->processAddMonitorDeviceHandler,
+        .handler = this->processaddDeviceHandler,
         .valid_handler = NULL,
         .args = settings,
     };
@@ -166,6 +180,7 @@ int WiFIController::setInterfaceUpDown(const char *ifName, bool up)
     if (up)
     {
         ifr.ifr_flags |= IFF_UP;
+        this->currentDeviceName = ifName;
     }
     else
     {
@@ -196,7 +211,7 @@ void WiFIController::setFreq(uint16_t freq, const char *bw)
         .id = NL80211_CMD_SET_WIPHY,
         .idby = CIB_NETDEV,
         .nlFlags = 0,
-        .device = if_nametoindex(MONITOR_INTERFACE_NAME),
+        .device = if_nametoindex(this->currentDeviceName.c_str()),
         .handler = processSetFreq,
         .valid_handler = NULL,
         .args = settings,
@@ -217,6 +232,48 @@ void WiFIController::removeInterface(const char *name, int ifIndex)
     this->nlExecCommand(cmd);
 }
 
+void WiFIController::createMonitorInteface()
+{
+    this->addDevice(MONITOR_INTERFACE_NAME, NL80211_IFTYPE_MONITOR);
+    while (this->currentInterfaceInfo.freq != Arguments::arguments.frequency)
+    {
+        try
+        {
+            this->setInterfaceUpDown(MONITOR_INTERFACE_NAME, true);
+            this->setFreq(Arguments::arguments.frequency, Arguments::arguments.bandwidth.c_str());
+        }
+        catch (const std::exception &e)
+        {
+            // Just skip
+        }
+
+        this->getInterfaceInfo(MONITOR_INTERFACE_NAME);
+    }
+    this->setTxPower();
+}
+
+void WiFIController::createApInteface()
+{
+    this->addDevice(AP_INTERFACE_NAME, NL80211_IFTYPE_MONITOR);
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    while (this->currentInterfaceInfo.ifType != NL80211_IFTYPE_AP)
+    {
+        try
+        {
+            this->setInterfaceUpDown(MONITOR_INTERFACE_NAME, false);
+            this->setApMode();
+        }
+        catch (const std::exception &e)
+        {
+            // Just skip
+        }
+
+        this->getInterfaceInfo(AP_INTERFACE_NAME);
+    }
+    this->setInterfaceUpDown(AP_INTERFACE_NAME, true);
+    this->setTxPower();
+}
+
 void WiFIController::getPhys()
 {
     Cmd cmd{
@@ -414,10 +471,10 @@ nla_put_failure:
     return -ENOBUFS;
 }
 
-int WiFIController::setTxPowerHandler(nl80211_state * state, nl_msg * msg, void * arg)
+int WiFIController::setTxPowerHandler(nl80211_state *state, nl_msg *msg, void *arg)
 {
     enum nl80211_tx_power_setting type = NL80211_TX_POWER_FIXED;
-    int mbm = Arguments::arguments.txPower * 100; //dBm to mbm *100
+    int mbm = Arguments::arguments.txPower * 100; // dBm to mbm *100
 
     NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_TX_POWER_SETTING, type);
     NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_TX_POWER_LEVEL, mbm);
@@ -428,7 +485,7 @@ nla_put_failure:
     return -ENOBUFS;
 }
 
-int WiFIController::processAddMonitorDeviceHandler(struct nl80211_state *state, struct nl_msg *msg, void *arg)
+int WiFIController::processaddDeviceHandler(struct nl80211_state *state, struct nl_msg *msg, void *arg)
 {
     void **settings = (void **)arg;
     const char *name = (const char *)settings[0];
@@ -436,6 +493,8 @@ int WiFIController::processAddMonitorDeviceHandler(struct nl80211_state *state,
 
     NLA_PUT_STRING(msg, NL80211_ATTR_IFNAME, name);
     NLA_PUT_U32(msg, NL80211_ATTR_IFTYPE, *type);
+    NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, Arguments::arguments.mac);
+
     return 0;
 nla_put_failure:
     return -ENOBUFS;
@@ -592,6 +651,104 @@ int WiFIController::processGetInterfacesHandler(struct nl_msg *msg, void *arg)
     return NL_OK;
 }
 
+int WiFIController::processGetInterfaceInfoHandler(struct nl_msg *msg, void *arg)
+{
+    struct genlmsghdr *gnlh = (genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
+    struct nlattr *tb_msg[NL80211_ATTR_MAX + 1];
+
+    nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
+              genlmsg_attrlen(gnlh, 0), NULL);
+    WiFIController *wc = (WiFIController *)arg;
+
+    if (tb_msg[NL80211_ATTR_IFNAME])
+    {
+        wc->currentInterfaceInfo.ifName = nla_get_string(tb_msg[NL80211_ATTR_IFNAME]);
+    }
+    else
+    {
+        wc->currentInterfaceInfo.ifName = "Unnamed/non-netdev interface";
+        return NL_OK;
+    }
+
+    if (tb_msg[NL80211_ATTR_IFINDEX])
+    {
+        wc->currentInterfaceInfo.ifIndex = nla_get_u32(tb_msg[NL80211_ATTR_IFINDEX]);
+    }
+    else
+    {
+        wc->currentInterfaceInfo.ifIndex = 0;
+    }
+
+    if (tb_msg[NL80211_ATTR_WDEV])
+    {
+        wc->currentInterfaceInfo.wdev = (uint64_t)nla_get_u64(tb_msg[NL80211_ATTR_WDEV]);
+    }
+    else
+    {
+        wc->currentInterfaceInfo.wdev = 0;
+    }
+
+    if (tb_msg[NL80211_ATTR_MAC])
+    {
+        wc->currentInterfaceInfo.mac = wc->macN2a((const unsigned char *)nla_data(tb_msg[NL80211_ATTR_MAC]));
+    }
+    else
+    {
+        wc->currentInterfaceInfo.mac = "";
+    }
+
+    if (tb_msg[NL80211_ATTR_SSID])
+    {
+        wc->currentInterfaceInfo.ssid = (const char *)nla_data(tb_msg[NL80211_ATTR_SSID]);
+    }
+    else
+    {
+        wc->currentInterfaceInfo.ssid = "";
+    }
+
+    if (tb_msg[NL80211_ATTR_IFTYPE])
+    {
+        wc->currentInterfaceInfo.ifType = nla_get_u32(tb_msg[NL80211_ATTR_IFTYPE]);
+    }
+    else
+    {
+        wc->currentInterfaceInfo.ifType = 0;
+    }
+
+    if (tb_msg[NL80211_ATTR_WIPHY])
+    {
+        wc->currentInterfaceInfo.wiphy = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY]);
+    }
+    else
+    {
+        wc->currentInterfaceInfo.wiphy = 0;
+    }
+
+    if (tb_msg[NL80211_ATTR_WIPHY_FREQ])
+    {
+        wc->currentInterfaceInfo.freq = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_FREQ]);
+        wc->currentInterfaceInfo.channel = wc->freqToCHannel(wc->currentInterfaceInfo.freq);
+    }
+    else
+    {
+        wc->currentInterfaceInfo.freq = 0;
+        wc->currentInterfaceInfo.channel = 0;
+    }
+
+    if (tb_msg[NL80211_ATTR_WIPHY_TX_POWER_LEVEL])
+    {
+        int32_t txp = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_TX_POWER_LEVEL]);
+        wc->currentInterfaceInfo.txdBm = txp % 100;
+        wc->currentInterfaceInfo.txdBm += txp / 100;
+    }
+    else
+    {
+        wc->currentInterfaceInfo.txdBm = 0;
+    }
+
+    return NL_OK;
+}
+
 std::string WiFIController::macN2a(const unsigned char *arg)
 {
     int i, l;
@@ -659,4 +816,26 @@ int WiFIController::phyLookup(char *name)
     buf[pos] = '\0';
     close(fd);
     return atoi(buf);
+}
+
+int WiFIController::setApMode()
+{
+    Cmd cmd{
+        .id = NL80211_CMD_SET_INTERFACE,
+        .idby = CIB_NETDEV,
+        .nlFlags = 0,
+        .device = if_nametoindex(AP_INTERFACE_NAME),
+        .handler = this->setApModeHandler,
+        .valid_handler = NULL,
+    };
+
+    return this->nlExecCommand(cmd);
+}
+
+int WiFIController::setApModeHandler(struct nl80211_state *state, struct nl_msg *msg, void *arg)
+{
+    NLA_PUT_U32(msg, NL80211_ATTR_IFTYPE, NL80211_IFTYPE_AP);
+    return 0;
+nla_put_failure:
+    return -ENOBUFS;
 }
\ No newline at end of file
diff --git a/src/WiFiCsiController.cpp b/src/WiFiCsiController.cpp
index 2e672aa..9615ac2 100644
--- a/src/WiFiCsiController.cpp
+++ b/src/WiFiCsiController.cpp
@@ -1,6 +1,6 @@
 /*
  * FeitCSI is the tool for extracting CSI information from supported intel NICs.
- * Copyright (C) 2023-2024 Miroslav Hutar.
+ * Copyright (C) 2023-2025 Miroslav Hutar.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -109,19 +109,23 @@ int WiFiCsiController::processListenToCsiHandler(struct nl_msg *msg, void *arg)
                 
                 )
                 {
-                    if (Arguments::arguments.verbose) {
-                        printDetail(c);
-                    }
-                    if ( MainController::getInstance()->udpSocket ) {
-                        c->sendUDP(MainController::getInstance()->udpSocket);
-                    } else {
-                        c->save();
-                    }
-                    if (Arguments::arguments.plot)
+                    printf("masm csi\n");
+                    if (!Arguments::arguments.strict || (Arguments::arguments.strict && (c->rawHeaderData.rateNflag & RATE_LEGACY_RATE_MSK) == Arguments::arguments.mcs))
                     {
-                        WiFiCsiController::csiQueueMutex.lock();
-                        WiFiCsiController::csiQueue.push(c);
-                        WiFiCsiController::csiQueueMutex.unlock();
+                        if (Arguments::arguments.verbose) {
+                            printDetail(c);
+                        }
+                        if ( MainController::getInstance()->udpSocket ) {
+                            c->sendUDP(MainController::getInstance()->udpSocket);
+                        } else {
+                            c->save();
+                        }
+                        if (Arguments::arguments.plot)
+                        {
+                            WiFiCsiController::csiQueueMutex.lock();
+                            WiFiCsiController::csiQueue.push(c);
+                            WiFiCsiController::csiQueueMutex.unlock();
+                        }
                     }
                     
                 }
@@ -129,6 +133,13 @@ int WiFiCsiController::processListenToCsiHandler(struct nl_msg *msg, void *arg)
         }
     }
 
+    WiFiCsiController *wcc = (WiFiCsiController*) arg;
+    auto now = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+    if (wcc->stopTime != 0 && wcc->stopTime < now)
+    {
+        return NL_OK;
+    }
+
     return NL_SKIP;
 }
 
@@ -161,23 +172,18 @@ void WiFiCsiController::printDetail(Csi *c)
     case RATE_MCS_LEGACY_OFDM_MSK:
         Logger::log(info, true) << "Format: LEGACY_OFDM\n";
         break;
-        break;
     case RATE_MCS_HT_MSK:
         Logger::log(info, true) << "Format: HT\n";
         break;
-        break;
     case RATE_MCS_VHT_MSK:
         Logger::log(info, true) << "Format: VHT\n";
         break;
-        break;
     case RATE_MCS_HE_MSK:
         Logger::log(info, true) << "Format: HE\n";
         break;
-        break;
     case RATE_MCS_EHT_MSK:
         Logger::log(info, true) << "Format: EHT\n";
         break;
-        break;
     }
 }
 
diff --git a/src/WiFiFtmController.cpp b/src/WiFiFtmController.cpp
new file mode 100644
index 0000000..e103847
--- /dev/null
+++ b/src/WiFiFtmController.cpp
@@ -0,0 +1,339 @@
+/*
+ * FeitCSI is the tool for extracting CSI information from supported intel NICs.
+ * Copyright (C) 2025 Miroslav Hutar.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "WiFiFtmController.h"
+#include "main.h"
+#include "Arguments.h"
+#include <net/if.h>
+#include "Logger.h"
+#include "MainController.h"
+#include <netlink/genl/genl.h>
+#include <fstream>
+#include <filesystem>
+#include <cstring>
+
+uint8_t beaconHeader[] = {0x80, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xea, 0xb9, 0xcc, 0xc2, 0x4a, 0xaf, 0xea, 0xb9, 0xcc, 0xc2, 0x4a, 0xaf, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x01, 0x04, 0x00, 0x07, 0x46, 0x65, 0x69, 0x74, 0x43, 0x53, 0x49, 0x01, 0x08, 0x82, 0x84, 0x8b, 0x96, 0x0c, 0x12, 0x18, 0x24, 0x03, 0x01, 0x01};
+
+void WiFiFtmController::init()
+{
+    Netlink::init();
+}
+
+int WiFiFtmController::startInitiator()
+{
+    Cmd cmd{
+        .id = NL80211_CMD_PEER_MEASUREMENT_START,
+        .idby = CIB_NETDEV,
+        .nlFlags = 0,
+        .device = if_nametoindex(AP_INTERFACE_NAME),
+        .handler = this->ftmHandler,
+        .valid_handler = this->processFtmHandler,
+    };
+
+    return this->nlExecCommand(cmd);
+}
+
+int WiFiFtmController::startResponder()
+{
+    Cmd cmd{
+        .id = NL80211_CMD_NEW_BEACON,
+        .idby = CIB_NETDEV,
+        .nlFlags = 0,
+        .device = if_nametoindex(AP_INTERFACE_NAME),
+        .handler = this->ftmResponderHandler,
+        .valid_handler = NULL,
+    };
+
+    return this->nlExecCommand(cmd);
+}
+
+int WiFiFtmController::setApMode()
+{
+    Cmd cmd{
+        .id = NL80211_CMD_SET_INTERFACE,
+        .idby = CIB_NETDEV,
+        .nlFlags = 0,
+        .device = if_nametoindex(AP_INTERFACE_NAME),
+        .handler = this->setApModeHandler,
+        .valid_handler = NULL,
+    };
+
+    return this->nlExecCommand(cmd);
+}
+
+int WiFiFtmController::ftmHandler(struct nl80211_state *state, struct nl_msg *msg, void *arg)
+{
+
+    struct nlattr *pmsr, *peers, *peer, *req, *reqdata, *ftm, *chan;
+
+    pmsr = nla_nest_start(msg, NL80211_ATTR_PEER_MEASUREMENTS);
+    peers = nla_nest_start(msg, NL80211_PMSR_ATTR_PEERS);
+
+    peer = nla_nest_start(msg, 1); // TODO multiple peers
+    NLA_PUT(msg, NL80211_PMSR_PEER_ATTR_ADDR, ETH_ALEN, Arguments::arguments.ftmTargetMac);
+
+    req = nla_nest_start(msg, NL80211_PMSR_PEER_ATTR_REQ);
+    reqdata = nla_nest_start(msg, NL80211_PMSR_REQ_ATTR_DATA);
+    ftm = nla_nest_start(msg, NL80211_PMSR_TYPE_FTM);
+
+    if (Arguments::arguments.ftmBurstExp)
+    {
+        NLA_PUT_U8(msg, NL80211_PMSR_FTM_REQ_ATTR_NUM_BURSTS_EXP, Arguments::arguments.ftmBurstExp);
+    }
+    if (Arguments::arguments.ftmBurstPeriod)
+    {
+        NLA_PUT_U16(msg, NL80211_PMSR_FTM_REQ_ATTR_BURST_PERIOD, Arguments::arguments.ftmBurstPeriod);
+    }
+    if (Arguments::arguments.ftmBurstDuration)
+    {
+        NLA_PUT_U8(msg, NL80211_PMSR_FTM_REQ_ATTR_BURST_DURATION, Arguments::arguments.ftmBurstDuration);
+    }
+    if (Arguments::arguments.ftmPerBurst)
+    {
+        NLA_PUT_U8(msg, NL80211_PMSR_FTM_REQ_ATTR_FTMS_PER_BURST, Arguments::arguments.ftmPerBurst);
+    }
+    if (Arguments::arguments.ftmAsap)
+    {
+        NLA_PUT_FLAG(msg, NL80211_PMSR_FTM_REQ_ATTR_ASAP);
+    }
+
+    if (Arguments::arguments.format == "NOHT")
+    {
+        NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE, NL80211_PREAMBLE_LEGACY);
+    }
+    else if (Arguments::arguments.format == "HT")
+    {
+        NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE, NL80211_PREAMBLE_HT);
+    }
+    else if (Arguments::arguments.format == "VHT")
+    {
+        NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE, NL80211_PREAMBLE_VHT);
+    }
+    else if (Arguments::arguments.format == "HESU")
+    {
+        NLA_PUT_U32(msg, NL80211_PMSR_FTM_REQ_ATTR_PREAMBLE, NL80211_PREAMBLE_HE);
+    }
+
+    nla_nest_end(msg, ftm);
+    nla_nest_end(msg, reqdata);
+    nla_nest_end(msg, req);
+
+    chan = nla_nest_start(msg, NL80211_PMSR_PEER_ATTR_CHAN);
+    NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, (uint32_t)Arguments::arguments.frequency);
+
+    switch (Arguments::arguments.channelWidth)
+    {
+    case 20:
+        NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH, NL80211_CHAN_WIDTH_20);
+        break;
+    case 40:
+        NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH, NL80211_CHAN_WIDTH_40);
+        break;
+    case 80:
+        NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH, NL80211_CHAN_WIDTH_80);
+        break;
+    case 160:
+        NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH, NL80211_CHAN_WIDTH_160);
+        break;
+    case 320:
+        NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH, NL80211_CHAN_WIDTH_320);
+        break;
+    }
+
+    nla_nest_end(msg, chan);
+    nla_nest_end(msg, peer);
+    nla_nest_end(msg, peers);
+    nla_nest_end(msg, pmsr);
+
+    return 0;
+nla_put_failure:
+    return -ENOBUFS;
+}
+
+int WiFiFtmController::ftmResponderHandler(struct nl80211_state *state, struct nl_msg *msg, void *arg)
+{
+    NLA_PUT(msg, NL80211_ATTR_BEACON_HEAD, 58, beaconHeader);
+
+    NLA_PUT_U32(msg, NL80211_ATTR_BEACON_INTERVAL, 100);
+    NLA_PUT_U32(msg, NL80211_ATTR_DTIM_PERIOD, 2);
+    NLA_PUT(msg, NL80211_ATTR_SSID, 7, "FeitCSI");
+    NLA_PUT_U32(msg, NL80211_ATTR_HIDDEN_SSID, NL80211_HIDDEN_SSID_NOT_IN_USE);
+    NLA_PUT_U32(msg, NL80211_ATTR_AUTH_TYPE, NL80211_AUTHTYPE_OPEN_SYSTEM);
+
+    struct nlattr *ftm;
+    ftm = nla_nest_start(msg, NL80211_ATTR_FTM_RESPONDER);
+    NLA_PUT_FLAG(msg, NL80211_FTM_RESP_ATTR_ENABLED);
+    nla_nest_end(msg, ftm);
+
+    NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, (uint32_t)Arguments::arguments.frequency);
+    switch (Arguments::arguments.channelWidth)
+    {
+    case 20:
+        NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH, NL80211_CHAN_WIDTH_20);
+        break;
+    case 40:
+        NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH, NL80211_CHAN_WIDTH_40);
+        break;
+    case 80:
+        NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH, NL80211_CHAN_WIDTH_80);
+        break;
+    case 160:
+        NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH, NL80211_CHAN_WIDTH_160);
+        break;
+    case 320:
+        NLA_PUT_U32(msg, NL80211_ATTR_CHANNEL_WIDTH, NL80211_CHAN_WIDTH_320);
+        break;
+    }
+
+    NLA_PUT_FLAG(msg, NL80211_ATTR_SOCKET_OWNER);
+
+    // nl_msg_dump(msg, stdout);
+
+    return 0;
+nla_put_failure:
+    return -ENOBUFS;
+}
+
+int WiFiFtmController::setApModeHandler(struct nl80211_state *state, struct nl_msg *msg, void *arg)
+{
+    NLA_PUT_U32(msg, NL80211_ATTR_IFTYPE, NL80211_IFTYPE_AP);
+    return 0;
+nla_put_failure:
+    return -ENOBUFS;
+}
+
+int WiFiFtmController::processFtmHandler(struct nl_msg *msg, void *arg)
+{
+    struct genlmsghdr *gnlh = (genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
+    struct nlattr *tb[NL80211_ATTR_MAX + 1];
+
+    nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
+
+    if (gnlh->cmd == NL80211_CMD_PEER_MEASUREMENT_COMPLETE)
+    {
+        return NL_OK;
+    }
+
+    if (gnlh->cmd == NL80211_CMD_PEER_MEASUREMENT_RESULT)
+    {
+        if (!tb[NL80211_ATTR_PEER_MEASUREMENTS])
+        {
+            return NL_OK;
+        }
+        struct nlattr *pmsr[NL80211_PMSR_ATTR_MAX + 1];
+        nla_parse_nested(pmsr, NL80211_PMSR_ATTR_MAX, tb[NL80211_ATTR_PEER_MEASUREMENTS], NULL);
+
+        if (!pmsr[NL80211_PMSR_ATTR_PEERS])
+        {
+            return NL_OK;
+        }
+
+        struct nlattr *peer = (nlattr *)nla_data(pmsr[NL80211_PMSR_ATTR_PEERS]);
+
+        struct nlattr *tbpeer[NL80211_PMSR_PEER_ATTR_MAX + 1];
+        struct nlattr *resp[NL80211_PMSR_RESP_ATTR_MAX + 1];
+        struct nlattr *data[NL80211_PMSR_TYPE_MAX + 1];
+
+        nla_parse_nested(tbpeer, NL80211_PMSR_PEER_ATTR_MAX, peer, NULL);
+
+        if (!tbpeer[NL80211_PMSR_PEER_ATTR_ADDR])
+        {
+            return NL_OK;
+        }
+
+        uint8_t mac[ETH_ALEN];
+        memcpy(mac, nla_data(tbpeer[NL80211_PMSR_PEER_ATTR_ADDR]), ETH_ALEN);
+
+        if (!tbpeer[NL80211_PMSR_PEER_ATTR_ADDR])
+        {
+            return NL_OK;
+        }
+
+        nla_parse_nested(resp, NL80211_PMSR_RESP_ATTR_MAX, tbpeer[NL80211_PMSR_PEER_ATTR_RESP], NULL);
+
+        if (!resp[NL80211_PMSR_RESP_ATTR_DATA])
+        {
+            return NL_OK;
+        }
+
+        nla_parse_nested(data, NL80211_PMSR_TYPE_MAX, resp[NL80211_PMSR_RESP_ATTR_DATA], NULL);
+
+        if (!data[NL80211_PMSR_TYPE_FTM])
+        {
+            return NL_OK;
+        }
+
+        struct nlattr *ftm[NL80211_PMSR_FTM_RESP_ATTR_MAX + 1];
+        nla_parse_nested(ftm, NL80211_PMSR_FTM_RESP_ATTR_MAX, data[NL80211_PMSR_TYPE_FTM], NULL);
+        WiFiFtmController *wfc = (WiFiFtmController *)arg;
+        if (ftm[NL80211_PMSR_FTM_RESP_ATTR_FAIL_REASON])
+        {
+            uint32_t reason = nla_get_u32(ftm[NL80211_PMSR_FTM_RESP_ATTR_FAIL_REASON]);
+            if (Arguments::arguments.verbose)
+            {
+                Logger::log(info) << "FTM failed to measure " << reason << "\n";
+            }
+            wfc->lastRttIsSuccess = false;
+            return NL_OK;
+        }
+
+        Ftm ftmData{
+            .burstIndex = (uint32_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_BURST_INDEX] ? nla_get_u32(ftm[NL80211_PMSR_FTM_RESP_ATTR_BURST_INDEX]) : 0),
+            .numFtmrAttempts = (uint32_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_NUM_FTMR_ATTEMPTS] ? nla_get_u32(ftm[NL80211_PMSR_FTM_RESP_ATTR_NUM_FTMR_ATTEMPTS]) : 0),
+            .numFtmrSuccesses = (uint32_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_NUM_FTMR_SUCCESSES] ? nla_get_u32(ftm[NL80211_PMSR_FTM_RESP_ATTR_NUM_FTMR_SUCCESSES]) : 0),
+            .numBurstsExp = (uint32_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_NUM_BURSTS_EXP] ? nla_get_u32(ftm[NL80211_PMSR_FTM_RESP_ATTR_NUM_BURSTS_EXP]) : 0),
+            .burstDuration = (uint8_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_BURST_DURATION] ? nla_get_u8(ftm[NL80211_PMSR_FTM_RESP_ATTR_BURST_DURATION]) : 0),
+            .ftmsPerBurst = (uint8_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_FTMS_PER_BURST] ? nla_get_u8(ftm[NL80211_PMSR_FTM_RESP_ATTR_FTMS_PER_BURST]) : 0),
+            .rssiAvg = (uint8_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_RSSI_AVG] ? nla_get_u8(ftm[NL80211_PMSR_FTM_RESP_ATTR_RSSI_AVG]) : 0),
+            .rssiSpread = (uint32_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_RSSI_SPREAD] ? nla_get_u32(ftm[NL80211_PMSR_FTM_RESP_ATTR_RSSI_SPREAD]) : 0),
+            .rttAvg = (uint64_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_RTT_AVG] ? nla_get_u64(ftm[NL80211_PMSR_FTM_RESP_ATTR_RTT_AVG]) : 0),
+            .rttVariance = (uint64_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_RTT_VARIANCE] ? nla_get_u64(ftm[NL80211_PMSR_FTM_RESP_ATTR_RTT_VARIANCE]) : 0),
+            .rttSpread = (uint64_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_RTT_SPREAD] ? nla_get_u64(ftm[NL80211_PMSR_FTM_RESP_ATTR_RTT_SPREAD]) : 0),
+            .distAvg = (uint64_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_DIST_AVG] ? nla_get_u64(ftm[NL80211_PMSR_FTM_RESP_ATTR_RTT_AVG]) : 0),
+            .distVariance = (uint64_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_DIST_VARIANCE] ? nla_get_u64(ftm[NL80211_PMSR_FTM_RESP_ATTR_DIST_VARIANCE]) : 0),
+            .distSpread = (uint64_t)(ftm[NL80211_PMSR_FTM_RESP_ATTR_DIST_SPREAD] ? nla_get_u64(ftm[NL80211_PMSR_FTM_RESP_ATTR_DIST_SPREAD]) : 0),
+            .timestamp = (uint64_t)std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count(),
+        };
+
+        if (Arguments::arguments.verbose)
+        {
+            Logger::log(info) << "FTM average RTT: " << ftmData.rttAvg << "ps\n";
+        }
+
+        if (MainController::getInstance()->udpSocket)
+        {
+            MainController::getInstance()->udpSocket->send(reinterpret_cast<char *>(&ftmData), FTM_SIZE);
+        } else {
+            std::ofstream outfile;
+            std::string filename = std::string("FTM_") + Arguments::arguments.outputFile;
+            outfile.open(filename, std::ios_base::app | std::ios::binary);
+            if (outfile.fail())
+            {
+                throw std::ios_base::failure("Open file failed: " + std::string(std::strerror(errno)));
+            }
+            outfile.write(reinterpret_cast<char *>(&ftmData), FTM_SIZE);
+            outfile.close();
+            std::filesystem::permissions(filename, std::filesystem::perms::all & ~(std::filesystem::perms::owner_exec | std::filesystem::perms::group_exec | std::filesystem::perms::others_exec), std::filesystem::perm_options::add);
+        }
+
+        wfc->lastRttIsSuccess = true;
+        return NL_OK;
+    }
+    
+    return NL_OK;
+}
\ No newline at end of file