serval-dna/radio_link.c

// -*- 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 "overlay_interface.h"
#include "golay.h"
#include "radio_link.h"
#include "debug.h"

#define MAVLINK_MSG_ID_RADIO 166
#define MAVLINK_MSG_ID_DATASTREAM 67

// use '3D' for 3DRadio
#define RADIO_SOURCE_SYSTEM '3'
#define RADIO_SOURCE_COMPONENT 'D'

/*
  we use a hand-crafted MAVLink packet based on the following
  message definition
  
struct mavlink_RADIO_v10 {
  uint16_t rxerrors;  // receive errors
  uint16_t fixed;     // count of error corrected packets
  uint8_t rssi;       // local signal strength
  uint8_t remrssi;    // remote signal strength
  uint8_t txbuf;      // percentage free space in transmit buffer
  uint8_t noise;      // background noise level
  uint8_t remnoise;   // remote background noise level
};

*/

#define FEC_LENGTH 32
#define FEC_MAX_BYTES 223
#define RADIO_HEADER_LENGTH 6
#define RADIO_USED_HEADER_LENGTH 4
#define RADIO_CRC_LENGTH 2

#define LINK_PAYLOAD_MTU (LINK_MTU - FEC_LENGTH - RADIO_HEADER_LENGTH - RADIO_CRC_LENGTH)

struct radio_link_state{
  // next seq for transmission
  int tx_seq;

  // small buffer for parsing incoming bytes from the serial interface, 
  // looking for recoverable link layer packets
  // should be large enough to hold at least one packet from the remote end
  // plus one heartbeat packet from the local firmware
  uint8_t payload[LINK_MTU*3];
  
  // decoded length of next link layer packet
  // including all header and footer bytes
  size_t payload_length;
  // last rx seq for reassembly
  int seq;
  // offset within payload that we have found a valid looking header
  unsigned payload_start;
  // offset after payload_start for incoming bytes
  unsigned payload_offset;
  
  // small buffer for assembling mdp payloads.
  uint8_t dst[MDP_MTU];
  // length of recovered packet
  size_t packet_length;
  
  // next firmware heartbeat
  time_ms_t next_heartbeat;
  
  time_ms_t last_packet;
  
  // parsed rssi
  int radio_rssi;
  int remote_rssi;
  // estimated firmware buffer space
  int32_t remaining_space;
  
  // next serial write
  time_ms_t next_tx_allowed;
  // partially sent packet
  struct overlay_buffer *tx_packet;
  
  // serial write buffer
  uint8_t txbuffer[LINK_MTU];
  int tx_bytes;
  int tx_pos;
};

/*
  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 radio_link_free(struct overlay_interface *interface)
{
  if (interface->radio_link_state){
    free(interface->radio_link_state);
    interface->radio_link_state=NULL;  
  }
  return 0;
}

int radio_link_init(struct overlay_interface *interface)
{
  interface->radio_link_state = emalloc_zero(sizeof(struct radio_link_state));
  return 0;
}

void radio_link_state_html(struct strbuf *b, struct overlay_interface *interface)
{
  struct radio_link_state *state = interface->radio_link_state;
  strbuf_sprintf(b, "RSSI: %ddB<br>", state->radio_rssi);
  strbuf_sprintf(b, "Remote RSSI: %ddB<br>", state->remote_rssi);
}

// write a new link layer packet to interface->txbuffer
// consuming more bytes from the next interface->tx_packet if required
static int radio_link_encode_packet(struct radio_link_state *link_state)
{
  // if we have nothing interesting left to send, don't create a packet at all
  if (!link_state->tx_packet)
    return 0;
    
  int count = ob_remaining(link_state->tx_packet);
  int startP = (ob_position(link_state->tx_packet) == 0);
  int endP = 1;
  if (count > LINK_PAYLOAD_MTU){
    count = LINK_PAYLOAD_MTU;
    endP = 0;
  }
  
  link_state->txbuffer[0]=0xfe; // mavlink v1.0 magic header
  
  // we need to add FEC_LENGTH for FEC, but the length field doesn't include the expected headers or CRC
  int len = count + FEC_LENGTH - RADIO_CRC_LENGTH;
  link_state->txbuffer[1]=len; // mavlink payload length
  link_state->txbuffer[2]=(len & 0xF);
  link_state->txbuffer[3]=0;
  
  // add golay encoding so that decoding the actual length is more reliable
  golay_encode(&link_state->txbuffer[1]);
  
  
  link_state->txbuffer[4]=(link_state->tx_seq++) & 0x3f;
  if (startP) link_state->txbuffer[4]|=0x40;
  if (endP) link_state->txbuffer[4]|=0x80;
  link_state->txbuffer[5]=MAVLINK_MSG_ID_DATASTREAM;
  
  ob_get_bytes(link_state->tx_packet, &link_state->txbuffer[6], count);
  
  encode_rs_8(&link_state->txbuffer[4], &link_state->txbuffer[6+count], FEC_MAX_BYTES - (count+2));
  link_state->tx_bytes=len + RADIO_CRC_LENGTH + RADIO_HEADER_LENGTH;
  if (endP){
    ob_free(link_state->tx_packet);
    link_state->tx_packet=NULL;
    overlay_queue_schedule_next(gettime_ms());
  }
  return 0;
}

int radio_link_is_busy(struct overlay_interface *interface)
{
  if (interface->radio_link_state && interface->radio_link_state->tx_packet)
    return 1;
  return 0;
}

int radio_link_queue_packet(struct overlay_interface *interface, struct overlay_buffer *buffer)
{
  struct radio_link_state *link_state = interface->radio_link_state;
  
  if (link_state->tx_packet){
    ob_free(buffer);
    return WHYF("Cannot send two packets to a stream at the same time");
  }

  // prepare the buffer for reading
  ob_flip(buffer);
  link_state->tx_packet = buffer;
  radio_link_tx(interface);
  
  return 0;
}

static int build_heartbeat(struct radio_link_state *link_state)
{
  int count=9;
  bzero(link_state->txbuffer, count + RADIO_CRC_LENGTH + RADIO_HEADER_LENGTH);
  
  link_state->txbuffer[0]=0xfe; // mavlink v1.0 link_state->txbuffer
  // Must be 9 to indicate heartbeat
  link_state->txbuffer[1]=count; // payload len, excluding 6 byte header and 2 byte CRC
  link_state->txbuffer[2]=(count & 0xF); // packet sequence
  link_state->txbuffer[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
  // we're golay encoding the length to improve the probability of skipping it correctly
  golay_encode(&link_state->txbuffer[1]);
  link_state->txbuffer[4]=0xf1; // component ID of sender (MAV_COMP_ID_UART_BRIDGE)
  // Must be zero to indicate heartbeat
  link_state->txbuffer[5]=0; // message ID type of this link_state->txbuffer: DATA_STREAM

  // extra magic number to help correctly detect remote heartbeat requests
  link_state->txbuffer[14]=0x55;
  link_state->txbuffer[15]=0x05;
  golay_encode(&link_state->txbuffer[14]);
  link_state->tx_bytes = count + RADIO_CRC_LENGTH + RADIO_HEADER_LENGTH;
  DEBUGF(radio_link, "Produced heartbeat");
  
  return 0;
}

// write a new link layer packet to interface->txbuffer
// consuming more bytes from the next interface->tx_packet if required
int radio_link_tx(struct overlay_interface *interface)
{
  struct radio_link_state *link_state = interface->radio_link_state;
  
  unschedule(&interface->alarm);
  interface->alarm.alarm = 0;
  time_ms_t next_tick = interface->destination->last_tx+interface->destination->ifconfig.tick_ms;
  time_ms_t now = gettime_ms();
  
  while(1){
    
    if (link_state->tx_bytes){
      if (link_state->next_tx_allowed > now){
	interface->alarm.alarm = link_state->next_tx_allowed;
	break;
      }
      
      int written=write(interface->alarm.poll.fd, &link_state->txbuffer[link_state->tx_pos], link_state->tx_bytes);
      if (written<=0){
	interface->alarm.poll.events|=POLLOUT;
	break;
      }
      link_state->remaining_space-=written;
      link_state->tx_bytes-=written;
      if (link_state->tx_bytes)
	link_state->tx_pos+=written;
      else
	link_state->tx_pos=0;
      continue;
    }
  
    interface->alarm.poll.events&=~POLLOUT;
    
    if (link_state->next_heartbeat<=now){
      build_heartbeat(link_state);
      link_state->next_heartbeat = now + 1000;
      continue;
    }
    
    // out of space? Don't bother to send anything interesting 
    // until we hear the next heartbeat response
    if (link_state->remaining_space < LINK_MTU + HEARTBEAT_SIZE){
      interface->alarm.alarm = link_state->next_heartbeat;
      break;
    }
    
    if (link_state->remaining_space < LINK_MTU + HEARTBEAT_SIZE)
      link_state->next_heartbeat = now;
    
    if (!link_state->tx_packet){
      // finished current packet, wait for more.
      interface->alarm.alarm = next_tick;
      break;
    }
    
    // encode another packet fragment
    radio_link_encode_packet(link_state);
    link_state->last_packet = now;
  }
  
  watch(&interface->alarm);
  if (interface->alarm.alarm<now)
    interface->alarm.alarm=now;
  if (interface->alarm.alarm){
    interface->alarm.deadline = interface->alarm.alarm+100;
    schedule(&interface->alarm);
  }
  
  return 0;
}

static int parse_heartbeat(struct radio_link_state *state, 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
    state->radio_rssi=(1.0*payload[10]-payload[13])/1.9;
    state->remote_rssi=(1.0*payload[11] - payload[14])/1.9;
    int free_space = payload[12];
    int free_bytes = (free_space * 1280) / 100 - 30;
    state->remaining_space = free_bytes;
    if (free_bytes>0)
      state->next_tx_allowed = gettime_ms();
    if (free_bytes>720)
      state->next_heartbeat=gettime_ms()+1000;
    DEBUGF(packetradio, "Link budget = %+ddB, remote link budget = %+ddB, buffer space = %d%% (approx %d)",
	   state->radio_rssi,
	   state->remote_rssi,
	   free_space, free_bytes);
    return 1;
  }
  return 0;
}

static int radio_link_parse(struct overlay_interface *interface, struct radio_link_state *state, 
  size_t packet_length, uint8_t *payload, int *backtrack)
{
  *backtrack=0;
  if (packet_length==17){
    // if we've heard the start and end of a remote heartbeat request
    // we can skip it without checking anything else
    int errs=0;
    int tail = golay_decode(&errs, &payload[14]);
    if (tail == 0x555){
      DEBUGF(radio_link, "Decoded remote heartbeat request");
      return 1;
    }
    return 0;
  }
  
  size_t data_bytes = packet_length - (RADIO_USED_HEADER_LENGTH + FEC_LENGTH);
  
  int errors=decode_rs_8(&payload[4], NULL, 0, FEC_MAX_BYTES - data_bytes);
  if (errors==-1){
    DEBUGF(radio_link, "Reed-Solomon error correction failed");
    return 0;
  }
  *backtrack=errors;
  data_bytes -= 2;
  int seq=payload[4]&0x3f;
  
  DEBUGF(radio_link, "Received RS protected message, len: %zd, errors: %d, seq: %d, flags:%s%s", 
         data_bytes,
         errors,
         seq,
         payload[4]&0x40?" start":"",
         payload[4]&0x80?" end":""
	);
  
  if (seq != ((state->seq+1)&0x3f)){
    // reject partial packet if we missed a sequence number
    DEBUGF(radio_link, "Rejecting packet, sequence jumped from %d to %d", state->seq, seq);
    state->packet_length=sizeof(state->dst)+1;
  }
  
  if (payload[4]&0x40){
    // start a new packet
    state->packet_length=0;
  }
  
  state->seq=payload[4]&0x3f;
  if (state->packet_length + data_bytes > sizeof(state->dst)){
    DEBUG(radio_link, "Fragmented packet is too long or a previous piece was missed - discarding");
    state->packet_length=sizeof(state->dst)+1;
    return 1;
  }
  
  bcopy(&payload[RADIO_HEADER_LENGTH], &state->dst[state->packet_length], data_bytes);
  state->packet_length+=data_bytes;
    
  if (payload[4]&0x80) {
    DEBUGF(radio_link, "PDU Complete (length=%zd)",state->packet_length);
    
    packetOkOverlay(interface, state->dst, state->packet_length, NULL);
    state->packet_length=sizeof(state->dst)+1;
  }
  return 1;
}

static int decode_length(struct radio_link_state *state, unsigned char *p)
{
  // look for a valid golay encoded length
  int errs=0;
  int gd = golay_decode(&errs, p);
  if (gd<0 || ((gd >>8) & 0xF) != (gd&0xF))
    return -1;
  size_t length = gd&0xFF;
  length += RADIO_HEADER_LENGTH + RADIO_CRC_LENGTH;
  
  if (length!=17 && (length <= FEC_LENGTH || length > LINK_MTU))
    return -1;
  
  if (errs || state->payload_length!=*p)
    DEBUGF(radio_link, "Decoded length %u to %zu with %d errs", *p, length, errs);
  
  state->payload_length=length;
  return 0;
}

// add one byte at a time from the serial link, and attempt to decode packets
int radio_link_decode(struct overlay_interface *interface, uint8_t c)
{
  IN();
  struct radio_link_state *state=interface->radio_link_state;
  
  if (state->payload_start + state->payload_offset >= sizeof state->payload){
    // drop one byte if we run out of space
    DEBUGF(radio_link, "Dropped %02x, buffer full", state->payload[0]);
    bcopy(state->payload+1, state->payload, sizeof(state->payload) -1);
    state->payload_start--;
  }
  
  unsigned char *p = &state->payload[state->payload_start];
  p[state->payload_offset++]=c;
  
  while(1){
    // look for packet length headers
    p = &state->payload[state->payload_start];
    while(state->payload_length==0 && state->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->payload_length=17;
	break;
      }
      
      if (decode_length(state, &p[1])==0)
	break;
      
      state->payload_start++;
      state->payload_offset--;
      p++;
    }
    
    // wait for a whole packet
    if (!state->payload_length || state->payload_offset < state->payload_length)
      RETURN(0);
    
    if (parse_heartbeat(state, p)){
      // cut the bytes of the heartbeat out of the buffer
      state->payload_offset -= state->payload_length;
      if (state->payload_offset){
	// shuffle bytes backwards
	bcopy(&p[state->payload_length], p, state->payload_offset);
      }
      // restart parsing for a valid header from the beginning of out buffer
      state->payload_offset+=state->payload_start;
      state->payload_start=0;
      state->payload_length=0;
      continue;
    }
    
    // is this a well formed packet?
    int backtrack=0;
    if (radio_link_parse(interface, state, state->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->payload_start)
	DEBUG_dump(radio_link, "Skipped", state->payload, state->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->payload_offset -= state->payload_length - backtrack;
      if (state->payload_offset){
	// shuffle all remaining bytes back to the start of the buffer
	bcopy(&state->payload[state->payload_start + state->payload_length - backtrack], 
	  state->payload, state->payload_offset);
      }
      state->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->payload_start++;
      state->payload_offset--;
    }
    state->payload_length=0;
  };
  RETURN(0);
}