diff --git a/mavlink.c b/mavlink.c index 27c7339d..208617dc 100644 --- a/mavlink.c +++ b/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; diff --git a/overlay_interface.c b/overlay_interface.c index 2ae1d4b7..12e5dade 100644 --- a/overlay_interface.c +++ b/overlay_interface.c @@ -849,21 +849,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); } @@ -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) - { - // Test decoding of the packet we send - struct slip_decode_state state; - state.encapsulator=SLIP_FORMAT_MAVLINK; - state.src_size=encoded; - state.src_offset=0; - state.src=buffer+out_len; - slip_decode(&state); - // clear received packet after processing - state.packet_length=0; - } + if (config.debug.slip){ + // Test decoding of the packet we send + struct slip_decode_state state; + state.encapsulator=SLIP_FORMAT_MAVLINK; + state.src_size=encoded; + state.src_offset=0; + state.src=interface->txbuffer+out_len; + slip_decode(&state); + // 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, - 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 @@ -964,6 +969,7 @@ overlay_broadcast_ensemble(struct network_destination *destination, } default: + ob_free(buffer); return WHY("Unsupported socket type"); } } diff --git a/overlay_queue.c b/overlay_queue.c index 0a7ba57f..fd2f4c79 100644 --- a/overlay_queue.c +++ b/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)); - ob_free(packet->buffer); + overlay_broadcast_ensemble(packet->destination, packet->buffer); ret=1; } if (packet->destination) diff --git a/serval.h b/serval.h index ed7cd6b9..f3ab79a3 100644 --- a/serval.h +++ b/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, - unsigned char *bytes,int len); +int overlay_broadcast_ensemble(struct network_destination *destination, struct overlay_buffer *buffer); 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 diff --git a/slip.c b/slip.c index e710ace6..66395cb5 100644 --- a/slip.c +++ b/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;idst_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); - // (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;idst_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;