WIP sprejemnik
parent
d20c552ee7
commit
9948e344d5
|
|
@ -0,0 +1,7 @@
|
|||
test:
|
||||
mkdir -p bin
|
||||
gcc main.c -Wall -o bin/receiver -llo
|
||||
|
||||
|
||||
clean:
|
||||
rm -r bin
|
||||
Binary file not shown.
|
|
@ -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 <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"
|
||||
|
||||
#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 <sys/time.h>
|
||||
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;
|
||||
}
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
#!/bin/bash
|
||||
|
||||
sudo ./bin/receiver wlxd0aeec558360
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -0,0 +1,84 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#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
|
||||
|
||||
Loading…
Reference in New Issue