From 6eaebe08e1aff3749075cc73b09d4ebb2c57dd5e Mon Sep 17 00:00:00 2001 From: Alexey Luchko Date: Mon, 18 Aug 2014 14:02:15 +0300 Subject: [AS-5] added bt_cfg command. --- makefiles/config_px4fmu-v2_default.mk | 1 + src/modules/bt_cfg/at.hpp | 148 +++++++++++++++++++++++++++++ src/modules/bt_cfg/main.cpp | 97 +++++++++++++++++++ src/modules/bt_cfg/module.mk | 42 +++++++++ src/modules/bt_cfg/read_write_log.hpp | 41 ++++++++ src/modules/bt_cfg/serial_config.hpp | 37 ++++++++ src/modules/bt_cfg/unique_file.hpp | 36 +++++++ src/modules/serial_echo/main.cpp | 150 ++++++++++++++++++++++++++++++ src/modules/serial_echo/module.mk | 2 +- src/modules/serial_echo/serial_config.hpp | 37 ++++++++ src/modules/serial_echo/serial_echo.cpp | 115 ----------------------- src/modules/serial_echo/unique_file.hpp | 36 +++++++ 12 files changed, 626 insertions(+), 116 deletions(-) create mode 100644 src/modules/bt_cfg/at.hpp create mode 100644 src/modules/bt_cfg/main.cpp create mode 100644 src/modules/bt_cfg/module.mk create mode 100644 src/modules/bt_cfg/read_write_log.hpp create mode 100644 src/modules/bt_cfg/serial_config.hpp create mode 100644 src/modules/bt_cfg/unique_file.hpp create mode 100644 src/modules/serial_echo/main.cpp create mode 100644 src/modules/serial_echo/serial_config.hpp delete mode 100644 src/modules/serial_echo/serial_echo.cpp create mode 100644 src/modules/serial_echo/unique_file.hpp diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 8959379c0..fdb7e2396 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -157,6 +157,7 @@ MODULES += lib/launchdetection # Hardware test #MODULES += examples/hwtest MODULES += modules/serial_echo +MODULES += modules/bt_cfg # # Transitional support - add commands from the NuttX export archive. diff --git a/src/modules/bt_cfg/at.hpp b/src/modules/bt_cfg/at.hpp new file mode 100644 index 000000000..7616c6a4e --- /dev/null +++ b/src/modules/bt_cfg/at.hpp @@ -0,0 +1,148 @@ +#pragma once + +#include + +#include +#include + +// #include +// #include +#include + +// #include "string_algorithms.hpp" + +namespace AT { + +constexpr size_t reply_buffer_size = 128; +constexpr size_t reply_read_retries = 25; +constexpr useconds_t reply_read_delay = 100 * 1000; + +// template +// std::pair // ok/err received, it was ok +// check_reply_ok_err(ForwardIt first, ForwardIt last) { +// bool found = false; +// bool ok = false; +// if (first != last) { +// auto crlf = find_crlf(first, last); +// while (crlf != last) { +// auto dist = std::distance(first, crlf); +// ok = dist == 2 and std::equal(first, crlf, "OK"); +// found = ok or (dist == 5 and std::equal(first, crlf, "ERROR")); +// +// if (found) break; +// +// std::advance(crlf, 2); +// first = crlf; +// crlf = find_crlf(first, last); +// } +// } +// return std::make_pair(found, ok); +// } +// +// template +// std::pair // ok/err received, it was ok +// wait_ok_error(Device &f) { +// auto retries = reply_read_retries; +// bool found = false; +// bool ok = false; +// char buf[reply_buffer_size]; +// ssize_t s = 0, n = 0; +// usleep(reply_read_delay); +// while (not found || retries > 0) { +// s = read(f, buf + n, sizeof(buf) - n); +// if (s < 0) break; +// if (s == 0) { +// usleep(reply_read_delay); +// --retries; +// s = read(f, buf + n, sizeof(buf) - n); +// if (s <= 0) break; +// } +// n += s; +// std::tie(found, ok) = check_reply_ok_err(buf, buf + n); +// } +// if (not found and n > 0) +// std::tie(found, ok) = check_reply_ok_err(buf, buf + n); +// return std::make_pair(found, ok); +// } + +template +std::pair // ok/err received, it was ok +wait_ok_error(Device &f) { + auto retries = reply_read_retries; + bool found = false; + bool ok = false; + char buf[reply_buffer_size + 1]; + ssize_t s = 0; + size_t n = 0; + usleep(reply_read_delay); + while (not found || retries > 0) { + s = read(f, buf + n, reply_buffer_size - n); + if (s < 0) break; + if (s == 0) { + usleep(reply_read_delay); + --retries; + s = read(f, buf + n, reply_buffer_size - n); + if (s <= 0) break; + } + n += s; + buf[n] = '\0'; + ok = std::strstr(buf, "\r\nOK\r\n"); + found = ok or std::strstr(buf, "\r\nERROR\r\n"); + } + if (not found and n > 0) { + buf[n] = '\0'; + ok = std::strstr(buf, "\r\nOK\r\n"); + found = ok or std::strstr(buf, "\r\nERROR\r\n"); + } + return std::make_pair(found, ok); +} + +template +inline bool +write_char(Device &f, char ch) { + return write(f, &ch, 1) == 1; +} + +template +inline bool +exec_cmd(Device &f, const char cmd[], std::size_t len) { + return write(f, cmd, len) == len and write_char(f, '\r') + and wait_ok_error(f).second; +} + +template +inline bool +exec_cmd(Device &f, const char * const cmd) { + return exec_cmd(f, cmd, std::strlen(cmd)); +} + +template +inline bool +exec_reset(Device &f) { + static const char at_reset[] = "AT*AMWS=0,0,0,0,1,0"; + const ssize_t len = sizeof(at_reset) - 1; + bool ok = write(f, at_reset, len) == len and write_char(f, '\r'); + if (ok) sleep(1); + return ok; +} + +template +inline bool +switch_to_at_mode(Device &f) { + sleep(1); + static const char escape_seq[3] = {'/', '/', '/'}; + bool ok = write(f, escape_seq, sizeof(escape_seq)); + if (ok) { + sleep(1); + ok = exec_cmd(f, "AT"); + } + return ok; +} + +template +inline bool +switch_to_data_mode(Device &f) { + return exec_cmd(f, "AT*ADDM"); +} + +} // end of namespace AT diff --git a/src/modules/bt_cfg/main.cpp b/src/modules/bt_cfg/main.cpp new file mode 100644 index 000000000..9ad41884e --- /dev/null +++ b/src/modules/bt_cfg/main.cpp @@ -0,0 +1,97 @@ +#include + +#include +#include + +#include +#include +#include + +extern "C" __EXPORT int +main(int argc, const char * const * argv); + +#include "at.hpp" + +namespace bt_cfg { + +#include "read_write_log.hpp" +#include "unique_file.hpp" + +#ifdef FORCE_SERIAL_TERMIOS_RAW +# include "serial_config.hpp" +#endif + +using namespace std; + +int +open_serial_default(const char name[]) { + int fd = ::open(name, O_RDWR | O_NONBLOCK | O_NOCTTY); + if (fd == -1) { + perror("open"); + exit(1); + } + +#ifdef FORCE_SERIAL_TERMIOS_RAW + if (not (serial_set_raw(fd) and serial_set_speed(fd, B57600))) { + fprintf(stderr, "failed setting raw mode and speed\n"); + exit(1); + } +#endif + + return fd; +} + +template +inline void +read_junk(Device &f) { + char buf[32]; + ssize_t s; + do { + s = ::read(f, s, sizeof(s)); + } while (s > 0); +} + +inline bool +streq(const char *a, const char *b) { + return not std::strcmp(a, b); +} + +} // end of namespace bt_cfg + +int +main(int argc, char const * const * argv) +{ + using namespace bt_cfg; + using namespace std; + + if (argc < 3) { + fprintf(stderr,"Usage: %s tty commands\n", argv[0]); + return 1; + } + + unique_file d = open_serial_default(argv[1]); + + DevLog f { d.get(), 2 }; + + bool ok; + const char * const *p; + for (p = argv + 2; *p; ++p) { + const char *cmd = *p; + + if (streq(cmd, "+at")) + ok = AT::switch_to_at_mode(f); + else if (streq(cmd, "-at")) + ok = AT::switch_to_data_mode(f); + else if (streq(cmd, "reset")) + ok = AT::exec_reset(f); + else + ok = AT::exec_cmd(f, cmd); + + if (not ok) { + fprintf(stderr, "Failed at command '%s'.\n", cmd); + return 1; + } + } + + return 0; +} diff --git a/src/modules/bt_cfg/module.mk b/src/modules/bt_cfg/module.mk new file mode 100644 index 000000000..68304cb89 --- /dev/null +++ b/src/modules/bt_cfg/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Basic example application +# + +MODULE_COMMAND = bt_cfg + +SRCS = main.cpp + +CXXFLAGS += -std=c++11 -Dmain=${MODULE_COMMAND}_main diff --git a/src/modules/bt_cfg/read_write_log.hpp b/src/modules/bt_cfg/read_write_log.hpp new file mode 100644 index 000000000..930249dee --- /dev/null +++ b/src/modules/bt_cfg/read_write_log.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include + +// #include "repr.hpp" + +struct DevLog { + int dev; + int log; + + DevLog(int d, int l) : dev {d}, log {l} {} + + friend ssize_t + read(DevLog &self, void *buf, size_t buf_size) + { + ssize_t r = read(self.dev, buf, buf_size); + if (r > 0) { write_log(self, true, buf, r); } + return r; + } + + friend ssize_t + write(DevLog &self, const void *buf, size_t buf_size) + { + ssize_t r = write(self.dev, buf, buf_size); + if (r > 0) { write_log(self, false, buf, buf_size); } + return r; + } + + friend void + write_log(DevLog &self, bool rw, const void *buf, size_t buf_size) + { + write(self.log, buf, buf_size); + + // static const char r[] = "read "; + // static const char w[sizeof(r)] = "write "; + // + // write(self.log, rw ? r : w, sizeof(r) - 1); + // write_repr(self.log, buf, buf_size); + // write(self.log, "\n", 2); + } +}; diff --git a/src/modules/bt_cfg/serial_config.hpp b/src/modules/bt_cfg/serial_config.hpp new file mode 100644 index 000000000..523ea938c --- /dev/null +++ b/src/modules/bt_cfg/serial_config.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include + +void +termios_raw_mode(struct termios & mode) { + mode.c_iflag = 0; + mode.c_oflag = 0; + mode.c_lflag = 0; + + mode.c_cflag &= ~CSIZE; + mode.c_cflag |= CS8; + mode.c_cflag &= ~CSTOPB; + mode.c_cflag &= ~(PARENB | PARODD); + mode.c_cflag |= CREAD; + mode.c_cflag |= CLOCAL; +} + +bool +serial_set_raw(int fd) { + struct termios mode; + bool ok = tcgetattr(fd, &mode) == 0; + if (ok) + { + termios_raw_mode(mode); + ok = tcsetattr(fd, TCSANOW, &mode) == 0; + } + return ok; +} + +bool +serial_set_speed(int fd, speed_t speed) { + struct termios mode; + return tcgetattr(fd, &mode) == 0 + and cfsetspeed(&mode, speed) == 0 + and tcsetattr(fd, TCSANOW, &mode) == 0; +} diff --git a/src/modules/bt_cfg/unique_file.hpp b/src/modules/bt_cfg/unique_file.hpp new file mode 100644 index 000000000..0735f7389 --- /dev/null +++ b/src/modules/bt_cfg/unique_file.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +class unique_file { +private: + int fd; + +public: + inline + unique_file() : fd(-1) {} + + inline + unique_file(int fd) : fd(fd) {} + + inline + unique_file(unique_file && other) : fd(other.fd) { + other.fd = -1; + } + + inline + ~unique_file() { + if (fd > -1) close(fd); + } + + inline + int get() { return fd; } + + friend inline std::size_t + read(unique_file & uf, void * buf, size_t buf_size) + { return ::read(uf.fd, buf, buf_size); } + + friend inline std::size_t + write(unique_file & uf, const void * buf, size_t buf_size) + { return ::write(uf.fd, buf, buf_size); } +}; diff --git a/src/modules/serial_echo/main.cpp b/src/modules/serial_echo/main.cpp new file mode 100644 index 000000000..e36821276 --- /dev/null +++ b/src/modules/serial_echo/main.cpp @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Example User + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +extern "C" __EXPORT int +main(int argc, char const * const * const argv); + +namespace serial_echo { + +#include "unique_file.hpp" + +#ifdef FORCE_SERIAL_TERMIOS_RAW +# include "serial_config.hpp" +#endif + + +using namespace std; + + +int +open_serial_default(const char name[]) { + int fd = open(name, O_RDWR | O_NONBLOCK | O_NOCTTY); + if (fd == -1) { + perror("open"); + exit(1); + } + +#ifdef FORCE_SERIAL_TERMIOS_RAW + if (not (serial_set_raw(fd) and serial_set_speed(fd, B57600))) { + fprintf(stderr, "failed setting raw mode and speed\n"); + exit(1); + } +#endif + + return fd; +} + +void +echo_forever(int dev) { + struct pollfd poll_dev; + poll_dev.fd = dev; + + static char buf[128]; + ssize_t n = 0; + ssize_t sr, sw; + + fprintf(stderr, "."); + fflush(stderr); + while (true) { + poll_dev.events = POLLIN; + if (n > 0) poll_dev.events |= POLLOUT; + + while (poll(&poll_dev, 1, 10000/*ms*/) != 1) { + fprintf(stderr, "."); + fflush(stderr); + } + + sr = 0; + sw = 0; + + if (poll_dev.revents & POLLIN) { + sr = read(dev, buf + n, sizeof(buf) - n); + if (sr > 0) n += sr; + } + + if (n > 0) { + sw = write(dev, buf, n); + if (sw > 0) + if (sw == n) { n = 0; } + else + { + n -= sw; + memcpy(buf, buf + sw, n); + } + } + + //if (n > 0 or sr > 0 or sw > 0) + // fprintf(stderr, "n %d sr %d sw %d\n", n, sr, sw); + } +} + +void +usage(const char * name) +{ + fprintf(stderr, + "_Forever_ reads data from a char-device and writes it back\n" + "Usage: %s tty_device\n", + name); + exit(1); +} + +} // end of namespace serial_echo + +int +main(int argc, char const * const * const argv) +{ + using namespace serial_echo; + + if (argc != 2) usage(argv[0]); + + fprintf(stderr, "open(\"%s\")\n", argv[1]); fflush(stderr); + unique_file dev = open_serial_default(argv[1]); + fprintf(stderr, "open -> %d\n", dev.get()); fflush(stderr); + + fprintf(stderr, "starting forever loop... bye\n"); fflush(stderr); + echo_forever(dev.get()); + + fprintf(stderr, "quit unexpectedly\n"); + + return 0; +} diff --git a/src/modules/serial_echo/module.mk b/src/modules/serial_echo/module.mk index 2c1f93719..a8a2ce36a 100644 --- a/src/modules/serial_echo/module.mk +++ b/src/modules/serial_echo/module.mk @@ -39,4 +39,4 @@ MODULE_COMMAND = serial_echo CXXFLAGS += -std=c++11 -Dmain=$(MODULE_COMMAND)_main -SRCS = serial_echo.cpp +SRCS = main.cpp diff --git a/src/modules/serial_echo/serial_config.hpp b/src/modules/serial_echo/serial_config.hpp new file mode 100644 index 000000000..523ea938c --- /dev/null +++ b/src/modules/serial_echo/serial_config.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include + +void +termios_raw_mode(struct termios & mode) { + mode.c_iflag = 0; + mode.c_oflag = 0; + mode.c_lflag = 0; + + mode.c_cflag &= ~CSIZE; + mode.c_cflag |= CS8; + mode.c_cflag &= ~CSTOPB; + mode.c_cflag &= ~(PARENB | PARODD); + mode.c_cflag |= CREAD; + mode.c_cflag |= CLOCAL; +} + +bool +serial_set_raw(int fd) { + struct termios mode; + bool ok = tcgetattr(fd, &mode) == 0; + if (ok) + { + termios_raw_mode(mode); + ok = tcsetattr(fd, TCSANOW, &mode) == 0; + } + return ok; +} + +bool +serial_set_speed(int fd, speed_t speed) { + struct termios mode; + return tcgetattr(fd, &mode) == 0 + and cfsetspeed(&mode, speed) == 0 + and tcsetattr(fd, TCSANOW, &mode) == 0; +} diff --git a/src/modules/serial_echo/serial_echo.cpp b/src/modules/serial_echo/serial_echo.cpp deleted file mode 100644 index 29bf59901..000000000 --- a/src/modules/serial_echo/serial_echo.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include - -#include -#include -#include - -#include -#include -#include - - -using namespace std; - - -extern "C" __EXPORT int -main(int argc, char ** const argv); - - -void -echo_forever(int dev) { - struct pollfd poll_dev; - poll_dev.fd = dev; - - char buf[32]; - ssize_t n = 0; - ssize_t sr, sw; - while (true) { - poll_dev.events = POLLIN; - if (n > 0) poll_dev.events |= POLLOUT; - - while (poll(&poll_dev, 1, 10000/*ms*/) != 1) - /* wait forever */ ; - - sr = 0; - sw = 0; - - if (poll_dev.revents & POLLIN) { - sr = read(dev, buf + n, sizeof(buf) - n); - if (sr > 0) n += sr; - } - - if (n > 0) { - sw = write(dev, buf, n); - if (sw > 0) - if (sw == n) n = 0; - else { - n -= sw; - memcpy(buf, buf + sw, n); - } - } - - if (n > 0 or sr > 0 or sw > 0) - fprintf(stderr, "n %d sr %d sw %d\n", n, sr, sw); - } - -} - - -void -usage(const char * name) -{ - fprintf(stderr, - "_Forever_ reads data from a char-device and writes it back\n" - "Usage: %s tty_device\n", - name); - exit(1); -} - -int -main(int argc, char ** const argv) -{ - if (argc != 2) usage(argv[0]); - - int dev = open(argv[1], O_RDWR | O_NONBLOCK | O_NOCTTY ); - if (dev < 0) { - perror("open"); - usage(argv[0]); - } - - echo_forever(dev); -} diff --git a/src/modules/serial_echo/unique_file.hpp b/src/modules/serial_echo/unique_file.hpp new file mode 100644 index 000000000..0735f7389 --- /dev/null +++ b/src/modules/serial_echo/unique_file.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +class unique_file { +private: + int fd; + +public: + inline + unique_file() : fd(-1) {} + + inline + unique_file(int fd) : fd(fd) {} + + inline + unique_file(unique_file && other) : fd(other.fd) { + other.fd = -1; + } + + inline + ~unique_file() { + if (fd > -1) close(fd); + } + + inline + int get() { return fd; } + + friend inline std::size_t + read(unique_file & uf, void * buf, size_t buf_size) + { return ::read(uf.fd, buf, buf_size); } + + friend inline std::size_t + write(unique_file & uf, const void * buf, size_t buf_size) + { return ::write(uf.fd, buf, buf_size); } +}; -- cgit v1.2.3