diff --git a/linux-receiver/Makefile b/linux-receiver/Makefile new file mode 100644 index 0000000..94da93c --- /dev/null +++ b/linux-receiver/Makefile @@ -0,0 +1,7 @@ +test: + mkdir -p bin + gcc main.c -Wall -o bin/receiver -llo + + +clean: + rm -r bin diff --git a/linux-receiver/bin/receiver b/linux-receiver/bin/receiver new file mode 100755 index 0000000..5e24fe2 Binary files /dev/null and b/linux-receiver/bin/receiver differ diff --git a/linux-receiver/main.c b/linux-receiver/main.c new file mode 100644 index 0000000..6a249c0 --- /dev/null +++ b/linux-receiver/main.c @@ -0,0 +1,253 @@ +/* + +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 +*/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Debug? +#define DEBUG + +// Booleans +#include + +// OSC +#include "lo/lo.h" +#include "lo/lo_lowlevel.h" +#include "lo/lo_osc_types.h" + +#define PACKET_LENGTH 400 //Approximate +#define MAX_PACKET_LEN 1000 + +// ESPNOW data payload starts at byte 60 +#define ESP_DATA_OFFSET 57 + +// Sensor message +#include "../src/sensor_msg.h" + +// Maksimalno stevilo +#define ST_SPREJEMNIKOV 10 + +uint8_t odcitekId; +sensor_msg odcitki[ST_SPREJEMNIKOV]; +bool poslji[ST_SPREJEMNIKOV]; + +#include +struct timeval cas; +struct timeval zdaj; +int eps = 0; + +/*our MAC address*/ +//{0xF8, 0x1A, 0x67, 0xB7, 0xeB, 0x0B}; + +/*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) { + /* + char macNaslov[18]; + snprintf(macNaslov, sizeof(macNaslov), "%02x:%02x:%02x:%02x:%02x:%02x", + mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]); + + printf("%s\n", macNaslov); + */ + //printf("Prejel podatke dolzine %i \n", len); + + // ID senzorja + odcitekId = (uint8_t) data[ESP_DATA_OFFSET]; + + // 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 + + + + + eps += 1; + gettimeofday(&zdaj, NULL); + + if (zdaj.tv_sec != cas.tv_sec) { + printf("Paketov na sekundo: %i\n", eps); + eps = 0; + gettimeofday(&cas, NULL); + } + return; + + // @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); + printf("%s\n", sporocilo); + lo_bundle_free(svezenj); + + //printf("%s\n", glava); + //free(sporocilo); + } + } + /* DEBUG - print whole raw packet + for (int i = 0; i < len; i++) { + printf("0x%02x ", data[i]); + } + printf("\n"); + */ +} + +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 */ + + 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; +} diff --git a/linux-receiver/pozeni.sh b/linux-receiver/pozeni.sh new file mode 100755 index 0000000..df49dc1 --- /dev/null +++ b/linux-receiver/pozeni.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +sudo ./bin/receiver wlxd0aeec558360 diff --git a/linux-receiver/prep.sh b/linux-receiver/prep.sh new file mode 100755 index 0000000..b8b775c --- /dev/null +++ b/linux-receiver/prep.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +dev="wlxd0aeec558360" +chan="1" +# sudo bash prep.sh *iface* *channel* +# sudo bash prep.sh wlp1s0 8 +ifconfig $dev down +iwconfig $dev mode monitor +ifconfig $dev up +iwconfig $dev channel $chan diff --git a/linux-receiver/slip.c b/linux-receiver/slip.c new file mode 100644 index 0000000..ea31304 --- /dev/null +++ b/linux-receiver/slip.c @@ -0,0 +1,198 @@ +#include "slip.h" + +/** + * @brief Internal SLIP character values. + **/ +typedef enum { + END = 0xC0, + ESC = 0xDB, + ESC_END = 0xDC, + ESC_ESC = 0xDD +} slip_char_t; + +/** + * @brief Internal SLIP encoder state. + **/ +typedef enum { + STATE_MESSAGE, + STATE_ERROR, + STATE_ESCAPE +} slip_state_t; + +/** + * @brief Initialise a SLIP encoder or decoder. + * + * @param[in] slip : Pointer to SLIP encoder or decoder. + * @param[in] buf : Buffer to hold encoded or decoded message. + * @param[in] len : Length of buffer + **/ +void SLIP_init(slip_t *slip, uint8_t *buf, size_t len, slip_encoding_t encoding) +{ + slip->buf = buf; + slip->len = len; + slip->state = STATE_MESSAGE; + slip->wp = 0; + slip->encoding = encoding; +} + +/** + * @brief Reset SLIP encoder or decoder. + * + * @param[in] slip : Pointer to SLIP encoder or decoder. + **/ +void SLIP_reset(slip_t *slip) +{ + slip->state = STATE_MESSAGE; + slip->wp = 0; +} + +/** + * @brief Gets the length of the encoded or decoded message so far. + * + * @param[in] slip : Pointer to SLIP encoder or decoder. + * + * @return Length of message. + */ +size_t SLIP_get_length(slip_t *slip) +{ + return slip->wp; +} + +/** + * @brief Consume byte for SLIP encoding. + * + * @param[in] slip : Pointer to SLIP encoder. + * @param[in] byte : Byte to be encoded. + * + * @return < 0 : Error + * = 0 : Encoded byte + * > 0 : Success/Finished (Length of encoded data) + **/ +static int SLIP_encode_byte(slip_t *slip, uint8_t byte) +{ + int remaining = slip->len - slip->wp; + int required = (byte == ESC || byte == END) ? 2 : 1; + + if (remaining < required) { + return -1; + } + + if (byte == ESC) + { + slip->buf[slip->wp++] = ESC; + slip->buf[slip->wp++] = ESC_ESC; + } + else if (byte == END) { + slip->buf[slip->wp++] = ESC; + slip->buf[slip->wp++] = ESC_END; + } + else { + slip->buf[slip->wp++] = byte; + } + + return 0; +} + +/** + * @brief SLIP encodes a given frame + * + * @param[in] slip : Pointer to SLIP encoder. + * @param[in] buf : Buffer containing frame. + * @param[in] len : Length of buffer + * + * @return > 0 : Success (Length of encoded data) + * = 0 : Failed + **/ +size_t SLIP_encode(slip_t *slip, const uint8_t *buf, size_t len) +{ + SLIP_reset(slip); + + if(slip->encoding == SLIP_ENCODING_DOUBLE_ENDED && slip->wp == 0) { + /* Start of message */ + /* Double encoded SLIP, where an END delimiter is also added to the beginning of the message */ + slip->buf[slip->wp++] = END; + } + + while(len--) { + if(SLIP_encode_byte(slip, *buf++) < 0) { + /* SLIP encoding failed */ + return 0; + } + } + + if((slip->len - slip->wp) < 1) { + /* No space for END delimiter */ + return 0; + } + + slip->buf[slip->wp++] = END; + return slip->wp; +} + +/** + * @brief Consume byte for SLIP decoding. + * + * @param[in] slip : Pointer to SLIP decoder. + * @param[in] byte : Byte to be decoded. + * + * @return < 0 : Error + * = 0 : Decoded byte + * > 0 : Success/Finished (Length of decoded data) + **/ +int SLIP_decode(slip_t *slip, uint8_t byte) +{ + int retVal = 0; + + if(byte == END) + { + if(slip->wp == 0) { + /* Double ended SLIP encoding. This indicatees the beginning of a SLIP message */ + retVal = 0; + } + else if(slip->wp > 0 && slip->state == STATE_MESSAGE) { + retVal = slip->wp; + } else { + retVal = -1; + } + SLIP_reset(slip); + return retVal; + } + + switch(slip->state) + { + case STATE_MESSAGE: + { + if(slip->wp == slip->len) { + /* ERROR: Ran out of buffer space */ + SLIP_reset(slip); + return -1; + } else if(byte == ESC) { + slip->state = STATE_ESCAPE; + } else { + slip->buf[slip->wp++] = byte; + } + break; + } + case STATE_ESCAPE: + { + if(byte == ESC_END) { + slip->buf[slip->wp++] = END; + slip->state = STATE_MESSAGE; + } else if(byte == ESC_ESC) { + slip->buf[slip->wp++] = ESC; + slip->state = STATE_MESSAGE; + } else { + /* ERROR: Wrong character received */ + SLIP_reset(slip); + return -1; + } + break; + } + case STATE_ERROR: { + SLIP_reset(slip); + return -1; + } + } + + return 0; +} \ No newline at end of file diff --git a/linux-receiver/slip.h b/linux-receiver/slip.h new file mode 100644 index 0000000..3752fb4 --- /dev/null +++ b/linux-receiver/slip.h @@ -0,0 +1,84 @@ +#pragma once + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Type of SLIP encoding to use. + */ +typedef enum { + SLIP_ENCODING_SINGLE_ENDED, /* END delimiter is only added to the end of the msg */ + SLIP_ENCODING_DOUBLE_ENDED /* END delimiter is added to the beginning and the end of the msg */ +} slip_encoding_t; + +/** + * @brief Private SLIP structure. + * @warning: Structure is not to be accessed directly by user. + */ +typedef struct { + uint8_t *buf; + size_t len; + size_t wp; + uint8_t state; + slip_encoding_t encoding; +} slip_t; + +/** + * @brief Initialise a SLIP encoder or decoder. + * + * @param[in] slip : Pointer to SLIP encoder or decoder. + * @param[in] buf : Buffer to hold encoded or decoded message. + * @param[in] len : Length of buffer + **/ +void SLIP_init(slip_t *slip, uint8_t *buf, size_t len, slip_encoding_t encoding); + +/** + * @brief Reset SLIP encoder or decoder. + * + * @param[in] slip : Pointer to SLIP encoder or decoder. + **/ +void SLIP_reset(slip_t *slip); + +/** + * @brief Gets the length of the encoded or decoded message so far. + * + * @param[in] slip : Pointer to SLIP encoder or decoder. + * + * @return Length of message. + */ +size_t SLIP_get_length(slip_t *slip); + +/** + * @brief SLIP encodes a given frame + * + * @param[in] slip : Pointer to SLIP encoder. + * @param[in] buf : Buffer containing frame. + * @param[in] len : Length of buffer + * + * @return > 0 : Success (Length of encoded data) + * = 0 : Failed + **/ +size_t SLIP_encode(slip_t *slip, const uint8_t *buf, size_t len); + +/** + * @brief Consume byte for SLIP decoding. + * + * @param[in] slip : Pointer to SLIP decoder. + * @param[in] byte : Byte to be decoded. + * + * @return < 0 : Error + * = 0 : Decoded byte + * > 0 : Success/Finished (Length of decoded data) + **/ +int SLIP_decode(slip_t *slip, uint8_t byte); + + + +#ifdef __cplusplus +} +#endif +