c2-utopia/linux-receiver/main.c

290 lines
8.3 KiB
C
Raw Normal View History

2024-09-11 20:16:41 +02:00
/*
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!
2024-09-11 20:16:41 +02:00
*/
#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
2024-09-11 20:16:41 +02:00
// 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
2024-09-11 20:16:41 +02:00
#define MAX_PACKET_LEN 1000
// Maksimalno stevilo
#define ST_SPREJEMNIKOV 10
// Receiver MAC start at byte 52
#define WLAN_DA_OFFSET 52
2024-09-26 15:04:46 +02:00
/* ESP receiver MAC address */
uint8_t sprejemnikMac[] = { 0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };
2024-09-26 15:04:46 +02:00
/* linux wifi receiver MAC address */
//uint8_t sprejemnikMac[] = { 0x9c, 0xb6, 0xd0, 0xc4, 0xe8, 0xb9 };
uint8_t wlan_da[] = { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 };
2024-09-26 15:04:46 +02:00
// 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
2024-09-11 20:16:41 +02:00
// Sensor message
#include "../src/sensor_msg.h"
uint8_t odcitekId;
2024-09-26 15:04:46 +02:00
sensor_msg odcitki[ST_SPREJEMNIKOV];
2024-09-11 20:16:41 +02:00
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);
}
2024-09-11 20:16:41 +02:00
// ID senzorja
memcpy(&odcitekId, data + ESP_DATA_OFFSET, 1);
#ifdef DEBUG
printf("Odcitek ID: %i\n", odcitekId);
#endif
2024-09-11 20:16:41 +02:00
// 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;
size_t dolzina;
for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
if (poslji[i]) {
// Ustvarim bundle (svezenj)
svezenj = lo_bundle_new(LO_TT_IMMEDIATE);
2024-09-26 15:04:46 +02:00
sprintf(glava, "/ww/%d/bat", i);
m = lo_message_new();
lo_message_add_float(m, odcitki[i].bat);
lo_bundle_add_message(svezenj, glava, m);
2024-09-11 20:16:41 +02:00
// 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);
lo_bundle_add_message(svezenj, glava, m);
2024-09-26 15:04:46 +02:00
2024-09-11 20:16:41 +02:00
sprintf(glava, "/ww/%d/quat", i);
m = lo_message_new();
lo_message_add_float(m, odcitki[i].qW);
2024-09-26 15:04:46 +02:00
lo_message_add_float(m, odcitki[i].qX);
lo_message_add_float(m, odcitki[i].qY);
lo_message_add_float(m, odcitki[i].qZ);
2024-09-11 20:16:41 +02:00
lo_bundle_add_message(svezenj, glava, m);
#ifdef DEBUG
2024-09-11 20:16:41 +02:00
lo_bundle_pp(svezenj);
#endif
lo_send_bundle(osc_dest, svezenj);
2024-09-26 15:04:46 +02:00
lo_bundle_free_recursive(svezenj);
poslji[i] = false;
2024-09-11 20:16:41 +02:00
}
}
}
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 == 3);
2024-09-11 20:16:41 +02:00
uint8_t buff[MAX_PACKET_LEN] = {0};
int sock_fd;
char *dev = argv[1];
char *scport = argv[2];
struct sock_fprog bpf = {FILTER_LENGTH, bpfcode};
2024-09-11 20:16:41 +02:00
sock_fd = create_raw_socket(dev, &bpf); /* Creating the raw socket */
2024-09-11 20:16:41 +02:00
osc_dest = lo_address_new("localhost", scport);
2024-09-11 20:16:41 +02:00
printf("\n Waiting to receive packets ........ \n");
gettimeofday(&cas, NULL);
2024-09-11 20:16:41 +02:00
while (1) {
int len = recvfrom(sock_fd, buff, MAX_PACKET_LEN, MSG_TRUNC, NULL, 0);
2024-09-11 20:16:41 +02:00
if (len < 0) {
perror("Socket receive failed or error");
break;
} else {
//printf("len:%d\n", len);
print_packet(buff, len);
2024-09-11 20:16:41 +02:00
}
}
close(sock_fd);
return 0;
2024-09-11 20:16:41 +02:00
}