285 lines
8.1 KiB
C
285 lines
8.1 KiB
C
/*
|
||
|
||
Florenc Caminade
|
||
Thomas FLayols
|
||
Etienne Arlaud
|
||
Jurij Podgoršek
|
||
|
||
Receive raw 802.11 packet and filter ESP-NOW vendor specific action frame using BPF filters.
|
||
https://hackaday.io/project/161896
|
||
https://github.com/thomasfla/Linux-ESPNOW
|
||
|
||
Adapted from :
|
||
https://stackoverflow.com/questions/10824827/raw-sockets-communication-over-wifi-receiver-not-able-to-receive-packets
|
||
|
||
1/Find your wifi interface:
|
||
$ iwconfig
|
||
|
||
2/Setup your interface in monitor mode :
|
||
$ sudo ifconfig wlp5s0 down
|
||
$ sudo iwconfig wlp5s0 mode monitor
|
||
$ sudo ifconfig wlp5s0 up
|
||
|
||
3/Run this code as root or regular user!
|
||
*/
|
||
#include <stdint.h>
|
||
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include <unistd.h>
|
||
#include <sys/socket.h>
|
||
#include <sys/ioctl.h>
|
||
#include <linux/if_arp.h>
|
||
#include <arpa/inet.h>
|
||
#include <assert.h>
|
||
#include <linux/filter.h>
|
||
|
||
// Debug?
|
||
#define DEBUG
|
||
|
||
// Booleans
|
||
#include <stdbool.h>
|
||
|
||
// OSC
|
||
#include "lo/lo.h"
|
||
#include "lo/lo_lowlevel.h"
|
||
#include "lo/lo_osc_types.h"
|
||
|
||
// OSC destination; localhost supercollider running @ default port (57121)
|
||
// @TODO get this from argv?
|
||
lo_address osc_dest;
|
||
|
||
//Approximate
|
||
#define PACKET_LENGTH 400
|
||
#define MAX_PACKET_LEN 1000
|
||
|
||
// Maksimalno stevilo
|
||
#define ST_SPREJEMNIKOV 10
|
||
|
||
// Receiver MAC start at byte 52
|
||
#define WLAN_DA_OFFSET 52
|
||
/*our MAC address*/
|
||
uint8_t sprejemnikMac[] = { 0x9c, 0xb6, 0xd0, 0xc4, 0xe8, 0xb9 };
|
||
uint8_t wlan_da[] = { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 };
|
||
|
||
// ESPNOW packet identifier
|
||
#define ESP_ID_OFFSET 82
|
||
uint8_t esp_id[] = { 0x18, 0xfe, 0x34, 0x04 };
|
||
uint8_t pkg_header[] = { 0x0, 0x0, 0x0, 0x0 };
|
||
|
||
// ESPNOW data payload starts at byte 87
|
||
#define ESP_DATA_OFFSET 87
|
||
|
||
// Sensor message
|
||
#include "../src/sensor_msg.h"
|
||
|
||
uint8_t odcitekId;
|
||
sensor_msg odcitki[ST_SPREJEMNIKOV];
|
||
bool poslji[ST_SPREJEMNIKOV];
|
||
|
||
#include <sys/time.h>
|
||
struct timeval cas;
|
||
struct timeval zdaj;
|
||
int eps = 0;
|
||
|
||
/*ESP8266 host MAC address*/
|
||
//{0x84,0xF3,0xEB,0x73,0x55,0x0D};
|
||
|
||
|
||
//filter action frame packets
|
||
//Equivalent for tcp dump :
|
||
//type 0 subtype 0xd0 and wlan[24:4]=0x7f18fe34 and wlan[32]=221 and wlan[33:4]&0xffffff = 0x18fe34 and wlan[37]=0x4
|
||
//NB : There is no filter on source or destination addresses, so this code will 'receive' the action frames sent by this computer...
|
||
#define FILTER_LENGTH 20
|
||
static struct sock_filter bpfcode[FILTER_LENGTH] = {
|
||
{ 0x30, 0, 0, 0x00000003 }, // ldb [3] // radiotap header length : MS byte
|
||
{ 0x64, 0, 0, 0x00000008 }, // lsh #8 // left shift it
|
||
{ 0x7, 0, 0, 0x00000000 }, // tax // 'store' it in X register
|
||
{ 0x30, 0, 0, 0x00000002 }, // ldb [2] // radiotap header length : LS byte
|
||
{ 0x4c, 0, 0, 0x00000000 }, // or x // combine A & X to get radiotap header length in A
|
||
{ 0x7, 0, 0, 0x00000000 }, // tax // 'store' it in X
|
||
{ 0x50, 0, 0, 0x00000000 }, // ldb [x + 0] // right after radiotap header is the type and subtype
|
||
{ 0x54, 0, 0, 0x000000fc }, // and #0xfc // mask the interesting bits, a.k.a 0b1111 1100
|
||
{ 0x15, 0, 10, 0x000000d0 }, // jeq #0xd0 jt 9 jf 19 // compare the types (0) and subtypes (0xd)
|
||
{ 0x40, 0, 0, 0x00000018 }, // Ld [x + 24] // 24 bytes after radiotap header is the end of MAC header, so it is category and OUI (for action frame layer)
|
||
{ 0x15, 0, 8, 0x7f18fe34 }, // jeq #0x7f18fe34 jt 11 jf 19 // Compare with category = 127 (Vendor specific) and OUI 18:fe:34
|
||
{ 0x50, 0, 0, 0x00000020 }, // ldb [x + 32] // Begining of Vendor specific content + 4 ?random? bytes : element id
|
||
{ 0x15, 0, 6, 0x000000dd }, // jeq #0xdd jt 13 jf 19 // element id should be 221 (according to the doc)
|
||
{ 0x40, 0, 0, 0x00000021 }, // Ld [x + 33] // OUI (again!) on 3 LS bytes
|
||
{ 0x54, 0, 0, 0x00ffffff }, // and #0xffffff // Mask the 3 LS bytes
|
||
{ 0x15, 0, 3, 0x0018fe34 }, // jeq #0x18fe34 jt 16 jf 19 // Compare with OUI 18:fe:34
|
||
{ 0x50, 0, 0, 0x00000025 }, // ldb [x + 37] // Type
|
||
{ 0x15, 0, 1, 0x00000004 }, // jeq #0x4 jt 18 jf 19 // Compare type with type 0x4 (corresponding to ESP_NOW)
|
||
{ 0x6, 0, 0, 0x00040000 }, // ret #262144 // return 'True'
|
||
{ 0x6, 0, 0, 0x00000000 }, // ret #0 // return 'False'
|
||
};
|
||
|
||
void print_packet(uint8_t *data, int len) {
|
||
// Beri samo primerne pakete!
|
||
memcpy(&wlan_da, data + WLAN_DA_OFFSET, 6);
|
||
#ifdef DEBUG
|
||
printf("Dest MAC: %02x:%02x:%02x:%02x:%02x:%02x\n", wlan_da[0],wlan_da[1],wlan_da[2],wlan_da[3],wlan_da[4],wlan_da[5]);
|
||
#endif
|
||
|
||
/* DEBUG - print whole raw packet */
|
||
#ifdef DEBUG
|
||
for (int i = 0; i < len; i++) {
|
||
printf("0x%02x ", data[i]);
|
||
}
|
||
printf("\n");
|
||
#endif
|
||
|
||
// Ignoriraj pakete, ki so namenjeni drugam
|
||
for (int i = 0; i < 6; i++) {
|
||
if (wlan_da[i] != sprejemnikMac[i]) {
|
||
return;
|
||
}
|
||
}
|
||
|
||
// Ignoriraj pakete, ki niso ESP paketi!
|
||
memcpy(&pkg_header, data + ESP_ID_OFFSET, 4);
|
||
for (int i = 0; i < 4; i++) {
|
||
if (pkg_header[i] != esp_id[i]) {
|
||
return;
|
||
}
|
||
}
|
||
|
||
// Stetje paketov na sekundo
|
||
eps += 1;
|
||
gettimeofday(&zdaj, NULL);
|
||
|
||
if (zdaj.tv_sec != cas.tv_sec) {
|
||
printf("Paketov na sekundo: %i\n", eps);
|
||
eps = 0;
|
||
gettimeofday(&cas, NULL);
|
||
}
|
||
|
||
|
||
// ID senzorja
|
||
memcpy(&odcitekId, data + ESP_DATA_OFFSET, 1);
|
||
|
||
#ifdef DEBUG
|
||
printf("Odcitek ID: %i\n", odcitekId);
|
||
#endif
|
||
|
||
// Vrednosti
|
||
memcpy(&odcitki[odcitekId], data + ESP_DATA_OFFSET, sizeof(sensor_msg));
|
||
poslji[odcitekId] = true;
|
||
|
||
#ifdef DEBUG
|
||
printf("senzor %i\n", odcitekId);
|
||
printf("aX: %f \n", odcitki[odcitekId].aX);
|
||
printf("aY: %f \n", odcitki[odcitekId].aY);
|
||
printf("aZ: %f \n", odcitki[odcitekId].aZ);
|
||
printf("qW: %f \n", odcitki[odcitekId].qW);
|
||
printf("qX: %f \n", odcitki[odcitekId].qX);
|
||
printf("qY: %f \n", odcitki[odcitekId].qY);
|
||
printf("qZ: %f \n", odcitki[odcitekId].qZ);
|
||
printf("bat: %f \n", odcitki[odcitekId].bat);
|
||
#endif
|
||
|
||
|
||
// @TODO locen thread za posiljanje?
|
||
char glava[32];
|
||
lo_bundle svezenj;
|
||
lo_message m;
|
||
char* sporocilo;
|
||
size_t dolzina;
|
||
|
||
for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
|
||
if (poslji[i]) {
|
||
// Ustvarim bundle (svezenj)
|
||
svezenj = lo_bundle_new(LO_TT_IMMEDIATE);
|
||
|
||
// Dodamo sporocila
|
||
sprintf(glava, "/ww/%d/acc", i);
|
||
m = lo_message_new();
|
||
lo_message_add_float(m, odcitki[i].aX);
|
||
lo_message_add_float(m, odcitki[i].aY);
|
||
lo_message_add_float(m, odcitki[i].aZ);
|
||
|
||
//free(sporocilo);
|
||
|
||
lo_bundle_add_message(svezenj, glava, m);
|
||
sprintf(glava, "/ww/%d/quat", i);
|
||
m = lo_message_new();
|
||
lo_message_add_float(m, odcitki[i].qW);
|
||
lo_message_add_float(m, odcitki[i].aX);
|
||
lo_message_add_float(m, odcitki[i].aY);
|
||
lo_message_add_float(m, odcitki[i].aZ);
|
||
|
||
lo_bundle_add_message(svezenj, glava, m);
|
||
|
||
sporocilo = lo_bundle_serialise(svezenj, NULL, &dolzina);
|
||
lo_bundle_pp(svezenj);
|
||
lo_send_bundle(osc_dest, svezenj);
|
||
printf("%s\n", sporocilo);
|
||
lo_bundle_free(svezenj);
|
||
|
||
//printf("%s\n", glava);
|
||
//free(sporocilo);
|
||
}
|
||
}
|
||
}
|
||
|
||
int create_raw_socket(char *dev, struct sock_fprog *bpf)
|
||
{
|
||
struct sockaddr_ll sll;
|
||
struct ifreq ifr;
|
||
int fd, ifi, rb, attach_filter;
|
||
|
||
bzero(&sll, sizeof(sll));
|
||
bzero(&ifr, sizeof(ifr));
|
||
|
||
fd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
|
||
assert(fd != -1);
|
||
|
||
strncpy((char *)ifr.ifr_name, dev, IFNAMSIZ);
|
||
ifi = ioctl(fd, SIOCGIFINDEX, &ifr);
|
||
assert(ifi != -1);
|
||
|
||
sll.sll_protocol = htons(ETH_P_ALL);
|
||
sll.sll_family = PF_PACKET;
|
||
sll.sll_ifindex = ifr.ifr_ifindex;
|
||
sll.sll_pkttype = PACKET_OTHERHOST;
|
||
|
||
rb = bind(fd, (struct sockaddr *)&sll, sizeof(sll));
|
||
assert(rb != -1);
|
||
|
||
attach_filter = setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, bpf, sizeof(*bpf));
|
||
assert(attach_filter != -1);
|
||
|
||
return fd;
|
||
}
|
||
|
||
int main(int argc, char **argv) {
|
||
assert(argc == 2);
|
||
|
||
uint8_t buff[MAX_PACKET_LEN] = {0};
|
||
int sock_fd;
|
||
char *dev = argv[1];
|
||
struct sock_fprog bpf = {FILTER_LENGTH, bpfcode};
|
||
|
||
sock_fd = create_raw_socket(dev, &bpf); /* Creating the raw socket */
|
||
|
||
// @TODO get this from args?
|
||
osc_dest = lo_address_new("localhost", "57121");
|
||
|
||
printf("\n Waiting to receive packets ........ \n");
|
||
gettimeofday(&cas, NULL);
|
||
|
||
while (1) {
|
||
int len = recvfrom(sock_fd, buff, MAX_PACKET_LEN, MSG_TRUNC, NULL, 0);
|
||
|
||
if (len < 0) {
|
||
perror("Socket receive failed or error");
|
||
break;
|
||
} else {
|
||
//printf("len:%d\n", len);
|
||
print_packet(buff, len);
|
||
}
|
||
}
|
||
close(sock_fd);
|
||
return 0;
|
||
}
|