c2-utopia/linux-receiver/main.c

290 lines
8.3 KiB
C
Raw Blame History

This file contains invisible Unicode characters!

This file contains invisible Unicode characters that may be processed differently from what appears below. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to reveal hidden characters.

/*
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
/* ESP receiver MAC address */
uint8_t sprejemnikMac[] = { 0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };
/* linux wifi receiver 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;
size_t dolzina;
for (int i = 0; i < ST_SPREJEMNIKOV; i++) {
if (poslji[i]) {
// Ustvarim bundle (svezenj)
svezenj = lo_bundle_new(LO_TT_IMMEDIATE);
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);
// 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);
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].qX);
lo_message_add_float(m, odcitki[i].qY);
lo_message_add_float(m, odcitki[i].qZ);
lo_bundle_add_message(svezenj, glava, m);
#ifdef DEBUG
lo_bundle_pp(svezenj);
#endif
lo_send_bundle(osc_dest, svezenj);
lo_bundle_free_recursive(svezenj);
poslji[i] = false;
}
}
}
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);
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};
sock_fd = create_raw_socket(dev, &bpf); /* Creating the raw socket */
osc_dest = lo_address_new("localhost", scport);
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;
}