Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse code

Netlink driver and radiotap data reading added.

  • Loading branch information...
commit 2eb817875657e799450f9ec2ea20c825a1f8230e 1 parent a4cb2c9
Siim Meerits authored
55 src/ao_context.cpp
@@ -73,8 +73,10 @@ bool c_context::init(
73 73 m_rx_type = DRIVER_PCAP;
74 74 if (!g_strcmp0(conf_mode_rx, "file")) {
75 75 m_rx_mode = PCAP_MODE_FILE;
  76 + g_message("[sys] rx enabled: driver=pcap, mode=file, file=%s", conf_dev_rx);
76 77 } else if (!g_strcmp0(conf_mode_rx, "dev")) {
77 78 m_rx_mode = PCAP_MODE_DEV;
  79 + g_message("[sys] rx enabled: driver=pcap, mode=dev, device=%s", conf_dev_rx);
78 80 } else {
79 81 g_critical("[sys] driver pcap does not support rx mode %s, please use either \"file\" or \"dev\"!", conf_mode_rx);
80 82 goto err_drv_find;
@@ -86,7 +88,9 @@ bool c_context::init(
86 88 } else if (!g_strcmp0(conf_drv_rx, "lorcon")) {
87 89 #ifdef LORCON_FOUND
88 90 m_rx_type = DRIVER_LORCON;
89   - if (g_strcmp0(conf_mode_rx, "dev")) {
  91 + if (!g_strcmp0(conf_mode_rx, "dev")) {
  92 + g_message("[sys] rx enabled: driver=lorcon, mode=dev, device=%s", conf_dev_rx);
  93 + } else {
90 94 g_critical("[sys] driver lorcon does not support rx mode %s, please use \"dev\"!", conf_mode_rx);
91 95 goto err_drv_find;
92 96 }
@@ -97,7 +101,9 @@ bool c_context::init(
97 101 } else if (!g_strcmp0(conf_drv_rx, "netlink")) {
98 102 #ifdef NETLINK_FOUND
99 103 m_rx_type = DRIVER_NETLINK;
100   - if (g_strcmp0(conf_mode_rx, "dev")) {
  104 + if (!g_strcmp0(conf_mode_rx, "dev")) {
  105 + g_message("[sys] rx enabled: driver=netlink, mode=dev, device=%s", conf_dev_rx);
  106 + } else {
101 107 g_critical("[sys] driver netlink does not support rx mode %s, please use \"dev\"!", conf_mode_rx);
102 108 goto err_drv_find;
103 109 }
@@ -113,6 +119,7 @@ bool c_context::init(
113 119 m_tx_type = DRIVER_PCAP;
114 120 if (!g_strcmp0(conf_mode_tx, "file")) {
115 121 m_tx_mode = PCAP_MODE_FILE;
  122 + g_message("[sys] tx enabled: driver=pcap, mode=file, file=%s", conf_dev_tx);
116 123 } else {
117 124 g_critical("[sys] driver pcap does not support tx mode %s, please use \"file\"!", conf_mode_tx);
118 125 goto err_drv_find;
@@ -124,7 +131,9 @@ bool c_context::init(
124 131 } else if (!g_strcmp0(conf_drv_tx, "lorcon")) {
125 132 #ifdef LORCON_FOUND
126 133 m_tx_type = DRIVER_LORCON;
127   - if (g_strcmp0(conf_mode_tx, "dev")) {
  134 + if (!g_strcmp0(conf_mode_tx, "dev")) {
  135 + g_message("[sys] tx enabled: driver=lorcon, mode=dev, device=%s", conf_dev_tx);
  136 + } else {
128 137 g_critical("[sys] driver lorcon does not support tx mode %s, please use \"dev\"!", conf_mode_tx);
129 138 goto err_drv_find;
130 139 }
@@ -135,7 +144,9 @@ bool c_context::init(
135 144 } else if (!g_strcmp0(conf_drv_tx, "netlink")) {
136 145 #ifdef NETLINK_FOUND
137 146 m_tx_type = DRIVER_NETLINK;
138   - if (g_strcmp0(conf_mode_tx, "dev")) {
  147 + if (!g_strcmp0(conf_mode_tx, "dev")) {
  148 + g_message("[sys] tx enabled: driver=netlink, mode=dev, device=%s", conf_dev_tx);
  149 + } else {
139 150 g_critical("[sys] driver netlink does not support tx mode %s, please use \"dev\"!", conf_mode_tx);
140 151 goto err_drv_find;
141 152 }
@@ -198,7 +209,28 @@ bool c_context::init(
198 209
199 210 // Netlink
200 211 #ifdef NETLINK_FOUND
201   -
  212 + if (m_rx_type == DRIVER_NETLINK || m_tx_type == DRIVER_NETLINK) {
  213 + if (m_rx_type == DRIVER_NETLINK && m_tx_type == DRIVER_NETLINK && !g_strcmp0(conf_dev_rx, conf_dev_tx)) {
  214 + m_tx_drv = m_rx_drv = new c_drv_netlink(conf_dev_rx);
  215 + if (!m_rx_drv->init())
  216 + goto err_drv_init;
  217 + } else if (m_rx_type == DRIVER_NETLINK && m_tx_type == DRIVER_NETLINK) {
  218 + m_rx_drv = new c_drv_netlink(conf_dev_rx);
  219 + if (!m_rx_drv->init())
  220 + goto err_drv_init;
  221 + m_tx_drv = new c_drv_netlink(conf_dev_tx);
  222 + if (!m_tx_drv->init())
  223 + goto err_drv_init;
  224 + } else if (m_rx_type == DRIVER_NETLINK) {
  225 + m_rx_drv = new c_drv_netlink(conf_dev_rx);
  226 + if (!m_rx_drv->init())
  227 + goto err_drv_init;
  228 + } else if (m_tx_type == DRIVER_NETLINK) {
  229 + m_tx_drv = new c_drv_netlink(conf_dev_tx);
  230 + if (!m_tx_drv->init())
  231 + goto err_drv_init;
  232 + }
  233 + }
202 234 #endif
203 235
204 236 /*
@@ -212,6 +244,7 @@ bool c_context::init(
212 244 */
213 245
214 246 // Mainloop
  247 + m_kill = false;
215 248 m_mainloop = g_main_loop_new(NULL, FALSE);
216 249 g_idle_add(c_context::f_loop_idle, this);
217 250
@@ -247,6 +280,9 @@ void c_context::end()
247 280 // Check
248 281 g_assert(m_active);
249 282
  283 + // Mainloop
  284 + g_main_loop_unref(m_mainloop);
  285 +
250 286 // Driver
251 287 if (m_rx_drv && m_tx_drv && m_rx_drv == m_tx_drv) {
252 288 delete m_rx_drv;
@@ -282,8 +318,7 @@ void c_context::kill()
282 318 g_assert(m_active);
283 319
284 320 // Kill
285   - if (g_main_loop_is_running(m_mainloop))
286   - g_main_loop_quit(m_mainloop);
  321 + m_kill = true;
287 322 }
288 323
289 324 // Mainloop
@@ -296,10 +331,8 @@ gboolean c_context::f_loop_idle(gpointer data)
296 331 if (ctx->m_rx_drv) {
297 332 st_pck_drv* pck_drv = ctx->m_rx_drv->pck_rx();
298 333 if (pck_drv) {
299   - //g_message("[pck] <========================================================================>");
300 334 c_layer_hw* pck_hw = new c_layer_hw();
301 335 if (pck_hw->init_unpack(pck_drv)) {
302   - //pck_hw->str_dump();
303 336 ctx->m_pck_hw = g_list_append(ctx->m_pck_hw, pck_hw);
304 337 } else {
305 338 delete pck_hw;
@@ -307,6 +340,10 @@ gboolean c_context::f_loop_idle(gpointer data)
307 340 }
308 341 }
309 342
  343 + // Kill
  344 + if (ctx->m_kill)
  345 + g_main_loop_quit(ctx->m_mainloop);
  346 +
310 347 // Return
311 348 return TRUE;
312 349 }
1  src/ao_context.h
@@ -86,6 +86,7 @@ class c_context {
86 86 */
87 87
88 88 // Mainloop
  89 + bool m_kill;
89 90 GMainLoop* m_mainloop;
90 91 static gboolean f_loop_idle(gpointer data);
91 92 };
7 src/ao_main.cpp
@@ -30,13 +30,11 @@
30 30 // Instance
31 31 static c_context* ao_inst = NULL;
32 32
33   -// Command-line
34   -
35   -
36 33 // Signal
37 34 static void ao_signal(int sig)
38 35 {
39 36 if (sig == SIGINT) {
  37 + g_print("\n");
40 38 if (ao_inst)
41 39 ao_inst->kill();
42 40 } else if (sig == SIGWINCH) {
@@ -51,7 +49,7 @@ int main(int argc, char* argv[])
51 49 const gchar* cmd_drv_rx = "pcap";
52 50 const gchar* cmd_mode_rx = "file";
53 51 const gchar* cmd_dev_rx = "test-rx.pcap";
54   - const gchar* cmd_drv_tx = "pcap";
  52 + const gchar* cmd_drv_tx = "none";
55 53 const gchar* cmd_mode_tx = "file";
56 54 const gchar* cmd_dev_tx = "test-tx.pcap";
57 55 GOptionEntry cmd_entry_main[] = {
@@ -151,6 +149,7 @@ int main(int argc, char* argv[])
151 149 if (ao_inst->init(cmd_drv_rx, cmd_mode_rx, cmd_dev_rx, cmd_drv_tx, cmd_mode_tx, cmd_dev_tx)) {
152 150 ao_inst->run();
153 151 }
  152 + delete ao_inst;
154 153
155 154 // Return
156 155 return 0;
351 src/drv_nl.cpp
@@ -29,13 +29,18 @@
29 29 /*
30 30 #include <sys/types.h>
31 31 #include <asm/types.h>
32   -#include <netlink/genl/genl.h>
33 32 #include <netlink/genl/family.h>
34   -#include <netlink/genl/ctrl.h>
35 33 #include <netlink/msg.h>
36 34 #include <netlink/attr.h>
37 35 */
  36 +
  37 +#include <sys/socket.h>
  38 +#include <netpacket/packet.h>
  39 +#include <net/ethernet.h>
  40 +
38 41 #include <netlink/netlink.h>
  42 +#include <netlink/genl/genl.h>
  43 +#include <netlink/genl/ctrl.h>
39 44
40 45 // Constructor and destructor
41 46 c_drv_netlink::c_drv_netlink(const gchar* dev)
@@ -61,12 +66,165 @@ c_drv_netlink::~c_drv_netlink()
61 66 // Init and end
62 67 bool c_drv_netlink::init()
63 68 {
64   - return false;
  69 + // Check
  70 + g_assert(!m_active);
  71 +
  72 + // Variables
  73 + struct ifreq if_req;
  74 + struct sockaddr_ll sa_ll;
  75 + struct packet_mreq mr;
  76 + int optval;
  77 + socklen_t optlen;
  78 +
  79 + // Interface name
  80 + m_nl_if = m_info_dev;
  81 +
  82 + // Queues
  83 + m_queue_rx = g_async_queue_new_full(c_drv_netlink::f_delete_pck);
  84 + m_queue_tx = g_async_queue_new_full(c_drv_netlink::f_delete_pck);
  85 +
  86 + // Netlink
  87 + if (f_nl_connect() != true) {
  88 + goto err_nl_connect;
  89 + }
  90 +
  91 + // Socket
  92 + m_nl_fd = socket(PF_PACKET, SOCK_RAW, g_htons(ETH_P_ALL));
  93 + if (m_nl_fd < 0) {
  94 + g_critical("[drv] failed to create injection socket! error=%s", strerror(errno));
  95 + goto err_nl_fd;
  96 + }
  97 +
  98 + // Interface index
  99 + memset(&if_req, 0, sizeof(if_req));
  100 + g_strlcpy(if_req.ifr_name, m_nl_if, IFNAMSIZ);
  101 + if (ioctl(m_nl_fd, SIOCGIFINDEX, &if_req) < 0) {
  102 + g_critical("[drv] failed to get interface index! error=%s", strerror(errno));
  103 + goto err_nl_index;
  104 + }
  105 +
  106 + // Bind
  107 + memset(&sa_ll, 0, sizeof(sa_ll));
  108 + sa_ll.sll_family = AF_PACKET;
  109 + sa_ll.sll_protocol = g_htons(ETH_P_ALL);
  110 + sa_ll.sll_ifindex = if_req.ifr_ifindex;
  111 + if (bind(m_nl_fd, (struct sockaddr*) &sa_ll, sizeof(sa_ll)) != 0) {
  112 + g_critical("[drv] failed to bind injection socket! error=%s", strerror(errno));
  113 + goto err_nl_bind;
  114 + }
  115 +
  116 + // Check if device is up
  117 + optlen = sizeof(optval);
  118 + optval = 0;
  119 + if (getsockopt(m_nl_fd, SOL_SOCKET, SO_ERROR, &optval, &optlen) == -1) {
  120 + g_critical("[drv] failed to get device error status! error=%s", strerror(errno));
  121 + goto err_nl_opt;
  122 + }
  123 + if (optval == ENETDOWN) {
  124 + g_critical("[drv] device seems to be down!");
  125 + goto err_nl_opt;
  126 + } else if (optval > 0) {
  127 + g_critical("[drv] unknown device error! error=%s", strerror(errno));
  128 + goto err_nl_opt;
  129 + }
  130 +
  131 + // Priority option
  132 + optlen = sizeof(optval);
  133 + optval = 20;
  134 + if (setsockopt(m_nl_fd, SOL_SOCKET, SO_PRIORITY, &optval, optlen) == -1) {
  135 + g_critical("[drv] failed to set priority on socket! error=%s", strerror(errno));
  136 + goto err_nl_opt;
  137 + }
  138 +
  139 + // Promisc option
  140 + memset(&mr, 0, sizeof(mr));
  141 + mr.mr_ifindex = if_req.ifr_ifindex;
  142 + mr.mr_type = PACKET_MR_PROMISC;
  143 + if (setsockopt(m_nl_fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mr, sizeof(mr)) == -1) {
  144 + g_critical("[drv] failed to set interface in promisc mode! error=%s", strerror(errno));
  145 + goto err_nl_opt;
  146 + }
  147 +
  148 + // Buffer
  149 + m_nl_recv.iov_len = 64000;
  150 + m_nl_recv.iov_base = g_malloc(m_nl_recv.iov_len);
  151 +
  152 + // Thread
  153 + {
  154 + GError* err = NULL;
  155 + m_thr_run = true;
  156 + m_thr_dead = false;
  157 + m_thr_mutex = g_mutex_new();
  158 + m_thr_thread = g_thread_create(f_loop, this, TRUE, &err);
  159 + if (!m_thr_thread) {
  160 + if (err) {
  161 + g_critical("[drv] failed to create thread! error=%s", err->message);
  162 + g_clear_error(&err);
  163 + } else {
  164 + g_critical("[drv] failed to create thread! error=unknown");
  165 + }
  166 + goto err_thread;
  167 + }
  168 + }
  169 +
  170 + // Return
  171 + m_active = true;
  172 + return true;
  173 +
  174 + // Error
  175 + err_thread:
  176 + // Thread
  177 + g_mutex_free(m_thr_mutex);
  178 +
  179 + // Buffer
  180 + g_free(m_nl_recv.iov_base);
  181 +
  182 + err_nl_opt:
  183 + err_nl_bind:
  184 + err_nl_index:
  185 + // Socket
  186 + close(m_nl_fd);
  187 +
  188 + err_nl_fd:
  189 + // Disconnect
  190 + f_nl_disconnect();
  191 +
  192 + err_nl_connect:
  193 + // Queues
  194 + g_async_queue_unref(m_queue_rx);
  195 + g_async_queue_unref(m_queue_tx);
  196 +
  197 + // Return
  198 + return false;
65 199 }
66 200
67 201 void c_drv_netlink::end()
68 202 {
69   -
  203 + // Check
  204 + g_assert(m_active);
  205 +
  206 + // Thread
  207 + g_mutex_lock(m_thr_mutex);
  208 + m_thr_run = false;
  209 + g_mutex_unlock(m_thr_mutex);
  210 + g_thread_join(m_thr_thread);
  211 + g_mutex_free(m_thr_mutex);
  212 +
  213 + // Buffer
  214 + g_free(m_nl_recv.iov_base);
  215 +
  216 + // Socket
  217 + close(m_nl_fd);
  218 +
  219 + // Disconnect
  220 + f_nl_disconnect();
  221 +
  222 + // Queues
  223 + g_async_queue_unref(m_queue_rx);
  224 + g_async_queue_unref(m_queue_tx);
  225 +
  226 + // Deactive
  227 + m_active = false;
70 228 }
71 229
72 230 // Output
@@ -78,18 +236,199 @@ void c_drv_netlink::help()
78 236
79 237 const gchar* c_drv_netlink::name()
80 238 {
81   - return NULL;
  239 + // Check
  240 + g_assert(m_active);
  241 +
  242 + // Return
  243 + return m_nl_if;
82 244 }
83 245
84 246 // Packets
85 247 st_pck_drv* c_drv_netlink::pck_rx()
86 248 {
87   - return NULL;
  249 + // Check
  250 + g_assert(m_active);
  251 +
  252 + // Pop data
  253 + return (st_pck_drv*) g_async_queue_try_pop(m_queue_rx);
88 254 }
89 255
90 256 void c_drv_netlink::pck_tx(st_pck_drv* data)
91 257 {
  258 + // Check
  259 + g_assert(m_active);
  260 +
  261 + // Push data
  262 + g_async_queue_push(m_queue_tx, data);
  263 +}
  264 +
  265 +// Loop functions
  266 +gpointer c_drv_netlink::f_loop(gpointer user)
  267 +{
  268 + // Instance
  269 + c_drv_netlink* ctx = (c_drv_netlink*) user;
  270 +
  271 + // Packet loop
  272 + while (true) {
  273 + // Exit check
  274 + g_mutex_lock(ctx->m_thr_mutex);
  275 + if (!ctx->m_thr_run) {
  276 + ctx->m_thr_dead = true;
  277 + g_mutex_unlock(ctx->m_thr_mutex);
  278 + return NULL;
  279 + }
  280 + g_mutex_unlock(ctx->m_thr_mutex);
  281 +
  282 + // Packet loop
  283 + bool ok = true;
  284 + while (ok) {
  285 + // Ok
  286 + ok = false;
  287 +
  288 + // Receive packet
  289 + //guint8 rx_ctmp[128];
  290 + struct msghdr rx_msg;
  291 + rx_msg.msg_name = NULL;
  292 + rx_msg.msg_namelen = 0;
  293 + rx_msg.msg_iov = &ctx->m_nl_recv;
  294 + rx_msg.msg_iovlen = 1;
  295 + rx_msg.msg_control = NULL; //&rx_ctmp;
  296 + rx_msg.msg_controllen = 0; //sizeof(rx_ctmp);
  297 + rx_msg.msg_flags = 0;
  298 + ssize_t rx_size = recvmsg(ctx->m_nl_fd, &rx_msg, MSG_DONTWAIT);
  299 + if (rx_size > 0) {
  300 + // Layer
  301 + /*
  302 + struct cmsghdr* rx_cmsg = CMSG_FIRSTHDR(&rx_msg);
  303 + if (rx_cmsg) {
  304 + g_message("[drv] got packet! level=%d", rx_cmsg->cmsg_level);
  305 + }
  306 + */
  307 +
  308 + // Packet
  309 + st_pck_drv* pck_drv = g_new(st_pck_drv, 1);
  310 + pck_drv->data = g_byte_array_new();
  311 + pck_drv->type = LAYER_RADIOTAP_PCK;
  312 + pck_drv->driver = ctx;
  313 +
  314 + // Data
  315 + g_byte_array_append(pck_drv->data, (guint8*) rx_msg.msg_iov[0].iov_base, rx_size);
  316 +
  317 + // Queue
  318 + g_async_queue_push(ctx->m_queue_rx, pck_drv);
  319 +
  320 + // Ok
  321 + ok = true;
  322 + } else if (errno != EAGAIN) {
  323 + // Error
  324 + g_critical("[drv] error reading packet! error=%s", strerror(errno));
  325 +
  326 + // Exit loop
  327 + g_mutex_lock(ctx->m_thr_mutex);
  328 + ctx->m_thr_dead = true;
  329 + g_mutex_unlock(ctx->m_thr_mutex);
  330 + return NULL;
  331 + }
  332 +
  333 + // Transmit packet
  334 + st_pck_drv* pck_drv = (st_pck_drv*) g_async_queue_try_pop(ctx->m_queue_tx);
  335 + if (pck_drv) {
  336 + // Message
  337 + struct iovec tx_iov;
  338 + tx_iov.iov_base = pck_drv->data->data;
  339 + tx_iov.iov_len = pck_drv->data->len;
  340 + struct msghdr tx_msg;
  341 + tx_msg.msg_name = NULL;
  342 + tx_msg.msg_namelen = 0;
  343 + tx_msg.msg_iovlen = 1;
  344 + tx_msg.msg_iov = &tx_iov;
  345 + tx_msg.msg_control = NULL;
  346 + tx_msg.msg_controllen = 0;
  347 + tx_msg.msg_flags = 0;
  348 +
  349 + // Send
  350 + ssize_t tx_size = sendmsg(ctx->m_nl_fd, &tx_msg, 0);
  351 + if (tx_size < 0) {
  352 + // Error
  353 + g_critical("[drv] error sending packet! error=%s", strerror(errno));
  354 +
  355 + // Exit loop
  356 + g_mutex_lock(ctx->m_thr_mutex);
  357 + ctx->m_thr_dead = true;
  358 + g_mutex_unlock(ctx->m_thr_mutex);
  359 + return NULL;
  360 + }
  361 +
  362 + // Delete
  363 + g_byte_array_unref(pck_drv->data);
  364 + g_free(pck_drv);
  365 +
  366 + // Ok
  367 + ok = true;
  368 + }
  369 + }
  370 +
  371 + }
  372 +
  373 + // Return
  374 + return NULL;
  375 +}
  376 +
  377 +void c_drv_netlink::f_delete_pck(gpointer data)
  378 +{
  379 + // Free packet
  380 + st_pck_drv* pck = (st_pck_drv*) data;
  381 + g_byte_array_unref(pck->data);
  382 + g_free(pck);
  383 +}
  384 +
  385 +// Netlink functions
  386 +bool c_drv_netlink::f_nl_connect()
  387 +{
  388 + // Netlink handle
  389 + if ((m_nl_handle = nl_handle_alloc()) == NULL) {
  390 + g_critical("[drv] failed to allocate nl_handle!");
  391 + goto err_handle;
  392 + }
92 393
  394 + if (genl_connect(m_nl_handle)) {
  395 + g_critical("[drv] failed to connect to generic netlink!");
  396 + goto err_connect;
  397 + }
  398 +
  399 + m_nl_cache = genl_ctrl_alloc_cache(m_nl_handle);
  400 + if (m_nl_cache == NULL) {
  401 + g_critical("[drv] failed to allocate generic netlink cache!");
  402 + goto err_cache;
  403 + }
  404 +
  405 + if ((m_nl_family = genl_ctrl_search_by_name(m_nl_cache, "nl80211")) == NULL) {
  406 + g_critical("[drv] failed to find nl80211 controls, kernel may be too old!");
  407 + goto err_family;
  408 + }
  409 +
  410 + // Return
  411 + return true;
  412 +
  413 + // Error
  414 + err_family:
  415 + // Cache
  416 + nl_cache_free(m_nl_cache);
  417 +
  418 + err_cache:
  419 + err_connect:
  420 + // Handle
  421 + nl_handle_destroy(m_nl_handle);
  422 +
  423 + err_handle:
  424 + // Return
  425 + return false;
  426 +}
  427 +
  428 +void c_drv_netlink::f_nl_disconnect()
  429 +{
  430 + // Netlink handle
  431 + nl_handle_destroy(m_nl_handle);
93 432 }
94 433
95 434 #endif
12 src/drv_nl.h
@@ -56,12 +56,16 @@ class c_drv_netlink : public c_drv {
56 56 gchar* m_info_dev;
57 57
58 58 // Netlink
59   -
  59 + gchar* m_nl_if;
  60 + gint m_nl_fd;
  61 + struct iovec m_nl_recv;
  62 + struct nl_handle* m_nl_handle;
  63 + struct nl_cache* m_nl_cache;
  64 + struct genl_family* m_nl_family;
60 65
61 66 // Thread
62 67 GThread* m_thr_thread;
63 68 GMutex* m_thr_mutex;
64   - GCond* m_thr_cond;
65 69 bool m_thr_run;
66 70 bool m_thr_dead;
67 71
@@ -72,6 +76,10 @@ class c_drv_netlink : public c_drv {
72 76 // Loop functions
73 77 static gpointer f_loop(gpointer user);
74 78 static void f_delete_pck(gpointer data);
  79 +
  80 + // Netlink functions
  81 + bool f_nl_connect();
  82 + void f_nl_disconnect();
75 83 };
76 84
77 85 #endif // NETLINK_FOUND
151 src/pck_radiotap.cpp
@@ -62,6 +62,122 @@ bool c_layer_radiotap::init_unpack(GNode* node, GByteArray* data)
62 62 if (m_hdr.it_version != 0)
63 63 return false;
64 64
  65 + // Extended presence
  66 + m_ext_val[0] = m_hdr.it_present;
  67 + for (guint i=1; i<m_ext_num; i++) {
  68 + if (m_ext_val[i - 1] & (1 << 31)) {
  69 + if (optlen < (gint)sizeof(guint32))
  70 + return false;
  71 + m_ext_val[i] = *((guint32*)data_cur);
  72 + data_cur += sizeof(guint32);
  73 + optlen -= sizeof(guint32);
  74 + } else {
  75 + m_ext_val[i] = 0;
  76 + break;
  77 + }
  78 + }
  79 +
  80 + // Options
  81 + m_flags_set = false;
  82 + m_rate_set = false;
  83 + m_chan_set = false;
  84 + m_fhss_set = false;
  85 + m_ant_noise_set = false;
  86 + m_ant_signal_db_set = false;
  87 + m_txflags_set = false;
  88 + for (guint i=0; i<m_ext_num; i++) {
  89 + // Scan bits
  90 + for (guint b=0; b<31; b++) {
  91 + // Check
  92 + if (!(m_ext_val[i] & (1 << b)))
  93 + continue;
  94 +
  95 + // Select field
  96 + guint id = (i * 30) + b + 1;
  97 + switch (id) {
  98 + case 1:
  99 + // Flags
  100 + if (optlen < (gint)sizeof(m_flags_val))
  101 + goto opt_continue;
  102 + m_flags_set = true;
  103 + m_flags_val = *data_cur;
  104 + data_cur += sizeof(m_flags_val);
  105 + optlen -= sizeof(m_flags_val);
  106 + break;
  107 +
  108 + case 2:
  109 + // Rate
  110 + if (optlen < (gint)sizeof(m_rate_val))
  111 + goto opt_continue;
  112 + m_rate_set = true;
  113 + m_rate_val = *data_cur;
  114 + data_cur += sizeof(m_rate_val);
  115 + optlen -= sizeof(m_rate_val);
  116 + break;
  117 +
  118 + case 3:
  119 + // Channel
  120 + if (optlen < (gint)sizeof(m_chan_val))
  121 + goto opt_continue;
  122 + m_chan_set = true;
  123 + g_memmove(&m_chan_val, data_cur, sizeof(m_chan_val));
  124 + data_cur += sizeof(m_chan_val);
  125 + optlen -= sizeof(m_chan_val);
  126 + break;
  127 +
  128 + case 4:
  129 + // FHSS
  130 + if (optlen < (gint)sizeof(m_fhss_val))
  131 + goto opt_continue;
  132 + m_fhss_set = true;
  133 + g_memmove(&m_fhss_val, data_cur, sizeof(m_fhss_val));
  134 + data_cur += sizeof(m_fhss_val);
  135 + optlen -= sizeof(m_fhss_val);
  136 + break;
  137 +
  138 + case 6:
  139 + // Antenna noise
  140 + if (optlen < (gint)sizeof(m_ant_noise_val))
  141 + goto opt_continue;
  142 + m_ant_noise_set = true;
  143 + m_ant_noise_val = (gint8) *data_cur;
  144 + data_cur += sizeof(m_ant_noise_val);
  145 + optlen -= sizeof(m_ant_noise_val);
  146 + break;
  147 +
  148 + case 12:
  149 + // Antenna noise
  150 + if (optlen < (gint)sizeof(m_ant_signal_db_val))
  151 + goto opt_continue;
  152 + m_ant_signal_db_set = true;
  153 + m_ant_signal_db_val = *data_cur;
  154 + data_cur += sizeof(m_ant_signal_db_val);
  155 + optlen -= sizeof(m_ant_signal_db_val);
  156 + break;
  157 +
  158 + case 15:
  159 + // TX flags
  160 + if (optlen < (gint)sizeof(m_txflags_val))
  161 + goto opt_continue;
  162 + m_txflags_set = true;
  163 + g_memmove(&m_txflags_val, data_cur, sizeof(m_txflags_val));
  164 + data_cur += sizeof(m_txflags_val);
  165 + optlen -= sizeof(m_txflags_val);
  166 + break;
  167 +
  168 + default:
  169 + // Unknown op
  170 + g_message("[pck] unhandled=%u, optlen=%d", id, optlen);
  171 + goto opt_continue;
  172 + }
  173 + }
  174 +
  175 + // End of flag variables
  176 + if (!(m_ext_val[i] & (1 << 31)))
  177 + break;
  178 + }
  179 + opt_continue:
  180 +
65 181 // Skip options
66 182 data_cur += optlen;
67 183
@@ -102,6 +218,41 @@ void c_layer_radiotap::str_dump()
102 218 {
103 219 // Check
104 220 g_assert(m_active);
  221 +
  222 + // Message
  223 + g_message(
  224 + "[pck] layer-radiotap: version=%u, len=%u, flags=%08x",
  225 + m_hdr.it_version, m_hdr.it_len, m_hdr.it_present
  226 + );
  227 + if (m_rate_set)
  228 + g_message("[pck] * rate=[%.2f Mbps]", (gdouble)m_rate_val * 0.5);
  229 + if (m_chan_set)
  230 + g_message(
  231 + "[pck] * freq=[%.3f GHz], turbo=%s, cck=%s, ofdm=%s, 2ghz=%s, 5ghz=%s, passive=%s",
  232 + (gdouble)m_chan_val.freq / 1000.0,
  233 + m_chan_val.flags & 0x0010 ? "true" : "false",
  234 + m_chan_val.flags & 0x0020 ? "true" : "false",
  235 + m_chan_val.flags & 0x0040 ? "true" : "false",
  236 + m_chan_val.flags & 0x0080 ? "true" : "false",
  237 + m_chan_val.flags & 0x0100 ? "true" : "false",
  238 + m_chan_val.flags & 0x0200 ? "true" : "false"
  239 + );
  240 + if (m_fhss_set)
  241 + g_message("[pck] * hop_set=%u, hop_pattern=%u", m_fhss_val.hop_set, m_fhss_val.hop_pattern);
  242 + if (m_ant_noise_set)
  243 + g_message("[pck] * ant_noise=[%d dBm]", m_ant_noise_val);
  244 + if (m_ant_signal_db_set)
  245 + g_message("[pck] * ant_signal=[%u dB]", m_ant_signal_db_val);
  246 + if (m_txflags_set) {
  247 + g_message(
  248 + "[pck] * retry_fail=%s, cts_to_self=%s, rts_cts_used=%s, no_ack_expected=%s, has_seqno=%s",
  249 + m_txflags_val & 0x0001 ? "true" : "false",
  250 + m_txflags_val & 0x0002 ? "true" : "false",
  251 + m_txflags_val & 0x0004 ? "true" : "false",
  252 + m_txflags_val & 0x0008 ? "true" : "false",
  253 + m_txflags_val & 0x0010 ? "true" : "false"
  254 + );
  255 + }
105 256 }
106 257
107 258 const gchar* c_layer_radiotap::str_name()
36 src/pck_radiotap.h
@@ -58,6 +58,42 @@ class c_layer_radiotap : public c_layer {
58 58 private:
59 59 // Headers
60 60 struct radiotap_header m_hdr;
  61 + static const guint m_ext_num = 5;
  62 + guint32 m_ext_val[m_ext_num];
  63 +
  64 + // Flags
  65 + bool m_flags_set;
  66 + guint8 m_flags_val;
  67 +
  68 + // Rate
  69 + bool m_rate_set;
  70 + guint8 m_rate_val;
  71 +
  72 + // Channel
  73 + bool m_chan_set;
  74 + struct {
  75 + guint16 freq;
  76 + guint16 flags;
  77 + } __attribute__((__packed__)) m_chan_val;
  78 +
  79 + // FHSS
  80 + bool m_fhss_set;
  81 + struct {
  82 + guint8 hop_set;
  83 + guint8 hop_pattern;
  84 + } __attribute__((__packed__)) m_fhss_val;
  85 +
  86 + // Antenna noise
  87 + bool m_ant_noise_set;
  88 + gint8 m_ant_noise_val;
  89 +
  90 + // Antenna signal dB
  91 + bool m_ant_signal_db_set;
  92 + guint8 m_ant_signal_db_val;
  93 +
  94 + // TX flags
  95 + bool m_txflags_set;
  96 + guint16 m_txflags_val;
61 97
62 98 // Endpoints
63 99 st_ep m_ep;

0 comments on commit 2eb8178

Please sign in to comment.
Something went wrong with that request. Please try again.