mirror of
https://github.com/ghsecuritylab/ameba_ws2812b.git
synced 2024-11-29 17:50:31 +00:00
334 lines
7 KiB
C
334 lines
7 KiB
C
|
#include "FreeRTOS.h"
|
||
|
|
#include "task.h"
|
||
|
|
#include "main.h"
|
||
|
|
#include "tcpip.h"
|
||
|
|
#include "wifi/wifi_conf.h"
|
||
|
|
|
||
|
|
#ifndef CONFIG_WLAN
|
||
|
|
#define CONFIG_WLAN 1
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#if CONFIG_WLAN
|
||
|
|
#include <platform/platform_stdlib.h>
|
||
|
|
|
||
|
|
struct eth_frame {
|
||
|
|
struct eth_frame *prev;
|
||
|
|
struct eth_frame *next;
|
||
|
|
unsigned char da[6];
|
||
|
|
unsigned char sa[6];
|
||
|
|
unsigned int len;
|
||
|
|
unsigned char type;
|
||
|
|
};
|
||
|
|
|
||
|
|
struct eth_buffer {
|
||
|
|
struct eth_frame *head;
|
||
|
|
struct eth_frame *tail;
|
||
|
|
};
|
||
|
|
|
||
|
|
static struct eth_buffer eth_buffer;
|
||
|
|
|
||
|
|
#ifdef CONFIG_PROMISC
|
||
|
|
#define MAX_PACKET_FILTER_INFO 5
|
||
|
|
#define FILTER_ID_INIT_VALUE 10
|
||
|
|
rtw_packet_filter_info_t paff_array[MAX_PACKET_FILTER_INFO]={0, 0, 0, 0, 0};
|
||
|
|
static u8 packet_filter_enable_num = 0;
|
||
|
|
|
||
|
|
void promisc_init_packet_filter()
|
||
|
|
{
|
||
|
|
int i = 0;
|
||
|
|
for(i=0; i<MAX_PACKET_FILTER_INFO; i++){
|
||
|
|
paff_array[i].filter_id = FILTER_ID_INIT_VALUE;
|
||
|
|
paff_array[i].enable = 0;
|
||
|
|
paff_array[i].patt.mask_size = 0;
|
||
|
|
paff_array[i].rule = RTW_POSITIVE_MATCHING;
|
||
|
|
paff_array[i].patt.mask = NULL;
|
||
|
|
paff_array[i].patt.pattern = NULL;
|
||
|
|
}
|
||
|
|
packet_filter_enable_num = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int promisc_add_packet_filter(u8 filter_id, rtw_packet_filter_pattern_t *patt, rtw_packet_filter_rule_e rule)
|
||
|
|
{
|
||
|
|
int i = 0;
|
||
|
|
while(i < MAX_PACKET_FILTER_INFO){
|
||
|
|
if(paff_array[i].filter_id == FILTER_ID_INIT_VALUE){
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
i++;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(i == MAX_PACKET_FILTER_INFO)
|
||
|
|
return -1;
|
||
|
|
|
||
|
|
paff_array[i].filter_id = filter_id;
|
||
|
|
|
||
|
|
paff_array[i].patt.offset= patt->offset;
|
||
|
|
paff_array[i].patt.mask_size = patt->mask_size;
|
||
|
|
paff_array[i].patt.mask = pvPortMalloc(patt->mask_size);
|
||
|
|
memcpy(paff_array[i].patt.mask, patt->mask, patt->mask_size);
|
||
|
|
paff_array[i].patt.pattern= pvPortMalloc(patt->mask_size);
|
||
|
|
memcpy(paff_array[i].patt.pattern, patt->pattern, patt->mask_size);
|
||
|
|
|
||
|
|
paff_array[i].rule = rule;
|
||
|
|
paff_array[i].enable = 0;
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int promisc_enable_packet_filter(u8 filter_id)
|
||
|
|
{
|
||
|
|
int i = 0;
|
||
|
|
while(i < MAX_PACKET_FILTER_INFO){
|
||
|
|
if(paff_array[i].filter_id == filter_id)
|
||
|
|
break;
|
||
|
|
i++;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(i == MAX_PACKET_FILTER_INFO)
|
||
|
|
return -1;
|
||
|
|
|
||
|
|
paff_array[i].enable = 1;
|
||
|
|
packet_filter_enable_num++;
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int promisc_disable_packet_filter(u8 filter_id)
|
||
|
|
{
|
||
|
|
int i = 0;
|
||
|
|
while(i < MAX_PACKET_FILTER_INFO){
|
||
|
|
if(paff_array[i].filter_id == filter_id)
|
||
|
|
break;
|
||
|
|
i++;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(i == MAX_PACKET_FILTER_INFO)
|
||
|
|
return -1;
|
||
|
|
|
||
|
|
paff_array[i].enable = 0;
|
||
|
|
packet_filter_enable_num--;
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int promisc_remove_packet_filter(u8 filter_id)
|
||
|
|
{
|
||
|
|
int i = 0;
|
||
|
|
while(i < MAX_PACKET_FILTER_INFO){
|
||
|
|
if(paff_array[i].filter_id == filter_id)
|
||
|
|
break;
|
||
|
|
i++;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(i == MAX_PACKET_FILTER_INFO)
|
||
|
|
return -1;
|
||
|
|
|
||
|
|
paff_array[i].filter_id = FILTER_ID_INIT_VALUE;
|
||
|
|
paff_array[i].enable = 0;
|
||
|
|
paff_array[i].patt.mask_size = 0;
|
||
|
|
paff_array[i].rule = 0;
|
||
|
|
if(paff_array[i].patt.mask){
|
||
|
|
vPortFree(paff_array[i].patt.mask);
|
||
|
|
paff_array[i].patt.mask = NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(paff_array[i].patt.pattern){
|
||
|
|
vPortFree(paff_array[i].patt.pattern);
|
||
|
|
paff_array[i].patt.pattern = NULL;
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/* Make callback simple to prevent latency to wlan rx when promiscuous mode */
|
||
|
|
static void promisc_callback(unsigned char *buf, unsigned int len, void* userdata)
|
||
|
|
{
|
||
|
|
struct eth_frame *frame = (struct eth_frame *) pvPortMalloc(sizeof(struct eth_frame));
|
||
|
|
|
||
|
|
if(frame) {
|
||
|
|
frame->prev = NULL;
|
||
|
|
frame->next = NULL;
|
||
|
|
memcpy(frame->da, buf, 6);
|
||
|
|
memcpy(frame->sa, buf+6, 6);
|
||
|
|
frame->len = len;
|
||
|
|
|
||
|
|
taskENTER_CRITICAL();
|
||
|
|
|
||
|
|
if(eth_buffer.tail) {
|
||
|
|
eth_buffer.tail->next = frame;
|
||
|
|
frame->prev = eth_buffer.tail;
|
||
|
|
eth_buffer.tail = frame;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
eth_buffer.head = frame;
|
||
|
|
eth_buffer.tail = frame;
|
||
|
|
}
|
||
|
|
|
||
|
|
taskEXIT_CRITICAL();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
struct eth_frame* retrieve_frame(void)
|
||
|
|
{
|
||
|
|
struct eth_frame *frame = NULL;
|
||
|
|
|
||
|
|
taskENTER_CRITICAL();
|
||
|
|
|
||
|
|
if(eth_buffer.head) {
|
||
|
|
frame = eth_buffer.head;
|
||
|
|
|
||
|
|
if(eth_buffer.head->next) {
|
||
|
|
eth_buffer.head = eth_buffer.head->next;
|
||
|
|
eth_buffer.head->prev = NULL;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
eth_buffer.head = NULL;
|
||
|
|
eth_buffer.tail = NULL;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
taskEXIT_CRITICAL();
|
||
|
|
|
||
|
|
return frame;
|
||
|
|
}
|
||
|
|
|
||
|
|
static void promisc_test(int duration, unsigned char len_used)
|
||
|
|
{
|
||
|
|
int ch;
|
||
|
|
unsigned int start_time;
|
||
|
|
struct eth_frame *frame;
|
||
|
|
eth_buffer.head = NULL;
|
||
|
|
eth_buffer.tail = NULL;
|
||
|
|
|
||
|
|
wifi_enter_promisc_mode();
|
||
|
|
wifi_set_promisc(RTW_PROMISC_ENABLE, promisc_callback, len_used);
|
||
|
|
|
||
|
|
for(ch = 1; ch <= 13; ch ++) {
|
||
|
|
if(wifi_set_channel(ch) == 0)
|
||
|
|
printf("\n\n\rSwitch to channel(%d)", ch);
|
||
|
|
|
||
|
|
start_time = xTaskGetTickCount();
|
||
|
|
|
||
|
|
while(1) {
|
||
|
|
unsigned int current_time = xTaskGetTickCount();
|
||
|
|
|
||
|
|
if((current_time - start_time) < (duration * configTICK_RATE_HZ)) {
|
||
|
|
frame = retrieve_frame();
|
||
|
|
|
||
|
|
if(frame) {
|
||
|
|
int i;
|
||
|
|
printf("\n\rDA:");
|
||
|
|
for(i = 0; i < 6; i ++)
|
||
|
|
printf(" %02x", frame->da[i]);
|
||
|
|
printf(", SA:");
|
||
|
|
for(i = 0; i < 6; i ++)
|
||
|
|
printf(" %02x", frame->sa[i]);
|
||
|
|
printf(", len=%d", frame->len);
|
||
|
|
|
||
|
|
vPortFree((void *) frame);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
vTaskDelay(1); //delay 1 tick
|
||
|
|
}
|
||
|
|
else
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
wifi_set_promisc(RTW_PROMISC_DISABLE, NULL, 0);
|
||
|
|
|
||
|
|
while((frame = retrieve_frame()) != NULL)
|
||
|
|
vPortFree((void *) frame);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void promisc_callback_all(unsigned char *buf, unsigned int len, void* userdata)
|
||
|
|
{
|
||
|
|
struct eth_frame *frame = (struct eth_frame *) pvPortMalloc(sizeof(struct eth_frame));
|
||
|
|
|
||
|
|
if(frame) {
|
||
|
|
frame->prev = NULL;
|
||
|
|
frame->next = NULL;
|
||
|
|
memcpy(frame->da, buf+4, 6);
|
||
|
|
memcpy(frame->sa, buf+10, 6);
|
||
|
|
frame->len = len;
|
||
|
|
frame->type = *buf;
|
||
|
|
|
||
|
|
taskENTER_CRITICAL();
|
||
|
|
|
||
|
|
if(eth_buffer.tail) {
|
||
|
|
eth_buffer.tail->next = frame;
|
||
|
|
frame->prev = eth_buffer.tail;
|
||
|
|
eth_buffer.tail = frame;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
eth_buffer.head = frame;
|
||
|
|
eth_buffer.tail = frame;
|
||
|
|
}
|
||
|
|
|
||
|
|
taskEXIT_CRITICAL();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
static void promisc_test_all(int duration, unsigned char len_used)
|
||
|
|
{
|
||
|
|
int ch;
|
||
|
|
unsigned int start_time;
|
||
|
|
struct eth_frame *frame;
|
||
|
|
eth_buffer.head = NULL;
|
||
|
|
eth_buffer.tail = NULL;
|
||
|
|
|
||
|
|
wifi_enter_promisc_mode();
|
||
|
|
wifi_set_promisc(RTW_PROMISC_ENABLE_2, promisc_callback_all, len_used);
|
||
|
|
|
||
|
|
for(ch = 1; ch <= 13; ch ++) {
|
||
|
|
if(wifi_set_channel(ch) == 0)
|
||
|
|
printf("\n\n\rSwitch to channel(%d)", ch);
|
||
|
|
|
||
|
|
start_time = xTaskGetTickCount();
|
||
|
|
|
||
|
|
while(1) {
|
||
|
|
unsigned int current_time = xTaskGetTickCount();
|
||
|
|
|
||
|
|
if((current_time - start_time) < (duration * configTICK_RATE_HZ)) {
|
||
|
|
frame = retrieve_frame();
|
||
|
|
|
||
|
|
if(frame) {
|
||
|
|
int i;
|
||
|
|
printf("\n\rTYPE: 0x%x, ", frame->type);
|
||
|
|
printf("DA:");
|
||
|
|
for(i = 0; i < 6; i ++)
|
||
|
|
printf(" %02x", frame->da[i]);
|
||
|
|
printf(", SA:");
|
||
|
|
for(i = 0; i < 6; i ++)
|
||
|
|
printf(" %02x", frame->sa[i]);
|
||
|
|
printf(", len=%d", frame->len);
|
||
|
|
|
||
|
|
vPortFree((void *) frame);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
vTaskDelay(1); //delay 1 tick
|
||
|
|
}
|
||
|
|
else
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
wifi_set_promisc(RTW_PROMISC_DISABLE, NULL, 0);
|
||
|
|
|
||
|
|
while((frame = retrieve_frame()) != NULL)
|
||
|
|
vPortFree((void *) frame);
|
||
|
|
}
|
||
|
|
|
||
|
|
void cmd_promisc(int argc, char **argv)
|
||
|
|
{
|
||
|
|
int duration;
|
||
|
|
#ifdef CONFIG_PROMISC
|
||
|
|
wifi_init_packet_filter();
|
||
|
|
#endif
|
||
|
|
if((argc == 2) && ((duration = atoi(argv[1])) > 0))
|
||
|
|
//promisc_test(duration, 0);
|
||
|
|
promisc_test_all(duration, 0);
|
||
|
|
else if((argc == 3) && ((duration = atoi(argv[1])) > 0) && (strcmp(argv[2], "with_len") == 0))
|
||
|
|
promisc_test(duration, 1);
|
||
|
|
else
|
||
|
|
printf("\n\rUsage: %s DURATION_SECONDS [with_len]", argv[0]);
|
||
|
|
}
|
||
|
|
#endif //#if CONFIG_WLAN
|