From bf318afedd47ac68cc179d8cd07fa9c3da1ca6d7 Mon Sep 17 00:00:00 2001 From: Pau Espin Pedrol Date: Sat, 28 Nov 2020 17:21:13 +0100 Subject: [PATCH] Initial commit --- .gitignore | 39 +++ COPYING | 339 ++++++++++++++++++++ Makefile.am | 20 ++ README.md | 22 ++ configure.ac | 94 ++++++ git-version-gen | 151 +++++++++ include/Makefile.am | 1 + include/osmocom/Makefile.am | 1 + include/osmocom/modbus/Makefile.am | 8 + include/osmocom/modbus/modbus.h | 40 +++ include/osmocom/modbus/modbus_conn.h | 69 +++++ include/osmocom/modbus/modbus_prim.h | 60 ++++ include/osmocom/modbus/modbus_rtu.h | 40 +++ libosmo-modbus.pc.in | 10 + src/Makefile.am | 28 ++ src/conn.c | 228 ++++++++++++++ src/conn_fsm.h | 26 ++ src/conn_master_fsm.c | 233 ++++++++++++++ src/conn_rtu.c | 444 +++++++++++++++++++++++++++ src/conn_slave_fsm.c | 174 +++++++++++ src/modbus_internal.h | 42 +++ src/prim.c | 91 ++++++ src/rtu_internal.h | 28 ++ src/rtu_transmit_fsm.c | 301 ++++++++++++++++++ src/rtu_transmit_fsm.h | 21 ++ utils/Makefile.am | 20 ++ utils/crc16_rtu_gen.c | 90 ++++++ utils/modbus_rtu_master.c | 270 ++++++++++++++++ utils/modbus_rtu_slave.c | 258 ++++++++++++++++ 29 files changed, 3148 insertions(+) create mode 100644 .gitignore create mode 100644 COPYING create mode 100644 Makefile.am create mode 100644 README.md create mode 100644 configure.ac create mode 100755 git-version-gen create mode 100644 include/Makefile.am create mode 100644 include/osmocom/Makefile.am create mode 100644 include/osmocom/modbus/Makefile.am create mode 100644 include/osmocom/modbus/modbus.h create mode 100644 include/osmocom/modbus/modbus_conn.h create mode 100644 include/osmocom/modbus/modbus_prim.h create mode 100644 include/osmocom/modbus/modbus_rtu.h create mode 100644 libosmo-modbus.pc.in create mode 100644 src/Makefile.am create mode 100644 src/conn.c create mode 100644 src/conn_fsm.h create mode 100644 src/conn_master_fsm.c create mode 100644 src/conn_rtu.c create mode 100644 src/conn_slave_fsm.c create mode 100644 src/modbus_internal.h create mode 100644 src/prim.c create mode 100644 src/rtu_internal.h create mode 100644 src/rtu_transmit_fsm.c create mode 100644 src/rtu_transmit_fsm.h create mode 100644 utils/Makefile.am create mode 100644 utils/crc16_rtu_gen.c create mode 100644 utils/modbus_rtu_master.c create mode 100644 utils/modbus_rtu_slave.c diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..6628b4d --- /dev/null +++ b/.gitignore @@ -0,0 +1,39 @@ +*.o +*.a +*.lo +*.la +.deps +Makefile +Makefile.in +openbsc.pc +*.*~ +*.sw? + +#configure +aclocal.m4 +autom4te.cache/ +compile +config.log +config.status +configure +configure.lineno +depcomp +install-sh +missing +stamp-h1 + +# libtool +ltmain.sh +libtool +.libs + +# git-version-gen magic +.tarball-version +.version + + +# apps and app data +utils/modbus_rtu_master + +*.pc +config.* diff --git a/COPYING b/COPYING new file mode 100644 index 0000000..d511905 --- /dev/null +++ b/COPYING @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/Makefile.am b/Makefile.am new file mode 100644 index 0000000..44e4381 --- /dev/null +++ b/Makefile.am @@ -0,0 +1,20 @@ +AUTOMAKE_OPTIONS = foreign dist-bzip2 1.6 + +AM_CPPFLAGS = $(all_includes) -I$(top_srcdir)/include +SUBDIRS = include src utils + +pkgconfigdir = $(libdir)/pkgconfig +pkgconfig_DATA = libosmo-modbus.pc + +EXTRA_DIST = \ + .version \ + git-version-gen \ + $(NULL) + +@RELMAKE@ + +BUILT_SOURCES = $(top_srcdir)/.version +$(top_srcdir)/.version: + echo $(VERSION) > $@-t && mv $@-t $@ +dist-hook: + echo $(VERSION) > $(distdir)/.tarball-version diff --git a/README.md b/README.md new file mode 100644 index 0000000..b5cb268 --- /dev/null +++ b/README.md @@ -0,0 +1,22 @@ +libosmo-modbus - Osmocom Modbus interface library +================================================= + +This repository contains a C-Language library providing an implementation and interface to manage a Modbus node in a Modbus bus. + +This library relies heavily on [libosmocore](https://osmocom.org/) library and it is aimed at being used by applications using that same library. + +The Modbus specs can be found here: https://www.modbus.org/specs.php + +Currently supported features include: +* Master and Slave roles +* RTU backend + +TODO: +* Implement ASCII backend +* Implement TCP backend +* Implement missing unicast messages/responses +* Implement sending exceptions (both to protocol peer and to the upper layer) +* Implement broadcast messages +* Add a sniffer util to sniff traffic and store it in a pcap file using libpcap +* Add a register storage using a rb_tree? +* Add unit tests diff --git a/configure.ac b/configure.ac new file mode 100644 index 0000000..6e13939 --- /dev/null +++ b/configure.ac @@ -0,0 +1,94 @@ +dnl Process this file with autoconf to produce a configure script +AC_INIT([osmo-modbus], + m4_esyscmd([./git-version-gen .tarball-version]), + [openbsc@lists.osmocom.org]) + +dnl *This* is the root dir, even if an install-sh exists in ../ or ../../ +AC_CONFIG_AUX_DIR([.]) + +AM_INIT_AUTOMAKE([dist-bzip2]) +AC_CONFIG_TESTDIR(tests) + +dnl kernel style compile messages +m4_ifdef([AM_SILENT_RULES], [AM_SILENT_RULES([yes])]) + +dnl include release helper +RELMAKE='-include osmo-release.mk' +AC_SUBST([RELMAKE]) + +dnl checks for programs +AC_PROG_MAKE_SET +AC_PROG_CC +AC_PROG_INSTALL +LT_INIT + +dnl patching ${archive_cmds} to affect generation of file "libtool" to fix linking with clang +AS_CASE(["$LD"],[*clang*], + [AS_CASE(["${host_os}"], + [*linux*],[archive_cmds='$CC -shared $pic_flag $libobjs $deplibs $compiler_flags $wl-soname $wl$soname -o $lib'])]) + +dnl check for pkg-config (explained in detail in libosmocore/configure.ac) +AC_PATH_PROG(PKG_CONFIG_INSTALLED, pkg-config, no) +if test "x$PKG_CONFIG_INSTALLED" = "xno"; then + AC_MSG_WARN([You need to install pkg-config]) +fi +PKG_PROG_PKG_CONFIG([0.20]) + +PKG_CHECK_MODULES(LIBOSMOCORE, libosmocore >= 1.4.0) +PKG_CHECK_MODULES(LIBOSMOVTY, libosmovty >= 1.4.0) + +AC_ARG_ENABLE(sanitize, + [AS_HELP_STRING( + [--enable-sanitize], + [Compile with address sanitizer enabled], + )], + [sanitize=$enableval], [sanitize="no"]) +if test x"$sanitize" = x"yes" +then + CFLAGS="$CFLAGS -fsanitize=address -fsanitize=undefined" + CPPFLAGS="$CPPFLAGS -fsanitize=address -fsanitize=undefined" +fi + +AC_ARG_ENABLE(werror, + [AS_HELP_STRING( + [--enable-werror], + [Turn all compiler warnings into errors, with exceptions: + a) deprecation (allow upstream to mark deprecation without breaking builds); + b) "#warning" pragmas (allow to remind ourselves of errors without breaking builds) + ] + )], + [werror=$enableval], [werror="no"]) +if test x"$werror" = x"yes" +then + WERROR_FLAGS="-Werror" + WERROR_FLAGS+=" -Wno-error=deprecated -Wno-error=deprecated-declarations" + WERROR_FLAGS+=" -Wno-error=cpp" # "#warning" + CFLAGS="$CFLAGS $WERROR_FLAGS" + CPPFLAGS="$CPPFLAGS $WERROR_FLAGS" +fi + +# The following test is taken from WebKit's webkit.m4 +saved_CFLAGS="$CFLAGS" +CFLAGS="$CFLAGS -fvisibility=hidden " +AC_MSG_CHECKING([if ${CC} supports -fvisibility=hidden]) +AC_COMPILE_IFELSE([AC_LANG_SOURCE([char foo;])], + [ AC_MSG_RESULT([yes]) + SYMBOL_VISIBILITY="-fvisibility=hidden"], + AC_MSG_RESULT([no])) +CFLAGS="$saved_CFLAGS" +AC_SUBST(SYMBOL_VISIBILITY) + +CFLAGS="$CFLAGS -Wall" +CPPFLAGS="$CPPFLAGS -Wall" + +AC_MSG_RESULT([CFLAGS="$CFLAGS"]) +AC_MSG_RESULT([CPPFLAGS="$CPPFLAGS"]) + +AC_OUTPUT( + libosmo-modbus.pc + include/osmocom/Makefile + include/osmocom/modbus/Makefile + include/Makefile + src/Makefile + utils/Makefile + Makefile) diff --git a/git-version-gen b/git-version-gen new file mode 100755 index 0000000..42cf3d2 --- /dev/null +++ b/git-version-gen @@ -0,0 +1,151 @@ +#!/bin/sh +# Print a version string. +scriptversion=2010-01-28.01 + +# Copyright (C) 2007-2010 Free Software Foundation, Inc. +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +# This script is derived from GIT-VERSION-GEN from GIT: http://git.or.cz/. +# It may be run two ways: +# - from a git repository in which the "git describe" command below +# produces useful output (thus requiring at least one signed tag) +# - from a non-git-repo directory containing a .tarball-version file, which +# presumes this script is invoked like "./git-version-gen .tarball-version". + +# In order to use intra-version strings in your project, you will need two +# separate generated version string files: +# +# .tarball-version - present only in a distribution tarball, and not in +# a checked-out repository. Created with contents that were learned at +# the last time autoconf was run, and used by git-version-gen. Must not +# be present in either $(srcdir) or $(builddir) for git-version-gen to +# give accurate answers during normal development with a checked out tree, +# but must be present in a tarball when there is no version control system. +# Therefore, it cannot be used in any dependencies. GNUmakefile has +# hooks to force a reconfigure at distribution time to get the value +# correct, without penalizing normal development with extra reconfigures. +# +# .version - present in a checked-out repository and in a distribution +# tarball. Usable in dependencies, particularly for files that don't +# want to depend on config.h but do want to track version changes. +# Delete this file prior to any autoconf run where you want to rebuild +# files to pick up a version string change; and leave it stale to +# minimize rebuild time after unrelated changes to configure sources. +# +# It is probably wise to add these two files to .gitignore, so that you +# don't accidentally commit either generated file. +# +# Use the following line in your configure.ac, so that $(VERSION) will +# automatically be up-to-date each time configure is run (and note that +# since configure.ac no longer includes a version string, Makefile rules +# should not depend on configure.ac for version updates). +# +# AC_INIT([GNU project], +# m4_esyscmd([build-aux/git-version-gen .tarball-version]), +# [bug-project@example]) +# +# Then use the following lines in your Makefile.am, so that .version +# will be present for dependencies, and so that .tarball-version will +# exist in distribution tarballs. +# +# BUILT_SOURCES = $(top_srcdir)/.version +# $(top_srcdir)/.version: +# echo $(VERSION) > $@-t && mv $@-t $@ +# dist-hook: +# echo $(VERSION) > $(distdir)/.tarball-version + +case $# in + 1) ;; + *) echo 1>&2 "Usage: $0 \$srcdir/.tarball-version"; exit 1;; +esac + +tarball_version_file=$1 +nl=' +' + +# First see if there is a tarball-only version file. +# then try "git describe", then default. +if test -f $tarball_version_file +then + v=`cat $tarball_version_file` || exit 1 + case $v in + *$nl*) v= ;; # reject multi-line output + [0-9]*) ;; + *) v= ;; + esac + test -z "$v" \ + && echo "$0: WARNING: $tarball_version_file seems to be damaged" 1>&2 +fi + +if test -n "$v" +then + : # use $v +elif + v=`git describe --abbrev=4 --match='v*' HEAD 2>/dev/null \ + || git describe --abbrev=4 HEAD 2>/dev/null` \ + && case $v in + [0-9]*) ;; + v[0-9]*) ;; + *) (exit 1) ;; + esac +then + # Is this a new git that lists number of commits since the last + # tag or the previous older version that did not? + # Newer: v6.10-77-g0f8faeb + # Older: v6.10-g0f8faeb + case $v in + *-*-*) : git describe is okay three part flavor ;; + *-*) + : git describe is older two part flavor + # Recreate the number of commits and rewrite such that the + # result is the same as if we were using the newer version + # of git describe. + vtag=`echo "$v" | sed 's/-.*//'` + numcommits=`git rev-list "$vtag"..HEAD | wc -l` + v=`echo "$v" | sed "s/\(.*\)-\(.*\)/\1-$numcommits-\2/"`; + ;; + esac + + # Change the first '-' to a '.', so version-comparing tools work properly. + # Remove the "g" in git describe's output string, to save a byte. + v=`echo "$v" | sed 's/-/./;s/\(.*\)-g/\1-/'`; +else + v=UNKNOWN +fi + +v=`echo "$v" |sed 's/^v//'` + +# Don't declare a version "dirty" merely because a time stamp has changed. +git status > /dev/null 2>&1 + +dirty=`sh -c 'git diff-index --name-only HEAD' 2>/dev/null` || dirty= +case "$dirty" in + '') ;; + *) # Append the suffix only if there isn't one already. + case $v in + *-dirty) ;; + *) v="$v-dirty" ;; + esac ;; +esac + +# Omit the trailing newline, so that m4_esyscmd can use the result directly. +echo "$v" | tr -d '\012' + +# Local variables: +# eval: (add-hook 'write-file-hooks 'time-stamp) +# time-stamp-start: "scriptversion=" +# time-stamp-format: "%:y-%02m-%02d.%02H" +# time-stamp-end: "$" +# End: diff --git a/include/Makefile.am b/include/Makefile.am new file mode 100644 index 0000000..3578a80 --- /dev/null +++ b/include/Makefile.am @@ -0,0 +1 @@ +SUBDIRS = osmocom diff --git a/include/osmocom/Makefile.am b/include/osmocom/Makefile.am new file mode 100644 index 0000000..5d00c86 --- /dev/null +++ b/include/osmocom/Makefile.am @@ -0,0 +1 @@ +SUBDIRS = modbus diff --git a/include/osmocom/modbus/Makefile.am b/include/osmocom/modbus/Makefile.am new file mode 100644 index 0000000..2bfd051 --- /dev/null +++ b/include/osmocom/modbus/Makefile.am @@ -0,0 +1,8 @@ +modbus_HEADERS = \ + modbus.h \ + modbus_conn.h \ + modbus_prim.h \ + modbus_rtu.h \ + $(NULL) + +modbusdir = $(includedir)/osmocom/modbus diff --git a/include/osmocom/modbus/modbus.h b/include/osmocom/modbus/modbus.h new file mode 100644 index 0000000..1e11325 --- /dev/null +++ b/include/osmocom/modbus/modbus.h @@ -0,0 +1,40 @@ +/*! \file modbus.h + * Osmocom modbus */ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#pragma once + +#include + +#include +#include +#include + +extern int DLMODBUS; +extern int DLMODBUS_RTU; +/* Overwrite with whatever number is wanted by the APP */ +unsigned int osmo_modbus_set_logging_category_offset(int offset); + +enum osmo_modbus_function_code { + OSMO_MODBUS_FUNC_READ_MULT_HOLD_REG = 0x03, +}; diff --git a/include/osmocom/modbus/modbus_conn.h b/include/osmocom/modbus/modbus_conn.h new file mode 100644 index 0000000..63cf2d9 --- /dev/null +++ b/include/osmocom/modbus/modbus_conn.h @@ -0,0 +1,69 @@ +/*! \file modbus_conn.h + * Osmocom modbus */ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#pragma once + +#include +#include + +struct osmo_modbus_conn_rtu; + +enum osmo_modbus_proto_type { + OSMO_MODBUS_PROTO_RTU, +}; + +enum osmo_modbus_conn_role { + OSMO_MODBUS_ROLE_MASTER, + OSMO_MODBUS_ROLE_SLAVE, +}; + +enum osmo_modbus_conn_timeout { + OSMO_MODBUS_TO_TURNAROUND = 1, + OSMO_MODBUS_TO_NORESPONSE = 2, +}; + +struct osmo_modbus_conn; +typedef int (*osmo_modbus_prim_cb)(struct osmo_modbus_conn *conn, struct osmo_modbus_prim *prim, void *ctx); + +struct osmo_modbus_conn* osmo_modbus_conn_alloc(void *tall_ctx, + enum osmo_modbus_conn_role role, + enum osmo_modbus_proto_type type); +void osmo_modbus_conn_free(struct osmo_modbus_conn* conn); + +int osmo_modbus_conn_connect(struct osmo_modbus_conn* conn); +bool osmo_modbus_conn_is_connected(struct osmo_modbus_conn* conn); +int osmo_modbus_conn_set_timeout(struct osmo_modbus_conn* conn, + enum osmo_modbus_conn_timeout to_type, + unsigned long val); +unsigned long osmo_modbus_conn_get_timeout(const struct osmo_modbus_conn* conn, + enum osmo_modbus_conn_timeout to_type); +int osmo_modbus_conn_set_address(struct osmo_modbus_conn* conn, uint16_t address); +uint16_t osmo_modbus_conn_get_address(const struct osmo_modbus_conn* conn); +void osmo_modbus_conn_set_prim_cb(struct osmo_modbus_conn* conn, + osmo_modbus_prim_cb prim_cb, void *ctx); +int osmo_modbus_conn_submit_prim(struct osmo_modbus_conn* conn, + struct osmo_modbus_prim *prim); +int osmo_modbus_conn_set_monitor_mode(struct osmo_modbus_conn* conn, bool enable); + +struct osmo_modbus_conn_rtu *osmo_modbus_conn_get_rtu(struct osmo_modbus_conn *conn); diff --git a/include/osmocom/modbus/modbus_prim.h b/include/osmocom/modbus/modbus_prim.h new file mode 100644 index 0000000..8d20f8e --- /dev/null +++ b/include/osmocom/modbus/modbus_prim.h @@ -0,0 +1,60 @@ +/*! \file modbus_prim.h + * Osmocom modbus primitives */ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#pragma once + +#include +/*! \brief Modbus primitives */ +enum osmo_modbus_prim_type { + OSMO_MODBUS_PRIM_RESPONSE_TIMEOUT, + OSMO_MODBUS_PRIM_N_MULT_HOLD_REG, +}; +extern const struct value_string osmo_modbus_prim_type_names[]; + +/* OSMO_MODBUS_PRIM_N_MULT_HOLD_REG */ +struct osmo_modbus_read_mult_hold_reg_req_param { + uint16_t first_reg; + uint16_t num_reg; + /* user data */ +}; + +/* OSMO_MODBUS_PRIM_N_MULT_HOLD_RESP */ +struct osmo_modbus_read_mult_hold_reg_resp_param { + uint16_t num_reg; + uint16_t registers[125]; + /* user data */ +}; + +struct osmo_modbus_prim { + struct osmo_prim_hdr oph; + uint16_t address; + union { + struct osmo_modbus_read_mult_hold_reg_req_param read_mult_hold_reg_req; + struct osmo_modbus_read_mult_hold_reg_resp_param read_mult_hold_reg_resp; + } u; +}; + +struct osmo_modbus_prim *osmo_modbus_makeprim_timeout_resp(uint16_t address); +struct osmo_modbus_prim *osmo_modbus_makeprim_mult_hold_reg_req(uint16_t address, uint16_t first_reg, uint16_t num_reg); +struct osmo_modbus_prim *osmo_modbus_makeprim_mult_hold_reg_resp(uint16_t address, uint8_t num_reg, uint16_t *registers); diff --git a/include/osmocom/modbus/modbus_rtu.h b/include/osmocom/modbus/modbus_rtu.h new file mode 100644 index 0000000..1c148cd --- /dev/null +++ b/include/osmocom/modbus/modbus_rtu.h @@ -0,0 +1,40 @@ +/*! \file modbus_rtu.h + * Osmocom modbus */ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#pragma once + +#include +#include +#include + +#include + +struct osmo_modbus_conn_rtu; + +struct osmo_modbus_conn_rtu* osmo_modbus_conn_rtu_alloc(struct osmo_modbus_conn* conn); + +int osmo_modbus_conn_rtu_set_device(struct osmo_modbus_conn_rtu* rtu, const char* serial_dev); +const char *osmo_modbus_conn_rtu_get_device(const struct osmo_modbus_conn_rtu* rtu); +int osmo_modbus_conn_rtu_set_baudrate(struct osmo_modbus_conn_rtu* rtu, unsigned baudrate); +unsigned osmo_modbus_conn_rtu_get_baudrate(const struct osmo_modbus_conn_rtu* rtu); diff --git a/libosmo-modbus.pc.in b/libosmo-modbus.pc.in new file mode 100644 index 0000000..2c66392 --- /dev/null +++ b/libosmo-modbus.pc.in @@ -0,0 +1,10 @@ +prefix=@prefix@ +exec_prefix=@exec_prefix@ +libdir=@libdir@ +includedir=@includedir@ + +Name: OsmoModbus Lib +Description: OsmoModbus Lib +Version: @VERSION@ +Libs: -L${libdir} -losmo-modbus +Cflags: -I${includedir}/ diff --git a/src/Makefile.am b/src/Makefile.am new file mode 100644 index 0000000..d9ff5f8 --- /dev/null +++ b/src/Makefile.am @@ -0,0 +1,28 @@ +AM_CPPFLAGS = $(all_includes) -I$(top_srcdir)/include -I$(top_builddir) +AM_CFLAGS=-Wall $(LIBOSMOCORE_CFLAGS) + +noinst_HEADERS = \ + modbus_internal.h \ + conn_fsm.h \ + rtu_transmit_fsm.h \ + rtu_internal.h \ + $(NULL) + +lib_LTLIBRARIES = libosmo-modbus.la + +# This is _NOT_ the library release version, it's an API version. +# Please read Chapter 6 "Library interface versions" of the libtool +# documentation before making any modification +LIBVERSION=0:0:0 + +libosmo_modbus_la_SOURCES = \ + conn.c \ + conn_master_fsm.c \ + conn_slave_fsm.c \ + conn_rtu.c \ + rtu_transmit_fsm.c \ + prim.c \ + $(NULL) + +libosmo_modbus_la_LDFLAGS = -version-info $(LIBVERSION) -no-undefined -export-symbols-regex '^(osmo_|DLMODBUS)' +libosmo_modbus_la_LIBADD = $(LIBOSMOCORE_LIBS) diff --git a/src/conn.c b/src/conn.c new file mode 100644 index 0000000..fbc50a8 --- /dev/null +++ b/src/conn.c @@ -0,0 +1,228 @@ +/*! \file conn.c + * modbus connection */ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "modbus_internal.h" +#include "conn_fsm.h" + +#define LOGPCONN(conn, subsys, level, fmt, args ...) \ + LOGP(subsys, level, "(addr=%u) " fmt, (conn)->address, ## args) + +int DLMODBUS = DLMODBUS_OFFSET; +int DLMODBUS_RTU = DLMODBUS_RTU_OFFSET; +unsigned int osmo_modbus_set_logging_category_offset(int offset) +{ + DLMODBUS = offset + DLMODBUS_OFFSET; + DLMODBUS_RTU = offset + DLMODBUS_RTU_OFFSET; + return 2; +} + +struct osmo_tdef g_conn_tdefs[] = { + { .T=OSMO_MODBUS_TO_TURNAROUND, .default_val=100, .unit = OSMO_TDEF_MS, .desc="Turnaround Delay Expiration Timeout" }, + { .T=OSMO_MODBUS_TO_NORESPONSE, .default_val=200, .unit = OSMO_TDEF_MS, .desc="Response Timeout" }, + {} +}; + +static void update_fi_name(struct osmo_modbus_conn* conn) +{ + if (conn->role == OSMO_MODBUS_ROLE_SLAVE) { + osmo_fsm_inst_update_id_f_sanitize(conn->fi, '-', "addr-%" PRIu16, + conn->address); + } +} + +struct osmo_modbus_conn* osmo_modbus_conn_alloc(void *tall_ctx, + enum osmo_modbus_conn_role role, + enum osmo_modbus_proto_type type) +{ + struct osmo_modbus_conn* conn = talloc_zero(tall_ctx, struct osmo_modbus_conn); + conn->role = role; + conn->proto_type = type; + INIT_LLIST_HEAD(&conn->msg_queue); + + switch (type) { + case OSMO_MODBUS_PROTO_RTU: + conn->proto = (void *)osmo_modbus_conn_rtu_alloc(conn); + break; + default: + goto err; + } + + if (!conn->proto) + goto err; + + conn->T_defs = talloc_zero_size(conn, sizeof(g_conn_tdefs)); + memcpy(conn->T_defs, g_conn_tdefs, sizeof(g_conn_tdefs)); + osmo_tdefs_reset(conn->T_defs); + + if (conn->role == OSMO_MODBUS_ROLE_MASTER) { + conn->address = 0x00; + conn_master_fsm.log_subsys = DLMODBUS; /* Update after app set the correct value */ + conn->fi = osmo_fsm_inst_alloc(&conn_master_fsm, conn, conn, LOGL_INFO, NULL); + } else { + conn->address = 0x01; + conn_master_fsm.log_subsys = DLMODBUS; /* Update after app set the correct value */ + conn->fi = osmo_fsm_inst_alloc(&conn_slave_fsm, conn, conn, LOGL_INFO, NULL); + osmo_fsm_inst_update_id_f_sanitize(conn->fi, '-', "addr-%" PRIu16, + conn->address); + } + + return conn; +err: + talloc_free(conn); + return NULL; +} + +void osmo_modbus_conn_free(struct osmo_modbus_conn* conn) +{ + if (conn->proto_ops.free) + conn->proto_ops.free(conn); + conn->proto = NULL; + + osmo_fsm_inst_free(conn->fi); + conn->fi = NULL; + + while (!llist_empty(&conn->msg_queue)) { + struct msgb *msg = msgb_dequeue(&conn->msg_queue); + msgb_free(msg); + } + + talloc_free(conn); +} + +int osmo_modbus_conn_connect(struct osmo_modbus_conn* conn) +{ + LOGPCONN(conn, DLMODBUS, LOGL_INFO, "Connecting...\n"); + bool connected = false; + int rc; + + rc = osmo_fsm_inst_dispatch(conn->fi, CONN_EV_CONNECT, &connected); + if (rc) + return rc; + return connected ? 0 : -ENOTCONN; +} + +bool osmo_modbus_conn_is_connected(struct osmo_modbus_conn* conn) +{ + LOGPCONN(conn, DLMODBUS, LOGL_INFO, "Connecting...\n"); + if ((conn->role == OSMO_MODBUS_ROLE_MASTER && conn->fi->state == CONN_MASTER_ST_DISCONNECTED) || + (conn->role == OSMO_MODBUS_ROLE_SLAVE && conn->fi->state == CONN_SLAVE_ST_DISCONNECTED)) + return false; + return conn->proto_ops.is_connected(conn); +} + +int osmo_modbus_conn_set_address(struct osmo_modbus_conn* conn, uint16_t address) +{ + /*TODO: for RTU it's only 1 byte, check that */ + conn->address = address; + update_fi_name(conn); + return 0; +} + +uint16_t osmo_modbus_conn_get_address(const struct osmo_modbus_conn* conn) +{ + return conn->address; +} + +int osmo_modbus_conn_set_timeout(struct osmo_modbus_conn* conn, + enum osmo_modbus_conn_timeout to_type, + unsigned long val) +{ + return osmo_tdef_set(conn->T_defs, to_type, val, OSMO_TDEF_MS); +} + +unsigned long osmo_modbus_conn_get_timeout(const struct osmo_modbus_conn* conn, + enum osmo_modbus_conn_timeout to_type) +{ + return osmo_tdef_get(conn->T_defs, to_type, OSMO_TDEF_MS, (unsigned long)-1); +} + +void osmo_modbus_conn_set_prim_cb(struct osmo_modbus_conn* conn, + osmo_modbus_prim_cb prim_cb, void *ctx) +{ + conn->prim_cb = prim_cb; + conn->prim_cb_ctx = ctx; +} + +int osmo_modbus_conn_submit_prim(struct osmo_modbus_conn* conn, + struct osmo_modbus_prim *prim) +{ + int rc; + + LOGPCONN(conn, DLMODBUS, LOGL_INFO, "Submitting prim operation '%s' on primitive '%s'\n", + get_value_string(osmo_prim_op_names, prim->oph.operation), + get_value_string(osmo_modbus_prim_type_names, prim->oph.primitive)); + if ((conn->role == OSMO_MODBUS_ROLE_MASTER && prim->oph.operation != PRIM_OP_REQUEST) || + (conn->role == OSMO_MODBUS_ROLE_SLAVE && prim->oph.operation != PRIM_OP_RESPONSE)) { + LOGPCONN(conn, DLMODBUS, LOGL_INFO, "Primitive %s not possible in role %d\n", + get_value_string(osmo_prim_op_names, prim->oph.operation), conn->role); + msgb_free(prim->oph.msg); + return -EINVAL; + } + + msgb_enqueue(&conn->msg_queue, prim->oph.msg); + rc = osmo_fsm_inst_dispatch(conn->fi, CONN_EV_SUBMIT_PRIM, NULL); + return rc; +} + +int osmo_modbus_conn_set_monitor_mode(struct osmo_modbus_conn* conn, bool enable) +{ + if (conn->role == OSMO_MODBUS_ROLE_MASTER) + return -EINVAL; + conn->slave.monitor = enable; + return 0; +} + +struct osmo_modbus_conn_rtu *osmo_modbus_conn_get_rtu(struct osmo_modbus_conn *conn) +{ + switch (conn->proto_type) { + case OSMO_MODBUS_PROTO_RTU: + return (struct osmo_modbus_conn_rtu *)conn->proto; + default: + OSMO_ASSERT(0); + } +} + +void osmo_modbus_conn_rx_prim(struct osmo_modbus_conn* conn, struct osmo_modbus_prim *prim) +{ + int rc; + LOGPCONN(conn, DLMODBUS, LOGL_INFO, "Received primitive operation '%s' on primitive '%s' on addr %" PRIu16 "\n", + get_value_string(osmo_prim_op_names, prim->oph.operation), + get_value_string(osmo_modbus_prim_type_names, prim->oph.primitive), + prim->address); + rc = osmo_fsm_inst_dispatch(conn->fi, CONN_EV_RECV_PRIM, prim); + if (rc) { + msgb_free(prim->oph.msg); + } +} diff --git a/src/conn_fsm.h b/src/conn_fsm.h new file mode 100644 index 0000000..45177df --- /dev/null +++ b/src/conn_fsm.h @@ -0,0 +1,26 @@ +/* Figure 7: Master state diagram */ +#pragma once + +enum conn_master_state { + CONN_MASTER_ST_DISCONNECTED, + CONN_MASTER_ST_IDLE, + CONN_MASTER_ST_WAIT_TURNAROUND_DELAY, + CONN_MASTER_ST_WAIT_REPLY, + //CONN_MASTER_STPROCESSING_REPLY, +}; + +enum conn_slave_state { + CONN_SLAVE_ST_DISCONNECTED, + CONN_SLAVE_ST_IDLE, + CONN_SLAVE_ST_CHECK_REQUEST, +}; + +enum conn_event { + CONN_EV_CONNECT, + CONN_EV_SUBMIT_PRIM, + CONN_EV_RECV_PRIM, + _NUM_CONN_EV, +}; + +extern struct osmo_fsm conn_master_fsm; +extern struct osmo_fsm conn_slave_fsm; diff --git a/src/conn_master_fsm.c b/src/conn_master_fsm.c new file mode 100644 index 0000000..a66a1c9 --- /dev/null +++ b/src/conn_master_fsm.c @@ -0,0 +1,233 @@ +/*! \file conn_master_fsm.c + * FSM for "Figure 14: RTU transmission mode state diagram" */ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "modbus_internal.h" +#include "conn_fsm.h" + +#define X(x) (1 << (x)) + +static const struct value_string conn_master_event_names[] = { + { CONN_EV_CONNECT, "Connect" }, + { CONN_EV_SUBMIT_PRIM, "SubmitPrim" }, + { CONN_EV_RECV_PRIM, "RxPrim" }, + { 0, NULL } +}; + +static const struct osmo_tdef_state_timeout conn_master_fsm_timeouts[32] = { + [CONN_MASTER_ST_DISCONNECTED] = {}, + [CONN_MASTER_ST_IDLE] = {}, + [CONN_MASTER_ST_WAIT_TURNAROUND_DELAY] = { .T = OSMO_MODBUS_TO_TURNAROUND }, + [CONN_MASTER_ST_WAIT_REPLY] = { .T = OSMO_MODBUS_TO_NORESPONSE }, +}; + +/* Transition to a state, using the T timer defined in assignment_fsm_timeouts. + * The actual timeout value is in turn obtained from conn->T_defs. + * Assumes local variable fi exists. */ +#define conn_master_fsm_state_chg(fi, state) \ + osmo_tdef_fsm_inst_state_chg(fi, state, \ + conn_master_fsm_timeouts, \ + ((struct osmo_modbus_conn*)(fi->priv))->T_defs, \ + -1) + +static void conn_master_fsm_st_disconnected_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ +} + +static void conn_master_fsm_st_disconnected(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_modbus_conn *conn = (struct osmo_modbus_conn*)fi->priv; + bool *connected; + int rc; + + switch (event) { + case CONN_EV_CONNECT: + connected = (bool*)data; + if ((rc = conn->proto_ops.connect(conn)) == 0) { + *connected = true; + conn_master_fsm_state_chg(fi, CONN_MASTER_ST_IDLE); + } else { + *connected = false; + } + break; + case CONN_EV_SUBMIT_PRIM: + /* Do nothing, conn enqueued the message */ + break; + default: + OSMO_ASSERT(0); + } +} + +static void conn_master_fsm_st_idle_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct osmo_modbus_conn *conn = (struct osmo_modbus_conn*)fi->priv; + /* TODO: once we support broadcast messages, check msg and do that transition */ + if (!llist_empty(&conn->msg_queue)) + conn_master_fsm_state_chg(fi, CONN_MASTER_ST_WAIT_REPLY); + +} + +static void conn_master_fsm_st_idle(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + //struct osmo_modbus_conn *conn = (struct osmo_modbus_conn*)fi->priv; + switch (event) { + case CONN_EV_SUBMIT_PRIM: + /* TODO: once we support broadcast messages, check msg and do that transition */ + conn_master_fsm_state_chg(fi, CONN_MASTER_ST_WAIT_REPLY); + break; + default: + OSMO_ASSERT(0); + } +} + +static void conn_master_fsm_st_wait_turnaround_delay_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + /* TODO: implement */ +} + +static void conn_master_fsm_st_wait_turnaround_delay(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + /* TODO: implement */ +} + +static void conn_master_fsm_st_wait_reply_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct osmo_modbus_conn *conn = (struct osmo_modbus_conn*)fi->priv; + struct msgb *msg; + struct osmo_modbus_prim *prim; + + if (llist_empty(&conn->msg_queue)) { + LOGPFSML(fi, LOGL_INFO, "Write queue is empty!\n"); + OSMO_ASSERT(0); + } + + msg = msgb_dequeue(&conn->msg_queue); + prim = (struct osmo_modbus_prim *)msgb_data(msg); + conn->master.req_for_addr = prim->address; + conn->proto_ops.tx_prim(conn, prim); + msgb_free(msg); +} + +static void conn_master_fsm_st_wait_reply(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_modbus_conn *conn = (struct osmo_modbus_conn*)fi->priv; + struct osmo_modbus_prim *prim; + + switch (event) { + case CONN_EV_SUBMIT_PRIM: + /* Do nothing, conn enqueued the message */ + break; + case CONN_EV_RECV_PRIM: + prim = (struct osmo_modbus_prim *)data; + /* TODO: check if addr is for us... */ + /* TODO: check if msg received is a reply for our last request... */ + if (conn->prim_cb) + conn->prim_cb(conn, prim, conn->prim_cb_ctx); + else + msgb_free(prim->oph.msg); + conn_master_fsm_state_chg(fi, CONN_MASTER_ST_IDLE); + break; + default: + OSMO_ASSERT(0); + } +} + +static const struct osmo_fsm_state conn_master_states[] = { + [CONN_MASTER_ST_DISCONNECTED]= { + .in_event_mask = X(CONN_EV_CONNECT) | + X(CONN_EV_SUBMIT_PRIM), + .out_state_mask = X(CONN_MASTER_ST_IDLE), + .name = "DISCONNECTED", + .action = conn_master_fsm_st_disconnected, + .onenter = conn_master_fsm_st_disconnected_onenter, + }, + [CONN_MASTER_ST_IDLE] = { + .in_event_mask = X(CONN_EV_SUBMIT_PRIM), + .out_state_mask = X(CONN_MASTER_ST_WAIT_TURNAROUND_DELAY) | + X(CONN_MASTER_ST_WAIT_REPLY), + .name = "IDLE", + .action = conn_master_fsm_st_idle, + .onenter = conn_master_fsm_st_idle_onenter, + }, + [CONN_MASTER_ST_WAIT_TURNAROUND_DELAY] = { + .in_event_mask = X(CONN_EV_SUBMIT_PRIM), + .out_state_mask = X(CONN_MASTER_ST_IDLE), + .name = "WAIT_TURNAROUND_DELAY", + .action = conn_master_fsm_st_wait_turnaround_delay, + .onenter = conn_master_fsm_st_wait_turnaround_delay_onenter, + }, + [CONN_MASTER_ST_WAIT_REPLY] = { + .in_event_mask = X(CONN_EV_SUBMIT_PRIM) | + X(CONN_EV_RECV_PRIM), + .out_state_mask = X(CONN_MASTER_ST_IDLE), + .name = "WAIT_REPLY", + .action = conn_master_fsm_st_wait_reply, + .onenter = conn_master_fsm_st_wait_reply_onenter, + }, +}; + +static int conn_master_fsm_timer_cb(struct osmo_fsm_inst *fi) +{ + struct osmo_modbus_conn *conn = (struct osmo_modbus_conn*)fi->priv; + struct osmo_modbus_prim *prim; + + switch (fi->T) { + case OSMO_MODBUS_TO_TURNAROUND: + conn_master_fsm_state_chg(fi, CONN_MASTER_ST_IDLE); + break; + case OSMO_MODBUS_TO_NORESPONSE: + prim = osmo_modbus_makeprim_timeout_resp(conn->master.req_for_addr); + if (conn->prim_cb) + conn->prim_cb(conn, prim, conn->prim_cb_ctx); + else + msgb_free(prim->oph.msg); + conn_master_fsm_state_chg(fi, CONN_MASTER_ST_IDLE); + break; + } + return 0; +} + +struct osmo_fsm conn_master_fsm = { + .name = "conn_master", + .states = conn_master_states, + .num_states = ARRAY_SIZE(conn_master_states), + .timer_cb = conn_master_fsm_timer_cb, + .log_subsys = DLMODBUS_OFFSET, + .event_names = conn_master_event_names, + //.cleanup = conn_master_fsm_cleanup, +}; + +static __attribute__((constructor)) void conn_master_fsm_init(void) +{ + OSMO_ASSERT(osmo_fsm_register(&conn_master_fsm) == 0); +} diff --git a/src/conn_rtu.c b/src/conn_rtu.c new file mode 100644 index 0000000..f2eaeb6 --- /dev/null +++ b/src/conn_rtu.c @@ -0,0 +1,444 @@ +/*! \file conn_rtu.c + * modbus connection RTU specifics */ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +/* https://www.modbus.org/docs/Modbus_over_serial_line_V1_02.pdf */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "modbus_internal.h" +#include "rtu_internal.h" +#include "rtu_transmit_fsm.h" + +#define RTU_DEFAULT_BAUDRATE 9600 + +#define LOGPRTU(rtu, subsys, level, fmt, args ...) \ + LOGP(subsys, level, "(addr=%" PRIu16 ",dev=%s) " fmt, (rtu)->conn->address, (rtu)->dev_path, ## args) + +static struct msgb *modbus_rtu_msgb_alloc(void) +{ + return msgb_alloc(MODBUS_MSGB_SIZE, ""); +} + +struct osmo_tdef g_rtu_tdefs[] = { + { .T=15, .default_val=1, .unit = OSMO_TDEF_US, .desc="Timeout for RTU transmission T1.5, in microseconds" }, + { .T=35, .default_val=1, .unit = OSMO_TDEF_US, .desc="Timeout for RTU transmission T3.5, in microseconds" }, + {} +}; + + +/* High-Order Byte Table */ +static const uint8_t table_crc_hi[] = { + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, + 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, + 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, + 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, + 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 +}; + +/* Low-Order Byte Table */ +static const uint8_t table_crc_lo[] = { + 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, + 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, + 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, + 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, + 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, + 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, + 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, + 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, + 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, + 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, + 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, + 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, + 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, + 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, + 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, + 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, + 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, + 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, + 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, + 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, + 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, + 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, + 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, + 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, + 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, + 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 +}; + +/* CRC Generation Function */ +uint16_t crc16(uint8_t *data, uint16_t data_len) +{ + uint8_t crc_hi = 0xFF; /* Initialized high CRC byte */ + uint8_t crc_lo = 0xFF; /* Initialized low CRC byte */ + unsigned int idx; /* will index into CRC lookup */ + + /* pass through message buffer */ + while (data_len--) { + idx = crc_hi ^ *data++; /* calculate the CRC */ + crc_hi = crc_lo ^ table_crc_hi[idx]; + crc_lo = table_crc_lo[idx]; + } + + return (crc_hi << 8 | crc_lo); +} + +struct msgb* prim2rtu(struct osmo_modbus_prim *prim) +{ + struct msgb *msg = modbus_rtu_msgb_alloc(); + uint8_t *code; + size_t len; + + msgb_put_u8(msg, (uint8_t)prim->address); + code = msgb_put(msg, 1); /* fll later */ + switch (OSMO_PRIM_HDR(&prim->oph)) { + case OSMO_PRIM(OSMO_MODBUS_PRIM_N_MULT_HOLD_REG, PRIM_OP_REQUEST): + *code = OSMO_MODBUS_FUNC_READ_MULT_HOLD_REG; + msgb_put_u16(msg, prim->u.read_mult_hold_reg_req.first_reg); + msgb_put_u16(msg, prim->u.read_mult_hold_reg_req.num_reg); + break; + case OSMO_PRIM(OSMO_MODBUS_PRIM_N_MULT_HOLD_REG, PRIM_OP_RESPONSE): + *code = OSMO_MODBUS_FUNC_READ_MULT_HOLD_REG; + msgb_put_u8(msg, prim->u.read_mult_hold_reg_resp.num_reg * 2); + len = prim->u.read_mult_hold_reg_resp.num_reg * sizeof(uint16_t); + memcpy(msgb_put(msg, len), + prim->u.read_mult_hold_reg_resp.registers, len); + break; + default: + OSMO_ASSERT(0); + } + msgb_put_u16(msg, crc16(msgb_data(msg), msgb_length(msg))); + return msg; +} + +/* Address (1Byte) + Function Code (1Byte) */ +#define RTU_HDR_LEN 2 +#define RTU_CRC_LEN 2 + +/* Returns size used if succeeded, returns -ENODATA if data missing to parse message */ +int rtu2prim(struct osmo_modbus_conn_rtu* rtu, struct msgb* msg, struct osmo_modbus_prim **prim) +{ + uint8_t *data = msgb_data(msg); + size_t len = msgb_length(msg); + uint8_t address; + uint8_t exp_len_nocrc; + uint16_t exp_crc; + uint8_t byte_count; + enum osmo_modbus_function_code code; + + if (len < RTU_HDR_LEN) { + return -ENODATA; + } + + address = data[0]; + code = (enum osmo_modbus_function_code)data[1]; + + switch (code) { + case OSMO_MODBUS_FUNC_READ_MULT_HOLD_REG: + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_INFO, "Received OSMO_MODBUS_FUNC_READ_MULT_HOLD_REG: %s\n", osmo_hexdump(data, len)); + if (len < RTU_HDR_LEN + 4) + return -ENODATA; + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_DEBUG, "Received total %zu bytes: %s\n", len, osmo_hexdump(data, len)); + /* Let's first try to decode Response */ + byte_count = data[RTU_HDR_LEN]; + exp_len_nocrc = RTU_HDR_LEN + 1 + byte_count; + if (len >= (exp_len_nocrc + RTU_CRC_LEN)) { + exp_crc = crc16(data, exp_len_nocrc); + osmo_store16be(exp_crc, &exp_crc); + if (memcmp(&exp_crc, &data[exp_len_nocrc], RTU_CRC_LEN) == 0) { + /* Its a response: */ + uint16_t * registers = (uint16_t*)&data[RTU_HDR_LEN + 1]; /* FIXME: copy to temp buffer to fix misalignment */ + *prim = osmo_modbus_makeprim_mult_hold_reg_resp(address, byte_count/2, registers); + return exp_len_nocrc + RTU_CRC_LEN; + } + } + /* try to decode Request */ + exp_len_nocrc = RTU_HDR_LEN + 2 + 2; + if (len >= exp_len_nocrc + RTU_CRC_LEN) { + exp_crc = crc16(data, exp_len_nocrc); + osmo_store16be(exp_crc, &exp_crc); + if (memcmp(&exp_crc, &data[exp_len_nocrc], RTU_CRC_LEN) == 0) { + /* Its a response: */ + uint16_t first_reg, num_reg; + first_reg = osmo_load16be(&data[RTU_HDR_LEN]); + num_reg = osmo_load16be(&data[RTU_HDR_LEN + 2]); + *prim = osmo_modbus_makeprim_mult_hold_reg_req(address, first_reg, num_reg); + return exp_len_nocrc + RTU_CRC_LEN; + } + } + /* Either CRC error or we miss data... */ + return -ENODATA; + default: + return -EINVAL; + } +} + +int rtu_read(struct osmo_modbus_conn_rtu* rtu) +{ + uint8_t *buf = msgb_data(rtu->rx_msg); + int offset = msgb_length(rtu->rx_msg); + int rc; + + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_DEBUG, "Read cb (buf=%d)\n", msgb_tailroom(rtu->rx_msg)); + rc = read(rtu->ofd.fd, buf + offset, msgb_tailroom(rtu->rx_msg)); + if (rc < 0) { + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_ERROR, "read() failed %d: %s\n", rc, strerror(errno)); + return rc; + } else if (rc == 0) { + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_NOTICE, "read() 0 bytes\n"); + return 0; + } + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_DEBUG, "Received %d bytes: %s\n", rc, osmo_hexdump(buf + offset, rc)); + msgb_put(rtu->rx_msg, rc); + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_DEBUG, "Received total %d bytes: %s\n", rc, osmo_hexdump(buf, msgb_length(rtu->rx_msg))); + osmo_fsm_inst_dispatch(rtu->fi, RTU_TRANSMIT_EV_CHAR_RECEIVED, NULL); + return 0; +} + +int rtu_write(struct osmo_modbus_conn_rtu* rtu) +{ + struct msgb *msg; + int rc; + + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_DEBUG, "Write cb!\n"); + + rtu->ofd.when &= ~OSMO_FD_WRITE; + + if (!rtu->tx_msg) { + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_NOTICE, "Write cb but no Tx Msg!\n"); + return 0; + } + msg = rtu->tx_msg; + rtu->tx_msg = NULL; + + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_INFO, "Writing: %s\n", msgb_hexdump(msg)); + rc = write(rtu->ofd.fd, msgb_data(msg), msgb_length(msg)); + if (rc < 0) { + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_ERROR, "write() failed %d: %s\n", rc, strerror(errno)); + } else if (rc != msgb_length(msg)) { + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_ERROR, "Wrote only %d / %d bytes!\n", rc, msgb_length(msg)); + } + msgb_free(msg); + return 0; +} + +static int rtu_ofd_cb(struct osmo_fd *ofd, unsigned int flags) +{ + struct osmo_modbus_conn_rtu *rtu = (struct osmo_modbus_conn_rtu*)ofd->data; + int rc = 0; + + if (flags & OSMO_FD_READ) { + rc = rtu_read(rtu); + if (rc == -EBADF) + goto err_badfd; + } + + /* FIXME: what to do here? Move to disconnected state?*/ + if (flags & OSMO_FD_EXCEPT) { + /* FIXME: what to do here? Move to disconnected state?*/ + /* if (rc == -EBADF) + goto err_badfd; */ + } + + if (flags & OSMO_FD_WRITE) { + rc = rtu_write(rtu); + if (rc == -EBADF) + goto err_badfd; + } + +err_badfd: + return rc; +} + +static int osmo_modbus_conn_rtu_connect(struct osmo_modbus_conn* conn) +{ + struct osmo_modbus_conn_rtu* rtu = (struct osmo_modbus_conn_rtu*) conn->proto; + speed_t speed; + int fd; + int flags; + + if (!rtu->dev_path || rtu->dev_path[0] == '\0') + return -EINVAL; + + if (osmo_serial_speed_t(rtu->baudrate, &speed) < 0) { + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_ERROR, "Failed to get speed_t from baudrate\n"); + return -EINVAL; + } + + fd = osmo_serial_init(rtu->dev_path, speed); + if (fd < 0) + return fd; + + osmo_fd_setup(&rtu->ofd, fd, OSMO_FD_READ, rtu_ofd_cb, rtu, 0); + if (osmo_fd_register(&rtu->ofd) != 0) { + LOGPRTU(rtu, DLMODBUS_RTU, LOGL_ERROR, "Failed to register the serial\n"); + return -EINVAL; + } + + /* Set serial socket to non-blocking mode of operation */ + flags = fcntl(rtu->ofd.fd, F_GETFL); + flags |= O_NONBLOCK; + fcntl(rtu->ofd.fd, F_SETFL, flags); + + return osmo_fsm_inst_dispatch(rtu->fi, RTU_TRANSMIT_EV_START, NULL); +} + +static bool osmo_modbus_conn_rtu_is_connected(struct osmo_modbus_conn* conn) +{ + struct osmo_modbus_conn_rtu* rtu = (struct osmo_modbus_conn_rtu*) conn->proto; + return rtu->ofd.fd >= 0; +} + +static int osmo_modbus_conn_rtu_tx_prim(struct osmo_modbus_conn* conn, struct osmo_modbus_prim *prim) +{ + struct osmo_modbus_conn_rtu* rtu = (struct osmo_modbus_conn_rtu*) conn->proto; + + OSMO_ASSERT(!rtu->tx_msg); + rtu->tx_msg = prim2rtu(prim); + + return osmo_fsm_inst_dispatch(rtu->fi, RTU_TRANSMIT_EV_DEMAND_OF_EMISSION, NULL); +} + +static void osmo_modbus_conn_rtu_free(struct osmo_modbus_conn* conn) +{ + struct osmo_modbus_conn_rtu* rtu = (struct osmo_modbus_conn_rtu*) conn->proto; + + osmo_fsm_inst_free(rtu->fi); + rtu->fi = NULL; + + if (rtu->ofd.fd >= 0) { + osmo_fd_unregister(&rtu->ofd); + close(rtu->ofd.fd); + rtu->ofd.fd = -1; + } + + msgb_free(rtu->tx_msg); + + talloc_free(rtu); +} + +static void recalc_baudrate_timers(struct osmo_modbus_conn_rtu* rtu) +{ + if (rtu->baudrate <= 19200) { + osmo_tdef_set(rtu->T_defs, 15, rtu_chars2bits(1500000) / rtu->baudrate, OSMO_TDEF_US); + osmo_tdef_set(rtu->T_defs, 35, rtu_chars2bits(3500000) / rtu->baudrate, OSMO_TDEF_US); + } else { + /* 2.5.1.1 MODBUS Message RTU Framing: Fixed values used for higher baudrates */ + osmo_tdef_set(rtu->T_defs, 15, 750, OSMO_TDEF_US); + osmo_tdef_set(rtu->T_defs, 35, 1750, OSMO_TDEF_US); + } +} + +static void update_fi_name(struct osmo_modbus_conn_rtu* rtu) +{ + osmo_fsm_inst_update_id_f_sanitize(rtu->fi, '-', "%s_%" PRIu16, + rtu->dev_path ? : "unknown", + rtu->conn->address); +} + +struct osmo_modbus_conn_rtu* osmo_modbus_conn_rtu_alloc(struct osmo_modbus_conn* conn) +{ + struct osmo_modbus_conn_rtu* rtu = talloc_zero(conn, struct osmo_modbus_conn_rtu); + rtu->conn = conn; + rtu->baudrate = 9600; + rtu->ofd.fd = -1; + rtu->rx_msg = modbus_rtu_msgb_alloc(); + rtu->T_defs = talloc_zero_size(rtu, sizeof(g_rtu_tdefs)); + memcpy(rtu->T_defs, g_rtu_tdefs, sizeof(g_rtu_tdefs)); + osmo_tdefs_reset(rtu->T_defs); + recalc_baudrate_timers(rtu); + + rtu_transmit_fsm.log_subsys = DLMODBUS_RTU; /* Update after app set the correct value */ + rtu->fi = osmo_fsm_inst_alloc(&rtu_transmit_fsm, rtu, rtu, LOGL_INFO, NULL); + + conn->proto_ops.connect = osmo_modbus_conn_rtu_connect; + conn->proto_ops.is_connected = osmo_modbus_conn_rtu_is_connected; + conn->proto_ops.tx_prim = osmo_modbus_conn_rtu_tx_prim; + conn->proto_ops.free = osmo_modbus_conn_rtu_free; + + return rtu; +} + +int osmo_modbus_conn_rtu_set_device(struct osmo_modbus_conn_rtu* rtu, const char* serial_dev) +{ + osmo_talloc_replace_string(rtu, &rtu->dev_path, serial_dev); + update_fi_name(rtu); + return 0; +} + +const char *osmo_modbus_conn_rtu_get_device(const struct osmo_modbus_conn_rtu* rtu) +{ + return rtu->dev_path; +} + +int osmo_modbus_conn_rtu_set_baudrate(struct osmo_modbus_conn_rtu* rtu, unsigned baudrate) +{ + speed_t speed; + if (osmo_serial_speed_t(baudrate, &speed) < 0) + return -EINVAL; + + rtu->baudrate = baudrate; + if (osmo_modbus_conn_rtu_is_connected(rtu->conn)) + return osmo_serial_set_baudrate(rtu->ofd.fd, speed); + recalc_baudrate_timers(rtu); + update_fi_name(rtu); + return 0; +} + +unsigned osmo_modbus_conn_rtu_get_baudrate(const struct osmo_modbus_conn_rtu* rtu) +{ + return rtu->baudrate; +} diff --git a/src/conn_slave_fsm.c b/src/conn_slave_fsm.c new file mode 100644 index 0000000..7de06b1 --- /dev/null +++ b/src/conn_slave_fsm.c @@ -0,0 +1,174 @@ +/*! \file conn_slave_fsm.c + * FSM for "Figure 14: RTU transmission mode state diagram" */ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "modbus_internal.h" +#include "conn_fsm.h" + +#define X(x) (1 << (x)) + +static const struct value_string conn_slave_event_names[] = { + { CONN_EV_CONNECT, "Connect" }, + { CONN_EV_SUBMIT_PRIM, "SubmitPrim" }, + { CONN_EV_RECV_PRIM, "RxPrim" }, + { 0, NULL } +}; + +#define conn_slave_fsm_state_chg(fi, state) \ + osmo_fsm_inst_state_chg(fi, state, 0, 0) + +static void conn_slave_fsm_st_disconnected_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ +} + +static void conn_slave_fsm_st_disconnected(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_modbus_conn *conn = (struct osmo_modbus_conn*)fi->priv; + bool *connected; + int rc; + + switch (event) { + case CONN_EV_CONNECT: + connected = (bool*)data; + if ((rc = conn->proto_ops.connect(conn)) == 0) { + *connected = true; + conn_slave_fsm_state_chg(fi, CONN_SLAVE_ST_IDLE); + } else { + *connected = false; + } + break; + default: + OSMO_ASSERT(0); + } +} + +static void conn_slave_fsm_st_idle_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ +} + +static void conn_slave_fsm_st_idle(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_modbus_conn *conn = (struct osmo_modbus_conn*)fi->priv; + struct osmo_modbus_prim *prim; + switch (event) { + case CONN_EV_RECV_PRIM: + prim = (struct osmo_modbus_prim *)data; + /* check if addr is for us... */ + if (!conn->prim_cb || conn->address != prim->address) { + LOGPFSML(fi, LOGL_DEBUG, "primitive not for us (addr=%" PRIu16 "), ignoring\n", + prim->address); + /* We still want to deliver the prim if monitor mode + enabled, but not wait for a primback from upper + layers */ + if (conn->prim_cb && conn->slave.monitor) + conn->prim_cb(conn, prim, conn->prim_cb_ctx); + else + msgb_free(prim->oph.msg); + return; + } + conn_slave_fsm_state_chg(fi, CONN_SLAVE_ST_CHECK_REQUEST); + /* Ideally this should go into st_check_request_onenter but then + * we need to store the prim pointer somewhere... */ + conn->prim_cb(conn, prim, conn->prim_cb_ctx); + break; + default: + OSMO_ASSERT(0); + } +} + +static void conn_slave_fsm_st_check_request_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ +} + +static void conn_slave_fsm_st_check_request(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_modbus_conn *conn = (struct osmo_modbus_conn*)fi->priv; + struct msgb *msg; + struct osmo_modbus_prim *prim; + + switch (event) { + case CONN_EV_SUBMIT_PRIM: + if (llist_empty(&conn->msg_queue)) { + LOGPFSML(fi, LOGL_INFO, "Write queue is empty!\n"); + OSMO_ASSERT(0); + } + + msg = msgb_dequeue(&conn->msg_queue); + prim = (struct osmo_modbus_prim *)msgb_data(msg); + conn->proto_ops.tx_prim(conn, prim); + msgb_free(msg); + conn_slave_fsm_state_chg(fi, CONN_SLAVE_ST_IDLE); + break; + default: + OSMO_ASSERT(0); + } +} + +static const struct osmo_fsm_state conn_slave_states[] = { + [CONN_SLAVE_ST_DISCONNECTED]= { + .in_event_mask = X(CONN_EV_CONNECT), + .out_state_mask = X(CONN_SLAVE_ST_IDLE), + .name = "DISCONNECTED", + .action = conn_slave_fsm_st_disconnected, + .onenter = conn_slave_fsm_st_disconnected_onenter, + }, + [CONN_SLAVE_ST_IDLE] = { + .in_event_mask = X(CONN_EV_RECV_PRIM), + .out_state_mask = X(CONN_SLAVE_ST_CHECK_REQUEST), + .name = "IDLE", + .action = conn_slave_fsm_st_idle, + .onenter = conn_slave_fsm_st_idle_onenter, + }, + [CONN_SLAVE_ST_CHECK_REQUEST] = { + .in_event_mask = X(CONN_EV_SUBMIT_PRIM), + .out_state_mask = X(CONN_SLAVE_ST_IDLE), + .name = "CHECK_REQUEST", + .action = conn_slave_fsm_st_check_request, + .onenter = conn_slave_fsm_st_check_request_onenter, + }, +}; + +struct osmo_fsm conn_slave_fsm = { + .name = "conn_slave", + .states = conn_slave_states, + .num_states = ARRAY_SIZE(conn_slave_states), + .log_subsys = DLMODBUS_OFFSET, + .event_names = conn_slave_event_names, + //.cleanup = conn_slave_fsm_cleanup, +}; + +static __attribute__((constructor)) void conn_slave_fsm_init(void) +{ + OSMO_ASSERT(osmo_fsm_register(&conn_slave_fsm) == 0); +} diff --git a/src/modbus_internal.h b/src/modbus_internal.h new file mode 100644 index 0000000..2a47daf --- /dev/null +++ b/src/modbus_internal.h @@ -0,0 +1,42 @@ +#pragma once + +#include + +#define MODBUS_MSGB_SIZE 256 + +enum { + DLMODBUS_OFFSET, + DLMODBUS_RTU_OFFSET, +}; + +struct osmo_modbus_conn { + enum osmo_modbus_conn_role role; + enum osmo_modbus_proto_type proto_type; + uint16_t address; + osmo_modbus_prim_cb prim_cb; + void *prim_cb_ctx; + struct llist_head msg_queue; + + /* role: master or slave */ + union { + struct { + uint16_t req_for_addr; /* Address of request tgt in progress */ + } master; + struct { + bool monitor; /* Is monitor mode enabled ? */ + } slave; + }; + struct osmo_tdef *T_defs; + struct osmo_fsm_inst *fi; + + /* proto private data + specific operations */ + void *proto; + struct { + int (*connect)(struct osmo_modbus_conn* conn); + bool (*is_connected)(struct osmo_modbus_conn* conn); + int (*tx_prim)(struct osmo_modbus_conn* conn, struct osmo_modbus_prim *prim); + void (*free)(struct osmo_modbus_conn* conn); + } proto_ops; +}; + +void osmo_modbus_conn_rx_prim(struct osmo_modbus_conn* conn, struct osmo_modbus_prim *prim); diff --git a/src/prim.c b/src/prim.c new file mode 100644 index 0000000..e7b2d11 --- /dev/null +++ b/src/prim.c @@ -0,0 +1,91 @@ +/*! \file prim.c + * modbus primitives */ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#include +#include + +#include + +#include +#include + +#define MODBUS_SAP 0 + +static struct msgb *modbus_prim_msgb_alloc(const char* desc) +{ + return msgb_alloc(sizeof(struct osmo_modbus_prim), desc); +} + +const struct value_string osmo_modbus_prim_type_names[] = { + { OSMO_MODBUS_PRIM_RESPONSE_TIMEOUT, "Response Timeout" }, + { OSMO_MODBUS_PRIM_N_MULT_HOLD_REG, "N Multiple Holding Registers" }, + { 0, NULL } +}; + +struct osmo_modbus_prim *osmo_modbus_makeprim_timeout_resp(uint16_t address) +{ + struct msgb *msg = modbus_prim_msgb_alloc(__func__); + struct osmo_modbus_prim *prim; + + prim = (struct osmo_modbus_prim *) msgb_put(msg, sizeof(*prim)); + osmo_prim_init(&prim->oph, MODBUS_SAP, + OSMO_MODBUS_PRIM_RESPONSE_TIMEOUT, + PRIM_OP_INDICATION, msg); + prim->address = address; + return prim; +} + +struct osmo_modbus_prim *osmo_modbus_makeprim_mult_hold_reg_req(uint16_t address, uint16_t first_reg, uint16_t num_reg) +{ + struct msgb *msg = modbus_prim_msgb_alloc(__func__); + struct osmo_modbus_prim *prim; + struct osmo_modbus_read_mult_hold_reg_req_param *param; + + prim = (struct osmo_modbus_prim *) msgb_put(msg, sizeof(*prim)); + osmo_prim_init(&prim->oph, MODBUS_SAP, + OSMO_MODBUS_PRIM_N_MULT_HOLD_REG, + PRIM_OP_REQUEST, msg); + prim->address = address; + param = &prim->u.read_mult_hold_reg_req; + param->first_reg = first_reg; + param->num_reg = num_reg; + return prim; +} + +struct osmo_modbus_prim *osmo_modbus_makeprim_mult_hold_reg_resp(uint16_t address, uint8_t num_reg, uint16_t *registers) +{ + struct msgb *msg = modbus_prim_msgb_alloc(__func__); + struct osmo_modbus_prim *prim; + struct osmo_modbus_read_mult_hold_reg_resp_param *param; + + prim = (struct osmo_modbus_prim *) msgb_put(msg, sizeof(*prim)); + osmo_prim_init(&prim->oph, MODBUS_SAP, + OSMO_MODBUS_PRIM_N_MULT_HOLD_REG, + PRIM_OP_RESPONSE, msg); + prim->address = address; + param = &prim->u.read_mult_hold_reg_resp; + param->num_reg = num_reg; + memcpy(param->registers, registers, num_reg * sizeof(uint16_t)); + return prim; +} diff --git a/src/rtu_internal.h b/src/rtu_internal.h new file mode 100644 index 0000000..5f59765 --- /dev/null +++ b/src/rtu_internal.h @@ -0,0 +1,28 @@ +#pragma once + +#include + +#include +#include + +struct osmo_modbus_conn_rtu { + struct osmo_modbus_conn* conn; /* backpointer */ + char *dev_path; + unsigned baudrate; + struct osmo_fd ofd; + struct msgb *rx_msg; + bool rx_msg_ok; /* OK (true) or NOK (false) */ /* TODO: use msg->cb instead to store the OK/NOK */ + struct msgb *tx_msg; + struct osmo_tdef *T_defs; + struct osmo_fsm_inst *fi; +}; + +struct msgb* prim2rtu(struct osmo_modbus_prim *prim); +int rtu2prim(struct osmo_modbus_conn_rtu* rtu, struct msgb* msg, struct osmo_modbus_prim **prim); + +/* 1 RTU char: start bit, 8 data bits, stop bit, and parity bit (or 2nd stop bit if no parity) */ +static inline unsigned long rtu_chars2bits(unsigned long num_chars) { + return num_chars*11; +} + +uint16_t crc16(uint8_t *buffer, uint16_t buffer_length); diff --git a/src/rtu_transmit_fsm.c b/src/rtu_transmit_fsm.c new file mode 100644 index 0000000..b40a48d --- /dev/null +++ b/src/rtu_transmit_fsm.c @@ -0,0 +1,301 @@ +/*! \file rtu_transmit_fsm.c + * FSM for "Figure 14: RTU transmission mode state diagram" */ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ +#include + +#include +#include +#include +#include +#include + +#include + +#include "modbus_internal.h" +#include "rtu_transmit_fsm.h" +#include "rtu_internal.h" + +#define X(x) (1 << (x)) + +static const struct value_string rtu_transmit_event_names[] = { + { RTU_TRANSMIT_EV_START, "Start" }, + { RTU_TRANSMIT_EV_T15_TIMEOUT, "T1.5 Timeout" }, + { RTU_TRANSMIT_EV_T35_TIMEOUT, "T3.5 Timeout" }, + { RTU_TRANSMIT_EV_CHAR_RECEIVED, "CharReceived" }, + { RTU_TRANSMIT_EV_DEMAND_OF_EMISSION, "DemandOfEmission" }, + { 0, NULL } +}; + +static const struct osmo_tdef_state_timeout rtu_transmit_fsm_timeouts[32] = { + [RTU_TRANSMIT_ST_INITIAL] = { .T=35 /* actually armed during EV START */ }, + [RTU_TRANSMIT_ST_IDLE] = { }, + [RTU_TRANSMIT_ST_EMISSION] = { /* dynamic */ }, + [RTU_TRANSMIT_ST_RECEPTION] = { .T=15 }, + [RTU_TRANSMIT_ST_CTRL_WAIT] = { /* dynamic */ }, +}; + +/* Transition to a state, using the T timer defined in assignment_fsm_timeouts. + * The actual timeout value is in turn obtained from rtu->T_defs. + * Assumes local variable fi exists. */ +#define rtu_transmit_fsm_state_chg(fi, state) \ + osmo_tdef_fsm_inst_state_chg(fi, state, \ + rtu_transmit_fsm_timeouts, \ + ((struct osmo_modbus_conn_rtu*)(fi->priv))->T_defs, \ + -1) + +static void rearm_timer_with_factor(struct osmo_fsm_inst *fi, int T, long factor_us) { + struct osmo_modbus_conn_rtu *rtu = (struct osmo_modbus_conn_rtu *)fi->priv; + unsigned long timeout_us = osmo_tdef_get(rtu->T_defs, T, OSMO_TDEF_US, -1); + timeout_us += factor_us; + fi->T = T; + LOGPFSML(fi, LOGL_DEBUG, "Rearm T%d {%ld, %ld} (%ld)\n", T, timeout_us / 1000000, timeout_us % 1000000, factor_us); + osmo_timer_schedule(&fi->timer, timeout_us / 1000000, timeout_us % 1000000); +} + +static void rearm_timer(struct osmo_fsm_inst *fi, int T) { + rearm_timer_with_factor(fi, T, 0); +} + +static void rtu_transmit_fsm_st_initial_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ +} + +static void rtu_transmit_fsm_st_initial(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + //struct osmo_modbus_conn_rtu *rtu = (struct osmo_modbus_conn_rtu *)fi->priv; + switch (event) { + case RTU_TRANSMIT_EV_START: + rearm_timer(fi, 35); + break; + case RTU_TRANSMIT_EV_CHAR_RECEIVED: + rearm_timer(fi, 35); + break; + case RTU_TRANSMIT_EV_T35_TIMEOUT: + rtu_transmit_fsm_state_chg(fi, RTU_TRANSMIT_ST_IDLE); + break; + default: + OSMO_ASSERT(0); + } +} + +static void rtu_transmit_fsm_st_idle_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ +} + +static void rtu_transmit_fsm_st_idle(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + //struct osmo_modbus_conn_rtu *rtu = (struct osmo_modbus_conn_rtu *)fi->priv; + switch (event) { + case RTU_TRANSMIT_EV_DEMAND_OF_EMISSION: + //rtu->tx_msg = (struct msgb *)data; + rtu_transmit_fsm_state_chg(fi, RTU_TRANSMIT_ST_EMISSION); + break; + case RTU_TRANSMIT_EV_CHAR_RECEIVED: + rtu_transmit_fsm_state_chg(fi, RTU_TRANSMIT_ST_RECEPTION); + break; + default: + OSMO_ASSERT(0); + } +} + +static void rtu_transmit_fsm_st_emission_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct osmo_modbus_conn_rtu *rtu = (struct osmo_modbus_conn_rtu *)fi->priv; + size_t char_len; + + /* Simply enable the write flag, fd will tell when we can send */ + rtu->ofd.when |= OSMO_FD_WRITE; + + char_len = msgb_length(rtu->tx_msg); + long time_factor_us = rtu_chars2bits(char_len) * 1000000 / rtu->baudrate; + rearm_timer_with_factor(fi, 35, time_factor_us); +} + +static void rtu_transmit_fsm_st_emission(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + switch (event) { + case RTU_TRANSMIT_EV_T35_TIMEOUT: + rtu_transmit_fsm_state_chg(fi, RTU_TRANSMIT_ST_IDLE); + break; + default: + OSMO_ASSERT(0); + } +} + +static void rtu_transmit_fsm_st_reception_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ +} + +static void rtu_transmit_fsm_st_reception(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + switch (event) { + case RTU_TRANSMIT_EV_CHAR_RECEIVED: + rearm_timer(fi, 15); + break; + case RTU_TRANSMIT_EV_T15_TIMEOUT: + rtu_transmit_fsm_state_chg(fi, RTU_TRANSMIT_ST_CTRL_WAIT); + break; + default: + OSMO_ASSERT(0); + } +} + +static void rtu_transmit_fsm_st_ctrlwait_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state) +{ + struct osmo_modbus_conn_rtu *rtu = (struct osmo_modbus_conn_rtu *)fi->priv; + uint16_t exp_crc, got_crc; + long time_factor_us; + uint8_t *data; + unsigned int len; + + /* T1.5 already triggered, which means to reach T3.5 we have to wait for + * "T2" aka 2 character timers */ + time_factor_us = -1 * (rtu_chars2bits(1500000) / rtu->baudrate); + rearm_timer_with_factor(fi, 35, time_factor_us); + + OSMO_ASSERT(rtu->rx_msg); + data = msgb_data(rtu->rx_msg); + len = msgb_length(rtu->rx_msg); + + if (len < sizeof(uint16_t)) { + LOGPFSML(fi, LOGL_INFO, "Cannot generate CRC, rx msg len: %d\n", len); + rtu->rx_msg_ok = false; + return; + } + + /* Mark NOK if CRC fails */ + memcpy(&got_crc, &data[len - sizeof(uint16_t)], sizeof(uint16_t)); + exp_crc = crc16(data, len - sizeof(uint16_t)); + osmo_store16be(exp_crc, &exp_crc); + rtu->rx_msg_ok = got_crc == exp_crc; + LOGPFSML(fi, LOGL_DEBUG, "CRC: got=0x08%x vs exp=0x08%x: %s\n", got_crc, exp_crc, + rtu->rx_msg_ok ? "OK" : "NOK"); +} + +static void rtu_transmit_fsm_st_ctrlwait(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + struct osmo_modbus_conn_rtu *rtu = (struct osmo_modbus_conn_rtu *)fi->priv; + struct osmo_modbus_prim *prim = NULL; + int rc; + + switch (event) { + case RTU_TRANSMIT_EV_CHAR_RECEIVED: + LOGP(DLMODBUS_RTU, LOGL_ERROR, "Char received while in state CTRL WAIT, marking rx msg as NOK\n"); + rtu->rx_msg_ok = false; + break; + case RTU_TRANSMIT_EV_T35_TIMEOUT: + /* TODO: submit OK rx_msg to upper layers */ + if (rtu->rx_msg_ok) { + rc = rtu2prim(rtu, rtu->rx_msg, &prim); + if (rc == -ENODATA) { /* Not enough data yet, simply wait until more data is received */ + LOGP(DLMODBUS_RTU, LOGL_DEBUG, "Not enough rx data yet\n"); + rtu->rx_msg_ok = false; + } + if (rc < 0) { + LOGP(DLMODBUS_RTU, LOGL_ERROR, "Rx Error!\n"); + rtu->rx_msg_ok = false; + } + } else { + LOGP(DLMODBUS_RTU, LOGL_ERROR, "Dropping NOK message\n"); + } + msgb_trim(rtu->rx_msg, 0); + rtu_transmit_fsm_state_chg(fi, RTU_TRANSMIT_ST_IDLE); + if (rtu->rx_msg_ok) + osmo_modbus_conn_rx_prim(rtu->conn, prim); + break; + default: + OSMO_ASSERT(0); + } +} + +static const struct osmo_fsm_state rtu_transmit_states[] = { + [RTU_TRANSMIT_ST_INITIAL] = { + .in_event_mask = X(RTU_TRANSMIT_EV_START) | + X(RTU_TRANSMIT_EV_CHAR_RECEIVED) | + X(RTU_TRANSMIT_EV_T35_TIMEOUT), + .out_state_mask = X(RTU_TRANSMIT_ST_IDLE), + .name = "INITIAL", + .action = rtu_transmit_fsm_st_initial, + .onenter = rtu_transmit_fsm_st_initial_onenter, + }, + [RTU_TRANSMIT_ST_IDLE] = { + .in_event_mask = X(RTU_TRANSMIT_EV_DEMAND_OF_EMISSION) | + X(RTU_TRANSMIT_EV_CHAR_RECEIVED), + .out_state_mask = X(RTU_TRANSMIT_ST_EMISSION) | + X(RTU_TRANSMIT_ST_RECEPTION), + .name = "IDLE", + .action = rtu_transmit_fsm_st_idle, + .onenter = rtu_transmit_fsm_st_idle_onenter, + }, + [RTU_TRANSMIT_ST_EMISSION] = { + .in_event_mask = X(RTU_TRANSMIT_EV_T35_TIMEOUT), + .out_state_mask = X(RTU_TRANSMIT_ST_IDLE), + .name = "EMISSION", + .action = rtu_transmit_fsm_st_emission, + .onenter = rtu_transmit_fsm_st_emission_onenter, + }, + [RTU_TRANSMIT_ST_RECEPTION] = { + .in_event_mask = X(RTU_TRANSMIT_EV_CHAR_RECEIVED) | + X(RTU_TRANSMIT_EV_T15_TIMEOUT), + .out_state_mask = X(RTU_TRANSMIT_ST_CTRL_WAIT), + .name = "RECEPTION", + .action = rtu_transmit_fsm_st_reception, + .onenter = rtu_transmit_fsm_st_reception_onenter, + }, + [RTU_TRANSMIT_ST_CTRL_WAIT] = { + .in_event_mask = X(RTU_TRANSMIT_EV_CHAR_RECEIVED) | + X(RTU_TRANSMIT_EV_T35_TIMEOUT), + .out_state_mask = X(RTU_TRANSMIT_ST_IDLE), + .name = "CTRL_WAIT", + .action = rtu_transmit_fsm_st_ctrlwait, + .onenter = rtu_transmit_fsm_st_ctrlwait_onenter, + }, +}; + +static int rtu_transmit_fsm_timer_cb(struct osmo_fsm_inst *fi) +{ + switch (fi->T) { + case 15: + osmo_fsm_inst_dispatch(fi, RTU_TRANSMIT_EV_T15_TIMEOUT, NULL); + break; + case 35: + osmo_fsm_inst_dispatch(fi, RTU_TRANSMIT_EV_T35_TIMEOUT, NULL); + break; + } + return 0; +} + +struct osmo_fsm rtu_transmit_fsm = { + .name = "RTU_TRANSMIT", + .states = rtu_transmit_states, + .num_states = ARRAY_SIZE(rtu_transmit_states), + .timer_cb = rtu_transmit_fsm_timer_cb, + .log_subsys = DLMODBUS_RTU_OFFSET, + .event_names = rtu_transmit_event_names, + //.cleanup = rtu_transmit_fsm_cleanup, +}; + +static __attribute__((constructor)) void rtu_transmit_fsm_init(void) +{ + OSMO_ASSERT(osmo_fsm_register(&rtu_transmit_fsm) == 0); +} diff --git a/src/rtu_transmit_fsm.h b/src/rtu_transmit_fsm.h new file mode 100644 index 0000000..eb897bd --- /dev/null +++ b/src/rtu_transmit_fsm.h @@ -0,0 +1,21 @@ +/* Figure 14: RTU transmission mode state diagram */ +#pragma once + +enum rtu_transmit_state { + RTU_TRANSMIT_ST_INITIAL, + RTU_TRANSMIT_ST_IDLE, + RTU_TRANSMIT_ST_EMISSION, + RTU_TRANSMIT_ST_RECEPTION, + RTU_TRANSMIT_ST_CTRL_WAIT, +}; + +enum rtu_transmit_event { + RTU_TRANSMIT_EV_START, + RTU_TRANSMIT_EV_T15_TIMEOUT, + RTU_TRANSMIT_EV_T35_TIMEOUT, + RTU_TRANSMIT_EV_CHAR_RECEIVED, + RTU_TRANSMIT_EV_DEMAND_OF_EMISSION, + _NUM_RTU_TRANSMIT_EV, +}; + +extern struct osmo_fsm rtu_transmit_fsm; diff --git a/utils/Makefile.am b/utils/Makefile.am new file mode 100644 index 0000000..0924792 --- /dev/null +++ b/utils/Makefile.am @@ -0,0 +1,20 @@ +AM_CPPFLAGS = $(all_includes) -I$(top_srcdir)/include +AM_CFLAGS=-Wall -g $(LIBOSMOCORE_CFLAGS) $(COVERAGE_FLAGS) +AM_LDFLAGS=$(COVERAGE_LDFLAGS) + +bin_PROGRAMS = modbus_rtu_master modbus_rtu_slave crc16_rtu_gen + +modbus_rtu_master_SOURCES = modbus_rtu_master.c +modbus_rtu_master_LDADD = $(top_builddir)/src/libosmo-modbus.la \ + $(LIBOSMOCORE_LIBS) \ + $(NULL) + +modbus_rtu_slave_SOURCES = modbus_rtu_slave.c +modbus_rtu_slave_LDADD = $(top_builddir)/src/libosmo-modbus.la \ + $(LIBOSMOCORE_LIBS) \ + $(NULL) + +crc16_rtu_gen_SOURCES = crc16_rtu_gen.c +crc16_rtu_gen_LDADD = $(top_builddir)/src/libosmo-modbus.la \ + $(LIBOSMOCORE_LIBS) \ + $(NULL) diff --git a/utils/crc16_rtu_gen.c b/utils/crc16_rtu_gen.c new file mode 100644 index 0000000..2df3250 --- /dev/null +++ b/utils/crc16_rtu_gen.c @@ -0,0 +1,90 @@ +#include +#include +#include +#include +#include + +/* High-Order Byte Table */ +static const uint8_t table_crc_hi[] = { + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, + 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, + 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, + 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, + 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 +}; + +/* Low-Order Byte Table */ +static const uint8_t table_crc_lo[] = { + 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, + 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, + 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, + 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, + 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, + 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, + 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, + 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, + 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, + 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, + 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, + 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, + 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, + 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, + 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, + 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, + 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, + 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, + 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, + 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, + 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, + 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, + 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, + 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, + 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, + 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 +}; + +/* CRC Generation Function */ +uint16_t crc16(uint8_t *data, uint16_t data_len) +{ + uint8_t crc_hi = 0xFF; /* Initialized high CRC byte */ + uint8_t crc_lo = 0xFF; /* Initialized low CRC byte */ + unsigned int idx; /* will index into CRC lookup */ + + /* pass through message buffer */ + while (data_len--) { + idx = crc_hi ^ *data++; /* calculate the CRC */ + crc_hi = crc_lo ^ table_crc_hi[idx]; + crc_lo = table_crc_lo[idx]; + } + + return (crc_hi << 8 | crc_lo); +} + + +int main(int argc, char *argv[]) { + uint8_t msg[] = {0x01, 0x03, 0x04, 0x00, 0x00, 0x00, 0x17}; + uint16_t crc = crc16(msg, sizeof(msg)); + printf("crc: 0x%08x\n", crc); + return 0; +} diff --git a/utils/modbus_rtu_master.c b/utils/modbus_rtu_master.c new file mode 100644 index 0000000..fcc92d7 --- /dev/null +++ b/utils/modbus_rtu_master.c @@ -0,0 +1,270 @@ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#define _GNU_SOURCE +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#define APP_NAME "OsmoModbusRTUmaster" + +static void *tall_ctx; +static struct osmo_timer_list timer_req; +static struct osmo_modbus_conn *conn; +static uint16_t slave_address = 0x01; +static char device_path[256] = "/dev/ttyUSB0"; +static size_t timeout_response = 0; + +static void print_help(void) +{ + printf(" -h --help This text.\n"); + printf(" -V --version Print the version of " APP_NAME "\n"); + printf(" -T --timestamp Print a timestamp in the debug output.\n"); + printf(" -s --serial-device PATH Set serial device (RTU connection)\n"); + printf(" -a --slave-addess ADDRESS Set slave address to talk to\n"); + printf(" -t --timeout-response Response tmeout, in milliseconds.\n"); +} + +static void handle_options(int argc, char **argv) +{ + while (1) { + int option_index = 0, c; + static const struct option long_options[] = { + { "help", 0, 0, 'h' }, + { "version", 0, 0, 'V' }, + {"timestamp", 0, 0, 'T'}, + {"serial-device", 1, 0, 's'}, + {"slave-address", 1, 0, 'a'}, + {"timeout-response", 1, 0, 'a'}, + { NULL, 0, 0, 0 } + }; + + c = getopt_long(argc, argv, "hVTs:a:t:", long_options, &option_index); + if (c == -1) + break; + + switch (c) { + case 'h': + print_help(); + exit(0); + break; + case 'V': + //print_version(1); + exit(0); + break; + case 'T': + log_set_print_timestamp(osmo_stderr_target, 1); + log_set_print_extended_timestamp(osmo_stderr_target, 1); + break; + case 's': + osmo_strlcpy(device_path, optarg, sizeof(device_path)); + break; + case 'a': + slave_address = atoi(optarg); + break; + case 't': + timeout_response = atoi(optarg); + break; + default: + fprintf(stderr, "Error in command line options. Exiting\n"); + exit(1); + break; + } + } + + if (argc > optind) { + fprintf(stderr, "Unsupported positional arguments in command line\n"); + exit(2); + } +} + +static void signal_handler(int signal) +{ + fprintf(stdout, "signal %u received\n", signal); + + switch (signal) { + case SIGINT: + case SIGTERM: + exit(0); + break; + case SIGABRT: + osmo_generate_backtrace(); + /* in case of abort, we want to obtain a talloc report + * and then return to the caller, who will abort the process */ + case SIGUSR1: + talloc_report_full(tall_ctx, stderr); + break; + case SIGUSR2: + break; + default: + break; + } +} + +enum { + DMAIN, +}; + +void _log_init(void *tall_ctx) +{ + unsigned own_logcats = 1; + unsigned lib_logcats; + lib_logcats = osmo_modbus_set_logging_category_offset(own_logcats); + struct log_info_cat log_info_cat[own_logcats + lib_logcats]; + log_info_cat[DMAIN] = (struct log_info_cat){ + .name = "DMAIN", + .description = "main", + .color = "\033[1;32m", + .enabled = 1, .loglevel = LOGL_DEBUG, + }; + log_info_cat[DLMODBUS] = (struct log_info_cat){ + .name = "DLMODBUS", + .description = "Modbus Library", + .color = "\033[1;33m", + .enabled = 1, .loglevel = LOGL_DEBUG, + }; + log_info_cat[DLMODBUS_RTU] = (struct log_info_cat){ + .name = "DLMODBUS_RTU", + .description = "Modbus Library (RTU)", + .color = "\033[1;34m", + .enabled = 1, .loglevel = LOGL_DEBUG, + }; + + const struct log_info log_info = { + .cat = log_info_cat, + .num_cat = ARRAY_SIZE(log_info_cat), + }; + osmo_init_logging2(tall_ctx, &log_info); + + log_set_print_category_hex(osmo_stderr_target, 0); + log_set_print_category(osmo_stderr_target, 1); + log_set_print_filename2(osmo_stderr_target, LOG_FILENAME_BASENAME); + osmo_fsm_log_addr(false); +} + +static void timer_req_cb(void *data) +{ + struct osmo_modbus_prim *prim; + int rc; + + prim = osmo_modbus_makeprim_mult_hold_reg_req(slave_address, 0x0C, 1); + rc = osmo_modbus_conn_submit_prim(conn, prim); + if (rc < 0) { + LOGP(DMAIN, LOGL_INFO, "Failed submitting primitive to address %u: %d\n", slave_address, rc); + exit(1); + } + osmo_timer_schedule(&timer_req, 10, 0); +} + +int prim_cb(struct osmo_modbus_conn *conn, struct osmo_modbus_prim *prim, void *ctx) +{ + LOGP(DMAIN, LOGL_INFO, "prim_cb()!\n"); + switch (OSMO_PRIM_HDR(&prim->oph)) { + case OSMO_PRIM(OSMO_MODBUS_PRIM_RESPONSE_TIMEOUT, PRIM_OP_INDICATION): + LOGP(DMAIN, LOGL_INFO, "Tx timeout!\n"); + break; + case OSMO_PRIM(OSMO_MODBUS_PRIM_N_MULT_HOLD_REG, PRIM_OP_RESPONSE): + LOGP(DMAIN, LOGL_INFO, "Received OSMO_MODBUS_PRIM_N_MULT_HOLD_REG RESPONSE!\n"); + LOGP(DMAIN, LOGL_INFO, "[addr=%u] Read %u registers: %s\n", prim->address, + prim->u.read_mult_hold_reg_resp.num_reg, + osmo_hexdump((uint8_t*)prim->u.read_mult_hold_reg_resp.registers, prim->u.read_mult_hold_reg_resp.num_reg*2)); + uint16_t voltage_dV = osmo_load16be(prim->u.read_mult_hold_reg_resp.registers); + double voltage = voltage_dV / 10.0f; + LOGP(DMAIN, LOGL_INFO, "Received voltage: %fV\n", voltage); + break; + default: + LOGP(DMAIN, LOGL_INFO, "Unhandled primitive operation %s on primitive %s\n", + get_value_string(osmo_prim_op_names, prim->oph.operation), + get_value_string(osmo_modbus_prim_type_names, prim->oph.primitive)); + } + msgb_free(prim->oph.msg); + return 0; +} + + +int main(int argc, char **argv) +{ + struct osmo_modbus_conn_rtu *rtu; + int rc; + + tall_ctx = talloc_named_const(NULL, 1, APP_NAME); + msgb_talloc_ctx_init(tall_ctx, 0); + _log_init(tall_ctx); + + handle_options(argc, argv); + + signal(SIGINT, &signal_handler); + signal(SIGTERM, &signal_handler); + signal(SIGABRT, &signal_handler); + signal(SIGUSR1, &signal_handler); + signal(SIGUSR2, &signal_handler); + osmo_init_ignore_signals(); + + LOGP(DMAIN, LOGL_INFO, "Initializig modbus conn...\n"); + conn = osmo_modbus_conn_alloc(tall_ctx, + OSMO_MODBUS_ROLE_MASTER, + OSMO_MODBUS_PROTO_RTU); + osmo_modbus_conn_set_prim_cb(conn, prim_cb, NULL); + rtu = osmo_modbus_conn_get_rtu(conn); + osmo_modbus_conn_rtu_set_device(rtu, device_path); + + if (timeout_response) { + if ((rc = osmo_modbus_conn_set_timeout(conn, + OSMO_MODBUS_TO_TURNAROUND, + timeout_response)) < 0) + LOGP(DMAIN, LOGL_INFO, "Failed setting Turnaround timeout to %zu\n", timeout_response); + if ((rc = osmo_modbus_conn_set_timeout(conn, + OSMO_MODBUS_TO_NORESPONSE, + timeout_response)) < 0) + LOGP(DMAIN, LOGL_INFO, "Failed setting Response timeout to %zu\n", timeout_response); + } + + if ((rc = osmo_modbus_conn_connect(conn)) < 0) { + LOGP(DMAIN, LOGL_INFO, "Connect to modbus serial device %s failed! %d\n", device_path, rc); + exit(1); + } + + osmo_timer_setup(&timer_req, timer_req_cb, NULL); + osmo_timer_schedule(&timer_req, 1, 0); + + while (1) { + rc = osmo_select_main(0); + if (rc < 0) + exit(3); + } +} diff --git a/utils/modbus_rtu_slave.c b/utils/modbus_rtu_slave.c new file mode 100644 index 0000000..31f0b63 --- /dev/null +++ b/utils/modbus_rtu_slave.c @@ -0,0 +1,258 @@ +/* + * Copyright (C) 2020 Pau Espin Pedrol + * + * All Rights Reserved + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +#define _GNU_SOURCE +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#define APP_NAME "OsmoModbusRTUslave" + +static void *tall_ctx; +static struct osmo_modbus_conn *conn; +static uint16_t slave_address = 0x01; +static char device_path[256] = "/dev/ttyUSB0"; +bool monitor; + +static void print_help(void) +{ + printf(" -h --help This text.\n"); + printf(" -V --version Print the version of " APP_NAME "\n"); + printf(" -T --timestamp Print a timestamp in the debug output.\n"); + printf(" -s --serial-device PATH Set serial device (RTU connection)\n"); + printf(" -a --slave-addess ADDRESS Set slave address to listen to\n"); + printf(" -m --monitor Enable monitor mode\n"); +} + +static void handle_options(int argc, char **argv) +{ + while (1) { + int option_index = 0, c; + static const struct option long_options[] = { + { "help", 0, 0, 'h' }, + { "version", 0, 0, 'V' }, + {"timestamp", 0, 0, 'T'}, + {"serial-device", 1, 0, 's'}, + {"slave-address", 1, 0, 'a'}, + {"monitor", 0, 0, 'm'}, + { NULL, 0, 0, 0 } + }; + + c = getopt_long(argc, argv, "hVTs:a:m", long_options, &option_index); + if (c == -1) + break; + + switch (c) { + case 'h': + print_help(); + exit(0); + break; + case 'V': + //print_version(1); + exit(0); + break; + case 'T': + log_set_print_timestamp(osmo_stderr_target, 1); + log_set_print_extended_timestamp(osmo_stderr_target, 1); + break; + case 's': + osmo_strlcpy(device_path, optarg, sizeof(device_path)); + break; + case 'a': + slave_address = atoi(optarg); + break; + case 'm': + monitor = true; + break; + default: + fprintf(stderr, "Error in command line options. Exiting\n"); + exit(1); + break; + } + } + + if (argc > optind) { + fprintf(stderr, "Unsupported positional arguments in command line\n"); + exit(2); + } +} + +static void signal_handler(int signal) +{ + fprintf(stdout, "signal %u received\n", signal); + + switch (signal) { + case SIGINT: + case SIGTERM: + exit(0); + break; + case SIGABRT: + osmo_generate_backtrace(); + /* in case of abort, we want to obtain a talloc report + * and then return to the caller, who will abort the process */ + case SIGUSR1: + talloc_report_full(tall_ctx, stderr); + break; + case SIGUSR2: + break; + default: + break; + } +} + +enum { + DMAIN, +}; + +void _log_init(void *tall_ctx) +{ + unsigned own_logcats = 1; + unsigned lib_logcats; + lib_logcats = osmo_modbus_set_logging_category_offset(own_logcats); + struct log_info_cat log_info_cat[own_logcats + lib_logcats]; + log_info_cat[DMAIN] = (struct log_info_cat){ + .name = "DMAIN", + .description = "main", + .color = "\033[1;32m", + .enabled = 1, .loglevel = LOGL_DEBUG, + }; + log_info_cat[DLMODBUS] = (struct log_info_cat){ + .name = "DLMODBUS", + .description = "Modbus Library", + .color = "\033[1;33m", + .enabled = 1, .loglevel = LOGL_DEBUG, + }; + log_info_cat[DLMODBUS_RTU] = (struct log_info_cat){ + .name = "DLMODBUS_RTU", + .description = "Modbus Library (RTU)", + .color = "\033[1;34m", + .enabled = 1, .loglevel = LOGL_DEBUG, + }; + + const struct log_info log_info = { + .cat = log_info_cat, + .num_cat = ARRAY_SIZE(log_info_cat), + }; + osmo_init_logging2(tall_ctx, &log_info); + + log_set_print_category_hex(osmo_stderr_target, 0); + log_set_print_category(osmo_stderr_target, 1); + log_set_print_filename2(osmo_stderr_target, LOG_FILENAME_BASENAME); + osmo_fsm_log_addr(false); +} + +int prim_cb(struct osmo_modbus_conn *conn, struct osmo_modbus_prim *prim, void *ctx) +{ + struct osmo_modbus_prim *resp; + uint16_t *buf; + int rc; + + LOGP(DMAIN, LOGL_INFO, "prim_cb()!\n"); + switch (OSMO_PRIM_HDR(&prim->oph)) { + case OSMO_PRIM(OSMO_MODBUS_PRIM_RESPONSE_TIMEOUT, PRIM_OP_INDICATION): + LOGP(DMAIN, LOGL_INFO, "Tx timeout!\n"); + break; + case OSMO_PRIM(OSMO_MODBUS_PRIM_N_MULT_HOLD_REG, PRIM_OP_REQUEST): + LOGP(DMAIN, LOGL_INFO, "Received OSMO_MODBUS_PRIM_N_MULT_HOLD_REG REQUEST!\n"); + LOGP(DMAIN, LOGL_INFO, "[addr=%u] Read %u registers: start from 0x%04x\n", prim->address, + prim->u.read_mult_hold_reg_req.num_reg, + prim->u.read_mult_hold_reg_req.first_reg); + + /* Avoid answering for requests not aimed at us if we enabled monitor mode */ + if (!monitor || prim->address == slave_address) { + buf = calloc(prim->u.read_mult_hold_reg_req.num_reg, sizeof(uint16_t)); + memset(buf, 0x2b, prim->u.read_mult_hold_reg_req.num_reg * sizeof(uint16_t)); + resp = osmo_modbus_makeprim_mult_hold_reg_resp(slave_address, + prim->u.read_mult_hold_reg_req.num_reg, + buf); + free(buf); + rc = osmo_modbus_conn_submit_prim(conn, resp); + if (rc < 0) { + LOGP(DMAIN, LOGL_INFO, "Failed submitting primitive: %d\n", rc); + exit(1); + } + } + break; + default: + LOGP(DMAIN, LOGL_INFO, "Unhandled primitive operation %s on primitive %s\n", + get_value_string(osmo_prim_op_names, prim->oph.operation), + get_value_string(osmo_modbus_prim_type_names, prim->oph.primitive)); + } + msgb_free(prim->oph.msg); + return 0; +} + + +int main(int argc, char **argv) +{ + struct osmo_modbus_conn_rtu *rtu; + int rc; + + tall_ctx = talloc_named_const(NULL, 1, APP_NAME); + msgb_talloc_ctx_init(tall_ctx, 0); + _log_init(tall_ctx); + + handle_options(argc, argv); + + signal(SIGINT, &signal_handler); + signal(SIGTERM, &signal_handler); + signal(SIGABRT, &signal_handler); + signal(SIGUSR1, &signal_handler); + signal(SIGUSR2, &signal_handler); + osmo_init_ignore_signals(); + + LOGP(DMAIN, LOGL_INFO, "Initializig modbus conn...\n"); + conn = osmo_modbus_conn_alloc(tall_ctx, + OSMO_MODBUS_ROLE_SLAVE, + OSMO_MODBUS_PROTO_RTU); + osmo_modbus_conn_set_prim_cb(conn, prim_cb, NULL); + osmo_modbus_conn_set_address(conn, slave_address); + osmo_modbus_conn_set_monitor_mode(conn, monitor); + rtu = osmo_modbus_conn_get_rtu(conn); + osmo_modbus_conn_rtu_set_device(rtu, device_path); + if ((rc = osmo_modbus_conn_connect(conn)) < 0) { + LOGP(DMAIN, LOGL_INFO, "Connect to modbus serial device %s failed! %d\n", device_path, rc); + exit(1); + } + + while (1) { + rc = osmo_select_main(0); + if (rc < 0) + exit(3); + } +}