protocol_splitter: simplify write()

With the protocol header we don't need to parse the protocol anymore.

src/drivers/protocol_splitter/protocol_splitter.cpp

@@ -283,13 +283,6 @@ protected:
 
 	Sp2Header _header;
 
-	uint16_t _packet_len;
-	enum class ParserState : uint8_t {
-		Idle = 0,
-		GotLength
-	};
-	ParserState _parser_state = ParserState::Idle;
-
 	ReadBuffer *_read_buffer;
 };
 
@@ -640,72 +633,24 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
 
 ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen)
 {
-	/*
-	 * we need to look into the data to make sure the output is locked for the duration
-	 * of a whole packet.
-	 * assumptions:
-	 * - packet header is written all at once (or at least it contains the payload length)
-	 * - a single write call does not contain multiple (or parts of multiple) packets
-	 */
-	ssize_t ret = 0;
-
-	switch (_parser_state) {
-	case ParserState::Idle:
-		ASSERT(buflen >= 3);
-
-		if ((unsigned char)buffer[0] == 253) {
-			uint8_t payload_len = buffer[1];
-			uint8_t incompat_flags = buffer[2];
-			_packet_len = payload_len + 12;
-
-			if (incompat_flags & 0x1) { //signing
-				_packet_len += 13;
-			}
-
-			_parser_state = ParserState::GotLength;
-			lock(Write);
-
-		} else if ((unsigned char)buffer[0] == 254) { // mavlink 1
-			uint8_t payload_len = buffer[1];
-			_packet_len = payload_len + 8;
-
-			_parser_state = ParserState::GotLength;
-			lock(Write);
-
-		} else {
-			PX4_ERR("parser error");
-			return 0;
+	_header.fields.len_h = (buflen >> 8) & 0x7f;
+	_header.fields.len_l = buflen & 0xff;
+	_header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2];
+	lock(Write);
+	int buf_free = 0;
+	::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free);
+	ssize_t ret = -1;
+
+	if ((size_t)buf_free >= sizeof(_header) + buflen) {
+		ret = ::write(_fd, _header.bytes, sizeof(_header));
+
+		if (ret == sizeof(_header)) {
+			ret = ::write(_fd, buffer, buflen);
 		}
-
-	/* FALLTHROUGH */
-
-	case ParserState::GotLength: {
-			_packet_len -= buflen;
-			int buf_free;
-			::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free);
-
-			if (buf_free < (int)buflen) {
-				//let write fail, to let mavlink know the buffer would overflow
-				//(this is because in the ioctl we pretend there is always enough space)
-				ret = -1;
-
-			} else {
-				_header.fields.len_h = (buflen >> 8) & 0x7f;
-				_header.fields.len_l = buflen & 0xff;
-				_header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2];
-				::write(_fd, _header.bytes, 4);
-				ret = ::write(_fd, buffer, buflen);
-			}
-
-			if (_packet_len == 0) {
-				unlock(Write);
-				_parser_state = ParserState::Idle;
-			}
-		}
-
-		break;
 	}
 
+	unlock(Write);
+
 	return ret;
 }
 
@@ -769,60 +714,24 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
 
 ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen)
 {
-	/*
-	 * we need to look into the data to make sure the output is locked for the duration
-	 * of a whole packet.
-	 * assumptions:
-	 * - packet header is written all at once (or at least it contains the payload length)
-	 * - a single write call does not contain multiple (or parts of multiple) packets
-	 */
-	ssize_t ret = 0;
-	uint16_t payload_len;
-
-	switch (_parser_state) {
-	case ParserState::Idle:
-		ASSERT(buflen >= HEADER_SIZE);
-
-		if (memcmp(buffer, ">>>", 3) != 0) {
-			PX4_ERR("parser error");
-			return 0;
+	_header.fields.len_h = (buflen >> 8) & 0x7f;
+	_header.fields.len_l = buflen & 0xff;
+	_header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2];
+	lock(Write);
+	int buf_free = 0;
+	ssize_t ret = -1;
+	::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free);
+
+	if ((size_t)buf_free >= sizeof(_header) + buflen) {
+		ret = ::write(_fd, _header.bytes, sizeof(_header));
+
+		if (ret == sizeof(_header)) {
+			ret = ::write(_fd, buffer, buflen);
 		}
-
-		payload_len = ((uint16_t)buffer[6] << 8) | buffer[7];
-		_packet_len = payload_len + HEADER_SIZE;
-		_parser_state = ParserState::GotLength;
-		lock(Write);
-
-	/* FALLTHROUGH */
-
-	case ParserState::GotLength: {
-			_packet_len -= buflen;
-			int buf_free;
-			::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free);
-
-			// TODO should I care about this for rtps?
-			if ((unsigned)buf_free < buflen) {
-				//let write fail, to let rtps know the buffer would overflow
-				//(this is because in the ioctl we pretend there is always enough space)
-				ret = -1;
-
-			} else {
-				_header.fields.len_h = (buflen >> 8) & 0x7f;
-				_header.fields.len_l = buflen & 0xff;
-				_header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2];
-				::write(_fd, _header.bytes, 4);
-				ret = ::write(_fd, buffer, buflen);
-			}
-
-			if (_packet_len == 0) {
-				unlock(Write);
-				_parser_state = ParserState::Idle;
-			}
-		}
-
-		break;
 	}
 
+	unlock(Write);
+
 	return ret;
 }