add 32bit crc to mavlink frames, and reject those that fail.

conf_schema.h

@@ -240,6 +240,7 @@ 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, mavlinkfsm,                0, boolean,, "")
 ATOM(bool_t, peers,                     0, boolean,, "")
 ATOM(bool_t, overlayframes,             0, boolean,, "")
 ATOM(bool_t, overlayabbreviations,      0, boolean,, "")

mavlink.c

@@ -117,10 +117,16 @@ int stream_as_mavlink(int sequence_number,int startP,int endP,
 		      unsigned char *data,int count,
 		      unsigned char *frame,int *outlen)
 {
-  if (count>252-6-2) return -1;
+  if (count>252-6-4) return -1;
   
   frame[0]=0xfe; // mavlink v1.0 frame
-  frame[1]=count; // payload len, excluding 6 byte header and 2 byte CRC
+  /* 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.
+  */
+  frame[1]=count+2; 
   frame[2]=sequence_number; // packet sequence
   frame[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
   frame[4]=0x40; // component ID of sender: we are reusing this to mark start,end of MDP frames
@@ -129,12 +135,14 @@ int stream_as_mavlink(int sequence_number,int startP,int endP,
   frame[5]=MAVLINK_MSG_ID_DATASTREAM; // message ID type of this frame: DATA_STREAM
   // payload follows (we reuse the DATA_STREAM message type parameters)
   bcopy(data,&frame[6],count);
-  // two-byte CRC follows
+  // Add our own 4-byte CRC of the data only
-  uint16_t crc=mavlink_crc(frame,count); // automatically adds 6 for header length
+  uint32_t crc=Crc32_ComputeBuf(0,&frame[1],-1+count+6);
-  frame[count+6]=crc&0xff;
+  frame[6+count+0]=(crc>>0)&0xff;
-  frame[count+7]=crc>>8;
+  frame[6+count+1]=(crc>>8)&0xff;
-  
+  frame[6+count+2]=(crc>>16)&0xff;
-  *outlen=count+6+2;
+  frame[6+count+3]=(crc>>24)&0xff;
+
+  *outlen=count+6+4;
   return 0;
 }
 
@@ -242,43 +250,61 @@ int mavlink_parse(struct slip_decode_state *state)
 
 int mavlink_decode(struct slip_decode_state *state,uint8_t c)
 {
-  if (config.debug.mavlink) DEBUGF("RX character %02x in state %s",
+  if (config.debug.mavlinkfsm) DEBUGF("RX character %02x in state %s",
-				   c,mavlink_describe_state(state->state));
+				      c,mavlink_describe_state(state->state));
   switch(state->state) {
   case MAVLINK_STATE_LENGTH:
+    state->mavlink_crc=0;
+    state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
     state->mavlink_payload_length=c;
     state->mavlink_payload_offset=0;
     state->state++;
     break;
   case MAVLINK_STATE_SEQ:
+    state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
     state->mavlink_sequence=c;
     state->state++;
     break;
   case MAVLINK_STATE_SYSID:
+    state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
     state->mavlink_sysid=c;
     state->state++;
     break;
   case MAVLINK_STATE_COMPONENTID:
+    state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
     state->mavlink_componentid=c;
     state->state++;
     break;
   case MAVLINK_STATE_MSGID:
+    state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
     state->mavlink_msgid=c;
     state->state++;
     break;
   case MAVLINK_STATE_PAYLOAD:
+    if (state->mavlink_payload_length-state->mavlink_payload_offset>2)
+      state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
+    if (state->mavlink_payload_length-state->mavlink_payload_offset==2)
+      state->mavlink_rxcrc=c;
+    if (state->mavlink_payload_length-state->mavlink_payload_offset==1)
+      state->mavlink_rxcrc|=c<<8;
     state->mavlink_payload[state->mavlink_payload_offset++]=c;
     if (state->mavlink_payload_offset==state->mavlink_payload_length)
       state->state++;
     break;
   case MAVLINK_STATE_CRC1:
-    state->mavlink_rxcrc=c;
+    state->mavlink_rxcrc|=c<<16;
     state->state++;
     break;
   case MAVLINK_STATE_CRC2:
-    state->mavlink_rxcrc|=c<<8;
+    state->mavlink_rxcrc|=c<<24;
     state->state=MAVLINK_STATE_UNKNOWN;
-    return mavlink_parse(state);
+    state->mavlink_payload_length-=2; // don't count the upper 16 bits of our 32bit CRC
+    if (state->mavlink_rxcrc==state->mavlink_crc)
+      return mavlink_parse(state);
+    if (config.debug.mavlink)
+      DEBUGF("CRC mismatch on mavlink encapsulated data: rxed=%08x, calced=%08x",
+	     state->mavlink_rxcrc,state->mavlink_crc);
+    state->mavlink_payload_length=0;
     break;
   case MAVLINK_STATE_UNKNOWN:
   default:

overlay_interface.c

@@ -896,6 +896,8 @@ overlay_broadcast_ensemble(struct network_destination *destination,
 	  state.src_offset=0;
 	  state.src=buffer+out_len;
 	  slip_decode(&state);
+	  // clear received packet after processing
+	  state.packet_length=0;
 	}
 
       out_len+=encoded;

serval.h

@@ -388,7 +388,8 @@ struct slip_decode_state{
   uint8_t mavlink_sysid;
   uint8_t mavlink_componentid;
   uint8_t mavlink_msgid;
-  uint16_t mavlink_rxcrc;
+  uint32_t mavlink_rxcrc;
+  uint32_t mavlink_crc;
 };
 
 struct overlay_interface;

slip.c

@@ -60,15 +60,13 @@ int slip_encode(int format,
 	  int startP=i?0:1;
 	  int endP=i+slice_bytes==src_bytes?1:0;
 	  stream_as_mavlink(0,startP,endP,&src[i],slice_bytes,&dst[dst_offset],&slice_len);
-	  if (config.debug.mavlink) {
-	    DEBUGF("Wrote %d bytes as %d byte MAVLink frame",
-		   slice_bytes,slice_len);
-	    dump("original data",&src[i],slice_bytes);
-	    dump("mavlink frame",&dst[dst_offset],slice_len);
-	  }
 	  dst_offset+=slice_len;
 	}
       }
+      if (config.debug.mavlink) {
+	DEBUGF("Wrote %d byte packet as MAVLink frames",src_bytes);
+      }
+
       return dst_offset;
     }
     break;
@@ -385,6 +383,9 @@ int slip_decode(struct slip_decode_state *state)
 	  // We have to increment src_offset manually here, because returning
 	  // prevents the post-increment in the for loop from triggering
 	  state->src_offset++;
+	  if (config.debug.mavlink) {
+	    DEBUGF("Read %d byte packet from MAVLink frames",state->packet_length);
+	  }
 	  return 1;
 	}
       }