From 6edb55c8745e89ba7d3f6836dccdc3cf8970bbab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 13 Aug 2021 14:10:48 +0200 Subject: [PATCH] protocol_splitter: simplify write() With the protocol header we don't need to parse the protocol anymore. --- .../protocol_splitter/protocol_splitter.cpp | 143 ++++-------------- 1 file changed, 26 insertions(+), 117 deletions(-) diff --git a/src/drivers/protocol_splitter/protocol_splitter.cpp b/src/drivers/protocol_splitter/protocol_splitter.cpp index 53ca8a730a..966f83c9a0 100644 --- a/src/drivers/protocol_splitter/protocol_splitter.cpp +++ b/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; + _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; - switch (_parser_state) { - case ParserState::Idle: - ASSERT(buflen >= 3); + if ((size_t)buf_free >= sizeof(_header) + buflen) { + ret = ::write(_fd, _header.bytes, sizeof(_header)); - 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; + 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; + _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); - switch (_parser_state) { - case ParserState::Idle: - ASSERT(buflen >= HEADER_SIZE); + if ((size_t)buf_free >= sizeof(_header) + buflen) { + ret = ::write(_fd, _header.bytes, sizeof(_header)); - if (memcmp(buffer, ">>>", 3) != 0) { - PX4_ERR("parser error"); - return 0; + 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; }