Pass ownership of final packet buffer to broadcast_ensemble

mavlink.c

@@ -135,7 +135,7 @@ 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 stream_as_mavlink(int sequence_number,int startP,int endP,
-		      unsigned char *data,int count,
+		      const unsigned char *data,int count,
 		      unsigned char *frame,int *outlen)
 {
   if (count>252-6-32) return -1;

overlay_interface.c

@@ -849,21 +849,22 @@ static void overlay_interface_poll(struct sched_ent *alarm)
   }  
 }
 
-int
+int overlay_broadcast_ensemble(struct network_destination *destination, struct overlay_buffer *buffer)
-overlay_broadcast_ensemble(struct network_destination *destination,
-			   unsigned char *bytes,int len)
 {
   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);
   }
 
@@ -873,31 +874,31 @@ overlay_broadcast_ensemble(struct network_destination *destination,
   switch(interface->socket_type){
     case SOCK_STREAM:
     {
-      if (interface->tx_bytes_pending>0)
+      if (interface->tx_bytes_pending>0){
+	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_MAVLINK,
-				bytes, len, buffer+out_len, sizeof(interface->txbuffer) - out_len);
+				bytes, len, 
+				interface->txbuffer+out_len, sizeof(interface->txbuffer) - out_len);
+      ob_free(buffer);
       if (encoded < 0)
 	return WHY("Buffer overflow");
-						
+      if (config.debug.slip){
-      if (config.debug.slip)
+	// Test decoding of the packet we send
-	{
+	struct slip_decode_state state;
-	  // Test decoding of the packet we send
+	state.encapsulator=SLIP_FORMAT_MAVLINK;
-	  struct slip_decode_state state;
+	state.src_size=encoded;
-	  state.encapsulator=SLIP_FORMAT_MAVLINK;
+	state.src_offset=0;
-	  state.src_size=encoded;
+	state.src=interface->txbuffer+out_len;
-	  state.src_offset=0;
+	slip_decode(&state);
-	  state.src=buffer+out_len;
+	// clear received packet after processing
-	  slip_decode(&state);
+	state.packet_length=0;
-	  // clear received packet after processing
+      }
-	  state.packet_length=0;
-	}
 
       out_len+=encoded;
       
@@ -924,6 +925,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. */
@@ -950,8 +952,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, 
+      int sent=sendto(interface->alarm.poll.fd, 
-		bytes, len, 0, (struct sockaddr *)&destination->address, sizeof(destination->address)) != len){
+		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
@@ -964,6 +969,7 @@ overlay_broadcast_ensemble(struct network_destination *destination,
     }
       
     default:
+      ob_free(buffer);
       return WHY("Unsupported socket type");
   }
 }

overlay_queue.c

@@ -515,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));
+    overlay_broadcast_ensemble(packet->destination, packet->buffer);
-    ob_free(packet->buffer);
     ret=1;
   }
   if (packet->destination)

serval.h

@@ -698,8 +698,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,
+int overlay_broadcast_ensemble(struct network_destination *destination, struct overlay_buffer *buffer);
-			       unsigned char *bytes,int len);
 
 int directory_registration();
 int directory_service_init();
@@ -849,7 +848,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,
@@ -880,7 +879,7 @@ int generate_nonce(unsigned char *nonce,int bytes);
 int mavlink_decode(struct slip_decode_state *state,uint8_t c);
 int mavlink_heartbeat(unsigned char *frame,int *outlen);
 int stream_as_mavlink(int sequence_number,int startP,int endP,
-		      unsigned char *data,int count,
+		      const unsigned char *data,int count,
 		      unsigned char *frame,int *outlen);
 
 #endif // __SERVALD_SERVALD_H

slip.c

@@ -42,8 +42,48 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 #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_MAVLINK:
@@ -87,51 +127,25 @@ int slip_encode(int 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);
+      int ret=encode_slip(src, src_bytes, dst + offset, dst_len - offset);
-      // (I'm assuming there are 4 extra bytes in memory here, which is very naughty...)
+      if (ret<0)
-      write_uint32(src+src_bytes, crc);
+	return ret;
+      offset+=ret;
       
-      for (i=0;i<src_bytes+4;i++){
+      unsigned char crc[4];
+      write_uint32(crc, Crc32_ComputeBuf( 0, src, src_bytes));
       
-	if (offset+3>dst_len)
+      ret=encode_slip(crc, 4, dst + offset, dst_len - offset);
-	  return WHY("Dest buffer full");
+      if (ret<0)
+	return ret;
+      offset+=ret;
       
-	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;