479 lines
14 KiB
C
479 lines
14 KiB
C
#include <sysinit.h>
|
|
|
|
#include "basic/basic.h"
|
|
#include "basic/byteorder.h"
|
|
|
|
#include "lcd/lcd.h"
|
|
#include "lcd/print.h"
|
|
|
|
#include "funk/nrf24l01p.h"
|
|
#include "usbcdc/usb.h"
|
|
#include "usbcdc/usbcore.h"
|
|
#include "usbcdc/usbhw.h"
|
|
#include "usbcdc/cdcuser.h"
|
|
#include "usbcdc/cdc_buf.h"
|
|
#include "usbcdc/util.h"
|
|
|
|
#include <string.h>
|
|
|
|
#define REMOTE_CHANNEL 91
|
|
#define REMOTE_MAC "REM0T"
|
|
#define MESH_CHANNEL 85
|
|
#define MESH_MAC "MESHB"
|
|
#define BEACON_CHANNEL 81
|
|
#define BEACON_MAC "\x1\x2\x3\x2\1"
|
|
|
|
#include "SECRETS.release"
|
|
|
|
char funkencrypt=0;
|
|
|
|
#if CFG_USBMSC
|
|
#error "MSC is defined"
|
|
#endif
|
|
|
|
#if !CFG_USBCDC
|
|
#error "CDC is not defined"
|
|
#endif
|
|
|
|
/**************************************************************************/
|
|
|
|
uint32_t thekey[4] = { 0x0, 0x0, 0x0, 0x0 };
|
|
|
|
struct NRF_CFG config = {
|
|
.channel= BEACON_CHANNEL,
|
|
.txmac= BEACON_MAC,
|
|
.nrmacs=1,
|
|
.mac0= BEACON_MAC,
|
|
.maclen ="\x10",
|
|
};
|
|
|
|
char type=0;
|
|
char filter=0;
|
|
uint8_t filterdata[10];
|
|
char outc[2]= {0,0};
|
|
|
|
static int process(char * input);
|
|
|
|
#define INPUTLEN 99
|
|
//# MENU serial
|
|
void dwim(void){
|
|
char input[INPUTLEN+1];
|
|
int inputptr=0;
|
|
char dirty=0;
|
|
|
|
usbCDCInit();
|
|
delayms(500);
|
|
getInputWaitRelease();
|
|
nrf_init();
|
|
gpioSetValue (RB_LED1, 0);
|
|
puts("D start\r\n");
|
|
|
|
while(!getInputRaw()){
|
|
|
|
// Input
|
|
int l=INPUTLEN-inputptr;
|
|
CDC_OutBufAvailChar (&l);
|
|
|
|
if(l>0){
|
|
dirty=10;
|
|
if(l>1)
|
|
dirty=50;
|
|
CDC_RdOutBuf (input+inputptr, &l);
|
|
if(input[inputptr]==8){
|
|
inputptr=0;
|
|
puts("<oops>\r\n");
|
|
continue;
|
|
};
|
|
input[inputptr+l]=0;
|
|
puts_plus(&input[inputptr]);
|
|
for(int i=0;i<l;i++){
|
|
if(input[inputptr+i] =='\r' || input[inputptr+i] =='\n'){
|
|
input[inputptr+i]=0;
|
|
process(input);
|
|
if(i<l)
|
|
memmove(input,input+inputptr+i+1,l-i);
|
|
inputptr=-i-1;
|
|
break;
|
|
};
|
|
};
|
|
}else{
|
|
delayms(1);
|
|
if(dirty>0)
|
|
dirty--;
|
|
if(dirty==1)
|
|
puts("");
|
|
};
|
|
inputptr+=l;
|
|
};
|
|
gpioSetValue (RB_LED1, 0);
|
|
puts("D exit\r\n");
|
|
}
|
|
|
|
#define BUFLEN 32
|
|
#define NYB(x) ((x>'9')?(x|0x20)-'a'+10:x-'0')
|
|
static uint8_t * hextobyte(char * input, int *len){
|
|
static uint8_t buf[BUFLEN];
|
|
|
|
int p=0;
|
|
int bp=0;
|
|
char c;
|
|
while(bp<BUFLEN){
|
|
if(input[p]==0 || input[p+1]==0)
|
|
break;
|
|
if(input[p]==' '){
|
|
p++;
|
|
continue;
|
|
};
|
|
buf[bp]= c=NYB(input[p])*16+NYB(input[p+1]);
|
|
bp++;p+=2;
|
|
};
|
|
*len=bp;
|
|
return buf;
|
|
};
|
|
|
|
static uint32_t gethexval(char * input){
|
|
int len;
|
|
uint8_t *hex=hextobyte(input,&len);
|
|
uint32_t v=hex[0];
|
|
while(--len>0)
|
|
v=v*256+(*++hex);
|
|
return v;
|
|
};
|
|
|
|
static int process(char * input){
|
|
if(input == NULL || input[0]==0)
|
|
return -1;
|
|
|
|
puts_plus("\r\n");
|
|
puts_plus("D [");
|
|
puts_plus(input);
|
|
puts("]\r\n");
|
|
|
|
if(input[0]=='i'){
|
|
nrf_init();
|
|
nrf_config_set(&config);
|
|
}else if(input[0]=='p'){
|
|
type=input[1];
|
|
if(input[1]=='m'){
|
|
config.channel=MESH_CHANNEL;
|
|
memcpy(config.txmac,MESH_MAC,5);
|
|
memcpy(config.mac0,MESH_MAC,5);
|
|
config.maclen[0]=0x20;
|
|
config.nrmacs=1;
|
|
nrf_config_set(&config);
|
|
memcpy(thekey,meshkey,sizeof(thekey));
|
|
funkencrypt=1;
|
|
}else if(input[1]=='M'){
|
|
config.channel=83;
|
|
memcpy(config.txmac,MESH_MAC,5);
|
|
memcpy(config.mac0,MESH_MAC,5);
|
|
config.maclen[0]=0x20;
|
|
config.nrmacs=1;
|
|
nrf_config_set(&config);
|
|
static const uint32_t const pubmesh[4] = {
|
|
0x00000042, 0x000005ec, 0x00000023, 0x00000005
|
|
};
|
|
memcpy(thekey,pubmesh,sizeof(thekey));
|
|
funkencrypt=1;
|
|
}else if(input[1]=='r'){
|
|
config.channel=REMOTE_CHANNEL;
|
|
memcpy(config.txmac,REMOTE_MAC,5);
|
|
memcpy(config.mac0,REMOTE_MAC,5);
|
|
config.maclen[0]=0x10;
|
|
config.nrmacs=1;
|
|
nrf_config_set(&config);
|
|
memcpy(thekey,remotekey,sizeof(thekey));
|
|
funkencrypt=1;
|
|
}else if(input[1]=='b'){
|
|
config.channel=BEACON_CHANNEL;
|
|
memcpy(config.txmac,BEACON_MAC,5);
|
|
memcpy(config.mac0,BEACON_MAC,5);
|
|
config.maclen[0]=0x10;
|
|
config.nrmacs=1;
|
|
nrf_config_set(&config);
|
|
memcpy(thekey,openbeaconkey,sizeof(thekey));
|
|
funkencrypt=1;
|
|
}else if(input[1]=='B'){
|
|
config.channel=BEACON_CHANNEL;
|
|
memcpy(config.txmac,BEACON_MAC,5);
|
|
memcpy(config.mac0,BEACON_MAC,5);
|
|
config.maclen[0]=0x10;
|
|
config.nrmacs=1;
|
|
nrf_config_set(&config);
|
|
static const uint32_t pubbeaconkey[4] = {
|
|
0xB4595344, 0xD3E119B6, 0xA814D0EC, 0xEFF5A24E
|
|
};
|
|
memcpy(thekey,pubbeaconkey,sizeof(thekey));
|
|
funkencrypt=1;
|
|
};
|
|
}else if(input[0]=='t'){
|
|
type=input[1];
|
|
}else if(input[0]=='c'){
|
|
int len;
|
|
uint8_t *hex=hextobyte(&input[2],&len);
|
|
if(input[1]=='k'){
|
|
thekey[0]=uint8ptouint32(hex);
|
|
thekey[1]=uint8ptouint32(hex+4);
|
|
thekey[2]=uint8ptouint32(hex+8);
|
|
thekey[3]=uint8ptouint32(hex+12);
|
|
}else if(input[1]=='m'){
|
|
memcpy(config.mac0,hex,5);
|
|
nrf_config_set(&config);
|
|
}else if(input[1]=='t'){
|
|
memcpy(config.txmac,hex,5);
|
|
nrf_config_set(&config);
|
|
}else if(input[1]=='c'){
|
|
config.channel=hex[0];
|
|
nrf_config_set(&config);
|
|
}else if(input[1]=='l'){
|
|
config.maclen[0]=hex[0];
|
|
nrf_config_set(&config);
|
|
}else if(input[1]=='e'){
|
|
funkencrypt= hex[0];
|
|
}else if(input[1]=='?'){
|
|
nrf_config_get(&config);
|
|
puts_plus("Ch: ");puts_plus(IntToStrX( config.channel,2 )); puts_plus("\r\n");
|
|
|
|
puts_plus("TxMac: ");
|
|
for(int i=0;i<5;i++)
|
|
puts_plus(IntToStrX( config.txmac[i],2 ));
|
|
puts_plus("\r\n");
|
|
|
|
puts_plus("Len : ");
|
|
for(int i=0;i<5;i++)
|
|
puts_plus(IntToStrX( config.maclen[i],2 ));
|
|
puts_plus("\r\n");
|
|
|
|
puts_plus("0mac : ");
|
|
for(int i=0;i<5;i++)
|
|
puts_plus(IntToStrX( config.mac0[i],2 ));
|
|
puts_plus("\r\n");
|
|
|
|
puts_plus("1mac : ");
|
|
for(int i=0;i<5;i++)
|
|
puts_plus(IntToStrX( config.mac1[i],2 ));
|
|
puts_plus("\r\n");
|
|
|
|
puts_plus("2345 : ");
|
|
for(int i=0;i<4;i++)
|
|
puts_plus(IntToStrX( config.mac2345[i],2 ));
|
|
puts_plus("\r\n");
|
|
|
|
puts_plus("key : ");
|
|
for(int i=0;i<4;i++){
|
|
puts_plus(IntToStrX( thekey[i],8 ));
|
|
puts_plus(" ");
|
|
};
|
|
puts_plus("\r\n");
|
|
|
|
puts_plus("encrypt : ");
|
|
puts_plus(IntToStrX( funkencrypt,2 ));
|
|
puts_plus("\r\n");
|
|
};
|
|
}else if (input[0]=='s'){
|
|
__attribute__ ((aligned (4))) uint8_t buf[32];
|
|
int status=0;
|
|
int len;
|
|
int idx=1;
|
|
char ctr=1;
|
|
char debug=0;
|
|
if(input[idx]=='d'){
|
|
debug=1;
|
|
idx++;
|
|
};
|
|
if(input[idx]=='+'){
|
|
ctr=10;
|
|
idx++;
|
|
};
|
|
|
|
if (input[idx]==' '){
|
|
uint8_t *hex=hextobyte(&input[idx],&len);
|
|
if(len<10) len=10; // minmal packet length?
|
|
len+=2; // Add crc!
|
|
|
|
memcpy(buf,hex,len);
|
|
status=nrf_snd_pkt_crc_encr(len,buf,funkencrypt?thekey:NULL);
|
|
|
|
if(debug){
|
|
puts_plus("P ");
|
|
puts_plus("[");puts_plus(IntToStrX(len,2));puts_plus("] ");
|
|
for(int i=0;i<len;i++){
|
|
puts_plus(IntToStrX( buf[i],2 ));
|
|
puts_plus(" ");
|
|
};
|
|
};
|
|
puts_plus("S ");
|
|
puts_plus("[");puts_plus(IntToStrX(len,2));puts_plus("] ");
|
|
puts_plus("state=");
|
|
puts_plus(IntToStrX( status,2 ));
|
|
puts("\r\n");
|
|
|
|
while(--ctr>0){
|
|
delayms(23);
|
|
memcpy(buf,hex,len);
|
|
status=nrf_snd_pkt_crc_encr(len,buf,funkencrypt?thekey:NULL);
|
|
};
|
|
}else{
|
|
;
|
|
};
|
|
puts_plus("\r\n");
|
|
}else if (input[0]=='f'){
|
|
int len;
|
|
filter=input[1];
|
|
uint8_t *hex=hextobyte(input+2,&len);
|
|
if(len>9)
|
|
len=9;
|
|
memcpy(filterdata,hex,len);
|
|
puts_plus("F ");
|
|
if(filter){
|
|
puts_plus("[");
|
|
outc[0]=filter;
|
|
puts_plus(outc);
|
|
puts_plus("]");
|
|
for(int i=0;i<len;i++){
|
|
puts_plus(IntToStrX(filterdata[i],2 ));
|
|
};
|
|
}else{
|
|
puts_plus("off");
|
|
};
|
|
puts_plus("\r\n");
|
|
}else if (input[0]=='r'){
|
|
__attribute__ ((aligned (4))) uint8_t buf[32];
|
|
int len;
|
|
int pctr=5;
|
|
int t=5;
|
|
|
|
if(input[1]=='+'){
|
|
if(input[2]!=0){
|
|
t=gethexval(&input[2]);
|
|
};
|
|
pctr=-1;
|
|
}
|
|
if(input[1]=='-'){
|
|
pctr=gethexval(&input[2]);
|
|
t=-1;
|
|
}
|
|
|
|
puts_plus("D rcv: ");
|
|
puts_plus(IntToStr(pctr,8,0));
|
|
puts_plus("pkts, ");
|
|
puts_plus(IntToStr(t,8,0));
|
|
puts_plus("secs ");
|
|
if(filter){
|
|
puts_plus("FILTER=");
|
|
outc[0]=filter;
|
|
puts_plus(outc);
|
|
};
|
|
puts_plus(" ...\r\n");
|
|
if(t>0)
|
|
t=getTimer()+t*1000/SYSTICKSPEED;
|
|
nrf_rcv_pkt_start();
|
|
do{
|
|
len=nrf_rcv_pkt_poll_dec(sizeof(buf),buf,funkencrypt?thekey:NULL);
|
|
// Receive
|
|
if(len==0||len==-1||len==-2){ //No pkt, Pkt2large, NoPktError
|
|
delayms(10);
|
|
continue;
|
|
};
|
|
if (!filter){
|
|
if(len==-3){
|
|
puts_plus("[!crc] ");
|
|
len=32;
|
|
};
|
|
};
|
|
if(type=='m' || type=='M'){
|
|
if(filter && filter!=buf[0])
|
|
continue;
|
|
puts_plus("R ");
|
|
puts_plus(IntToStrX( buf[0],2 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( buf[1],2 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( uint8ptouint32(buf+2),8 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( uint8ptouint32(buf+6),8 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( uint8ptouint32(buf+10),8 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( uint8ptouint32(buf+14),8 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( uint8ptouint32(buf+18),8 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( uint8ptouint32(buf+22),8 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( uint8ptouint32(buf+26),8 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( buf[30],2 ));
|
|
puts_plus(IntToStrX( buf[31],2 ));
|
|
puts_plus(" ");
|
|
}else if(type=='B'){
|
|
if(filter)
|
|
if(uint8ptouint32(buf+8)!=uint8ptouint32(filterdata))
|
|
continue;
|
|
puts_plus("RF ");
|
|
puts_plus(IntToStrX( buf[2],2 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( uint8ptouint32(buf+8),8 ));
|
|
}else if(type=='b'){
|
|
if(filter=='.' || filter=='u')
|
|
if(uint8ptouint32(buf+8)!=uint8ptouint32(filterdata))
|
|
continue;
|
|
if(filter=='0' && buf[3] != 0x00)
|
|
continue;
|
|
if(filter=='5' && buf[3] != 0x55)
|
|
continue;
|
|
if(filter=='a' && buf[3] != 0xaa)
|
|
continue;
|
|
if(filter=='A' && buf[3] != 0xaa)
|
|
continue;
|
|
if(filter=='f' && buf[3] != 0xff)
|
|
continue;
|
|
if(filter=='F' && buf[3] != 0xff)
|
|
continue;
|
|
puts_plus("R ");
|
|
puts_plus(IntToStrX( buf[0],2 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( buf[1],2 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( buf[2],2 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( buf[3],2 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( uint8ptouint32(buf+4),8 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( uint8ptouint32(buf+8),8 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( buf[12],2 ));
|
|
puts_plus(IntToStrX( buf[13],2 ));
|
|
puts_plus(" ");
|
|
puts_plus(IntToStrX( buf[14],2 ));
|
|
puts_plus(IntToStrX( buf[15],2 ));
|
|
}else{
|
|
puts_plus("R ");
|
|
puts_plus("[");puts_plus(IntToStrX(len,2));puts_plus("] ");
|
|
for(int i=0;i<len;i++){
|
|
puts_plus(IntToStrX( buf[i],2 ));
|
|
puts_plus(" ");
|
|
};
|
|
};
|
|
if(1){
|
|
int l=0;
|
|
CDC_OutBufAvailChar (&l);
|
|
if(l>0){
|
|
puts_plus("\r\nD Abort.\r\n");
|
|
break;
|
|
};
|
|
};
|
|
puts("\r\n");
|
|
if(--pctr==0)
|
|
break;
|
|
}while((t>0)?(t>getTimer()):1);
|
|
|
|
nrf_rcv_pkt_end();
|
|
}else{
|
|
puts_plus("D no action\r\n");
|
|
};
|
|
puts("D done.\r\n");
|
|
return 0;
|
|
};
|