From a761eb431713418800d887f9f22647730cc205f4 Mon Sep 17 00:00:00 2001
From: Carl Klemm <carl@uvos.xyz>
Date: Thu, 13 Feb 2020 19:25:28 +0100
Subject: [PATCH] added apgetconnected to git

---
 src/apgetconnected.cpp | 172 +++++++++++++++++++++++++++++++++++++++++
 src/apgetconnected.h   |  19 +++++
 2 files changed, 191 insertions(+)
 create mode 100644 src/apgetconnected.cpp
 create mode 100644 src/apgetconnected.h

diff --git a/src/apgetconnected.cpp b/src/apgetconnected.cpp
new file mode 100644
index 0000000..018feaa
--- /dev/null
+++ b/src/apgetconnected.cpp
@@ -0,0 +1,172 @@
+#include "apgetconnected.h"
+
+#include <iostream>
+#include <sstream>
+#include <iomanip>
+
+#include <linux/nl80211.h>
+#include <net/if.h>
+
+#include <netlink/genl/genl.h>
+#include <netlink/genl/ctrl.h>
+#include <netlink/msg.h>
+
+
+namespace ap
+{
+    
+static int nl80211Init(struct nl_sock* nl_sock, int* nl80211_id)
+{
+    int err;
+
+    if (!nl_sock) 
+    {
+            std::cerr<<"Failed to allocate netlink socket.\n";
+            return -ENOMEM;
+    }
+
+    if (genl_connect(nl_sock)) 
+    {
+            std::cerr<<"Failed to connect to generic netlink.\n";
+            err = -ENOLINK;
+            nl_socket_free(nl_sock);
+            return err;
+    }
+
+    nl_socket_set_buffer_size(nl_sock, 8192, 8192);
+
+    /* try to set NETLINK_EXT_ACK to 1, ignoring errors */
+    err = 1;
+    setsockopt(nl_socket_get_fd(nl_sock), SOL_NETLINK, NETLINK_EXT_ACK, &err, sizeof(err));
+
+    *nl80211_id = genl_ctrl_resolve(nl_sock, "nl80211");
+    if (*nl80211_id < 0)
+    {
+            std::cerr<<"nl80211 not found.\n";
+            err = -ENOENT;
+            nl_socket_free(nl_sock);
+            return err;
+    }
+
+    return 0;
+}
+
+static int errorHandler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg)
+{
+    printf("netlink error\n");
+    *reinterpret_cast<int*>(arg) = 0;
+    return NL_STOP;
+}
+
+static int finishHandler(struct nl_msg *msg, void *arg)
+{
+    *reinterpret_cast<int*>(arg) = 0;
+    return NL_SKIP;
+}
+
+static int ackHandler(struct nl_msg *msg, void *arg)
+{
+    *reinterpret_cast<int*>(arg) = 0;
+    return NL_STOP;
+}
+
+    
+static int extractDeviceMac(struct nl_msg *msg, void *arg)
+{
+    struct nlattr* tb[NL80211_ATTR_MAX + 1];
+    struct genlmsghdr* gnlh = (struct genlmsghdr*)nlmsg_data(nlmsg_hdr(msg));
+    
+    std::vector<uint64_t>* deviceMacAddrs = reinterpret_cast<std::vector<uint64_t>*>(arg);
+    
+    nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
+    if (!tb[NL80211_ATTR_STA_INFO]) 
+    {
+        std::cerr<<"sta stats missing!\n";
+        return -1;
+    }
+    
+    const unsigned char* macChar = reinterpret_cast<const unsigned char*>(nla_data(tb[NL80211_ATTR_MAC]));
+    uint64_t macAddr = 0;
+    memcpy(&macAddr, macChar, 6);
+    deviceMacAddrs->push_back(macAddr);
+    
+    return 0;
+}
+
+std::vector<uint64_t> connectedDevices(const std::string& ifDevice, int& error)
+{
+    struct nl_sock* nl_sock = nl_socket_alloc();
+    int nl80211_id = 0;
+    error = 0;
+    
+    if(nl80211Init(nl_sock, &nl80211_id)) 
+    {
+        std::cerr<<"Can not init nl80211\n";
+        error = ERR_INIT;
+    }
+    
+    signed long long devidx = if_nametoindex(ifDevice.c_str());
+    if(!devidx) 
+    {
+        std::cerr<<ifDevice<<" is not a valid if device\n";
+        error = ERR_NO_DEV;
+    }
+    
+    struct nl_msg *msg;
+    msg = nlmsg_alloc();
+    if(!msg) error = ERR_ALLOC;
+    
+    struct nl_cb *cb = nl_cb_alloc(NL_CB_DEFAULT);
+    struct nl_cb *s_cb = nl_cb_alloc(NL_CB_DEFAULT);
+    if (!cb || !s_cb)  error = ERR_ALLOC;
+    
+    std::vector<uint64_t> deviceMacAddrs;
+    
+    if(error == 0)
+    {
+        if(!genlmsg_put(msg, 0, 0, nl80211_id, 0, NLM_F_DUMP, NL80211_CMD_GET_STATION, 0))
+        {
+            std::cerr<<"genlmsg_put() failed\n";
+            error = ERR_GENERAL;
+        }
+        
+        if(error == 0)
+        {
+            NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, devidx);
+            
+            nl_socket_set_cb(nl_sock, s_cb);
+            nl_send_auto_complete(nl_sock, msg);
+            
+            {
+                int ret = 1;
+                
+                nl_cb_err(cb, NL_CB_CUSTOM, errorHandler, &ret);
+                nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finishHandler, &ret);
+                nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ackHandler, &ret);
+                nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, extractDeviceMac, &deviceMacAddrs);
+                
+                while (ret > 0) nl_recvmsgs(nl_sock, cb);
+            }
+        }
+    }
+    
+    nla_put_failure:
+    nl_cb_put(cb);
+    nl_cb_put(s_cb);
+    nlmsg_free(msg);
+    nl_socket_free(nl_sock);
+    return deviceMacAddrs;
+}
+
+std::string macAddrToString(uint64_t macAddr)
+{
+    unsigned char* macAddrPtr = reinterpret_cast<unsigned char*>(&macAddr);
+    std::stringstream ss;
+    ss<<std::uppercase<<std::setfill('0')<<std::setw(2)<<std::hex;
+    for(unsigned int i = 0; i < 6; ++i) ss<<+macAddrPtr[i]<<':';
+    std::string macString = ss.str();
+    macString.pop_back();
+    return macString;
+}
+
+}
diff --git a/src/apgetconnected.h b/src/apgetconnected.h
new file mode 100644
index 0000000..c08ea7a
--- /dev/null
+++ b/src/apgetconnected.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include<vector>
+#include<string>
+#include<stdint.h>
+
+namespace ap
+{
+    enum ERRORS
+    {
+        SUCESS,
+        ERR_INIT,
+        ERR_NO_DEV,
+        ERR_ALLOC,
+        ERR_GENERAL
+    };
+    std::vector<uint64_t> connectedDevices(const std::string& ifDevice, int& error);
+    std::string macAddrToString(uint64_t macAddr);
+}