diff options
author | Don Gagne <don@thegagnes.com> | 2014-06-27 20:33:39 -0700 |
---|---|---|
committer | Don Gagne <don@thegagnes.com> | 2014-06-27 20:33:39 -0700 |
commit | a398d5cbcc1d39d83611543b787c5c8aecae10dd (patch) | |
tree | d01fb8081510d72e55105d976d348b11db68c1fe | |
parent | 7546b99a24a33ff160b29b5be31a9e0d39bbce1a (diff) | |
download | px4-firmware-a398d5cbcc1d39d83611543b787c5c8aecae10dd.tar.gz px4-firmware-a398d5cbcc1d39d83611543b787c5c8aecae10dd.tar.bz2 px4-firmware-a398d5cbcc1d39d83611543b787c5c8aecae10dd.zip |
Support for List, Open, Read, Terminate commands
Fixed various bugs. Flattened Mavlink::Session class while fixing bugs
in this area.
-rw-r--r-- | src/modules/mavlink/mavlink_ftp.cpp | 167 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_ftp.h | 33 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 54 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 3 |
4 files changed, 122 insertions, 135 deletions
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index bfee17342..f1436efb8 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -55,6 +55,11 @@ MavlinkFTP::MavlinkFTP() // initialise the request freelist dq_init(&_workFree); sem_init(&_lock, 0, 1); + + // initialize session list + for (size_t i=0; i<kMaxSession; i++) { + _session_fds[i] = -1; + } // drop work entries onto the free list for (unsigned i = 0; i < kRequestQueueSize; i++) { @@ -122,13 +127,11 @@ MavlinkFTP::_worker(Request *req) break; case kCmdTerminate: - if (!Session::terminate(hdr->session)) { - errorCode = kErrNoRequest; - } + errorCode = _workTerminate(req); break; case kCmdReset: - Session::reset(); + errorCode = _workReset(); break; case kCmdList: @@ -219,6 +222,12 @@ MavlinkFTP::_workList(Request *req) // no more entries? if (result == nullptr) { + if (hdr->offset != 0 && offset == 0) { + // User is requesting subsequent dir entries but there were none. This means the user asked + // to seek past EOF. + errorCode = kErrEOF; + } + // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that break; } @@ -256,20 +265,21 @@ MavlinkFTP::ErrorCode MavlinkFTP::_workOpen(Request *req, bool create) { auto hdr = req->header(); - - // allocate session ID - int session = Session::allocate(); - if (session < 0) { + + int session_index = _findUnusedSession(); + if (session_index < 0) { return kErrNoSession; } - // get the session to open the file - if (!Session::get(session)->open(req->dataAsCString(), create)) { + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; + + int fd = ::open(req->dataAsCString(), oflag); + if (fd < 0) { return create ? kErrPerm : kErrNotFile; } + _session_fds[session_index] = fd; - // save the session ID in the reply - hdr->session = session; + hdr->session = session_index; hdr->size = 0; return kErrNone; @@ -280,29 +290,40 @@ MavlinkFTP::_workRead(Request *req) { auto hdr = req->header(); - // look up session - auto session = Session::get(hdr->session); - if (session == nullptr) { + int session_index = hdr->session; + + if (!_validSession(session_index)) { return kErrNoSession; + } + + // Seek to the specified position + printf("Seek %d\n", hdr->offset); + if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + return kErrEOF; } - - // read from file - int result = session->read(hdr->offset, &hdr->data[0], hdr->size); - - if (result < 0) { + + int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); + if (bytes_read < 0) { + // Negative return indicates error other than eof return kErrIO; } - hdr->size = result; + + printf("Read success %d\n", bytes_read); + hdr->size = bytes_read; + return kErrNone; } MavlinkFTP::ErrorCode MavlinkFTP::_workWrite(Request *req) { +#if 0 + // NYI: Coming soon auto hdr = req->header(); // look up session - auto session = Session::get(hdr->session); + auto session = getSession(hdr->session); if (session == nullptr) { return kErrNoSession; } @@ -317,6 +338,9 @@ MavlinkFTP::_workWrite(Request *req) hdr->size = result; return kErrNone; +#else + return kErrPerm; +#endif } MavlinkFTP::ErrorCode @@ -328,91 +352,52 @@ MavlinkFTP::_workRemove(Request *req) return kErrPerm; } -MavlinkFTP::Session MavlinkFTP::Session::_sessions[MavlinkFTP::Session::kMaxSession]; - -int -MavlinkFTP::Session::allocate() -{ - for (unsigned i = 0; i < kMaxSession; i++) { - if (_sessions[i]._fd < 0) { - return i; - } - } - return -1; -} - -MavlinkFTP::Session * -MavlinkFTP::Session::get(unsigned index) -{ - if ((index >= kMaxSession) || (_sessions[index]._fd < 0)) { - return nullptr; - } - return &_sessions[index]; -} - -void -MavlinkFTP::Session::terminate() +MavlinkFTP::ErrorCode +MavlinkFTP::_workTerminate(Request *req) { - // clean up aborted transfers? - if (_fd >= 0) { - close(_fd); - _fd = -1; - } -} - -bool -MavlinkFTP::Session::terminate(unsigned index) - { - Session *session = get(index); - - if (session == nullptr) { - return false; - } + auto hdr = req->header(); + + if (!_validSession(hdr->session)) { + return kErrNoSession; + } + + ::close(_session_fds[hdr->session]); - session->terminate(); - return true; + return kErrNone; } -void -MavlinkFTP::Session::reset() +MavlinkFTP::ErrorCode +MavlinkFTP::_workReset(void) { - for (unsigned i = 0; i < kMaxSession; i++) { - terminate(i); - } + for (size_t i=0; i<kMaxSession; i++) { + if (_session_fds[i] != -1) { + ::close(_session_fds[i]); + _session_fds[i] = -1; + } + } + + return kErrNone; } bool -MavlinkFTP::Session::open(const char *path, bool create) +MavlinkFTP::_validSession(unsigned index) { - int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - - _fd = open(path, oflag); - if (_fd < 0) { + if ((index >= kMaxSession) || (_session_fds[index] < 0)) { return false; } return true; } int -MavlinkFTP::Session::read(off_t offset, uint8_t *buf, uint8_t count) +MavlinkFTP::_findUnusedSession(void) { - // can we seek to the location? - if (lseek(_fd, offset, SEEK_SET) < 0) { - return -1; - } - - return read(_fd, buf, count); -} - -int -MavlinkFTP::Session::append(off_t offset, uint8_t *buf, uint8_t count) -{ - // make sure that the requested offset matches our current position - off_t pos = lseek(_fd, 0, SEEK_CUR); - if (pos != offset) { - return -1; - } - return write(_fd, buf, count); + for (size_t i=0; i<kMaxSession; i++) { + if (_session_fds[i] == -1) { + return i; + } + } + + return -1; } char * diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 462594301..8a1b3fbaa 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -115,27 +115,11 @@ private: kErrPerm }; - class Session - { - public: - Session() : _fd(-1) {} - - static int allocate(); - static Session *get(unsigned index); - static bool terminate(unsigned index); - static void reset(); - - void terminate(); - bool open(const char *path, bool create); - int read(off_t offset, uint8_t *buf, uint8_t count); - int append(off_t offset, uint8_t *buf, uint8_t count); + int _findUnusedSession(void); + bool _validSession(unsigned index); - private: - static const unsigned kMaxSession = 2; - static Session _sessions[kMaxSession]; - - int _fd; - }; + static const unsigned kMaxSession = 2; + int _session_fds[kMaxSession]; class Request { @@ -163,7 +147,11 @@ private: unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), _mavlink->get_channel(), &msg, sequence(), rawData()); - if (!_mavlink->message_buffer_write(&msg, len)) { + _mavlink->lockMessageBufferMutex(); + bool fError = _mavlink->message_buffer_write(&msg, len); + _mavlink->unlockMessageBufferMutex(); + + if (!fError) { warnx("FTP TX ERR"); } else { warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", @@ -211,6 +199,8 @@ private: ErrorCode _workRead(Request *req); ErrorCode _workWrite(Request *req); ErrorCode _workRemove(Request *req); + ErrorCode _workTerminate(Request *req); + ErrorCode _workReset(); // work freelist Request _workBufs[kRequestQueueSize]; @@ -232,4 +222,5 @@ private: _qUnlock(); return req; } + }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e9cce175d..c77f3c6ca 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2074,38 +2074,46 @@ Mavlink::task_main(int argc, char *argv[]) if (_passing_on || _ftp_on) { bool is_part; - void *read_ptr; + uint8_t *read_ptr; + uint8_t *write_ptr; - /* guard get ptr by mutex */ pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr(&read_ptr, &is_part); + int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - - // int oldseq = mavlink_get_channel_status(get_channel())->current_tx_seq; - - const mavlink_message_t* msg = (const mavlink_message_t*)read_ptr; - /* write first part of buffer */ - _mavlink_resend_uart(_channel, msg); - - // mavlink_get_channel_status(get_channel())->current_tx_seq = oldseq; - // mavlink_msg_system_time_send(get_channel(), 255, 255); - - message_buffer_mark_read(available); - + // Reconstruct message from buffer + + mavlink_message_t msg; + write_ptr = (uint8_t*)&msg; + + // Pull a single message from the buffer + int read_count = available; + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ - // XXX this doesn't quite work, as the resend UART call assumes a continous block - if (is_part) { - /* guard get ptr by mutex */ - pthread_mutex_lock(&_message_buffer_mutex); - available = message_buffer_get_ptr(&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + if (is_part && read_count < sizeof(mavlink_message_t)) { + write_ptr += read_count; + available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } + + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 76ce42943..d44db0819 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -228,6 +228,9 @@ public: bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } bool message_buffer_write(void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } protected: Mavlink *next; |