crashtest-r0ket/firmware/funk/mesh.c

378 lines
9.5 KiB
C
Raw Normal View History

2011-08-03 14:46:48 +00:00
#include <sysdefs.h>
2011-07-30 14:10:30 +00:00
#include <sysinit.h>
#include <string.h>
#include "basic/basic.h"
#include "funk/mesh.h"
#include "funk/nrf24l01p.h"
#include "basic/byteorder.h"
#include "basic/random.h"
2011-08-04 15:57:59 +00:00
#include "basic/config.h"
#include "lcd/print.h"
2011-07-30 14:10:30 +00:00
char meshgen=0; // Generation
2011-08-03 21:12:43 +00:00
char meshincctr=0;
char meshmsg=0;
2011-08-05 00:19:22 +00:00
char meshnice=0;
2011-07-30 14:10:30 +00:00
MPKT meshbuffer[MESHBUFSIZE];
#include "SECRETS"
2011-07-30 14:10:30 +00:00
struct NRF_CFG oldconfig;
2011-12-25 19:02:41 +00:00
static int mesh_gt(char curgen, char newgen){
unsigned char dif=curgen-newgen;
if(curgen==0)
return 1;
return (dif>128);
};
2011-07-30 14:10:30 +00:00
void initMesh(void){
for(int i=0;i<MESHBUFSIZE;i++){
meshbuffer[i].flags=MF_FREE;
};
memset(meshbuffer[0].pkt,0,MESHPKTSIZE);
meshbuffer[0].pkt[0]='T';
MO_TIME_set(meshbuffer[0].pkt,getSeconds());
meshbuffer[0].flags=MF_USED;
};
#define MP_OK 0
#define MP_SEND 1
#define MP_RECV 2
#define MP_IGNORE 4
int mesh_sanity(uint8_t * pkt){
if(MO_TYPE(pkt)>0x7f || MO_TYPE(pkt)<0x20)
return MP_SEND;
if(MO_TYPE(pkt)=='T' && MO_BODY(pkt)[5])
return MP_SEND;
if(MO_TYPE(pkt)=='T' && MO_TIME(pkt)<86400)
return MP_OK;
if(MO_TYPE(pkt)>='A' && MO_TYPE(pkt)<='Z'){
if(MO_TIME(pkt)>1326409200)
2012-01-03 21:23:01 +00:00
return MP_SEND|MP_RECV;
if(MO_TIME(pkt)<1324602000)
2012-01-03 21:23:01 +00:00
return MP_SEND|MP_RECV;
}else if(MO_TYPE(pkt)>='a' && MO_TYPE(pkt)<='z'){
if(MO_TIME(pkt)>16777216)
return MP_SEND;
if(MO_TIME(pkt)<0)
return MP_SEND;
};
if(MO_TYPE(pkt)!='A' &&
MO_TYPE(pkt)!='a' &&
2011-12-25 19:02:41 +00:00
MO_TYPE(pkt)!='B' &&
MO_TYPE(pkt)!='E' &&
MO_TYPE(pkt)!='F' &&
MO_TYPE(pkt)!='G' &&
MO_TYPE(pkt)!='T'
){
return MP_IGNORE;
2011-08-12 22:52:41 +00:00
};
return MP_OK;
};
MPKT * meshGetMessage(uint8_t type){
int free=-1;
for(int i=0;i<MESHBUFSIZE;i++){
if ( ((meshbuffer[i].flags&MF_USED)==0) && free<0 )
free=i;
if ( (meshbuffer[i].flags&MF_USED) &&
(MO_TYPE(meshbuffer[i].pkt) == type)){
free=i;
break;
};
};
if(free==-1){ // Buffer full. Ah well. Kill a random packet
free= (int)(getRandom() % MESHBUFSIZE);
meshbuffer[free].flags=MF_FREE;
};
if(meshbuffer[free].flags==MF_FREE){
memset(&meshbuffer[free],0,sizeof(MPKT));
MO_TYPE_set(meshbuffer[free].pkt,type);
2011-08-03 21:12:43 +00:00
MO_GEN_set(meshbuffer[free].pkt,meshgen);
meshbuffer[free].flags=MF_USED;
};
return &meshbuffer[free];
2011-07-30 14:10:30 +00:00
};
2012-01-03 01:45:48 +00:00
void meshPanic(uint8_t * pkt,int bufno){
2012-01-03 01:48:39 +00:00
static int done=0;
#if 1
2012-01-03 01:48:39 +00:00
if(!done){
setSystemFont();
lcdClear();
lcdPrint("PANIC[");
lcdPrint(IntToStrX(bufno,2));
lcdPrint("]");
lcdNl();
for(int i=0;i<32;i++){
lcdPrint(IntToStrX(pkt[i],2));
if(i%6==5)
lcdNl();
}
lcdRefresh();
while ((getInputRaw())==BTN_NONE);
};
done=1;
#endif
};
2011-07-30 14:10:30 +00:00
void mesh_cleanup(void){
time_t now=getSeconds();
for(int i=1;i<MESHBUFSIZE;i++){
if(meshbuffer[i].flags&MF_LOCK)
continue;
2011-07-30 14:10:30 +00:00
if(meshbuffer[i].flags&MF_USED){
if (MO_GEN(meshbuffer[i].pkt)<meshgen)
meshbuffer[i].flags=MF_FREE;
if (MO_TYPE(meshbuffer[i].pkt)>='a' &&
MO_TYPE(meshbuffer[i].pkt)<='z'){
;
}else{
if (MO_TIME(meshbuffer[i].pkt)<now)
meshbuffer[i].flags=MF_FREE;
if (MO_TIME(meshbuffer[i].pkt)-now>SECS_DAY)
meshbuffer[i].flags=MF_FREE;
};
2012-01-03 04:45:36 +00:00
if((mesh_sanity(meshbuffer[i].pkt)&MP_SEND)!=0){
meshbuffer[i].flags=MF_FREE;
2012-01-03 01:45:48 +00:00
meshPanic(meshbuffer[i].pkt,i);
};
2011-07-30 14:10:30 +00:00
};
};
};
2011-08-04 15:52:38 +00:00
void mesh_sendloop(void){
int ctr=0;
2011-07-30 14:10:30 +00:00
__attribute__ ((aligned (4))) uint8_t buf[32];
2011-08-04 15:52:38 +00:00
int status;
2011-08-05 01:07:41 +00:00
uint32_t rnd=0xffffffff;
if(meshnice)
rnd=getRandom();
2011-08-04 15:52:38 +00:00
nrf_config_get(&oldconfig);
nrf_set_channel(MESH_CHANNEL);
nrf_set_tx_mac(strlen(MESH_MAC),(uint8_t*)MESH_MAC);
// Update [T]ime packet
MO_TIME_set(meshbuffer[0].pkt,getSeconds());
MO_GEN_set(meshbuffer[0].pkt,meshgen);
if(GLOBAL(privacy)==0)
2011-08-06 01:05:36 +00:00
uint32touint8p(GetUUID32(),meshbuffer[0].pkt+26);
2011-08-04 15:57:59 +00:00
else
2011-08-06 01:05:36 +00:00
uint32touint8p(0,meshbuffer[0].pkt+26);
2011-08-04 15:52:38 +00:00
2011-08-05 00:19:22 +00:00
MO_BODY(meshbuffer[0].pkt)[4]=meshnice;
2011-08-04 15:52:38 +00:00
for (int i=0;i<MESHBUFSIZE;i++){
if(!meshbuffer[i].flags&MF_USED)
continue;
if(meshbuffer[i].flags&MF_LOCK)
continue;
2011-08-05 00:19:22 +00:00
if(meshnice&0xf){
2011-08-05 01:07:41 +00:00
if((rnd++)%0xf < (meshnice&0x0f)){
2011-08-05 00:19:22 +00:00
meshincctr++;
continue;
};
};
2011-08-04 15:52:38 +00:00
ctr++;
memcpy(buf,meshbuffer[i].pkt,MESHPKTSIZE);
2011-12-18 00:59:57 +00:00
status=nrf_snd_pkt_crc_encr(MESHPKTSIZE,buf,NULL);
2011-08-04 15:52:38 +00:00
//Check status? But what would we do...
};
nrf_config_set(&oldconfig);
};
void mesh_recvqloop_setup(void){
2011-07-30 14:10:30 +00:00
nrf_config_get(&oldconfig);
nrf_set_channel(MESH_CHANNEL);
nrf_set_rx_mac(0,MESHPKTSIZE,strlen(MESH_MAC),(uint8_t*)MESH_MAC);
mesh_cleanup();
nrf_rcv_pkt_start();
2011-08-04 15:52:38 +00:00
};
static inline uint32_t popcount(uint32_t *buf, uint8_t n){
int cnt=0;
do {
unsigned m = *buf++;
m = (m & 0x55555555) + ((m & 0xaaaaaaaa) >> 1);
m = (m & 0x33333333) + ((m & 0xcccccccc) >> 2);
m = (m & 0x0f0f0f0f) + ((m & 0xf0f0f0f0) >> 4);
m = (m & 0x00ff00ff) + ((m & 0xff00ff00) >> 8);
m = (m & 0x0000ffff) + ((m & 0xffff0000) >> 16);
cnt += m;
} while(--n);
return cnt;
}
2011-08-04 15:52:38 +00:00
uint8_t mesh_recvqloop_work(void){
__attribute__ ((aligned (4))) uint8_t buf[32];
unsigned int len;
2011-08-04 15:52:38 +00:00
2011-12-18 00:59:57 +00:00
len=nrf_rcv_pkt_poll_dec(sizeof(buf),buf,NULL);
2011-07-30 14:10:30 +00:00
// Receive
if(len<=0){
2011-08-04 15:52:38 +00:00
return 0;
2011-07-30 14:10:30 +00:00
};
if(mesh_sanity(buf)){
meshincctr++;
2012-01-03 04:45:36 +00:00
if((mesh_sanity(buf)&MP_RECV)!=0){
2012-01-03 01:45:48 +00:00
meshPanic(buf,-1);
};
return 0;
};
// New mesh generation?
2011-07-30 14:10:30 +00:00
if(MO_TYPE(buf)=='T'){
2011-12-25 19:02:41 +00:00
if(mesh_gt(meshgen,MO_GEN(buf))){
_timet=0;
meshincctr=0;
meshnice=MO_BODY(buf)[4];
meshgen=MO_GEN(buf);
};
};
// Discard packets with wrong generation
if(meshgen != MO_GEN(buf)){
return 0;
};
// Set new time iff newer
if(MO_TYPE(buf)=='T'){
2011-08-03 14:46:48 +00:00
time_t toff=MO_TIME(buf)-((getTimer()+(600/SYSTICKSPEED))/(1000/SYSTICKSPEED));
if (toff>_timet){ // Do not live in the past.
2011-07-30 14:10:30 +00:00
_timet = toff;
2011-08-03 14:46:48 +00:00
meshincctr++;
};
2011-08-04 15:52:38 +00:00
return 1;
2011-07-30 14:10:30 +00:00
};
// Safety: Truncate ascii packets by 0-ing the CRC
buf[MESHPKTSIZE-2]=0;
2011-07-30 14:10:30 +00:00
// Store packet in a same/free slot
MPKT* mpkt=meshGetMessage(MO_TYPE(buf));
2011-07-30 14:10:30 +00:00
2011-12-25 19:02:41 +00:00
// Propagation test
if(MO_TYPE(buf)=='B'){
if(! mesh_gt(MO_BODY(mpkt->pkt)[0],MO_BODY(buf)[0]) )
return 0;
(*(
(uint32_t*)(MO_BODY(buf)+6)
))++;
if(GLOBAL(privacy)==0)
uint32touint8p(GetUUID32(),MO_BODY(buf)+20);
2011-12-25 19:02:41 +00:00
MO_TIME_set(mpkt->pkt,0);
};
#if 0
// Schnitzel
if(MO_TYPE(buf)=='Z'){
mpkt->flags=MF_USED|MF_LOCK;
MO_TIME_set(mpkt->pkt,getSeconds());
MO_GEN_set(mpkt->pkt,0x70);
for(int i=6;i<MESHPKTSIZE;i++)
mpkt->pkt[i]|=buf[i];
2011-08-05 16:11:37 +00:00
int score=popcount((uint32_t*)MO_BODY(mpkt->pkt),6);
MPKT* reply=meshGetMessage('z');
if(MO_TIME(reply->pkt)>=score)
return 1;
MO_TIME_set(reply->pkt,score);
strcpy((char*)MO_BODY(reply->pkt),GLOBAL(nickname));
2011-08-05 09:54:20 +00:00
if(GLOBAL(privacy)==0){
uint32touint8p(GetUUID32(),meshbuffer[0].pkt+26);
meshbuffer[0].pkt[25]=0;
};
return 1;
};
#endif
2011-07-30 14:10:30 +00:00
// only accept newer/better packets
if(mpkt->flags==MF_USED)
if(MO_TIME(buf)<=MO_TIME(mpkt->pkt))
return 2;
#if 0
if((MO_TYPE(buf)>='A' && MO_TYPE(buf)<='C') ||
2011-08-12 12:22:51 +00:00
(MO_TYPE(buf)>='a' && MO_TYPE(buf)<='c'))
meshmsg=1;
#endif
memcpy(mpkt->pkt,buf,MESHPKTSIZE);
mpkt->flags=MF_USED;
return 1;
2011-08-04 15:52:38 +00:00
};
2011-07-30 14:10:30 +00:00
2011-08-04 15:52:38 +00:00
void mesh_recvqloop_end(void){
2011-07-30 14:10:30 +00:00
nrf_rcv_pkt_end();
nrf_config_set(&oldconfig);
}
2011-08-04 15:52:38 +00:00
void mesh_recvloop(void){
int recvend=M_RECVTIM/SYSTICKSPEED+getTimer();
int pktctr=0;
2011-07-30 14:10:30 +00:00
2011-08-04 15:52:38 +00:00
mesh_recvqloop_setup();
do{
if( mesh_recvqloop_work() ){
pktctr++;
}else{
delayms_power(10);
};
}while(getTimer()<recvend || pktctr>MESHBUFSIZE);
mesh_recvqloop_end();
2011-07-30 14:10:30 +00:00
};
uint8_t mesh_recvloop_plus(uint8_t state){
static int recvend=0;
static int pktctr=0;
if (state==0){
recvend=M_RECVTIM/SYSTICKSPEED+getTimer();
pktctr=0;
mesh_recvqloop_setup();
state=1;
};
if(state==1){
if( mesh_recvqloop_work() ){
pktctr++;
}else{
delayms_power(10);
};
if(getTimer()>recvend || pktctr>MESHBUFSIZE)
state=QS_END;
};
return state;
};
2011-07-30 14:10:30 +00:00
void mesh_systick(void){
static int rcvctr=0;
static int sendctr=0;
if(rcvctr--<0){
push_queue_plus(&mesh_recvloop_plus);
2011-07-30 14:10:30 +00:00
rcvctr=M_RECVINT/SYSTICKSPEED/2;
rcvctr+=getRandom()%(rcvctr*2);
};
if(sendctr--<0){
push_queue(&mesh_sendloop);
sendctr=M_SENDINT/SYSTICKSPEED/2;
sendctr+=getRandom()%(sendctr*2);
};
};
2011-08-04 15:52:38 +00:00