From e823e78623d957ee0b9d433c111f14add05a317e Mon Sep 17 00:00:00 2001 From: Gregory Nutt Date: Wed, 15 May 2013 08:35:53 -0600 Subject: Add a copy of the Osmocom-BB osmocon tool --- misc/tools/osmocon/COPYING | 339 +++++++ misc/tools/osmocon/Makefile | 103 +++ misc/tools/osmocon/crc16.c | 62 ++ misc/tools/osmocon/crc16.h | 34 + misc/tools/osmocon/linuxlist.h | 360 ++++++++ misc/tools/osmocon/linuxrbtree.h | 160 ++++ misc/tools/osmocon/msgb.c | 154 ++++ misc/tools/osmocon/msgb.h | 401 +++++++++ misc/tools/osmocon/osmocon.c | 1521 +++++++++++++++++++++++++++++++ misc/tools/osmocon/osmoload.c | 1216 +++++++++++++++++++++++++ misc/tools/osmocon/panic.c | 25 + misc/tools/osmocon/panic.h | 20 + misc/tools/osmocon/protocol.h | 37 + misc/tools/osmocon/rbtree.c | 383 ++++++++ misc/tools/osmocon/select.c | 166 ++++ misc/tools/osmocon/select.h | 45 + misc/tools/osmocon/sercomm.c | 268 ++++++ misc/tools/osmocon/sercomm.h | 59 ++ misc/tools/osmocon/serial.c | 229 +++++ misc/tools/osmocon/serial.h | 43 + misc/tools/osmocon/talloc.c | 1804 +++++++++++++++++++++++++++++++++++++ misc/tools/osmocon/talloc.h | 192 ++++ misc/tools/osmocon/timer.c | 264 ++++++ misc/tools/osmocon/timer.h | 89 ++ misc/tools/osmocon/timer_compat.h | 79 ++ misc/tools/osmocon/tpu_debug.c | 138 +++ misc/tools/osmocon/utils.h | 56 ++ 27 files changed, 8247 insertions(+) create mode 100644 misc/tools/osmocon/COPYING create mode 100644 misc/tools/osmocon/Makefile create mode 100644 misc/tools/osmocon/crc16.c create mode 100644 misc/tools/osmocon/crc16.h create mode 100644 misc/tools/osmocon/linuxlist.h create mode 100644 misc/tools/osmocon/linuxrbtree.h create mode 100644 misc/tools/osmocon/msgb.c create mode 100644 misc/tools/osmocon/msgb.h create mode 100644 misc/tools/osmocon/osmocon.c create mode 100644 misc/tools/osmocon/osmoload.c create mode 100644 misc/tools/osmocon/panic.c create mode 100644 misc/tools/osmocon/panic.h create mode 100644 misc/tools/osmocon/protocol.h create mode 100644 misc/tools/osmocon/rbtree.c create mode 100644 misc/tools/osmocon/select.c create mode 100644 misc/tools/osmocon/select.h create mode 100644 misc/tools/osmocon/sercomm.c create mode 100644 misc/tools/osmocon/sercomm.h create mode 100644 misc/tools/osmocon/serial.c create mode 100644 misc/tools/osmocon/serial.h create mode 100644 misc/tools/osmocon/talloc.c create mode 100644 misc/tools/osmocon/talloc.h create mode 100644 misc/tools/osmocon/timer.c create mode 100644 misc/tools/osmocon/timer.h create mode 100644 misc/tools/osmocon/timer_compat.h create mode 100644 misc/tools/osmocon/tpu_debug.c create mode 100644 misc/tools/osmocon/utils.h (limited to 'misc/tools/osmocon') diff --git a/misc/tools/osmocon/COPYING b/misc/tools/osmocon/COPYING new file mode 100644 index 000000000..d511905c1 --- /dev/null +++ b/misc/tools/osmocon/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/misc/tools/osmocon/Makefile b/misc/tools/osmocon/Makefile new file mode 100644 index 000000000..2d64f86f2 --- /dev/null +++ b/misc/tools/osmocon/Makefile @@ -0,0 +1,103 @@ +############################################################################ +# Makefile +# +# Copyright (C) 2013 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# Things you might want to override from the make command line + +CC ?= gcc +LD ?= ld +OPT ?= -g +INSTALLDIR ?= /usr/local/bin + +CFLAGS = $(OPT) +CFLAGS += -I. +CFLAGS += -Wall -Wstrict-prototypes -Wshadow + +# Control build verbosity + +ifeq ($(V),1) + export Q := +else + export Q := @ +endif + + +# Files + +OCSRCS1 = osmocon.c tpu_debug.c +OCSRCS2 = msgb.c serial.c panic.c talloc.c timer.c select.c rbtree.c sercomm.c +OCSRCS = $(OCSRCS1) $(OCSRCS2) + +OLSRCS1 = osmoload.c +OLSRCS2 = msgb.c panic.c talloc.c timer.c select.c rbtree.c crc16.c +OLSRCS = $(OLSRCS1) $(OLSRCS2) + +CMNSRCS = msgb.c serial.c panic.c talloc.c timer.c select.c rbtree.c sercomm.c crc16.c + +SRCS = $(OCSRCS1) $(OLSRCS1) $(CMNSRCS) + +OCOBJS = $(OCSRCS:.c=.o) +OLOBJS = $(OLSRCS:.c=.o) + +OBJS = $(SRCS:.c=.o) + +ifdef EXEET +.PHONY: all osmocon osmoload clean install +else +.PHONY: all clean install +endif + +all: osmocon osmoload + +$(OBJS): %.o: %.c + $(Q) $(CC) -c $(CFLAGS) $< -o $@ + +osmocon$(EXEEXT): $(OCOBJS) + $(Q) $(CC) $(OCOBJS) -o osmocon$(EXEEXT) + +osmoload$(EXEEXT): $(OLOBJS) + $(Q) $(CC) $(OLOBJS) -o osmoload$(EXEEXT) + +ifdef EXEET +osmocon: osmocon$(EXEEXT) + +osmoload$: osmoload$(EXEEXT) +endif + +install: osmocon$(EXEEXT) osmoload$(EXEEXT) + $(Q) install -m 755 osmocon$(EXEEXT) $(INSTALLDIR)/osmocon$(EXEEXT) + $(Q) install -m 755 osmoload$(EXEEXT) $(INSTALLDIR)/osmoload$(EXEEXT) + +clean: + $(Q) rm -f osmocon$(EXEEXT) osmoload$(EXEEXT) *.o diff --git a/misc/tools/osmocon/crc16.c b/misc/tools/osmocon/crc16.c new file mode 100644 index 000000000..a6d82d698 --- /dev/null +++ b/misc/tools/osmocon/crc16.c @@ -0,0 +1,62 @@ +/* + * This was copied from the linux kernel and adjusted for our types. + */ +/* + * crc16.c + * + * This source code is licensed under the GNU General Public License, + * Version 2. See the file COPYING for more details. + */ + +#include "crc16.h" + +/** CRC table for the CRC-16. The poly is 0x8005 (x^16 + x^15 + x^2 + 1) */ +uint16_t const osmo_crc16_table[256] = { + 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, + 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, + 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, + 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841, + 0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40, + 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41, + 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641, + 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040, + 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240, + 0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, + 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, + 0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, + 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, + 0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40, + 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640, + 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041, + 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240, + 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441, + 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41, + 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, + 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41, + 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, + 0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640, + 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041, + 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241, + 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440, + 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40, + 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, + 0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, + 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, + 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, + 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 +}; + +/** + * crc16 - compute the CRC-16 for the data buffer + * @crc: previous CRC value + * @buffer: data pointer + * @len: number of bytes in the buffer + * + * Returns the updated CRC value. + */ +uint16_t osmo_crc16(uint16_t crc, uint8_t const *buffer, size_t len) +{ + while (len--) + crc = osmo_crc16_byte(crc, *buffer++); + return crc; +} diff --git a/misc/tools/osmocon/crc16.h b/misc/tools/osmocon/crc16.h new file mode 100644 index 000000000..0e5241768 --- /dev/null +++ b/misc/tools/osmocon/crc16.h @@ -0,0 +1,34 @@ +/* + * This was copied from the linux kernel and adjusted for our types. + */ +/* + * crc16.h - CRC-16 routine + * + * Implements the standard CRC-16: + * Width 16 + * Poly 0x8005 (x^16 + x^15 + x^2 + 1) + * Init 0 + * + * Copyright (c) 2005 Ben Gardner + * + * This source code is licensed under the GNU General Public License, + * Version 2. See the file COPYING for more details. + */ + +#ifndef __CRC16_H +#define __CRC16_H + +#include + +#include + +extern uint16_t const osmo_crc16_table[256]; + +extern uint16_t osmo_crc16(uint16_t crc, const uint8_t *buffer, size_t len); + +static inline uint16_t osmo_crc16_byte(uint16_t crc, const uint8_t data) +{ + return (crc >> 8) ^ osmo_crc16_table[(crc ^ data) & 0xff]; +} + +#endif /* __CRC16_H */ diff --git a/misc/tools/osmocon/linuxlist.h b/misc/tools/osmocon/linuxlist.h new file mode 100644 index 000000000..ff2c49156 --- /dev/null +++ b/misc/tools/osmocon/linuxlist.h @@ -0,0 +1,360 @@ +#ifndef _LINUX_LLIST_H +#define _LINUX_LLIST_H + +#include + +#ifndef inline +#define inline __inline__ +#endif + +static inline void prefetch(__attribute__((unused)) const void *x) {;} + +/** + * container_of - cast a member of a structure out to the containing structure + * + * @ptr: the pointer to the member. + * @type: the type of the container struct this is embedded in. + * @member: the name of the member within the struct. + * + */ +#define container_of(ptr, type, member) ({ \ + const typeof( ((type *)0)->member ) *__mptr = (typeof( ((type *)0)->member ) *)(ptr); \ + (type *)( (char *)__mptr - offsetof(type, member) );}) + + +/* + * These are non-NULL pointers that will result in page faults + * under normal circumstances, used to verify that nobody uses + * non-initialized llist entries. + */ +#define LLIST_POISON1 ((void *) 0x00100100) +#define LLIST_POISON2 ((void *) 0x00200200) + +/* + * Simple doubly linked llist implementation. + * + * Some of the internal functions ("__xxx") are useful when + * manipulating whole llists rather than single entries, as + * sometimes we already know the next/prev entries and we can + * generate better code by using them directly rather than + * using the generic single-entry routines. + */ + +struct llist_head { + struct llist_head *next, *prev; +}; + +#define LLIST_HEAD_INIT(name) { &(name), &(name) } + +#define LLIST_HEAD(name) \ + struct llist_head name = LLIST_HEAD_INIT(name) + +#define INIT_LLIST_HEAD(ptr) do { \ + (ptr)->next = (ptr); (ptr)->prev = (ptr); \ +} while (0) + +/* + * Insert a new entry between two known consecutive entries. + * + * This is only for internal llist manipulation where we know + * the prev/next entries already! + */ +static inline void __llist_add(struct llist_head *_new, + struct llist_head *prev, + struct llist_head *next) +{ + next->prev = _new; + _new->next = next; + _new->prev = prev; + prev->next = _new; +} + +/** + * llist_add - add a new entry + * @new: new entry to be added + * @head: llist head to add it after + * + * Insert a new entry after the specified head. + * This is good for implementing stacks. + */ +static inline void llist_add(struct llist_head *_new, struct llist_head *head) +{ + __llist_add(_new, head, head->next); +} + +/** + * llist_add_tail - add a new entry + * @new: new entry to be added + * @head: llist head to add it before + * + * Insert a new entry before the specified head. + * This is useful for implementing queues. + */ +static inline void llist_add_tail(struct llist_head *_new, struct llist_head *head) +{ + __llist_add(_new, head->prev, head); +} + +/* + * Delete a llist entry by making the prev/next entries + * point to each other. + * + * This is only for internal llist manipulation where we know + * the prev/next entries already! + */ +static inline void __llist_del(struct llist_head * prev, struct llist_head * next) +{ + next->prev = prev; + prev->next = next; +} + +/** + * llist_del - deletes entry from llist. + * @entry: the element to delete from the llist. + * Note: llist_empty on entry does not return true after this, the entry is + * in an undefined state. + */ +static inline void llist_del(struct llist_head *entry) +{ + __llist_del(entry->prev, entry->next); + entry->next = (struct llist_head *)LLIST_POISON1; + entry->prev = (struct llist_head *)LLIST_POISON2; +} + +/** + * llist_del_init - deletes entry from llist and reinitialize it. + * @entry: the element to delete from the llist. + */ +static inline void llist_del_init(struct llist_head *entry) +{ + __llist_del(entry->prev, entry->next); + INIT_LLIST_HEAD(entry); +} + +/** + * llist_move - delete from one llist and add as another's head + * @llist: the entry to move + * @head: the head that will precede our entry + */ +static inline void llist_move(struct llist_head *llist, struct llist_head *head) +{ + __llist_del(llist->prev, llist->next); + llist_add(llist, head); +} + +/** + * llist_move_tail - delete from one llist and add as another's tail + * @llist: the entry to move + * @head: the head that will follow our entry + */ +static inline void llist_move_tail(struct llist_head *llist, + struct llist_head *head) +{ + __llist_del(llist->prev, llist->next); + llist_add_tail(llist, head); +} + +/** + * llist_empty - tests whether a llist is empty + * @head: the llist to test. + */ +static inline int llist_empty(const struct llist_head *head) +{ + return head->next == head; +} + +static inline void __llist_splice(struct llist_head *llist, + struct llist_head *head) +{ + struct llist_head *first = llist->next; + struct llist_head *last = llist->prev; + struct llist_head *at = head->next; + + first->prev = head; + head->next = first; + + last->next = at; + at->prev = last; +} + +/** + * llist_splice - join two llists + * @llist: the new llist to add. + * @head: the place to add it in the first llist. + */ +static inline void llist_splice(struct llist_head *llist, struct llist_head *head) +{ + if (!llist_empty(llist)) + __llist_splice(llist, head); +} + +/** + * llist_splice_init - join two llists and reinitialise the emptied llist. + * @llist: the new llist to add. + * @head: the place to add it in the first llist. + * + * The llist at @llist is reinitialised + */ +static inline void llist_splice_init(struct llist_head *llist, + struct llist_head *head) +{ + if (!llist_empty(llist)) { + __llist_splice(llist, head); + INIT_LLIST_HEAD(llist); + } +} + +/** + * llist_entry - get the struct for this entry + * @ptr: the &struct llist_head pointer. + * @type: the type of the struct this is embedded in. + * @member: the name of the llist_struct within the struct. + */ +#define llist_entry(ptr, type, member) \ + container_of(ptr, type, member) + +/** + * llist_for_each - iterate over a llist + * @pos: the &struct llist_head to use as a loop counter. + * @head: the head for your llist. + */ +#define llist_for_each(pos, head) \ + for (pos = (head)->next, prefetch(pos->next); pos != (head); \ + pos = pos->next, prefetch(pos->next)) + +/** + * __llist_for_each - iterate over a llist + * @pos: the &struct llist_head to use as a loop counter. + * @head: the head for your llist. + * + * This variant differs from llist_for_each() in that it's the + * simplest possible llist iteration code, no prefetching is done. + * Use this for code that knows the llist to be very short (empty + * or 1 entry) most of the time. + */ +#define __llist_for_each(pos, head) \ + for (pos = (head)->next; pos != (head); pos = pos->next) + +/** + * llist_for_each_prev - iterate over a llist backwards + * @pos: the &struct llist_head to use as a loop counter. + * @head: the head for your llist. + */ +#define llist_for_each_prev(pos, head) \ + for (pos = (head)->prev, prefetch(pos->prev); pos != (head); \ + pos = pos->prev, prefetch(pos->prev)) + +/** + * llist_for_each_safe - iterate over a llist safe against removal of llist entry + * @pos: the &struct llist_head to use as a loop counter. + * @n: another &struct llist_head to use as temporary storage + * @head: the head for your llist. + */ +#define llist_for_each_safe(pos, n, head) \ + for (pos = (head)->next, n = pos->next; pos != (head); \ + pos = n, n = pos->next) + +/** + * llist_for_each_entry - iterate over llist of given type + * @pos: the type * to use as a loop counter. + * @head: the head for your llist. + * @member: the name of the llist_struct within the struct. + */ +#define llist_for_each_entry(pos, head, member) \ + for (pos = llist_entry((head)->next, typeof(*pos), member), \ + prefetch(pos->member.next); \ + &pos->member != (head); \ + pos = llist_entry(pos->member.next, typeof(*pos), member), \ + prefetch(pos->member.next)) + +/** + * llist_for_each_entry_reverse - iterate backwards over llist of given type. + * @pos: the type * to use as a loop counter. + * @head: the head for your llist. + * @member: the name of the llist_struct within the struct. + */ +#define llist_for_each_entry_reverse(pos, head, member) \ + for (pos = llist_entry((head)->prev, typeof(*pos), member), \ + prefetch(pos->member.prev); \ + &pos->member != (head); \ + pos = llist_entry(pos->member.prev, typeof(*pos), member), \ + prefetch(pos->member.prev)) + +/** + * llist_for_each_entry_continue - iterate over llist of given type + * continuing after existing point + * @pos: the type * to use as a loop counter. + * @head: the head for your llist. + * @member: the name of the llist_struct within the struct. + */ +#define llist_for_each_entry_continue(pos, head, member) \ + for (pos = llist_entry(pos->member.next, typeof(*pos), member), \ + prefetch(pos->member.next); \ + &pos->member != (head); \ + pos = llist_entry(pos->member.next, typeof(*pos), member), \ + prefetch(pos->member.next)) + +/** + * llist_for_each_entry_safe - iterate over llist of given type safe against removal of llist entry + * @pos: the type * to use as a loop counter. + * @n: another type * to use as temporary storage + * @head: the head for your llist. + * @member: the name of the llist_struct within the struct. + */ +#define llist_for_each_entry_safe(pos, n, head, member) \ + for (pos = llist_entry((head)->next, typeof(*pos), member), \ + n = llist_entry(pos->member.next, typeof(*pos), member); \ + &pos->member != (head); \ + pos = n, n = llist_entry(n->member.next, typeof(*n), member)) + +/** + * llist_for_each_rcu - iterate over an rcu-protected llist + * @pos: the &struct llist_head to use as a loop counter. + * @head: the head for your llist. + */ +#define llist_for_each_rcu(pos, head) \ + for (pos = (head)->next, prefetch(pos->next); pos != (head); \ + pos = pos->next, ({ smp_read_barrier_depends(); 0;}), prefetch(pos->next)) + +#define __llist_for_each_rcu(pos, head) \ + for (pos = (head)->next; pos != (head); \ + pos = pos->next, ({ smp_read_barrier_depends(); 0;})) + +/** + * llist_for_each_safe_rcu - iterate over an rcu-protected llist safe + * against removal of llist entry + * @pos: the &struct llist_head to use as a loop counter. + * @n: another &struct llist_head to use as temporary storage + * @head: the head for your llist. + */ +#define llist_for_each_safe_rcu(pos, n, head) \ + for (pos = (head)->next, n = pos->next; pos != (head); \ + pos = n, ({ smp_read_barrier_depends(); 0;}), n = pos->next) + +/** + * llist_for_each_entry_rcu - iterate over rcu llist of given type + * @pos: the type * to use as a loop counter. + * @head: the head for your llist. + * @member: the name of the llist_struct within the struct. + */ +#define llist_for_each_entry_rcu(pos, head, member) \ + for (pos = llist_entry((head)->next, typeof(*pos), member), \ + prefetch(pos->member.next); \ + &pos->member != (head); \ + pos = llist_entry(pos->member.next, typeof(*pos), member), \ + ({ smp_read_barrier_depends(); 0;}), \ + prefetch(pos->member.next)) + + +/** + * llist_for_each_continue_rcu - iterate over an rcu-protected llist + * continuing after existing point. + * @pos: the &struct llist_head to use as a loop counter. + * @head: the head for your llist. + */ +#define llist_for_each_continue_rcu(pos, head) \ + for ((pos) = (pos)->next, prefetch((pos)->next); (pos) != (head); \ + (pos) = (pos)->next, ({ smp_read_barrier_depends(); 0;}), prefetch((pos)->next)) + + +#endif diff --git a/misc/tools/osmocon/linuxrbtree.h b/misc/tools/osmocon/linuxrbtree.h new file mode 100644 index 000000000..079f440d0 --- /dev/null +++ b/misc/tools/osmocon/linuxrbtree.h @@ -0,0 +1,160 @@ +/* + Red Black Trees + (C) 1999 Andrea Arcangeli + + 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + + linux/include/linux/rbtree.h + + To use rbtrees you'll have to implement your own insert and search cores. + This will avoid us to use callbacks and to drop drammatically performances. + I know it's not the cleaner way, but in C (not in C++) to get + performances and genericity... + + Some example of insert and search follows here. The search is a plain + normal search over an ordered tree. The insert instead must be implemented + int two steps: as first thing the code must insert the element in + order as a red leaf in the tree, then the support library function + rb_insert_color() must be called. Such function will do the + not trivial work to rebalance the rbtree if necessary. + +----------------------------------------------------------------------- +static inline struct page * rb_search_page_cache(struct inode * inode, + unsigned long offset) +{ + struct rb_node * n = inode->i_rb_page_cache.rb_node; + struct page * page; + + while (n) + { + page = rb_entry(n, struct page, rb_page_cache); + + if (offset < page->offset) + n = n->rb_left; + else if (offset > page->offset) + n = n->rb_right; + else + return page; + } + return NULL; +} + +static inline struct page * __rb_insert_page_cache(struct inode * inode, + unsigned long offset, + struct rb_node * node) +{ + struct rb_node ** p = &inode->i_rb_page_cache.rb_node; + struct rb_node * parent = NULL; + struct page * page; + + while (*p) + { + parent = *p; + page = rb_entry(parent, struct page, rb_page_cache); + + if (offset < page->offset) + p = &(*p)->rb_left; + else if (offset > page->offset) + p = &(*p)->rb_right; + else + return page; + } + + rb_link_node(node, parent, p); + + return NULL; +} + +static inline struct page * rb_insert_page_cache(struct inode * inode, + unsigned long offset, + struct rb_node * node) +{ + struct page * ret; + if ((ret = __rb_insert_page_cache(inode, offset, node))) + goto out; + rb_insert_color(node, &inode->i_rb_page_cache); + out: + return ret; +} +----------------------------------------------------------------------- +*/ + +#ifndef _LINUX_RBTREE_H +#define _LINUX_RBTREE_H + +#include + +struct rb_node +{ + unsigned long rb_parent_color; +#define RB_RED 0 +#define RB_BLACK 1 + struct rb_node *rb_right; + struct rb_node *rb_left; +} __attribute__((aligned(sizeof(long)))); + /* The alignment might seem pointless, but allegedly CRIS needs it */ + +struct rb_root +{ + struct rb_node *rb_node; +}; + + +#define rb_parent(r) ((struct rb_node *)((r)->rb_parent_color & ~3)) +#define rb_color(r) ((r)->rb_parent_color & 1) +#define rb_is_red(r) (!rb_color(r)) +#define rb_is_black(r) rb_color(r) +#define rb_set_red(r) do { (r)->rb_parent_color &= ~1; } while (0) +#define rb_set_black(r) do { (r)->rb_parent_color |= 1; } while (0) + +static inline void rb_set_parent(struct rb_node *rb, struct rb_node *p) +{ + rb->rb_parent_color = (rb->rb_parent_color & 3) | (unsigned long)p; +} +static inline void rb_set_color(struct rb_node *rb, int color) +{ + rb->rb_parent_color = (rb->rb_parent_color & ~1) | color; +} + +#define RB_ROOT { NULL, } +#define rb_entry(ptr, type, member) container_of(ptr, type, member) + +#define RB_EMPTY_ROOT(root) ((root)->rb_node == NULL) +#define RB_EMPTY_NODE(node) (rb_parent(node) == node) +#define RB_CLEAR_NODE(node) (rb_set_parent(node, node)) + +extern void rb_insert_color(struct rb_node *, struct rb_root *); +extern void rb_erase(struct rb_node *, struct rb_root *); + +/* Find logical next and previous nodes in a tree */ +extern struct rb_node *rb_next(const struct rb_node *); +extern struct rb_node *rb_prev(const struct rb_node *); +extern struct rb_node *rb_first(const struct rb_root *); +extern struct rb_node *rb_last(const struct rb_root *); + +/* Fast replacement of a single node without remove/rebalance/add/rebalance */ +extern void rb_replace_node(struct rb_node *victim, struct rb_node *_new, + struct rb_root *root); + +static inline void rb_link_node(struct rb_node * node, struct rb_node * parent, + struct rb_node ** rb_link) +{ + node->rb_parent_color = (unsigned long )parent; + node->rb_left = node->rb_right = NULL; + + *rb_link = node; +} + +#endif /* _LINUX_RBTREE_H */ diff --git a/misc/tools/osmocon/msgb.c b/misc/tools/osmocon/msgb.c new file mode 100644 index 000000000..e25366e5c --- /dev/null +++ b/misc/tools/osmocon/msgb.c @@ -0,0 +1,154 @@ +/* (C) 2008 by Harald Welte + * (C) 2010 by Holger Hans Peter Freyther + * All Rights Reserved + * + * 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. + * + */ + +/*! \addtogroup msgb + * @{ + */ + +/*! \file msgb.c + */ + +#include +#include +#include + +#include "msgb.h" +#include "talloc.h" + +void *tall_msgb_ctx; + +/*! \brief Allocate a new message buffer + * \param[in] size Length in octets, including headroom + * \param[in] name Human-readable name to be associated with msgb + * + * This function allocates a 'struct msgb' as well as the underlying + * memory buffer for the actual message data (size specified by \a size) + * using the talloc memory context previously set by \ref msgb_set_talloc_ctx + */ +struct msgb *msgb_alloc(uint16_t size, const char *name) +{ + struct msgb *msg; + + msg = _talloc_zero(tall_msgb_ctx, sizeof(*msg) + size, name); + + if (!msg) { + //LOGP(DRSL, LOGL_FATAL, "unable to allocate msgb\n"); + return NULL; + } + + msg->data_len = size; + msg->len = 0; + msg->data = msg->_data; + msg->head = msg->_data; + msg->tail = msg->_data; + + return msg; +} + +/*! \brief Release given message buffer + * \param[in] m Message buffer to be free'd + */ +void msgb_free(struct msgb *m) +{ + talloc_free(m); +} + +/*! \brief Enqueue message buffer to tail of a queue + * \param[in] queue linked list header of queue + * \param[in] msgb message buffer to be added to the queue + * + * The function will append the specified message buffer \a msg to the + * queue implemented by \ref llist_head \a queue + */ +void msgb_enqueue(struct llist_head *queue, struct msgb *msg) +{ + llist_add_tail(&msg->list, queue); +} + +/*! \brief Dequeue message buffer from head of queue + * \param[in] queue linked list header of queue + * \returns message buffer (if any) or NULL if queue empty + * + * The function will remove the first message buffer from the queue + * implemented by 'ref llist_head \a queue. + */ +struct msgb *msgb_dequeue(struct llist_head *queue) +{ + struct llist_head *lh; + + if (llist_empty(queue)) + return NULL; + + lh = queue->next; + llist_del(lh); + + return llist_entry(lh, struct msgb, list); +} + +/*! \brief Re-set all message buffer pointers + * \param[in] m message buffer that is to be resetted + * + * This will re-set the various internal pointers into the underlying + * message buffer, i.e. remvoe all headroom and treat the msgb as + * completely empty. It also initializes the control buffer to zero. + */ +void msgb_reset(struct msgb *msg) +{ + msg->len = 0; + msg->data = msg->_data; + msg->head = msg->_data; + msg->tail = msg->_data; + + msg->trx = NULL; + msg->lchan = NULL; + msg->l2h = NULL; + msg->l3h = NULL; + msg->l4h = NULL; + + memset(&msg->cb, 0, sizeof(msg->cb)); +} + +/*! \brief get pointer to data section of message buffer + * \param[in] msg message buffer + * \returns pointer to data section of message buffer + */ +uint8_t *msgb_data(const struct msgb *msg) +{ + return msg->data; +} + +/*! \brief get length of message buffer + * \param[in] msg message buffer + * \returns length of data section in message buffer + */ +uint16_t msgb_length(const struct msgb *msg) +{ + return msg->len; +} + +/*! \brief Set the talloc context for \ref msgb_alloc + * \param[in] ctx talloc context to be used as root for msgb allocations + */ +void msgb_set_talloc_ctx(void *ctx) +{ + tall_msgb_ctx = ctx; +} + +/*! @} */ diff --git a/misc/tools/osmocon/msgb.h b/misc/tools/osmocon/msgb.h new file mode 100644 index 000000000..7b044384d --- /dev/null +++ b/misc/tools/osmocon/msgb.h @@ -0,0 +1,401 @@ +#ifndef _MSGB_H +#define _MSGB_H + +/* (C) 2008 by Harald Welte + * All Rights Reserved + * + * 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 "linuxlist.h" +#include "utils.h" + +/*! \defgroup msgb Message buffers + * @{ + */ + +/*! \file msgb.h + * \brief Osmocom message buffers + * The Osmocom message buffers are modelled after the 'struct skb' + * inside the Linux kernel network stack. As they exist in userspace, + * they are much simplified. However, terminology such as headroom, + * tailroom, push/pull/put etc. remains the same. + */ + +#define MSGB_DEBUG + +/*! \brief Osmocom message buffer */ +struct msgb { + struct llist_head list; /*!< \brief linked list header */ + + + /* Part of which TRX logical channel we were received / transmitted */ + /* FIXME: move them into the control buffer */ + union { + void *dst; /*!< \brief reference of origin/destination */ + struct gsm_bts_trx *trx; + }; + struct gsm_lchan *lchan; /*!< \brief logical channel */ + + unsigned char *l1h; /*!< \brief pointer to Layer1 header (if any) */ + unsigned char *l2h; /*!< \brief pointer to A-bis layer 2 header: OML, RSL(RLL), NS */ + unsigned char *l3h; /*!< \brief pointer to Layer 3 header. For OML: FOM; RSL: 04.08; GPRS: BSSGP */ + unsigned char *l4h; /*!< \brief pointer to layer 4 header */ + + unsigned long cb[5]; /*!< \brief control buffer */ + + uint16_t data_len; /*!< \brief length of underlying data array */ + uint16_t len; /*!< \brief length of bytes used in msgb */ + + unsigned char *head; /*!< \brief start of underlying memory buffer */ + unsigned char *tail; /*!< \brief end of message in buffer */ + unsigned char *data; /*!< \brief start of message in buffer */ + unsigned char _data[0]; /*!< \brief optional immediate data array */ +}; + +extern struct msgb *msgb_alloc(uint16_t size, const char *name); +extern void msgb_free(struct msgb *m); +extern void msgb_enqueue(struct llist_head *queue, struct msgb *msg); +extern struct msgb *msgb_dequeue(struct llist_head *queue); +extern void msgb_reset(struct msgb *m); +uint16_t msgb_length(const struct msgb *msg); + +#ifdef MSGB_DEBUG +#include "panic.h" +#define MSGB_ABORT(msg, fmt, args ...) do { \ + osmo_panic("msgb(%p): " fmt, msg, ## args); \ + } while(0) +#else +#define MSGB_ABORT(msg, fmt, args ...) +#endif + +/*! \brief obtain L1 header of msgb */ +#define msgb_l1(m) ((void *)(m->l1h)) +/*! \brief obtain L2 header of msgb */ +#define msgb_l2(m) ((void *)(m->l2h)) +/*! \brief obtain L3 header of msgb */ +#define msgb_l3(m) ((void *)(m->l3h)) +/*! \brief obtain SMS header of msgb */ +#define msgb_sms(m) ((void *)(m->l4h)) + +/*! \brief determine length of L1 message + * \param[in] msgb message buffer + * \returns size of L1 message in bytes + * + * This function computes the number of bytes between the tail of the + * message and the layer 1 header. + */ +static inline unsigned int msgb_l1len(const struct msgb *msgb) +{ + return msgb->tail - (uint8_t *)msgb_l1(msgb); +} + +/*! \brief determine length of L2 message + * \param[in] msgb message buffer + * \returns size of L2 message in bytes + * + * This function computes the number of bytes between the tail of the + * message and the layer 2 header. + */ +static inline unsigned int msgb_l2len(const struct msgb *msgb) +{ + return msgb->tail - (uint8_t *)msgb_l2(msgb); +} + +/*! \brief determine length of L3 message + * \param[in] msgb message buffer + * \returns size of L3 message in bytes + * + * This function computes the number of bytes between the tail of the + * message and the layer 3 header. + */ +static inline unsigned int msgb_l3len(const struct msgb *msgb) +{ + return msgb->tail - (uint8_t *)msgb_l3(msgb); +} + +/*! \brief determine the length of the header + * \param[in] msgb message buffer + * \returns number of bytes between start of buffer and start of msg + * + * This function computes the length difference between the underlying + * data buffer and the used section of the \a msgb. + */ +static inline unsigned int msgb_headlen(const struct msgb *msgb) +{ + return msgb->len - msgb->data_len; +} + +/*! \brief determine how much tail room is left in msgb + * \param[in] msgb message buffer + * \returns number of bytes remaining at end of msgb + * + * This function computes the amount of octets left in the underlying + * data buffer after the end of the message. + */ +static inline int msgb_tailroom(const struct msgb *msgb) +{ + return (msgb->head + msgb->data_len) - msgb->tail; +} + +/*! \brief determine the amount of headroom in msgb + * \param[in] msgb message buffer + * \returns number of bytes left ahead of message start in msgb + * + * This function computes the amount of bytes left in the underlying + * data buffer before the start of the actual message. + */ +static inline int msgb_headroom(const struct msgb *msgb) +{ + return (msgb->data - msgb->head); +} + +/*! \brief append data to end of message buffer + * \param[in] msgb message buffer + * \param[in] len number of bytes to append to message + * \returns pointer to start of newly-appended data + * + * This function will move the \a tail pointer of the message buffer \a + * len bytes further, thus enlarging the message by \a len bytes. + * + * The return value is a pointer to start of the newly added section at + * the end of the message and can be used for actually filling/copying + * data into it. + */ +static inline unsigned char *msgb_put(struct msgb *msgb, unsigned int len) +{ + unsigned char *tmp = msgb->tail; + if (msgb_tailroom(msgb) < (int) len) + MSGB_ABORT(msgb, "Not enough tailroom msgb_push (%u < %u)\n", + msgb_tailroom(msgb), len); + msgb->tail += len; + msgb->len += len; + return tmp; +} + +/*! \brief append a uint8 value to the end of the message + * \param[in] msgb message buffer + * \param[in] word unsigned 8bit byte to be appended + */ +static inline void msgb_put_u8(struct msgb *msgb, uint8_t word) +{ + uint8_t *space = msgb_put(msgb, 1); + space[0] = word & 0xFF; +} + +/*! \brief append a uint16 value to the end of the message + * \param[in] msgb message buffer + * \param[in] word unsigned 16bit byte to be appended + */ +static inline void msgb_put_u16(struct msgb *msgb, uint16_t word) +{ + uint8_t *space = msgb_put(msgb, 2); + space[0] = word >> 8 & 0xFF; + space[1] = word & 0xFF; +} + +/*! \brief append a uint32 value to the end of the message + * \param[in] msgb message buffer + * \param[in] word unsigned 32bit byte to be appended + */ +static inline void msgb_put_u32(struct msgb *msgb, uint32_t word) +{ + uint8_t *space = msgb_put(msgb, 4); + space[0] = word >> 24 & 0xFF; + space[1] = word >> 16 & 0xFF; + space[2] = word >> 8 & 0xFF; + space[3] = word & 0xFF; +} + +/*! \brief remove data from end of message + * \param[in] msgb message buffer + * \param[in] len number of bytes to remove from end + */ +static inline unsigned char *msgb_get(struct msgb *msgb, unsigned int len) +{ + unsigned char *tmp = msgb->data - len; + if (msgb_length(msgb) < len) + MSGB_ABORT(msgb, "msgb too small to get %u (len %u)\n", + len, msgb_length(msgb)); + msgb->tail -= len; + msgb->len -= len; + return tmp; +} +/*! \brief remove uint8 from end of message + * \param[in] msgb message buffer + * \returns 8bit value taken from end of msgb + */ +static inline uint8_t msgb_get_u8(struct msgb *msgb) +{ + uint8_t *space = msgb_get(msgb, 1); + return space[0]; +} +/*! \brief remove uint16 from end of message + * \param[in] msgb message buffer + * \returns 16bit value taken from end of msgb + */ +static inline uint16_t msgb_get_u16(struct msgb *msgb) +{ + uint8_t *space = msgb_get(msgb, 2); + return space[0] << 8 | space[1]; +} +/*! \brief remove uint32 from end of message + * \param[in] msgb message buffer + * \returns 32bit value taken from end of msgb + */ +static inline uint32_t msgb_get_u32(struct msgb *msgb) +{ + uint8_t *space = msgb_get(msgb, 4); + return space[0] << 24 | space[1] << 16 | space[2] << 8 | space[3]; +} + +/*! \brief prepend (push) some data to start of message + * \param[in] msgb message buffer + * \param[in] len number of bytes to pre-pend + * \returns pointer to newly added portion at start of \a msgb + * + * This function moves the \a data pointer of the \ref msgb further + * to the front (by \a len bytes), thereby enlarging the message by \a + * len bytes. + * + * The return value is a pointer to the newly added section in the + * beginning of the message. It can be used to fill/copy data into it. + */ +static inline unsigned char *msgb_push(struct msgb *msgb, unsigned int len) +{ + if (msgb_headroom(msgb) < (int) len) + MSGB_ABORT(msgb, "Not enough headroom msgb_push (%u < %u)\n", + msgb_headroom(msgb), len); + msgb->data -= len; + msgb->len += len; + return msgb->data; +} +/*! \brief remove (pull) a header from the front of the message buffer + * \param[in] msgb message buffer + * \param[in] len number of octets to be pulled + * \returns pointer to new start of msgb + * + * This function moves the \a data pointer of the \ref msgb further back + * in the message, thereby shrinking the size of the message by \a len + * bytes. + */ +static inline unsigned char *msgb_pull(struct msgb *msgb, unsigned int len) +{ + msgb->len -= len; + return msgb->data += len; +} + +/*! \brief remove uint8 from front of message + * \param[in] msgb message buffer + * \returns 8bit value taken from end of msgb + */ +static inline uint8_t msgb_pull_u8(struct msgb *msgb) +{ + uint8_t *space = msgb_pull(msgb, 1) - 1; + return space[0]; +} +/*! \brief remove uint16 from front of message + * \param[in] msgb message buffer + * \returns 16bit value taken from end of msgb + */ +static inline uint16_t msgb_pull_u16(struct msgb *msgb) +{ + uint8_t *space = msgb_pull(msgb, 2) - 2; + return space[0] << 8 | space[1]; +} +/*! \brief remove uint32 from front of message + * \param[in] msgb message buffer + * \returns 32bit value taken from end of msgb + */ +static inline uint32_t msgb_pull_u32(struct msgb *msgb) +{ + uint8_t *space = msgb_pull(msgb, 4) - 4; + return space[0] << 24 | space[1] << 16 | space[2] << 8 | space[3]; +} + +/*! \brief Increase headroom of empty msgb, reducing the tailroom + * \param[in] msg message buffer + * \param[in] len amount of extra octets to be reserved as headroom + * + * This function reserves some memory at the beginning of the underlying + * data buffer. The idea is to reserve space in case further headers + * have to be pushed to the \ref msgb during further processing. + * + * Calling this function leads to undefined reusults if it is called on + * a non-empty \ref msgb. + */ +static inline void msgb_reserve(struct msgb *msg, int len) +{ + msg->data += len; + msg->tail += len; +} + +/*! \brief Trim the msgb to a given absolute length + * \param[in] msg message buffer + * \param[in] len new total length of buffer + * \returns 0 in case of success, negative in case of error + */ +static inline int msgb_trim(struct msgb *msg, int len) +{ + if (len > msg->data_len) + return -1; + + msg->len = len; + msg->tail = msg->data + len; + + return 0; +} + +/*! \brief Trim the msgb to a given layer3 length + * \pram[in] msg message buffer + * \param[in] l3len new layer3 length + * \returns 0 in case of success, negative in case of error + */ +static inline int msgb_l3trim(struct msgb *msg, int l3len) +{ + return msgb_trim(msg, (msg->l3h - msg->data) + l3len); +} + +/*! \brief Allocate message buffer with specified headroom + * \param[in] size size in bytes, including headroom + * \param[in] headroom headroom in bytes + * \param[in] name human-readable name + * \returns allocated message buffer with specified headroom + * + * This function is a convenience wrapper around \ref msgb_alloc + * followed by \ref msgb_reserve in order to create a new \ref msgb with + * user-specified amount of headroom. + */ +static inline struct msgb *msgb_alloc_headroom(int size, int headroom, + const char *name) +{ + osmo_static_assert(size > headroom, headroom_bigger); + + struct msgb *msg = msgb_alloc(size, name); + if (msg) + msgb_reserve(msg, headroom); + return msg; +} + +/* non inline functions to ease binding */ + +uint8_t *msgb_data(const struct msgb *msg); +void msgb_set_talloc_ctx(void *ctx); + +/*! @} */ + +#endif /* _MSGB_H */ diff --git a/misc/tools/osmocon/osmocon.c b/misc/tools/osmocon/osmocon.c new file mode 100644 index 000000000..3dd738928 --- /dev/null +++ b/misc/tools/osmocon/osmocon.c @@ -0,0 +1,1521 @@ +/* osmocon */ + +/* (C) 2010 by Harald Welte + * (C) 2010 by Holger Hans Peter Freyther + * (C) 2010 by Steve Markgraf + * + * All Rights Reserved + * + * 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 +#include +#include +#include +#include + +#include + +#include "linuxlist.h" +#include "select.h" +#include "serial.h" +#include "talloc.h" +#include "timer.h" + +#include + +#define MODEM_BAUDRATE B115200 +#define MAX_DNLOAD_SIZE 0xFFFF +#define MAX_HDR_SIZE 128 +#define MAGIC_OFFSET 0x3be2 + +#define DEFAULT_BEACON_INTERVAL 50000 +#define ROMLOAD_INIT_BAUDRATE B19200 +#define ROMLOAD_DL_BAUDRATE B115200 +#define ROMLOAD_BLOCK_HDR_LEN 10 +#define ROMLOAD_ADDRESS 0x820000 + +#define MTK_INIT_BAUDRATE B19200 +#define MTK_ADDRESS 0x40001400 +#define MTK_BLOCK_SIZE 1024 + +#define PACKAGE_VERSION "NuttX-Special" + +struct tool_server *tool_server_for_dlci[256]; + +/** + * a connection from some other tool + */ +struct tool_connection { + struct tool_server *server; + struct llist_head entry; + struct osmo_fd fd; +}; + +/** + * server for a tool + */ +struct tool_server { + struct osmo_fd bfd; + uint8_t dlci; + struct llist_head connections; +}; + + +enum dnload_state { + WAITING_PROMPT1, + WAITING_PROMPT2, + DOWNLOADING, +}; + +enum romload_state { + WAITING_IDENTIFICATION, + WAITING_PARAM_ACK, + SENDING_BLOCKS, + SENDING_LAST_BLOCK, + LAST_BLOCK_SENT, + WAITING_BLOCK_ACK, + WAITING_CHECKSUM_ACK, + WAITING_BRANCH_ACK, + FINISHED, +}; + +enum mtk_state { + MTK_INIT_1, + MTK_INIT_2, + MTK_INIT_3, + MTK_INIT_4, + MTK_WAIT_WRITE_ACK, + MTK_WAIT_ADDR_ACK, + MTK_WAIT_SIZE_ACK, + MTK_SENDING_BLOCKS, + MTK_WAIT_BRANCH_CMD_ACK, + MTK_WAIT_BRANCH_ADDR_ACK, + MTK_FINISHED, +}; + +enum dnload_mode { + MODE_C123, + MODE_C123xor, + MODE_C140, + MODE_C140xor, + MODE_C155, + MODE_ROMLOAD, + MODE_MTK, + MODE_INVALID, +}; + +struct dnload { + enum dnload_state state; + enum romload_state romload_state; + enum mtk_state mtk_state; + enum dnload_mode mode, previous_mode; + struct osmo_fd serial_fd; + char *filename; + + int expect_hdlc; + int do_chainload; + + int dump_rx; + int dump_tx; + int beacon_interval; + + /* data to be downloaded */ + uint8_t *data; + int data_len; + + uint8_t *write_ptr; + + /* romload: block to be downloaded */ + uint8_t *block; + int block_len; + uint8_t block_number; + uint16_t block_payload_size; + int romload_dl_checksum; + uint8_t *block_ptr; + uint8_t load_address[4]; + + uint8_t mtk_send_size[4]; + int block_count; + int echo_bytecount; + + struct tool_server layer2_server; + struct tool_server loader_server; +}; + + +static struct dnload dnload; +static struct osmo_timer_list tick_timer; + +/* Compal ramloader specific */ +static const uint8_t phone_prompt1[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x01, 0x40 }; +static const uint8_t dnload_cmd[] = { 0x1b, 0xf6, 0x02, 0x00, 0x52, 0x01, 0x53 }; +static const uint8_t phone_prompt2[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x02, 0x43 }; +static const uint8_t phone_ack[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x03, 0x42 }; +static const uint8_t phone_nack_magic[]= { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x03, 0x57 }; +static const uint8_t phone_nack[] = { 0x1b, 0xf6, 0x02, 0x00, 0x45, 0x53, 0x16 }; +static const uint8_t ftmtool[] = { 0x66, 0x74, 0x6d, 0x74, 0x6f, 0x6f, 0x6c }; +static const uint8_t phone_magic[] = { 0x31, 0x30, 0x30, 0x33 }; /* "1003" */ + +/* The C123 has a hard-coded check inside the ramloader that requires the + * following bytes to be always the first four bytes of the image */ +static const uint8_t data_hdr_c123[] = { 0xee, 0x4c, 0x9f, 0x63 }; + +/* The C155 doesn't have some strange restriction on what the first four bytes + * have to be, but it starts the ramloader in THUMB mode. We use the following + * four bytes to switch back to ARM mode: + 800100: 4778 bx pc + 800102: 46c0 nop ; (mov r8, r8) + */ +static const uint8_t data_hdr_c155[] = { 0x78, 0x47, 0xc0, 0x46 }; + +/* small loader that enables the bootrom and executes the TI romloader: + * _start: + * ldr r1, =0x000a0000 + * wait: + * subs r1, r1, #1 + * bne wait + * ldr r1, =0xfffffb10 + * ldr r2, =0x100 + * strh r2, [r1] + * ldr pc, =0x0 + */ +static const uint8_t chainloader[] = { + 0x0a, 0x18, 0xa0, 0xe3, 0x01, 0x10, 0x51, 0xe2, 0xfd, 0xff, 0xff, + 0x1a, 0x08, 0x10, 0x9f, 0xe5, 0x01, 0x2c, 0xa0, 0xe3, 0xb0, 0x20, + 0xc1, 0xe1, 0x00, 0xf0, 0xa0, 0xe3, 0x10, 0xfb, 0xff, 0xff, +}; + +/* Calypso romloader specific */ +static const uint8_t romload_ident_cmd[] = { 0x3c, 0x69 }; /* i */ +static const uint8_t romload_param_ack[] = { 0x3e, 0x70 }; /* >p */ +static const uint8_t romload_param_nack[] = { 0x3e, 0x50 }; /* >P */ +static const uint8_t romload_block_ack[] = { 0x3e, 0x77 }; /* >w */ +static const uint8_t romload_block_nack[] = { 0x3e, 0x57 }; /* >W */ +static const uint8_t romload_checksum_ack[] = { 0x3e, 0x63 }; /* >c */ +static const uint8_t romload_checksum_nack[] = { 0x3e, 0x43 }; /* >C */ +static const uint8_t romload_branch_ack[] = { 0x3e, 0x62 }; /* >b */ +static const uint8_t romload_branch_nack[] = { 0x3e, 0x42 }; /* >B */ + +/* romload_param: {" MAX_DNLOAD_SIZE) && (dnload.mode != MODE_ROMLOAD)) { + fprintf(stderr, "The maximum file size is 64kBytes (%u bytes)\n", + MAX_DNLOAD_SIZE); + return -EFBIG; + } + } else { + st.st_size = sizeof(chainloader); + } + + free(dnload.data); + dnload.data = NULL; + + if (dnload.mode == MODE_C140 || dnload.mode == MODE_C140xor) { + if (st.st_size < (MAGIC_OFFSET + sizeof(phone_magic))) + payload_size = MAGIC_OFFSET + sizeof(phone_magic); + else { + printf("\nThe filesize is larger than 15kb, code on " + "the magic address will be overwritten!\nUse " + "loader.bin and upload the application with " + "osmoload instead!\n\n"); + payload_size = st.st_size; + } + } else + payload_size = st.st_size; + + dnload.data = malloc(MAX_HDR_SIZE + payload_size); + + if (!dnload.data) { + close(fd); + fprintf(stderr, "No memory\n"); + return -ENOMEM; + } + + /* copy in the header, if any */ + switch (dnload.mode) { + case MODE_C155: + hdr = data_hdr_c155; + hdr_len = sizeof(data_hdr_c155); + break; + case MODE_C140: + case MODE_C140xor: + case MODE_C123: + case MODE_C123xor: + hdr = data_hdr_c123; + hdr_len = sizeof(data_hdr_c123); + break; + case MODE_ROMLOAD: + break; + default: + break; + } + + if (hdr && hdr_len) + memcpy(dnload.data, hdr, hdr_len); + + /* 2 bytes for length + header */ + file_data = dnload.data + 2 + hdr_len; + + /* write the length, keep running XOR */ + tot_len = hdr_len + payload_size; + nibble = tot_len >> 8; + dnload.data[0] = nibble; + running_xor ^= nibble; + nibble = tot_len & 0xff; + dnload.data[1] = nibble; + running_xor ^= nibble; + + if (hdr_len && hdr) { + memcpy(dnload.data+2, hdr, hdr_len); + + for (i = 0; i < hdr_len; i++) + running_xor ^= hdr[i]; + } + + if (!chainload) { + rc = read(fd, file_data, st.st_size); + if (rc < 0) { + perror("error reading file\n"); + free(dnload.data); + dnload.data = NULL; + close(fd); + return -EIO; + } + if (rc < st.st_size) { + free(dnload.data); + dnload.data = NULL; + close(fd); + fprintf(stderr, "Short read of file (%d < %d)\n", + rc, (int)st.st_size); + return -EIO; + } + close(fd); + } else { + memcpy(file_data, chainloader, st.st_size); + } + + dnload.data_len = (file_data+payload_size) - dnload.data; + + /* fill memory between data end and magic, add magic */ + if(dnload.mode == MODE_C140 || dnload.mode == MODE_C140xor) { + if (st.st_size < MAGIC_OFFSET) + memset(file_data + st.st_size, 0x00, + payload_size - st.st_size); + memcpy(dnload.data + MAGIC_OFFSET, phone_magic, + sizeof(phone_magic)); + } + + /* calculate XOR sum */ + for (i = 0; i < payload_size; i++) + running_xor ^= file_data[i]; + + dnload.data[dnload.data_len++] = running_xor; + + /* initialize write pointer to start of data */ + dnload.write_ptr = dnload.data; + + printf("read_file(%s): file_size=%u, hdr_len=%u, dnload_len=%u\n", + chainload ? "chainloader" : filename, (int)st.st_size, + hdr_len, dnload.data_len); + + return 0; +} + +static void osmocon_osmo_hexdump(const uint8_t *data, unsigned int len) +{ + int n; + + for (n=0; n < len; n++) + printf("%02x ", data[n]); + printf(" "); + for (n=0; n < len; n++) + if (isprint(data[n])) + putchar(data[n]); + else + putchar('.'); + printf("\n"); +} + +static int romload_prepare_block(void) +{ + int i; + + int block_checksum = 5; + int remaining_bytes; + int fill_bytes; + uint8_t *block_data; + uint32_t block_address; + + dnload.block_len = ROMLOAD_BLOCK_HDR_LEN + dnload.block_payload_size; + + /* if first block, allocate memory */ + if (!dnload.block_number) { + dnload.block = malloc(dnload.block_len); + if (!dnload.block) { + fprintf(stderr, "No memory\n"); + return -ENOMEM; + } + dnload.romload_dl_checksum = 0; + /* initialize write pointer to start of data */ + dnload.write_ptr = dnload.data; + } + + block_address = ROMLOAD_ADDRESS + + (dnload.block_number * dnload.block_payload_size); + + /* prepare our block header (10 bytes) */ + memcpy(dnload.block, romload_write_cmd, sizeof(romload_write_cmd)); + dnload.block[2] = 0x01; /* block index */ + /* should normally be the block number, but hangs when sending !0x01 */ + dnload.block[3] = 0x01; /* dnload.block_number+1 */ + dnload.block[4] = (dnload.block_payload_size >> 8) & 0xff; + dnload.block[5] = dnload.block_payload_size & 0xff; + dnload.block[6] = (block_address >> 24) & 0xff; + dnload.block[7] = (block_address >> 16) & 0xff; + dnload.block[8] = (block_address >> 8) & 0xff; + dnload.block[9] = block_address & 0xff; + + block_data = dnload.block + ROMLOAD_BLOCK_HDR_LEN; + dnload.write_ptr = dnload.data + 2 + + (dnload.block_payload_size * dnload.block_number); + + remaining_bytes = dnload.data_len - 3 - + (dnload.block_payload_size * dnload.block_number); + + memcpy(block_data, dnload.write_ptr, dnload.block_payload_size); + + if (remaining_bytes <= dnload.block_payload_size) { + fill_bytes = (dnload.block_payload_size - remaining_bytes); + memset(block_data + remaining_bytes, 0x00, fill_bytes); + dnload.romload_state = SENDING_LAST_BLOCK; + } else { + dnload.romload_state = SENDING_BLOCKS; + } + + /* block checksum is lsb of ~(5 + block_size_lsb + all bytes of + * block_address + all data bytes) */ + for (i = 5; i < ROMLOAD_BLOCK_HDR_LEN + dnload.block_payload_size; i++) + block_checksum += dnload.block[i]; + + /* checksum is lsb of ~(sum of LSBs of all block checksums) */ + dnload.romload_dl_checksum += ~(block_checksum) & 0xff; + + /* initialize block pointer to start of block */ + dnload.block_ptr = dnload.block; + + dnload.block_number++; + dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE; + return 0; +} + +static int mtk_prepare_block(void) +{ + int rc, i; + + int remaining_bytes; + int fill_bytes; + uint8_t *block_data; + uint8_t tmp_byteswap; + uint32_t tmp_size; + + dnload.block_len = MTK_BLOCK_SIZE; + dnload.echo_bytecount = 0; + + /* if first block, allocate memory */ + if (!dnload.block_number) { + dnload.block = malloc(dnload.block_len); + if (!dnload.block) { + fprintf(stderr, "No memory\n"); + return -ENOMEM; + } + + /* calculate the number of blocks we need to send */ + dnload.block_count = (dnload.data_len-3) / MTK_BLOCK_SIZE; + /* add one more block if no multiple of blocksize */ + if((dnload.data_len-3) % MTK_BLOCK_SIZE) + dnload.block_count++; + + /* divide by 2, since we have to tell the mtk loader the size + * as count of uint16 (odd transfer sizes are not possible) */ + tmp_size = (dnload.block_count * MTK_BLOCK_SIZE)/2; + dnload.mtk_send_size[0] = (tmp_size >> 24) & 0xff; + dnload.mtk_send_size[1] = (tmp_size >> 16) & 0xff; + dnload.mtk_send_size[2] = (tmp_size >> 8) & 0xff; + dnload.mtk_send_size[3] = tmp_size & 0xff; + + /* initialize write pointer to start of data */ + dnload.write_ptr = dnload.data; + } + + block_data = dnload.block; + dnload.write_ptr = dnload.data + 2 + + (dnload.block_len * dnload.block_number); + + remaining_bytes = dnload.data_len - 3 - + (dnload.block_len * dnload.block_number); + + memcpy(block_data, dnload.write_ptr, MTK_BLOCK_SIZE); + + if (remaining_bytes <= MTK_BLOCK_SIZE) { + fill_bytes = (MTK_BLOCK_SIZE - remaining_bytes); + printf("Preparing the last block, filling %i bytes\n", + fill_bytes); + memset(block_data + remaining_bytes, 0x00, fill_bytes); + dnload.romload_state = SENDING_LAST_BLOCK; + } else { + dnload.romload_state = SENDING_BLOCKS; + printf("Preparing block %i\n", dnload.block_number+1); + } + + /* for the mtk romloader we need to swap MSB <-> LSB */ + for (i = 0; i < dnload.block_len; i += 2) { + tmp_byteswap = dnload.block[i]; + dnload.block[i] = dnload.block[i+1]; + dnload.block[i+1] = tmp_byteswap; + } + + /* initialize block pointer to start of block */ + dnload.block_ptr = dnload.block; + + dnload.block_number++; + return rc; +} + +static int handle_write_block(void) +{ + int bytes_left, write_len, rc; + int progress = 100 * (dnload.block_number * dnload.block_payload_size) + / dnload.data_len; + + if (dnload.block_ptr >= dnload.block + dnload.block_len) { + printf("Progress: %i%%\r", progress); + fflush(stdout); + dnload.write_ptr = dnload.data; + dnload.serial_fd.when &= ~BSC_FD_WRITE; + if (dnload.romload_state == SENDING_LAST_BLOCK) { + dnload.romload_state = LAST_BLOCK_SENT; + printf("Finished, sent %i blocks in total\n", + dnload.block_number); + } else { + dnload.romload_state = WAITING_BLOCK_ACK; + } + + return 0; + } + + /* try to write a maximum of block_len bytes */ + bytes_left = (dnload.block + dnload.block_len) - dnload.block_ptr; + write_len = dnload.block_len; + if (bytes_left < dnload.block_len) + write_len = bytes_left; + + rc = write(dnload.serial_fd.fd, dnload.block_ptr, write_len); + if (rc < 0) { + perror("Error during write"); + return rc; + } + + dnload.block_ptr += rc; + + return 0; +} + +#define WRITE_BLOCK 4096 + +static int handle_write_dnload(void) +{ + int bytes_left, write_len, rc; + uint8_t xor_init = 0x02; + + printf("handle_write(): "); + if (dnload.write_ptr == dnload.data) { + /* no bytes have been transferred yet */ + switch (dnload.mode) { + case MODE_C155: + case MODE_C140xor: + case MODE_C123xor: + rc = write(dnload.serial_fd.fd, &xor_init, 1); + break; + default: + break; + } + } else if (dnload.write_ptr >= dnload.data + dnload.data_len) { + printf("finished\n"); + dnload.write_ptr = dnload.data; + dnload.serial_fd.when &= ~BSC_FD_WRITE; + return 1; + } + + /* try to write a maximum of WRITE_BLOCK bytes */ + bytes_left = (dnload.data + dnload.data_len) - dnload.write_ptr; + write_len = WRITE_BLOCK; + if (bytes_left < WRITE_BLOCK) + write_len = bytes_left; + + rc = write(dnload.serial_fd.fd, dnload.write_ptr, write_len); + if (rc < 0) { + perror("Error during write"); + return rc; + } + + dnload.write_ptr += rc; + + printf("%u bytes (%tu/%u)\n", rc, dnload.write_ptr - dnload.data, + dnload.data_len); + + return 0; +} + +static int handle_sercomm_write(void) +{ + uint8_t c; + + if (sercomm_drv_pull(&c) != 0) { + if (write(dnload.serial_fd.fd, &c, 1) != 1) + perror("short write"); + } else + dnload.serial_fd.when &= ~BSC_FD_WRITE; + + return 0; +} + +static int handle_write(void) +{ + /* TODO: simplify this again (global state: downloading, sercomm) */ + switch (dnload.mode) { + case MODE_ROMLOAD: + switch (dnload.romload_state) { + case SENDING_BLOCKS: + case SENDING_LAST_BLOCK: + return handle_write_block(); + default: + return handle_sercomm_write(); + } + break; + case MODE_MTK: + switch (dnload.mtk_state) { + case MTK_SENDING_BLOCKS: + return handle_write_block(); + default: + return handle_sercomm_write(); + } + break; + default: + switch (dnload.state) { + case DOWNLOADING: + return handle_write_dnload(); + default: + return handle_sercomm_write(); + } + } + + return 0; +} + +static uint8_t buffer[sizeof(phone_prompt1)]; +static uint8_t *bufptr = buffer; + +static void hdlc_send_to_phone(uint8_t dlci, uint8_t *data, int len) +{ + struct msgb *msg; + uint8_t *dest; + + if(dnload.dump_tx) { + printf("hdlc_send(dlci=%u): ", dlci); + osmocon_osmo_hexdump(data, len); + } + + if (len > 512) { + fprintf(stderr, "Too much data to send. %u\n", len); + return; + } + + /* push the message into the stack */ + msg = sercomm_alloc_msgb(512); + if (!msg) { + fprintf(stderr, "Failed to create data for the frame.\n"); + return; + } + + /* copy the data */ + dest = msgb_put(msg, len); + memcpy(dest, data, len); + + sercomm_sendmsg(dlci, msg); + + dnload.serial_fd.when |= BSC_FD_WRITE; +} + +static void hdlc_console_cb(uint8_t dlci, struct msgb *msg) +{ + write(1, msg->data, msg->len); + msgb_free(msg); +} + +static void hdlc_tool_cb(uint8_t dlci, struct msgb *msg) +{ + struct tool_server *srv = tool_server_for_dlci[dlci]; + + if(dnload.dump_rx) { + printf("hdlc_recv(dlci=%u): ", dlci); + osmocon_osmo_hexdump(msg->data, msg->len); + } + + if(srv) { + struct tool_connection *con; + uint16_t *len; + + len = (uint16_t *) msgb_push(msg, 2); + *len = htons(msg->len - sizeof(*len)); + + llist_for_each_entry(con, &srv->connections, entry) { + if (write(con->fd.fd, msg->data, msg->len) != msg->len) { + fprintf(stderr, + "Failed to write msg to the socket..\n"); + continue; + } + } + } + + msgb_free(msg); +} + +static int handle_buffer(int buf_used_len) +{ + int nbytes, buf_left, i; + + buf_left = buf_used_len - (bufptr - buffer); + if (buf_left <= 0) { + memmove(buffer, buffer+1, buf_used_len-1); + bufptr -= 1; + buf_left = 1; + } + + nbytes = read(dnload.serial_fd.fd, bufptr, buf_left); + if (nbytes <= 0) + return nbytes; + + if (!dnload.expect_hdlc) { + printf("got %i bytes from modem, ", nbytes); + printf("data looks like: "); + osmocon_osmo_hexdump(bufptr, nbytes); + } else { + for (i = 0; i < nbytes; ++i) + if (sercomm_drv_rx_char(bufptr[i]) == 0) + printf("Dropping sample '%c'\n", bufptr[i]); + } + + return nbytes; +} + +/* Compal ramloader */ +static int handle_read(void) +{ + int rc, nbytes; + + nbytes = handle_buffer(sizeof(buffer)); + if (nbytes <= 0) + return nbytes; + + if (!memcmp(buffer, phone_prompt1, sizeof(phone_prompt1))) { + printf("Received PROMPT1 from phone, responding with CMD\n"); + dnload.expect_hdlc = 0; + dnload.state = WAITING_PROMPT2; + if(dnload.filename) { + rc = write(dnload.serial_fd.fd, dnload_cmd, sizeof(dnload_cmd)); + + /* re-read file */ + rc = read_file(dnload.filename, dnload.do_chainload); + if (rc < 0) { + fprintf(stderr, "read_file(%s) failed with %d\n", + dnload.filename, rc); + exit(1); + } + } + } else if (!memcmp(buffer, phone_prompt2, sizeof(phone_prompt2))) { + printf("Received PROMPT2 from phone, starting download\n"); + dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE; + dnload.state = DOWNLOADING; + } else if (!memcmp(buffer, phone_ack, sizeof(phone_ack))) { + printf("Received DOWNLOAD ACK from phone, your code is" + " running now!\n"); + dnload.serial_fd.when = BSC_FD_READ; + dnload.state = WAITING_PROMPT1; + dnload.write_ptr = dnload.data; + dnload.expect_hdlc = 1; + + /* check for romloader chainloading mode used as a workaround + * for the magic on the C139/C140 and J100i */ + if (dnload.do_chainload) { + printf("Enabled Compal ramloader -> Calypso romloader" + " chainloading mode\n"); + bufptr = buffer; + dnload.previous_mode = dnload.mode; + dnload.mode = MODE_ROMLOAD; + osmo_serial_set_baudrate(dnload.serial_fd.fd, ROMLOAD_INIT_BAUDRATE); + tick_timer.cb = &beacon_timer_cb; + tick_timer.data = &tick_timer; + osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval); + } + } else if (!memcmp(buffer, phone_nack, sizeof(phone_nack))) { + printf("Received DOWNLOAD NACK from phone, something went" + " wrong :(\n"); + dnload.serial_fd.when = BSC_FD_READ; + dnload.state = WAITING_PROMPT1; + dnload.write_ptr = dnload.data; + } else if (!memcmp(buffer, phone_nack_magic, sizeof(phone_nack_magic))) { + printf("Received MAGIC NACK from phone, you need to" + " have \"1003\" at 0x803ce0\n"); + dnload.serial_fd.when = BSC_FD_READ; + dnload.state = WAITING_PROMPT1; + dnload.write_ptr = dnload.data; + } else if (!memcmp(buffer, ftmtool, sizeof(ftmtool))) { + printf("Received FTMTOOL from phone, ramloader has aborted\n"); + dnload.serial_fd.when = BSC_FD_READ; + dnload.state = WAITING_PROMPT1; + dnload.write_ptr = dnload.data; + } + bufptr += nbytes; + + return nbytes; +} + +/* "Calypso non-secure romloader" */ +static int handle_read_romload(void) +{ + int rc, nbytes, buf_used_len; + + /* virtually limit buffer length for romloader, since responses + * are shorter and vary in length */ + + switch (dnload.romload_state) { + case WAITING_PARAM_ACK: + buf_used_len = 4; /* ">p" + uint16_t len */ + break; + case WAITING_CHECKSUM_ACK: + buf_used_len = 3; /* ">c" + uint8_t checksum */ + break; + case FINISHED: + buf_used_len = sizeof(buffer); + break; + default: + buf_used_len = 2; /* ">*" */ + } + + nbytes = handle_buffer(buf_used_len); + if (nbytes <= 0) + return nbytes; + + switch (dnload.romload_state) { + case WAITING_IDENTIFICATION: + if (memcmp(buffer, romload_ident_ack, + sizeof(romload_ident_ack))) + break; + + printf("Received ident ack from phone, sending " + "parameter sequence\n"); + dnload.expect_hdlc = 1; + dnload.romload_state = WAITING_PARAM_ACK; + rc = write(dnload.serial_fd.fd, romload_param, + sizeof(romload_param)); + /* re-read file */ + rc = read_file(dnload.filename, 0); + if (rc < 0) { + fprintf(stderr, "read_file(%s) failed with %d\n", + dnload.filename, rc); + exit(1); + } + break; + case WAITING_PARAM_ACK: + if (memcmp(buffer, romload_param_ack, + sizeof(romload_param_ack))) + break; + + printf("Received parameter ack from phone, " + "starting download\n"); + osmo_serial_set_baudrate(dnload.serial_fd.fd, ROMLOAD_DL_BAUDRATE); + + /* using the max blocksize the phone tells us */ + dnload.block_payload_size = ((buffer[3] << 8) + buffer[2]); + dnload.block_payload_size -= ROMLOAD_BLOCK_HDR_LEN; + dnload.romload_state = SENDING_BLOCKS; + dnload.block_number = 0; + romload_prepare_block(); + bufptr -= 2; + break; + case WAITING_BLOCK_ACK: + case LAST_BLOCK_SENT: + if (!memcmp(buffer, romload_block_ack, + sizeof(romload_block_ack))) { + if (dnload.romload_state == LAST_BLOCK_SENT) { + /* send the checksum */ + uint8_t final_checksum = + (~(dnload.romload_dl_checksum) & 0xff); + rc = write(dnload.serial_fd.fd, + romload_checksum_cmd, + sizeof(romload_checksum_cmd)); + rc = write(dnload.serial_fd.fd, + &final_checksum, 1); + dnload.romload_state = WAITING_CHECKSUM_ACK; + } else + romload_prepare_block(); + } else if (!memcmp(buffer, romload_block_nack, + sizeof(romload_block_nack))) { + printf("Received block nack from phone, " + "something went wrong, aborting\n"); + osmo_serial_set_baudrate(dnload.serial_fd.fd, ROMLOAD_INIT_BAUDRATE); + dnload.romload_state = WAITING_IDENTIFICATION; + osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval); + } + break; + case WAITING_CHECKSUM_ACK: + if (!memcmp(buffer, romload_checksum_ack, + sizeof(romload_checksum_ack))) { + + rc = write(dnload.serial_fd.fd, romload_branch_cmd, + sizeof(romload_branch_cmd)); + rc = write(dnload.serial_fd.fd, &dnload.load_address, + sizeof(dnload.load_address)); + dnload.romload_state = WAITING_BRANCH_ACK; + bufptr -= 1; + } else if (!memcmp(buffer, romload_checksum_nack, + sizeof(romload_checksum_nack))) { + printf("Checksum on phone side (0x%02x) doesn't " + "match ours, aborting\n", ~buffer[2]); + osmo_serial_set_baudrate(dnload.serial_fd.fd, ROMLOAD_INIT_BAUDRATE); + dnload.romload_state = WAITING_IDENTIFICATION; + osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval); + bufptr -= 1; + } + break; + case WAITING_BRANCH_ACK: + if (!memcmp(buffer, romload_branch_ack, + sizeof(romload_branch_ack))) { + printf("Received branch ack, your code is running now!\n"); + dnload.serial_fd.when = BSC_FD_READ; + dnload.romload_state = FINISHED; + dnload.write_ptr = dnload.data; + dnload.expect_hdlc = 1; + + if (!dnload.do_chainload) + break; + + /* if using chainloading mode, switch back to the Compal + * ramloader settings to make sure the auto-reload + * feature works */ + bufptr = buffer; + dnload.mode = dnload.previous_mode; + dnload.romload_state = WAITING_IDENTIFICATION; + osmo_serial_set_baudrate(dnload.serial_fd.fd, MODEM_BAUDRATE); + } else if (!memcmp(buffer, romload_branch_nack, + sizeof(romload_branch_nack))) { + printf("Received branch nack, aborting\n"); + osmo_serial_set_baudrate(dnload.serial_fd.fd, ROMLOAD_INIT_BAUDRATE); + dnload.romload_state = WAITING_IDENTIFICATION; + osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval); + } + break; + default: + break; + } + + bufptr += nbytes; + return nbytes; +} + +/* MTK romloader */ +static int handle_read_mtk(void) +{ + int rc, nbytes, buf_used_len; + + switch (dnload.mtk_state) { + case MTK_WAIT_ADDR_ACK: + case MTK_WAIT_SIZE_ACK: + case MTK_WAIT_BRANCH_ADDR_ACK: + buf_used_len = 4; + break; + case MTK_FINISHED: + buf_used_len = sizeof(buffer); + break; + default: + buf_used_len = 1; + } + + nbytes = handle_buffer(buf_used_len); + if (nbytes <= 0) + return nbytes; + + switch (dnload.mtk_state) { + case MTK_INIT_1: + if (!(buffer[0] == mtk_init_resp[0])) + break; + dnload.mtk_state = MTK_INIT_2; + printf("Received init magic byte 1\n"); + rc = write(dnload.serial_fd.fd, &mtk_init_cmd[1], 1); + break; + case MTK_INIT_2: + if (!(buffer[0] == mtk_init_resp[1])) + break; + dnload.mtk_state = MTK_INIT_3; + printf("Received init magic byte 2\n"); + rc = write(dnload.serial_fd.fd, &mtk_init_cmd[2], 1); + break; + case MTK_INIT_3: + if (!(buffer[0] == mtk_init_resp[2])) + break; + dnload.mtk_state = MTK_INIT_4; + printf("Received init magic byte 3\n"); + rc = write(dnload.serial_fd.fd, &mtk_init_cmd[3], 1); + break; + case MTK_INIT_4: + if (!(buffer[0] == mtk_init_resp[3])) + break; + dnload.mtk_state = MTK_WAIT_WRITE_ACK; + printf("Received init magic byte 4, requesting write\n"); + rc = write(dnload.serial_fd.fd, &mtk_command[0], 1); + break; + case MTK_WAIT_WRITE_ACK: + if (!(buffer[0] == mtk_command[0])) + break; + dnload.mtk_state = MTK_WAIT_ADDR_ACK; + printf("Received write ack, sending load address\n"); + + rc = write(dnload.serial_fd.fd, &dnload.load_address, + sizeof(dnload.load_address)); + break; + case MTK_WAIT_ADDR_ACK: + if (memcmp(buffer, dnload.load_address, + sizeof(dnload.load_address))) + break; + printf("Received address ack from phone, sending loadsize\n"); + /* re-read file */ + rc = read_file(dnload.filename, 0); + if (rc < 0) { + fprintf(stderr, "read_file(%s) failed with %d\n", + dnload.filename, rc); + exit(1); + } + dnload.block_number = 0; + mtk_prepare_block(); + dnload.mtk_state = MTK_WAIT_SIZE_ACK; + rc = write(dnload.serial_fd.fd, &dnload.mtk_send_size, + sizeof(dnload.mtk_send_size)); + break; + case MTK_WAIT_SIZE_ACK: + if (memcmp(buffer, dnload.mtk_send_size, + sizeof(dnload.mtk_send_size))) + break; + printf("Received size ack\n"); + dnload.expect_hdlc = 1; + dnload.mtk_state = MTK_SENDING_BLOCKS; + dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE; + bufptr -= 3; + break; + case MTK_SENDING_BLOCKS: + if (!(buffer[0] == dnload.block[dnload.echo_bytecount])) + printf("Warning: Byte %i of Block %i doesn't match," + " check your serial connection!\n", + dnload.echo_bytecount, dnload.block_number); + dnload.echo_bytecount++; + + if ((dnload.echo_bytecount+1) > MTK_BLOCK_SIZE) { + if ( dnload.block_number == dnload.block_count) { + rc = write(dnload.serial_fd.fd, + &mtk_command[3], 1); + printf("Sending branch command\n"); + dnload.expect_hdlc = 0; + dnload.mtk_state = MTK_WAIT_BRANCH_CMD_ACK; + break; + } + printf("Received Block %i preparing next block\n", + dnload.block_number); + mtk_prepare_block(); + dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE; + } + break; + case MTK_WAIT_BRANCH_CMD_ACK: + if (!(buffer[0] == mtk_command[3])) + break; + dnload.mtk_state = MTK_WAIT_BRANCH_ADDR_ACK; + printf("Received branch command ack, sending address\n"); + + rc = write(dnload.serial_fd.fd, &dnload.load_address, + sizeof(dnload.load_address)); + break; + case MTK_WAIT_BRANCH_ADDR_ACK: + if (memcmp(buffer, dnload.load_address, + sizeof(dnload.load_address))) + break; + printf("Received branch address ack, code should run now\n"); + osmo_serial_set_baudrate(dnload.serial_fd.fd, MODEM_BAUDRATE); + dnload.serial_fd.when = BSC_FD_READ; + dnload.mtk_state = MTK_FINISHED; + dnload.write_ptr = dnload.data; + dnload.expect_hdlc = 1; + break; + default: + break; + } + + bufptr += nbytes; + return nbytes; +} + +static int serial_read(struct osmo_fd *fd, unsigned int flags) +{ + int rc; + if (flags & BSC_FD_READ) { + switch (dnload.mode) { + case MODE_ROMLOAD: + rc = handle_read_romload(); + break; + case MODE_MTK: + rc = handle_read_mtk(); + break; + default: + rc = handle_read(); + break; + } + if (rc == 0) + exit(2); + } + + if (flags & BSC_FD_WRITE) { + rc = handle_write(); + if (rc == 1) + dnload.state = WAITING_PROMPT1; + } + return 0; +} + +static int parse_mode(const char *arg) +{ + if (!strcasecmp(arg, "c123")) + return MODE_C123; + else if (!strcasecmp(arg, "c123xor")) + return MODE_C123xor; + else if (!strcasecmp(arg, "c140")) + return MODE_C140; + else if (!strcasecmp(arg, "c140xor")) + return MODE_C140xor; + else if (!strcasecmp(arg, "c155")) + return MODE_C155; + else if (!strcasecmp(arg, "romload")) + return MODE_ROMLOAD; + else if (!strcasecmp(arg, "mtk")) + return MODE_MTK; + + return MODE_INVALID; +} + +#define HELP_TEXT \ + "[ -v | -h ] [ -d [t][r] ] [ -p /dev/ttyXXXX ]\n" \ + "\t\t [ -c ] (enable chainloading of highram-images)\n" \ + "\t\t [ -s /tmp/osmocom_l2 ]\n" \ + "\t\t [ -l /tmp/osmocom_loader ]\n" \ + "\t\t [ -m {c123,c123xor,c140,c140xor,c155,romload,mtk} ]\n" \ + "\t\t [ -i beacon-interval (mS) ]\n" \ + "\t\t file.bin\n\n" \ + "* Open serial port /dev/ttyXXXX (connected to your phone)\n" \ + "* Perform handshaking with the ramloader in the phone\n" \ + "* Download file.bin to the attached phone (base address 0x00800100)\n" + +static int usage(const char *name) +{ + printf("Usage: %s ", name); + printf(HELP_TEXT); + exit(2); +} + +static int version(const char *name) +{ + printf("%s version %s\n", name, PACKAGE_VERSION); + exit(2); +} + +static int un_tool_read(struct osmo_fd *fd, unsigned int flags) +{ + int rc, c; + uint16_t length = 0xffff; + uint8_t buf[4096]; + struct tool_connection *con = (struct tool_connection *)fd->data; + + c = 0; + while(c < 2) { + rc = read(fd->fd, &buf + c, 2 - c); + if(rc == 0) { + // disconnect + goto close; + } + if(rc < 0) { + if(errno == EAGAIN) { + continue; + } + fprintf(stderr, "Err from socket: %s\n", strerror(errno)); + goto close; + } + c += rc; + } + + memcpy(&length, buf, sizeof length); + length = ntohs(length); + + c = 0; + while(c < length) { + rc = read(fd->fd, &buf + c, length - c); + if(rc == 0) { + // disconnect + goto close; + } + if(rc < 0) { + if(errno == EAGAIN) { + continue; + } + fprintf(stderr, "Err from socket: %s\n", strerror(errno)); + goto close; + } + c += rc; + } + + hdlc_send_to_phone(con->server->dlci, buf, length); + + return 0; +close: + + close(fd->fd); + osmo_fd_unregister(fd); + llist_del(&con->entry); + talloc_free(con); + return -1; +} + +/* accept a new connection */ +static int tool_accept(struct osmo_fd *fd, unsigned int flags) +{ + struct tool_server *srv = (struct tool_server *)fd->data; + struct tool_connection *con; + struct sockaddr_un un_addr; + socklen_t len; + int rc; + + len = sizeof(un_addr); + rc = accept(fd->fd, (struct sockaddr *) &un_addr, &len); + if (rc < 0) { + fprintf(stderr, "Failed to accept a new connection.\n"); + return -1; + } + + con = talloc_zero(NULL, struct tool_connection); + if (!con) { + fprintf(stderr, "Failed to create tool connection.\n"); + return -1; + } + + con->server = srv; + + con->fd.fd = rc; + con->fd.when = BSC_FD_READ; + con->fd.cb = un_tool_read; + con->fd.data = con; + if (osmo_fd_register(&con->fd) != 0) { + fprintf(stderr, "Failed to register the fd.\n"); + return -1; + } + + llist_add(&con->entry, &srv->connections); + return 0; +} + +/* + * Register and start a tool server + */ +static int register_tool_server(struct tool_server *ts, + const char *path, + uint8_t dlci) +{ + struct osmo_fd *bfd = &ts->bfd; + struct sockaddr_un local; + unsigned int namelen; + int rc; + + bfd->fd = socket(AF_UNIX, SOCK_STREAM, 0); + + if (bfd->fd < 0) { + fprintf(stderr, "Failed to create Unix Domain Socket.\n"); + return -1; + } + + local.sun_family = AF_UNIX; + strncpy(local.sun_path, path, sizeof(local.sun_path)); + local.sun_path[sizeof(local.sun_path) - 1] = '\0'; + unlink(local.sun_path); + + /* we use the same magic that X11 uses in Xtranssock.c for + * calculating the proper length of the sockaddr */ +#if defined(BSD44SOCKETS) || defined(__UNIXWARE__) + local.sun_len = strlen(local.sun_path); +#endif +#if defined(BSD44SOCKETS) || defined(SUN_LEN) + namelen = SUN_LEN(&local); +#else + namelen = strlen(local.sun_path) + + offsetof(struct sockaddr_un, sun_path); +#endif + + rc = bind(bfd->fd, (struct sockaddr *) &local, namelen); + if (rc != 0) { + fprintf(stderr, "Failed to bind the unix domain socket. '%s'\n", + local.sun_path); + return -1; + } + + if (listen(bfd->fd, 0) != 0) { + fprintf(stderr, "Failed to listen.\n"); + return -1; + } + + bfd->when = BSC_FD_READ; + bfd->cb = tool_accept; + bfd->data = ts; + + ts->dlci = dlci; + INIT_LLIST_HEAD(&ts->connections); + + tool_server_for_dlci[dlci] = ts; + + sercomm_register_rx_cb(dlci, hdlc_tool_cb); + + if (osmo_fd_register(bfd) != 0) { + fprintf(stderr, "Failed to register the bfd.\n"); + return -1; + } + + return 0; +} + +extern void hdlc_tpudbg_cb(uint8_t dlci, struct msgb *msg); + +void parse_debug(const char *str) +{ + while(*str) { + switch(*str) { + case 't': + dnload.dump_tx = 1; + break; + case 'r': + dnload.dump_rx = 1; + break; + default: + printf("Unknown debug flag %c\n", *str); + abort(); + break; + } + str++; + } +} + +int main(int argc, char **argv) +{ + int opt, flags; + uint32_t tmp_load_address = ROMLOAD_ADDRESS; + const char *serial_dev = "/dev/ttyUSB1"; + const char *layer2_un_path = "/tmp/osmocom_l2"; + const char *loader_un_path = "/tmp/osmocom_loader"; + + dnload.mode = MODE_C123; + dnload.beacon_interval = DEFAULT_BEACON_INTERVAL; + dnload.do_chainload = 0; + + while ((opt = getopt(argc, argv, "d:hl:p:m:cs:i:v")) != -1) { + switch (opt) { + case 'p': + serial_dev = optarg; + break; + case 'm': + dnload.mode = parse_mode(optarg); + if (dnload.mode == MODE_INVALID) + usage(argv[0]); + break; + case 's': + layer2_un_path = optarg; + break; + case 'l': + loader_un_path = optarg; + break; + case 'v': + version(argv[0]); + break; + case 'd': + parse_debug(optarg); + break; + case 'c': + dnload.do_chainload = 1; + break; + case 'i': + dnload.beacon_interval = atoi(optarg) * 1000; + break; + case 'h': + default: + usage(argv[0]); + break; + } + } + + if (argc <= optind) { + dnload.filename = NULL; + } else { + dnload.filename = argv[optind]; + } + + dnload.serial_fd.fd = osmo_serial_init(serial_dev, MODEM_BAUDRATE); + if (dnload.serial_fd.fd < 0) { + fprintf(stderr, "Cannot open serial device %s\n", serial_dev); + exit(1); + } + + if (osmo_fd_register(&dnload.serial_fd) != 0) { + fprintf(stderr, "Failed to register the serial.\n"); + exit(1); + } + + /* Set serial socket to non-blocking mode of operation */ + flags = fcntl(dnload.serial_fd.fd, F_GETFL); + flags |= O_NONBLOCK; + fcntl(dnload.serial_fd.fd, F_SETFL, flags); + + dnload.serial_fd.when = BSC_FD_READ; + dnload.serial_fd.cb = serial_read; + + /* initialize the HDLC layer */ + sercomm_init(); + sercomm_register_rx_cb(SC_DLCI_CONSOLE, hdlc_console_cb); + sercomm_register_rx_cb(SC_DLCI_DEBUG, hdlc_tpudbg_cb); + + /* unix domain socket handling */ + if (register_tool_server(&dnload.layer2_server, layer2_un_path, + SC_DLCI_L1A_L23) != 0) + exit(1); + + if (register_tool_server(&dnload.loader_server, loader_un_path, + SC_DLCI_LOADER) != 0) + exit(1); + + /* if in romload mode, start our beacon timer */ + if (dnload.mode == MODE_ROMLOAD) { + tmp_load_address = ROMLOAD_ADDRESS; + osmo_serial_set_baudrate(dnload.serial_fd.fd, ROMLOAD_INIT_BAUDRATE); + tick_timer.cb = &beacon_timer_cb; + tick_timer.data = &tick_timer; + osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval); + } + else if (dnload.mode == MODE_MTK) { + tmp_load_address = MTK_ADDRESS; + osmo_serial_set_baudrate(dnload.serial_fd.fd, MTK_INIT_BAUDRATE); + tick_timer.cb = &mtk_timer_cb; + tick_timer.data = &tick_timer; + osmo_timer_schedule(&tick_timer, 0, dnload.beacon_interval); + } + + dnload.load_address[0] = (tmp_load_address >> 24) & 0xff; + dnload.load_address[1] = (tmp_load_address >> 16) & 0xff; + dnload.load_address[2] = (tmp_load_address >> 8) & 0xff; + dnload.load_address[3] = tmp_load_address & 0xff; + + while (1) { + if (osmo_select_main(0) < 0) + break; + } + + close(dnload.serial_fd.fd); + + exit(0); +} diff --git a/misc/tools/osmocon/osmoload.c b/misc/tools/osmocon/osmoload.c new file mode 100644 index 000000000..a0f80e15f --- /dev/null +++ b/misc/tools/osmocon/osmoload.c @@ -0,0 +1,1216 @@ +/* control utility for the Calypso bootloader */ + +/* (C) 2010 by Ingo Albrecht + * + * All Rights Reserved + * + * 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 +#include + +#include "msgb.h" +#include "select.h" +#include "timer.h" +#include "crc16.h" + +#include "protocol.h" + +#define MSGB_MAX 256 + +#define MEM_MSG_MAX (MSGB_MAX - 16) + +#define DEFAULT_SOCKET "/tmp/osmocom_loader" + +static struct osmo_fd connection; + +enum { + STATE_INIT, + STATE_QUERY_PENDING, + STATE_DUMP_IN_PROGRESS, + STATE_LOAD_IN_PROGRESS, + STATE_FLASHRANGE_GET_INFO, + STATE_FLASHRANGE_IN_PROGRESS, + STATE_PROGRAM_GET_INFO, + STATE_PROGRAM_IN_PROGRESS, + STATE_DUMPING, +}; + +struct flashblock { + uint8_t fb_chip; + uint32_t fb_offset; + uint32_t fb_addr; + uint32_t fb_size; +}; + +static struct { + /* debug flags */ + unsigned char print_requests; + unsigned char print_replies; + + /* quit flag for main loop */ + unsigned char quit; + + /* main state machine */ + int state; + + /* pending query command */ + uint8_t command; + + /* general timeout */ + struct osmo_timer_list timeout; + + /* binary i/o for firmware images */ + FILE *binfile; + /* buffer containing binfile data */ + char *binbuf; + + /* memory operation state */ + uint8_t memchip; /* target chip (for flashes) */ + uint32_t membase; /* target base address of operation */ + uint32_t memlen; /* length of entire operation */ + uint32_t memoff; /* offset for next request */ + uint16_t memcrc; /* crc for current request */ + uint16_t memreq; /* length of current request */ + + /* array of all flash blocks */ + uint8_t flashcommand; + uint32_t numblocks; + struct flashblock blocks[512]; +} osmoload; + +static int usage(const char *name) +{ + printf("Usage: %s [ -v | -h ] [ -d tr ] [ -m {c123,c155} ] [ -l /tmp/osmocom_loader ] COMMAND ...\n", name); + + puts("\n Memory commands:"); + puts(" memget - Peek at memory"); + puts(" memput - Poke at memory"); + puts(" memdump - Dump memory to file"); + puts(" memload - Load file into memory"); + + puts("\n Flash commands:"); + puts(" finfo - Information about flash chips"); + puts(" funlock
- Unlock flash block"); + puts(" flock
- Lock flash block"); + puts(" flockdown
- Lock down flash block"); + puts(" fgetlock
- Get locking state of block"); + puts(" ferase
- Erase flash range"); + puts(" fprogram
- Program file into flash"); + + puts("\n Execution commands:"); + puts(" jump - Jump to address"); + puts(" jumpflash - Jump to flash loader"); + puts(" jumprom - Jump to rom loader"); + + puts("\n Device lifecycle:"); + puts(" ping - Ping the loader"); + puts(" reset - Reset device"); + puts(" off - Power off device"); + + puts("\n Debug:"); + puts(" dump - Dump loader traffic to console"); + + exit(2); +} + +static int version(const char *name) +{ + //printf("\n%s version %s\n", name, VERSION); + exit(2); +} + +static void osmoload_osmo_hexdump(const uint8_t *data, unsigned int len) +{ + const uint8_t *bufptr = data; + const uint8_t const *endptr = bufptr + len; + int n, m, i, hexchr; + + for (n=0; n < len; n+=32, bufptr += 32) { + hexchr = 0; + for(m = 0; m < 32 && bufptr < endptr; m++, bufptr++) { + if((m) && !(m%4)) { + putchar(' '); + hexchr++; + } + printf("%02x", *bufptr); + hexchr+=2; + } + bufptr -= m; + int n = 71 - hexchr; + for(i = 0; i < n; i++) { + putchar(' '); + } + + putchar(' '); + + for(m = 0; m < 32 && bufptr < endptr; m++, bufptr++) { + if(isgraph(*bufptr)) { + putchar(*bufptr); + } else { + putchar('.'); + } + } + bufptr -= m; + + putchar('\n'); + } +} + +static void +loader_send_request(struct msgb *msg) { + int rc; + uint16_t len = htons(msg->len); + + if(osmoload.print_requests) { + printf("Sending %d bytes:\n", msg->len); + osmoload_osmo_hexdump(msg->data, msg->len); + } + + rc = write(connection.fd, &len, sizeof(len)); + if(rc != sizeof(len)) { + fprintf(stderr, "Error writing.\n"); + exit(2); + } + + rc = write(connection.fd, msg->data, msg->len); + if(rc != msg->len) { + fprintf(stderr, "Error writing.\n"); + exit(2); + } +} + +static void loader_do_memdump(uint16_t crc, void *address, size_t length); +static void loader_do_memload(); +static void loader_do_fprogram(); +static void loader_do_flashrange(uint8_t cmd, struct msgb *msg, uint8_t chip, uint32_t address, uint32_t status); + +static void memop_timeout(void *dummy) { + switch(osmoload.state) { + case STATE_LOAD_IN_PROGRESS: + printf("Timeout. Repeating."); + osmoload.memoff -= osmoload.memreq; + loader_do_memload(); + break; + default: + break; + } + return; +} + +static void +loader_parse_flash_info(struct msgb *msg) { + uint8_t nchips; + + nchips = msgb_pull_u8(msg); + + osmoload.numblocks = 0; + + int chip; + for(chip = 0; chip < nchips; chip++) { + + uint32_t address; + address = msgb_pull_u32(msg); + + uint32_t chipsize; + chipsize = msgb_pull_u32(msg); + + uint8_t nregions; + nregions = msgb_pull_u8(msg); + + printf(" chip %d at 0x%8.8x of %d bytes in %d regions\n", chip, address, chipsize, nregions); + + uint32_t curoffset = 0; + int region; + for(region = 0; region < nregions; region++) { + uint16_t blockcount = msgb_pull_u32(msg); + uint32_t blocksize = msgb_pull_u32(msg); + + printf(" region %d with %d blocks of %d bytes each\n", region, blockcount, blocksize); + + int block; + for(block = 0; block < blockcount; block++) { + osmoload.blocks[osmoload.numblocks].fb_chip = chip; + osmoload.blocks[osmoload.numblocks].fb_offset = curoffset; + osmoload.blocks[osmoload.numblocks].fb_addr = address + curoffset; + osmoload.blocks[osmoload.numblocks].fb_size = blocksize; + + printf(" block %d with %d bytes at 0x%8.8x on chip %d\n", + osmoload.numblocks, blocksize, address + curoffset, chip); + + curoffset += blocksize; + + osmoload.numblocks++; + } + } + } +} + + +static void +loader_handle_reply(struct msgb *msg) { + if(osmoload.print_replies) { + printf("Received %d bytes:\n", msg->len); + osmoload_osmo_hexdump(msg->data, msg->len); + } + + uint8_t cmd = msgb_pull_u8(msg); + + uint8_t chip; + uint8_t length; + uint16_t crc; + uint32_t address; + uint32_t entrypoint; + uint32_t status; + + void *data; + + switch(cmd) { + case LOADER_INIT: + address = msgb_pull_u32(msg); + entrypoint = msgb_pull_u32(msg); + printf("Loader at entry %x has been started, requesting load to %x\n", entrypoint, address); + break; + case LOADER_PING: + case LOADER_RESET: + case LOADER_POWEROFF: + case LOADER_ENTER_ROM_LOADER: + case LOADER_ENTER_FLASH_LOADER: + break; + case LOADER_MEM_READ: + length = msgb_pull_u8(msg); + crc = msgb_pull_u16(msg); + address = msgb_pull_u32(msg); + data = msgb_pull(msg, length); + break; + case LOADER_MEM_WRITE: + length = msgb_pull_u8(msg); + crc = msgb_pull_u16(msg); + address = msgb_pull_u32(msg); + break; + case LOADER_JUMP: + address = msgb_pull_u32(msg); + break; + case LOADER_FLASH_INFO: + break; + case LOADER_FLASH_GETLOCK: + case LOADER_FLASH_ERASE: + case LOADER_FLASH_UNLOCK: + case LOADER_FLASH_LOCK: + case LOADER_FLASH_LOCKDOWN: + chip = msgb_pull_u8(msg); + address = msgb_pull_u32(msg); + status = msgb_pull_u32(msg); + break; + case LOADER_FLASH_PROGRAM: + length = msgb_pull_u8(msg); + crc = msgb_pull_u16(msg); + msgb_pull_u8(msg); // XXX align + chip = msgb_pull_u8(msg); + address = msgb_pull_u32(msg); + status = msgb_pull_u32(msg); + break; + default: + printf("Received unknown reply %d:\n", cmd); + osmoload_osmo_hexdump(msg->data, msg->len); + osmoload.quit = 1; + return; + } + + switch(osmoload.state) { + case STATE_QUERY_PENDING: + case STATE_DUMPING: + switch(cmd) { + case LOADER_PING: + printf("Received pong.\n"); + break; + case LOADER_RESET: + printf("Reset confirmed.\n"); + break; + case LOADER_POWEROFF: + printf("Poweroff confirmed.\n"); + break; + case LOADER_ENTER_ROM_LOADER: + printf("Jump to ROM loader confirmed.\n"); + break; + case LOADER_ENTER_FLASH_LOADER: + printf("Jump to flash loader confirmed.\n"); + break; + case LOADER_MEM_READ: + printf("Received memory dump of %d bytes at 0x%x:\n", length, address); + osmoload_osmo_hexdump(data, length); + break; + case LOADER_MEM_WRITE: + printf("Confirmed memory write of %d bytes at 0x%x.\n", length, address); + break; + case LOADER_JUMP: + printf("Confirmed jump to 0x%x.\n", address); + break; + case LOADER_FLASH_ERASE: + printf("Confirmed flash erase of chip %d address 0x%8.8x, status %s\n", + chip, address, status ? "FAILED" : "ok"); + break; + case LOADER_FLASH_GETLOCK: + printf("Lock state of chip %d address 0x%8.8x is %s\n", + chip, address, (status == LOADER_FLASH_LOCKED ? "locked" + : (status == LOADER_FLASH_LOCKED_DOWN ? "locked down" + : (status == LOADER_FLASH_UNLOCKED ? "unlocked" + : "UNKNOWN")))); + break; + case LOADER_FLASH_UNLOCK: + printf("Confirmed flash unlock of chip %d address 0x%8.8x, status %s\n", + chip, address, status ? "FAILED" : "ok"); + break; + case LOADER_FLASH_LOCK: + printf("Confirmed flash lock of chip %d address 0x%8.8x, status %s\n", + chip, address, status ? "FAILED" : "ok"); + break; + case LOADER_FLASH_LOCKDOWN: + printf("Confirmed flash lockdown of chip %d address 0x%8.8x, status %s\n", + chip, address, status ? "FAILED" : "ok"); + break; + case LOADER_FLASH_INFO: + loader_parse_flash_info(msg); + break; + default: + break; + } + if(osmoload.state == STATE_QUERY_PENDING) { + if(osmoload.command == cmd) { + osmoload.quit = 1; + } + } + break; + case STATE_DUMP_IN_PROGRESS: + if(cmd == LOADER_MEM_READ) { + loader_do_memdump(crc, data, length); + } + break; + case STATE_LOAD_IN_PROGRESS: + if(cmd == LOADER_MEM_WRITE) { + if(osmoload.memcrc != crc) { + osmoload.memoff -= osmoload.memreq; + printf("\nbad crc %4.4x (not %4.4x) at offset 0x%8.8x", crc, osmoload.memcrc, osmoload.memoff); + } else { + putchar('.'); + } + loader_do_memload(); + } + break; + case STATE_PROGRAM_GET_INFO: + case STATE_PROGRAM_IN_PROGRESS: + if(cmd == LOADER_FLASH_PROGRAM) { + if(osmoload.memcrc != crc) { + osmoload.memoff -= osmoload.memreq; + printf("\nbad crc %4.4x (not %4.4x) at offset 0x%8.8x", crc, osmoload.memcrc, osmoload.memoff); + } else { + putchar('.'); + } + if(((int)status) != 0) { + printf("\nstatus %d, aborting\n", status); + exit(1); + } + loader_do_fprogram(); + } + break; + case STATE_FLASHRANGE_GET_INFO: + case STATE_FLASHRANGE_IN_PROGRESS: + loader_do_flashrange(cmd, msg, chip, address, status); + break; + default: + break; + } + + fflush(stdout); +} + +static int +loader_read_cb(struct osmo_fd *fd, unsigned int flags) { + struct msgb *msg; + uint16_t len; + int rc; + + msg = msgb_alloc(MSGB_MAX, "loader"); + if (!msg) { + fprintf(stderr, "Failed to allocate msg.\n"); + return -1; + } + + rc = read(fd->fd, &len, sizeof(len)); + if (rc < sizeof(len)) { + fprintf(stderr, "Short read. Error.\n"); + exit(2); + } + + if (ntohs(len) > MSGB_MAX) { + fprintf(stderr, "Length is too big: %u\n", ntohs(len)); + msgb_free(msg); + return -1; + } + + /* blocking read for the poor... we can starve in here... */ + msg->l2h = msgb_put(msg, ntohs(len)); + rc = read(fd->fd, msg->l2h, msgb_l2len(msg)); + if (rc != msgb_l2len(msg)) { + fprintf(stderr, "Can not read data: rc: %d errno: %d\n", rc, errno); + msgb_free(msg); + return -1; + } + + loader_handle_reply(msg); + + msgb_free(msg); + + return 0; +} + +static void +loader_connect(const char *socket_path) { + int rc; + struct sockaddr_un local; + struct osmo_fd *conn = &connection; + + local.sun_family = AF_UNIX; + strncpy(local.sun_path, socket_path, sizeof(local.sun_path)); + local.sun_path[sizeof(local.sun_path) - 1] = '\0'; + + conn->fd = socket(AF_UNIX, SOCK_STREAM, 0); + if (conn->fd < 0) { + fprintf(stderr, "Failed to create unix domain socket.\n"); + exit(1); + } + + rc = connect(conn->fd, (struct sockaddr *) &local, + sizeof(local.sun_family) + strlen(local.sun_path)); + if (rc < 0) { + fprintf(stderr, "Failed to connect to '%s'.\n", local.sun_path); + exit(1); + } + + conn->when = BSC_FD_READ; + conn->cb = loader_read_cb; + conn->data = NULL; + + if (osmo_fd_register(conn) != 0) { + fprintf(stderr, "Failed to register fd.\n"); + exit(1); + } +} + +static void +loader_send_simple(uint8_t command) { + struct msgb *msg = msgb_alloc(MSGB_MAX, "loader"); + msgb_put_u8(msg, command); + loader_send_request(msg); + msgb_free(msg); + + osmoload.command = command; +} + +static void +loader_start_query(uint8_t command) { + loader_send_simple(command); + osmoload.state = STATE_QUERY_PENDING; +} + +static void +loader_send_flash_query(uint8_t command, uint8_t chip, uint32_t address) { + struct msgb *msg = msgb_alloc(MSGB_MAX, "loader"); + msgb_put_u8(msg, command); + msgb_put_u8(msg, chip); + msgb_put_u32(msg, address); + loader_send_request(msg); + msgb_free(msg); + + osmoload.command = command; +} + +static void +loader_start_flash_query(uint8_t command, uint8_t chip, uint32_t address) { + loader_send_flash_query(command, chip, address); + osmoload.state = STATE_QUERY_PENDING; +} + +static void +loader_start_memget(uint8_t length, uint32_t address) { + struct msgb *msg = msgb_alloc(MSGB_MAX, "loader"); + msgb_put_u8(msg, LOADER_MEM_READ); + msgb_put_u8(msg, length); + msgb_put_u32(msg, address); + loader_send_request(msg); + msgb_free(msg); + + osmoload.state = STATE_QUERY_PENDING; + osmoload.command = LOADER_MEM_READ; +} + +static void +loader_start_memput(uint8_t length, uint32_t address, void *data) { + struct msgb *msg = msgb_alloc(MSGB_MAX, "loader"); + msgb_put_u8(msg, LOADER_MEM_WRITE); + msgb_put_u8(msg, length); + msgb_put_u16(msg, osmo_crc16(0, data, length)); + msgb_put_u32(msg, address); + memcpy(msgb_put(msg, length), data, length); + loader_send_request(msg); + msgb_free(msg); + + osmoload.state = STATE_QUERY_PENDING; + osmoload.command = LOADER_MEM_WRITE; +} + +static void +loader_start_jump(uint32_t address) { + struct msgb *msg = msgb_alloc(MSGB_MAX, "loader"); + msgb_put_u8(msg, LOADER_JUMP); + msgb_put_u32(msg, address); + loader_send_request(msg); + msgb_free(msg); + + osmoload.state = STATE_QUERY_PENDING; + osmoload.command = LOADER_JUMP; +} + + +static void +loader_do_memdump(uint16_t crc, void *data, size_t length) { + int rc; + + if(data && length) { + osmoload.memcrc = osmo_crc16(0, data, length); + if(osmoload.memcrc != crc) { + osmoload.memoff -= osmoload.memreq; + printf("\nbad crc %4.4x (not %4.4x) at offset 0x%8.8x", crc, osmoload.memcrc, osmoload.memoff); + } else { + putchar('.'); + } + + memcpy(osmoload.binbuf + osmoload.memoff, data, length); + osmoload.memoff += length; + } + + uint32_t rembytes = osmoload.memlen - osmoload.memoff; + + if(!rembytes) { + puts("done."); + osmoload.quit = 1; + + unsigned c = osmoload.memlen; + char *p = osmoload.binbuf; + while(c) { + rc = fwrite(p, 1, c, osmoload.binfile); + if(ferror(osmoload.binfile)) { + printf("Could not read from file: %s\n", strerror(errno)); + exit(1); + } + c -= rc; + p += rc; + } + fclose(osmoload.binfile); + osmoload.binfile = NULL; + + free(osmoload.binbuf); + + return; + } + + uint8_t reqbytes = (rembytes < MEM_MSG_MAX) ? rembytes : MEM_MSG_MAX; + + struct msgb *msg = msgb_alloc(MSGB_MAX, "loader"); + + msgb_put_u8(msg, LOADER_MEM_READ); + msgb_put_u8(msg, reqbytes); + msgb_put_u32(msg, osmoload.membase + osmoload.memoff); + loader_send_request(msg); + msgb_free(msg); +} + +static void +loader_do_memload() { + uint32_t rembytes = osmoload.memlen - osmoload.memoff; + + if(!rembytes) { + puts("done."); + osmoload.quit = 1; + return; + } + + osmo_timer_schedule(&osmoload.timeout, 0, 500000); + + uint8_t reqbytes = (rembytes < MEM_MSG_MAX) ? rembytes : MEM_MSG_MAX; + + osmoload.memcrc = osmo_crc16(0, (uint8_t *) osmoload.binbuf + osmoload.memoff, reqbytes); + osmoload.memreq = reqbytes; + + struct msgb *msg = msgb_alloc(MSGB_MAX, "loader"); + + msgb_put_u8(msg, LOADER_MEM_WRITE); + msgb_put_u8(msg, reqbytes); + msgb_put_u16(msg, osmoload.memcrc); + msgb_put_u32(msg, osmoload.membase + osmoload.memoff); + + unsigned char *p = msgb_put(msg, reqbytes); + memcpy(p, osmoload.binbuf + osmoload.memoff, reqbytes); + +#if 0 + printf("Sending %u bytes at offset %u to address %x with crc %x\n", + reqbytes, osmoload.memoff, osmoload.membase + osmoload.memoff, + osmoload.memcrc); +#endif + + loader_send_request(msg); + + msgb_free(msg); + + osmoload.memoff += reqbytes; +} + +static void +loader_do_fprogram() { + uint32_t rembytes = osmoload.memlen - osmoload.memoff; + + if(!rembytes) { + puts("done."); + osmoload.quit = 1; + return; + } + + osmo_timer_schedule(&osmoload.timeout, 0, 10000000); + + uint8_t reqbytes = (rembytes < MEM_MSG_MAX) ? rembytes : MEM_MSG_MAX; + + osmoload.memcrc = osmo_crc16(0, (uint8_t *) osmoload.binbuf + osmoload.memoff, reqbytes); + osmoload.memreq = reqbytes; + + struct msgb *msg = msgb_alloc(MSGB_MAX, "loader"); + + msgb_put_u8(msg, LOADER_FLASH_PROGRAM); + msgb_put_u8(msg, reqbytes); + msgb_put_u16(msg, osmoload.memcrc); + msgb_put_u8(msg, 0); // XXX: align data to 16bit + msgb_put_u8(msg, osmoload.memchip); + msgb_put_u32(msg, osmoload.membase + osmoload.memoff); + + unsigned char *p = msgb_put(msg, reqbytes); + memcpy(p, osmoload.binbuf + osmoload.memoff, reqbytes); + +#if 0 + printf("Sending %u bytes at offset %u to address %x with crc %x\n", + reqbytes, osmoload.memoff, osmoload.membase + osmoload.memoff, + osmoload.memcrc); +#endif + + loader_send_request(msg); + + msgb_free(msg); + + osmoload.memoff += reqbytes; +} + +static void +loader_start_memdump(uint32_t length, uint32_t address, char *file) { + printf("Dumping %u bytes of memory at 0x%x to file %s\n", length, address, file); + + osmoload.binbuf = malloc(length); + if(!osmoload.binbuf) { + printf("Could not allocate %u bytes for %s.\n", length, file); + exit(1); + } + + osmoload.binfile = fopen(file, "wb"); + if(!osmoload.binfile) { + printf("Could not open %s: %s\n", file, strerror(errno)); + exit(1); + } + + osmoload.membase = address; + osmoload.memlen = length; + osmoload.memoff = 0; + + osmoload.state = STATE_DUMP_IN_PROGRESS; + loader_do_memdump(0, NULL, 0); +} + +static void +loader_start_memload(uint32_t address, char *file) { + int rc; + struct stat st; + + rc = stat(file, &st); + if(rc < 0) { + printf("Could not stat %s: %s\n", file, strerror(errno)); + exit(1); + } + + uint32_t length = st.st_size; + + printf("Loading %u bytes of memory to address 0x%x from file %s\n", length, address, file); + + osmoload.binbuf = malloc(length); + if(!osmoload.binbuf) { + printf("Could not allocate %u bytes for %s.\n", length, file); + exit(1); + } + + osmoload.binfile = fopen(file, "rb"); + if(!osmoload.binfile) { + printf("Could not open %s: %s\n", file, strerror(errno)); + exit(1); + } + + unsigned c = length; + char *p = osmoload.binbuf; + while(c) { + rc = fread(p, 1, c, osmoload.binfile); + if(ferror(osmoload.binfile)) { + printf("Could not read from file: %s\n", strerror(errno)); + exit(1); + } + c -= rc; + p += rc; + } + fclose(osmoload.binfile); + osmoload.binfile = NULL; + + osmoload.membase = address; + osmoload.memlen = length; + osmoload.memoff = 0; + + osmoload.state = STATE_LOAD_IN_PROGRESS; + loader_do_memload(); +} + +static void +loader_start_flashrange(uint8_t command, uint32_t address, uint32_t length) { + switch(command) { + case LOADER_FLASH_ERASE: + printf("Erasing %u bytes of flash at 0x%x\n", length, address); + break; + case LOADER_FLASH_LOCK: + printf("Locking %u bytes of flash at 0x%x\n", length, address); + break; + case LOADER_FLASH_LOCKDOWN: + printf("Locking down %u bytes of flash at 0x%x\n", length, address); + break; + case LOADER_FLASH_UNLOCK: + printf("Unlocking %u bytes of flash at 0x%x\n", length, address); + break; + case LOADER_FLASH_GETLOCK: + printf("Getlocking %u bytes of flash at 0x%x\n", length, address); + break; + default: + puts("Unknown range command"); + abort(); + break; + } + + osmoload.flashcommand = command; + + osmoload.membase = address; + osmoload.memlen = length; + osmoload.memoff = 0; + + printf(" requesting flash info to determine block layout\n"); + + osmoload.state = STATE_FLASHRANGE_GET_INFO; + + loader_send_simple(LOADER_FLASH_INFO); +} + +static void +loader_do_flashrange(uint8_t cmd, struct msgb *msg, uint8_t chip, uint32_t address, uint32_t status) { + switch(osmoload.state) { + case STATE_FLASHRANGE_GET_INFO: + if(cmd == LOADER_FLASH_INFO) { + loader_parse_flash_info(msg); + osmoload.state = STATE_FLASHRANGE_IN_PROGRESS; + loader_do_flashrange(0, NULL, 0, 0, 0); + } + break; + case STATE_FLASHRANGE_IN_PROGRESS: + { + if(msg) { + if(cmd == osmoload.flashcommand) { + if(cmd == LOADER_FLASH_GETLOCK) { + printf(" lock state of chip %d address 0x%8.8x is %s\n", + chip, address, (status == LOADER_FLASH_LOCKED ? "locked" + : (status == LOADER_FLASH_LOCKED_DOWN ? "locked down" + : (status == LOADER_FLASH_UNLOCKED ? "unlocked" + : "UNKNOWN")))); + } else { + printf(" confirmed operation on chip %d address 0x%8.8x, status %s\n", + chip, address, status ? "FAILED" : "ok"); + } + } else { + break; + } + } + + uint32_t addr = osmoload.membase + osmoload.memoff; + + if(osmoload.memoff >= osmoload.memlen) { + puts(" operation done"); + osmoload.quit = 1; + break; + } + + uint8_t found = 0; + int i; + for(i = 0; i < osmoload.numblocks; i++) { + struct flashblock *b = &osmoload.blocks[i]; + if(b->fb_addr == addr) { + loader_send_flash_query(osmoload.flashcommand, b->fb_chip, b->fb_offset); + osmoload.memoff += b->fb_size; + found = 1; + break; + } + } + if(!found) { + puts("Oops!? Block not found?"); // XXX message + abort(); + } + } + break; + } +} + +static void +loader_start_fprogram(uint8_t chip, uint32_t address, char *file) { + int rc; + struct stat st; + + rc = stat(file, &st); + if(rc < 0) { + printf("Could not stat %s: %s\n", file, strerror(errno)); + exit(1); + } + + uint32_t length = st.st_size; + + printf("Loading %u bytes of memory at 0x%x in chip %d from file %s\n", length, address, chip, file); + + osmoload.binbuf = malloc(length); + if(!osmoload.binbuf) { + printf("Could not allocate %u bytes for %s.\n", length, file); + exit(1); + } + + osmoload.binfile = fopen(file, "rb"); + if(!osmoload.binfile) { + printf("Could not open %s: %s\n", file, strerror(errno)); + exit(1); + } + + unsigned c = length; + char *p = osmoload.binbuf; + while(c) { + rc = fread(p, 1, c, osmoload.binfile); + if(ferror(osmoload.binfile)) { + printf("Could not read from file: %s\n", strerror(errno)); + exit(1); + } + c -= rc; + p += rc; + } + fclose(osmoload.binfile); + osmoload.binfile = NULL; + + osmoload.memchip = chip; + osmoload.membase = address; + osmoload.memlen = length; + osmoload.memoff = 0; + + osmoload.state = STATE_PROGRAM_IN_PROGRESS; + + loader_do_fprogram(); +} + +static void +query_timeout(void *dummy) { + puts("Query timed out."); + exit(2); +} + +static void +loader_command(char *name, int cmdc, char **cmdv) { + if(!cmdc) { + usage(name); + } + + char *cmd = cmdv[0]; + + char buf[MEM_MSG_MAX]; + memset(buf, 23, sizeof(buf)); + + if(!strcmp(cmd, "dump")) { + osmoload.state = STATE_DUMPING; + } else if(!strcmp(cmd, "ping")) { + loader_start_query(LOADER_PING); + } else if(!strcmp(cmd, "off")) { + loader_start_query(LOADER_POWEROFF); + } else if(!strcmp(cmd, "reset")) { + loader_start_query(LOADER_RESET); + } else if(!strcmp(cmd, "jumprom")) { + loader_start_query(LOADER_ENTER_ROM_LOADER); + } else if(!strcmp(cmd, "jumpflash")) { + loader_start_query(LOADER_ENTER_FLASH_LOADER); + } else if(!strcmp(cmd, "finfo")) { + puts("Requesting flash layout info"); + loader_start_query(LOADER_FLASH_INFO); + } else if(!strcmp(cmd, "memput")) { + uint32_t address; + + if(cmdc < 3) { + usage(name); + } + + address = strtoul(cmdv[1], NULL, 16); + + unsigned int i; + char *hex = cmdv[2]; + if(strlen(hex)&1) { + puts("Invalid hex string."); + exit(2); + } + for(i = 0; i <= sizeof(buf) && i < strlen(hex)/2; i++) { + if(i >= sizeof(buf)) { + puts("Value too long for single message"); + exit(2); + } + unsigned int byte; + int count = sscanf(hex + i * 2, "%02x", &byte); + if(count != 1) { + puts("Invalid hex string."); + exit(2); + } + buf[i] = byte & 0xFF; + } + + loader_start_memput(i & 0xFF, address, buf); + } else if(!strcmp(cmd, "memget")) { + uint32_t address; + uint8_t length; + + if(cmdc < 3) { + usage(name); + } + + address = strtoul(cmdv[1], NULL, 16); + length = strtoul(cmdv[2], NULL, 16); + + if(length > MEM_MSG_MAX) { + puts("Too many bytes"); + exit(2); + } + + loader_start_memget(length, address); + } else if(!strcmp(cmd, "jump")) { + uint32_t address; + + if(cmdc < 2) { + usage(name); + } + + address = strtoul(cmdv[1], NULL, 16); + + loader_start_jump(address); + } else if(!strcmp(cmd, "memdump")) { + uint32_t address; + uint32_t length; + + if(cmdc < 4) { + usage(name); + } + + address = strtoul(cmdv[1], NULL, 16); + length = strtoul(cmdv[2], NULL, 16); + + loader_start_memdump(length, address, cmdv[3]); + } else if(!strcmp(cmd, "memload")) { + uint32_t address; + + if(cmdc < 3) { + usage(name); + } + + address = strtoul(cmdv[1], NULL, 16); + + loader_start_memload(address, cmdv[2]); + } else if(!strcmp(cmd, "fprogram")) { + uint8_t chip; + uint32_t address; + + if(cmdc < 4) { + usage(name); + } + + chip = strtoul(cmdv[1], NULL, 10); + address = strtoul(cmdv[2], NULL, 16); + + loader_start_fprogram(chip, address, cmdv[3]); + } else if(!strcmp(cmd, "ferase")) { + uint32_t address; + uint32_t length; + + if(cmdc < 3) { + usage(name); + } + + address = strtoul(cmdv[1], NULL, 16); + length = strtoul(cmdv[2], NULL, 16); + + loader_start_flashrange(LOADER_FLASH_ERASE, address, length); + } else if(!strcmp(cmd, "flock")) { + uint32_t address; + uint32_t length; + + if(cmdc < 3) { + usage(name); + } + + address = strtoul(cmdv[1], NULL, 16); + length = strtoul(cmdv[2], NULL, 16); + + loader_start_flashrange(LOADER_FLASH_LOCK, address, length); + } else if(!strcmp(cmd, "flockdown")) { + uint32_t address; + uint32_t length; + + if(cmdc < 3) { + usage(name); + } + + address = strtoul(cmdv[1], NULL, 16); + length = strtoul(cmdv[2], NULL, 16); + + loader_start_flashrange(LOADER_FLASH_LOCKDOWN, address, length); + } else if(!strcmp(cmd, "funlock")) { + uint32_t address; + uint32_t length; + + if(cmdc < 3) { + usage(name); + } + + address = strtoul(cmdv[1], NULL, 16); + length = strtoul(cmdv[2], NULL, 16); + + loader_start_flashrange(LOADER_FLASH_UNLOCK, address, length); + } else if(!strcmp(cmd, "fgetlock")) { + uint32_t address; + uint32_t length; + + if(cmdc < 3) { + usage(name); + } + + address = strtoul(cmdv[1], NULL, 16); + length = strtoul(cmdv[2], NULL, 16); + + loader_start_flashrange(LOADER_FLASH_GETLOCK, address, length); + } else if(!strcmp(cmd, "help")) { + usage(name); + } else { + printf("Unknown command '%s'\n", cmd); + usage(name); + } + + if(osmoload.state == STATE_QUERY_PENDING) { + osmoload.timeout.cb = &query_timeout; + osmo_timer_schedule(&osmoload.timeout, 0, 5000000); + } + if(osmoload.state == STATE_LOAD_IN_PROGRESS) { + osmoload.timeout.cb = &memop_timeout; + } + +} + +void +setdebug(const char *name, char c) { + switch(c) { + case 't': + osmoload.print_requests = 1; + break; + case 'r': + osmoload.print_replies = 1; + break; + default: + usage(name); + break; + } +} + +int +main(int argc, char **argv) { + int opt; + char *loader_un_path = "/tmp/osmocom_loader"; + const char *debugopt; + + while((opt = getopt(argc, argv, "d:hl:m:v")) != -1) { + switch(opt) { + case 'd': + debugopt = optarg; + while(*debugopt) { + setdebug(argv[0], *debugopt); + debugopt++; + } + break; + case 'l': + loader_un_path = optarg; + break; + case 'm': + puts("model selection not implemented"); + exit(2); + break; + case 'v': + version(argv[0]); + break; + case 'h': + default: + usage(argv[0]); + break; + } + } + + osmoload.quit = 0; + + loader_connect(loader_un_path); + + loader_command(argv[0], argc - optind, argv + optind); + + while(!osmoload.quit) { + osmo_select_main(0); + } + + if(osmoload.binfile) { + fclose(osmoload.binfile); + } + + return 0; +} diff --git a/misc/tools/osmocon/panic.c b/misc/tools/osmocon/panic.c new file mode 100644 index 000000000..0dc7dcc6e --- /dev/null +++ b/misc/tools/osmocon/panic.c @@ -0,0 +1,25 @@ +#include +#include + +#include "panic.h" + +static void osmo_panic_default(const char *fmt, va_list args) +{ + vfprintf(stderr, fmt, args); + abort(); +} + +void osmo_panic(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + osmo_panic_default(fmt, args); + va_end(args); +} + + +void osmo_set_panic_handler(osmo_panic_handler_t h) +{ +} + diff --git a/misc/tools/osmocon/panic.h b/misc/tools/osmocon/panic.h new file mode 100644 index 000000000..fd5cf2082 --- /dev/null +++ b/misc/tools/osmocon/panic.h @@ -0,0 +1,20 @@ +#ifndef OSMOCORE_PANIC_H +#define OSMOCORE_PANIC_H + +/*! \addtogroup utils + * @{ + */ + +/*! \file panic.h */ + +#include + +/*! \brief panic handler callback function type */ +typedef void (*osmo_panic_handler_t)(const char *fmt, va_list args); + +extern void osmo_panic(const char *fmt, ...); +extern void osmo_set_panic_handler(osmo_panic_handler_t h); + +/*! @} */ + +#endif /* OSMOCORE_PANIC_H */ diff --git a/misc/tools/osmocon/protocol.h b/misc/tools/osmocon/protocol.h new file mode 100644 index 000000000..0a61c89ec --- /dev/null +++ b/misc/tools/osmocon/protocol.h @@ -0,0 +1,37 @@ + +enum loader_command { + /* init message from loader */ + LOADER_INIT, + + /* ping / pong */ + LOADER_PING, + + /* lifecycle requests */ + LOADER_RESET, + LOADER_POWEROFF, + + /* jumps */ + LOADER_JUMP, + LOADER_ENTER_ROM_LOADER, + LOADER_ENTER_FLASH_LOADER, + + /* generic memory ops */ + LOADER_MEM_READ, + LOADER_MEM_WRITE, + + /* flash operations */ + LOADER_FLASH_INFO, + LOADER_FLASH_ERASE, + LOADER_FLASH_UNLOCK, + LOADER_FLASH_LOCK, + LOADER_FLASH_LOCKDOWN, + LOADER_FLASH_GETLOCK, + LOADER_FLASH_PROGRAM, + +}; + +enum loader_flash_lock { + LOADER_FLASH_UNLOCKED = 0, + LOADER_FLASH_LOCKED, + LOADER_FLASH_LOCKED_DOWN, +}; diff --git a/misc/tools/osmocon/rbtree.c b/misc/tools/osmocon/rbtree.c new file mode 100644 index 000000000..9033a84ac --- /dev/null +++ b/misc/tools/osmocon/rbtree.c @@ -0,0 +1,383 @@ +/* + Red Black Trees + (C) 1999 Andrea Arcangeli + (C) 2002 David Woodhouse + + 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + + linux/lib/rbtree.c +*/ + +#include "linuxrbtree.h" + +static void __rb_rotate_left(struct rb_node *node, struct rb_root *root) +{ + struct rb_node *right = node->rb_right; + struct rb_node *parent = rb_parent(node); + + if ((node->rb_right = right->rb_left)) + rb_set_parent(right->rb_left, node); + right->rb_left = node; + + rb_set_parent(right, parent); + + if (parent) + { + if (node == parent->rb_left) + parent->rb_left = right; + else + parent->rb_right = right; + } + else + root->rb_node = right; + rb_set_parent(node, right); +} + +static void __rb_rotate_right(struct rb_node *node, struct rb_root *root) +{ + struct rb_node *left = node->rb_left; + struct rb_node *parent = rb_parent(node); + + if ((node->rb_left = left->rb_right)) + rb_set_parent(left->rb_right, node); + left->rb_right = node; + + rb_set_parent(left, parent); + + if (parent) + { + if (node == parent->rb_right) + parent->rb_right = left; + else + parent->rb_left = left; + } + else + root->rb_node = left; + rb_set_parent(node, left); +} + +void rb_insert_color(struct rb_node *node, struct rb_root *root) +{ + struct rb_node *parent, *gparent; + + while ((parent = rb_parent(node)) && rb_is_red(parent)) + { + gparent = rb_parent(parent); + + if (parent == gparent->rb_left) + { + { + register struct rb_node *uncle = gparent->rb_right; + if (uncle && rb_is_red(uncle)) + { + rb_set_black(uncle); + rb_set_black(parent); + rb_set_red(gparent); + node = gparent; + continue; + } + } + + if (parent->rb_right == node) + { + register struct rb_node *tmp; + __rb_rotate_left(parent, root); + tmp = parent; + parent = node; + node = tmp; + } + + rb_set_black(parent); + rb_set_red(gparent); + __rb_rotate_right(gparent, root); + } else { + { + register struct rb_node *uncle = gparent->rb_left; + if (uncle && rb_is_red(uncle)) + { + rb_set_black(uncle); + rb_set_black(parent); + rb_set_red(gparent); + node = gparent; + continue; + } + } + + if (parent->rb_left == node) + { + register struct rb_node *tmp; + __rb_rotate_right(parent, root); + tmp = parent; + parent = node; + node = tmp; + } + + rb_set_black(parent); + rb_set_red(gparent); + __rb_rotate_left(gparent, root); + } + } + + rb_set_black(root->rb_node); +} + +static void __rb_erase_color(struct rb_node *node, struct rb_node *parent, + struct rb_root *root) +{ + struct rb_node *other; + + while ((!node || rb_is_black(node)) && node != root->rb_node) + { + if (parent->rb_left == node) + { + other = parent->rb_right; + if (rb_is_red(other)) + { + rb_set_black(other); + rb_set_red(parent); + __rb_rotate_left(parent, root); + other = parent->rb_right; + } + if ((!other->rb_left || rb_is_black(other->rb_left)) && + (!other->rb_right || rb_is_black(other->rb_right))) + { + rb_set_red(other); + node = parent; + parent = rb_parent(node); + } + else + { + if (!other->rb_right || rb_is_black(other->rb_right)) + { + rb_set_black(other->rb_left); + rb_set_red(other); + __rb_rotate_right(other, root); + other = parent->rb_right; + } + rb_set_color(other, rb_color(parent)); + rb_set_black(parent); + rb_set_black(other->rb_right); + __rb_rotate_left(parent, root); + node = root->rb_node; + break; + } + } + else + { + other = parent->rb_left; + if (rb_is_red(other)) + { + rb_set_black(other); + rb_set_red(parent); + __rb_rotate_right(parent, root); + other = parent->rb_left; + } + if ((!other->rb_left || rb_is_black(other->rb_left)) && + (!other->rb_right || rb_is_black(other->rb_right))) + { + rb_set_red(other); + node = parent; + parent = rb_parent(node); + } + else + { + if (!other->rb_left || rb_is_black(other->rb_left)) + { + rb_set_black(other->rb_right); + rb_set_red(other); + __rb_rotate_left(other, root); + other = parent->rb_left; + } + rb_set_color(other, rb_color(parent)); + rb_set_black(parent); + rb_set_black(other->rb_left); + __rb_rotate_right(parent, root); + node = root->rb_node; + break; + } + } + } + if (node) + rb_set_black(node); +} + +void rb_erase(struct rb_node *node, struct rb_root *root) +{ + struct rb_node *child, *parent; + int color; + + if (!node->rb_left) + child = node->rb_right; + else if (!node->rb_right) + child = node->rb_left; + else + { + struct rb_node *old = node, *left; + + node = node->rb_right; + while ((left = node->rb_left) != NULL) + node = left; + + if (rb_parent(old)) { + if (rb_parent(old)->rb_left == old) + rb_parent(old)->rb_left = node; + else + rb_parent(old)->rb_right = node; + } else + root->rb_node = node; + + child = node->rb_right; + parent = rb_parent(node); + color = rb_color(node); + + if (parent == old) { + parent = node; + } else { + if (child) + rb_set_parent(child, parent); + parent->rb_left = child; + + node->rb_right = old->rb_right; + rb_set_parent(old->rb_right, node); + } + + node->rb_parent_color = old->rb_parent_color; + node->rb_left = old->rb_left; + rb_set_parent(old->rb_left, node); + + goto color; + } + + parent = rb_parent(node); + color = rb_color(node); + + if (child) + rb_set_parent(child, parent); + if (parent) + { + if (parent->rb_left == node) + parent->rb_left = child; + else + parent->rb_right = child; + } + else + root->rb_node = child; + + color: + if (color == RB_BLACK) + __rb_erase_color(child, parent, root); +} + +/* + * This function returns the first node (in sort order) of the tree. + */ +struct rb_node *rb_first(const struct rb_root *root) +{ + struct rb_node *n; + + n = root->rb_node; + if (!n) + return NULL; + while (n->rb_left) + n = n->rb_left; + return n; +} + +struct rb_node *rb_last(const struct rb_root *root) +{ + struct rb_node *n; + + n = root->rb_node; + if (!n) + return NULL; + while (n->rb_right) + n = n->rb_right; + return n; +} + +struct rb_node *rb_next(const struct rb_node *node) +{ + struct rb_node *parent; + + if (rb_parent(node) == node) + return NULL; + + /* If we have a right-hand child, go down and then left as far + as we can. */ + if (node->rb_right) { + node = node->rb_right; + while (node->rb_left) + node=node->rb_left; + return (struct rb_node *)node; + } + + /* No right-hand children. Everything down and left is + smaller than us, so any 'next' node must be in the general + direction of our parent. Go up the tree; any time the + ancestor is a right-hand child of its parent, keep going + up. First time it's a left-hand child of its parent, said + parent is our 'next' node. */ + while ((parent = rb_parent(node)) && node == parent->rb_right) + node = parent; + + return parent; +} + +struct rb_node *rb_prev(const struct rb_node *node) +{ + struct rb_node *parent; + + if (rb_parent(node) == node) + return NULL; + + /* If we have a left-hand child, go down and then right as far + as we can. */ + if (node->rb_left) { + node = node->rb_left; + while (node->rb_right) + node=node->rb_right; + return (struct rb_node *)node; + } + + /* No left-hand children. Go up till we find an ancestor which + is a right-hand child of its parent */ + while ((parent = rb_parent(node)) && node == parent->rb_left) + node = parent; + + return parent; +} + +void rb_replace_node(struct rb_node *victim, struct rb_node *new, + struct rb_root *root) +{ + struct rb_node *parent = rb_parent(victim); + + /* Set the surrounding nodes to point to the replacement */ + if (parent) { + if (victim == parent->rb_left) + parent->rb_left = new; + else + parent->rb_right = new; + } else { + root->rb_node = new; + } + if (victim->rb_left) + rb_set_parent(victim->rb_left, new); + if (victim->rb_right) + rb_set_parent(victim->rb_right, new); + + /* Copy the pointers/colour from the victim to the replacement */ + *new = *victim; +} diff --git a/misc/tools/osmocon/select.c b/misc/tools/osmocon/select.c new file mode 100644 index 000000000..716df568d --- /dev/null +++ b/misc/tools/osmocon/select.c @@ -0,0 +1,166 @@ +/* select filedescriptor handling, taken from: + * userspace logging daemon for the iptables ULOG target + * of the linux 2.4 netfilter subsystem. + * + * (C) 2000-2009 by Harald Welte + * + * 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include + +#include "select.h" +#include "linuxlist.h" +#include "timer.h" + +/*! \addtogroup select + * @{ + */ + +/*! \file select.c + * \brief select loop abstraction + */ + +static int maxfd = 0; +static LLIST_HEAD(osmo_fds); +static int unregistered_count; + +/*! \brief Register a new file descriptor with select loop abstraction + * \param[in] fd osmocom file descriptor to be registered + */ +int osmo_fd_register(struct osmo_fd *fd) +{ + int flags; + + /* make FD nonblocking */ + flags = fcntl(fd->fd, F_GETFL); + if (flags < 0) + return flags; + flags |= O_NONBLOCK; + flags = fcntl(fd->fd, F_SETFL, flags); + if (flags < 0) + return flags; + + /* set close-on-exec flag */ + flags = fcntl(fd->fd, F_GETFD); + if (flags < 0) + return flags; + flags |= FD_CLOEXEC; + flags = fcntl(fd->fd, F_SETFD, flags); + if (flags < 0) + return flags; + + /* Register FD */ + if (fd->fd > maxfd) + maxfd = fd->fd; + +#ifdef BSC_FD_CHECK + struct osmo_fd *entry; + llist_for_each_entry(entry, &osmo_fds, list) { + if (entry == fd) { + fprintf(stderr, "Adding a osmo_fd that is already in the list.\n"); + return 0; + } + } +#endif + + llist_add_tail(&fd->list, &osmo_fds); + + return 0; +} + +/*! \brief Unregister a file descriptor from select loop abstraction + * \param[in] fd osmocom file descriptor to be unregistered + */ +void osmo_fd_unregister(struct osmo_fd *fd) +{ + unregistered_count++; + llist_del(&fd->list); +} + +/*! \brief select main loop integration + * \param[in] polling should we pollonly (1) or block on select (0) + */ +int osmo_select_main(int polling) +{ + struct osmo_fd *ufd, *tmp; + fd_set readset, writeset, exceptset; + int work = 0, rc; + struct timeval no_time = {0, 0}; + + FD_ZERO(&readset); + FD_ZERO(&writeset); + FD_ZERO(&exceptset); + + /* prepare read and write fdsets */ + llist_for_each_entry(ufd, &osmo_fds, list) { + if (ufd->when & BSC_FD_READ) + FD_SET(ufd->fd, &readset); + + if (ufd->when & BSC_FD_WRITE) + FD_SET(ufd->fd, &writeset); + + if (ufd->when & BSC_FD_EXCEPT) + FD_SET(ufd->fd, &exceptset); + } + + osmo_timers_check(); + + if (!polling) + osmo_timers_prepare(); + rc = select(maxfd+1, &readset, &writeset, &exceptset, polling ? &no_time : osmo_timers_nearest()); + if (rc < 0) + return 0; + + /* fire timers */ + osmo_timers_update(); + + /* call registered callback functions */ +restart: + unregistered_count = 0; + llist_for_each_entry_safe(ufd, tmp, &osmo_fds, list) { + int flags = 0; + + if (FD_ISSET(ufd->fd, &readset)) { + flags |= BSC_FD_READ; + FD_CLR(ufd->fd, &readset); + } + + if (FD_ISSET(ufd->fd, &writeset)) { + flags |= BSC_FD_WRITE; + FD_CLR(ufd->fd, &writeset); + } + + if (FD_ISSET(ufd->fd, &exceptset)) { + flags |= BSC_FD_EXCEPT; + FD_CLR(ufd->fd, &exceptset); + } + + if (flags) { + work = 1; + ufd->cb(ufd, flags); + } + /* ugly, ugly hack. If more than one filedescriptors were + * unregistered, they might have been consecutive and + * llist_for_each_entry_safe() is no longer safe */ + /* this seems to happen with the last element of the list as well */ + if (unregistered_count >= 1) + goto restart; + } + return work; +} + +/*! @} */ diff --git a/misc/tools/osmocon/select.h b/misc/tools/osmocon/select.h new file mode 100644 index 000000000..0842b177e --- /dev/null +++ b/misc/tools/osmocon/select.h @@ -0,0 +1,45 @@ +#ifndef _BSC_SELECT_H +#define _BSC_SELECT_H + +#include "linuxlist.h" + +/*! \defgroup select Select loop abstraction + * @{ + */ + +/*! \file select.h + * \brief select loop abstraction + */ + +/*! \brief Indicate interest in reading from the file descriptor */ +#define BSC_FD_READ 0x0001 +/*! \brief Indicate interest in writing to the file descriptor */ +#define BSC_FD_WRITE 0x0002 +/*! \brief Indicate interest in exceptions from the file descriptor */ +#define BSC_FD_EXCEPT 0x0004 + +/*! \brief Structure representing a file dsecriptor */ +struct osmo_fd { + /*! linked list for internal management */ + struct llist_head list; + /*! actual operating-system level file decriptor */ + int fd; + /*! bit-mask or of \ref BSC_FD_READ, \ref BSC_FD_WRITE and/or + * \ref BSC_FD_EXCEPT */ + unsigned int when; + /*! call-back function to be called once file descriptor becomes + * available */ + int (*cb)(struct osmo_fd *fd, unsigned int what); + /*! data pointer passed through to call-back function */ + void *data; + /*! private number, extending \a data */ + unsigned int priv_nr; +}; + +int osmo_fd_register(struct osmo_fd *fd); +void osmo_fd_unregister(struct osmo_fd *fd); +int osmo_select_main(int polling); + +/*! @} */ + +#endif /* _BSC_SELECT_H */ diff --git a/misc/tools/osmocon/sercomm.c b/misc/tools/osmocon/sercomm.c new file mode 100644 index 000000000..44cf6646e --- /dev/null +++ b/misc/tools/osmocon/sercomm.c @@ -0,0 +1,268 @@ +/* Serial communications layer, based on HDLC */ + +/* (C) 2010 by Harald Welte + * + * All Rights Reserved + * + * 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 "msgb.h" + +# define SERCOMM_RX_MSG_SIZE 2048 +# ifndef ARRAY_SIZE +# define ARRAY_SIZE(x) (sizeof(x)/sizeof(x[0])) +# endif +# include + +static inline void sercomm_lock(unsigned long __attribute__((unused)) *flags) {} +static inline void sercomm_unlock(unsigned long __attribute__((unused)) *flags) {} + +enum rx_state { + RX_ST_WAIT_START, + RX_ST_ADDR, + RX_ST_CTRL, + RX_ST_DATA, + RX_ST_ESCAPE, +}; + +static struct { + int initialized; + int uart_id; + + /* transmit side */ + struct { + struct llist_head dlci_queues[_SC_DLCI_MAX]; + struct msgb *msg; + enum rx_state state; + uint8_t *next_char; + } tx; + + /* receive side */ + struct { + dlci_cb_t dlci_handler[_SC_DLCI_MAX]; + struct msgb *msg; + enum rx_state state; + uint8_t dlci; + uint8_t ctrl; + } rx; + +} sercomm; + +void sercomm_init(void) +{ + unsigned int i; + for (i = 0; i < ARRAY_SIZE(sercomm.tx.dlci_queues); i++) + INIT_LLIST_HEAD(&sercomm.tx.dlci_queues[i]); + + sercomm.rx.msg = NULL; + sercomm.initialized = 1; + + /* set up the echo dlci */ + sercomm_register_rx_cb(SC_DLCI_ECHO, &sercomm_sendmsg); +} + +int sercomm_initialized(void) +{ + return sercomm.initialized; +} + +/* user interface for transmitting messages for a given DLCI */ +void sercomm_sendmsg(uint8_t dlci, struct msgb *msg) +{ + unsigned long flags; + uint8_t *hdr; + + /* prepend address + control octet */ + hdr = msgb_push(msg, 2); + hdr[0] = dlci; + hdr[1] = HDLC_C_UI; + + /* This functiion can be called from any context: FIQ, IRQ + * and supervisor context. Proper locking is important! */ + sercomm_lock(&flags); + msgb_enqueue(&sercomm.tx.dlci_queues[dlci], msg); + sercomm_unlock(&flags); +} + +/* how deep is the Tx queue for a given DLCI */ +unsigned int sercomm_tx_queue_depth(uint8_t dlci) +{ + struct llist_head *le; + unsigned int num = 0; + + llist_for_each(le, &sercomm.tx.dlci_queues[dlci]) { + num++; + } + + return num; +} + +/* fetch one octet of to-be-transmitted serial data */ +int sercomm_drv_pull(uint8_t *ch) +{ + unsigned long flags; + + /* we may be called from interrupt context, but we stiff need to lock + * because sercomm could be accessed from a FIQ context ... */ + + sercomm_lock(&flags); + + if (!sercomm.tx.msg) { + unsigned int i; + /* dequeue a new message from the queues */ + for (i = 0; i < ARRAY_SIZE(sercomm.tx.dlci_queues); i++) { + sercomm.tx.msg = msgb_dequeue(&sercomm.tx.dlci_queues[i]); + if (sercomm.tx.msg) + break; + } + if (sercomm.tx.msg) { + /* start of a new message, send start flag octet */ + *ch = HDLC_FLAG; + sercomm.tx.next_char = sercomm.tx.msg->data; + sercomm_unlock(&flags); + return 1; + } else { + /* no more data avilable */ + sercomm_unlock(&flags); + return 0; + } + } + + if (sercomm.tx.state == RX_ST_ESCAPE) { + /* we've already transmitted the ESCAPE octet, + * we now need to transmit the escaped data */ + *ch = *sercomm.tx.next_char++; + sercomm.tx.state = RX_ST_DATA; + } else if (sercomm.tx.next_char >= sercomm.tx.msg->tail) { + /* last character has already been transmitted, + * send end-of-message octet */ + *ch = HDLC_FLAG; + /* we've reached the end of the message buffer */ + msgb_free(sercomm.tx.msg); + sercomm.tx.msg = NULL; + sercomm.tx.next_char = NULL; + /* escaping for the two control octets */ + } else if (*sercomm.tx.next_char == HDLC_FLAG || + *sercomm.tx.next_char == HDLC_ESCAPE || + *sercomm.tx.next_char == 0x00) { + /* send an escape octet */ + *ch = HDLC_ESCAPE; + /* invert bit 5 of the next octet to be sent */ + *sercomm.tx.next_char ^= (1 << 5); + sercomm.tx.state = RX_ST_ESCAPE; + } else { + /* standard case, simply send next octet */ + *ch = *sercomm.tx.next_char++; + } + + sercomm_unlock(&flags); + return 1; +} + +/* register a handler for a given DLCI */ +int sercomm_register_rx_cb(uint8_t dlci, dlci_cb_t cb) +{ + if (dlci >= ARRAY_SIZE(sercomm.rx.dlci_handler)) + return -EINVAL; + + if (sercomm.rx.dlci_handler[dlci]) + return -EBUSY; + + sercomm.rx.dlci_handler[dlci] = cb; + return 0; +} + +/* dispatch an incoming message once it is completely received */ +static void dispatch_rx_msg(uint8_t dlci, struct msgb *msg) +{ + if (dlci >= ARRAY_SIZE(sercomm.rx.dlci_handler) || + !sercomm.rx.dlci_handler[dlci]) { + msgb_free(msg); + return; + } + sercomm.rx.dlci_handler[dlci](dlci, msg); +} + +/* the driver has received one byte, pass it into sercomm layer */ +int sercomm_drv_rx_char(uint8_t ch) +{ + uint8_t *ptr; + + /* we are always called from interrupt context in this function, + * which means that any data structures we use need to be for + * our exclusive access */ + if (!sercomm.rx.msg) + sercomm.rx.msg = sercomm_alloc_msgb(SERCOMM_RX_MSG_SIZE); + + if (msgb_tailroom(sercomm.rx.msg) == 0) { + //cons_puts("sercomm_drv_rx_char() overflow!\n"); + msgb_free(sercomm.rx.msg); + sercomm.rx.msg = sercomm_alloc_msgb(SERCOMM_RX_MSG_SIZE); + sercomm.rx.state = RX_ST_WAIT_START; + return 0; + } + + switch (sercomm.rx.state) { + case RX_ST_WAIT_START: + if (ch != HDLC_FLAG) + break; + sercomm.rx.state = RX_ST_ADDR; + break; + case RX_ST_ADDR: + sercomm.rx.dlci = ch; + sercomm.rx.state = RX_ST_CTRL; + break; + case RX_ST_CTRL: + sercomm.rx.ctrl = ch; + sercomm.rx.state = RX_ST_DATA; + break; + case RX_ST_DATA: + if (ch == HDLC_ESCAPE) { + /* drop the escape octet, but change state */ + sercomm.rx.state = RX_ST_ESCAPE; + break; + } else if (ch == HDLC_FLAG) { + /* message is finished */ + dispatch_rx_msg(sercomm.rx.dlci, sercomm.rx.msg); + /* allocate new buffer */ + sercomm.rx.msg = NULL; + /* start all over again */ + sercomm.rx.state = RX_ST_WAIT_START; + + /* do not add the control char */ + break; + } + /* default case: store the octet */ + ptr = msgb_put(sercomm.rx.msg, 1); + *ptr = ch; + break; + case RX_ST_ESCAPE: + /* store bif-5-inverted octet in buffer */ + ch ^= (1 << 5); + ptr = msgb_put(sercomm.rx.msg, 1); + *ptr = ch; + /* transition back to normal DATA state */ + sercomm.rx.state = RX_ST_DATA; + break; + } + + return 1; +} diff --git a/misc/tools/osmocon/sercomm.h b/misc/tools/osmocon/sercomm.h new file mode 100644 index 000000000..72b8c11d6 --- /dev/null +++ b/misc/tools/osmocon/sercomm.h @@ -0,0 +1,59 @@ +#ifndef _SERCOMM_H +#define _SERCOMM_H + +#include "msgb.h" + +#define HDLC_FLAG 0x7E +#define HDLC_ESCAPE 0x7D + +#define HDLC_C_UI 0x03 +#define HDLC_C_P_BIT (1 << 4) +#define HDLC_C_F_BIT (1 << 4) + +/* a low sercomm_dlci means high priority. A high DLCI means low priority */ +enum sercomm_dlci { + SC_DLCI_HIGHEST = 0, + SC_DLCI_DEBUG = 4, + SC_DLCI_L1A_L23 = 5, + SC_DLCI_LOADER = 9, + SC_DLCI_CONSOLE = 10, + SC_DLCI_ECHO = 128, + _SC_DLCI_MAX +}; + +#ifndef HOST_BUILD +/* helper functions for target */ +void sercomm_bind_uart(int uart); +int sercomm_get_uart(void); +#endif + +void sercomm_init(void); +int sercomm_initialized(void); + +/* User Interface: Tx */ + +/* user interface for transmitting messages for a given DLCI */ +void sercomm_sendmsg(uint8_t dlci, struct msgb *msg); +/* how deep is the Tx queue for a given DLCI */ +unsigned int sercomm_tx_queue_depth(uint8_t dlci); + +/* User Interface: Rx */ + +/* receiving messages for a given DLCI */ +typedef void (*dlci_cb_t)(uint8_t dlci, struct msgb *msg); +int sercomm_register_rx_cb(uint8_t dlci, dlci_cb_t cb); + +/* Driver Interface */ + +/* fetch one octet of to-be-transmitted serial data. returns 0 if no more data */ +int sercomm_drv_pull(uint8_t *ch); +/* the driver has received one byte, pass it into sercomm layer. + returns 1 in case of success, 0 in case of unrecognized char */ +int sercomm_drv_rx_char(uint8_t ch); + +static inline struct msgb *sercomm_alloc_msgb(unsigned int len) +{ + return msgb_alloc_headroom(len+4, 4, "sercomm_tx"); +} + +#endif /* _SERCOMM_H */ diff --git a/misc/tools/osmocon/serial.c b/misc/tools/osmocon/serial.c new file mode 100644 index 000000000..a2a190535 --- /dev/null +++ b/misc/tools/osmocon/serial.c @@ -0,0 +1,229 @@ +/* + * serial.c + * + * Utility functions to deal with serial ports + * + * Copyright (C) 2011 Sylvain Munaut + * + * All Rights Reserved + * + * 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. + */ + +/*! \addtogroup serial + * @{ + */ + +/*! \file serial.c + * \file Osmocom serial port helpers + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef __linux__ +#include +#endif + +#include "serial.h" + + +#if 0 +# define dbg_perror(x) perror(x) +#else +# define dbg_perror(x) do { } while (0) +#endif + +/*! \brief Open serial device and does base init + * \param[in] dev Path to the device node to open + * \param[in] baudrate Baudrate constant (speed_t: B9600, B...) + * \returns >=0 file descriptor in case of success or negative errno. + */ +int +osmo_serial_init(const char *dev, speed_t baudrate) +{ + int rc, fd=0, v24; + struct termios tio; + + /* Open device */ + fd = open(dev, O_RDWR | O_NOCTTY); + if (fd < 0) { + dbg_perror("open"); + return -errno; + } + + /* Configure serial interface */ + rc = tcgetattr(fd, &tio); + if (rc < 0) { + dbg_perror("tcgetattr()"); + rc = -errno; + goto error; + } + + cfsetispeed(&tio, baudrate); + cfsetospeed(&tio, baudrate); + + tio.c_cflag &= ~(PARENB | CSTOPB | CSIZE | CRTSCTS); + tio.c_cflag |= (CREAD | CLOCAL | CS8); + tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + tio.c_iflag |= (INPCK | ISTRIP); + tio.c_iflag &= ~(ISTRIP | IXON | IXOFF | IGNBRK | INLCR | ICRNL | IGNCR); + tio.c_oflag &= ~(OPOST | ONLCR); + + rc = tcsetattr(fd, TCSANOW, &tio); + if (rc < 0) { + dbg_perror("tcsetattr()"); + rc = -errno; + goto error; + } + + /* Set ready to read/write */ + v24 = TIOCM_DTR | TIOCM_RTS; + rc = ioctl(fd, TIOCMBIS, &v24); + if (rc < 0) { + dbg_perror("ioctl(TIOCMBIS)"); + rc = -errno; + goto error; + } + + return fd; + +error: + if (fd) + close(fd); + return rc; +} + +static int +_osmo_serial_set_baudrate(int fd, speed_t baudrate) +{ + int rc; + struct termios tio; + + rc = tcgetattr(fd, &tio); + if (rc < 0) { + dbg_perror("tcgetattr()"); + return -errno; + } + cfsetispeed(&tio, baudrate); + cfsetospeed(&tio, baudrate); + + rc = tcsetattr(fd, TCSANOW, &tio); + if (rc < 0) { + dbg_perror("tcgetattr()"); + return -errno; + } + + return 0; +} + +/*! \brief Change current baudrate + * \param[in] fd File descriptor of the open device + * \param[in] baudrate Baudrate constant (speed_t: B9600, B...) + * \returns 0 for success or negative errno. + */ +int +osmo_serial_set_baudrate(int fd, speed_t baudrate) +{ + osmo_serial_clear_custom_baudrate(fd); + return _osmo_serial_set_baudrate(fd, baudrate); +} + +/*! \brief Change current baudrate to a custom one using OS specific method + * \param[in] fd File descriptor of the open device + * \param[in] baudrate Baudrate as integer + * \returns 0 for success or negative errno. + * + * This function might not work on all OS or with all type of serial adapters + */ +int +osmo_serial_set_custom_baudrate(int fd, int baudrate) +{ +#ifdef __linux__ + int rc; + struct serial_struct ser_info; + + rc = ioctl(fd, TIOCGSERIAL, &ser_info); + if (rc < 0) { + dbg_perror("ioctl(TIOCGSERIAL)"); + return -errno; + } + + ser_info.flags = ASYNC_SPD_CUST | ASYNC_LOW_LATENCY; + ser_info.custom_divisor = ser_info.baud_base / baudrate; + + rc = ioctl(fd, TIOCSSERIAL, &ser_info); + if (rc < 0) { + dbg_perror("ioctl(TIOCSSERIAL)"); + return -errno; + } + + return _osmo_serial_set_baudrate(fd, B38400); /* 38400 is a kind of magic ... */ +#elif defined(__APPLE__) +#ifndef IOSSIOSPEED +#define IOSSIOSPEED _IOW('T', 2, speed_t) +#endif + int rc; + + unsigned int speed = baudrate; + rc = ioctl(fd, IOSSIOSPEED, &speed); + if (rc < 0) { + dbg_perror("ioctl(IOSSIOSPEED)"); + return -errno; + } + return 0; +#else +#warning osmo_serial_set_custom_baudrate: unsupported platform + return 0; +#endif +} + +/*! \brief Clear any custom baudrate + * \param[in] fd File descriptor of the open device + * \returns 0 for success or negative errno. + * + * This function might not work on all OS or with all type of serial adapters + */ +int +osmo_serial_clear_custom_baudrate(int fd) +{ +#ifdef __linux__ + int rc; + struct serial_struct ser_info; + + rc = ioctl(fd, TIOCGSERIAL, &ser_info); + if (rc < 0) { + dbg_perror("ioctl(TIOCGSERIAL)"); + return -errno; + } + + ser_info.flags = ASYNC_LOW_LATENCY; + ser_info.custom_divisor = 0; + + rc = ioctl(fd, TIOCSSERIAL, &ser_info); + if (rc < 0) { + dbg_perror("ioctl(TIOCSSERIAL)"); + return -errno; + } +#endif + return 0; +} + +/*! @} */ diff --git a/misc/tools/osmocon/serial.h b/misc/tools/osmocon/serial.h new file mode 100644 index 000000000..889bd8a12 --- /dev/null +++ b/misc/tools/osmocon/serial.h @@ -0,0 +1,43 @@ +/* + * serial.h + * + * Copyright (C) 2011 Sylvain Munaut + * + * All Rights Reserved + * + * 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. + */ + +/*! \defgroup serial Utility functions to deal with serial ports + * @{ + */ + +/*! \file serial.h + * \file Osmocom serial port helpers + */ + +#ifndef __OSMO_SERIAL_H__ +#define __OSMO_SERIAL_H__ + +#include + +int osmo_serial_init(const char *dev, speed_t baudrate); +int osmo_serial_set_baudrate(int fd, speed_t baudrate); +int osmo_serial_set_custom_baudrate(int fd, int baudrate); +int osmo_serial_clear_custom_baudrate(int fd); + +/*! @} */ + +#endif /* __OSMO_SERIAL_H__ */ diff --git a/misc/tools/osmocon/talloc.c b/misc/tools/osmocon/talloc.c new file mode 100644 index 000000000..c0072441d --- /dev/null +++ b/misc/tools/osmocon/talloc.c @@ -0,0 +1,1804 @@ +/* + Samba Unix SMB/CIFS implementation. + + Samba trivial allocation library - new interface + + NOTE: Please read talloc_guide.txt for full documentation + + Copyright (C) Andrew Tridgell 2004 + Copyright (C) Stefan Metzmacher 2006 + + ** NOTE! The following LGPL license applies to the talloc + ** library. This does NOT imply that all of Samba is released + ** under the LGPL + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 3 of the License, or (at your option) any later version. + + This library 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, see . +*/ + +/* + inspired by http://swapped.cc/halloc/ +*/ + +#ifdef _SAMBA_BUILD_ +#include "version.h" +#if (SAMBA_VERSION_MAJOR<4) +#include "includes.h" +/* This is to circumvent SAMBA3's paranoid malloc checker. Here in this file + * we trust ourselves... */ +#ifdef malloc +#undef malloc +#endif +#ifdef realloc +#undef realloc +#endif +#define _TALLOC_SAMBA3 +#endif /* (SAMBA_VERSION_MAJOR<4) */ +#endif /* _SAMBA_BUILD_ */ + +#ifndef _TALLOC_SAMBA3 +//#include "replace.h" +#include +#include +#include +#define __USE_GNU +#include +#undef __USE_GNU +#include "talloc.h" +#define MIN(x,y) ((x) < (y) ? (x) : (y)) +#endif /* not _TALLOC_SAMBA3 */ + +/* use this to force every realloc to change the pointer, to stress test + code that might not cope */ +#define ALWAYS_REALLOC 0 + + +#define MAX_TALLOC_SIZE 0x10000000 +#define TALLOC_MAGIC 0xe814ec70 +#define TALLOC_FLAG_FREE 0x01 +#define TALLOC_FLAG_LOOP 0x02 +#define TALLOC_FLAG_POOL 0x04 /* This is a talloc pool */ +#define TALLOC_FLAG_POOLMEM 0x08 /* This is allocated in a pool */ +#define TALLOC_MAGIC_REFERENCE ((const char *)1) + +/* by default we abort when given a bad pointer (such as when talloc_free() is called + on a pointer that came from malloc() */ +#ifndef TALLOC_ABORT +#define TALLOC_ABORT(reason) abort() +#endif + +#ifndef discard_const_p +#if defined(__intptr_t_defined) || defined(HAVE_INTPTR_T) +# define discard_const_p(type, ptr) ((type *)((intptr_t)(ptr))) +#else +# define discard_const_p(type, ptr) ((type *)(ptr)) +#endif +#endif + +/* these macros gain us a few percent of speed on gcc */ +#if (__GNUC__ >= 3) +/* the strange !! is to ensure that __builtin_expect() takes either 0 or 1 + as its first argument */ +#ifndef likely +#define likely(x) __builtin_expect(!!(x), 1) +#endif +#ifndef unlikely +#define unlikely(x) __builtin_expect(!!(x), 0) +#endif +#else +#ifndef likely +#define likely(x) (x) +#endif +#ifndef unlikely +#define unlikely(x) (x) +#endif +#endif + +#ifdef __APPLE__ +/* taken from http://insanecoding.blogspot.com/2007/03/methods-for-safe-string-handling.html */ +size_t strnlen(const char *s, size_t n) +{ + const char *p = (const char *)memchr(s, 0, n); + return(p ? p-s : n); +} +#endif + +/* this null_context is only used if talloc_enable_leak_report() or + talloc_enable_leak_report_full() is called, otherwise it remains + NULL +*/ +static void *null_context; +static void *autofree_context; + +struct talloc_reference_handle { + struct talloc_reference_handle *next, *prev; + void *ptr; +}; + +typedef int (*talloc_destructor_t)(void *); + +struct talloc_chunk { + struct talloc_chunk *next, *prev; + struct talloc_chunk *parent, *child; + struct talloc_reference_handle *refs; + talloc_destructor_t destructor; + const char *name; + size_t size; + unsigned flags; + + /* + * "pool" has dual use: + * + * For the talloc pool itself (i.e. TALLOC_FLAG_POOL is set), "pool" + * marks the end of the currently allocated area. + * + * For members of the pool (i.e. TALLOC_FLAG_POOLMEM is set), "pool" + * is a pointer to the struct talloc_chunk of the pool that it was + * allocated from. This way children can quickly find the pool to chew + * from. + */ + void *pool; +}; + +/* 16 byte alignment seems to keep everyone happy */ +#define TC_HDR_SIZE ((sizeof(struct talloc_chunk)+15)&~15) +#define TC_PTR_FROM_CHUNK(tc) ((void *)(TC_HDR_SIZE + (char*)tc)) + +static void (*talloc_abort_fn)(const char *reason); + +void talloc_set_abort_fn(void (*abort_fn)(const char *reason)) +{ + talloc_abort_fn = abort_fn; +} + +static void talloc_abort(const char *reason) +{ + if (!talloc_abort_fn) { + TALLOC_ABORT(reason); + } + + talloc_abort_fn(reason); +} + +static void talloc_abort_double_free(void) +{ + talloc_abort("Bad talloc magic value - double free"); +} + +static void talloc_abort_unknown_value(void) +{ + talloc_abort("Bad talloc magic value - unknown value"); +} + +/* panic if we get a bad magic value */ +static inline struct talloc_chunk *talloc_chunk_from_ptr(const void *ptr) +{ + const char *pp = (const char *)ptr; + struct talloc_chunk *tc = discard_const_p(struct talloc_chunk, pp - TC_HDR_SIZE); + if (unlikely((tc->flags & (TALLOC_FLAG_FREE | ~0xF)) != TALLOC_MAGIC)) { + if (tc->flags & TALLOC_FLAG_FREE) { + talloc_abort_double_free(); + } else { + talloc_abort_unknown_value(); + } + } + return tc; +} + +/* hook into the front of the list */ +#define _TLIST_ADD(list, p) \ +do { \ + if (!(list)) { \ + (list) = (p); \ + (p)->next = (p)->prev = NULL; \ + } else { \ + (list)->prev = (p); \ + (p)->next = (list); \ + (p)->prev = NULL; \ + (list) = (p); \ + }\ +} while (0) + +/* remove an element from a list - element doesn't have to be in list. */ +#define _TLIST_REMOVE(list, p) \ +do { \ + if ((p) == (list)) { \ + (list) = (p)->next; \ + if (list) (list)->prev = NULL; \ + } else { \ + if ((p)->prev) (p)->prev->next = (p)->next; \ + if ((p)->next) (p)->next->prev = (p)->prev; \ + } \ + if ((p) && ((p) != (list))) (p)->next = (p)->prev = NULL; \ +} while (0) + + +/* + return the parent chunk of a pointer +*/ +static inline struct talloc_chunk *talloc_parent_chunk(const void *ptr) +{ + struct talloc_chunk *tc; + + if (unlikely(ptr == NULL)) { + return NULL; + } + + tc = talloc_chunk_from_ptr(ptr); + while (tc->prev) tc=tc->prev; + + return tc->parent; +} + +void *talloc_parent(const void *ptr) +{ + struct talloc_chunk *tc = talloc_parent_chunk(ptr); + return tc? TC_PTR_FROM_CHUNK(tc) : NULL; +} + +/* + find parents name +*/ +const char *talloc_parent_name(const void *ptr) +{ + struct talloc_chunk *tc = talloc_parent_chunk(ptr); + return tc? tc->name : NULL; +} + +/* + A pool carries an in-pool object count count in the first 16 bytes. + bytes. This is done to support talloc_steal() to a parent outside of the + pool. The count includes the pool itself, so a talloc_free() on a pool will + only destroy the pool if the count has dropped to zero. A talloc_free() of a + pool member will reduce the count, and eventually also call free(3) on the + pool memory. + + The object count is not put into "struct talloc_chunk" because it is only + relevant for talloc pools and the alignment to 16 bytes would increase the + memory footprint of each talloc chunk by those 16 bytes. +*/ + +#define TALLOC_POOL_HDR_SIZE 16 + +static unsigned int *talloc_pool_objectcount(struct talloc_chunk *tc) +{ + return (unsigned int *)((char *)tc + sizeof(struct talloc_chunk)); +} + +/* + Allocate from a pool +*/ + +static struct talloc_chunk *talloc_alloc_pool(struct talloc_chunk *parent, + size_t size) +{ + struct talloc_chunk *pool_ctx = NULL; + size_t space_left; + struct talloc_chunk *result; + size_t chunk_size; + + if (parent == NULL) { + return NULL; + } + + if (parent->flags & TALLOC_FLAG_POOL) { + pool_ctx = parent; + } + else if (parent->flags & TALLOC_FLAG_POOLMEM) { + pool_ctx = (struct talloc_chunk *)parent->pool; + } + + if (pool_ctx == NULL) { + return NULL; + } + + space_left = ((char *)pool_ctx + TC_HDR_SIZE + pool_ctx->size) + - ((char *)pool_ctx->pool); + + /* + * Align size to 16 bytes + */ + chunk_size = ((size + 15) & ~15); + + if (space_left < chunk_size) { + return NULL; + } + + result = (struct talloc_chunk *)pool_ctx->pool; + +#if defined(DEVELOPER) && defined(VALGRIND_MAKE_MEM_UNDEFINED) + VALGRIND_MAKE_MEM_UNDEFINED(result, size); +#endif + + pool_ctx->pool = (void *)((char *)result + chunk_size); + + result->flags = TALLOC_MAGIC | TALLOC_FLAG_POOLMEM; + result->pool = pool_ctx; + + *talloc_pool_objectcount(pool_ctx) += 1; + + return result; +} + +/* + Allocate a bit of memory as a child of an existing pointer +*/ +static inline void *__talloc(const void *context, size_t size) +{ + struct talloc_chunk *tc = NULL; + + if (unlikely(context == NULL)) { + context = null_context; + } + + if (unlikely(size >= MAX_TALLOC_SIZE)) { + return NULL; + } + + if (context != NULL) { + tc = talloc_alloc_pool(talloc_chunk_from_ptr(context), + TC_HDR_SIZE+size); + } + + if (tc == NULL) { + tc = (struct talloc_chunk *)malloc(TC_HDR_SIZE+size); + if (unlikely(tc == NULL)) return NULL; + tc->flags = TALLOC_MAGIC; + tc->pool = NULL; + } + + tc->size = size; + tc->destructor = NULL; + tc->child = NULL; + tc->name = NULL; + tc->refs = NULL; + + if (likely(context)) { + struct talloc_chunk *parent = talloc_chunk_from_ptr(context); + + if (parent->child) { + parent->child->parent = NULL; + tc->next = parent->child; + tc->next->prev = tc; + } else { + tc->next = NULL; + } + tc->parent = parent; + tc->prev = NULL; + parent->child = tc; + } else { + tc->next = tc->prev = tc->parent = NULL; + } + + return TC_PTR_FROM_CHUNK(tc); +} + +/* + * Create a talloc pool + */ + +void *talloc_pool(const void *context, size_t size) +{ + void *result = __talloc(context, size + TALLOC_POOL_HDR_SIZE); + struct talloc_chunk *tc; + + if (unlikely(result == NULL)) { + return NULL; + } + + tc = talloc_chunk_from_ptr(result); + + tc->flags |= TALLOC_FLAG_POOL; + tc->pool = (char *)result + TALLOC_POOL_HDR_SIZE; + + *talloc_pool_objectcount(tc) = 1; + +#if defined(DEVELOPER) && defined(VALGRIND_MAKE_MEM_NOACCESS) + VALGRIND_MAKE_MEM_NOACCESS(tc->pool, size); +#endif + + return result; +} + +/* + setup a destructor to be called on free of a pointer + the destructor should return 0 on success, or -1 on failure. + if the destructor fails then the free is failed, and the memory can + be continued to be used +*/ +void _talloc_set_destructor(const void *ptr, int (*destructor)(void *)) +{ + struct talloc_chunk *tc = talloc_chunk_from_ptr(ptr); + tc->destructor = destructor; +} + +/* + increase the reference count on a piece of memory. +*/ +int talloc_increase_ref_count(const void *ptr) +{ + if (unlikely(!talloc_reference(null_context, ptr))) { + return -1; + } + return 0; +} + +/* + helper for talloc_reference() + + this is referenced by a function pointer and should not be inline +*/ +static int talloc_reference_destructor(struct talloc_reference_handle *handle) +{ + struct talloc_chunk *ptr_tc = talloc_chunk_from_ptr(handle->ptr); + _TLIST_REMOVE(ptr_tc->refs, handle); + return 0; +} + +/* + more efficient way to add a name to a pointer - the name must point to a + true string constant +*/ +static inline void _talloc_set_name_const(const void *ptr, const char *name) +{ + struct talloc_chunk *tc = talloc_chunk_from_ptr(ptr); + tc->name = name; +} + +/* + internal talloc_named_const() +*/ +static inline void *_talloc_named_const(const void *context, size_t size, const char *name) +{ + void *ptr; + + ptr = __talloc(context, size); + if (unlikely(ptr == NULL)) { + return NULL; + } + + _talloc_set_name_const(ptr, name); + + return ptr; +} + +/* + make a secondary reference to a pointer, hanging off the given context. + the pointer remains valid until both the original caller and this given + context are freed. + + the major use for this is when two different structures need to reference the + same underlying data, and you want to be able to free the two instances separately, + and in either order +*/ +void *_talloc_reference(const void *context, const void *ptr) +{ + struct talloc_chunk *tc; + struct talloc_reference_handle *handle; + if (unlikely(ptr == NULL)) return NULL; + + tc = talloc_chunk_from_ptr(ptr); + handle = (struct talloc_reference_handle *)_talloc_named_const(context, + sizeof(struct talloc_reference_handle), + TALLOC_MAGIC_REFERENCE); + if (unlikely(handle == NULL)) return NULL; + + /* note that we hang the destructor off the handle, not the + main context as that allows the caller to still setup their + own destructor on the context if they want to */ + talloc_set_destructor(handle, talloc_reference_destructor); + handle->ptr = discard_const_p(void, ptr); + _TLIST_ADD(tc->refs, handle); + return handle->ptr; +} + + +/* + internal talloc_free call +*/ +static inline int _talloc_free(void *ptr) +{ + struct talloc_chunk *tc; + + if (unlikely(ptr == NULL)) { + return -1; + } + + tc = talloc_chunk_from_ptr(ptr); + + if (unlikely(tc->refs)) { + int is_child; + /* check this is a reference from a child or grantchild + * back to it's parent or grantparent + * + * in that case we need to remove the reference and + * call another instance of talloc_free() on the current + * pointer. + */ + is_child = talloc_is_parent(tc->refs, ptr); + _talloc_free(tc->refs); + if (is_child) { + return _talloc_free(ptr); + } + return -1; + } + + if (unlikely(tc->flags & TALLOC_FLAG_LOOP)) { + /* we have a free loop - stop looping */ + return 0; + } + + if (unlikely(tc->destructor)) { + talloc_destructor_t d = tc->destructor; + if (d == (talloc_destructor_t)-1) { + return -1; + } + tc->destructor = (talloc_destructor_t)-1; + if (d(ptr) == -1) { + tc->destructor = d; + return -1; + } + tc->destructor = NULL; + } + + if (tc->parent) { + _TLIST_REMOVE(tc->parent->child, tc); + if (tc->parent->child) { + tc->parent->child->parent = tc->parent; + } + } else { + if (tc->prev) tc->prev->next = tc->next; + if (tc->next) tc->next->prev = tc->prev; + } + + tc->flags |= TALLOC_FLAG_LOOP; + + while (tc->child) { + /* we need to work out who will own an abandoned child + if it cannot be freed. In priority order, the first + choice is owner of any remaining reference to this + pointer, the second choice is our parent, and the + final choice is the null context. */ + void *child = TC_PTR_FROM_CHUNK(tc->child); + const void *new_parent = null_context; + if (unlikely(tc->child->refs)) { + struct talloc_chunk *p = talloc_parent_chunk(tc->child->refs); + if (p) new_parent = TC_PTR_FROM_CHUNK(p); + } + if (unlikely(_talloc_free(child) == -1)) { + if (new_parent == null_context) { + struct talloc_chunk *p = talloc_parent_chunk(ptr); + if (p) new_parent = TC_PTR_FROM_CHUNK(p); + } + talloc_steal(new_parent, child); + } + } + + tc->flags |= TALLOC_FLAG_FREE; + + if (tc->flags & (TALLOC_FLAG_POOL|TALLOC_FLAG_POOLMEM)) { + struct talloc_chunk *pool; + unsigned int *pool_object_count; + + pool = (tc->flags & TALLOC_FLAG_POOL) + ? tc : (struct talloc_chunk *)tc->pool; + + pool_object_count = talloc_pool_objectcount(pool); + + if (*pool_object_count == 0) { + talloc_abort("Pool object count zero!"); + } + + *pool_object_count -= 1; + + if (*pool_object_count == 0) { + free(pool); + } + } + else { + free(tc); + } + return 0; +} + +/* + move a lump of memory from one talloc context to another return the + ptr on success, or NULL if it could not be transferred. + passing NULL as ptr will always return NULL with no side effects. +*/ +void *_talloc_steal(const void *new_ctx, const void *ptr) +{ + struct talloc_chunk *tc, *new_tc; + + if (unlikely(!ptr)) { + return NULL; + } + + if (unlikely(new_ctx == NULL)) { + new_ctx = null_context; + } + + tc = talloc_chunk_from_ptr(ptr); + + if (unlikely(new_ctx == NULL)) { + if (tc->parent) { + _TLIST_REMOVE(tc->parent->child, tc); + if (tc->parent->child) { + tc->parent->child->parent = tc->parent; + } + } else { + if (tc->prev) tc->prev->next = tc->next; + if (tc->next) tc->next->prev = tc->prev; + } + + tc->parent = tc->next = tc->prev = NULL; + return discard_const_p(void, ptr); + } + + new_tc = talloc_chunk_from_ptr(new_ctx); + + if (unlikely(tc == new_tc || tc->parent == new_tc)) { + return discard_const_p(void, ptr); + } + + if (tc->parent) { + _TLIST_REMOVE(tc->parent->child, tc); + if (tc->parent->child) { + tc->parent->child->parent = tc->parent; + } + } else { + if (tc->prev) tc->prev->next = tc->next; + if (tc->next) tc->next->prev = tc->prev; + } + + tc->parent = new_tc; + if (new_tc->child) new_tc->child->parent = NULL; + _TLIST_ADD(new_tc->child, tc); + + return discard_const_p(void, ptr); +} + + + +/* + remove a secondary reference to a pointer. This undo's what + talloc_reference() has done. The context and pointer arguments + must match those given to a talloc_reference() +*/ +static inline int talloc_unreference(const void *context, const void *ptr) +{ + struct talloc_chunk *tc = talloc_chunk_from_ptr(ptr); + struct talloc_reference_handle *h; + + if (unlikely(context == NULL)) { + context = null_context; + } + + for (h=tc->refs;h;h=h->next) { + struct talloc_chunk *p = talloc_parent_chunk(h); + if (p == NULL) { + if (context == NULL) break; + } else if (TC_PTR_FROM_CHUNK(p) == context) { + break; + } + } + if (h == NULL) { + return -1; + } + + return _talloc_free(h); +} + +/* + remove a specific parent context from a pointer. This is a more + controlled varient of talloc_free() +*/ +int talloc_unlink(const void *context, void *ptr) +{ + struct talloc_chunk *tc_p, *new_p; + void *new_parent; + + if (ptr == NULL) { + return -1; + } + + if (context == NULL) { + context = null_context; + } + + if (talloc_unreference(context, ptr) == 0) { + return 0; + } + + if (context == NULL) { + if (talloc_parent_chunk(ptr) != NULL) { + return -1; + } + } else { + if (talloc_chunk_from_ptr(context) != talloc_parent_chunk(ptr)) { + return -1; + } + } + + tc_p = talloc_chunk_from_ptr(ptr); + + if (tc_p->refs == NULL) { + return _talloc_free(ptr); + } + + new_p = talloc_parent_chunk(tc_p->refs); + if (new_p) { + new_parent = TC_PTR_FROM_CHUNK(new_p); + } else { + new_parent = NULL; + } + + if (talloc_unreference(new_parent, ptr) != 0) { + return -1; + } + + talloc_steal(new_parent, ptr); + + return 0; +} + +/* + add a name to an existing pointer - va_list version +*/ +static inline const char *talloc_set_name_v(const void *ptr, const char *fmt, va_list ap) PRINTF_ATTRIBUTE(2,0); + +static inline const char *talloc_set_name_v(const void *ptr, const char *fmt, va_list ap) +{ + struct talloc_chunk *tc = talloc_chunk_from_ptr(ptr); + tc->name = talloc_vasprintf(ptr, fmt, ap); + if (likely(tc->name)) { + _talloc_set_name_const(tc->name, ".name"); + } + return tc->name; +} + +/* + add a name to an existing pointer +*/ +const char *talloc_set_name(const void *ptr, const char *fmt, ...) +{ + const char *name; + va_list ap; + va_start(ap, fmt); + name = talloc_set_name_v(ptr, fmt, ap); + va_end(ap); + return name; +} + + +/* + create a named talloc pointer. Any talloc pointer can be named, and + talloc_named() operates just like talloc() except that it allows you + to name the pointer. +*/ +void *talloc_named(const void *context, size_t size, const char *fmt, ...) +{ + va_list ap; + void *ptr; + const char *name; + + ptr = __talloc(context, size); + if (unlikely(ptr == NULL)) return NULL; + + va_start(ap, fmt); + name = talloc_set_name_v(ptr, fmt, ap); + va_end(ap); + + if (unlikely(name == NULL)) { + _talloc_free(ptr); + return NULL; + } + + return ptr; +} + +/* + return the name of a talloc ptr, or "UNNAMED" +*/ +const char *talloc_get_name(const void *ptr) +{ + struct talloc_chunk *tc = talloc_chunk_from_ptr(ptr); + if (unlikely(tc->name == TALLOC_MAGIC_REFERENCE)) { + return ".reference"; + } + if (likely(tc->name)) { + return tc->name; + } + return "UNNAMED"; +} + + +/* + check if a pointer has the given name. If it does, return the pointer, + otherwise return NULL +*/ +void *talloc_check_name(const void *ptr, const char *name) +{ + const char *pname; + if (unlikely(ptr == NULL)) return NULL; + pname = talloc_get_name(ptr); + if (likely(pname == name || strcmp(pname, name) == 0)) { + return discard_const_p(void, ptr); + } + return NULL; +} + +static void talloc_abort_type_missmatch(const char *location, + const char *name, + const char *expected) +{ + const char *reason; + + reason = talloc_asprintf(NULL, + "%s: Type mismatch: name[%s] expected[%s]", + location, + name?name:"NULL", + expected); + if (!reason) { + reason = "Type mismatch"; + } + + talloc_abort(reason); +} + +void *_talloc_get_type_abort(const void *ptr, const char *name, const char *location) +{ + const char *pname; + + if (unlikely(ptr == NULL)) { + talloc_abort_type_missmatch(location, NULL, name); + return NULL; + } + + pname = talloc_get_name(ptr); + if (likely(pname == name || strcmp(pname, name) == 0)) { + return discard_const_p(void, ptr); + } + + talloc_abort_type_missmatch(location, pname, name); + return NULL; +} + +/* + this is for compatibility with older versions of talloc +*/ +void *talloc_init(const char *fmt, ...) +{ + va_list ap; + void *ptr; + const char *name; + + /* + * samba3 expects talloc_report_depth_cb(NULL, ...) + * reports all talloc'ed memory, so we need to enable + * null_tracking + */ + talloc_enable_null_tracking(); + + ptr = __talloc(NULL, 0); + if (unlikely(ptr == NULL)) return NULL; + + va_start(ap, fmt); + name = talloc_set_name_v(ptr, fmt, ap); + va_end(ap); + + if (unlikely(name == NULL)) { + _talloc_free(ptr); + return NULL; + } + + return ptr; +} + +/* + this is a replacement for the Samba3 talloc_destroy_pool functionality. It + should probably not be used in new code. It's in here to keep the talloc + code consistent across Samba 3 and 4. +*/ +void talloc_free_children(void *ptr) +{ + struct talloc_chunk *tc; + + if (unlikely(ptr == NULL)) { + return; + } + + tc = talloc_chunk_from_ptr(ptr); + + while (tc->child) { + /* we need to work out who will own an abandoned child + if it cannot be freed. In priority order, the first + choice is owner of any remaining reference to this + pointer, the second choice is our parent, and the + final choice is the null context. */ + void *child = TC_PTR_FROM_CHUNK(tc->child); + const void *new_parent = null_context; + if (unlikely(tc->child->refs)) { + struct talloc_chunk *p = talloc_parent_chunk(tc->child->refs); + if (p) new_parent = TC_PTR_FROM_CHUNK(p); + } + if (unlikely(_talloc_free(child) == -1)) { + if (new_parent == null_context) { + struct talloc_chunk *p = talloc_parent_chunk(ptr); + if (p) new_parent = TC_PTR_FROM_CHUNK(p); + } + talloc_steal(new_parent, child); + } + } + + if ((tc->flags & TALLOC_FLAG_POOL) + && (*talloc_pool_objectcount(tc) == 1)) { + tc->pool = ((char *)tc + TC_HDR_SIZE + TALLOC_POOL_HDR_SIZE); +#if defined(DEVELOPER) && defined(VALGRIND_MAKE_MEM_NOACCESS) + VALGRIND_MAKE_MEM_NOACCESS( + tc->pool, tc->size - TALLOC_POOL_HDR_SIZE); +#endif + } +} + +/* + Allocate a bit of memory as a child of an existing pointer +*/ +void *_talloc(const void *context, size_t size) +{ + return __talloc(context, size); +} + +/* + externally callable talloc_set_name_const() +*/ +void talloc_set_name_const(const void *ptr, const char *name) +{ + _talloc_set_name_const(ptr, name); +} + +/* + create a named talloc pointer. Any talloc pointer can be named, and + talloc_named() operates just like talloc() except that it allows you + to name the pointer. +*/ +void *talloc_named_const(const void *context, size_t size, const char *name) +{ + return _talloc_named_const(context, size, name); +} + +/* + free a talloc pointer. This also frees all child pointers of this + pointer recursively + + return 0 if the memory is actually freed, otherwise -1. The memory + will not be freed if the ref_count is > 1 or the destructor (if + any) returns non-zero +*/ +int talloc_free(void *ptr) +{ + return _talloc_free(ptr); +} + + + +/* + A talloc version of realloc. The context argument is only used if + ptr is NULL +*/ +void *_talloc_realloc(const void *context, void *ptr, size_t size, const char *name) +{ + struct talloc_chunk *tc; + void *new_ptr; + bool malloced = false; + + /* size zero is equivalent to free() */ + if (unlikely(size == 0)) { + _talloc_free(ptr); + return NULL; + } + + if (unlikely(size >= MAX_TALLOC_SIZE)) { + return NULL; + } + + /* realloc(NULL) is equivalent to malloc() */ + if (ptr == NULL) { + return _talloc_named_const(context, size, name); + } + + tc = talloc_chunk_from_ptr(ptr); + + /* don't allow realloc on referenced pointers */ + if (unlikely(tc->refs)) { + return NULL; + } + + /* don't let anybody try to realloc a talloc_pool */ + if (unlikely(tc->flags & TALLOC_FLAG_POOL)) { + return NULL; + } + + /* don't shrink if we have less than 1k to gain */ + if ((size < tc->size) && ((tc->size - size) < 1024)) { + tc->size = size; + return ptr; + } + + /* by resetting magic we catch users of the old memory */ + tc->flags |= TALLOC_FLAG_FREE; + +#if ALWAYS_REALLOC + new_ptr = malloc(size + TC_HDR_SIZE); + if (new_ptr) { + memcpy(new_ptr, tc, tc->size + TC_HDR_SIZE); + free(tc); + } +#else + if (tc->flags & TALLOC_FLAG_POOLMEM) { + + new_ptr = talloc_alloc_pool(tc, size + TC_HDR_SIZE); + *talloc_pool_objectcount((struct talloc_chunk *) + (tc->pool)) -= 1; + + if (new_ptr == NULL) { + new_ptr = malloc(TC_HDR_SIZE+size); + malloced = true; + } + + if (new_ptr) { + memcpy(new_ptr, tc, MIN(tc->size,size) + TC_HDR_SIZE); + } + } + else { + new_ptr = realloc(tc, size + TC_HDR_SIZE); + } +#endif + if (unlikely(!new_ptr)) { + tc->flags &= ~TALLOC_FLAG_FREE; + return NULL; + } + + tc = (struct talloc_chunk *)new_ptr; + tc->flags &= ~TALLOC_FLAG_FREE; + if (malloced) { + tc->flags &= ~TALLOC_FLAG_POOLMEM; + } + if (tc->parent) { + tc->parent->child = tc; + } + if (tc->child) { + tc->child->parent = tc; + } + + if (tc->prev) { + tc->prev->next = tc; + } + if (tc->next) { + tc->next->prev = tc; + } + + tc->size = size; + _talloc_set_name_const(TC_PTR_FROM_CHUNK(tc), name); + + return TC_PTR_FROM_CHUNK(tc); +} + +/* + a wrapper around talloc_steal() for situations where you are moving a pointer + between two structures, and want the old pointer to be set to NULL +*/ +void *_talloc_move(const void *new_ctx, const void *_pptr) +{ + const void **pptr = discard_const_p(const void *,_pptr); + void *ret = _talloc_steal(new_ctx, *pptr); + (*pptr) = NULL; + return ret; +} + +/* + return the total size of a talloc pool (subtree) +*/ +size_t talloc_total_size(const void *ptr) +{ + size_t total = 0; + struct talloc_chunk *c, *tc; + + if (ptr == NULL) { + ptr = null_context; + } + if (ptr == NULL) { + return 0; + } + + tc = talloc_chunk_from_ptr(ptr); + + if (tc->flags & TALLOC_FLAG_LOOP) { + return 0; + } + + tc->flags |= TALLOC_FLAG_LOOP; + + total = tc->size; + for (c=tc->child;c;c=c->next) { + total += talloc_total_size(TC_PTR_FROM_CHUNK(c)); + } + + tc->flags &= ~TALLOC_FLAG_LOOP; + + return total; +} + +/* + return the total number of blocks in a talloc pool (subtree) +*/ +size_t talloc_total_blocks(const void *ptr) +{ + size_t total = 0; + struct talloc_chunk *c, *tc = talloc_chunk_from_ptr(ptr); + + if (tc->flags & TALLOC_FLAG_LOOP) { + return 0; + } + + tc->flags |= TALLOC_FLAG_LOOP; + + total++; + for (c=tc->child;c;c=c->next) { + total += talloc_total_blocks(TC_PTR_FROM_CHUNK(c)); + } + + tc->flags &= ~TALLOC_FLAG_LOOP; + + return total; +} + +/* + return the number of external references to a pointer +*/ +size_t talloc_reference_count(const void *ptr) +{ + struct talloc_chunk *tc = talloc_chunk_from_ptr(ptr); + struct talloc_reference_handle *h; + size_t ret = 0; + + for (h=tc->refs;h;h=h->next) { + ret++; + } + return ret; +} + +/* + report on memory usage by all children of a pointer, giving a full tree view +*/ +void talloc_report_depth_cb(const void *ptr, int depth, int max_depth, + void (*callback)(const void *ptr, + int depth, int max_depth, + int is_ref, + void *private_data), + void *private_data) +{ + struct talloc_chunk *c, *tc; + + if (ptr == NULL) { + ptr = null_context; + } + if (ptr == NULL) return; + + tc = talloc_chunk_from_ptr(ptr); + + if (tc->flags & TALLOC_FLAG_LOOP) { + return; + } + + callback(ptr, depth, max_depth, 0, private_data); + + if (max_depth >= 0 && depth >= max_depth) { + return; + } + + tc->flags |= TALLOC_FLAG_LOOP; + for (c=tc->child;c;c=c->next) { + if (c->name == TALLOC_MAGIC_REFERENCE) { + struct talloc_reference_handle *h = (struct talloc_reference_handle *)TC_PTR_FROM_CHUNK(c); + callback(h->ptr, depth + 1, max_depth, 1, private_data); + } else { + talloc_report_depth_cb(TC_PTR_FROM_CHUNK(c), depth + 1, max_depth, callback, private_data); + } + } + tc->flags &= ~TALLOC_FLAG_LOOP; +} + +static void talloc_report_depth_FILE_helper(const void *ptr, int depth, int max_depth, int is_ref, void *_f) +{ + const char *name = talloc_get_name(ptr); + FILE *f = (FILE *)_f; + + if (is_ref) { + fprintf(f, "%*sreference to: %s\n", depth*4, "", name); + return; + } + + if (depth == 0) { + fprintf(f,"%stalloc report on '%s' (total %6lu bytes in %3lu blocks)\n", + (max_depth < 0 ? "full " :""), name, + (unsigned long)talloc_total_size(ptr), + (unsigned long)talloc_total_blocks(ptr)); + return; + } + + fprintf(f, "%*s%-30s contains %6lu bytes in %3lu blocks (ref %d) %p\n", + depth*4, "", + name, + (unsigned long)talloc_total_size(ptr), + (unsigned long)talloc_total_blocks(ptr), + (int)talloc_reference_count(ptr), ptr); + +#if 0 + fprintf(f, "content: "); + if (talloc_total_size(ptr)) { + int tot = talloc_total_size(ptr); + int i; + + for (i = 0; i < tot; i++) { + if ((((char *)ptr)[i] > 31) && (((char *)ptr)[i] < 126)) { + fprintf(f, "%c", ((char *)ptr)[i]); + } else { + fprintf(f, "~%02x", ((char *)ptr)[i]); + } + } + } + fprintf(f, "\n"); +#endif +} + +/* + report on memory usage by all children of a pointer, giving a full tree view +*/ +void talloc_report_depth_file(const void *ptr, int depth, int max_depth, FILE *f) +{ + talloc_report_depth_cb(ptr, depth, max_depth, talloc_report_depth_FILE_helper, f); + fflush(f); +} + +/* + report on memory usage by all children of a pointer, giving a full tree view +*/ +void talloc_report_full(const void *ptr, FILE *f) +{ + talloc_report_depth_file(ptr, 0, -1, f); +} + +/* + report on memory usage by all children of a pointer +*/ +void talloc_report(const void *ptr, FILE *f) +{ + talloc_report_depth_file(ptr, 0, 1, f); +} + +/* + report on any memory hanging off the null context +*/ +static void talloc_report_null(void) +{ + if (talloc_total_size(null_context) != 0) { + talloc_report(null_context, stderr); + } +} + +/* + report on any memory hanging off the null context +*/ +static void talloc_report_null_full(void) +{ + if (talloc_total_size(null_context) != 0) { + talloc_report_full(null_context, stderr); + } +} + +/* + enable tracking of the NULL context +*/ +void talloc_enable_null_tracking(void) +{ + if (null_context == NULL) { + null_context = _talloc_named_const(NULL, 0, "null_context"); + } +} + +/* + disable tracking of the NULL context +*/ +void talloc_disable_null_tracking(void) +{ + _talloc_free(null_context); + null_context = NULL; +} + +/* + enable leak reporting on exit +*/ +void talloc_enable_leak_report(void) +{ + talloc_enable_null_tracking(); + atexit(talloc_report_null); +} + +/* + enable full leak reporting on exit +*/ +void talloc_enable_leak_report_full(void) +{ + talloc_enable_null_tracking(); + atexit(talloc_report_null_full); +} + +/* + talloc and zero memory. +*/ +void *_talloc_zero(const void *ctx, size_t size, const char *name) +{ + void *p = _talloc_named_const(ctx, size, name); + + if (p) { + memset(p, '\0', size); + } + + return p; +} + +/* + memdup with a talloc. +*/ +void *_talloc_memdup(const void *t, const void *p, size_t size, const char *name) +{ + void *newp = _talloc_named_const(t, size, name); + + if (likely(newp)) { + memcpy(newp, p, size); + } + + return newp; +} + +static inline char *__talloc_strlendup(const void *t, const char *p, size_t len) +{ + char *ret; + + ret = (char *)__talloc(t, len + 1); + if (unlikely(!ret)) return NULL; + + memcpy(ret, p, len); + ret[len] = 0; + + _talloc_set_name_const(ret, ret); + return ret; +} + +/* + strdup with a talloc +*/ +char *talloc_strdup(const void *t, const char *p) +{ + if (unlikely(!p)) return NULL; + return __talloc_strlendup(t, p, strlen(p)); +} + +/* + strndup with a talloc +*/ +char *talloc_strndup(const void *t, const char *p, size_t n) +{ + if (unlikely(!p)) return NULL; + return __talloc_strlendup(t, p, strnlen(p, n)); +} + +static inline char *__talloc_strlendup_append(char *s, size_t slen, + const char *a, size_t alen) +{ + char *ret; + + ret = talloc_realloc(NULL, s, char, slen + alen + 1); + if (unlikely(!ret)) return NULL; + + /* append the string and the trailing \0 */ + memcpy(&ret[slen], a, alen); + ret[slen+alen] = 0; + + _talloc_set_name_const(ret, ret); + return ret; +} + +/* + * Appends at the end of the string. + */ +char *talloc_strdup_append(char *s, const char *a) +{ + if (unlikely(!s)) { + return talloc_strdup(NULL, a); + } + + if (unlikely(!a)) { + return s; + } + + return __talloc_strlendup_append(s, strlen(s), a, strlen(a)); +} + +/* + * Appends at the end of the talloc'ed buffer, + * not the end of the string. + */ +char *talloc_strdup_append_buffer(char *s, const char *a) +{ + size_t slen; + + if (unlikely(!s)) { + return talloc_strdup(NULL, a); + } + + if (unlikely(!a)) { + return s; + } + + slen = talloc_get_size(s); + if (likely(slen > 0)) { + slen--; + } + + return __talloc_strlendup_append(s, slen, a, strlen(a)); +} + +/* + * Appends at the end of the string. + */ +char *talloc_strndup_append(char *s, const char *a, size_t n) +{ + if (unlikely(!s)) { + return talloc_strdup(NULL, a); + } + + if (unlikely(!a)) { + return s; + } + + return __talloc_strlendup_append(s, strlen(s), a, strnlen(a, n)); +} + +/* + * Appends at the end of the talloc'ed buffer, + * not the end of the string. + */ +char *talloc_strndup_append_buffer(char *s, const char *a, size_t n) +{ + size_t slen; + + if (unlikely(!s)) { + return talloc_strdup(NULL, a); + } + + if (unlikely(!a)) { + return s; + } + + slen = talloc_get_size(s); + if (likely(slen > 0)) { + slen--; + } + + return __talloc_strlendup_append(s, slen, a, strnlen(a, n)); +} + +#ifndef HAVE_VA_COPY +#ifdef HAVE___VA_COPY +#define va_copy(dest, src) __va_copy(dest, src) +#else +#define va_copy(dest, src) (dest) = (src) +#endif +#endif + +char *talloc_vasprintf(const void *t, const char *fmt, va_list ap) +{ + int len; + char *ret; + va_list ap2; + char c; + + /* this call looks strange, but it makes it work on older solaris boxes */ + va_copy(ap2, ap); + len = vsnprintf(&c, 1, fmt, ap2); + va_end(ap2); + if (unlikely(len < 0)) { + return NULL; + } + + ret = (char *)__talloc(t, len+1); + if (unlikely(!ret)) return NULL; + + va_copy(ap2, ap); + vsnprintf(ret, len+1, fmt, ap2); + va_end(ap2); + + _talloc_set_name_const(ret, ret); + return ret; +} + + +/* + Perform string formatting, and return a pointer to newly allocated + memory holding the result, inside a memory pool. + */ +char *talloc_asprintf(const void *t, const char *fmt, ...) +{ + va_list ap; + char *ret; + + va_start(ap, fmt); + ret = talloc_vasprintf(t, fmt, ap); + va_end(ap); + return ret; +} + +static inline char *__talloc_vaslenprintf_append(char *s, size_t slen, + const char *fmt, va_list ap) + PRINTF_ATTRIBUTE(3,0); + +static inline char *__talloc_vaslenprintf_append(char *s, size_t slen, + const char *fmt, va_list ap) +{ + ssize_t alen; + va_list ap2; + char c; + + va_copy(ap2, ap); + alen = vsnprintf(&c, 1, fmt, ap2); + va_end(ap2); + + if (alen <= 0) { + /* Either the vsnprintf failed or the format resulted in + * no characters being formatted. In the former case, we + * ought to return NULL, in the latter we ought to return + * the original string. Most current callers of this + * function expect it to never return NULL. + */ + return s; + } + + s = talloc_realloc(NULL, s, char, slen + alen + 1); + if (!s) return NULL; + + va_copy(ap2, ap); + vsnprintf(s + slen, alen + 1, fmt, ap2); + va_end(ap2); + + _talloc_set_name_const(s, s); + return s; +} + +/** + * Realloc @p s to append the formatted result of @p fmt and @p ap, + * and return @p s, which may have moved. Good for gradually + * accumulating output into a string buffer. Appends at the end + * of the string. + **/ +char *talloc_vasprintf_append(char *s, const char *fmt, va_list ap) +{ + if (unlikely(!s)) { + return talloc_vasprintf(NULL, fmt, ap); + } + + return __talloc_vaslenprintf_append(s, strlen(s), fmt, ap); +} + +/** + * Realloc @p s to append the formatted result of @p fmt and @p ap, + * and return @p s, which may have moved. Always appends at the + * end of the talloc'ed buffer, not the end of the string. + **/ +char *talloc_vasprintf_append_buffer(char *s, const char *fmt, va_list ap) +{ + size_t slen; + + if (unlikely(!s)) { + return talloc_vasprintf(NULL, fmt, ap); + } + + slen = talloc_get_size(s); + if (likely(slen > 0)) { + slen--; + } + + return __talloc_vaslenprintf_append(s, slen, fmt, ap); +} + +/* + Realloc @p s to append the formatted result of @p fmt and return @p + s, which may have moved. Good for gradually accumulating output + into a string buffer. + */ +char *talloc_asprintf_append(char *s, const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + s = talloc_vasprintf_append(s, fmt, ap); + va_end(ap); + return s; +} + +/* + Realloc @p s to append the formatted result of @p fmt and return @p + s, which may have moved. Good for gradually accumulating output + into a buffer. + */ +char *talloc_asprintf_append_buffer(char *s, const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + s = talloc_vasprintf_append_buffer(s, fmt, ap); + va_end(ap); + return s; +} + +/* + alloc an array, checking for integer overflow in the array size +*/ +void *_talloc_array(const void *ctx, size_t el_size, unsigned count, const char *name) +{ + if (count >= MAX_TALLOC_SIZE/el_size) { + return NULL; + } + return _talloc_named_const(ctx, el_size * count, name); +} + +/* + alloc an zero array, checking for integer overflow in the array size +*/ +void *_talloc_zero_array(const void *ctx, size_t el_size, unsigned count, const char *name) +{ + if (count >= MAX_TALLOC_SIZE/el_size) { + return NULL; + } + return _talloc_zero(ctx, el_size * count, name); +} + +/* + realloc an array, checking for integer overflow in the array size +*/ +void *_talloc_realloc_array(const void *ctx, void *ptr, size_t el_size, unsigned count, const char *name) +{ + if (count >= MAX_TALLOC_SIZE/el_size) { + return NULL; + } + return _talloc_realloc(ctx, ptr, el_size * count, name); +} + +/* + a function version of talloc_realloc(), so it can be passed as a function pointer + to libraries that want a realloc function (a realloc function encapsulates + all the basic capabilities of an allocation library, which is why this is useful) +*/ +void *talloc_realloc_fn(const void *context, void *ptr, size_t size) +{ + return _talloc_realloc(context, ptr, size, NULL); +} + + +static int talloc_autofree_destructor(void *ptr) +{ + autofree_context = NULL; + return 0; +} + +static void talloc_autofree(void) +{ + _talloc_free(autofree_context); +} + +/* + return a context which will be auto-freed on exit + this is useful for reducing the noise in leak reports +*/ +void *talloc_autofree_context(void) +{ + if (autofree_context == NULL) { + autofree_context = _talloc_named_const(NULL, 0, "autofree_context"); + talloc_set_destructor(autofree_context, talloc_autofree_destructor); + atexit(talloc_autofree); + } + return autofree_context; +} + +size_t talloc_get_size(const void *context) +{ + struct talloc_chunk *tc; + + if (context == NULL) + return 0; + + tc = talloc_chunk_from_ptr(context); + + return tc->size; +} + +/* + find a parent of this context that has the given name, if any +*/ +void *talloc_find_parent_byname(const void *context, const char *name) +{ + struct talloc_chunk *tc; + + if (context == NULL) { + return NULL; + } + + tc = talloc_chunk_from_ptr(context); + while (tc) { + if (tc->name && strcmp(tc->name, name) == 0) { + return TC_PTR_FROM_CHUNK(tc); + } + while (tc && tc->prev) tc = tc->prev; + if (tc) { + tc = tc->parent; + } + } + return NULL; +} + +/* + show the parentage of a context +*/ +void talloc_show_parents(const void *context, FILE *file) +{ + struct talloc_chunk *tc; + + if (context == NULL) { + fprintf(file, "talloc no parents for NULL\n"); + return; + } + + tc = talloc_chunk_from_ptr(context); + fprintf(file, "talloc parents of '%s'\n", talloc_get_name(context)); + while (tc) { + fprintf(file, "\t'%s'\n", talloc_get_name(TC_PTR_FROM_CHUNK(tc))); + while (tc && tc->prev) tc = tc->prev; + if (tc) { + tc = tc->parent; + } + } + fflush(file); +} + +/* + return 1 if ptr is a parent of context +*/ +int talloc_is_parent(const void *context, const void *ptr) +{ + struct talloc_chunk *tc; + + if (context == NULL) { + return 0; + } + + tc = talloc_chunk_from_ptr(context); + while (tc) { + if (TC_PTR_FROM_CHUNK(tc) == ptr) return 1; + while (tc && tc->prev) tc = tc->prev; + if (tc) { + tc = tc->parent; + } + } + return 0; +} diff --git a/misc/tools/osmocon/talloc.h b/misc/tools/osmocon/talloc.h new file mode 100644 index 000000000..f7f7643b8 --- /dev/null +++ b/misc/tools/osmocon/talloc.h @@ -0,0 +1,192 @@ +#ifndef _TALLOC_H_ +#define _TALLOC_H_ +/* + Unix SMB/CIFS implementation. + Samba temporary memory allocation functions + + Copyright (C) Andrew Tridgell 2004-2005 + Copyright (C) Stefan Metzmacher 2006 + + ** NOTE! The following LGPL license applies to the talloc + ** library. This does NOT imply that all of Samba is released + ** under the LGPL + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 3 of the License, or (at your option) any later version. + + This library 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, see . +*/ + +#include +#include +#include + +#define HAVE_VA_COPY + +/* this is only needed for compatibility with the old talloc */ +typedef void TALLOC_CTX; + +/* + this uses a little trick to allow __LINE__ to be stringified +*/ +#ifndef __location__ +#define __TALLOC_STRING_LINE1__(s) #s +#define __TALLOC_STRING_LINE2__(s) __TALLOC_STRING_LINE1__(s) +#define __TALLOC_STRING_LINE3__ __TALLOC_STRING_LINE2__(__LINE__) +#define __location__ __FILE__ ":" __TALLOC_STRING_LINE3__ +#endif + +#ifndef TALLOC_DEPRECATED +#define TALLOC_DEPRECATED 0 +#endif + +#ifndef PRINTF_ATTRIBUTE +#if (__GNUC__ >= 3) +/** Use gcc attribute to check printf fns. a1 is the 1-based index of + * the parameter containing the format, and a2 the index of the first + * argument. Note that some gcc 2.x versions don't handle this + * properly **/ +#define PRINTF_ATTRIBUTE(a1, a2) __attribute__ ((format (__printf__, a1, a2))) +#else +#define PRINTF_ATTRIBUTE(a1, a2) +#endif +#endif + +/* try to make talloc_set_destructor() and talloc_steal() type safe, + if we have a recent gcc */ +#if (__GNUC__ >= 3) +#define _TALLOC_TYPEOF(ptr) __typeof__(ptr) +#define talloc_set_destructor(ptr, function) \ + do { \ + int (*_talloc_destructor_fn)(_TALLOC_TYPEOF(ptr)) = (function); \ + _talloc_set_destructor((ptr), (int (*)(void *))_talloc_destructor_fn); \ + } while(0) +/* this extremely strange macro is to avoid some braindamaged warning + stupidity in gcc 4.1.x */ +#define talloc_steal(ctx, ptr) ({ _TALLOC_TYPEOF(ptr) __talloc_steal_ret = (_TALLOC_TYPEOF(ptr))_talloc_steal((ctx),(ptr)); __talloc_steal_ret; }) +#else +#define talloc_set_destructor(ptr, function) \ + _talloc_set_destructor((ptr), (int (*)(void *))(function)) +#define _TALLOC_TYPEOF(ptr) void * +#define talloc_steal(ctx, ptr) (_TALLOC_TYPEOF(ptr))_talloc_steal((ctx),(ptr)) +#endif + +#define talloc_reference(ctx, ptr) (_TALLOC_TYPEOF(ptr))_talloc_reference((ctx),(ptr)) +#define talloc_move(ctx, ptr) (_TALLOC_TYPEOF(*(ptr)))_talloc_move((ctx),(void *)(ptr)) + +/* useful macros for creating type checked pointers */ +#define talloc(ctx, type) (type *)talloc_named_const(ctx, sizeof(type), #type) +#define talloc_size(ctx, size) talloc_named_const(ctx, size, __location__) +#define talloc_ptrtype(ctx, ptr) (_TALLOC_TYPEOF(ptr))talloc_size(ctx, sizeof(*(ptr))) + +#define talloc_new(ctx) talloc_named_const(ctx, 0, "talloc_new: " __location__) + +#define talloc_zero(ctx, type) (type *)_talloc_zero(ctx, sizeof(type), #type) +#define talloc_zero_size(ctx, size) _talloc_zero(ctx, size, __location__) + +#define talloc_zero_array(ctx, type, count) (type *)_talloc_zero_array(ctx, sizeof(type), count, #type) +#define talloc_array(ctx, type, count) (type *)_talloc_array(ctx, sizeof(type), count, #type) +#define talloc_array_size(ctx, size, count) _talloc_array(ctx, size, count, __location__) +#define talloc_array_ptrtype(ctx, ptr, count) (_TALLOC_TYPEOF(ptr))talloc_array_size(ctx, sizeof(*(ptr)), count) +#define talloc_array_length(ctx) (talloc_get_size(ctx)/sizeof(*ctx)) + +#define talloc_realloc(ctx, p, type, count) (type *)_talloc_realloc_array(ctx, p, sizeof(type), count, #type) +#define talloc_realloc_size(ctx, ptr, size) _talloc_realloc(ctx, ptr, size, __location__) + +#define talloc_memdup(t, p, size) _talloc_memdup(t, p, size, __location__) + +#define talloc_set_type(ptr, type) talloc_set_name_const(ptr, #type) +#define talloc_get_type(ptr, type) (type *)talloc_check_name(ptr, #type) +#define talloc_get_type_abort(ptr, type) (type *)_talloc_get_type_abort(ptr, #type, __location__) + +#define talloc_find_parent_bytype(ptr, type) (type *)talloc_find_parent_byname(ptr, #type) + +#if TALLOC_DEPRECATED +#define talloc_zero_p(ctx, type) talloc_zero(ctx, type) +#define talloc_p(ctx, type) talloc(ctx, type) +#define talloc_array_p(ctx, type, count) talloc_array(ctx, type, count) +#define talloc_realloc_p(ctx, p, type, count) talloc_realloc(ctx, p, type, count) +#define talloc_destroy(ctx) talloc_free(ctx) +#define talloc_append_string(c, s, a) (s?talloc_strdup_append(s,a):talloc_strdup(c, a)) +#endif + +#define TALLOC_FREE(ctx) do { talloc_free(ctx); ctx=NULL; } while(0) + +/* The following definitions come from talloc.c */ +void *_talloc(const void *context, size_t size); +void *talloc_pool(const void *context, size_t size); +void _talloc_set_destructor(const void *ptr, int (*_destructor)(void *)); +int talloc_increase_ref_count(const void *ptr); +size_t talloc_reference_count(const void *ptr); +void *_talloc_reference(const void *context, const void *ptr); +int talloc_unlink(const void *context, void *ptr); +const char *talloc_set_name(const void *ptr, const char *fmt, ...) PRINTF_ATTRIBUTE(2,3); +void talloc_set_name_const(const void *ptr, const char *name); +void *talloc_named(const void *context, size_t size, + const char *fmt, ...) PRINTF_ATTRIBUTE(3,4); +void *talloc_named_const(const void *context, size_t size, const char *name); +const char *talloc_get_name(const void *ptr); +void *talloc_check_name(const void *ptr, const char *name); +void *_talloc_get_type_abort(const void *ptr, const char *name, const char *location); +void *talloc_parent(const void *ptr); +const char *talloc_parent_name(const void *ptr); +void *talloc_init(const char *fmt, ...) PRINTF_ATTRIBUTE(1,2); +int talloc_free(void *ptr); +void talloc_free_children(void *ptr); +void *_talloc_realloc(const void *context, void *ptr, size_t size, const char *name); +void *_talloc_steal(const void *new_ctx, const void *ptr); +void *_talloc_move(const void *new_ctx, const void *pptr); +size_t talloc_total_size(const void *ptr); +size_t talloc_total_blocks(const void *ptr); +void talloc_report_depth_cb(const void *ptr, int depth, int max_depth, + void (*callback)(const void *ptr, + int depth, int max_depth, + int is_ref, + void *private_data), + void *private_data); +void talloc_report_depth_file(const void *ptr, int depth, int max_depth, FILE *f); +void talloc_report_full(const void *ptr, FILE *f); +void talloc_report(const void *ptr, FILE *f); +void talloc_enable_null_tracking(void); +void talloc_disable_null_tracking(void); +void talloc_enable_leak_report(void); +void talloc_enable_leak_report_full(void); +void *_talloc_zero(const void *ctx, size_t size, const char *name); +void *_talloc_memdup(const void *t, const void *p, size_t size, const char *name); +void *_talloc_array(const void *ctx, size_t el_size, unsigned count, const char *name); +void *_talloc_zero_array(const void *ctx, size_t el_size, unsigned count, const char *name); +void *_talloc_realloc_array(const void *ctx, void *ptr, size_t el_size, unsigned count, const char *name); +void *talloc_realloc_fn(const void *context, void *ptr, size_t size); +void *talloc_autofree_context(void); +size_t talloc_get_size(const void *ctx); +void *talloc_find_parent_byname(const void *ctx, const char *name); +void talloc_show_parents(const void *context, FILE *file); +int talloc_is_parent(const void *context, const void *ptr); + +char *talloc_strdup(const void *t, const char *p); +char *talloc_strdup_append(char *s, const char *a); +char *talloc_strdup_append_buffer(char *s, const char *a); + +char *talloc_strndup(const void *t, const char *p, size_t n); +char *talloc_strndup_append(char *s, const char *a, size_t n); +char *talloc_strndup_append_buffer(char *s, const char *a, size_t n); + +char *talloc_vasprintf(const void *t, const char *fmt, va_list ap) PRINTF_ATTRIBUTE(2,0); +char *talloc_vasprintf_append(char *s, const char *fmt, va_list ap) PRINTF_ATTRIBUTE(2,0); +char *talloc_vasprintf_append_buffer(char *s, const char *fmt, va_list ap) PRINTF_ATTRIBUTE(2,0); + +char *talloc_asprintf(const void *t, const char *fmt, ...) PRINTF_ATTRIBUTE(2,3); +char *talloc_asprintf_append(char *s, const char *fmt, ...) PRINTF_ATTRIBUTE(2,3); +char *talloc_asprintf_append_buffer(char *s, const char *fmt, ...) PRINTF_ATTRIBUTE(2,3); + +void talloc_set_abort_fn(void (*abort_fn)(const char *reason)); + +#endif diff --git a/misc/tools/osmocon/timer.c b/misc/tools/osmocon/timer.c new file mode 100644 index 000000000..1f1c7896b --- /dev/null +++ b/misc/tools/osmocon/timer.c @@ -0,0 +1,264 @@ +/* + * (C) 2008,2009 by Holger Hans Peter Freyther + * (C) 2011 by Harald Welte + * All Rights Reserved + * + * Authors: Holger Hans Peter Freyther + * Harald Welte + * Pablo Neira Ayuso + * + * 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. + * + */ + +/* These store the amount of time that we wait until next timer expires. */ +static struct timeval nearest; +static struct timeval *nearest_p; + +/*! \addtogroup timer + * @{ + */ + +/*! \file timer.c + */ + +#include +#include +#include +#include "timer.h" +#include "timer_compat.h" +#include "linuxlist.h" + +static struct rb_root timer_root = RB_ROOT; + +static void __add_timer(struct osmo_timer_list *timer) +{ + struct rb_node **new = &(timer_root.rb_node); + struct rb_node *parent = NULL; + + while (*new) { + struct osmo_timer_list *this; + + this = container_of(*new, struct osmo_timer_list, node); + + parent = *new; + if (timercmp(&timer->timeout, &this->timeout, <)) + new = &((*new)->rb_left); + else + new = &((*new)->rb_right); + } + + rb_link_node(&timer->node, parent, new); + rb_insert_color(&timer->node, &timer_root); +} + +/*! \brief add a new timer to the timer management + * \param[in] timer the timer that should be added + */ +void osmo_timer_add(struct osmo_timer_list *timer) +{ + osmo_timer_del(timer); + timer->active = 1; + INIT_LLIST_HEAD(&timer->list); + __add_timer(timer); +} + +/*! \brief schedule a timer at a given future relative time + * \param[in] timer the to-be-added timer + * \param[in] seconds number of seconds from now + * \param[in] microseconds number of microseconds from now + * + * This function can be used to (re-)schedule a given timer at a + * specified number of seconds+microseconds in the future. It will + * internally add it to the timer management data structures, thus + * osmo_timer_add() is automatically called. + */ +void +osmo_timer_schedule(struct osmo_timer_list *timer, int seconds, int microseconds) +{ + struct timeval current_time; + + gettimeofday(¤t_time, NULL); + timer->timeout.tv_sec = seconds; + timer->timeout.tv_usec = microseconds; + timeradd(&timer->timeout, ¤t_time, &timer->timeout); + osmo_timer_add(timer); +} + +/*! \brief delete a timer from timer management + * \param[in] timer the to-be-deleted timer + * + * This function can be used to delete a previously added/scheduled + * timer from the timer management code. + */ +void osmo_timer_del(struct osmo_timer_list *timer) +{ + if (timer->active) { + timer->active = 0; + rb_erase(&timer->node, &timer_root); + /* make sure this is not already scheduled for removal. */ + if (!llist_empty(&timer->list)) + llist_del_init(&timer->list); + } +} + +/*! \brief check if given timer is still pending + * \param[in] timer the to-be-checked timer + * \return 1 if pending, 0 otherwise + * + * This function can be used to determine whether a given timer + * has alredy expired (returns 0) or is still pending (returns 1) + */ +int osmo_timer_pending(struct osmo_timer_list *timer) +{ + return timer->active; +} + +/*! \brief compute the remaining time of a timer + * \param[in] timer the to-be-checked timer + * \param[in] the current time (NULL if not known) + * \param[out] remaining remaining time until timer fires + * \return 0 if timer has not expired yet, -1 if it has + * + * This function can be used to determine the amount of time + * remaining until the expiration of the timer. + */ +int osmo_timer_remaining(const struct osmo_timer_list *timer, + const struct timeval *now, + struct timeval *remaining) +{ + struct timeval current_time; + + if (!now) { + gettimeofday(¤t_time, NULL); + now = ¤t_time; + } + + timersub(&timer->timeout, ¤t_time, remaining); + + if (remaining->tv_sec < 0) + return -1; + + return 0; +} + +/* + * if we have a nearest time return the delta between the current + * time and the time of the nearest timer. + * If the nearest timer timed out return NULL and then we will + * dispatch everything after the select + */ +struct timeval *osmo_timers_nearest(void) +{ + /* nearest_p is exactly what we need already: NULL if nothing is + * waiting, {0,0} if we must dispatch immediately, and the correct + * delay if we need to wait */ + return nearest_p; +} + +static void update_nearest(struct timeval *cand, struct timeval *current) +{ + if (cand->tv_sec != LONG_MAX) { + if (timercmp(cand, current, >)) + timersub(cand, current, &nearest); + else { + /* loop again inmediately */ + nearest.tv_sec = 0; + nearest.tv_usec = 0; + } + nearest_p = &nearest; + } else { + nearest_p = NULL; + } +} + +/* + * Find the nearest time and update s_nearest_time + */ +void osmo_timers_prepare(void) +{ + struct rb_node *node; + struct timeval current; + + gettimeofday(¤t, NULL); + + node = rb_first(&timer_root); + if (node) { + struct osmo_timer_list *this; + this = container_of(node, struct osmo_timer_list, node); + update_nearest(&this->timeout, ¤t); + } else { + nearest_p = NULL; + } +} + +/* + * fire all timers... and remove them + */ +int osmo_timers_update(void) +{ + struct timeval current_time; + struct rb_node *node; + struct llist_head timer_eviction_list; + struct osmo_timer_list *this; + int work = 0; + + gettimeofday(¤t_time, NULL); + + INIT_LLIST_HEAD(&timer_eviction_list); + for (node = rb_first(&timer_root); node; node = rb_next(node)) { + this = container_of(node, struct osmo_timer_list, node); + + if (timercmp(&this->timeout, ¤t_time, >)) + break; + + llist_add(&this->list, &timer_eviction_list); + } + + /* + * The callbacks might mess with our list and in this case + * even llist_for_each_entry_safe is not safe to use. To allow + * osmo_timer_del to be called from within the callback we need + * to restart the iteration for each element scheduled for removal. + * + * The problematic scenario is the following: Given two timers A + * and B that have expired at the same time. Thus, they are both + * in the eviction list in this order: A, then B. If we remove + * timer B from the A's callback, we continue with B in the next + * iteration step, leading to an access-after-release. + */ +restart: + llist_for_each_entry(this, &timer_eviction_list, list) { + osmo_timer_del(this); + this->cb(this->data); + work = 1; + goto restart; + } + + return work; +} + +int osmo_timers_check(void) +{ + struct rb_node *node; + int i = 0; + + for (node = rb_first(&timer_root); node; node = rb_next(node)) { + i++; + } + return i; +} + +/*! @} */ diff --git a/misc/tools/osmocon/timer.h b/misc/tools/osmocon/timer.h new file mode 100644 index 000000000..1a03631d3 --- /dev/null +++ b/misc/tools/osmocon/timer.h @@ -0,0 +1,89 @@ +/* + * (C) 2008, 2009 by Holger Hans Peter Freyther + * All Rights Reserved + * + * 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. + * + */ + +/*! \defgroup timer Osmocom timers + * @{ + */ + +/*! \file timer.h + * \brief Osmocom timer handling routines + */ + +#ifndef TIMER_H +#define TIMER_H + +#include + +#include "linuxlist.h" +#include "linuxrbtree.h" + +/** + * Timer management: + * - Create a struct osmo_timer_list + * - Fill out timeout and use add_timer or + * use schedule_timer to schedule a timer in + * x seconds and microseconds from now... + * - Use del_timer to remove the timer + * + * Internally: + * - We hook into select.c to give a timeval of the + * nearest timer. On already passed timers we give + * it a 0 to immediately fire after the select + * - update_timers will call the callbacks and remove + * the timers. + * + */ +/*! \brief A structure representing a single instance of a timer */ +struct osmo_timer_list { + struct rb_node node; /*!< \brief rb-tree node header */ + struct llist_head list; /*!< \brief internal list header */ + struct timeval timeout; /*!< \brief expiration time */ + unsigned int active : 1; /*!< \brief is it active? */ + + void (*cb)(void*); /*!< \brief call-back called at timeout */ + void *data; /*!< \brief user data for callback */ +}; + +/** + * timer management + */ + +void osmo_timer_add(struct osmo_timer_list *timer); + +void osmo_timer_schedule(struct osmo_timer_list *timer, int seconds, int microseconds); + +void osmo_timer_del(struct osmo_timer_list *timer); + +int osmo_timer_pending(struct osmo_timer_list *timer); + +int osmo_timer_remaining(const struct osmo_timer_list *timer, + const struct timeval *now, + struct timeval *remaining); +/* + * internal timer list management + */ +struct timeval *osmo_timers_nearest(void); +void osmo_timers_prepare(void); +int osmo_timers_update(void); +int osmo_timers_check(void); + +/*! @} */ + +#endif diff --git a/misc/tools/osmocon/timer_compat.h b/misc/tools/osmocon/timer_compat.h new file mode 100644 index 000000000..d86c109e2 --- /dev/null +++ b/misc/tools/osmocon/timer_compat.h @@ -0,0 +1,79 @@ +/* + * (C) 2011 Sylvain Munaut + * All Rights Reserved + * + * 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. + * + */ + +/*! \defgroup timer Osmocom timers + * @{ + */ + +/*! \file timer_compat.h + * \brief Compatibility header with some helpers + */ + +#ifndef TIMER_COMPAT_H +#define TIMER_COMPAT_H + + +/* Convenience macros for operations on timevals. + NOTE: `timercmp' does not work for >= or <=. */ + +#ifndef timerisset +# define timerisset(tvp) ((tvp)->tv_sec || (tvp)->tv_usec) +#endif + +#ifndef timerclear +# define timerclear(tvp) ((tvp)->tv_sec = (tvp)->tv_usec = 0) +#endif + +#ifndef timercmp +# define timercmp(a, b, CMP) \ + (((a)->tv_sec == (b)->tv_sec) ? \ + ((a)->tv_usec CMP (b)->tv_usec) : \ + ((a)->tv_sec CMP (b)->tv_sec)) +#endif + +#ifndef timeradd +# define timeradd(a, b, result) \ + do { \ + (result)->tv_sec = (a)->tv_sec + (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec + (b)->tv_usec; \ + if ((result)->tv_usec >= 1000000) \ + { \ + ++(result)->tv_sec; \ + (result)->tv_usec -= 1000000; \ + } \ + } while (0) +#endif + +#ifndef timersub +# define timersub(a, b, result) \ + do { \ + (result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \ + (result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \ + if ((result)->tv_usec < 0) { \ + --(result)->tv_sec; \ + (result)->tv_usec += 1000000; \ + } \ + } while (0) +#endif + + +/*! @} */ + +#endif /* TIMER_COMPAT_H */ diff --git a/misc/tools/osmocon/tpu_debug.c b/misc/tools/osmocon/tpu_debug.c new file mode 100644 index 000000000..918cdedfd --- /dev/null +++ b/misc/tools/osmocon/tpu_debug.c @@ -0,0 +1,138 @@ +/* Calypso TPU debugger, displays and decodes TPU instruction RAM */ + +/* (C) 2010 by Harald Welte + * + * All Rights Reserved + * + * 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 "msgb.h" + +/* TPU disassembler begin */ + +static const char *tpu_instr_name[] = { + [0] = "SLEEP", + [1] = "AT", + [2] = "OFFSET", + [3] = "SYNCHRO", + [4] = "MOVE", + [5] = "WAIT", + [6] = "UNDEFINED6", + [7] = "UNDEFINED7", +}; + +static const char *tpu_addr_name[0x1f] = { + [0] = "TSP_CTLR1", + [1] = "TSP_CTRL2", + [4] = "TSP_TX_1", + [3] = "TSP_TX_2", + [2] = "TSP_TX_3", + [5] = "TSP_TX_4", + [6] = "TSPACT_L", + [7] = "TSPACT_H", + [9] = "TSP_SET1", + [0xa] = "TSP_SET2", + [0xb] = "TSP_SET3", + [0x10] = "DSP_INT_PG", + [0x11] = "GAUGING_EN", +}; + +static uint8_t tpu_reg_cache[0x1f]; +static uint16_t tpu_qbit; + +static void tpu_show_instr(uint16_t tpu) +{ + uint16_t instr = tpu >> 13; + uint16_t param = tpu & 0x1fff; + uint16_t addr, data, bitlen; + uint32_t tsp_data; + + tpu_qbit++; + + printf("\t %04u %04x %s ", tpu_qbit, tpu, tpu_instr_name[instr]); + switch (instr) { + case 0: + tpu_qbit = 0; + default: + break; + case 1: + tpu_qbit = param; + printf("%u ", param); + break; + case 5: + tpu_qbit += param; + printf("%u ", param); + break; + case 2: + case 3: + printf("%u ", param); + break; + case 4: + addr = param & 0x1f; + data = param >> 5; + tpu_reg_cache[addr] = data; + printf("%10s=0x%04x ", tpu_addr_name[addr], data); + switch (addr) { + case 0: + bitlen = (data & 0x1f) + 1; + printf("DEV_IDX=%u, BITLEN=%u ", data >> 5, bitlen); + if (bitlen <= 8) { + tsp_data = tpu_reg_cache[4]; + printf(" TSP_DATA=0x%02x ", tsp_data); + } else if (bitlen <= 16) { + tsp_data = tpu_reg_cache[3]; + tsp_data |= tpu_reg_cache[4] << 8; + printf(" TSP_DATA=0x%04x ", tsp_data); + } else if (bitlen <= 24) { + tsp_data = tpu_reg_cache[2]; + tsp_data |= tpu_reg_cache[3] << 8; + tsp_data |= tpu_reg_cache[4] << 16; + printf(" TSP_DATA=0x%06x ", tsp_data); + } else { + tsp_data = tpu_reg_cache[5]; + tsp_data |= tpu_reg_cache[2] << 8; + tsp_data |= tpu_reg_cache[3] << 16; + tsp_data |= tpu_reg_cache[4] << 24; + printf(" TSP_DATA=0x%08x ", tsp_data); + } + break; + case 1: + if (data & 0x01) + printf("READ "); + if (data & 0x02) + printf("WRITE "); + break; + } + } + printf("\n"); +} + +void hdlc_tpudbg_cb(uint8_t dlci, struct msgb *msg) +{ + uint32_t *fn = (uint32_t *) msg->data; + uint16_t *tpu; + + printf("TPU FN %u\n", *fn); + for (tpu = (uint16_t *) (msg->data + 4); tpu < (uint16_t *) msg->tail; tpu++) + tpu_show_instr(*tpu); + + msgb_free(msg); +} diff --git a/misc/tools/osmocon/utils.h b/misc/tools/osmocon/utils.h new file mode 100644 index 000000000..03861d789 --- /dev/null +++ b/misc/tools/osmocon/utils.h @@ -0,0 +1,56 @@ +#ifndef OSMOCORE_UTIL_H +#define OSMOCORE_UTIL_H + +/*! \defgroup utils General-purpose utility functions + * @{ + */ + +/*! \file utils.h */ + +/*! \brief Determine number of elements in an array of static size */ +#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0])) +/*! \brief Return the maximum of two specified values */ +#define OSMO_MAX(a, b) ((a) >= (b) ? (a) : (b)) +/*! \brief Return the minimum of two specified values */ +#define OSMO_MIN(a, b) ((a) >= (b) ? (b) : (a)) + +#include + +/*! \brief A mapping between human-readable string and numeric value */ +struct value_string { + unsigned int value; /*!< \brief numeric value */ + const char *str; /*!< \brief human-readable string */ +}; + +const char *get_value_string(const struct value_string *vs, uint32_t val); + +int get_string_value(const struct value_string *vs, const char *str); + +char osmo_bcd2char(uint8_t bcd); +/* only works for numbers in ascci */ +uint8_t osmo_char2bcd(char c); + +int osmo_hexparse(const char *str, uint8_t *b, int max_len); + +char *osmo_ubit_dump(const uint8_t *bits, unsigned int len); +char *osmo_hexdump(const unsigned char *buf, int len); +char *osmo_hexdump_nospc(const unsigned char *buf, int len); +char *osmo_osmo_hexdump_nospc(const unsigned char *buf, int len) __attribute__((__deprecated__)); + +#define osmo_static_assert(exp, name) typedef int dummy##name [(exp) ? 1 : -1]; + +void osmo_str2lower(char *out, const char *in); +void osmo_str2upper(char *out, const char *in); + +#define OSMO_SNPRINTF_RET(ret, rem, offset, len) \ +do { \ + len += ret; \ + if (ret > rem) \ + ret = rem; \ + offset += ret; \ + rem -= ret; \ +} while (0) + +/*! @} */ + +#endif -- cgit v1.2.3