ExternalVendorCode/serval-dna
1
0
Fork 0
mirror of https://github.com/servalproject/serval-dna.git synced 2025-01-29 15:43:56 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge branch 'mavlink' into development

Browse Source
This commit is contained in:
Jeremy Lakeman 2013-09-20 14:58:29 +09:30
commit 57577abca6
39 changed files with 2576 additions and 390 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
conf_schema.h
View File

@ -228,17 +228,22 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
STRUCT(debug)
ATOM(bool_t, verbose, 0, boolean,, "")
ATOM(bool_t, ack, 0, boolean,, "")
ATOM(bool_t, dnaresponses, 0, boolean,, "")
ATOM(bool_t, dnahelper, 0, boolean,, "")
ATOM(bool_t, queues, 0, boolean,, "")
ATOM(bool_t, timing, 0, boolean,, "")
ATOM(bool_t, io, 0, boolean,, "")
ATOM(bool_t, verbose_io, 0, boolean,, "")
ATOM(bool_t, interactive_io, 0, boolean,, "")
ATOM(bool_t, packetformats, 0, boolean,, "")
ATOM(bool_t, gateway, 0, boolean,, "")
ATOM(bool_t, keyring, 0, boolean,, "")
ATOM(bool_t, security, 0, boolean,, "")
ATOM(bool_t, mdprequests, 0, boolean,, "")
ATOM(bool_t, mavlink, 0, boolean,, "")
ATOM(bool_t, mavlink_payloads, 0, boolean,, "")
ATOM(bool_t, mavlinkfsm, 0, boolean,, "")
ATOM(bool_t, peers, 0, boolean,, "")
ATOM(bool_t, overlayframes, 0, boolean,, "")
ATOM(bool_t, overlayabbreviations, 0, boolean,, "")
@ -260,6 +265,7 @@ ATOM(bool_t, rhizome_rx, 0, boolean,, "")
ATOM(bool_t, rhizome_ads, 0, boolean,, "")
ATOM(bool_t, rhizome_nohttptx, 0, boolean,, "")
ATOM(bool_t, rhizome_mdp_rx, 0, boolean,, "")
ATOM(bool_t, throttling, 0, boolean,, "")
ATOM(bool_t, meshms, 0, boolean,, "")
ATOM(bool_t, manifests, 0, boolean,, "")
ATOM(bool_t, vomp, 0, boolean,, "")
@ -432,7 +438,7 @@ END_ARRAY(32)
STRUCT(network_interface, vld_network_interface)
ATOM(bool_t, exclude, 0, boolean,, "If true, do not use matching interfaces")
ATOM(struct pattern_list, match, PATTERN_LIST_EMPTY, pattern_list,, "Names that match network interface")
ATOM(short, socket_type, SOCK_UNSPECIFIED, socket_type,, "Type of network socket")
ATOM(short, socket_type, SOCK_UNSPECIFIED, socket_type,, "Type of network socket; stream, dgram or file")
ATOM(short, encapsulation, ENCAP_OVERLAY, encapsulation,, "Type of packet encapsulation")
STRING(256, file, "", str_nonempty,, "Path of interface file, absolute or relative to server.interface_path")
ATOM(struct in_addr, dummy_address, hton_in_addr(INADDR_LOOPBACK), in_addr,, "Dummy interface address")
@ -451,6 +457,8 @@ ATOM(bool_t, debug, 0, boolean,, "If true, log details
ATOM(bool_t, point_to_point, 0, boolean,, "If true, assume there will only be two devices on this interface")
ATOM(bool_t, ctsrts, 0, boolean,, "If true, enable CTS/RTS hardware handshaking")
ATOM(int32_t, uartbps, 57600, int32_rs232baudrate,, "Speed of serial UART link speed (which may be different to serial device link speed)")
ATOM(int32_t, throttle, 0, int32_nonneg,, "Limit transmit speed of serial interface (bytes per second)")
ATOM(int32_t, burst_size, 0, int32_nonneg,, "Write no more than this many bytes at a time to a serial interface")
END_STRUCT
ARRAY(interface_list, NO_DUPLICATES)

506
fakeradio.c
View File

@ -2,6 +2,7 @@
#include <sys/types.h>
#include <sys/stat.h>
#include <stdlib.h>
#include <stdint.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
@ -11,17 +12,26 @@
#include <string.h>
#include <unistd.h>
#define PACKET_SIZE 255
int chars_per_ms=1;
long ber=0;
struct radio_state {
int fd;
int state;
const char *name;
char commandbuffer[128];
int cb_len;
unsigned char txbuffer[1024];
unsigned char txbuffer[1280];
int txb_len;
int tx_count;
int wait_count;
unsigned char rxbuffer[512];
int rxb_len;
long long last_char_ms;
long long last_tx_ms;
long long last_rssi_time_ms;
long long next_rssi_time_ms;
int rssi_output;
int tx_rate;
unsigned char seqnum;
};
#define STATE_ONLINE 0
@ -39,188 +49,400 @@ long long gettime_ms()
return nowtv.tv_sec * 1000LL + nowtv.tv_usec / 1000;
}
int emit(int fd,char *s)
{
return write(fd,s,strlen(s));
void log_time(){
struct timeval tv;
struct tm tm;
gettimeofday(&tv, NULL);
localtime_r(&tv.tv_sec, &tm);
char buf[50];
if (strftime(buf, sizeof buf, "%T", &tm) == 0)
fprintf(stderr, "EMPTYTIME___ ");
else
fprintf(stderr, "%s.%03u ", buf, (unsigned int)tv.tv_usec / 1000);
}
int processCommand(int fd,struct radio_state *s,int out_fd)
int append_bytes(struct radio_state *s, const char *bytes, int len)
{
if (len==-1)
len = strlen(bytes);
if (len + s->rxb_len > sizeof(s->rxbuffer))
return -1;
bcopy(bytes, &s->rxbuffer[s->rxb_len], len);
s->rxb_len+=len;
return len;
}
int processCommand(struct radio_state *s)
{
if (!s->cb_len) return 0;
s->commandbuffer[s->cb_len]=0;
char *cmd=s->commandbuffer;
log_time();
fprintf(stderr, "Processing command from %s \"%s\"\n", s->name, cmd);
if (!strcasecmp(cmd,"AT")) {
// Noop
append_bytes(s, "OK\r", -1);
return 0;
}
if (!strcasecmp(cmd,"ATO")) {
emit(fd,"OK\r");
append_bytes(s, "OK\r", -1);
s->state=STATE_ONLINE;
return 0;
}
if (!strcasecmp(cmd,"AT&T")) {
emit(fd,"OK\r");
append_bytes(s, "OK\r", -1);
s->rssi_output=0;
return 0;
}
if (!strcasecmp(cmd,"AT&T=RSSI")) {
emit(fd,"OK\r");
append_bytes(s, "OK\r", -1);
s->rssi_output=1;
return 0;
}
if (!strcasecmp(cmd,"ATI")) {
emit(fd,"RFD900a SIMULATOR 1.6\r");
emit(fd,"OK\r");
append_bytes(s, "RFD900a SIMULATOR 1.6\rOK\r", -1);
return 0;
}
emit(fd,"ERROR\r");
append_bytes(s, "ERROR\r", -1);
return 1;
}
int print_report=0;
int updateState(int fd,struct radio_state *s,int out_fd)
int dump(char *name, unsigned char *addr, int len)
{
int i;
print_report=0;
// Read bytes from stdin
int bytes=read(fd,&s->txbuffer[s->txb_len],sizeof(s->txbuffer)-s->txb_len);
if (bytes>0) { s->txb_len+=bytes; print_report=1; }
// Switch to command mode if required
if (bytes<1&&s->state==STATE_PLUSPLUSPLUS&&
(gettime_ms()-s->last_char_ms)>=1000) {
s->state=STATE_COMMAND;
print_report=1;
emit(fd,"OK\r\n");
} else
if (bytes>0)
s->last_char_ms=gettime_ms();
if (bytes>0) {
fprintf(stderr,"Received %d bytes: ",bytes);
for(i=0;i<bytes&&i<32;i++) {
unsigned char c=s->txbuffer[s->txb_len-bytes+i];
if (c>=' '&&c<0x7d) fprintf(stderr,"%c",c); else fprintf(stderr,"?");
}
if (bytes>20) fprintf(stderr,"...");
int i,j;
if (name)
fprintf(stderr,"Dump of %s\n",name);
for(i=0;i<len;i+=16){
fprintf(stderr," %04x :",i);
for(j=0;j<16&&(i+j)<len;j++)
fprintf(stderr," %02x",addr[i+j]);
for(;j<16;j++)
fprintf(stderr," ");
fprintf(stderr," ");
for(j=0;j<16&&(i+j)<len;j++)
fprintf(stderr,"%c",addr[i+j]>=' '&&addr[i+j]<0x7f?addr[i+j]:'.');
fprintf(stderr,"\n");
}
return 0;
}
// work out how many bytes we can dispatch
long long tx_count_allowed=gettime_ms()-s->last_tx_ms;
static void store_char(struct radio_state *s, unsigned char c)
{
if(s->txb_len<sizeof(s->txbuffer)){
s->txbuffer[s->txb_len++]=c;
}else{
log_time();
fprintf(stderr, "*** Dropped char %02x\n", c);
}
}
// now go through the TX buffer and dispatch them
// (or change state as appropriate)
for(i=0;i<tx_count_allowed;i++) {
if (s->txb_len<1) break;
switch(s->state) {
case STATE_ONLINE:
if (s->txbuffer[0]!='+') {
s->state=STATE_ONLINE;
char c[2]; c[0]=s->txbuffer[0]; c[1]=0;
emit(out_fd,c);
} else { s->state=STATE_PLUS; i--; }
break;
case STATE_PLUS:
if (s->txbuffer[0]!='+') {
s->state=STATE_ONLINE;
emit(out_fd,"+"); i+=1;
} else { s->state=STATE_PLUSPLUS; i--; }
break;
case STATE_PLUSPLUS:
if (s->txbuffer[0]!='+') {
s->state=STATE_ONLINE;
emit(out_fd,"++"); i+=2;
} else { s->state=STATE_PLUSPLUSPLUS; i--; }
break;
case STATE_PLUSPLUSPLUS:
if (s->txbuffer[0]!='+') {
s->state=STATE_ONLINE;
emit(out_fd,"+++"); i+=3;
} else {
// more than 3 pluses, so start outputting the
// extras
emit(out_fd,"+"); i+=1;
s->state=STATE_PLUSPLUSPLUS; i--;
}
break;
case STATE_COMMAND:
{
char c[2];
c[0]=s->txbuffer[0]; c[1]=0;
emit(fd,c);
if (s->txbuffer[0]=='\r'||s->txbuffer[0]=='\n') {
// end of command
processCommand(fd,s,out_fd);
s->cb_len=0;
} else {
if (s->cb_len<127) {
s->commandbuffer[s->cb_len++]=s->txbuffer[0];
}
}
}
}
// Remove processed character
if (s->txb_len>0) {
bcopy(&s->txbuffer[1],&s->txbuffer[0],s->txb_len);
s->txb_len--;
int read_bytes(struct radio_state *s)
{
unsigned char buff[8];
int i;
int bytes=read(s->fd,buff,sizeof(buff));
if (bytes<=0)
return bytes;
log_time();
fprintf(stderr, "Read from %s\n", s->name);
dump(NULL,buff,bytes);
s->last_char_ms = gettime_ms();
// process incoming bytes
for (i=0;i<bytes;i++){
// either append to a command buffer
if (s->state==STATE_COMMAND){
if (buff[i]=='\r'){
// and process the commend on EOL
processCommand(s);
s->cb_len=0;
// backspace characters
}else if (buff[i]=='\b'||buff[i]=='\x7f'){
if (s->cb_len>0)
s->cb_len--;
// append to command buffer
}else if (s->cb_len<127)
s->commandbuffer[s->cb_len++]=buff[i];
continue;
}
// or watch for "+++"
if (buff[i]=='+'){
if (s->state < STATE_PLUSPLUSPLUS)
s->state++;
}else
s->state=STATE_ONLINE;
// or append to the transmit buffer if there's room
store_char(s,buff[i]);
}
return bytes;
}
int write_bytes(struct radio_state *s)
{
int wrote=s->rxb_len;
if (wrote>8)
wrote=8;
if (s->last_char_ms)
wrote = write(s->fd, s->rxbuffer, wrote);
if (wrote>0){
log_time();
fprintf(stderr, "Wrote to %s\n", s->name);
dump(NULL, s->rxbuffer, wrote);
if (wrote < s->rxb_len)
bcopy(&s->rxbuffer[wrote], s->rxbuffer, s->rxb_len - wrote);
s->rxb_len -= wrote;
}
return wrote;
}
int transmitter=0;
long long next_transmit_time=0;
#define MAVLINK10_STX 254
#define RADIO_SOURCE_SYSTEM '3'
#define RADIO_SOURCE_COMPONENT 'D'
#define MAVLINK_MSG_ID_RADIO 166
#define MAVLINK_HDR 8
int MAVLINK_MESSAGE_CRCS[]={72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7};
uint16_t mavlink_crc(unsigned char *buf,int length)
{
uint16_t sum = 0xFFFF;
uint8_t i, stoplen;
stoplen = length + 6;
// MAVLink 1.0 has an extra CRC seed
buf[length+6] = MAVLINK_MESSAGE_CRCS[buf[5]];
stoplen++;
i = 1;
while (i<stoplen) {
uint8_t tmp;
tmp = buf[i] ^ (uint8_t)(sum&0xff);
tmp ^= (tmp<<4);
sum = (sum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4);
i++;
}
// Remember the current time for TX throttling
s->last_tx_ms=gettime_ms();
return sum;
}
// Output radio link status if requested
if (s->rssi_output&&(gettime_ms()-s->last_rssi_time_ms)>=1000) {
emit(fd,"L/R RSSI: 200/190 L/R noise: 80/70 pkts: 10 txe=0 rxe=0 stx=0 srx=0 ecc=0/0 temp=42 dco=0\r\n");
s->last_rssi_time_ms=gettime_ms();
}
if (print_report) {
s->commandbuffer[s->cb_len]=0;
fprintf(stderr,"Radio #%d state: %d rssi_output=%d cbuf='%s', txb_len=%d\n",
fd,s->state,s->rssi_output,s->commandbuffer,s->txb_len);
}
int build_heartbeat(struct radio_state *s){
if (s->rxb_len + MAVLINK_HDR + 9 > sizeof(s->rxbuffer))
return -1;
log_time();
fprintf(stderr,"Building heartbeat for %s\n", s->name);
unsigned char *b=&s->rxbuffer[s->rxb_len];
b[0] = MAVLINK10_STX;
b[1] = 9;
b[2] = s->seqnum++;
b[3] = RADIO_SOURCE_SYSTEM;
b[4] = RADIO_SOURCE_COMPONENT;
b[5] = MAVLINK_MSG_ID_RADIO;
b[6] = 0; //rxerrors
b[7] = 0; //rxerrors
b[8] = 0; //fixed
b[9] = 0; //fixed
b[10] = 43; //average RSSI
b[11] = 35; //remote average RSSI
int space = sizeof(s->txbuffer) - s->txb_len;
b[12] = ((space/8)*100) / (sizeof(s->txbuffer)/8); //txbuf space
b[13] = 20; //noise
b[14] = 20; //remote noise
uint16_t crc = mavlink_crc(b, 9);
b[15]=crc&0xFF;
b[16]=(crc>>8)&0xFF;
s->rxb_len += MAVLINK_HDR+9;
return 0;
}
int transfer_bytes(struct radio_state *radios)
{
// if there's data to transmit, copy a radio packet from one device to the other
int receiver = transmitter^1;
struct radio_state *r = &radios[receiver];
struct radio_state *t = &radios[transmitter];
int bytes=t->txb_len;
if (bytes > PACKET_SIZE)
bytes = PACKET_SIZE;
// try to send some number of whole mavlink frames from our buffer
{
int p=0, send=0;
while(p < bytes){
if (t->txbuffer[p]==MAVLINK10_STX){
// a mavlink header
// we can send everything before this header
if (p>0)
send = p-1;
// wait for more bytes or for the next transmit slot
// TODO add time limit
if (p+1 >= bytes)
break;
// how big is this mavlink frame?
int size = t->txbuffer[p+1];
// if the size is valid, try to send the whole packet at once
if (size <= PACKET_SIZE - MAVLINK_HDR){
// wait for more bytes or for the next transmit slot
// TODO add time limit
if (p+size+MAVLINK_HDR > bytes)
break;
// detect when we are about to transmit a heartbeat frame
if (size==9 && t->txbuffer[p+5]==0){
// reply to the host with a heartbeat
build_heartbeat(t);
}
p+=size+MAVLINK_HDR;
send=p;
continue;
}
}
// no valid mavlink frames? just send as much as we can
send=p;
p++;
}
if (send<bytes && !send){
if (bytes < PACKET_SIZE && t->wait_count++ <5){
log_time();
fprintf(stderr,"Waiting for more bytes for %s\n", t->name);
dump(NULL, t->txbuffer, bytes);
}else
send = bytes;
}
if (send)
t->wait_count=0;
bytes=send;
}
if (bytes>0){
log_time();
fprintf(stderr, "Transferring %d byte packet from %s to %s\n", bytes, t->name, r->name);
}
int i, j;
for (i=0;i<bytes && r->rxb_len<sizeof(r->rxbuffer);i++){
char byte = t->txbuffer[i];
// introduce bit errors
for(j=0;j<8;j++) {
if (random()<ber) {
byte^=(1<<j);
fprintf(stderr,"Flipped a bit\n");
}
}
r->rxbuffer[r->rxb_len++]=byte;
}
if (bytes>0 && bytes < t->txb_len)
bcopy(&t->txbuffer[bytes], t->txbuffer, t->txb_len - bytes);
t->txb_len-=bytes;
if (bytes==0 || --t->tx_count<=0){
// swap who's turn it is to transmit
transmitter = receiver;
r->tx_count=6;
}
// set the wait time for the next transmission
next_transmit_time = gettime_ms() + (bytes+10)/chars_per_ms;
return bytes;
}
int main(int argc,char **argv)
{
struct radio_state left_state,right_state;
bzero(&left_state,sizeof left_state);
bzero(&right_state,sizeof right_state);
// set actual throughput to match real RFD900 radios running at 128kbit with golay encoding
// (assumes 70% efficiency for TDMA)
left_state.tx_rate=128000/2*0.7;
right_state.tx_rate=128000/2*0.7;
int left=posix_openpt(O_RDWR|O_NOCTTY);
grantpt(left); unlockpt(left);
int right=posix_openpt(O_RDWR|O_NOCTTY);
grantpt(right); unlockpt(right);
fprintf(stdout,"%s\n",ptsname(left));
fprintf(stdout,"%s\n",ptsname(right));
fflush(stdout);
fcntl(left,F_SETFL,fcntl(left, F_GETFL, NULL)|O_NONBLOCK);
fcntl(right,F_SETFL,fcntl(right, F_GETFL, NULL)|O_NONBLOCK);
if (argv[1]) {
chars_per_ms=atol(argv[1]);
if (argv[2])
ber=atol(argv[2]);
}
fprintf(stderr, "Sending %d bytes per ms\n", chars_per_ms);
fprintf(stderr, "Introducing %f%% bit errors\n", (ber * 100.0) / 0xFFFFFFFF);
struct pollfd fds[2];
struct radio_state radios[2];
bzero(&radios,sizeof radios);
int i;
fds[0].fd=left;
fds[0].events=POLLIN;
fds[1].fd=right;
fds[1].events=POLLIN;
for (i=0;i<2;i++){
radios[i].fd=posix_openpt(O_RDWR|O_NOCTTY);
grantpt(radios[i].fd);
unlockpt(radios[i].fd);
fcntl(radios[i].fd,F_SETFL,fcntl(radios[i].fd, F_GETFL, NULL)|O_NONBLOCK);
fprintf(stdout,"%s\n",ptsname(radios[i].fd));
fds[i].fd = radios[i].fd;
}
radios[0].name="left";
radios[1].name="right";
fflush(stdout);
while(1) {
poll(fds,2,10);
updateState(left,&left_state,right);
updateState(right,&right_state,left);
for(i=0;i<2;i++) {
fds[i].revents=0;
if (fds[i].revents&~POLLIN)
printf("revents %x\n", fds[i].revents);
// what events do we need to poll for? how long can we block?
long long now = gettime_ms();
long long next_event = now+10000;
for (i=0;i<2;i++){
// always watch for incoming data, though we will throw it away if we run out of buffer space
fds[i].events = POLLIN;
// if we have data to write data, watch for POLLOUT too.
if (radios[i].rxb_len)
fds[i].events |= POLLOUT;
if (radios[i].rssi_output && next_event > radios[i].next_rssi_time_ms)
next_event = radios[i].next_rssi_time_ms;
if (radios[i].state==STATE_PLUSPLUSPLUS && next_event > radios[i].last_char_ms+1000)
next_event = radios[i].last_char_ms+1000;
if (radios[i].txb_len && next_event > next_transmit_time)
next_event = next_transmit_time;
}
int delay = next_event - now;
if (delay<0)
delay=0;
poll(fds,2,delay);
for (i=0;i<2;i++){
if (fds[i].revents & POLLIN)
read_bytes(&radios[i]);
if (fds[i].revents & POLLOUT)
write_bytes(&radios[i]);
now = gettime_ms();
if (radios[i].rssi_output && now >= radios[i].next_rssi_time_ms){
if (append_bytes(&radios[i], "L/R RSSI: 200/190 L/R noise: 80/70 pkts: 10 txe=0 rxe=0 stx=0 srx=0 ecc=0/0 temp=42 dco=0\r\n", -1)>0)
radios[i].next_rssi_time_ms=now+1000;
}
if (radios[i].state==STATE_PLUSPLUSPLUS && now >= radios[i].last_char_ms+1000){
fprintf(stderr, "Detected +++ from %s\n",radios[i].name);
if (append_bytes(&radios[i], "OK\r\n", -1)>0)
radios[i].state=STATE_COMMAND;
}
}
if (now >= next_transmit_time)
transfer_bytes(radios);
}
return 0;

2
fec-3.0.1/README Normal file
View File

@ -0,0 +1,2 @@
Code from http://www.ka9q.net/code/fec/
Used under the LGPL.

43
fec-3.0.1/ccsds_tables.c Normal file
View File

@ -0,0 +1,43 @@
char CCSDS_alpha_to[] = {
0x01,0x02,0x04,0x08,0x10,0x20,0x40,0x80,0x87,0x89,0x95,0xad,0xdd,0x3d,0x7a,0xf4,
0x6f,0xde,0x3b,0x76,0xec,0x5f,0xbe,0xfb,0x71,0xe2,0x43,0x86,0x8b,0x91,0xa5,0xcd,
0x1d,0x3a,0x74,0xe8,0x57,0xae,0xdb,0x31,0x62,0xc4,0x0f,0x1e,0x3c,0x78,0xf0,0x67,
0xce,0x1b,0x36,0x6c,0xd8,0x37,0x6e,0xdc,0x3f,0x7e,0xfc,0x7f,0xfe,0x7b,0xf6,0x6b,
0xd6,0x2b,0x56,0xac,0xdf,0x39,0x72,0xe4,0x4f,0x9e,0xbb,0xf1,0x65,0xca,0x13,0x26,
0x4c,0x98,0xb7,0xe9,0x55,0xaa,0xd3,0x21,0x42,0x84,0x8f,0x99,0xb5,0xed,0x5d,0xba,
0xf3,0x61,0xc2,0x03,0x06,0x0c,0x18,0x30,0x60,0xc0,0x07,0x0e,0x1c,0x38,0x70,0xe0,
0x47,0x8e,0x9b,0xb1,0xe5,0x4d,0x9a,0xb3,0xe1,0x45,0x8a,0x93,0xa1,0xc5,0x0d,0x1a,
0x34,0x68,0xd0,0x27,0x4e,0x9c,0xbf,0xf9,0x75,0xea,0x53,0xa6,0xcb,0x11,0x22,0x44,
0x88,0x97,0xa9,0xd5,0x2d,0x5a,0xb4,0xef,0x59,0xb2,0xe3,0x41,0x82,0x83,0x81,0x85,
0x8d,0x9d,0xbd,0xfd,0x7d,0xfa,0x73,0xe6,0x4b,0x96,0xab,0xd1,0x25,0x4a,0x94,0xaf,
0xd9,0x35,0x6a,0xd4,0x2f,0x5e,0xbc,0xff,0x79,0xf2,0x63,0xc6,0x0b,0x16,0x2c,0x58,
0xb0,0xe7,0x49,0x92,0xa3,0xc1,0x05,0x0a,0x14,0x28,0x50,0xa0,0xc7,0x09,0x12,0x24,
0x48,0x90,0xa7,0xc9,0x15,0x2a,0x54,0xa8,0xd7,0x29,0x52,0xa4,0xcf,0x19,0x32,0x64,
0xc8,0x17,0x2e,0x5c,0xb8,0xf7,0x69,0xd2,0x23,0x46,0x8c,0x9f,0xb9,0xf5,0x6d,0xda,
0x33,0x66,0xcc,0x1f,0x3e,0x7c,0xf8,0x77,0xee,0x5b,0xb6,0xeb,0x51,0xa2,0xc3,0x00,
};
char CCSDS_index_of[] = {
255, 0, 1, 99, 2,198,100,106, 3,205,199,188,101,126,107, 42,
4,141,206, 78,200,212,189,225,102,221,127, 49,108, 32, 43,243,
5, 87,142,232,207,172, 79,131,201,217,213, 65,190,148,226,180,
103, 39,222,240,128,177, 50, 53,109, 69, 33, 18, 44, 13,244, 56,
6,155, 88, 26,143,121,233,112,208,194,173,168, 80,117,132, 72,
202,252,218,138,214, 84, 66, 36,191,152,149,249,227, 94,181, 21,
104, 97, 40,186,223, 76,241, 47,129,230,178, 63, 51,238, 54, 16,
110, 24, 70,166, 34,136, 19,247, 45,184, 14, 61,245,164, 57, 59,
7,158,156,157, 89,159, 27, 8,144, 9,122, 28,234,160,113, 90,
209, 29,195,123,174, 10,169,145, 81, 91,118,114,133,161, 73,235,
203,124,253,196,219, 30,139,210,215,146, 85,170, 67, 11, 37,175,
192,115,153,119,150, 92,250, 82,228,236, 95, 74,182,162, 22,134,
105,197, 98,254, 41,125,187,204,224,211, 77,140,242, 31, 48,220,
130,171,231, 86,179,147, 64,216, 52,176,239, 38, 55, 12, 17, 68,
111,120, 25,154, 71,116,167,193, 35, 83,137,251, 20, 93,248,151,
46, 75,185, 96, 15,237, 62,229,246,135,165, 23, 58,163, 60,183,
};
char CCSDS_poly[] = {
0,249, 59, 66, 4, 43,126,251, 97, 30, 3,213, 50, 66,170, 5,
24, 5,170, 66, 50,213, 3, 30, 97,251,126, 43, 4, 66, 59,249,
0,
};

24
fec-3.0.1/char.h Normal file
View File

@ -0,0 +1,24 @@
/* Stuff specific to the 8-bit symbol version of the general purpose RS codecs
*
* Copyright 2003, Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
typedef unsigned char data_t;
#define MODNN(x) modnn(rs,x)
#define MM (rs->mm)
#define NN (rs->nn)
#define ALPHA_TO (rs->alpha_to)
#define INDEX_OF (rs->index_of)
#define GENPOLY (rs->genpoly)
#define NROOTS (rs->nroots)
#define FCR (rs->fcr)
#define PRIM (rs->prim)
#define IPRIM (rs->iprim)
#define PAD (rs->pad)
#define A0 (NN)

298
fec-3.0.1/decode_rs.h Normal file
View File

@ -0,0 +1,298 @@
/* The guts of the Reed-Solomon decoder, meant to be #included
* into a function body with the following typedefs, macros and variables supplied
* according to the code parameters:
* data_t - a typedef for the data symbol
* data_t data[] - array of NN data and parity symbols to be corrected in place
* retval - an integer lvalue into which the decoder's return code is written
* NROOTS - the number of roots in the RS code generator polynomial,
* which is the same as the number of parity symbols in a block.
Integer variable or literal.
* NN - the total number of symbols in a RS block. Integer variable or literal.
* PAD - the number of pad symbols in a block. Integer variable or literal.
* ALPHA_TO - The address of an array of NN elements to convert Galois field
* elements in index (log) form to polynomial form. Read only.
* INDEX_OF - The address of an array of NN elements to convert Galois field
* elements in polynomial form to index (log) form. Read only.
* MODNN - a function to reduce its argument modulo NN. May be inline or a macro.
* FCR - An integer literal or variable specifying the first consecutive root of the
* Reed-Solomon generator polynomial. Integer variable or literal.
* PRIM - The primitive root of the generator poly. Integer variable or literal.
* DEBUG - If set to 1 or more, do various internal consistency checking. Leave this
* undefined for production code
* The memset(), memmove(), and memcpy() functions are used. The appropriate header
* file declaring these functions (usually <string.h>) must be included by the calling
* program.
*/
#if !defined(NROOTS)
#error "NROOTS not defined"
#endif
#if !defined(NN)
#error "NN not defined"
#endif
#if !defined(PAD)
#error "PAD not defined"
#endif
#if !defined(ALPHA_TO)
#error "ALPHA_TO not defined"
#endif
#if !defined(INDEX_OF)
#error "INDEX_OF not defined"
#endif
#if !defined(MODNN)
#error "MODNN not defined"
#endif
#if !defined(FCR)
#error "FCR not defined"
#endif
#if !defined(PRIM)
#error "PRIM not defined"
#endif
#if !defined(NULL)
#define NULL ((void *)0)
#endif
#undef MIN
#define MIN(a,b) ((a) < (b) ? (a) : (b))
#undef A0
#define A0 (NN)
{
int deg_lambda, el, deg_omega;
int i, j, r,k;
data_t u,q,tmp,num1,num2,den,discr_r;
data_t lambda[NROOTS+1], s[NROOTS]; /* Err+Eras Locator poly
* and syndrome poly */
data_t b[NROOTS+1], t[NROOTS+1], omega[NROOTS+1];
data_t root[NROOTS], reg[NROOTS+1], loc[NROOTS];
int syn_error, count;
/* form the syndromes; i.e., evaluate data(x) at roots of g(x) */
for(i=0;i<NROOTS;i++)
s[i] = data[0];
for(j=1;j<NN-PAD;j++){
for(i=0;i<NROOTS;i++){
if(s[i] == 0){
s[i] = data[j];
} else {
s[i] = data[j] ^ ALPHA_TO[MODNN(INDEX_OF[s[i]] + (FCR+i)*PRIM)];
}
}
}
/* Convert syndromes to index form, checking for nonzero condition */
syn_error = 0;
for(i=0;i<NROOTS;i++){
syn_error |= s[i];
s[i] = INDEX_OF[s[i]];
}
if (!syn_error) {
/* if syndrome is zero, data[] is a codeword and there are no
* errors to correct. So return data[] unmodified
*/
count = 0;
goto finish;
}
memset(&lambda[1],0,NROOTS*sizeof(lambda[0]));
lambda[0] = 1;
if (no_eras > 0) {
/* Init lambda to be the erasure locator polynomial */
lambda[1] = ALPHA_TO[MODNN(PRIM*(NN-1-eras_pos[0]))];
for (i = 1; i < no_eras; i++) {
u = MODNN(PRIM*(NN-1-eras_pos[i]));
for (j = i+1; j > 0; j--) {
tmp = INDEX_OF[lambda[j - 1]];
if(tmp != A0)
lambda[j] ^= ALPHA_TO[MODNN(u + tmp)];
}
}
#if DEBUG >= 1
/* Test code that verifies the erasure locator polynomial just constructed
Needed only for decoder debugging. */
/* find roots of the erasure location polynomial */
for(i=1;i<=no_eras;i++)
reg[i] = INDEX_OF[lambda[i]];
count = 0;
for (i = 1,k=IPRIM-1; i <= NN; i++,k = MODNN(k+IPRIM)) {
q = 1;
for (j = 1; j <= no_eras; j++)
if (reg[j] != A0) {
reg[j] = MODNN(reg[j] + j);
q ^= ALPHA_TO[reg[j]];
}
if (q != 0)
continue;
/* store root and error location number indices */
root[count] = i;
loc[count] = k;
count++;
}
if (count != no_eras) {
printf("count = %d no_eras = %d\n lambda(x) is WRONG\n",count,no_eras);
count = -1;
goto finish;
}
#if DEBUG >= 2
printf("\n Erasure positions as determined by roots of Eras Loc Poly:\n");
for (i = 0; i < count; i++)
printf("%d ", loc[i]);
printf("\n");
#endif
#endif
}
for(i=0;i<NROOTS+1;i++)
b[i] = INDEX_OF[lambda[i]];
/*
* Begin Berlekamp-Massey algorithm to determine error+erasure
* locator polynomial
*/
r = no_eras;
el = no_eras;
while (++r <= NROOTS) { /* r is the step number */
/* Compute discrepancy at the r-th step in poly-form */
discr_r = 0;
for (i = 0; i < r; i++){
if ((lambda[i] != 0) && (s[r-i-1] != A0)) {
discr_r ^= ALPHA_TO[MODNN(INDEX_OF[lambda[i]] + s[r-i-1])];
}
}
discr_r = INDEX_OF[discr_r]; /* Index form */
if (discr_r == A0) {
/* 2 lines below: B(x) <-- x*B(x) */
memmove(&b[1],b,NROOTS*sizeof(b[0]));
b[0] = A0;
} else {
/* 7 lines below: T(x) <-- lambda(x) - discr_r*x*b(x) */
t[0] = lambda[0];
for (i = 0 ; i < NROOTS; i++) {
if(b[i] != A0)
t[i+1] = lambda[i+1] ^ ALPHA_TO[MODNN(discr_r + b[i])];
else
t[i+1] = lambda[i+1];
}
if (2 * el <= r + no_eras - 1) {
el = r + no_eras - el;
/*
* 2 lines below: B(x) <-- inv(discr_r) *
* lambda(x)
*/
for (i = 0; i <= NROOTS; i++)
b[i] = (lambda[i] == 0) ? A0 : MODNN(INDEX_OF[lambda[i]] - discr_r + NN);
} else {
/* 2 lines below: B(x) <-- x*B(x) */
memmove(&b[1],b,NROOTS*sizeof(b[0]));
b[0] = A0;
}
memcpy(lambda,t,(NROOTS+1)*sizeof(t[0]));
}
}
/* Convert lambda to index form and compute deg(lambda(x)) */
deg_lambda = 0;
for(i=0;i<NROOTS+1;i++){
lambda[i] = INDEX_OF[lambda[i]];
if(lambda[i] != A0)
deg_lambda = i;
}
/* Find roots of the error+erasure locator polynomial by Chien search */
memcpy(&reg[1],&lambda[1],NROOTS*sizeof(reg[0]));
count = 0; /* Number of roots of lambda(x) */
for (i = 1,k=IPRIM-1; i <= NN; i++,k = MODNN(k+IPRIM)) {
q = 1; /* lambda[0] is always 0 */
for (j = deg_lambda; j > 0; j--){
if (reg[j] != A0) {
reg[j] = MODNN(reg[j] + j);
q ^= ALPHA_TO[reg[j]];
}
}
if (q != 0)
continue; /* Not a root */
/* store root (index-form) and error location number */
#if DEBUG>=2
printf("count %d root %d loc %d\n",count,i,k);
#endif
root[count] = i;
loc[count] = k;
/* If we've already found max possible roots,
* abort the search to save time
*/
if(++count == deg_lambda)
break;
}
if (deg_lambda != count) {
/*
* deg(lambda) unequal to number of roots => uncorrectable
* error detected
*/
count = -1;
goto finish;
}
/*
* Compute err+eras evaluator poly omega(x) = s(x)*lambda(x) (modulo
* x**NROOTS). in index form. Also find deg(omega).
*/
deg_omega = deg_lambda-1;
for (i = 0; i <= deg_omega;i++){
tmp = 0;
for(j=i;j >= 0; j--){
if ((s[i - j] != A0) && (lambda[j] != A0))
tmp ^= ALPHA_TO[MODNN(s[i - j] + lambda[j])];
}
omega[i] = INDEX_OF[tmp];
}
/*
* Compute error values in poly-form. num1 = omega(inv(X(l))), num2 =
* inv(X(l))**(FCR-1) and den = lambda_pr(inv(X(l))) all in poly-form
*/
for (j = count-1; j >=0; j--) {
num1 = 0;
for (i = deg_omega; i >= 0; i--) {
if (omega[i] != A0)
num1 ^= ALPHA_TO[MODNN(omega[i] + i * root[j])];
}
num2 = ALPHA_TO[MODNN(root[j] * (FCR - 1) + NN)];
den = 0;
/* lambda[i+1] for i even is the formal derivative lambda_pr of lambda[i] */
for (i = MIN(deg_lambda,NROOTS-1) & ~1; i >= 0; i -=2) {
if(lambda[i+1] != A0)
den ^= ALPHA_TO[MODNN(lambda[i+1] + i * root[j])];
}
#if DEBUG >= 1
if (den == 0) {
printf("\n ERROR: denominator = 0\n");
count = -1;
goto finish;
}
#endif
/* Apply error to data */
if (num1 != 0 && loc[j] >= PAD && loc[j] < NN-NROOTS) {
data[loc[j]-PAD] ^= ALPHA_TO[MODNN(INDEX_OF[num1] + INDEX_OF[num2] + NN - INDEX_OF[den])];
}
}
finish:
if(eras_pos != NULL){
for(i=0;i<count;i++)
eras_pos[i] = loc[i];
}
retval = count;
}

24
fec-3.0.1/decode_rs_8.c Normal file
View File

@ -0,0 +1,24 @@
/* General purpose Reed-Solomon decoder for 8-bit symbols or less
* Copyright 2003 Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
#ifdef DEBUG
#include <stdio.h>
#endif
#include <string.h>
#include "fixed.h"
int decode_rs_8(data_t *data, int *eras_pos, int no_eras, int pad){
int retval;
if(pad < 0 || pad > 222){
return -1;
}
#include "decode_rs.h"
return retval;
}

58
fec-3.0.1/encode_rs.h Normal file
View File

@ -0,0 +1,58 @@
/* The guts of the Reed-Solomon encoder, meant to be #included
* into a function body with the following typedefs, macros and variables supplied
* according to the code parameters:
* data_t - a typedef for the data symbol
* data_t data[] - array of NN-NROOTS-PAD and type data_t to be encoded
* data_t parity[] - an array of NROOTS and type data_t to be written with parity symbols
* NROOTS - the number of roots in the RS code generator polynomial,
* which is the same as the number of parity symbols in a block.
Integer variable or literal.
*
* NN - the total number of symbols in a RS block. Integer variable or literal.
* PAD - the number of pad symbols in a block. Integer variable or literal.
* ALPHA_TO - The address of an array of NN elements to convert Galois field
* elements in index (log) form to polynomial form. Read only.
* INDEX_OF - The address of an array of NN elements to convert Galois field
* elements in polynomial form to index (log) form. Read only.
* MODNN - a function to reduce its argument modulo NN. May be inline or a macro.
* GENPOLY - an array of NROOTS+1 elements containing the generator polynomial in index form
* The memset() and memmove() functions are used. The appropriate header
* file declaring these functions (usually <string.h>) must be included by the calling
* program.
* Copyright 2004, Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
#undef A0
#define A0 (NN) /* Special reserved value encoding zero in index form */
{
int i, j;
data_t feedback;
memset(parity,0,NROOTS*sizeof(data_t));
for(i=0;i<NN-NROOTS-PAD;i++){
feedback = INDEX_OF[data[i] ^ parity[0]];
if(feedback != A0){ /* feedback term is non-zero */
#ifdef UNNORMALIZED
/* This line is unnecessary when GENPOLY[NROOTS] is unity, as it must
* always be for the polynomials constructed by init_rs()
*/
feedback = MODNN(NN - GENPOLY[NROOTS] + feedback);
#endif
for(j=1;j<NROOTS;j++)
parity[j] ^= ALPHA_TO[MODNN(feedback + GENPOLY[NROOTS-j])];
}
/* Shift */
memmove(&parity[0],&parity[1],sizeof(data_t)*(NROOTS-1));
if(feedback != A0)
parity[NROOTS-1] = ALPHA_TO[MODNN(feedback + GENPOLY[0])];
else
parity[NROOTS-1] = 0;
}
}

81
fec-3.0.1/encode_rs_8.c Normal file
View File

@ -0,0 +1,81 @@
/* Reed-Solomon encoder
* Copyright 2004, Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
#include <string.h>
#include "fixed.h"
#ifdef __VEC__
#include <sys/sysctl.h>
#endif
static enum {UNKNOWN=0,MMX,SSE,SSE2,ALTIVEC,PORT} cpu_mode;
static void encode_rs_8_c(data_t *data, data_t *parity,int pad);
#if __vec__
static void encode_rs_8_av(data_t *data, data_t *parity,int pad);
#endif
void encode_rs_8(data_t *data, data_t *parity,int pad){
if(cpu_mode == UNKNOWN){
cpu_mode = PORT;
}
switch(cpu_mode){
#if __vec__
case ALTIVEC:
encode_rs_8_av(data,parity,pad);
return;
#endif
#if __i386__
case MMX:
case SSE:
case SSE2:
#endif
default:
encode_rs_8_c(data,parity,pad);
return;
}
}
#if __vec__ /* PowerPC G4/G5 Altivec instructions are available */
static vector unsigned char reverse = (vector unsigned char)(0,15,14,13,12,11,10,9,8,7,6,5,4,3,2,1);
static vector unsigned char shift_right = (vector unsigned char)(15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30);
/* Lookup table for feedback multiplications
* These are the low half of the coefficients. Since the generator polynomial is
* palindromic, we form the other half by reversing this one
*/
extern static union { vector unsigned char v; unsigned char c[16]; } table[256];
static void encode_rs_8_av(data_t *data, data_t *parity,int pad){
union { vector unsigned char v[2]; unsigned char c[32]; } shift_register;
int i;
shift_register.v[0] = (vector unsigned char)(0);
shift_register.v[1] = (vector unsigned char)(0);
for(i=0;i<NN-NROOTS-pad;i++){
vector unsigned char feedback0,feedback1;
unsigned char f;
f = data[i] ^ shift_register.c[31];
feedback1 = table[f].v;
feedback0 = vec_perm(feedback1,feedback1,reverse);
/* Shift right one byte */
shift_register.v[1] = vec_perm(shift_register.v[0],shift_register.v[1],shift_right) ^ feedback1;
shift_register.v[0] = vec_sro(shift_register.v[0],(vector unsigned char)(8)) ^ feedback0;
shift_register.c[0] = f;
}
for(i=0;i<NROOTS;i++)
parity[NROOTS-i-1] = shift_register.c[i];
}
#endif
/* Portable C version */
static void encode_rs_8_c(data_t *data, data_t *parity,int pad){
#include "encode_rs.h"
}

347
fec-3.0.1/fec.h Normal file
View File

@ -0,0 +1,347 @@
/* User include file for libfec
* Copyright 2004, Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
#ifndef _FEC_H_
#define _FEC_H_
/* r=1/2 k=7 convolutional encoder polynomials
* The NASA-DSN convention is to use V27POLYA inverted, then V27POLYB
* The CCSDS/NASA-GSFC convention is to use V27POLYB, then V27POLYA inverted
*/
#define V27POLYA 0x6d
#define V27POLYB 0x4f
void *create_viterbi27(int len);
void set_viterbi27_polynomial(int polys[2]);
int init_viterbi27(void *vp,int starting_state);
int update_viterbi27_blk(void *vp,unsigned char sym[],int npairs);
int chainback_viterbi27(void *vp, unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi27(void *vp);
#ifdef __VEC__
void *create_viterbi27_av(int len);
void set_viterbi27_polynomial_av(int polys[2]);
int init_viterbi27_av(void *p,int starting_state);
int chainback_viterbi27_av(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi27_av(void *p);
int update_viterbi27_blk_av(void *p,unsigned char *syms,int nbits);
#endif
#ifdef __i386__
void *create_viterbi27_mmx(int len);
void set_viterbi27_polynomial_mmx(int polys[2]);
int init_viterbi27_mmx(void *p,int starting_state);
int chainback_viterbi27_mmx(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi27_mmx(void *p);
int update_viterbi27_blk_mmx(void *p,unsigned char *syms,int nbits);
void *create_viterbi27_sse(int len);
void set_viterbi27_polynomial_sse(int polys[2]);
int init_viterbi27_sse(void *p,int starting_state);
int chainback_viterbi27_sse(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi27_sse(void *p);
int update_viterbi27_blk_sse(void *p,unsigned char *syms,int nbits);
void *create_viterbi27_sse2(int len);
void set_viterbi27_polynomial_sse2(int polys[2]);
int init_viterbi27_sse2(void *p,int starting_state);
int chainback_viterbi27_sse2(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi27_sse2(void *p);
int update_viterbi27_blk_sse2(void *p,unsigned char *syms,int nbits);
#endif
void *create_viterbi27_port(int len);
void set_viterbi27_polynomial_port(int polys[2]);
int init_viterbi27_port(void *p,int starting_state);
int chainback_viterbi27_port(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi27_port(void *p);
int update_viterbi27_blk_port(void *p,unsigned char *syms,int nbits);
/* r=1/2 k=9 convolutional encoder polynomials */
#define V29POLYA 0x1af
#define V29POLYB 0x11d
void *create_viterbi29(int len);
void set_viterbi29_polynomial(int polys[2]);
int init_viterbi29(void *vp,int starting_state);
int update_viterbi29_blk(void *vp,unsigned char syms[],int nbits);
int chainback_viterbi29(void *vp, unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi29(void *vp);
#ifdef __VEC__
void *create_viterbi29_av(int len);
void set_viterbi29_polynomial_av(int polys[2]);
int init_viterbi29_av(void *p,int starting_state);
int chainback_viterbi29_av(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi29_av(void *p);
int update_viterbi29_blk_av(void *p,unsigned char *syms,int nbits);
#endif
#ifdef __i386__
void *create_viterbi29_mmx(int len);
void set_viterbi29_polynomial_mmx(int polys[2]);
int init_viterbi29_mmx(void *p,int starting_state);
int chainback_viterbi29_mmx(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi29_mmx(void *p);
int update_viterbi29_blk_mmx(void *p,unsigned char *syms,int nbits);
void *create_viterbi29_sse(int len);
void set_viterbi29_polynomial_sse(int polys[2]);
int init_viterbi29_sse(void *p,int starting_state);
int chainback_viterbi29_sse(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi29_sse(void *p);
int update_viterbi29_blk_sse(void *p,unsigned char *syms,int nbits);
void *create_viterbi29_sse2(int len);
void set_viterbi29_polynomial_sse2(int polys[2]);
int init_viterbi29_sse2(void *p,int starting_state);
int chainback_viterbi29_sse2(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi29_sse2(void *p);
int update_viterbi29_blk_sse2(void *p,unsigned char *syms,int nbits);
#endif
void *create_viterbi29_port(int len);
void set_viterbi29_polynomial_port(int polys[2]);
int init_viterbi29_port(void *p,int starting_state);
int chainback_viterbi29_port(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi29_port(void *p);
int update_viterbi29_blk_port(void *p,unsigned char *syms,int nbits);
/* r=1/3 k=9 convolutional encoder polynomials */
#define V39POLYA 0x1ed
#define V39POLYB 0x19b
#define V39POLYC 0x127
void *create_viterbi39(int len);
void set_viterbi39_polynomial(int polys[3]);
int init_viterbi39(void *vp,int starting_state);
int update_viterbi39_blk(void *vp,unsigned char syms[],int nbits);
int chainback_viterbi39(void *vp, unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi39(void *vp);
#ifdef __VEC__
void *create_viterbi39_av(int len);
void set_viterbi39_polynomial_av(int polys[3]);
int init_viterbi39_av(void *p,int starting_state);
int chainback_viterbi39_av(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi39_av(void *p);
int update_viterbi39_blk_av(void *p,unsigned char *syms,int nbits);
#endif
#ifdef __i386__
void *create_viterbi39_mmx(int len);
void set_viterbi39_polynomial_mmx(int polys[3]);
int init_viterbi39_mmx(void *p,int starting_state);
int chainback_viterbi39_mmx(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi39_mmx(void *p);
int update_viterbi39_blk_mmx(void *p,unsigned char *syms,int nbits);
void *create_viterbi39_sse(int len);
void set_viterbi39_polynomial_sse(int polys[3]);
int init_viterbi39_sse(void *p,int starting_state);
int chainback_viterbi39_sse(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi39_sse(void *p);
int update_viterbi39_blk_sse(void *p,unsigned char *syms,int nbits);
void *create_viterbi39_sse2(int len);
void set_viterbi39_polynomial_sse2(int polys[3]);
int init_viterbi39_sse2(void *p,int starting_state);
int chainback_viterbi39_sse2(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi39_sse2(void *p);
int update_viterbi39_blk_sse2(void *p,unsigned char *syms,int nbits);
#endif
void *create_viterbi39_port(int len);
void set_viterbi39_polynomial_port(int polys[3]);
int init_viterbi39_port(void *p,int starting_state);
int chainback_viterbi39_port(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi39_port(void *p);
int update_viterbi39_blk_port(void *p,unsigned char *syms,int nbits);
/* r=1/6 k=15 Cassini convolutional encoder polynomials without symbol inversion
* dfree = 56
* These bits may be left-right flipped from some textbook representations;
* here I have the bits entering the shift register from the right (low) end
*
* Some other spacecraft use the same code, but with the polynomials in a different order.
* E.g., Mars Pathfinder and STEREO swap POLYC and POLYD. All use alternate symbol inversion,
* so use set_viterbi615_polynomial() as appropriate.
*/
#define V615POLYA 042631
#define V615POLYB 047245
#define V615POLYC 056507
#define V615POLYD 073363
#define V615POLYE 077267
#define V615POLYF 064537
void *create_viterbi615(int len);
void set_viterbi615_polynomial(int polys[6]);
int init_viterbi615(void *vp,int starting_state);
int update_viterbi615_blk(void *vp,unsigned char *syms,int nbits);
int chainback_viterbi615(void *vp, unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi615(void *vp);
#ifdef __VEC__
void *create_viterbi615_av(int len);
void set_viterbi615_polynomial_av(int polys[6]);
int init_viterbi615_av(void *p,int starting_state);
int chainback_viterbi615_av(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi615_av(void *p);
int update_viterbi615_blk_av(void *p,unsigned char *syms,int nbits);
#endif
#ifdef __i386__
void *create_viterbi615_mmx(int len);
void set_viterbi615_polynomial_mmx(int polys[6]);
int init_viterbi615_mmx(void *p,int starting_state);
int chainback_viterbi615_mmx(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi615_mmx(void *p);
int update_viterbi615_blk_mmx(void *p,unsigned char *syms,int nbits);
void *create_viterbi615_sse(int len);
void set_viterbi615_polynomial_sse(int polys[6]);
int init_viterbi615_sse(void *p,int starting_state);
int chainback_viterbi615_sse(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi615_sse(void *p);
int update_viterbi615_blk_sse(void *p,unsigned char *syms,int nbits);
void *create_viterbi615_sse2(int len);
void set_viterbi615_polynomial_sse2(int polys[6]);
int init_viterbi615_sse2(void *p,int starting_state);
int chainback_viterbi615_sse2(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi615_sse2(void *p);
int update_viterbi615_blk_sse2(void *p,unsigned char *syms,int nbits);
#endif
void *create_viterbi615_port(int len);
void set_viterbi615_polynomial_port(int polys[6]);
int init_viterbi615_port(void *p,int starting_state);
int chainback_viterbi615_port(void *p,unsigned char *data,unsigned int nbits,unsigned int endstate);
void delete_viterbi615_port(void *p);
int update_viterbi615_blk_port(void *p,unsigned char *syms,int nbits);
/* General purpose RS codec, 8-bit symbols */
void encode_rs_char(void *rs,unsigned char *data,unsigned char *parity);
int decode_rs_char(void *rs,unsigned char *data,int *eras_pos,
int no_eras);
void *init_rs_char(int symsize,int gfpoly,
int fcr,int prim,int nroots,
int pad);
void free_rs_char(void *rs);
/* General purpose RS codec, integer symbols */
void encode_rs_int(void *rs,int *data,int *parity);
int decode_rs_int(void *rs,int *data,int *eras_pos,int no_eras);
void *init_rs_int(int symsize,int gfpoly,int fcr,
int prim,int nroots,int pad);
void free_rs_int(void *rs);
/* CCSDS standard (255,223) RS codec with conventional (*not* dual-basis)
* symbol representation
*/
void encode_rs_8(unsigned char *data,unsigned char *parity,int pad);
int decode_rs_8(unsigned char *data,int *eras_pos,int no_eras,int pad);
/* CCSDS standard (255,223) RS codec with dual-basis symbol representation */
void encode_rs_ccsds(unsigned char *data,unsigned char *parity,int pad);
int decode_rs_ccsds(unsigned char *data,int *eras_pos,int no_eras,int pad);
/* Tables to map from conventional->dual (Taltab) and
* dual->conventional (Tal1tab) bases
*/
extern unsigned char Taltab[],Tal1tab[];
/* CPU SIMD instruction set available */
extern enum cpu_mode {UNKNOWN=0,PORT,MMX,SSE,SSE2,ALTIVEC} Cpu_mode;
void find_cpu_mode(void); /* Call this once at startup to set Cpu_mode */
/* Determine parity of argument: 1 = odd, 0 = even */
#ifdef __i386__
static inline int parityb(unsigned char x){
__asm__ __volatile__ ("test %1,%1;setpo %0" : "=g"(x) : "r" (x));
return x;
}
#else
void partab_init();
static inline int parityb(unsigned char x){
extern unsigned char Partab[256];
extern int P_init;
if(!P_init){
partab_init();
}
return Partab[x];
}
#endif
static inline int parity(int x){
/* Fold down to one byte */
x ^= (x >> 16);
x ^= (x >> 8);
return parityb(x);
}
/* Useful utilities for simulation */
double normal_rand(double mean, double std_dev);
unsigned char addnoise(int sym,double amp,double gain,double offset,int clip);
extern int Bitcnt[];
/* Dot product functions */
void *initdp(signed short coeffs[],int len);
void freedp(void *dp);
long dotprod(void *dp,signed short a[]);
void *initdp_port(signed short coeffs[],int len);
void freedp_port(void *dp);
long dotprod_port(void *dp,signed short a[]);
#ifdef __i386__
void *initdp_mmx(signed short coeffs[],int len);
void freedp_mmx(void *dp);
long dotprod_mmx(void *dp,signed short a[]);
void *initdp_sse(signed short coeffs[],int len);
void freedp_sse(void *dp);
long dotprod_sse(void *dp,signed short a[]);
void *initdp_sse2(signed short coeffs[],int len);
void freedp_sse2(void *dp);
long dotprod_sse2(void *dp,signed short a[]);
#endif
#ifdef __VEC__
void *initdp_av(signed short coeffs[],int len);
void freedp_av(void *dp);
long dotprod_av(void *dp,signed short a[]);
#endif
/* Sum of squares - accepts signed shorts, produces unsigned long long */
unsigned long long sumsq(signed short *in,int cnt);
unsigned long long sumsq_port(signed short *in,int cnt);
#ifdef __i386__
unsigned long long sumsq_mmx(signed short *in,int cnt);
unsigned long long sumsq_sse(signed short *in,int cnt);
unsigned long long sumsq_sse2(signed short *in,int cnt);
#endif
#ifdef __VEC__
unsigned long long sumsq_av(signed short *in,int cnt);
#endif
/* Low-level data structures and routines */
int cpu_features(void);
#endif /* _FEC_H_ */

33
fec-3.0.1/fixed.h Normal file
View File

@ -0,0 +1,33 @@
/* Stuff specific to the CCSDS (255,223) RS codec
* (255,223) code over GF(256). Note: the conventional basis is still
* used; the dual-basis mappings are performed in [en|de]code_rs_ccsds.c
*
* Copyright 2003 Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
typedef unsigned char data_t;
static inline int mod255(int x){
while (x >= 255) {
x -= 255;
x = (x >> 8) + (x & 255);
}
return x;
}
#define MODNN(x) mod255(x)
extern data_t CCSDS_alpha_to[];
extern data_t CCSDS_index_of[];
extern data_t CCSDS_poly[];
#define MM 8
#define NN 255
#define ALPHA_TO CCSDS_alpha_to
#define INDEX_OF CCSDS_index_of
#define GENPOLY CCSDS_poly
#define NROOTS 32
#define FCR 112
#define PRIM 11
#define IPRIM 116
#define PAD pad

39
fec-3.0.1/gen_ccsds.c Normal file
View File

@ -0,0 +1,39 @@
/* Generate tables for CCSDS code
* Copyright 2002 Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include "char.h"
#include "rs-common.h"
#include "fec.h"
int main(){
struct rs *rs;
int i;
rs = init_rs_char(8,0x187,112,11,32,0); /* CCSDS standard */
assert(rs != NULL);
printf("char CCSDS_alpha_to[] = {");
for(i=0;i<256;i++){
if((i % 16) == 0)
printf("\n");
printf("0x%02x,",rs->alpha_to[i]);
}
printf("\n};\n\nchar CCSDS_index_of[] = {");
for(i=0;i<256;i++){
if((i % 16) == 0)
printf("\n");
printf("%3d,",rs->index_of[i]);
}
printf("\n};\n\nchar CCSDS_poly[] = {");
for(i=0;i<33;i++){
if((i % 16) == 0)
printf("\n");
printf("%3d,",rs->genpoly[i]);
}
printf("\n};\n");
exit(0);
}

106
fec-3.0.1/init_rs.h Normal file
View File

@ -0,0 +1,106 @@
/* Common code for intializing a Reed-Solomon control block (char or int symbols)
* Copyright 2004 Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
#undef NULL
#define NULL ((void *)0)
{
int i, j, sr,root,iprim;
rs = NULL;
/* Check parameter ranges */
if(symsize < 0 || symsize > 8*sizeof(data_t)){
goto done;
}
if(fcr < 0 || fcr >= (1<<symsize))
goto done;
if(prim <= 0 || prim >= (1<<symsize))
goto done;
if(nroots < 0 || nroots >= (1<<symsize))
goto done; /* Can't have more roots than symbol values! */
if(pad < 0 || pad >= ((1<<symsize) -1 - nroots))
goto done; /* Too much padding */
rs = (struct rs *)calloc(1,sizeof(struct rs));
if(rs == NULL)
goto done;
rs->mm = symsize;
rs->nn = (1<<symsize)-1;
rs->pad = pad;
rs->alpha_to = (data_t *)malloc(sizeof(data_t)*(rs->nn+1));
if(rs->alpha_to == NULL){
free(rs);
rs = NULL;
goto done;
}
rs->index_of = (data_t *)malloc(sizeof(data_t)*(rs->nn+1));
if(rs->index_of == NULL){
free(rs->alpha_to);
free(rs);
rs = NULL;
goto done;
}
/* Generate Galois field lookup tables */
rs->index_of[0] = A0; /* log(zero) = -inf */
rs->alpha_to[A0] = 0; /* alpha**-inf = 0 */
sr = 1;
for(i=0;i<rs->nn;i++){
rs->index_of[sr] = i;
rs->alpha_to[i] = sr;
sr <<= 1;
if(sr & (1<<symsize))
sr ^= gfpoly;
sr &= rs->nn;
}
if(sr != 1){
/* field generator polynomial is not primitive! */
free(rs->alpha_to);
free(rs->index_of);
free(rs);
rs = NULL;
goto done;
}
/* Form RS code generator polynomial from its roots */
rs->genpoly = (data_t *)malloc(sizeof(data_t)*(nroots+1));
if(rs->genpoly == NULL){
free(rs->alpha_to);
free(rs->index_of);
free(rs);
rs = NULL;
goto done;
}
rs->fcr = fcr;
rs->prim = prim;
rs->nroots = nroots;
/* Find prim-th root of 1, used in decoding */
for(iprim=1;(iprim % prim) != 0;iprim += rs->nn)
;
rs->iprim = iprim / prim;
rs->genpoly[0] = 1;
for (i = 0,root=fcr*prim; i < nroots; i++,root += prim) {
rs->genpoly[i+1] = 1;
/* Multiply rs->genpoly[] by @**(root + x) */
for (j = i; j > 0; j--){
if (rs->genpoly[j] != 0)
rs->genpoly[j] = rs->genpoly[j-1] ^ rs->alpha_to[modnn(rs,rs->index_of[rs->genpoly[j]] + root)];
else
rs->genpoly[j] = rs->genpoly[j-1];
}
/* rs->genpoly[0] can never be zero */
rs->genpoly[0] = rs->alpha_to[modnn(rs,rs->index_of[rs->genpoly[0]] + root)];
}
/* convert rs->genpoly[] to index form for quicker encoding */
for (i = 0; i <= nroots; i++)
rs->genpoly[i] = rs->index_of[rs->genpoly[i]];
done:;
}

35
fec-3.0.1/init_rs_char.c Normal file
View File

@ -0,0 +1,35 @@
/* Initialize a RS codec
*
* Copyright 2002 Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
#include <stdlib.h>
#include "char.h"
#include "rs-common.h"
void free_rs_char(void *p){
struct rs *rs = (struct rs *)p;
free(rs->alpha_to);
free(rs->index_of);
free(rs->genpoly);
free(rs);
}
/* Initialize a Reed-Solomon codec
* symsize = symbol size, bits
* gfpoly = Field generator polynomial coefficients
* fcr = first root of RS code generator polynomial, index form
* prim = primitive element to generate polynomial roots
* nroots = RS code generator polynomial degree (number of roots)
* pad = padding bytes at front of shortened block
*/
void *init_rs_char(int symsize,int gfpoly,int fcr,int prim,
int nroots,int pad){
struct rs *rs;
#include "init_rs.h"
return rs;
}

49
fec-3.0.1/main.c Normal file
View File

@ -0,0 +1,49 @@
#include <stdio.h>
#include "fixed.h"
void encode_rs_8(data_t *data, data_t *parity,int pad);
int decode_rs_8(data_t *data, int *eras_pos, int no_eras, int pad);
int dump(char *name,unsigned char *addr,int len)
{
int i,j;
fprintf(stderr,"Dump of %s\n",name);
for(i=0;i<len;i+=16)
{
fprintf(stderr," %04x :",i);
for(j=0;j<16&&(i+j)<len;j++) fprintf(stderr," %02x",addr[i+j]);
for(;j<16;j++) fprintf(stderr," ");
fprintf(stderr," ");
for(j=0;j<16&&(i+j)<len;j++) fprintf(stderr,"%c",addr[i+j]>=' '&&addr[i+j]<0x7f?addr[i+j]:'.');
fprintf(stderr,"\n");
}
return 0;
}
int main(int argc,char **argv)
{
unsigned char in[255];
unsigned char out[255];
srandom(getpid());
int i;
for(i=0;i<223;i++) in[i]=i;
encode_rs_8(&in[0],&in[223],0);
bcopy(in,out,255);
dump("data with parity",out,255);
for(i=0;i<16;i++) out[random()%255]^=0xff;
dump("data with errors added",out,255);
decode_rs_8(out,NULL,0,0);
dump("data after error correction",out,223);
return 0;
}

26
fec-3.0.1/rs-common.h Normal file
View File

@ -0,0 +1,26 @@
/* Stuff common to all the general-purpose Reed-Solomon codecs
* Copyright 2004 Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
/* Reed-Solomon codec control block */
struct rs {
int mm; /* Bits per symbol */
int nn; /* Symbols per block (= (1<<mm)-1) */
data_t *alpha_to; /* log lookup table */
data_t *index_of; /* Antilog lookup table */
data_t *genpoly; /* Generator polynomial */
int nroots; /* Number of generator roots = number of parity symbols */
int fcr; /* First consecutive root, index form */
int prim; /* Primitive element, index form */
int iprim; /* prim-th root of 1, index form */
int pad; /* Padding bytes in shortened block */
};
static inline int modnn(struct rs *rs,int x){
while (x >= rs->nn) {
x -= rs->nn;
x = (x >> rs->mm) + (x & rs->nn);
}
return x;
}

203
golay.c Normal file
View File

@ -0,0 +1,203 @@
#define POLY 0xAE3 /* or use the other polynomial, 0xC75 */
static unsigned long golay(unsigned long cw)
/* This function calculates [23,12] Golay codewords.
The format of the returned longint is
[checkbits(11),data(12)]. */
{
int i;
unsigned long c;
cw&=0xfffl;
c=cw; /* save original codeword */
for (i=1; i<=12; i++) /* examine each data bit */
{
if (cw & 1) /* test data bit */
cw^=POLY; /* XOR polynomial */
cw>>=1; /* shift intermediate result */
}
return((cw<<12)|c); /* assemble codeword */
}
static int parity(unsigned long cw)
/* This function checks the overall parity of codeword cw.
If parity is even, 0 is returned, else 1. */
{
unsigned char p;
/* XOR the bytes of the codeword */
p=*(unsigned char*)&cw;
p^=*((unsigned char*)&cw+1);
p^=*((unsigned char*)&cw+2);
/* XOR the halves of the intermediate result */
p=p ^ (p>>4);
p=p ^ (p>>2);
p=p ^ (p>>1);
/* return the parity result */
return(p & 1);
}
int golay_encode(unsigned char *data)
{
unsigned long cw = data[0] | (data[1]<<8) | (data[2]<<16);
cw = golay(cw);
if (parity(cw))
cw|=0x800000l;
data[0]=cw&0xFF;
data[1]=(cw>>8)&0xFF;
data[2]=(cw>>16)&0xFF;
return 0;
}
static unsigned long syndrome(unsigned long cw)
/* This function calculates and returns the syndrome
of a [23,12] Golay codeword. */
{
int i;
cw&=0x7fffffl;
for (i=1; i<=12; i++) /* examine each data bit */
{
if (cw & 1) /* test data bit */
cw^=POLY; /* XOR polynomial */
cw>>=1; /* shift intermediate result */
}
return(cw<<12); /* value pairs with upper bits of cw */
}
static int weight(unsigned long cw)
/* This function calculates the weight of
23 bit codeword cw. */
{
int bits,k;
/* nibble weight table */
const char wgt[16] = {0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4};
bits=0; /* bit counter */
k=0;
/* do all bits, six nibbles max */
while ((k<6) && (cw))
{
bits=bits+wgt[cw & 0xf];
cw>>=4;
k++;
}
return(bits);
}
static unsigned long rotate_left(unsigned long cw, int n)
/* This function rotates 23 bit codeword cw left by n bits. */
{
int i;
if (n != 0)
{
for (i=1; i<=n; i++)
{
if ((cw & 0x400000l) != 0)
cw=(cw << 1) | 1;
else
cw<<=1;
}
}
return(cw & 0x7fffffl);
}
static unsigned long rotate_right(unsigned long cw, int n)
/* This function rotates 23 bit codeword cw right by n bits. */
{
int i;
if (n != 0)
{
for (i=1; i<=n; i++)
{
if ((cw & 1) != 0)
cw=(cw >> 1) | 0x400000l;
else
cw>>=1;
}
}
return(cw & 0x7fffffl);
}
static unsigned long correct(unsigned long cw, int *errs)
/* This function corrects Golay [23,12] codeword cw, returning the
corrected codeword. This function will produce the corrected codeword
for three or fewer errors. It will produce some other valid Golay
codeword for four or more errors, possibly not the intended
one. *errs is set to the number of bit errors corrected. */
{
unsigned char
w; /* current syndrome limit weight, 2 or 3 */
unsigned long
mask; /* mask for bit flipping */
int
i,j; /* index */
unsigned long
s, /* calculated syndrome */
cwsaver; /* saves initial value of cw */
cwsaver=cw; /* save */
*errs=0;
w=3; /* initial syndrome weight threshold */
j=-1; /* -1 = no trial bit flipping on first pass */
mask=1;
while (j<23) /* flip each trial bit */
{
if (j != -1) /* toggle a trial bit */
{
if (j>0) /* restore last trial bit */
{
cw=cwsaver ^ mask;
mask+=mask; /* point to next bit */
}
cw=cwsaver ^ mask; /* flip next trial bit */
w=2; /* lower the threshold while bit diddling */
}
s=syndrome(cw); /* look for errors */
if (s) /* errors exist */
{
for (i=0; i<23; i++) /* check syndrome of each cyclic shift */
{
if ((*errs=weight(s)) <= w) /* syndrome matches error pattern */
{
cw=cw ^ s; /* remove errors */
cw=rotate_right(cw,i); /* unrotate data */
return(s=cw);
}
else
{
cw=rotate_left(cw,1); /* rotate to next pattern */
s=syndrome(cw); /* calc new syndrome */
}
}
j++; /* toggle next trial bit */
}
else
return(cw); /* return corrected codeword */
}
return(cwsaver); /* return original if no corrections */
} /* correct */
int golay_decode(int *errs, const unsigned char *data)
/* This function decodes codeword *cw , error correction is attempted,
with *errs set to the number of bits corrected, and returning 0 if
no errors exist, or 1 if parity errors exist. */
{
unsigned long cw = data[0] | (data[1]<<8) | (data[2]<<16);
unsigned long parity_bit=cw & 0x800000l;
cw&=~0x800000l; /* remove parity bit for correction */
cw=correct(cw, errs); /* correct up to three bits */
cw|=parity_bit;
if (parity(cw))
return -1;
return cw&0xFFF;
} /* decode */

2
golay.h Normal file
View File

@ -0,0 +1,2 @@
int golay_encode(unsigned char *data);
int golay_decode(int *errs, unsigned char *data);

1
log.h
View File

@ -107,6 +107,7 @@ void logConfigChanged();
int logDump(int level, struct __sourceloc whence, char *name, const unsigned char *addr, size_t len);
ssize_t get_self_executable_path(char *buf, size_t len);
int log_backtrace(int level, struct __sourceloc whence);
struct strbuf;
#define __HERE__ ((struct __sourceloc){ .file = __FILE__, .line = __LINE__, .function = __FUNCTION__ })

5
lsif.c
View File

@ -54,10 +54,13 @@
#ifdef HAVE_LINUX_IF_H
#include <linux/if.h>
#else
#ifdef HAVE_NET_IF_H
#if HAVE_NET_IF_H || __MACH__ || __NetBSD__ || __OpenBSD__ || __FreeBSD__
#include <net/if.h>
#endif
#endif
#ifdef HAVE_IFADDRS_H
#include <ifaddrs.h>
#endif
/* On platforms that have variable length
ifreq use the old fixed length interface instead */

414
mavlink.c Normal file
View File

@ -0,0 +1,414 @@
// -*- Mode: C; c-basic-offset: 2; -*-
//
// Copyright (c) 2012 Andrew Tridgell, All Rights Reserved
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// o Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// o Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in
// the documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.
//
/*
Portions Copyright (C) 2013 Paul Gardner-Stephen
This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/
#include "serval.h"
#include "conf.h"
#include "overlay_buffer.h"
#include "golay.h"
#define MAVLINK_MSG_ID_RADIO 166
#define MAVLINK_MSG_ID_DATASTREAM 67
int MAVLINK_MESSAGE_CRCS[]={72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7};
// use '3D' for 3DRadio
#define RADIO_SOURCE_SYSTEM '3'
#define RADIO_SOURCE_COMPONENT 'D'
uint16_t mavlink_crc(unsigned char *buf,int length)
{
uint16_t sum = 0xFFFF;
uint8_t i, stoplen;
stoplen = length + 6;
// MAVLink 1.0 has an extra CRC seed
buf[length+6] = MAVLINK_MESSAGE_CRCS[buf[5]];
stoplen++;
i = 1;
while (i<stoplen) {
uint8_t tmp;
tmp = buf[i] ^ (uint8_t)(sum&0xff);
tmp ^= (tmp<<4);
sum = (sum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4);
i++;
}
buf[length+6]=sum&0xff;
buf[length+7]=sum>>8;
return sum;
}
/*
we use a hand-crafted MAVLink packet based on the following
message definition
<message name="RADIO" id="166">
<description>Status generated by radio</description>
<field type="uint8_t" name="rssi">local signal strength</field>
<field type="uint8_t" name="remrssi">remote signal strength</field>
<field type="uint8_t" name="txbuf">percentage free space in transmit buffer</field>
<field type="uint8_t" name="noise">background noise level</field>
<field type="uint8_t" name="remnoise">remote background noise level</field>
<field type="uint16_t" name="rxerrors">receive errors</field>
<field type="uint16_t" name="fixed">count of error corrected packets</field>
</message>
*/
struct mavlink_RADIO_v09 {
uint8_t rssi;
uint8_t remrssi;
uint8_t txbuf;
uint8_t noise;
uint8_t remnoise;
uint16_t rxerrors;
uint16_t fixed;
};
struct mavlink_RADIO_v10 {
uint16_t rxerrors;
uint16_t fixed;
uint8_t rssi;
uint8_t remrssi;
uint8_t txbuf;
uint8_t noise;
uint8_t remnoise;
};
/*
Each mavlink frame consists of 0xfe followed by a standard 6 byte header.
Normally the payload plus a 2-byte CRC follows.
We are replacing the CRC check with a Reed-Solomon code to correct as well
as detect upto 16 bytes with errors, in return for a 32-byte overhead.
The nature of the particular library we are using is that the overhead is
basically fixed, but we can shorten the data section.
Note that the mavlink headers are not protected against errors. This is a
limitation of the radio firmware at present. One day we will re-write the
radio firmware so that we can send and receive raw radio frames, and get
rid of the mavlink framing altogether, and just send R-S protected payloads.
Not ideal, but will be fine for now.
*/
#include "fec-3.0.1/fixed.h"
void encode_rs_8(data_t *data, data_t *parity,int pad);
int decode_rs_8(data_t *data, int *eras_pos, int no_eras, int pad);
int mavlink_encode_packet(struct overlay_interface *interface)
{
int count = ob_remaining(interface->tx_packet);
int startP = !ob_position(interface->tx_packet);
int endP = 1;
if (count+6+32 > 255){
count = 255-6-32;
endP = 0;
}
interface->txbuffer[0]=0xfe; // mavlink v1.0 frame
/* payload len, excluding 6 byte header and 2 byte CRC.
But we use a 4-byte CRC, so need to add two to count to make packet lengths
be as expected.
Note that this construction will result in CRC errors by non-servald
programmes, which is probably more helpful than otherwise.
*/
// we need 32 bytes for the parity, but this field assumes
// that there is a 2 byte CRC, so we can save two bytes
int len = count+32 - 2;
interface->txbuffer[1]=len;
interface->txbuffer[2]=(len & 0xF);
interface->txbuffer[3]=0;
golay_encode(&interface->txbuffer[1]);
interface->txbuffer[4]=(interface->mavlink_seq++) & 0x3f;
if (startP) interface->txbuffer[4]|=0x40;
if (endP) interface->txbuffer[4]|=0x80;
interface->txbuffer[5]=MAVLINK_MSG_ID_DATASTREAM;
ob_get_bytes(interface->tx_packet, &interface->txbuffer[6], count);
encode_rs_8(&interface->txbuffer[4], &interface->txbuffer[6+count], 223 - (count+2));
interface->tx_bytes_pending=len + 8;
if (endP){
ob_free(interface->tx_packet);
interface->tx_packet=NULL;
overlay_queue_schedule_next(gettime_ms());
}
return 0;
}
int mavlink_heartbeat(unsigned char *frame,int *outlen)
{
int count=9;
bzero(frame, count+8);
frame[0]=0xfe; // mavlink v1.0 frame
// Must be 9 to indicate heartbeat
frame[1]=count; // payload len, excluding 6 byte header and 2 byte CRC
frame[2]=(count & 0xF); // packet sequence
frame[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
golay_encode(&frame[1]);
frame[4]=0xf1; // component ID of sender (MAV_COMP_ID_UART_BRIDGE)
// Must be zero to indicate heartbeat
frame[5]=0; // message ID type of this frame: DATA_STREAM
// extra magic number to detect remote heartbeat requests
frame[14]=0x55;
frame[15]=0x05;
golay_encode(&frame[14]);
*outlen=count+8;
return 0;
}
extern unsigned long long last_rssi_time;
extern int last_radio_rssi;
extern int last_radio_temperature;
extern int last_radio_rxpackets;
static int parse_heartbeat(struct overlay_interface *interface, const unsigned char *payload)
{
if (payload[0]==0xFE
&& payload[1]==9
&& payload[3]==RADIO_SOURCE_SYSTEM
&& payload[4]==RADIO_SOURCE_COMPONENT
&& payload[5]==MAVLINK_MSG_ID_RADIO){
// we can assume that radio status packets arrive without corruption
last_radio_rssi=(1.0*payload[10]-payload[13])/1.9;
last_radio_temperature=-999; // doesn't get reported
last_radio_rxpackets=-999; // doesn't get reported
int free_space = payload[12];
int free_bytes = (free_space * 1280) / 100 - 30;
interface->remaining_space = free_bytes;
if (free_bytes>0)
interface->next_tx_allowed = gettime_ms();
if (free_bytes>720)
interface->next_heartbeat=gettime_ms()+1000;
if (config.debug.mavlink||gettime_ms()-last_rssi_time>30000) {
INFOF("Link budget = %+ddB, remote link budget = %+ddB, buffer space = %d%% (approx %d)",
last_radio_rssi,
(int)((1.0*payload[11] - payload[14])/1.9),
free_space, free_bytes);
last_rssi_time=gettime_ms();
}
return 1;
}
return 0;
}
static int mavlink_parse(struct overlay_interface *interface, struct slip_decode_state *state,
int packet_length, unsigned char *payload, int *backtrack)
{
*backtrack=0;
if (packet_length==9){
// make sure we've heard the start and end of a remote heartbeat request
int errs=0;
int tail = golay_decode(&errs, &payload[14]);
if (tail == 0x555){
return 1;
}
return 0;
}
int data_bytes = packet_length - (32 - 2);
// preserve the last 16 bytes of data
unsigned char old_footer[32];
unsigned char *payload_footer=&payload[packet_length+8-sizeof(old_footer)];
bcopy(payload_footer, old_footer, sizeof(old_footer));
int pad=223 - (data_bytes + 2);
int errors=decode_rs_8(&payload[4], NULL, 0, pad);
if (errors==-1){
if (config.debug.mavlink)
DEBUGF("Reed-Solomon error correction failed");
return 0;
}
*backtrack=errors;
int seq=payload[4]&0x3f;
if (config.debug.mavlink){
DEBUGF("Received RS protected message, len: %d, errors: %d, seq: %d, flags:%s%s",
data_bytes,
errors,
seq,
payload[4]&0x40?" start":"",
payload[4]&0x80?" end":"");
}
if (seq != ((state->mavlink_seq+1)&0x3f)){
// reject partial packet if we missed a sequence number
if (config.debug.mavlink)
DEBUGF("Rejecting packet, sequence jumped from %d to %d", state->mavlink_seq, seq);
state->packet_length=sizeof(state->dst)+1;
}
if (payload[4]&0x40){
// start a new packet
state->packet_length=0;
}
state->mavlink_seq=payload[4]&0x3f;
if (state->packet_length + data_bytes > sizeof(state->dst)){
if (config.debug.mavlink)
DEBUG("Fragmented packet is too long or a previous piece was missed - discarding");
state->packet_length=sizeof(state->dst)+1;
return 1;
}
bcopy(&payload[6], &state->dst[state->packet_length], data_bytes);
state->packet_length+=data_bytes;
if (payload[4]&0x80) {
if (config.debug.mavlink)
DEBUGF("PDU Complete (length=%d)",state->packet_length);
state->dst_offset=0;
packetOkOverlay(interface, state->dst, state->packet_length, -1, NULL, -1);
state->packet_length=sizeof(state->dst)+1;
}
return 1;
}
static int decode_length(struct slip_decode_state *state, unsigned char *p)
{
// look for a valid golay encoded length
int errs=0;
int length = golay_decode(&errs, p);
if (length<0 || ((length >>8) & 0xF) != (length&0xF))
return -1;
length=length&0xFF;
if (length!=9 && (length<31 || length+8>255))
return -1;
if (config.debug.mavlink && (errs || state->mavlink_payload_length!=*p))
DEBUGF("Decoded length %d to %d with %d errs", *p, length, errs);
state->mavlink_payload_length=length;
return 0;
}
int mavlink_decode(struct overlay_interface *interface, struct slip_decode_state *state, uint8_t c)
{
if (state->mavlink_payload_start + state->mavlink_payload_offset >= sizeof(state->mavlink_payload)){
// drop one byte if we run out of space
if (config.debug.mavlink)
DEBUGF("Dropped %02x, buffer full", state->mavlink_payload[0]);
bcopy(state->mavlink_payload+1, state->mavlink_payload, sizeof(state->mavlink_payload) -1);
state->mavlink_payload_start--;
}
unsigned char *p = &state->mavlink_payload[state->mavlink_payload_start];
p[state->mavlink_payload_offset++]=c;
while(1){
// look for packet length headers
p = &state->mavlink_payload[state->mavlink_payload_start];
while(state->mavlink_payload_length==0 && state->mavlink_payload_offset>=6){
if (p[0]==0xFE
&& p[1]==9
&& p[3]==RADIO_SOURCE_SYSTEM
&& p[4]==RADIO_SOURCE_COMPONENT
&& p[5]==MAVLINK_MSG_ID_RADIO){
//looks like a valid heartbeat response header, read the rest and process it
state->mavlink_payload_length=9;
break;
}
if (decode_length(state, &p[1])==0)
break;
state->mavlink_payload_start++;
state->mavlink_payload_offset--;
p++;
}
// wait for a whole packet
if (!state->mavlink_payload_length || state->mavlink_payload_offset < state->mavlink_payload_length+8)
return 0;
if (parse_heartbeat(interface, p)){
// cut the bytes of the heartbeat out of the buffer
state->mavlink_payload_offset -= state->mavlink_payload_length+8;
if (state->mavlink_payload_offset){
// shuffle bytes backwards
bcopy(&p[state->mavlink_payload_length+8], p, state->mavlink_payload_offset);
}
// restart parsing for a valid header from the beginning of out buffer
state->mavlink_payload_offset+=state->mavlink_payload_start;
state->mavlink_payload_start=0;
state->mavlink_payload_length=0;
continue;
}
// is this a well formed packet?
int backtrack=0;
if (mavlink_parse(interface, state, state->mavlink_payload_length, p, &backtrack)==1){
// Since we know we've synced with the remote party,
// and there's nothing we can do about any earlier data
// throw away everything before the end of this packet
if (state->mavlink_payload_start && config.debug.mavlink)
dump("Skipped", state->mavlink_payload, state->mavlink_payload_start);
// If the packet is truncated by less than 16 bytes, RS protection should be enough to recover the packet,
// but we may need to examine the last few bytes to find the start of the next packet.
state->mavlink_payload_offset -= state->mavlink_payload_length+8-backtrack;
if (state->mavlink_payload_offset){
// shuffle all remaining bytes back to the start of the buffer
bcopy(&state->mavlink_payload[state->mavlink_payload_start + state->mavlink_payload_length+8-backtrack],
state->mavlink_payload, state->mavlink_payload_offset);
}
state->mavlink_payload_start=0;
}else{
// ignore the first byte for now and start looking for another packet header
// we may find a heartbeat in the middle that we need to cut out first
state->mavlink_payload_start++;
state->mavlink_payload_offset--;
}
state->mavlink_payload_length=0;
};
}

18
overlay_address.c
View File

@ -283,7 +283,7 @@ static int add_explain_response(struct subscriber *subscriber, void *context){
static int find_subscr_buffer(struct decode_context *context, struct overlay_buffer *b, int len, struct subscriber **subscriber){
if (len<=0 || len>SID_SIZE){
return WHY("Invalid abbreviation length");
return WHYF("Invalid abbreviation length %d", len);
}
unsigned char *id = ob_get_bytes_ptr(b, len);
@ -355,7 +355,15 @@ int overlay_address_parse(struct decode_context *context, struct overlay_buffer
*subscriber=context->point_to_point_device;
context->previous=*subscriber;
}else{
WHYF("Could not resolve address, I don't know who is on the other end of this link!");
// add the abbreviation you told me about
if (!context->please_explain){
context->please_explain = calloc(sizeof(struct overlay_frame),1);
context->please_explain->payload=ob_new();
ob_limitsize(context->please_explain->payload, MDP_MTU);
}
INFOF("Asking for explanation of YOU");
ob_append_byte(context->please_explain->payload, OA_CODE_P2P_YOU);
context->invalid_addresses=1;
}
return 0;
@ -438,6 +446,12 @@ int process_explain(struct overlay_frame *frame){
while(ob_remaining(b)>0){
int len = ob_get(b);
if (len==OA_CODE_P2P_YOU){
add_explain_response(my_subscriber, &context);
continue;
}
if (len<=0 || len>SID_SIZE)
return WHY("Badly formatted explain message");
unsigned char *sid = ob_get_bytes_ptr(b, len);

9
overlay_buffer.c
View File

@ -139,6 +139,15 @@ int ob_unlimitsize(struct overlay_buffer *b)
return 0;
}
int ob_flip(struct overlay_buffer *b)
{
b->checkpointLength=0;
if (ob_limitsize(b, b->position))
return -1;
b->position=0;
return 0;
}
int _ob_makespace(struct __sourceloc __whence, struct overlay_buffer *b,int bytes)
{
if (b->sizeLimit!=-1 && b->position+bytes>b->sizeLimit) {

1
overlay_buffer.h
View File

@ -50,6 +50,7 @@ int ob_free(struct overlay_buffer *b);
int ob_checkpoint(struct overlay_buffer *b);
int ob_rewind(struct overlay_buffer *b);
int ob_limitsize(struct overlay_buffer *b,int bytes);
int ob_flip(struct overlay_buffer *b);
int ob_unlimitsize(struct overlay_buffer *b);
int _ob_makespace(struct __sourceloc whence, struct overlay_buffer *b,int bytes);
int ob_set(struct overlay_buffer *b, int ofs, unsigned char byte);

275
overlay_interface.c
View File

@ -48,16 +48,14 @@ struct sockaddr_in sock_any_addr;
struct profile_total sock_any_stats;
static void overlay_interface_poll(struct sched_ent *alarm);
static void logServalPacket(int level, struct __sourceloc __whence, const char *message, const unsigned char *packet, size_t len);
static int re_init_socket(int interface_index);
#define DEBUG_packet_visualise(M,P,N) logServalPacket(LOG_LEVEL_DEBUG, __WHENCE__, (M), (P), (N))
static void write_stream_buffer(overlay_interface *interface);
static void
overlay_interface_close(overlay_interface *interface){
link_interface_down(interface);
INFOF("Interface %s addr %s is down",
interface->name, inet_ntoa(interface->address.sin_addr));
interface->name, inet_ntoa(interface->address.sin_addr));
unschedule(&interface->alarm);
unwatch(&interface->alarm);
close(interface->alarm.poll.fd);
@ -233,20 +231,7 @@ overlay_interface_read_any(struct sched_ent *alarm){
return;
}
/* We have a frame from this interface */
if (config.debug.packetrx)
DEBUG_packet_visualise("Read from real interface", packet,plen);
if (config.debug.overlayinterfaces)
DEBUGF("Received %d bytes from %s on interface %s (ANY)",plen,
inet_ntoa(src),
interface->name);
if (packetOkOverlay(interface, packet, plen, recvttl, &src_addr, addrlen)<0) {
if (config.debug.rejecteddata) {
WHYF("Malformed packet (length = %d)",plen);
dump("the malformed packet",packet,plen);
}
}
packetOkOverlay(interface, packet, plen, recvttl, &src_addr, addrlen);
}
if (alarm->poll.revents & (POLLHUP | POLLERR)) {
INFO("Closing broadcast socket due to error");
@ -378,6 +363,9 @@ overlay_interface_init(const char *name, struct in_addr src_addr, struct in_addr
interface->ctsrts = ifconfig->ctsrts;
set_destination_ref(&interface->destination, NULL);
interface->destination = new_destination(interface, ifconfig->encapsulation);
interface->throttle_bytes_per_second = ifconfig->throttle;
interface->throttle_burst_write_size = ifconfig->burst_size;
/* Pick a reasonable default MTU.
This will ultimately get tuned by the bandwidth and other properties of the interface */
interface->mtu = 1200;
@ -492,9 +480,10 @@ overlay_interface_init(const char *name, struct in_addr src_addr, struct in_addr
interface->slip_decode_state.dst_offset=0;
/* The encapsulation type should be configurable, but for now default to the one that should
be safe on the RFD900 radios, and that also allows us to receive RSSI reports inline */
interface->slip_decode_state.encapsulator=SLIP_FORMAT_UPPER7;
interface->alarm.poll.events=POLLIN;
interface->slip_decode_state.encapsulator=SLIP_FORMAT_MAVLINK;
interface->alarm.poll.events=POLLIN|POLLOUT;
watch(&interface->alarm);
break;
case SOCK_FILE:
/* Seek to end of file as initial reading point */
@ -548,21 +537,7 @@ static void interface_read_dgram(struct overlay_interface *interface){
return;
}
/* We have a frame from this interface */
if (config.debug.packetrx)
DEBUG_packet_visualise("Read from real interface", packet,plen);
if (config.debug.overlayinterfaces) {
struct in_addr src = ((struct sockaddr_in *)&src_addr)->sin_addr; // avoid strict-alias warning on Solaris (gcc 4.4)
DEBUGF("Received %d bytes from %s on interface %s",plen,
inet_ntoa(src),
interface->name);
}
if (packetOkOverlay(interface, packet, plen, recvttl, &src_addr, addrlen)<0) {
if (config.debug.rejecteddata) {
WHYF("Malformed packet (length = %d)",plen);
dump("the malformed packet",packet,plen);
}
}
packetOkOverlay(interface, packet, plen, recvttl, &src_addr, addrlen);
}
struct file_packet{
@ -640,23 +615,13 @@ static void interface_read_file(struct overlay_interface *interface)
if (nread == sizeof packet) {
interface->recv_offset += nread;
if (config.debug.packetrx)
DEBUG_packet_visualise("Read from dummy interface", packet.payload, packet.payload_length);
if (should_drop(interface, packet.dst_addr) || (packet.pid == getpid() && !interface->local_echo)){
if (config.debug.packetrx)
DEBUGF("Ignoring packet from %d, addressed to %s:%d", packet.pid,
inet_ntoa(packet.dst_addr.sin_addr), ntohs(packet.dst_addr.sin_port));
}else{
if (packetOkOverlay(interface, packet.payload, packet.payload_length, -1,
(struct sockaddr*)&packet.src_addr, sizeof(packet.src_addr))<0) {
if (config.debug.rejecteddata) {
WARN("Unsupported packet from dummy interface");
WHYF("Malformed packet (length = %d)",packet.payload_length);
dump("the malformed packet",packet.payload,packet.payload_length);
}
}
packetOkOverlay(interface, packet.payload, packet.payload_length, -1,
(struct sockaddr*)&packet.src_addr, sizeof(packet.src_addr));
}
}
}
@ -690,57 +655,104 @@ static void interface_read_stream(struct overlay_interface *interface){
OUT();
return;
}
if (config.debug.packetradio)
dump("read bytes", buffer, nread);
struct slip_decode_state *state=&interface->slip_decode_state;
if (config.debug.slip) {
dump("RX bytes", buffer, nread);
}
state->src=buffer;
state->src_size=nread;
state->src_offset=0;
while (state->src_offset < state->src_size) {
int ret = slip_decode(state);
if (ret==1){
if (packetOkOverlay(interface, state->dst, state->packet_length, -1, NULL, -1)<0) {
if (config.debug.rejecteddata) {
WHYF("Malformed packet (length = %d)",state->packet_length);
dump("the malformed packet",state->dst,state->packet_length);
}
}
state->dst_offset=0;
}
}
int i;
for (i=0;i<nread;i++)
mavlink_decode(interface, state, buffer[i]);
OUT();
}
static void write_stream_buffer(overlay_interface *interface){
if (interface->tx_bytes_pending>0) {
int written=write(interface->alarm.poll.fd,interface->txbuffer,
interface->tx_bytes_pending);
if (config.debug.packetradio) DEBUGF("Trying to write %d bytes",
interface->tx_bytes_pending);
if (written>0) {
interface->tx_bytes_pending-=written;
time_ms_t now = gettime_ms();
// Throttle output to a prescribed bit-rate
// first, reduce the number of bytes based on the configured burst size
int bytes_allowed=interface->throttle_burst_write_size;
int total_written=0;
while (interface->tx_bytes_pending>0 || interface->tx_packet || interface->next_heartbeat <= now) {
if (interface->tx_bytes_pending==0){
if (interface->next_heartbeat <= now){
// Queue a hearbeat now
mavlink_heartbeat(interface->txbuffer,&interface->tx_bytes_pending);
if (config.debug.packetradio)
DEBUGF("Built %d byte heartbeat", interface->tx_bytes_pending);
interface->next_heartbeat = now+1000;
}else if(interface->tx_packet && interface->remaining_space >= 256 + 8+9){
// prepare a new link layer packet in txbuffer
if (mavlink_encode_packet(interface))
break;
if (config.debug.packetradio)
DEBUGF("Built %d byte payload from packet (%d)", interface->tx_bytes_pending, interface->remaining_space);
if (interface->remaining_space - interface->tx_bytes_pending < 256 + 8+9)
interface->next_heartbeat = now;
}
}
if (interface->next_tx_allowed > now)
break;
int bytes = interface->tx_bytes_pending;
if (interface->throttle_burst_write_size && bytes>bytes_allowed)
bytes=bytes_allowed;
if (bytes<=0)
break;
if (config.debug.packetradio)
DEBUGF("Trying to write %d bytes of %d%s", bytes, interface->tx_bytes_pending, interface->tx_packet?", pending packet":"");
int written=write(interface->alarm.poll.fd, interface->txbuffer, bytes);
if (written<=0){
DEBUGF("Blocking for POLLOUT");
break;
}
interface->remaining_space-=written;
interface->tx_bytes_pending-=written;
total_written+=written;
bytes_allowed-=written;
if (interface->tx_bytes_pending){
bcopy(&interface->txbuffer[written],&interface->txbuffer[0],
interface->tx_bytes_pending);
if (config.debug.packetradio) DEBUGF("Wrote %d bytes (%d left pending)",
written,interface->tx_bytes_pending);
} else {
if (config.debug.packetradio) DEBUGF("Failed to write any data");
DEBUGF("Partial write, %d left", interface->tx_bytes_pending);
}
if (config.debug.packetradio)
DEBUGF("Wrote %d bytes (%d left pending, %d remains)", written, interface->tx_bytes_pending, interface->remaining_space);
}
if (total_written>0){
// Now when are we allowed to send more?
int rate = interface->throttle_bytes_per_second;
if (interface->remaining_space<=0)
rate = 600;
if (rate){
int delay = total_written*1000/rate;
if (config.debug.throttling)
DEBUGF("Throttling for %dms (%d).", delay, interface->remaining_space);
interface->next_tx_allowed = now + delay;
}
}
if (interface->tx_bytes_pending>0) {
// more to write, so keep POLLOUT flag
time_ms_t next_write = interface->next_tx_allowed;
if (interface->tx_bytes_pending<=0){
next_write = interface->next_heartbeat;
}
if (interface->alarm.alarm==-1 || next_write < interface->alarm.alarm){
interface->alarm.alarm = next_write;
interface->alarm.deadline = interface->alarm.alarm+10;
}
if (interface->tx_bytes_pending>0 && next_write <= now){
// more to write, so set the POLLOUT flag
interface->alarm.poll.events|=POLLOUT;
} else {
// nothing more to write, so clear POLLOUT flag
// Nothing to write, so clear POLLOUT flag
interface->alarm.poll.events&=~POLLOUT;
// try to empty another packet from the queue ASAP
overlay_queue_schedule_next(gettime_ms());
}
watch(&interface->alarm);
}
@ -749,14 +761,15 @@ static void write_stream_buffer(overlay_interface *interface){
static void overlay_interface_poll(struct sched_ent *alarm)
{
struct overlay_interface *interface = (overlay_interface *)alarm;
time_ms_t now = gettime_ms();
if (alarm->poll.revents==0){
alarm->alarm=-1;
time_ms_t now = gettime_ms();
if (interface->state==INTERFACE_STATE_UP
&& interface->destination->tick_ms>0
&& interface->send_broadcasts){
&& interface->send_broadcasts
&& !interface->tx_packet){
if (now >= interface->destination->last_tx+interface->destination->tick_ms)
overlay_send_tick_packet(interface->destination);
@ -766,15 +779,18 @@ static void overlay_interface_poll(struct sched_ent *alarm)
}
switch(interface->socket_type){
case SOCK_DGRAM:
case SOCK_STREAM:
write_stream_buffer(interface);
break;
case SOCK_DGRAM:
break;
case SOCK_FILE:
interface_read_file(interface);
now = gettime_ms();
break;
}
unschedule(alarm);
if (alarm->alarm!=-1 && interface->state==INTERFACE_STATE_UP) {
if (alarm->alarm < now)
alarm->alarm = now;
@ -786,6 +802,12 @@ static void overlay_interface_poll(struct sched_ent *alarm)
switch(interface->socket_type){
case SOCK_STREAM:
write_stream_buffer(interface);
if (alarm->alarm!=-1 && interface->state==INTERFACE_STATE_UP) {
if (alarm->alarm < now)
alarm->alarm = now;
unschedule(alarm);
schedule(alarm);
}
break;
case SOCK_DGRAM:
case SOCK_FILE:
@ -801,6 +823,16 @@ static void overlay_interface_poll(struct sched_ent *alarm)
break;
case SOCK_STREAM:
interface_read_stream(interface);
// if we read a valid heartbeat packet, we may be able to write more bytes now.
if (interface->state==INTERFACE_STATE_UP && interface->remaining_space>0){
write_stream_buffer(interface);
if (alarm->alarm!=-1 && interface->state==INTERFACE_STATE_UP) {
if (alarm->alarm < now)
alarm->alarm = now;
unschedule(alarm);
schedule(alarm);
}
}
break;
case SOCK_FILE:
interface_read_file(interface);
@ -813,21 +845,22 @@ static void overlay_interface_poll(struct sched_ent *alarm)
}
}
int
overlay_broadcast_ensemble(struct network_destination *destination,
unsigned char *bytes,int len)
int overlay_broadcast_ensemble(struct network_destination *destination, struct overlay_buffer *buffer)
{
assert(destination && destination->interface);
const unsigned char *bytes = ob_ptr(buffer);
int len = ob_position(buffer);
struct overlay_interface *interface = destination->interface;
destination->last_tx = gettime_ms();
if (config.debug.packettx){
DEBUGF("Sending this packet via interface %s (len=%d)",interface->name,len);
DEBUG_packet_visualise(NULL,bytes,len);
DEBUG_packet_visualise(NULL, bytes, len);
}
if (interface->state!=INTERFACE_STATE_UP){
ob_free(buffer);
return WHYF("Cannot send to interface %s as it is down", interface->name);
}
@ -837,35 +870,21 @@ overlay_broadcast_ensemble(struct network_destination *destination,
switch(interface->socket_type){
case SOCK_STREAM:
{
if (interface->tx_bytes_pending>0)
if (interface->tx_packet){
ob_free(buffer);
return WHYF("Cannot send two packets to a stream at the same time");
}
/* Encode packet with SLIP escaping.
XXX - Add error correction here also */
unsigned char *buffer = interface->txbuffer;
int out_len=0;
int encoded = slip_encode(SLIP_FORMAT_UPPER7,
bytes, len, buffer+out_len, sizeof(interface->txbuffer) - out_len);
if (encoded < 0)
return WHY("Buffer overflow");
if (config.debug.slip)
{
// Test decoding of the packet we send
struct slip_decode_state state;
state.encapsulator=SLIP_FORMAT_UPPER7;
state.src_size=encoded;
state.src_offset=0;
state.src=buffer+out_len;
slip_decode(&state);
}
out_len+=encoded;
interface->tx_bytes_pending=out_len;
// prepare the buffer for reading
ob_flip(buffer);
interface->tx_packet = buffer;
write_stream_buffer(interface);
if (interface->alarm.alarm!=-1){
unschedule(&interface->alarm);
schedule(&interface->alarm);
}
return 0;
}
@ -883,6 +902,7 @@ overlay_broadcast_ensemble(struct network_destination *destination,
}
packet.payload_length=len;
bcopy(bytes, packet.payload, len);
ob_free(buffer);
/* This lseek() is unneccessary because the dummy file is opened in O_APPEND mode. It's
only purpose is to find out the offset to print in the DEBUG statement. It is vulnerable
to a race condition with other processes appending to the same file. */
@ -909,8 +929,11 @@ overlay_broadcast_ensemble(struct network_destination *destination,
{
if (config.debug.overlayinterfaces)
DEBUGF("Sending %d byte overlay frame on %s to %s",len,interface->name,inet_ntoa(destination->address.sin_addr));
if(sendto(interface->alarm.poll.fd,
bytes, len, 0, (struct sockaddr *)&destination->address, sizeof(destination->address)) != len){
int sent=sendto(interface->alarm.poll.fd,
bytes, len, 0,
(struct sockaddr *)&destination->address, sizeof(destination->address));
ob_free(buffer);
if (sent!= len){
WHY_perror("sendto(c)");
// close the interface if we had any error while sending broadcast packets,
// unicast packets should not bring the interface down
@ -923,6 +946,7 @@ overlay_broadcast_ensemble(struct network_destination *destination,
}
default:
ob_free(buffer);
return WHY("Unsupported socket type");
}
}
@ -1023,7 +1047,7 @@ void overlay_interface_discover(struct sched_ent *alarm)
int i;
for (i = 0; i < overlay_interface_count; i++)
if (overlay_interfaces[i].state==INTERFACE_STATE_UP)
overlay_interfaces[i].state=INTERFACE_STATE_DETECTING;
overlay_interfaces[i].state=INTERFACE_STATE_DETECTING;
/* Register new dummy interfaces */
int detect_real_interfaces = 0;
@ -1075,8 +1099,10 @@ void overlay_interface_discover(struct sched_ent *alarm)
// Close any interfaces that have gone away.
for(i = 0; i < overlay_interface_count; i++)
if (overlay_interfaces[i].state==INTERFACE_STATE_DETECTING)
if (overlay_interfaces[i].state==INTERFACE_STATE_DETECTING) {
DEBUGF("Closing interface stuck in DETECTING state.");
overlay_interface_close(&overlay_interfaces[i]);
}
alarm->alarm = gettime_ms()+5000;
alarm->deadline = alarm->alarm + 10000;
@ -1084,8 +1110,7 @@ void overlay_interface_discover(struct sched_ent *alarm)
return;
}
static void
logServalPacket(int level, struct __sourceloc __whence, const char *message, const unsigned char *packet, size_t len) {
void logServalPacket(int level, struct __sourceloc __whence, const char *message, const unsigned char *packet, size_t len) {
struct mallocbuf mb = STRUCT_MALLOCBUF_NULL;
if (!message) message="<no message>";
if (serval_packetvisualise_xpf(XPRINTF_MALLOCBUF(&mb), message, packet, len) == -1)

14
overlay_packetformats.c
View File

@ -378,6 +378,17 @@ int packetOkOverlay(struct overlay_interface *interface,unsigned char *packet, s
the source having received the frame from elsewhere.
*/
if (config.debug.packetrx || interface->debug) {
DEBUGF("Received on %s, len %d", interface->name, (int)len);
DEBUG_packet_visualise("Received packet",packet,len);
if (config.debug.interactive_io) {
fprintf(stderr,"Press ENTER to continue..."); fflush(stderr);
char buffer[80];
if (!fgets(buffer,80,stdin))
FATAL_perror("calling fgets");
}
}
if (recvaddr&&recvaddr->sa_family!=AF_INET)
RETURN(WHYF("Unexpected protocol family %d",recvaddr->sa_family));
@ -396,9 +407,6 @@ int packetOkOverlay(struct overlay_interface *interface,unsigned char *packet, s
else
bzero(&f.recvaddr, sizeof f.recvaddr);
if (interface->debug)
DEBUGF("Received on %s, len %d: %s", interface->name, (int)len, alloca_tohex(packet, len>64?64:len));
int ret=parseEnvelopeHeader(&context, interface, (struct sockaddr_in *)recvaddr, b);
if (ret){
ob_free(b);

20
overlay_packetradio.c
View File

@ -68,33 +68,13 @@ int overlay_packetradio_setup_port(overlay_interface *interface)
if (tcsetattr(interface->alarm.poll.fd, TCSANOW, &t))
WHY_perror("Failed to set terminal parameters");
// Ask radio to report RSSI
(void)write_all(interface->alarm.poll.fd,"\r",1);
sleep_ms(600);
(void)write_all(interface->alarm.poll.fd,"\r",1);
sleep_ms(600);
(void)write_all(interface->alarm.poll.fd,"+++",3);
sleep_ms(1200);
(void)write_all(interface->alarm.poll.fd,"\rAT&T\rAT&T=RSSI\rATO\r",20);
if (config.debug.packetradio) {
tcgetattr(interface->alarm.poll.fd, &t);
int in_speed=cfgetispeed(&t);
int out_speed=cfgetospeed(&t);
DEBUGF("Enabled RSSI reporting for RFD900 radios");
DEBUGF("Sent ATO to make sure we are in on-line mode");
DEBUGF("uart speed reported as %d/%d",in_speed,out_speed);
}
if (0){
// dummy write of all possible ascii values
char buff[256];
int i;
for (i=0;i<sizeof buff;i++)
buff[i]=i;
(void)write_all(interface->alarm.poll.fd,buff,sizeof buff);
}
set_nonblock(interface->alarm.poll.fd);
return 0;

15
overlay_queue.c
View File

@ -288,6 +288,8 @@ overlay_calc_queue_time(overlay_txqueue *queue, struct overlay_frame *frame){
int i;
for(i=0;i<frame->destination_count;i++)
{
if (frame->destinations[i].destination->interface->tx_packet)
continue;
time_ms_t next_packet = limit_next_allowed(&frame->destinations[i].destination->transfer_limit);
if (frame->destinations[i].transmit_time){
time_ms_t delay_until = frame->destinations[i].transmit_time + frame->destinations[i].destination->resend_delay;
@ -399,7 +401,7 @@ overlay_stuff_packet(struct outgoing_packet *packet, overlay_txqueue *queue, tim
}else{
// skip this interface if the stream tx buffer has data
if (dest->interface->socket_type==SOCK_STREAM
&& dest->interface->tx_bytes_pending>0)
&& dest->interface->tx_packet)
continue;
// can we send a packet on this interface now?
@ -513,8 +515,7 @@ overlay_fill_send_packet(struct outgoing_packet *packet, time_ms_t now) {
if (config.debug.packetconstruction)
ob_dump(packet->buffer,"assembled packet");
overlay_broadcast_ensemble(packet->destination, ob_ptr(packet->buffer), ob_position(packet->buffer));
ob_free(packet->buffer);
overlay_broadcast_ensemble(packet->destination, packet->buffer);
ret=1;
}
if (packet->destination)
@ -574,8 +575,8 @@ int overlay_queue_ack(struct subscriber *neighbour, struct network_destination *
if (!destination->max_rtt || rtt > destination->max_rtt)
destination->max_rtt = rtt;
if (config.debug.overlayframes)
DEBUGF("Packet %p to %s sent by seq %d, acked with seq %d",
if (config.debug.ack)
DEBUGF("DROPPED DUE TO ACK: Packet %p to %s sent by seq %d, acked with seq %d",
frame, alloca_tohex_sid(neighbour->sid), frame_seq, ack_seq);
// drop packets that don't need to be retransmitted
@ -587,8 +588,8 @@ int overlay_queue_ack(struct subscriber *neighbour, struct network_destination *
}else if (seq_delta < 128 && frame->destination && frame->delay_until>now){
// retransmit asap
if (config.debug.overlayframes)
DEBUGF("Requeue packet %p to %s sent by seq %d due to ack of seq %d", frame, alloca_tohex_sid(neighbour->sid), frame_seq, ack_seq);
if (config.debug.ack)
DEBUGF("RE-TX DUE TO NACK: Requeue packet %p to %s sent by seq %d due to ack of seq %d", frame, alloca_tohex_sid(neighbour->sid), frame_seq, ack_seq);
frame->delay_until = now;
overlay_calc_queue_time(&overlay_tx[i], frame);
}

4
rhizome.h
View File

@ -73,7 +73,7 @@ extern time_ms_t rhizome_voice_timeout;
#define RHIZOME_PRIORITY_SERVAL_BULK 1
#define RHIZOME_PRIORITY_NOTINTERESTED 0
#define RHIZOME_IDLE_TIMEOUT 10000
#define RHIZOME_IDLE_TIMEOUT 20000
#define EXISTING_BUNDLE_ID 1
#define NEW_BUNDLE_ID 2
@ -386,7 +386,7 @@ int rhizome_ignore_manifest_check(unsigned char *bid_prefix, int prefix_len);
#define MAX_CANDIDATES 32
int rhizome_suggest_queue_manifest_import(rhizome_manifest *m, const struct sockaddr_in *peerip,const unsigned char peersid[SID_SIZE]);
rhizome_manifest * rhizome_fetch_search(unsigned char *id, int prefix_length);
rhizome_manifest * rhizome_fetch_search(const unsigned char *id, int prefix_length);
/* Rhizome file storage api */
struct rhizome_write_buffer{

2
rhizome_database.c
View File

@ -1032,6 +1032,8 @@ int rhizome_store_bundle(rhizome_manifest *m)
m->version
);
monitor_announce_bundle(m);
if (serverMode)
rhizome_sync_announce();
return 0;
}
rollback:

18
rhizome_fetch.c
View File

@ -229,7 +229,7 @@ static struct rhizome_fetch_slot *rhizome_find_fetch_slot(long long size)
// find the first matching active slot for this bundle
static struct rhizome_fetch_slot *fetch_search_slot(unsigned char *id, int prefix_length)
static struct rhizome_fetch_slot *fetch_search_slot(const unsigned char *id, int prefix_length)
{
int i;
for (i = 0; i < NQUEUES; ++i) {
@ -243,7 +243,7 @@ static struct rhizome_fetch_slot *fetch_search_slot(unsigned char *id, int prefi
}
// find the first matching candidate for this bundle
static struct rhizome_fetch_candidate *fetch_search_candidate(unsigned char *id, int prefix_length)
static struct rhizome_fetch_candidate *fetch_search_candidate(const unsigned char *id, int prefix_length)
{
int i, j;
for (i = 0; i < NQUEUES; ++i) {
@ -261,7 +261,7 @@ static struct rhizome_fetch_candidate *fetch_search_candidate(unsigned char *id,
}
/* Search all fetch slots, including active downloads, for a matching manifest */
rhizome_manifest * rhizome_fetch_search(unsigned char *id, int prefix_length){
rhizome_manifest * rhizome_fetch_search(const unsigned char *id, int prefix_length){
struct rhizome_fetch_slot *s = fetch_search_slot(id, prefix_length);
if (s)
return s->manifest;
@ -1333,16 +1333,14 @@ int rhizome_received_content(unsigned char *bidprefix,
int count,unsigned char *bytes,int type)
{
IN();
if (config.debug.rhizome)
DEBUGF("Rhizome over MDP receiving %d bytes.",count);
if (!is_rhizome_mdp_enabled()) {
if (config.debug.rhizome)
DEBUGF("Rhizome over MDP is not enabled");
RETURN(-1);
}
struct rhizome_fetch_slot *slot=fetch_search_slot(bidprefix, 16);
if (slot && slot->bidVersion == version && slot->state == RHIZOME_FETCH_RXFILEMDP){
if (config.debug.rhizome)
DEBUGF("Rhizome over MDP receiving %d bytes.",count);
if (rhizome_random_write(&slot->write_state, offset, bytes, count)){
if (config.debug.rhizome)
DEBUGF("Write failed!");
@ -1364,10 +1362,6 @@ int rhizome_received_content(unsigned char *bidprefix,
rhizome_fetch_mdp_requestblocks(slot);
}
RETURN(0);
} else {
if (config.debug.rhizome)
DEBUGF("Ignoring received block: slot=%p, version=%016"PRIx64", slot->bidVersion=%016"PRIx64", slot->state=%d (should be %d)",
slot,version,slot?slot->bidVersion:0,slot?slot->state:-999,RHIZOME_FETCH_RXFILEMDP);
}
// if we get a packet containing an entire payload
@ -1529,7 +1523,7 @@ void rhizome_fetch_poll(struct sched_ent *alarm)
rhizome_fetch_mdp_slot_callback(alarm);
break;
default:
default:
// timeout or socket error, close the socket
if (config.debug.rhizome_rx)
DEBUGF("Closing due to timeout or error %x (%x %x)", alarm->poll.revents, POLLHUP, POLLERR);

35
rhizome_packetformats.c
View File

@ -186,6 +186,10 @@ void overlay_rhizome_advertise(struct sched_ent *alarm){
int (*oldfunc)() = sqlite_set_tracefunc(is_debug_rhizome_ads);
sqlite_retry_state retry = SQLITE_RETRY_STATE_DEFAULT;
// DEPRECATE REST OF THIS CODE WHICH SEEMS TO BE CAUSING TOO MUCH CHATTER
// ESPECIALLY FOR PACKET-RADIO
goto end;
/* Get number of bundles available */
if (sqlite_exec_int64_retry(&retry, &bundles_available, "SELECT COUNT(BAR) FROM MANIFESTS;") != 1){
WHY("Could not count BARs for advertisement");
@ -242,9 +246,10 @@ int rhizome_advertise_manifest(struct subscriber *dest, rhizome_manifest *m){
bzero(frame,sizeof(struct overlay_frame));
frame->type = OF_TYPE_RHIZOME_ADVERT;
frame->source = my_subscriber;
if (dest && (dest->reachable==REACHABLE_UNICAST || dest->reachable==REACHABLE_INDIRECT))
if (dest && dest->reachable&REACHABLE)
frame->destination = dest;
frame->ttl = 1;
else
frame->ttl = 1;
frame->queue = OQ_OPPORTUNISTIC;
frame->payload = ob_new();
@ -256,6 +261,9 @@ int rhizome_advertise_manifest(struct subscriber *dest, rhizome_manifest *m){
if (ob_append_bytes(frame->payload, m->manifestdata, m->manifest_all_bytes)) goto error;
ob_append_byte(frame->payload, 0xFF);
if (overlay_payload_enqueue(frame)) goto error;
if (config.debug.rhizome_ads)
DEBUGF("Advertising manifest %s %"PRId64" to %s",
alloca_tohex_bid(m->cryptoSignPublic), m->version, dest?alloca_tohex_sid(dest->sid):"broadcast");
return 0;
error:
@ -322,18 +330,11 @@ int overlay_rhizome_saw_advertisements(int i, struct decode_context *context, st
goto next;
}
char manifest_id_prefix[RHIZOME_MANIFEST_ID_STRLEN + 1];
if (rhizome_manifest_get(m, "id", manifest_id_prefix, sizeof manifest_id_prefix) == NULL) {
WHY("Manifest does not contain 'id' field");
goto next;
}
/* trim manifest ID to a prefix for ease of debugging
(that is the only use of this */
if (config.debug.rhizome_ads){
manifest_id_prefix[8]=0;
long long version = rhizome_manifest_get_ll(m, "version");
DEBUGF("manifest id=%s* version=%lld", manifest_id_prefix, version);
DEBUGF("manifest id=%s version=%lld", alloca_tohex_bid(m->cryptoSignPublic), version);
}
/* Crude signature presence test */
@ -350,11 +351,10 @@ int overlay_rhizome_saw_advertisements(int i, struct decode_context *context, st
goto next;
}
if (rhizome_ignore_manifest_check(m->cryptoSignPublic,
crypto_sign_edwards25519sha512batch_PUBLICKEYBYTES)){
if (rhizome_ignore_manifest_check(m->cryptoSignPublic, RHIZOME_MANIFEST_ID_BYTES)){
/* Ignoring manifest that has caused us problems recently */
if (config.debug.rhizome_ads)
DEBUGF("Ignoring manifest with errors: %s*", manifest_id_prefix);
DEBUGF("Ignoring manifest with errors: %s", alloca_tohex_bid(m->cryptoSignPublic));
goto next;
}
@ -364,11 +364,16 @@ int overlay_rhizome_saw_advertisements(int i, struct decode_context *context, st
/* Don't waste any time on this manifest in future attempts for at least
a minute. */
rhizome_queue_ignore_manifest(m->cryptoSignPublic,
crypto_sign_edwards25519sha512batch_PUBLICKEYBYTES, 60000);
RHIZOME_MANIFEST_ID_BYTES, 60000);
goto next;
}
/* Manifest is okay, so see if it is worth storing */
// are we already fetching this bundle [or later]?
rhizome_manifest *mf=rhizome_fetch_search(m->cryptoSignPublic, RHIZOME_MANIFEST_ID_BYTES);
if (mf && mf->version >= m->version)
goto next;
if (!rhizome_is_manifest_interesting(m)) {
/* We already have this version or newer */
if (config.debug.rhizome_ads)

8
rhizome_sync.c
View File

@ -326,7 +326,7 @@ static int append_response(struct overlay_buffer *b, uint64_t token, const unsig
}
static uint64_t max_token=0;
static void sync_send_response(struct subscriber *dest, int forwards, uint64_t token)
static void sync_send_response(struct subscriber *dest, int forwards, uint64_t token, int max_count)
{
IN();
overlay_mdp_frame mdp;
@ -380,7 +380,7 @@ static void sync_send_response(struct subscriber *dest, int forwards, uint64_t t
rhizome_sync_bundle_inserted(bar);
}
if (count < BARS_PER_RESPONSE){
if (count < BARS_PER_RESPONSE && (max_count==0 || count < max_count)){
// make sure we include the exact rowid that was requested, even if we just deleted / replaced the manifest
if (count==0 && rowid!=token){
if (token!=HEAD_FLAG){
@ -433,7 +433,7 @@ static void sync_send_response(struct subscriber *dest, int forwards, uint64_t t
int rhizome_sync_announce()
{
sync_send_response(NULL, 0, HEAD_FLAG);
sync_send_response(NULL, 0, HEAD_FLAG, 5);
return 0;
}
@ -457,7 +457,7 @@ int overlay_mdp_service_rhizome_sync(struct overlay_frame *frame, overlay_mdp_fr
{
int forwards = ob_get(b);
uint64_t token = ob_get_packed_ui64(b);
sync_send_response(frame->source, forwards, token);
sync_send_response(frame->source, forwards, token, 0);
}
break;
}

21
route_link.c
View File

@ -690,7 +690,7 @@ static int neighbour_link_sent(struct overlay_frame *frame, int sequence, void *
if (!neighbour)
return 0;
neighbour->last_update_seq = sequence;
if (config.debug.linkstate && config.debug.verbose)
if ((config.debug.linkstate && config.debug.verbose)||config.debug.ack)
DEBUGF("LINK STATE; ack sent to neighbour %s in seq %d", alloca_tohex_sid(subscriber->sid), sequence);
return 0;
}
@ -721,7 +721,7 @@ static int send_neighbour_link(struct neighbour *n)
frame->destination = n->subscriber;
}else{
// no routing decision yet? send this packet to all probable destinations.
if (config.debug.linkstate && config.debug.verbose)
if ((config.debug.linkstate && config.debug.verbose)|| config.debug.ack)
DEBUGF("Sending link state ack to all possibilities");
struct link_out *out = n->out_links;
while(out){
@ -740,9 +740,9 @@ static int send_neighbour_link(struct neighbour *n)
else
flags|=FLAG_BROADCAST;
if (config.debug.linkstate && config.debug.verbose)
if (config.debug.ack)
DEBUGF("LINK STATE; Sending ack to %s for seq %d", alloca_tohex_sid(n->subscriber->sid), n->best_link->ack_sequence);
append_link_state(frame->payload, flags, n->subscriber, my_subscriber, n->best_link->neighbour_interface, 1,
n->best_link->ack_sequence, n->best_link->ack_mask, -1);
if (overlay_payload_enqueue(frame))
@ -752,7 +752,7 @@ static int send_neighbour_link(struct neighbour *n)
n->last_update = now;
}
n->next_neighbour_update = n->last_update + n->best_link->interface->destination->tick_ms;
if (config.debug.linkstate && config.debug.verbose)
if (config.debug.ack)
DEBUGF("Next update for %s in %lldms", alloca_tohex_sid(n->subscriber->sid), n->next_neighbour_update - gettime_ms());
OUT();
return 0;
@ -961,8 +961,11 @@ int link_state_ack_soon(struct subscriber *subscriber){
if (neighbour->using_us
&& subscriber->reachable & REACHABLE_DIRECT
&& subscriber->destination){
if (neighbour->next_neighbour_update > now + subscriber->destination->min_rtt)
if (neighbour->next_neighbour_update > now + subscriber->destination->min_rtt){
neighbour->next_neighbour_update = now + subscriber->destination->min_rtt;
if (config.debug.ack)
DEBUGF("Asking for next ACK Real Soon Now");
}
}
update_alarm(neighbour->next_neighbour_update);
OUT();
@ -1076,7 +1079,7 @@ int link_received_packet(struct decode_context *context, int sender_seq, char un
if (link->ack_sequence == sender_seq)
break;
// missed a packet? send a link state soon
if (config.debug.verbose && config.debug.linkstate)
if ((config.debug.verbose && config.debug.linkstate)||config.debug.ack)
DEBUGF("LINK STATE; missed seq %d from %s on %s",
link->ack_sequence, alloca_tohex_sid(context->sender->sid), context->interface->name);
link->ack_mask = link->ack_mask << 1;
@ -1188,7 +1191,7 @@ int link_receive(struct overlay_frame *frame, overlay_mdp_frame *mdp)
if (context.invalid_addresses)
continue;
if (config.debug.verbose && config.debug.linkstate)
if ((config.debug.verbose && config.debug.linkstate)||config.debug.ack)
DEBUGF("LINK STATE; record - %d, %s, %s, %d, %d, %x, %d",
flags,
receiver?alloca_tohex_sid(receiver->sid):"NULL",
@ -1286,7 +1289,7 @@ int link_receive(struct overlay_frame *frame, overlay_mdp_frame *mdp)
neighbour->last_update_seq = -1;
}else if(seq_delta < 128){
// send another ack asap
if (config.debug.linkstate && config.debug.verbose)
if (config.debug.ack)
DEBUGF("LINK STATE; neighbour %s missed ack %d, queue another", alloca_tohex_sid(sender->sid), neighbour->last_update_seq);
neighbour->next_neighbour_update=now+5;
update_alarm(neighbour->next_neighbour_update);

29
serval.h
View File

@ -391,6 +391,7 @@ extern int overlayMode;
struct slip_decode_state{
#define SLIP_FORMAT_SLIP 0
#define SLIP_FORMAT_UPPER7 1
#define SLIP_FORMAT_MAVLINK 2
int encapsulator;
int state;
unsigned char *src;
@ -402,6 +403,12 @@ struct slip_decode_state{
uint32_t crc;
int src_offset;
int dst_offset;
int mavlink_payload_length;
int mavlink_seq;
int mavlink_payload_start;
int mavlink_payload_offset;
uint8_t mavlink_payload[1024];
};
struct overlay_interface;
@ -464,8 +471,19 @@ typedef struct overlay_interface {
char name[256];
int recv_offset; /* file offset */
// stream socket tx state;
struct overlay_buffer *tx_packet;
unsigned char txbuffer[OVERLAY_INTERFACE_RX_BUFFER_SIZE];
int tx_bytes_pending;
// Throttle TX rate if required (stream interfaces only for now)
uint32_t throttle_bytes_per_second;
uint32_t throttle_burst_write_size;
uint64_t next_tx_allowed;
int32_t remaining_space;
time_ms_t next_heartbeat;
int mavlink_seq;
struct slip_decode_state slip_decode_state;
@ -600,6 +618,8 @@ int overlay_saw_mdp_containing_frame(struct overlay_frame *f, time_ms_t now);
int serval_packetvisualise(const char *message, const unsigned char *packet, size_t len);
int serval_packetvisualise_xpf(XPRINTF xpf, const char *message, const unsigned char *packet, size_t len);
void logServalPacket(int level, struct __sourceloc __whence, const char *message, const unsigned char *packet, size_t len);
#define DEBUG_packet_visualise(M,P,N) logServalPacket(LOG_LEVEL_DEBUG, __WHENCE__, (M), (P), (N))
int rhizome_fetching_get_fds(struct pollfd *fds,int *fdcount,int fdmax);
int rhizome_opendb();
@ -702,8 +722,7 @@ overlay_interface * overlay_interface_get_default();
overlay_interface * overlay_interface_find(struct in_addr addr, int return_default);
overlay_interface * overlay_interface_find_name(const char *name);
int overlay_interface_compare(overlay_interface *one, overlay_interface *two);
int overlay_broadcast_ensemble(struct network_destination *destination,
unsigned char *bytes,int len);
int overlay_broadcast_ensemble(struct network_destination *destination, struct overlay_buffer *buffer);
int directory_registration();
int directory_service_init();
@ -853,7 +872,7 @@ int measure_packed_uint(uint64_t v);
int unpack_uint(unsigned char *buffer, int buff_size, uint64_t *v);
int slip_encode(int format,
unsigned char *src, int src_bytes, unsigned char *dst, int dst_len);
const unsigned char *src, int src_bytes, unsigned char *dst, int dst_len);
int slip_decode(struct slip_decode_state *state);
int upper7_decode(struct slip_decode_state *state,unsigned char byte);
uint32_t Crc32_ComputeBuf( uint32_t inCrc32, const void *buf,
@ -881,4 +900,8 @@ int link_add_destinations(struct overlay_frame *frame);
int generate_nonce(unsigned char *nonce,int bytes);
int mavlink_decode(struct overlay_interface *interface, struct slip_decode_state *state,uint8_t c);
int mavlink_heartbeat(unsigned char *frame,int *outlen);
int mavlink_encode_packet(struct overlay_interface *interface);
#endif // __SERVALD_SERVALD_H

119
slip.c
View File

@ -1,7 +1,28 @@
/*
Serval Distributed Numbering Architecture (DNA)
Copyright (C) 2012 Paul Gardner-Stephen
This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/
#include "serval.h"
#include "conf.h"
#include "log.h"
#define DEBUG_packet_visualise(M,P,N) logServalPacket(LOG_LEVEL_DEBUG, __WHENCE__, (M), (P), (N))
/* SLIP-style escape characters used for serial packet radio interfaces */
#define SLIP_END 0xc0
#define SLIP_ESC 0xdb
@ -21,58 +42,72 @@
#define DC_VALID 1
#define DC_ESC 2
static int encode_slip(const unsigned char *src, int src_bytes, unsigned char *dst, int dst_len)
{
int i, offset=0;
for (i=0;i<src_bytes;i++){
if (offset+3>dst_len)
return WHY("Dest buffer full");
switch(src[i]) {
case SLIP_END:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_END;
break;
case SLIP_ESC:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_ESC;
break;
case SLIP_0a:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_0a;
break;
case SLIP_0d:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_0d;
break;
case SLIP_0f:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_0f;
break;
case SLIP_1b:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_1b;
break;
default:
dst[offset++]=src[i];
}
}
return offset;
}
int slip_encode(int format,
unsigned char *src, int src_bytes, unsigned char *dst, int dst_len)
const unsigned char *src, int src_bytes, unsigned char *dst, int dst_len)
{
switch(format) {
case SLIP_FORMAT_SLIP:
{
int offset=0;
int i;
if (offset+2>dst_len)
return WHY("Dest buffer full");
dst[offset++]=SLIP_END;
uint32_t crc=Crc32_ComputeBuf( 0, src, src_bytes);
// (I'm assuming there are 4 extra bytes in memory here, which is very naughty...)
write_uint32(src+src_bytes, crc);
int ret=encode_slip(src, src_bytes, dst + offset, dst_len - offset);
if (ret<0)
return ret;
offset+=ret;
unsigned char crc[4];
write_uint32(crc, Crc32_ComputeBuf( 0, src, src_bytes));
ret=encode_slip(crc, 4, dst + offset, dst_len - offset);
if (ret<0)
return ret;
offset+=ret;
for (i=0;i<src_bytes+4;i++){
if (offset+3>dst_len)
return WHY("Dest buffer full");
switch(src[i]) {
case SLIP_END:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_END;
break;
case SLIP_ESC:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_ESC;
break;
case SLIP_0a:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_0a;
break;
case SLIP_0d:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_0d;
break;
case SLIP_0f:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_0f;
break;
case SLIP_1b:
dst[offset++]=SLIP_ESC;
dst[offset++]=SLIP_ESC_1b;
break;
default:
dst[offset++]=src[i];
}
}
dst[offset++]=SLIP_END;
return offset;
@ -162,13 +197,13 @@ int parse_rfd900_rssi(char *s)
&lrssi,&rrssi,&lnoise,&rnoise,&rxpackets, &temp)==6)
{
int lmargin=(lrssi-lnoise)/1.9;
int rmargin=(lrssi-lnoise)/1.9;
int rmargin=(rrssi-rnoise)/1.9;
int maxmargin=lmargin; if (rmargin>maxmargin) maxmargin=rmargin;
last_radio_rssi=maxmargin;
last_radio_temperature=temp;
last_radio_rxpackets=rxpackets;
if (config.debug.packetradio||(gettime_ms()-last_rssi_time>30000)) {
if (gettime_ms()-last_rssi_time>30000) {
INFOF("Link budget = %+ddB, temperature=%dC",maxmargin,temp);
last_rssi_time=gettime_ms();
}

8
sourcefiles.mk
View File

@ -13,10 +13,12 @@ SERVAL_SOURCES = \
$(SERVAL_BASE)encode.c \
$(SERVAL_BASE)fdqueue.c \
$(SERVAL_BASE)fifo.c \
$(SERVAL_BASE)golay.c \
$(SERVAL_BASE)keyring.c \
$(SERVAL_BASE)log.c \
$(SERVAL_BASE)lsif.c \
$(SERVAL_BASE)main.c \
$(SERVAL_BASE)mavlink.c \
$(SERVAL_BASE)meshms.c \
$(SERVAL_BASE)mdp_client.c \
$(SERVAL_BASE)os.c \
@ -69,4 +71,8 @@ SERVAL_SOURCES = \
$(SERVAL_BASE)strlcpy.c \
$(SERVAL_BASE)vomp.c \
$(SERVAL_BASE)vomp_console.c \
$(SERVAL_BASE)xprintf.c
$(SERVAL_BASE)xprintf.c \
$(SERVAL_BASE)fec-3.0.1/ccsds_tables.c \
$(SERVAL_BASE)fec-3.0.1/decode_rs_8.c \
$(SERVAL_BASE)fec-3.0.1/encode_rs_8.c \
$(SERVAL_BASE)fec-3.0.1/init_rs_char.c

55
tests/rhizomeprotocol
View File

@ -219,6 +219,61 @@ test_UnicastTransfer() {
receive_and_update_bundle
}
doc_SimulatedRadio="MDP Transfer over simulated radio link"
interface_up() {
$GREP "Interface .* is up" $instance_servald_log || return 1
return 0
}
start_radio_instance() {
executeOk_servald config \
set monitor.socket "org.servalproject.servald.monitor.socket.$TFWUNIQUE.$instance_name" \
set mdp.socket "org.servalproject.servald.mdp.socket.$TFWUNIQUE.$instance_name" \
set debug.rhizome on \
set debug.rhizome_ads on \
set debug.rhizome_tx on \
set debug.rhizome_rx on \
set debug.throttling on \
set debug.mavlink on \
set rhizome.advertise.interval 5000 \
set rhizome.rhizome_mdp_block_size 350 \
set log.console.level debug \
set log.console.show_pid on \
set log.console.show_time on \
set interfaces.1.type CATEAR \
set interfaces.1.mdp_tick_ms 5000 \
set interfaces.1.socket_type STREAM \
set interfaces.1.encapsulation SINGLE \
set interfaces.1.point_to_point on
start_servald_server
wait_until interface_up
}
setup_SimulatedRadio() {
setup_common
$servald_build_root/fakeradio 6 10000000 > "$SERVALD_VAR/radioout" 2> "$SERVALD_VAR/radioerr" &
FAKERADIO_PID=$!
sleep 1
local END1=`head "$SERVALD_VAR/radioout" -n 1`
local END2=`tail "$SERVALD_VAR/radioout" -n 1`
tfw_log "Started fakeradio pid=$FAKERADIO_PID, end1=$END1, end2=$END2"
set_instance +A
rhizome_add_file file1 10000
executeOk_servald config \
set interfaces.1.file "$END1"
set_instance +B
executeOk_servald config \
set interfaces.1.file "$END2"
foreach_instance +A +B start_radio_instance
}
test_SimulatedRadio() {
receive_and_update_bundle
}
teardown_SimulatedRadio() {
teardown
tfw_log "Killing fakeradio, pid=$FAKERADIO_PID"
kill $FAKERADIO_PID
tfw_cat "$SERVALD_VAR/radioerr"
}
doc_journalMDP="Transfer and update a journal bundle via MDP"
setup_journalMDP() {
setup_common

9
tests/routing
View File

@ -216,7 +216,7 @@ setup_simulate_extender() {
setup_servald
assert_no_servald_processes
foreach_instance +A +B create_single_identity
$servald_build_root/fakeradio > "$SERVALD_VAR/radioout" 2> "$SERVALD_VAR/radioerr" &
$servald_build_root/fakeradio 1 20000000 > "$SERVALD_VAR/radioout" 2> "$SERVALD_VAR/radioerr" &
FAKERADIO_PID=$!
sleep 1
local END1=`head "$SERVALD_VAR/radioout" -n 1`
@ -230,7 +230,11 @@ setup_simulate_extender() {
set interfaces.1.file "$END2"
foreach_instance +A +B \
executeOk_servald config \
set debug.throttling on \
set debug.packetradio on \
set debug.mavlink on \
set interfaces.1.type CATEAR \
set interfaces.1.mdp_tick_ms 5000 \
set interfaces.1.socket_type STREAM \
set interfaces.1.encapsulation SINGLE \
set interfaces.1.point_to_point on \
@ -240,6 +244,9 @@ setup_simulate_extender() {
test_simulate_extender() {
wait_until path_exists +A +B
wait_until path_exists +B +A
set_instance +A
executeOk_servald mdp ping --timeout=3 $SIDB 1
tfw_cat --stdout --stderr
}
teardown_simulate_extender() {
teardown