diff options
| author | Kevin O'Connor <kevin@koconnor.net> | 2016-05-25 18:37:40 +0300 |
|---|---|---|
| committer | Kevin O'Connor <kevin@koconnor.net> | 2016-05-25 18:37:40 +0300 |
| commit | f582a36e4df16d5709943f7df17a900c8bcc12ab (patch) | |
| tree | 628d927c4f3e19e54618f7f47c7a44af66bf0c2f | |
| parent | 37a91e9c10648208de002c75df304e23ca89e256 (diff) | |
Initial commit of source code.v0.2.0
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
71 files changed, 9950 insertions, 0 deletions
diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..bfb0690c7 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +out +*.so +*.pyc +.config +.config.old diff --git a/COPYING b/COPYING new file mode 100644 index 000000000..94a9ed024 --- /dev/null +++ b/COPYING @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/> + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. 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 +them 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 prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. 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. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey 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; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If 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 convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU 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 that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + 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. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +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. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + 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 +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + <one line to give the program's name and a brief idea of what it does.> + Copyright (C) <year> <name of author> + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + <program> Copyright (C) <year> <name of author> + This program 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, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +<http://www.gnu.org/licenses/>. + + The GNU 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. But first, please read +<http://www.gnu.org/philosophy/why-not-lgpl.html>. diff --git a/Makefile b/Makefile new file mode 100644 index 000000000..8c8967ea5 --- /dev/null +++ b/Makefile @@ -0,0 +1,123 @@ +# Klipper build system +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +# Output directory +OUT=out/ + +# Kconfig includes +export HOSTCC := $(CC) +export CONFIG_SHELL := sh +export KCONFIG_AUTOHEADER := autoconf.h +export KCONFIG_CONFIG := $(CURDIR)/.config +-include $(KCONFIG_CONFIG) + +# Common command definitions +CC=$(CROSS_PREFIX)gcc +AS=$(CROSS_PREFIX)as +LD=$(CROSS_PREFIX)ld +OBJCOPY=$(CROSS_PREFIX)objcopy +OBJDUMP=$(CROSS_PREFIX)objdump +STRIP=$(CROSS_PREFIX)strip +CPP=cpp +PYTHON=python + +# Source files +src-y=sched.c command.c stepper.c basecmd.c gpiocmds.c spicmds.c endstop.c +DIRS=src src/avr src/simulator + +# Default compiler flags +cc-option=$(shell if test -z "`$(1) $(2) -S -o /dev/null -xc /dev/null 2>&1`" \ + ; then echo "$(2)"; else echo "$(3)"; fi ;) + +CFLAGS-y := -I$(OUT) -Isrc -Os -MD -g \ + -Wall -Wold-style-definition $(call cc-option,$(CC),-Wtype-limits,) \ + -ffunction-sections -fdata-sections +CFLAGS-y += -flto -fwhole-program + +LDFLAGS-y := -Wl,--gc-sections + +CPPFLAGS = -P -MD -MT $@ + +CFLAGS = $(CFLAGS-y) +LDFLAGS = $(LDFLAGS-y) + +# Default targets +target-y := $(OUT)klipper.elf + +all: + +# Run with "make V=1" to see the actual compile commands +ifdef V +Q= +else +Q=@ +MAKEFLAGS += --no-print-directory +endif + +# Include board specific makefile +-include src/$(patsubst "%",%,$(CONFIG_BOARD_DIRECTORY))/Makefile + +################ Common build rules + +$(OUT)%.o: %.c $(OUT)autoconf.h $(OUT)board-link + @echo " Compiling $@" + $(Q)$(CC) $(CFLAGS) -c $< -o $@ + +################ Main build rules + +$(OUT)board-link: $(KCONFIG_CONFIG) + @echo " Creating symbolic link $(OUT)board" + $(Q)touch $@ + $(Q)ln -Tsf $(PWD)/src/$(CONFIG_BOARD_DIRECTORY) $(OUT)board + +$(OUT)declfunc.lds: src/declfunc.lds.S + @echo " Precompiling $@" + $(Q)$(CPP) $(CPPFLAGS) -D__ASSEMBLY__ $< -o $@ + +$(OUT)klipper.o: $(patsubst %.c, $(OUT)src/%.o,$(src-y)) $(OUT)declfunc.lds + @echo " Linking $@" + $(Q)$(CC) $(CFLAGS) -Wl,-r -Wl,-T,$(OUT)declfunc.lds -nostdlib $(patsubst %.c, $(OUT)src/%.o,$(src-y)) -o $@ + +$(OUT)compile_time_request.o: $(OUT)klipper.o ./scripts/buildcommands.py + @echo " Building $@" + $(Q)$(OBJCOPY) -j '.compile_time_request' -O binary $< $(OUT)klipper.o.compile_time_request + $(Q)$(PYTHON) ./scripts/buildcommands.py $(OUT)klipper.o.compile_time_request $(OUT)autoconf.h $(OUT)compile_time_request.c + $(Q)$(CC) $(CFLAGS) -c $(OUT)compile_time_request.c -o $@ + +$(OUT)klipper.elf: $(OUT)klipper.o $(OUT)compile_time_request.o + @echo " Linking $@" + $(Q)$(CC) $(CFLAGS) $(LDFLAGS) $^ -o $@ + +################ Kconfig rules + +define do-kconfig +$(Q)mkdir -p $(OUT)/scripts/kconfig/lxdialog +$(Q)mkdir -p $(OUT)/include/config +$(Q)mkdir -p $(addprefix $(OUT), $(DIRS)) +$(Q)$(MAKE) -C $(OUT) -f $(CURDIR)/scripts/kconfig/Makefile srctree=$(CURDIR) src=scripts/kconfig obj=scripts/kconfig Q=$(Q) Kconfig=$(CURDIR)/src/Kconfig $1 +endef + +$(OUT)autoconf.h : $(KCONFIG_CONFIG) ; $(call do-kconfig, silentoldconfig) +$(KCONFIG_CONFIG): src/Kconfig ; $(call do-kconfig, olddefconfig) +%onfig: ; $(call do-kconfig, $@) +help: ; $(call do-kconfig, $@) + + +################ Generic rules + +# Make definitions +.PHONY : all clean distclean FORCE +.DELETE_ON_ERROR: + +all: $(target-y) + +clean: + $(Q)rm -rf $(OUT) + +distclean: clean + $(Q)rm -f .config .config.old + +-include $(patsubst %,$(OUT)%/*.d,$(DIRS)) diff --git a/README.md b/README.md new file mode 100644 index 000000000..f594c95d3 --- /dev/null +++ b/README.md @@ -0,0 +1,26 @@ +Welcome to the Klipper project! + +This project implements a 3d-printer firmware. There are two parts to +this firmware - code that runs on a micro-controller and code that +runs on a host machine. The host software does the work to build a +schedule of events, while the micro-controller software does the work +to execute the provided schedule at the specified times. + +Please see the [documentation](docs/Overview.md) for more information +on running and working with Klipper. + +License +======= + +Klipper is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +Klipper 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 Klipper. If not, see <http://www.gnu.org/licenses/>. diff --git a/config/avrsim.cfg b/config/avrsim.cfg new file mode 100644 index 000000000..5b1d0f74d --- /dev/null +++ b/config/avrsim.cfg @@ -0,0 +1,83 @@ +# Support for internal testing with the "simulavr" program. To use +# this config, compile the firmware for an AVR atmega644p, disable the +# AVR watchdog timer, set the MCU frequency to 20000000, and set the +# serial baud rate to 115200. + +[stepper_x] +# Pins: PA5, PA4, PA1 +step_pin: ar29 +dir_pin: ar28 +enable_pin: ar25 +step_distance: .0225 +max_velocity: 500 +max_accel: 3000 +endstop_pin: ^!ar0 +position_min: -0.25 +position_endstop: 0 +position_max: 200 + +[stepper_y] +# Pins: PA3, PA2 +step_pin: ar27 +dir_pin: ar26 +enable_pin: ar25 +step_distance: .0225 +max_velocity: 500 +max_accel: 3000 +endstop_pin: ^!ar1 +position_min: -0.25 +position_endstop: 0 +position_max: 200 + +[stepper_z] +# Pins: PC7, PC6 +step_pin: ar23 +dir_pin: ar22 +enable_pin: ar25 +step_distance: .005 +max_velocity: 250 +max_accel: 30 +endstop_pin: ^!ar2 +position_min: 0.1 +position_endstop: 0.5 +position_max: 200 + +[stepper_e] +# Pins: PC3, PC2 +step_pin: ar19 +dir_pin: ar18 +enable_pin: ar25 +step_distance: .004242 +max_velocity: 200000 +max_accel: 3000 + +[heater_nozzle] +heater_pin: ar4 +thermistor_pin: analog1 +thermistor_type: EPCOS 100K B57560G104F +control: pid +pid_Kp: 22.2 +pid_Ki: 1.08 +pid_Kd: 114 +min_temp: 0 +max_temp: 210 + +[heater_bed] +heater_pin: ar3 +thermistor_pin: analog0 +thermistor_type: EPCOS 100K B57560G104F +control: watermark +min_temp: 0 +max_temp: 110 + +[fan] +pin: ar14 +hard_pwm: 1 + +[mcu] +serial: /tmp/pseudoserial +baud: 115200 +pin_map: arduino + +[printer] +kinematics: cartesian diff --git a/config/example.cfg b/config/example.cfg new file mode 100644 index 000000000..3090ba15e --- /dev/null +++ b/config/example.cfg @@ -0,0 +1,173 @@ +# This file serves as documentation for config parameters. One may +# copy and edit this file to configure a new printer. + +# DO NOT COPY THIS FILE WITHOUT CAREFULLY READING AND UPDATING IT +# FIRST. Incorrectly configured parameters may cause damage. + +# A note on pin names: pins may be configured with a hardware name +# (such as PA4) or with a board name (such as ar29). In order to use a +# board name, the pin_map variable in the mcu section must specify +# which board definition to use. (See the klippy/pins.py file for the +# available pin and board names.) +# Pin names may be preceded by an '!' to indicate that a reverse +# polarity should be used (eg, trigger on low instead of high). Input +# pins may be prceded by an '^' to indicate that a hardware pull-up +# resistor should be enabled for the pin. + + +# The stepper_x section is used to describe the stepper controlling +# the X axis in a cartesian robot +[stepper_x] +step_pin: ar29 +# Step GPIO pin (triggered high) +dir_pin: ar28 +# Direction GPIO pin (low indicates positive direction) +enable_pin: !ar25 +# Enable pin (default is enable high; use ! to indicate enable low) +step_distance: .0225 +# Distance in mm that each step causes the axis to travel +max_velocity: 500 +# Maximum velocity (in mm/s) of the stepper +max_accel: 3000 +# Maximum acceleration (in mm/s^2) of the stepper +endstop_pin: ^ar0 +# Endstop switch detection pin +homing_speed: 50.0 +# Maximum velocity (in mm/s) of the stepper when homing +homing_retract_dist: 5.0 +# Distance to backoff (in mm) before homing a second time during homing +homing_positive_dir: False +# If true, homes in a positive direction (away from zero) +position_min: -0.25 +# Minimum valid distance (in mm) the user may command the stepper to +# move to (not currently enforced) +position_endstop: 0 +# Location of the endstop (in mm) +position_max: 200 +# Maximum valid distance (in mm) the user may command the stepper to +# move to (not currently enforced) + +# The stepper_y section is used to describe the stepper controlling +# the Y axis in a cartesian robot. It has the same settings as the +# stepper_x section +[stepper_y] +step_pin: ar27 +dir_pin: ar26 +enable_pin: !ar25 +step_distance: .0225 +max_velocity: 500 +max_accel: 3000 +endstop_pin: ^ar1 +position_min: -0.25 +position_endstop: 0 +position_max: 200 + +# The stepper_z section is used to describe the stepper controlling +# the Z axis in a cartesian robot. It has the same settings as the +# stepper_x section +[stepper_z] +step_pin: ar23 +dir_pin: ar22 +enable_pin: !ar25 +step_distance: .005 +max_velocity: 250 +max_accel: 30 +endstop_pin: ^ar2 +position_min: 0.1 +position_endstop: 0.5 +position_max: 200 + +# The stepper_e section is used to describe the stepper controlling +# the printer extruder. It has the same settings as the stepper_x +# section +[stepper_e] +step_pin: ar19 +dir_pin: ar18 +enable_pin: !ar25 +step_distance: .004242 +max_velocity: 200000 +max_accel: 3000 + +# The heater_nozzle section describes the extruder and extruder heater +[heater_nozzle] +heater_pin: ar4 +# PWM output pin controlling the heater +thermistor_pin: analog1 +# Analog input pin connected to thermistor +thermistor_type: EPCOS 100K B57560G104F +# Type of thermistor (see klippy/heater.py for available types) +pullup_resistor: 4700 +# The resistance (in ohms) of the pullup attached to the thermistor +control: pid +# Control algorithm (either pid or watermark) +pid_Kp: 22.2 +# Kp is the "proportional" constant for the pid +pid_Ki: 1.08 +# Ki is the "integral" constant for the pid +pid_Kd: 114 +# Kd is the "derivative" constant for the pid +pid_deriv_time: 2.0 +# A time value (in seconds) over which the derivative in the pid +# will be smoothed to reduce the impact of measurement noise +pid_integral_max: 255 +# The maximum "windup" the integral term may accumulate +min_temp: 0 +# Minimum temperature in Celsius (mcu will shutdown if not met) +max_temp: 210 +# Maximum temperature (mcu will shutdown if temperature is above +# this value) + +# The heater_bed section describes a heated bed (if present - omit +# section if not present). It has the same settings as the +# heater_nozzle section +[heater_bed] +heater_pin: ar3 +thermistor_pin: analog0 +thermistor_type: EPCOS 100K B57560G104F +control: watermark +max_delta: 2.0 +# The number of degrees in Celsius above the target temperature +# before disabling the heater as well as the number of degrees below +# the target before re-enabling the heater. +min_temp: 0 +max_temp: 110 + +# Extruder print fan (omit section if fan not present) +[fan] +pin: ar14 +# PWM output pin controlling the heater +hard_pwm: 1 +# Set this value to force hardware PWM instead of software PWM. Set +# to 1 to force a hardware PWM at the fastest rate; set to a higher +# number (eg, 1024) to force hardware PWM with the given cycle time +# in clock ticks. +kick_start_time: 0.100 +# Time (in seconds) to run the fan at full speed when first enabling +# it (helps get the fan spinning) + +# Micro-controller information +[mcu] +serial: /dev/ttyACM0 +# The serial port to connect to the MCU +baud: 115200 +# The baud rate to use +pin_map: arduino +# This option may be used to add board specific pin name aliases +custom: +# This option may be used to specify a set of custom +# micro-controller commands to be sent at the start of the +# connection. It may be used to configure the initial settings of +# LEDs, to configure micro-stepping pins, to configure a digipot, +# etc. + +# The printer section controls high level printer settings +[printer] +kinematics: cartesian +# This option must currently always be "cartesian" +motor_off_time: 60 +# Time (in seconds) of idle time before the printer will try to +# disable active motors. +junction_deviation: 0.02 +# Distance (in mm) used to control the internal approximated +# centripetal velocity cornering algorithm. A larger number will +# permit higher "cornering speeds" at the junction of two moves. diff --git a/config/makergear-m2-2012.cfg b/config/makergear-m2-2012.cfg new file mode 100644 index 000000000..7a3080d2a --- /dev/null +++ b/config/makergear-m2-2012.cfg @@ -0,0 +1,103 @@ +# Support for Makergear M2 printers circa 2012 that have the RAMBo +# v1.0d electronics. The electronics use Allegro A4984 stepper +# drivers with 1/8th micro-stepping. To use this config, the firmware +# should be compiled for the AVR atmega2560. + +[stepper_x] +step_pin: PC0 +dir_pin: PL1 +enable_pin: !PA7 +step_distance: .0225 +max_velocity: 500 +max_accel: 3000 +endstop_pin: ^PB6 +homing_speed: 50.0 +position_min: -0.25 +position_endstop: 0.0 +position_max: 200 + +[stepper_y] +step_pin: PC1 +dir_pin: !PL0 +enable_pin: !PA6 +step_distance: .0225 +max_velocity: 500 +max_accel: 3000 +endstop_pin: ^PB5 +homing_speed: 50.0 +position_min: -0.25 +position_endstop: 0.0 +position_max: 250 + +[stepper_z] +step_pin: PC2 +dir_pin: PL2 +enable_pin: !PA5 +step_distance: .005 +max_velocity: 250 +max_accel: 30 +endstop_pin: ^PB4 +homing_speed: 4.0 +homing_retract_dist: 2.0 +position_min: 0.1 +position_endstop: 0.7 +position_max: 200 + +[stepper_e] +step_pin: PC3 +dir_pin: !PL6 +enable_pin: !PA4 +step_distance: .004242 +max_velocity: 200000 +max_accel: 3000 + +[heater_nozzle] +heater_pin: PH6 +thermistor_pin: PF0 +thermistor_type: EPCOS 100K B57560G104F +control: pid +pid_Kp: 7.0 +pid_Ki: 0.1 +pid_Kd: 12 +min_temp: 0 +max_temp: 210 + +[heater_bed] +heater_pin: PE5 +thermistor_pin: PF2 +thermistor_type: EPCOS 100K B57560G104F +control: watermark +min_temp: 0 +max_temp: 100 + +[fan] +pin: PH5 +hard_pwm: 1 + +[mcu] +serial: /dev/ttyACM0 +baud: 250000 +custom: + # Nozzle fan + set_pwm_out pin=PH3 cycle_ticks=1 value=155 + # Turn off yellow led + set_digital_out pin=PB7 value=0 + # Stepper micro-step pins + set_digital_out pin=PG1 value=1 + set_digital_out pin=PG0 value=1 + set_digital_out pin=PK7 value=1 + set_digital_out pin=PG2 value=1 + set_digital_out pin=PK6 value=1 + set_digital_out pin=PK5 value=1 + set_digital_out pin=PK3 value=1 + set_digital_out pin=PK4 value=1 + # Initialize digipot + send_spi_message pin=PD7 msg=0487 # X = ~0.75A + send_spi_message pin=PD7 msg=0587 # Y = ~0.75A + send_spi_message pin=PD7 msg=0387 # Z = ~0.75A + send_spi_message pin=PD7 msg=00A5 # E0 + send_spi_message pin=PD7 msg=017D # E1 + +[printer] +kinematics: cartesian +motor_off_time: 600 diff --git a/docs/Code_Overview.md b/docs/Code_Overview.md new file mode 100644 index 000000000..34550e251 --- /dev/null +++ b/docs/Code_Overview.md @@ -0,0 +1,93 @@ +This document describes the overall code layout and major code flow of +Klipper. + +Directory Layout +================ + +The **src/** directory contains the C source for the micro-controller +code. The **src/avr/** directory contains specific code for Atmel +ATmega micro-controllers. The **src/simulator/** contains code stubs +that allow the micro-controller to be test compiled on other +architectures. + +The **klippy/** directory contains the C and Python source for the +host part of the firmware. + +The **config/** directory contains example printer configuration +files. + +The **scripts/** directory contains build-time scripts useful for +compiling the micro-controller code. + +During compilation, the build may create an **out/** directory. This +contains temporary build time objects. The final micro-controller +object that is built is in **out/klipper.elf.hex** + +Micro-controller code flow +========================== + +Execution of the micro-controller code starts in **src/avr/main.c** +which calls sched_main() located in **src/sched.c**. The sched_main() +code starts by running all functions that have been tagged with the +DECL_INIT() macro. It then goes on to repeatedly run all functions +tagged with the DECL_TASK() macro. + +One of the main task functions is command_task() located in +**src/command.c**. This function processes incoming serial commands +and runs the associated command function for them. Command functions +are declared using the DECL_COMMAND() macro. + +Task, init, and command functions always run with interrupts enabled +(however, they can temporarily disable interrupts if needed). These +functions should never pause, delay, or do any work that lasts more +than a few micro-seconds. These functions schedule work at specific +times by scheduling timers. + +Timer functions are scheduled by calling sched_timer() (located in +**src/sched.c**). The scheduler code will arrange for the given +function to be called at the requested clock time. Timer interrupts +are initially handled in an interrupt handler in **src/avr/timer.c**, +but this just calls sched_timer_kick() located in **src/sched.c**. The +timer interrupt leads to execution of schedule timer functions. Timer +functions always run with interrupts disabled. The timer functions +should always complete within a few micro-seconds. At completion of +the timer event, the function may choose to reschedule itself. + +In the event an error is detected the code can invoke shutdown() (a +macro which calls sched_shutdown() located in **src/sched.c**). +Invoking shutdown() causes all functions tagged with the +DECL_SHUTDOWN() macro to be run. Shutdown functions always run with +interrupts disabled. + +Much of the functionality of the micro-controller involves working +with General-Purpose Input/Output pins (GPIO). In order to abstract +the low-level architecture specific code from the high-level task +code, all GPIO events are implemented via wrappers. These wrappers are +located in **src/avr/gpio.c**. The code is compiled with gcc's "-flto +-fwhole-program" optimization which does an excellent job of inlining +functions across compilation units, so most of these tiny gpio +functions are inlined into their callers, and there is no run-time +cost to using them. + +Klippy code overview +==================== + +The host code (Klippy) is intended to run on a low-cost computer (such +as a Raspberry Pi) paired with the micro-controller. The code is +primarily written in Python, however it does use CFFI to implement +some functionality in C code. + +Initial execution starts in **klippy/klippy.py**. This reads the +command-line arguments, opens the printer config file, instantiates +the main printer objects, and starts the serial connection. The main +execution of gcode commands is in the process_commands() method in +**klippy/gcode.py**. This code translates the gcode commands into +printer object calls, which frequently translate the actions to +commands to be executed on the micro-controller (as declared via the +DECL_COMMAND macro in the micro-controller code). + +There are three threads in the Klippy host code. The main thread +handles incoming gcode commands. A second thread (which resides +entirely in the **klippy/serialqueue.c** C code) handles low-level IO +with the serial port. The third thread is used to process response +messages from the micro-controller in the Python code. diff --git a/docs/Debugging.md b/docs/Debugging.md new file mode 100644 index 000000000..ed2d92c90 --- /dev/null +++ b/docs/Debugging.md @@ -0,0 +1,90 @@ +The Klippy host code has some tools to help in debugging the firmware. + +Testing with simulavr +===================== + +The [simulavr](http://www.nongnu.org/simulavr/) tool enables one to +simulate an Atmel ATmega micro-controller. This section describes how +one can run test gcode files through simulavr. It is recommended to +run this on a desktop class machine (not a Raspberry Pi) as it does +require significant cpu to run efficiently. + +To use simulavr, download the simulavr package and compile with python +support: + +``` +git clone git://git.savannah.nongnu.org/simulavr.git +cd simulavr +./bootstrap +./configure --enable-python +make +``` + +Note that the build system may need to have some packages (such as +swig) installed in order to build the python module. Make sure the +file **src/python/_pysimulavr.so** is present after the above +compilation. + +To compile Klipper for use in simulavr, run: + +``` +cd /patch/to/klipper +make menuconfig +``` + +and compile the firmware for an AVR atmega644p, disable the AVR +watchdog timer, set the MCU frequency to 20000000, and set the serial +baud rate to 115200. Then one can compile Klipper (run `make`) and +then start the simulation with: + +``` +PYTHONPATH=/path/to/simulavr/src/python/ ./scripts/avrsim.py -m atmega644 -s 20000000 -b 115200 out/klipper.elf +``` + +It may be necessary to create a python virtual environment to run +Klippy on the target machine. To do so, run: + +``` +virtualenv ~/klippy-env +~/klippy-env/bin/pip install cffi==1.6.0 pyserial==2.7 +``` + +Then, with simulavr running in another window, one can run the +following to read gcode from a file (eg, "test.gcode"), process it +with Klippy, and send it to Klipper running in simulavr: + +``` +~/klippy-env/bin/python ./klippy/klippy.py config/avrsim.cfg -i test.gcode -v +``` + +Using simulavr with gtkwave +--------------------------- + +One useful feature of simulavr is its ability to create signal wave +generation files with the exact timing of events. To do this, follow +the directions above, but run avrsim.py with a command-line like the +following: + +``` +PYTHONPATH=/path/to/simulavr/src/python/ ./scripts/avrsim.py -m atmega644 -s 20000000 -b 115200 out/klipper.elf -t PORTA.PORT,PORTC.PORT +``` + +The above would create a file **avrsim.vcd** with information on each +change to the GPIOs on PORTA and PORTB. This could then be viewed +using gtkwave with: + +``` +gtkwave avrsim.vcd +``` + +Manually sending commands to the micro-controller +------------------------------------------------- + +Normally, Klippy would be used to translate gcode commands to Klipper +commands. However, it's also possible to manually send Klipper +commands (functions marked with the DECL_COMMAND() macro in the +Klipper source code). To do so, run: + +``` +~/klippy-env/bin/python ./klippy/console.py /tmp/pseudoserial 115200 +``` diff --git a/docs/Installation.md b/docs/Installation.md new file mode 100644 index 000000000..aae135115 --- /dev/null +++ b/docs/Installation.md @@ -0,0 +1,118 @@ +Klipper is currently in an experimental state. These instructions +assume the software will run on a Raspberry Pi computer in conjunction +with OctoPrint. Klipper supports only Atmel ATmega based +micro-controllers at this time. + +It is recommended that a Raspberry Pi 2 or Raspberry Pi 3 computer be +used as the host. The software will run on a first generation +Raspberry Pi, but the combined load of OctoPrint, Klipper, and a web +cam (if applicable) can overwhelm its CPU leading to print stalls. + +Prepping an OS image +==================== + +Start by installing [OctoPi](https://github.com/guysoft/OctoPi) on the +Raspberry Pi computer. Use version 0.13.0 or later - see the +[octopi releases](https://github.com/guysoft/OctoPi/releases) for +release information. One should verify that OctoPi boots, that the +OctoPrint web server works, and that one can ssh to the octopi server +(ssh pi@octopi -- password is "raspberry") before continuing. + +After installing OctoPi, ssh into the target machine and run the +following commands: + +``` +sudo apt-get update +sudo apt-get install avrdude gcc-avr binutils-avr avr-libc libncurses-dev +``` + +The host software (Klippy) requires a one-time setup - run as the +regular "pi" user: + +``` +virtualenv ~/klippy-env +~/klippy-env/bin/pip install cffi==1.6.0 pyserial==2.7 +``` + +Building Klipper +================ + +To obtain Klipper, run the following command on the target machine: + +``` +git clone https://github.com/KevinOConnor/klipper +cd klipper/ +``` + +To compile the micro-controller code, start by configuring it: + +``` +make menuconfig +``` + +Select the appropriate micro-controller and serial baud rate. Once +configured, run: + +``` +make +``` + +Ignore any warnings you may see about "misspelled signal handler" (it +is due to a bug fixed in gcc v4.8.3). + +Installing Klipper on a micro-controller +---------------------------------------- + +The avrdude package can be used to install the micro-controller code +on an AVR ATmega chip. The exact syntax of the avrdude command is +different for each micro-controller. The following is an example +command for atmega2560 chips: + +``` +example-only$ avrdude -C/etc/avrdude.conf -v -patmega2560 -cwiring -P/dev/ttyACM0 -b115200 -D -Uflash:w:/home/pi/klipper/out/klipper.elf.hex:i +``` + +Setting up the printer configuration +==================================== + +It is necessary to configure the printer. This is done by modifying a +configuration file that resides on the host. Start by copying an +example configuration and editing it. For example: + +``` +cp ~/klipper/config/example.cfg ~/printer.cfg +nano printer.cfg +``` + +Make sure to look at and update each setting that is appropriate for +the hardware. + +Configuring OctoPrint to use Klippy +=================================== + +The OctoPrint web server needs to be configured to communicate with +the Klippy host software. Using a web-browser, login to the OctoPrint +web page, and navigate to the Settings tab. Then configure the +following items: + +Under "Serial Connection" in "Additional serial ports" add +"/tmp/printer". Then click "Save". + +Enter the Settings tab again and under "Serial Connection" change the +"Serial Port" setting to "/tmp/printer". + +Under the "Features" tab, unselect "Enable SD support". Then click +"Save". + +Running the host software +========================= + +The host software is executed by running the following as the regular +"pi" user: + +``` +~/klippy-env/bin/python ~/klipper/klippy/klippy.py ~/printer.cfg -l /tmp/klippy.log < /dev/null > /tmp/klippy-errors.log 2>&1 & +``` + +Once Klippy is running, use a web-browser and navigate to the +OctoPrint web site. Click on "Connect" under the "Connection" tab. diff --git a/docs/Overview.md b/docs/Overview.md new file mode 100644 index 000000000..05311c27f --- /dev/null +++ b/docs/Overview.md @@ -0,0 +1,8 @@ +See [installation](Installation.md) for information on compiling, +installing, and running Klipper. + +See [code overview](Code_Overview.md) for developer information on the +structure and layout of the Klipper code. + +See [debugging](Debugging.md) for developer information on how to test +and debug Klipper. diff --git a/klippy/cartesian.py b/klippy/cartesian.py new file mode 100644 index 000000000..40840077b --- /dev/null +++ b/klippy/cartesian.py @@ -0,0 +1,252 @@ +# Code for handling cartesian (standard x, y, z planes) moves +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import math, logging, time +import lookahead, stepper, homing + +# Common suffixes: _d is distance (in mm), _v is velocity (in +# mm/second), _t is time (in seconds), _r is ratio (scalar between +# 0.0 and 1.0) + +StepList = (0, 1, 2, 3) + +class Move: + def __init__(self, kin, relsteps, speed): + self.kin = kin + self.relsteps = relsteps + self.junction_max = self.junction_start_max = self.junction_delta = 0. + # Calculate requested distance to travel (in mm) + steppers = self.kin.steppers + absrelsteps = [abs(relsteps[i]) for i in StepList] + stepper_d = [absrelsteps[i] * steppers[i].step_dist + for i in StepList] + self.move_d = math.sqrt(sum([d*d for d in stepper_d[:3]])) + if not self.move_d: + self.move_d = stepper_d[3] + if not self.move_d: + return + # Limit velocity to max for each stepper + velocity_factor = min([steppers[i].max_step_velocity / absrelsteps[i] + for i in StepList if absrelsteps[i]]) + move_v = min(speed, velocity_factor * self.move_d) + self.junction_max = move_v**2 + # Find max acceleration factor + accel_factor = min([steppers[i].max_step_accel / absrelsteps[i] + for i in StepList if absrelsteps[i]]) + accel = min(self.kin.max_accel, accel_factor * self.move_d) + self.junction_delta = 2.0 * self.move_d * accel + def calc_junction(self, prev_move): + # Find max start junction velocity using approximated + # centripetal velocity as described at: + # https://onehossshay.wordpress.com/2011/09/24/improving_grbl_cornering_algorithm/ + if not prev_move.move_d or self.relsteps[2] or prev_move.relsteps[2]: + return + steppers = self.kin.steppers + junction_cos_theta = -sum([ + self.relsteps[i] * prev_move.relsteps[i] * steppers[i].step_dist**2 + for i in range(2)]) / (self.move_d * prev_move.move_d) + if junction_cos_theta > 0.999999: + return + junction_cos_theta = max(junction_cos_theta, -0.999999) + sin_theta_d2 = math.sqrt(0.5*(1.0-junction_cos_theta)); + R = self.kin.junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2) + accel = self.junction_delta / (2.0 * self.move_d) + self.junction_start_max = min( + accel * R, self.junction_max, prev_move.junction_max) + def process(self, junction_start, junction_end): + # Determine accel, cruise, and decel portions of the move + junction_cruise = self.junction_max + inv_junction_delta = 1. / self.junction_delta + accel_r = (junction_cruise-junction_start) * inv_junction_delta + decel_r = (junction_cruise-junction_end) * inv_junction_delta + cruise_r = 1. - accel_r - decel_r + if cruise_r < 0.: + accel_r += 0.5 * cruise_r + decel_r = 1.0 - accel_r + cruise_r = 0. + junction_cruise = junction_start + accel_r*self.junction_delta + # Determine the move velocities and time spent in each portion + start_v = math.sqrt(junction_start) + cruise_v = math.sqrt(junction_cruise) + end_v = math.sqrt(junction_end) + inv_cruise_v = 1. / cruise_v + inv_accel = 2.0 * self.move_d * inv_junction_delta + accel_t = 2.0 * self.move_d * accel_r / (start_v+cruise_v) + cruise_t = self.move_d * cruise_r * inv_cruise_v + decel_t = 2.0 * self.move_d * decel_r / (end_v+cruise_v) + + #logging.debug("Move: %s v=%.2f/%.2f/%.2f mt=%.3f st=%.3f %.3f %.3f" % ( + # self.relsteps, start_v, cruise_v, end_v, move_t + # , next_move_time, accel_r, cruise_r)) + + # Calculate step times for the move + next_move_time = self.kin.get_next_move_time() + for i in StepList: + steps = self.relsteps[i] + if not steps: + continue + sdir = 0 + if steps < 0: + sdir = 1 + steps = -steps + clock_offset, clock_freq, so = self.kin.steppers[i].prep_move( + sdir, next_move_time) + + step_dist = self.move_d / steps + step_offset = 0.5 + + # Acceleration steps + #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel + accel_clock_offset = start_v * inv_accel * clock_freq + accel_sqrt_offset = accel_clock_offset**2 + accel_multiplier = 2.0 * step_dist * inv_accel * clock_freq**2 + accel_steps = accel_r * steps + step_offset = so.step_sqrt( + accel_steps, step_offset, clock_offset - accel_clock_offset + , accel_sqrt_offset, accel_multiplier) + clock_offset += accel_t * clock_freq + # Cruising steps + #t = pos/cruise_v + cruise_multiplier = step_dist * inv_cruise_v * clock_freq + cruise_steps = cruise_r * steps + step_offset = so.step_factor( + cruise_steps, step_offset, clock_offset, cruise_multiplier) + clock_offset += cruise_t * clock_freq + # Deceleration steps + #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel) + decel_clock_offset = cruise_v * inv_accel * clock_freq + decel_sqrt_offset = decel_clock_offset**2 + decel_steps = decel_r * steps + so.step_sqrt( + decel_steps, step_offset, clock_offset + decel_clock_offset + , decel_sqrt_offset, -accel_multiplier) + self.kin.update_move_time(accel_t + cruise_t + decel_t) + +STALL_TIME = 0.100 + +class CartKinematics: + def __init__(self, printer, config): + self.printer = printer + self.reactor = printer.reactor + steppers = ['stepper_x', 'stepper_y', 'stepper_z', 'stepper_e'] + self.steppers = [stepper.PrinterStepper(printer, config.getsection(n)) + for n in steppers] + self.max_accel = min(s.max_step_accel*s.step_dist + for s in self.steppers[:2]) # XXX + dummy_move = Move(self, [0]*len(self.steppers), 0.) + dummy_move.junction_max = 0. + self.junction_deviation = config.getfloat('junction_deviation', 0.02) + self.move_queue = lookahead.MoveQueue(dummy_move) + self.pos = [0, 0, 0, 0] + # Print time tracking + self.buffer_time_high = config.getfloat('buffer_time_high', 5.000) + self.buffer_time_low = config.getfloat('buffer_time_low', 0.150) + self.move_flush_time = config.getfloat('move_flush_time', 0.050) + self.motor_off_delay = config.getfloat('motor_off_time', 60.000) + self.print_time = 0. + self.print_time_stall = 0 + self.motor_off_time = self.reactor.NEVER + self.flush_timer = self.reactor.register_timer(self.flush_handler) + def build_config(self): + for stepper in self.steppers: + stepper.build_config() + # Print time tracking + def update_move_time(self, movetime): + self.print_time += movetime + flush_to_time = self.print_time - self.move_flush_time + self.printer.mcu.flush_moves(flush_to_time) + def get_next_move_time(self): + if not self.print_time: + self.print_time = self.buffer_time_low + STALL_TIME + curtime = time.time() + self.printer.mcu.set_print_start_time(curtime) + self.reactor.update_timer(self.flush_timer, self.reactor.NOW) + return self.print_time + def get_last_move_time(self): + self.move_queue.flush() + return self.get_next_move_time() + def reset_motor_off_time(self, eventtime): + self.motor_off_time = eventtime + self.motor_off_delay + def reset_print_time(self): + self.move_queue.flush() + self.printer.mcu.flush_moves(self.print_time) + self.print_time = 0. + self.reset_motor_off_time(time.time()) + self.reactor.update_timer(self.flush_timer, self.motor_off_time) + def check_busy(self, eventtime): + if not self.print_time: + # XXX - find better way to flush initial move_queue items + if self.move_queue.queue: + self.reactor.update_timer(self.flush_timer, eventtime + 0.100) + return False + buffer_time = self.printer.mcu.get_print_buffer_time( + eventtime, self.print_time) + return buffer_time > self.buffer_time_high + def flush_handler(self, eventtime): + if not self.print_time: + self.move_queue.flush() + if not self.print_time: + if eventtime >= self.motor_off_time: + self.motor_off() + self.reset_print_time() + self.motor_off_time = self.reactor.NEVER + return self.motor_off_time + print_time = self.print_time + buffer_time = self.printer.mcu.get_print_buffer_time( + eventtime, print_time) + if buffer_time > self.buffer_time_low: + return eventtime + buffer_time - self.buffer_time_low + self.move_queue.flush() + if print_time != self.print_time: + self.print_time_stall += 1 + self.dwell(self.buffer_time_low + STALL_TIME) + return self.reactor.NOW + self.reset_print_time() + return self.motor_off_time + def stats(self, eventtime): + buffer_time = 0. + if self.print_time: + buffer_time = self.printer.mcu.get_print_buffer_time( + eventtime, self.print_time) + return "print_time=%.3f buffer_time=%.3f print_time_stall=%d" % ( + self.print_time, buffer_time, self.print_time_stall) + # Movement commands + def get_position(self): + return [self.pos[i] * self.steppers[i].step_dist + for i in StepList] + def set_position(self, newpos): + self.pos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5) + for i in StepList] + def move(self, newpos, speed, sloppy=False): + # Round to closest step position + newpos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5) + for i in StepList] + relsteps = [newpos[i] - self.pos[i] for i in StepList] + self.pos = newpos + if relsteps == [0]*len(newpos): + # no move + return + #logging.debug("; dist %s @ %d\n" % ( + # [newpos[i]*self.steppers[i].step_dist for i in StepList], speed)) + # Create move and queue it + move = Move(self, relsteps, speed) + move.calc_junction(self.move_queue.prev_move()) + self.move_queue.add_move(move) + def home(self, axis): + # Each axis is homed independently and in order + homing_state = homing.Homing(self, self.steppers) + for a in axis: + homing_state.plan_home(a) + return homing_state + def dwell(self, delay): + self.get_last_move_time() + self.update_move_time(delay) + def motor_off(self): + self.dwell(STALL_TIME) + last_move_time = self.get_last_move_time() + for stepper in self.steppers: + stepper.motor_enable(last_move_time, 0) + self.dwell(STALL_TIME) + logging.debug('; Max time of %f' % (last_move_time,)) diff --git a/klippy/chelper.py b/klippy/chelper.py new file mode 100644 index 000000000..99ec3f4fd --- /dev/null +++ b/klippy/chelper.py @@ -0,0 +1,95 @@ +# Wrapper around C helper code +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import os, logging +import cffi + +COMPILE_CMD = "gcc -Wall -g -O -shared -fPIC -o %s %s" +SOURCE_FILES = ['stepcompress.c', 'serialqueue.c'] +DEST_LIB = "c_helper.so" +OTHER_FILES = ['list.h', 'serialqueue.h'] + +defs_stepcompress = """ + struct stepcompress *stepcompress_alloc(uint32_t max_error + , uint32_t queue_step_msgid, uint32_t oid); + void stepcompress_push(struct stepcompress *sc, double step_clock); + double stepcompress_push_factor(struct stepcompress *sc + , double steps, double step_offset + , double clock_offset, double factor); + double stepcompress_push_sqrt(struct stepcompress *sc + , double steps, double step_offset + , double clock_offset, double sqrt_offset, double factor); + void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); + void stepcompress_queue_msg(struct stepcompress *sc + , uint32_t *data, int len); + uint32_t stepcompress_get_errors(struct stepcompress *sc); + + struct steppersync *steppersync_alloc(struct serialqueue *sq + , struct stepcompress **sc_list, int sc_num, int move_num); + void steppersync_flush(struct steppersync *ss, uint64_t move_clock); +""" + +defs_serialqueue = """ + #define MESSAGE_MAX 64 + struct pull_queue_message { + uint8_t msg[MESSAGE_MAX]; + int len; + double sent_time, receive_time; + }; + + struct serialqueue *serialqueue_alloc(int serial_fd, double baud_adjust + , int write_only); + void serialqueue_exit(struct serialqueue *sq); + struct command_queue *serialqueue_alloc_commandqueue(void); + void serialqueue_send(struct serialqueue *sq, struct command_queue *cq + , uint8_t *msg, int len, uint64_t min_clock, uint64_t req_clock); + void serialqueue_encode_and_send(struct serialqueue *sq + , struct command_queue *cq, uint32_t *data, int len + , uint64_t min_clock, uint64_t req_clock); + void serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm); + void serialqueue_set_clock_est(struct serialqueue *sq, double est_clock + , double last_ack_time, uint64_t last_ack_clock); + void serialqueue_flush_ready(struct serialqueue *sq); + void serialqueue_get_stats(struct serialqueue *sq, char *buf, int len); + int serialqueue_extract_old(struct serialqueue *sq, int sentq + , struct pull_queue_message *q, int max); +""" + +# Return the list of file modification times +def get_mtimes(srcdir, filelist): + out = [] + for filename in filelist: + pathname = os.path.join(srcdir, filename) + try: + t = os.path.getmtime(pathname) + except os.error: + continue + out.append(t) + return out + +# Check if the code needs to be compiled +def check_build_code(srcdir): + src_times = get_mtimes(srcdir, SOURCE_FILES + OTHER_FILES) + obj_times = get_mtimes(srcdir, [DEST_LIB]) + if not obj_times or max(src_times) > min(obj_times): + logging.info("Building C code module") + srcfiles = [os.path.join(srcdir, fname) for fname in SOURCE_FILES] + destlib = os.path.join(srcdir, DEST_LIB) + os.system(COMPILE_CMD % (destlib, ' '.join(srcfiles))) + +FFI_main = None +FFI_lib = None + +# Return the Foreign Function Interface api to the caller +def get_ffi(): + global FFI_main, FFI_lib + if FFI_lib is None: + srcdir = os.path.dirname(os.path.realpath(__file__)) + check_build_code(srcdir) + FFI_main = cffi.FFI() + FFI_main.cdef(defs_stepcompress) + FFI_main.cdef(defs_serialqueue) + FFI_lib = FFI_main.dlopen(os.path.join(srcdir, DEST_LIB)) + return FFI_main, FFI_lib diff --git a/klippy/console.py b/klippy/console.py new file mode 100755 index 000000000..4782702a1 --- /dev/null +++ b/klippy/console.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python +# Script to implement a test console with firmware over serial port +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import sys, optparse, os, re, logging + +import reactor, serialhdl, pins, util, msgproto + +re_eval = re.compile(r'\{(?P<eval>[^}]*)\}') + +class KeyboardReader: + def __init__(self, ser, reactor): + self.ser = ser + self.reactor = reactor + self.fd = sys.stdin.fileno() + util.set_nonblock(self.fd) + self.pins = None + self.data = "" + self.reactor.register_fd(self.fd, self.process_kbd) + self.local_commands = { "PINS": self.set_pin_map } + self.eval_globals = {} + def update_evals(self, eventtime): + f = self.ser.msgparser.config.get('CLOCK_FREQ', 1) + c = (eventtime - self.ser.last_ack_time) * f + self.ser.last_ack_clock + self.eval_globals['freq'] = f + self.eval_globals['clock'] = int(c) + def set_pin_map(self, parts): + mcu = self.ser.msgparser.config['MCU'] + self.pins = pins.map_pins(parts[1], mcu) + def lookup_pin(self, value): + if self.pins is None: + self.pins = pins.mcu_to_pins(self.ser.msgparser.config['MCU']) + return self.pins[value] + def translate(self, line, eventtime): + evalparts = re_eval.split(line) + if len(evalparts) > 1: + self.update_evals(eventtime) + try: + for i in range(1, len(evalparts), 2): + evalparts[i] = str(eval(evalparts[i], self.eval_globals)) + except: + print "Unable to evaluate: ", line + return None + line = ''.join(evalparts) + print "Eval:", line + if self.pins is None and self.ser.msgparser.config: + self.pins = pins.mcu_to_pins(self.ser.msgparser.config['MCU']) + if self.pins is not None: + try: + line = pins.update_command(line, self.pins).strip() + except: + print "Unable to map pin: ", line + return None + if line: + parts = line.split() + if parts[0] in self.local_commands: + self.local_commands[parts[0]](parts) + return None + try: + msg = self.ser.msgparser.create_command(line) + except msgproto.error, e: + print "Error:", e + return None + return msg + def process_kbd(self, eventtime): + self.data += os.read(self.fd, 4096) + + kbdlines = self.data.split('\n') + for line in kbdlines[:-1]: + line = line.strip() + cpos = line.find('#') + if cpos >= 0: + line = line[:cpos] + if not line: + continue + msg = self.translate(line.strip(), eventtime) + if msg is None: + continue + self.ser.send(msg) + self.data = kbdlines[-1] + +def main(): + usage = "%prog [options] <serialdevice> <baud>" + opts = optparse.OptionParser(usage) + options, args = opts.parse_args() + serialport, baud = args + baud = int(baud) + + logging.basicConfig(level=logging.DEBUG) + r = reactor.Reactor() + ser = serialhdl.SerialReader(r, serialport, baud) + ser.connect() + kbd = KeyboardReader(ser, r) + try: + r.run() + except KeyboardInterrupt: + sys.stdout.write("\n") + +if __name__ == '__main__': + main() diff --git a/klippy/fan.py b/klippy/fan.py new file mode 100644 index 000000000..523748446 --- /dev/null +++ b/klippy/fan.py @@ -0,0 +1,39 @@ +# Printer fan support +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +FAN_MIN_TIME = 0.1 + +class PrinterFan: + def __init__(self, printer, config): + self.printer = printer + self.config = config + self.mcu_fan = None + self.last_fan_clock = self.last_fan_value = 0 + self.min_fan_clock = 0 + self.kick_start_clock = 0 + def build_config(self): + pin = self.config.get('pin') + hard_pwm = self.config.getint('hard_pwm', 128) + mcu_freq = self.printer.mcu.get_mcu_freq() + self.min_fan_clock = int(FAN_MIN_TIME * mcu_freq) + kst = self.config.getfloat('kick_start_time', 0.1) + self.kick_start_clock = int(kst * mcu_freq) + self.mcu_fan = self.printer.mcu.create_pwm(pin, hard_pwm, 0) + # External commands + def set_speed(self, print_time, value): + value = max(0, min(255, int(value*255. + 0.5))) + if value == self.last_fan_value: + return + pc = int(self.mcu_fan.get_print_clock(print_time)) + pc = max(self.last_fan_clock + self.min_fan_clock, pc) + if (value and value < 255 + and not self.last_fan_value and self.kick_start_clock): + # Run fan at full speed for specified kick_start_time + self.mcu_fan.set_pwm(pc, 255) + pc += self.kick_start_clock + self.mcu_fan.set_pwm(pc, value) + self.last_fan_clock = pc + self.last_fan_value = value diff --git a/klippy/gcode.py b/klippy/gcode.py new file mode 100644 index 000000000..fc697b982 --- /dev/null +++ b/klippy/gcode.py @@ -0,0 +1,315 @@ +# Parse gcode commands +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import os, re, logging + +# Parse out incoming GCode and find and translate head movements +class GCodeParser: + RETRY_TIME = 0.100 + def __init__(self, printer, fd, inputfile=False): + self.printer = printer + self.fd = fd + self.inputfile = inputfile + # Input handling + self.reactor = printer.reactor + self.fd_handle = None + self.input_commands = [""] + self.need_register_fd = False + self.bytes_read = 0 + # Busy handling + self.busy_timer = self.reactor.register_timer(self.busy_handler) + self.busy_state = None + # Command handling + self.gcode_handlers = {} + self.is_shutdown = False + self.need_ack = False + self.kin = self.heater_nozzle = self.heater_bed = self.fan = None + self.movemult = 1.0 + self.speed = 1.0 + self.absolutecoord = self.absoluteextrude = True + self.base_position = [0.0, 0.0, 0.0, 0.0] + self.last_position = [0.0, 0.0, 0.0, 0.0] + self.homing_add = [0.0, 0.0, 0.0, 0.0] + self.axis2pos = {'X': 0, 'Y': 1, 'Z': 2, 'E': 3} + def build_config(self): + self.kin = self.printer.objects['kinematics'] + self.heater_nozzle = self.printer.objects.get('heater_nozzle') + self.heater_bed = self.printer.objects.get('heater_bed') + self.fan = self.printer.objects.get('fan') + self.build_handlers() + def build_handlers(self): + shutdown_handlers = ['M105', 'M110', 'M114'] + handlers = ['G0', 'G1', 'G4', 'G20', 'G21', 'G28', 'G90', 'G91', 'G92', + 'M18', 'M82', 'M83', 'M84', 'M110', 'M114', 'M206'] + if self.heater_nozzle is not None: + handlers.extend(['M104', 'M105', 'M109', 'M303']) + if self.heater_bed is not None: + handlers.extend(['M140', 'M190']) + if self.fan is not None: + handlers.extend(['M106', 'M107']) + if self.is_shutdown: + handlers = [h for h in handlers if h in shutdown_handlers] + self.gcode_handlers = dict((h, getattr(self, 'cmd_'+h)) + for h in handlers) + def run(self): + if self.heater_nozzle is not None: + self.heater_nozzle.run() + if self.heater_bed is not None: + self.heater_bed.run() + self.fd_handle = self.reactor.register_fd(self.fd, self.process_data) + self.reactor.run() + def finish(self): + self.reactor.end() + self.kin.motor_off() + logging.debug('Completed translation by klippy') + def stats(self, eventtime): + return "gcodein=%d" % (self.bytes_read,) + def shutdown(self): + self.is_shutdown = True + self.build_handlers() + # Parse input into commands + args_r = re.compile('([a-zA-Z*])') + def process_commands(self, eventtime): + i = -1 + for i in range(len(self.input_commands)-1): + line = self.input_commands[i] + # Ignore comments and leading/trailing spaces + line = origline = line.strip() + cpos = line.find(';') + if cpos >= 0: + line = line[:cpos] + # Break command into parts + parts = self.args_r.split(line)[1:] + params = dict((parts[i].upper(), parts[i+1].strip()) + for i in range(0, len(parts), 2)) + params['#original'] = origline + if parts and parts[0].upper() == 'N': + # Skip line number at start of command + del parts[:2] + if not parts: + self.cmd_default(params) + continue + params['#command'] = cmd = parts[0] + parts[1].strip() + # Invoke handler for command + self.need_ack = True + handler = self.gcode_handlers.get(cmd, self.cmd_default) + try: + handler(params) + except: + logging.exception("Exception in command handler") + self.respond('echo:Internal error on command:"%s"' % (cmd,)) + # Check if machine can process next command or must stall input + if self.busy_state is not None: + break + if self.kin.check_busy(eventtime): + self.set_busy(self.kin) + break + self.ack() + del self.input_commands[:i+1] + def process_data(self, eventtime): + if self.busy_state is not None: + self.reactor.unregister_fd(self.fd_handle) + self.need_register_fd = True + return + data = os.read(self.fd, 4096) + self.bytes_read += len(data) + lines = data.split('\n') + lines[0] = self.input_commands[0] + lines[0] + self.input_commands = lines + self.process_commands(eventtime) + if not data and self.inputfile: + self.finish() + # Response handling + def ack(self, msg=None): + if not self.need_ack or self.inputfile: + return + if msg: + os.write(self.fd, "ok %s\n" % (msg,)) + else: + os.write(self.fd, "ok\n") + self.need_ack = False + def respond(self, msg): + logging.debug(msg) + if self.inputfile: + return + os.write(self.fd, msg+"\n") + # Busy handling + def set_busy(self, busy_handler): + self.busy_state = busy_handler + self.reactor.update_timer(self.busy_timer, self.reactor.NOW) + def busy_handler(self, eventtime): + busy = self.busy_state.check_busy(eventtime) + if busy: + self.kin.reset_motor_off_time(eventtime) + return eventtime + self.RETRY_TIME + self.busy_state = None + self.ack() + self.process_commands(eventtime) + if self.busy_state is not None: + return self.reactor.NOW + if self.need_register_fd: + self.need_register_fd = False + self.fd_handle = self.reactor.register_fd(self.fd, self.process_data) + return self.reactor.NEVER + # Temperature wrappers + def get_temp(self): + # T:XXX /YYY B:XXX /YYY + out = [] + if self.heater_nozzle: + cur, target = self.heater_nozzle.get_temp() + out.append("T:%.1f /%.1f" % (cur, target)) + if self.heater_bed: + cur, target = self.heater_bed.get_temp() + out.append("B:%.1f /%.1f" % (cur, target)) + return " ".join(out) + def bg_temp(self, heater): + # Wrapper class for check_busy() that periodically prints current temp + class temp_busy_handler_wrapper: + gcode = self + last_temp_time = 0. + cur_heater = heater + def check_busy(self, eventtime): + if eventtime > self.last_temp_time + 1.0: + self.gcode.respond(self.gcode.get_temp()) + self.last_temp_time = eventtime + return self.cur_heater.check_busy(eventtime) + if self.inputfile: + return + self.set_busy(temp_busy_handler_wrapper()) + def set_temp(self, heater, params, wait=False): + print_time = self.kin.get_last_move_time() + temp = float(params.get('S', '0')) + heater.set_temp(print_time, temp) + if wait: + self.bg_temp(heater) + # Individual command handlers + def cmd_default(self, params): + if self.is_shutdown: + self.respond('Error: Machine is shutdown') + return + cmd = params.get('#command') + if not cmd: + logging.debug(params['#original']) + return + self.respond('echo:Unknown command:"%s"' % (cmd,)) + def cmd_G0(self, params): + self.cmd_G1(params, sloppy=True) + def cmd_G1(self, params, sloppy=False): + # Move + for a, p in self.axis2pos.items(): + if a in params: + v = float(params[a]) + if not self.absolutecoord or (p>2 and not self.absoluteextrude): + # value relative to position of last move + self.last_position[p] += v + else: + # value relative to base coordinate position + self.last_position[p] = v + self.base_position[p] + if 'F' in params: + self.speed = float(params['F']) / 60. + self.kin.move(self.last_position, self.speed, sloppy) + def cmd_G4(self, params): + # Dwell + if 'S' in params: + delay = float(params['S']) + else: + delay = float(params.get('P', '0')) / 1000. + self.kin.dwell(delay) + def cmd_G20(self, params): + # Set units to inches + self.movemult = 25.4 + def cmd_G21(self, params): + # Set units to millimeters + self.movemult = 1.0 + def cmd_G28(self, params): + # Move to origin + axis = [] + for a in 'XYZ': + if a in params: + axis.append(self.axis2pos[a]) + if not axis: + axis = [0, 1, 2] + busy_handler = self.kin.home(axis) + def axis_update(axis): + newpos = self.kin.get_position() + for a in axis: + self.last_position[a] = newpos[a] + self.base_position[a] = -self.homing_add[a] + busy_handler.plan_axis_update(axis_update) + self.set_busy(busy_handler) + def cmd_G90(self, params): + # Use absolute coordinates + self.absolutecoord = True + def cmd_G91(self, params): + # Use relative coordinates + self.absolutecoord = False + def cmd_G92(self, params): + # Set position + mcount = 0 + for a, p in self.axis2pos.items(): + if a in params: + self.base_position[p] = self.last_position[p] - float(params[a]) + mcount += 1 + if not mcount: + self.base_position = list(self.last_position) + def cmd_M82(self, params): + # Use absolute distances for extrusion + self.absoluteextrude = True + def cmd_M83(self, params): + # Use relative distances for extrusion + self.absoluteextrude = False + def cmd_M18(self, params): + # Turn off motors + self.kin.motor_off() + def cmd_M84(self, params): + # Stop idle hold + self.kin.motor_off() + def cmd_M105(self, params): + # Get Extruder Temperature + self.ack(self.get_temp()) + def cmd_M104(self, params): + # Set Extruder Temperature + self.set_temp(self.heater_nozzle, params) + def cmd_M109(self, params): + # Set Extruder Temperature and Wait + self.set_temp(self.heater_nozzle, params, wait=True) + def cmd_M110(self, params): + # Set Current Line Number + pass + def cmd_M114(self, params): + # Get Current Position + kinpos = self.kin.get_position() + self.respond("X:%.3f Y:%.3f Z:%.3f E:%.3f Count X:%.3f Y:%.3f Z:%.3f" % ( + self.last_position[0], self.last_position[1], + self.last_position[2], self.last_position[3], + kinpos[0], kinpos[1], kinpos[2])) + def cmd_M140(self, params): + # Set Bed Temperature + self.set_temp(self.heater_bed, params) + def cmd_M190(self, params): + # Set Bed Temperature and Wait + self.set_temp(self.heater_bed, params, wait=True) + def cmd_M106(self, params): + # Set fan speed + print_time = self.kin.get_last_move_time() + self.fan.set_speed(print_time, float(params.get('S', '255')) / 255.) + def cmd_M107(self, params): + # Turn fan off + print_time = self.kin.get_last_move_time() + self.fan.set_speed(print_time, 0) + def cmd_M206(self, params): + # Set home offset + for a, p in self.axis2pos.items(): + if a in params: + v = float(params[a]) + self.base_position[p] += self.homing_add[p] - v + self.homing_add[p] = v + def cmd_M303(self, params): + # Run PID tuning + heater = int(params.get('E', '0')) + heater = {0: self.heater_nozzle, -1: self.heater_bed}[heater] + temp = float(params.get('S', '60')) + heater.start_auto_tune(temp) + self.bg_temp(heater) diff --git a/klippy/heater.py b/klippy/heater.py new file mode 100644 index 000000000..d05035a0a --- /dev/null +++ b/klippy/heater.py @@ -0,0 +1,288 @@ +# Printer heater support +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import math, logging, threading + +# Mapping from name to Steinhart-Hart coefficients +Thermistors = { + "EPCOS 100K B57560G104F": ( + 0.000722136308968056, 0.000216766566488498, 8.92935804531095e-08) +} + +SAMPLE_TIME = 0.001 +SAMPLE_COUNT = 8 +REPORT_TIME = 0.300 +KELVIN_TO_CELCIUS = -273.15 +MAX_HEAT_TIME = 5.0 +AMBIENT_TEMP = 25. +PWM_MAX = 255 + +class PrinterHeater: + def __init__(self, printer, config): + self.printer = printer + self.config = config + self.mcu_pwm = self.mcu_adc = None + self.thermistor_c = Thermistors.get(config.get('thermistor_type')) + self.pullup_r = config.getfloat('pullup_resistor', 4700.) + self.lock = threading.Lock() + self.last_temp = 0. + self.last_temp_clock = 0 + self.target_temp = 0. + self.report_clock = 0 + self.control = None + # pwm caching + self.next_pwm_clock = 0 + self.last_pwm_value = 0 + self.resend_clock = 0 + self.pwm_offset_clock = 0 + def build_config(self): + heater_pin = self.config.get('heater_pin') + thermistor_pin = self.config.get('thermistor_pin') + self.mcu_pwm = self.printer.mcu.create_pwm(heater_pin, 0, MAX_HEAT_TIME) + self.mcu_adc = self.printer.mcu.create_adc(thermistor_pin) + min_adc = self.calc_adc(self.config.getfloat('max_temp')) + max_adc = self.calc_adc(self.config.getfloat('min_temp')) + freq = self.printer.mcu.get_mcu_freq() + sample_clock = int(SAMPLE_TIME*freq) + self.mcu_adc.set_minmax( + sample_clock, SAMPLE_COUNT, minval=min_adc, maxval=max_adc) + self.mcu_adc.set_adc_callback(self.adc_callback) + self.report_clock = int(REPORT_TIME*freq) + control_algo = self.config.get('control', 'watermark') + algos = {'watermark': ControlBangBang, 'pid': ControlPID} + self.control = algos[control_algo](self, self.config) + self.next_pwm_clock = 0 + self.last_pwm_value = 0 + self.resend_clock = int(MAX_HEAT_TIME * freq * 3. / 4.) + self.pwm_offset_clock = sample_clock*SAMPLE_COUNT + self.report_clock + def run(self): + self.mcu_adc.query_analog_in(self.report_clock) + def set_pwm(self, read_clock, value): + if value: + if self.target_temp <= 0.: + return + if (read_clock < self.next_pwm_clock + and abs(value - self.last_pwm_value) < 15): + return + elif not self.last_pwm_value: + return + pwm_clock = read_clock + self.pwm_offset_clock + self.next_pwm_clock = pwm_clock + self.resend_clock + self.last_pwm_value = value + logging.debug("pwm=%d@%d (%d)" % (value, read_clock, pwm_clock)) + self.mcu_pwm.set_pwm(pwm_clock, value) + # Temperature calculation + def calc_temp(self, adc): + r = self.pullup_r * adc / (1.0 - adc) + ln_r = math.log(r) + c1, c2, c3 = self.thermistor_c + temp_inv = c1 + c2*ln_r + c3*math.pow(ln_r, 3) + return 1.0/temp_inv + KELVIN_TO_CELCIUS + def calc_adc(self, temp): + if temp is None: + return None + c1, c2, c3 = self.thermistor_c + temp -= KELVIN_TO_CELCIUS + temp_inv = 1./temp + y = (c1 - temp_inv) / (2*c3) + x = math.sqrt(math.pow(c2 / (3.*c3), 3.) + math.pow(y, 2.)) + r = math.exp(math.pow(x-y, 1./3.) - math.pow(x+y, 1./3.)) + return r / (self.pullup_r + r) + def adc_callback(self, read_clock, read_value): + temp = self.calc_temp(float(read_value)) + with self.lock: + self.last_temp = temp + self.last_temp_clock = read_clock + self.control.adc_callback(read_clock, temp) + #logging.debug("temp: %d(%d) %f = %f" % ( + # read_clock, read_clock & 0xffffffff, read_value, temp)) + # External commands + def set_temp(self, print_time, degrees): + with self.lock: + self.target_temp = degrees + def get_temp(self): + with self.lock: + return self.last_temp, self.target_temp + def check_busy(self, eventtime): + with self.lock: + return self.control.check_busy(eventtime) + def start_auto_tune(self, temp): + with self.lock: + self.control = ControlAutoTune(self, self.control, temp) + + +###################################################################### +# Bang-bang control algo +###################################################################### + +class ControlBangBang: + def __init__(self, heater, config): + self.heater = heater + self.max_delta = config.getfloat('max_delta', 2.0) + self.heating = False + def adc_callback(self, read_clock, temp): + if self.heating and temp >= self.heater.target_temp+self.max_delta: + self.heating = False + elif not self.heating and temp <= self.heater.target_temp-self.max_delta: + self.heating = True + if self.heating: + self.heater.set_pwm(read_clock, PWM_MAX) + else: + self.heater.set_pwm(read_clock, 0) + def check_busy(self, eventtime): + return self.heater.last_temp < self.heater.target_temp-self.max_delta + + +###################################################################### +# Proportional Integral Derivative (PID) control algo +###################################################################### + +class ControlPID: + def __init__(self, heater, config): + self.heater = heater + self.Kp = config.getfloat('pid_Kp') + self.Ki = config.getfloat('pid_Ki') + self.Kd = config.getfloat('pid_Kd') + self.min_deriv_time = config.getfloat('pid_deriv_time', 2.) + imax = config.getint('pid_integral_max', PWM_MAX) + self.temp_integ_max = imax / self.Ki + self.prev_temp = AMBIENT_TEMP + self.prev_temp_clock = 0 + self.prev_temp_deriv = 0. + self.prev_temp_integ = 0. + self.inv_mcu_freq = 1. / self.heater.printer.mcu.get_mcu_freq() + def adc_callback(self, read_clock, temp): + time_diff = (read_clock - self.prev_temp_clock) * self.inv_mcu_freq + # Calculate change of temperature + temp_diff = temp - self.prev_temp + if time_diff >= self.min_deriv_time: + temp_deriv = temp_diff / time_diff + else: + temp_deriv = (self.prev_temp_deriv * (self.min_deriv_time-time_diff) + + temp_diff) / self.min_deriv_time + # Calculate accumulated temperature "error" + temp_err = self.heater.target_temp - temp + temp_integ = self.prev_temp_integ + temp_err * time_diff + temp_integ = max(0., min(self.temp_integ_max, temp_integ)) + # Calculate output + co = int(self.Kp*temp_err + self.Ki*temp_integ - self.Kd*temp_deriv) + #logging.debug("pid: %f@%d -> diff=%f deriv=%f err=%f integ=%f co=%d" % ( + # temp, read_clock, temp_diff, temp_deriv, temp_err, temp_integ, co)) + bounded_co = max(0, min(PWM_MAX, co)) + self.heater.set_pwm(read_clock, bounded_co) + # Store state for next measurement + self.prev_temp = temp + self.prev_temp_clock = read_clock + self.prev_temp_deriv = temp_deriv + if co == bounded_co: + self.prev_temp_integ = temp_integ + def check_busy(self, eventtime): + temp_diff = self.heater.target_temp - self.heater.last_temp + return abs(temp_diff) > 1. or abs(self.prev_temp_deriv) > 0.1 + + +###################################################################### +# Ziegler-Nichols PID autotuning +###################################################################### + +TUNE_PID_DELTA = 5.0 + +class ControlAutoTune: + def __init__(self, heater, old_control, target_temp): + self.heater = heater + self.old_control = old_control + self.target_temp = target_temp + self.heating = False + self.peaks = [] + self.peak = 0. + self.peak_clock = 0 + def adc_callback(self, read_clock, temp): + if self.heating and temp >= self.target_temp: + self.heating = False + self.check_peaks() + elif not self.heating and temp <= self.target_temp - TUNE_PID_DELTA: + self.heating = True + self.check_peaks() + if self.heating: + self.heater.set_pwm(read_clock, PWM_MAX) + if temp < self.peak: + self.peak = temp + self.peak_clock = read_clock + else: + self.heater.set_pwm(read_clock, 0) + if temp > self.peak: + self.peak = temp + self.peak_clock = read_clock + def check_peaks(self): + self.peaks.append((self.peak, self.peak_clock)) + if self.heating: + self.peak = 9999999. + else: + self.peak = -9999999. + if len(self.peaks) < 4: + return + temp_diff = self.peaks[-1][0] - self.peaks[-2][0] + clock_diff = self.peaks[-1][1] - self.peaks[-3][1] + pwm_diff = PWM_MAX - 0 + Ku = 4. * (2. * pwm_diff) / (abs(temp_diff) * math.pi) + Tu = clock_diff / self.heater.printer.mcu.get_mcu_freq() + + Kp = 0.6 * Ku + Ti = 0.5 * Tu + Td = 0.125 * Tu + Ki = Kp / Ti + Kd = Kp * Td + logging.info("Autotune: raw=%f/%d/%d Ku=%f Tu=%f Kp=%f Ki=%f Kd=%f" % ( + temp_diff, clock_diff, pwm_diff, Ku, Tu, Kp, Ki, Kd)) + def check_busy(self, eventtime): + if self.heating or len(self.peaks) < 12: + return True + self.heater.control = self.old_control + return False + + +###################################################################### +# Tuning information test +###################################################################### + +class ControlBumpTest: + def __init__(self, heater, old_control, target_temp): + self.heater = heater + self.old_control = old_control + self.target_temp = target_temp + self.temp_samples = {} + self.pwm_samples = {} + self.state = 0 + def set_pwm(self, read_clock, value): + self.pwm_samples[read_clock + 2*self.heater.report_clock] = value + self.heater.set_pwm(read_clock, value) + def adc_callback(self, read_clock, temp): + self.temp_samples[read_clock] = temp + if not self.state: + self.set_pwm(read_clock, 0) + if len(self.temp_samples) >= 20: + self.state += 1 + elif self.state == 1: + if temp < self.target_temp: + self.set_pwm(read_clock, PWM_MAX) + return + self.set_pwm(read_clock, 0) + self.state += 1 + elif self.state == 2: + self.set_pwm(read_clock, 0) + if temp <= (self.target_temp + AMBIENT_TEMP) / 2.: + self.dump_stats() + self.state += 1 + def dump_stats(self): + out = ["%d %.1f %d" % (clock, temp, self.pwm_samples.get(clock, -1)) + for clock, temp in sorted(self.temp_samples.items())] + f = open("/tmp/heattest.txt", "wb") + f.write('\n'.join(out)) + f.close() + def check_busy(self, eventtime): + if self.state < 3: + return True + self.heater.control = self.old_control + return False diff --git a/klippy/homing.py b/klippy/homing.py new file mode 100644 index 000000000..566cdf95e --- /dev/null +++ b/klippy/homing.py @@ -0,0 +1,82 @@ +# Code for state tracking during homing operations +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +import logging + +class Homing: + def __init__(self, kin, steppers): + self.kin = kin + self.steppers = steppers + + self.states = [] + self.endstops = [] + self.changed_axis = [] + def plan_home(self, axis, precise=False): + s = self.steppers[axis] + state = self.states + self.changed_axis.append(axis) + speed = s.homing_speed + if s.homing_positive_dir: + pos = s.position_endstop + 1.5*(s.position_min - s.position_endstop) + rpos = s.position_endstop - s.homing_retract_dist + r2pos = rpos - s.homing_retract_dist + else: + pos = s.position_endstop + 1.5*(s.position_max - s.position_endstop) + rpos = s.position_endstop + s.homing_retract_dist + r2pos = rpos + s.homing_retract_dist + # Initial homing + state.append((self.do_home, ({axis: pos}, speed))) + state.append((self.do_wait_endstop, ({axis: 1},))) + # Retract + state.append((self.do_move, ({axis: rpos}, speed))) + # Home again + state.append((self.do_home, ({axis: r2pos}, speed/2.0))) + state.append((self.do_wait_endstop, ({axis: 1},))) + def plan_axis_update(self, callback): + self.states.append((callback, (self.changed_axis,))) + def check_busy(self, eventtime): + while self.states: + first = self.states[0] + ret = first[0](*first[1]) + if ret: + return True + self.states.pop(0) + return False + + def do_move(self, axis_pos, speed): + # Issue a move command to axis_pos + newpos = self.kin.get_position() + for axis, pos in axis_pos.items(): + newpos[axis] = pos + self.kin.move(newpos, speed) + return False + def do_home(self, axis_pos, speed): + # Alter kinematics class to think printer is at axis_pos + newpos = self.kin.get_position() + forcepos = list(newpos) + for axis, pos in axis_pos.items(): + newpos[axis] = self.steppers[axis].position_endstop + forcepos[axis] = pos + self.kin.set_position(forcepos) + # Start homing and issue move + print_time = self.kin.get_last_move_time() + for axis in axis_pos: + hz = speed * self.steppers[axis].inv_step_dist + es = self.steppers[axis].enable_endstop_checking(print_time, hz) + self.endstops.append(es) + self.kin.move(newpos, speed) + self.kin.reset_print_time() + for es in self.endstops: + es.home_finalize() + return False + def do_wait_endstop(self, axis_wait): + # Check if axis_wait endstops have triggered + for es in self.endstops: + if es.is_homing(): + return True + # Finished + del self.endstops[:] + return False diff --git a/klippy/klippy.py b/klippy/klippy.py new file mode 100644 index 000000000..143dd7624 --- /dev/null +++ b/klippy/klippy.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python +# Main code for host side printer firmware +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import sys, optparse, ConfigParser, logging, time, threading +import gcode, cartesian, util, mcu, fan, heater, reactor + +class ConfigWrapper: + def __init__(self, printer, section): + self.printer = printer + self.section = section + def get(self, option, default=None): + if not self.printer.fileconfig.has_option(self.section, option): + return default + return self.printer.fileconfig.get(self.section, option) + def getint(self, option, default=None): + if not self.printer.fileconfig.has_option(self.section, option): + return default + return self.printer.fileconfig.getint(self.section, option) + def getfloat(self, option, default=None): + if not self.printer.fileconfig.has_option(self.section, option): + return default + return self.printer.fileconfig.getfloat(self.section, option) + def getboolean(self, option, default=None): + if not self.printer.fileconfig.has_option(self.section, option): + return default + return self.printer.fileconfig.getboolean(self.section, option) + def getsection(self, section): + return ConfigWrapper(self.printer, section) + +class Printer: + def __init__(self, conffile, debuginput=None): + self.fileconfig = ConfigParser.RawConfigParser() + self.fileconfig.read(conffile) + self.reactor = reactor.Reactor() + + self._pconfig = ConfigWrapper(self, 'printer') + ptty = self._pconfig.get('pseudo_tty', '/tmp/printer') + if debuginput is None: + pseudo_tty = util.create_pty(ptty) + else: + pseudo_tty = debuginput.fileno() + + self.gcode = gcode.GCodeParser( + self, pseudo_tty, inputfile=debuginput is not None) + self.mcu = None + self.stat_timer = None + + self.objects = {} + if self.fileconfig.has_section('fan'): + self.objects['fan'] = fan.PrinterFan( + self, ConfigWrapper(self, 'fan')) + if self.fileconfig.has_section('heater_nozzle'): + self.objects['heater_nozzle'] = heater.PrinterHeater( + self, ConfigWrapper(self, 'heater_nozzle')) + if self.fileconfig.has_section('heater_bed'): + self.objects['heater_bed'] = heater.PrinterHeater( + self, ConfigWrapper(self, 'heater_bed')) + self.objects['kinematics'] = cartesian.CartKinematics( + self, self._pconfig) + + def stats(self, eventtime): + out = [] + out.append(self.gcode.stats(eventtime)) + out.append(self.objects['kinematics'].stats(eventtime)) + out.append(self.mcu.stats(eventtime)) + logging.info("Stats %.0f: %s" % (eventtime, ' '.join(out))) + return eventtime + 1. + def build_config(self): + for oname in sorted(self.objects.keys()): + self.objects[oname].build_config() + self.gcode.build_config() + self.mcu.build_config() + def connect(self): + self.mcu = mcu.MCU(self, ConfigWrapper(self, 'mcu')) + self.mcu.connect() + self.build_config() + self.stats_timer = self.reactor.register_timer( + self.stats, self.reactor.NOW) + def connect_debug(self, debugoutput): + self.mcu = mcu.DummyMCU(debugoutput) + self.mcu.connect() + self.build_config() + def connect_file(self, output, dictionary): + self.mcu = mcu.MCU(self, ConfigWrapper(self, 'mcu')) + self.mcu.connect_file(output, dictionary) + self.build_config() + def run(self): + self.gcode.run() + # If gcode exits, then exit the MCU + self.stats(time.time()) + self.mcu.disconnect() + self.stats(time.time()) + def shutdown(self): + self.gcode.shutdown() + + +###################################################################### +# Startup +###################################################################### + +def read_dictionary(filename): + dfile = open(filename, 'rb') + dictionary = dfile.read() + dfile.close() + return dictionary + +def store_dictionary(filename, printer): + f = open(filename, 'wb') + f.write(printer.mcu.serial.msgparser.raw_identify_data) + f.close() + +def main(): + usage = "%prog [options] <config file>" + opts = optparse.OptionParser(usage) + opts.add_option("-o", "--debugoutput", dest="outputfile", + help="write output to file instead of to serial port") + opts.add_option("-i", "--debuginput", dest="inputfile", + help="read commands from file instead of from tty port") + opts.add_option("-l", "--logfile", dest="logfile", + help="write log to file instead of stderr") + opts.add_option("-v", action="store_true", dest="verbose", + help="enable debug messages") + opts.add_option("-d", dest="read_dictionary", + help="file to read for mcu protocol dictionary") + opts.add_option("-D", dest="write_dictionary", + help="file to write mcu protocol dictionary") + options, args = opts.parse_args() + if len(args) != 1: + opts.error("Incorrect number of arguments") + conffile = args[0] + + debuginput = debugoutput = None + + debuglevel = logging.INFO + if options.verbose: + debuglevel = logging.DEBUG + if options.inputfile: + debuginput = open(options.inputfile, 'rb') + if options.outputfile: + debugoutput = open(options.outputfile, 'wb') + if options.logfile: + logoutput = open(options.logfile, 'wb') + logging.basicConfig(stream=logoutput, level=debuglevel) + else: + logging.basicConfig(level=debuglevel) + logging.info("Starting Klippy...") + + # Start firmware + printer = Printer(conffile, debuginput=debuginput) + if debugoutput: + proto_dict = read_dictionary(options.read_dictionary) + printer.connect_file(debugoutput, proto_dict) + else: + printer.connect() + if options.write_dictionary: + store_dictionary(options.write_dictionary, printer) + printer.run() + +if __name__ == '__main__': + main() diff --git a/klippy/list.h b/klippy/list.h new file mode 100644 index 000000000..317a109cf --- /dev/null +++ b/klippy/list.h @@ -0,0 +1,108 @@ +#ifndef __LIST_H +#define __LIST_H + +#define container_of(ptr, type, member) ({ \ + const typeof( ((type *)0)->member ) *__mptr = (ptr); \ + (type *)( (char *)__mptr - offsetof(type,member) );}) + + +/**************************************************************** + * list - Double linked lists + ****************************************************************/ + +struct list_node { + struct list_node *next, *prev; +}; + +struct list_head { + struct list_node root; +}; + +static inline void +list_init(struct list_head *h) +{ + h->root.prev = h->root.next = &h->root; +} + +static inline int +list_empty(const struct list_head *h) +{ + return h->root.next == &h->root; +} + +static inline void +list_del(struct list_node *n) +{ + struct list_node *prev = n->prev; + struct list_node *next = n->next; + next->prev = prev; + prev->next = next; +} + +static inline void +__list_add(struct list_node *n, struct list_node *prev, struct list_node *next) +{ + next->prev = n; + n->next = next; + n->prev = prev; + prev->next = n; +} + +static inline void +list_add_after(struct list_node *n, struct list_node *prev) +{ + __list_add(n, prev, prev->next); +} + +static inline void +list_add_before(struct list_node *n, struct list_node *next) +{ + __list_add(n, next->prev, next); +} + +static inline void +list_add_head(struct list_node *n, struct list_head *h) +{ + list_add_after(n, &h->root); +} + +static inline void +list_add_tail(struct list_node *n, struct list_head *h) +{ + list_add_before(n, &h->root); +} + +static inline void +list_join_tail(struct list_head *add, struct list_head *h) +{ + if (!list_empty(add)) { + struct list_node *prev = h->root.prev; + struct list_node *next = &h->root; + struct list_node *first = add->root.next; + struct list_node *last = add->root.prev; + first->prev = prev; + prev->next = first; + last->next = next; + next->prev = last; + } +} + +#define list_next_entry(pos, member) \ + container_of((pos)->member.next, typeof(*pos), member) + +#define list_first_entry(head, type, member) \ + container_of((head)->root.next, type, member) + +#define list_for_each_entry(pos, head, member) \ + for (pos = list_first_entry((head), typeof(*pos), member) \ + ; &pos->member != &(head)->root \ + ; pos = list_next_entry(pos, member)) + +#define list_for_each_entry_safe(pos, n, head, member) \ + for (pos = list_first_entry((head), typeof(*pos), member) \ + , n = list_next_entry(pos, member) \ + ; &pos->member != &(head)->root \ + ; pos = n, n = list_next_entry(n, member)) + + +#endif // list.h diff --git a/klippy/lookahead.py b/klippy/lookahead.py new file mode 100644 index 000000000..9e377463e --- /dev/null +++ b/klippy/lookahead.py @@ -0,0 +1,50 @@ +# Move queue look-ahead +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +class MoveQueue: + def __init__(self, dummy_move): + self.dummy_move = dummy_move + self.queue = [] + self.prev_junction_max = 0. + self.junction_flush = 0. + def prev_move(self): + if self.queue: + return self.queue[-1] + return self.dummy_move + def flush(self, lazy=False): + next_junction_max = 0. + can_flush = not lazy + flush_count = len(self.queue) + junction_end = [None] * flush_count + for i in range(len(self.queue)-1, -1, -1): + move = self.queue[i] + junction_end[i] = next_junction_max + if not can_flush: + flush_count -= 1 + next_junction_max = next_junction_max + move.junction_delta + if next_junction_max >= move.junction_start_max: + next_junction_max = move.junction_start_max + can_flush = True + prev_junction_max = self.prev_junction_max + for i in range(flush_count): + move = self.queue[i] + next_junction_max = min(prev_junction_max + move.junction_delta + , junction_end[i]) + move.process(prev_junction_max, next_junction_max) + prev_junction_max = next_junction_max + del self.queue[:flush_count] + self.prev_junction_max = prev_junction_max + self.junction_flush = 0. + if self.queue: + self.junction_flush = self.queue[-1].junction_max + def add_move(self, move): + self.queue.append(move) + if len(self.queue) == 1: + self.junction_flush = move.junction_max + return + self.junction_flush -= move.junction_delta + if self.junction_flush <= 0.: + self.flush(lazy=True) diff --git a/klippy/mcu.py b/klippy/mcu.py new file mode 100644 index 000000000..728ec91d0 --- /dev/null +++ b/klippy/mcu.py @@ -0,0 +1,510 @@ +# Multi-processor safe interface to micro-controller +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import sys, zlib, logging, time, math +import serialhdl, pins, chelper + +def parse_pin_extras(pin, can_pullup=False): + pullup = invert = 0 + if can_pullup and pin.startswith('^'): + pullup = invert = 1 + pin = pin[1:].strip() + if pin.startswith('!'): + invert = invert ^ 1 + pin = pin[1:].strip() + return pin, pullup, invert + +class MCU_stepper: + def __init__(self, mcu, step_pin, dir_pin, min_stop_interval, max_error): + self._mcu = mcu + self._oid = mcu.create_oid() + step_pin, pullup, invert_step = parse_pin_extras(step_pin) + dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin) + self._sdir = -1 + self._last_move_clock = -2**29 + mcu.add_config_cmd( + "config_stepper oid=%d step_pin=%s dir_pin=%s" + " min_stop_interval=%d invert_step=%d" % ( + self._oid, step_pin, dir_pin, min_stop_interval, invert_step)) + mcu.register_stepper(self) + self._step_cmd = mcu.lookup_command( + "queue_step oid=%c interval=%u count=%hu add=%hi") + self._dir_cmd = mcu.lookup_command( + "set_next_step_dir oid=%c dir=%c") + self._reset_cmd = mcu.lookup_command( + "reset_step_clock oid=%c clock=%u") + ffi_main, self.ffi_lib = chelper.get_ffi() + self._stepqueue = self.ffi_lib.stepcompress_alloc( + max_error, self._step_cmd.msgid, self._oid) + def get_oid(self): + return self._oid + def note_stepper_stop(self): + self._sdir = -1 + self._last_move_clock = -2**29 + def reset_step_clock(self, clock): + self.ffi_lib.stepcompress_reset(self._stepqueue, clock) + data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff) + self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data)) + def set_next_step_dir(self, sdir, clock): + if clock - self._last_move_clock >= 2**29: + self.reset_step_clock(clock) + self._last_move_clock = clock + if self._sdir == sdir: + return + self._sdir = sdir + data = (self._dir_cmd.msgid, self._oid, sdir ^ self._invert_dir) + self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data)) + def step(self, steptime): + self.ffi_lib.stepcompress_push(self._stepqueue, steptime) + def step_sqrt(self, steps, step_offset, clock_offset, sqrt_offset, factor): + return self.ffi_lib.stepcompress_push_sqrt( + self._stepqueue, steps, step_offset, clock_offset + , sqrt_offset, factor) + def step_factor(self, steps, step_offset, clock_offset, factor): + return self.ffi_lib.stepcompress_push_factor( + self._stepqueue, steps, step_offset, clock_offset, factor) + def get_errors(self): + return self.ffi_lib.stepcompress_get_errors(self._stepqueue) + def get_print_clock(self, print_time): + return self._mcu.get_print_clock(print_time) + +class MCU_endstop: + RETRY_QUERY = 1.000 + def __init__(self, mcu, pin, stepper): + self._mcu = mcu + self._oid = mcu.create_oid() + self._stepper = stepper + stepper_oid = stepper.get_oid() + pin, pullup, self._invert = parse_pin_extras(pin, can_pullup=True) + self._cmd_queue = mcu.alloc_command_queue() + mcu.add_config_cmd( + "config_end_stop oid=%d pin=%s pull_up=%d stepper_oid=%d" % ( + self._oid, pin, pullup, stepper_oid)) + self._home_cmd = mcu.lookup_command( + "end_stop_home oid=%c clock=%u rest_ticks=%u pin_value=%c") + mcu.register_msg(self._handle_end_stop_state, "end_stop_state" + , self._oid) + self._query_cmd = mcu.lookup_command("end_stop_query oid=%c") + self._homing = False + self._next_query_clock = 0 + mcu_freq = self._mcu.get_mcu_freq() + self._retry_query_ticks = mcu_freq * self.RETRY_QUERY + def home(self, clock, rest_ticks): + self._homing = True + self._next_query_clock = clock + self._retry_query_ticks + msg = self._home_cmd.encode( + self._oid, clock, rest_ticks, 1 ^ self._invert) + self._mcu.send(msg, reqclock=clock, cq=self._cmd_queue) + def home_finalize(self): + # XXX - this flushes the serial port of messages ready to be + # sent, but doesn't flush messages if they had an unmet minclock + self._mcu.serial.send_flush() + self._stepper.note_stepper_stop() + def _handle_end_stop_state(self, params): + logging.debug("end_stop_state %s" % (params,)) + self._homing = params['homing'] != 0 + def is_homing(self): + if not self._homing: + return self._homing + if self._mcu.output_file_mode: + return False + last_clock = self._mcu.get_last_clock() + if last_clock >= self._next_query_clock: + self._next_query_clock = last_clock + self._retry_query_ticks + msg = self._query_cmd.encode(self._oid) + self._mcu.send(msg, cq=self._cmd_queue) + return self._homing + def get_print_clock(self, print_time): + return self._mcu.get_print_clock(print_time) + +class MCU_digital_out: + def __init__(self, mcu, pin, max_duration): + self._mcu = mcu + self._oid = mcu.create_oid() + pin, pullup, self._invert = parse_pin_extras(pin) + self._last_clock = 0 + self._last_value = None + self._cmd_queue = mcu.alloc_command_queue() + mcu.add_config_cmd( + "config_digital_out oid=%d pin=%s default_value=%d" + " max_duration=%d" % (self._oid, pin, self._invert, max_duration)) + self._set_cmd = mcu.lookup_command( + "schedule_digital_out oid=%c clock=%u value=%c") + def set_digital(self, clock, value): + msg = self._set_cmd.encode(self._oid, clock, value ^ self._invert) + self._mcu.send(msg, minclock=self._last_clock, reqclock=clock + , cq=self._cmd_queue) + self._last_clock = clock + self._last_value = value + def get_last_setting(self): + return self._last_value + def set_pwm(self, clock, value): + dval = 0 + if value > 127: + dval = 1 + self.set_digital(clock, dval) + def get_print_clock(self, print_time): + return self._mcu.get_print_clock(print_time) + +class MCU_pwm: + def __init__(self, mcu, pin, cycle_ticks, max_duration, hard_pwm=True): + self._mcu = mcu + self._oid = mcu.create_oid() + self._last_clock = 0 + self._cmd_queue = mcu.alloc_command_queue() + if hard_pwm: + mcu.add_config_cmd( + "config_pwm_out oid=%d pin=%s cycle_ticks=%d default_value=0" + " max_duration=%d" % (self._oid, pin, cycle_ticks, max_duration)) + self._set_cmd = mcu.lookup_command( + "schedule_pwm_out oid=%c clock=%u value=%c") + else: + mcu.add_config_cmd( + "config_soft_pwm_out oid=%d pin=%s cycle_ticks=%d" + " default_value=0 max_duration=%d" % ( + self._oid, pin, cycle_ticks, max_duration)) + self._set_cmd = mcu.lookup_command( + "schedule_soft_pwm_out oid=%c clock=%u value=%c") + def set_pwm(self, clock, value): + msg = self._set_cmd.encode(self._oid, clock, value) + self._mcu.send(msg, minclock=self._last_clock, reqclock=clock + , cq=self._cmd_queue) + self._last_clock = clock + def get_print_clock(self, print_time): + return self._mcu.get_print_clock(print_time) + +class MCU_adc: + ADC_MAX = 1024 # 10bit adc + def __init__(self, mcu, pin): + self._mcu = mcu + self._oid = mcu.create_oid() + self._min_sample = 0 + self._max_sample = 0xffff + self._sample_ticks = 0 + self._sample_count = 1 + self._report_clock = 0 + self._last_value = 0 + self._last_read_clock = 0 + self._callback = None + self._max_adc_inv = 0. + self._cmd_queue = mcu.alloc_command_queue() + mcu.add_config_cmd("config_analog_in oid=%d pin=%s" % (self._oid, pin)) + mcu.register_msg(self._handle_analog_in_state, "analog_in_state" + , self._oid) + self._query_cmd = mcu.lookup_command( + "query_analog_in oid=%c clock=%u sample_ticks=%u sample_count=%c" + " rest_ticks=%u min_value=%hu max_value=%hu") + def set_minmax(self, sample_ticks, sample_count, minval=None, maxval=None): + if minval is None: + minval = 0 + if maxval is None: + maxval = 0xffff + self._sample_ticks = sample_ticks + self._sample_count = sample_count + max_adc = sample_count * self.ADC_MAX + self._min_sample = int(minval * max_adc) + self._max_sample = min(0xffff, int(math.ceil(maxval * max_adc))) + self._max_adc_inv = 1.0 / max_adc + def query_analog_in(self, report_clock): + self._report_clock = report_clock + mcu_freq = self._mcu.get_mcu_freq() + cur_clock = self._mcu.get_last_clock() + clock = cur_clock + int(mcu_freq * (1.0 + self._oid * 0.01)) # XXX + msg = self._query_cmd.encode( + self._oid, clock, self._sample_ticks, self._sample_count + , report_clock, self._min_sample, self._max_sample) + self._mcu.send(msg, reqclock=clock, cq=self._cmd_queue) + def _handle_analog_in_state(self, params): + self._last_value = params['value'] * self._max_adc_inv + next_clock = self._mcu.serial.translate_clock(params['next_clock']) + self._last_read_clock = next_clock - self._report_clock + if self._callback is not None: + self._callback(self._last_read_clock, self._last_value) + def set_adc_callback(self, cb): + self._callback = cb + def get_print_clock(self, print_time): + return self._mcu.get_print_clock(print_time) + +class MCU: + def __init__(self, printer, config): + self._printer = printer + self._config = config + # Serial port + baud = config.getint('baud', 115200) + serialport = config.get('serial', '/dev/ttyS0') + self.serial = serialhdl.SerialReader(printer.reactor, serialport, baud) + self.is_shutdown = False + self.output_file_mode = False + # Config building + self._num_oids = 0 + self._config_cmds = [] + self._config_crc = None + # Move command queuing + ffi_main, self.ffi_lib = chelper.get_ffi() + self._steppers = [] + self._steppersync = None + # Print time to clock epoch calculations + self._print_start_clock = 0. + self._clock_freq = 0. + # Stats + self._mcu_tick_avg = 0. + self._mcu_tick_stddev = 0. + def handle_mcu_stats(self, params): + logging.debug("mcu stats: %s" % (params,)) + count = params['count'] + tick_sum = params['sum'] + c = 1.0 / (count * self._clock_freq) + self._mcu_tick_avg = tick_sum * c + tick_sumsq = params['sumsq'] + tick_sumavgsq = ((tick_sum // (256*count)) * count)**2 + self._mcu_tick_stddev = c * 256. * math.sqrt( + count * tick_sumsq - tick_sumavgsq) + def handle_shutdown(self, params): + if self.is_shutdown: + return + self.is_shutdown = True + logging.info("%s: %s" % (params['#name'], params['#msg'])) + self.serial.dump_debug() + self._printer.shutdown() + # Connection phase + def _init_steppersync(self, count): + stepqueues = tuple(s._stepqueue for s in self._steppers) + self._steppersync = self.ffi_lib.steppersync_alloc( + self.serial.serialqueue, stepqueues, len(stepqueues), count) + def connect(self): + def handle_serial_state(params): + if params['#state'] == 'connected': + self._printer.reactor.end() + self.serial.register_callback(handle_serial_state, '#state') + self.serial.connect() + self._printer.reactor.run() + self.serial.unregister_callback('#state') + logging.info("serial connected") + self._clock_freq = float(self.serial.msgparser.config['CLOCK_FREQ']) + self.register_msg(self.handle_shutdown, 'shutdown') + self.register_msg(self.handle_shutdown, 'is_shutdown') + self.register_msg(self.handle_mcu_stats, 'stats') + def connect_file(self, debugoutput, dictionary, pace=False): + self.output_file_mode = True + self.serial.connect_file(debugoutput, dictionary) + self._clock_freq = float(self.serial.msgparser.config['CLOCK_FREQ']) + def dummy_build_config(): + self._init_steppersync(500) + self.build_config = dummy_build_config + if not pace: + def dummy_set_print_start_time(eventtime): + pass + def dummy_get_print_buffer_time(eventtime, last_move_end): + return 0.250 + self.set_print_start_time = dummy_set_print_start_time + self.get_print_buffer_time = dummy_get_print_buffer_time + def disconnect(self): + self.serial.disconnect() + def stats(self, eventtime): + stats = self.serial.stats(eventtime) + stats += " mcu_task_avg=%.06f mcu_task_stddev=%.06f" % ( + self._mcu_tick_avg, self._mcu_tick_stddev) + err = 0 + for s in self._steppers: + err += s.get_errors() + if err: + stats += " step_errors=%d" % (err,) + return stats + # Configuration phase + def _add_custom(self): + data = self._config.get('custom', '') + for line in data.split('\n'): + line = line.strip() + cpos = line.find('#') + if cpos >= 0: + line = line[:cpos].strip() + if not line: + continue + self.add_config_cmd(line) + def build_config(self): + # Build config commands + self._add_custom() + self._config_cmds.insert(0, "allocate_oids count=%d" % ( + self._num_oids,)) + + # Resolve pin names + mcu = self.serial.msgparser.config['MCU'] + pin_map = self._config.get('pin_map') + if pin_map is None: + pnames = pins.mcu_to_pins(mcu) + else: + pnames = pins.map_pins(pin_map, mcu) + self._config_cmds = [pins.update_command(c, pnames) + for c in self._config_cmds] + + # Calculate config CRC + self._config_crc = zlib.crc32('\n'.join(self._config_cmds)) & 0xffffffff + self.add_config_cmd("finalize_config crc=%d" % (self._config_crc,)) + + self._send_config() + def _send_config(self): + msg = self.create_command("get_config") + config_params = {} + sent_config = False + def handle_get_config(params): + config_params.update(params) + done = not sent_config or params['is_config'] + if done: + self._printer.reactor.end() + return done + while 1: + self.serial.send_with_response(msg, handle_get_config, 'config') + self._printer.reactor.run() + if not config_params['is_config']: + # Send config commands + for c in self._config_cmds: + self.send(self.create_command(c)) + config_params.clear() + sent_config = True + continue + if self._config_crc != config_params['crc']: + logging.error("Printer CRC does not match config") + sys.exit(1) + break + logging.info("Configured") + self._init_steppersync(config_params['move_count']) + # Config creation helpers + def create_oid(self): + oid = self._num_oids + self._num_oids += 1 + return oid + def add_config_cmd(self, cmd): + self._config_cmds.append(cmd) + def register_msg(self, cb, msg, oid=None): + self.serial.register_callback(cb, msg, oid) + def register_stepper(self, stepper): + self._steppers.append(stepper) + def alloc_command_queue(self): + return self.serial.alloc_command_queue() + def lookup_command(self, msgformat): + return self.serial.msgparser.lookup_command(msgformat) + def create_command(self, msg): + return self.serial.msgparser.create_command(msg) + # Wrappers for mcu object creation + def create_stepper(self, step_pin, dir_pin, min_stop_interval, max_error): + return MCU_stepper(self, step_pin, dir_pin, min_stop_interval, max_error) + def create_endstop(self, pin, stepper): + return MCU_endstop(self, pin, stepper) + def create_digital_out(self, pin, max_duration=2.): + max_duration = int(max_duration * self._clock_freq) + return MCU_digital_out(self, pin, max_duration) + def create_pwm(self, pin, hard_cycle_ticks, max_duration=2.): + max_duration = int(max_duration * self._clock_freq) + if hard_cycle_ticks: + return MCU_pwm(self, pin, hard_cycle_ticks, max_duration) + if hard_cycle_ticks < 0: + return MCU_digital_out(self, pin, max_duration) + cycle_ticks = int(self._clock_freq / 10.) + return MCU_pwm(self, pin, cycle_ticks, max_duration, hard_pwm=False) + def create_adc(self, pin): + return MCU_adc(self, pin) + # Clock syncing + def set_print_start_time(self, eventtime): + self._print_start_clock = self.serial.get_clock(eventtime) + def get_print_buffer_time(self, eventtime, last_move_end): + clock_diff = self.serial.get_clock(eventtime) - self._print_start_clock + return last_move_end - (float(clock_diff) / self._clock_freq) + def get_print_clock(self, print_time): + return print_time * self._clock_freq + self._print_start_clock + def get_mcu_freq(self): + return self._clock_freq + def get_last_clock(self): + return self.serial.get_last_clock() + # Move command queuing + def send(self, cmd, minclock=0, reqclock=0, cq=None): + self.serial.send(cmd, minclock, reqclock, cq=cq) + def flush_moves(self, print_time): + move_clock = int(self.get_print_clock(print_time)) + self.ffi_lib.steppersync_flush(self._steppersync, move_clock) + + +###################################################################### +# MCU Unit testing +###################################################################### + +class Dummy_MCU_stepper: + def __init__(self, mcu, stepid): + self._mcu = mcu + self._stepid = stepid + self._sdir = None + def queue_step(self, interval, count, add, clock): + dirstr = countstr = addstr = "" + if self._sdir is not None: + dirstr = "D%d" % (self._sdir+1,) + self._sdir = None + if count != 1: + countstr = "C%d" % (count,) + if add: + addstr = "A%d" % (add,) + self._mcu.outfile.write("G5S%d%s%s%sT%d\n" % ( + self._stepid, dirstr, countstr, addstr, interval)) + def set_next_step_dir(self, dir): + self._sdir = dir + def reset_step_clock(self, clock): + self._mcu.outfile.write("G6S%dT%d\n" % (self._stepid, clock)) + def get_print_clock(self, print_time): + return self._mcu.get_print_clock(print_time) + +class Dummy_MCU_obj: + def __init__(self, mcu): + self._mcu = mcu + def home(self, clock, rest_ticks): + pass + def is_homing(self): + return False + def home_finalize(self): + pass + def set_pwm(self, print_time, value): + pass + def set_minmax(self, sample_ticks, sample_count, minval=None, maxval=None): + pass + def query_analog_in(self, report_clock): + pass + def set_adc_callback(self, cb): + pass + def get_print_clock(self, print_time): + return self._mcu.get_print_clock(print_time) + +class DummyMCU: + def __init__(self, outfile): + self.outfile = outfile + self._stepid = -1 + self._print_start_clock = 0. + self._clock_freq = 16000000. + logging.debug('Translated by klippy') + def connect(self): + pass + def disconnect(self): + pass + def stats(self, eventtime): + return "" + def build_config(self): + pass + def create_stepper(self, step_pin, dir_pin, min_stop_interval, max_error): + self._stepid += 1 + return Dummy_MCU_stepper(self, self._stepid) + def create_endstop(self, pin, stepper): + return Dummy_MCU_obj(self) + def create_digital_out(self, pin, max_duration=2.): + return None + def create_pwm(self, pin, hard_cycle_ticks, max_duration=2.): + return Dummy_MCU_obj(self) + def create_adc(self, pin): + return Dummy_MCU_obj(self) + def set_print_start_time(self, eventtime): + pass + def get_print_buffer_time(self, eventtime, last_move_end): + return 0.250 + def get_print_clock(self, print_time): + return print_time * self._clock_freq + self._print_start_clock + def get_mcu_freq(self): + return self._clock_freq + def flush_moves(self, print_time): + pass diff --git a/klippy/msgproto.py b/klippy/msgproto.py new file mode 100644 index 000000000..73c8d2d5f --- /dev/null +++ b/klippy/msgproto.py @@ -0,0 +1,313 @@ +# Protocol definitions for firmware communication +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import json, zlib, logging + +DefaultMessages = { + 0: "identify_response offset=%u data=%.*s", + 1: "identify offset=%u count=%c", +} + +MESSAGE_MIN = 5 +MESSAGE_MAX = 64 +MESSAGE_HEADER_SIZE = 2 +MESSAGE_TRAILER_SIZE = 3 +MESSAGE_POS_LEN = 0 +MESSAGE_POS_SEQ = 1 +MESSAGE_TRAILER_CRC = 3 +MESSAGE_TRAILER_SYNC = 1 +MESSAGE_PAYLOAD_MAX = MESSAGE_MAX - MESSAGE_MIN +MESSAGE_SEQ_MASK = 0x0f +MESSAGE_DEST = 0x10 +MESSAGE_SYNC = '\x7E' + +class error(Exception): + def __init__(self, msg): + self.msg = msg + def __str__(self): + return self.msg + +def crc16_ccitt(buf): + crc = 0xffff + for data in buf: + data = ord(data) + data ^= crc & 0xff + data ^= (data & 0x0f) << 4 + crc = ((data << 8) | (crc >> 8)) ^ (data >> 4) ^ (data << 3) + crc = chr(crc >> 8) + chr(crc & 0xff) + return crc + +class PT_uint32: + is_int = 1 + max_length = 5 + signed = 0 + def encode(self, out, v): + if v >= 0xc000000 or v < -0x4000000: out.append((v>>28) & 0x7f | 0x80) + if v >= 0x180000 or v < -0x80000: out.append((v>>21) & 0x7f | 0x80) + if v >= 0x3000 or v < -0x1000: out.append((v>>14) & 0x7f | 0x80) + if v >= 0x60 or v < -0x20: out.append((v>>7) & 0x7f | 0x80) + out.append(v & 0x7f) + def parse(self, s, pos): + c = s[pos] + pos += 1 + v = c & 0x7f + if (c & 0x60) == 0x60: + v |= -0x20 + while c & 0x80: + c = s[pos] + pos += 1 + v = (v<<7) | (c & 0x7f) + if not self.signed: + v = int(v & 0xffffffff) + return v, pos + +class PT_int32(PT_uint32): + signed = 1 +class PT_uint16(PT_uint32): + max_length = 3 +class PT_int16(PT_int32): + signed = 1 + max_length = 3 +class PT_byte(PT_uint32): + max_length = 2 + +class PT_string: + is_int = 0 + max_length = 64 + def encode(self, out, v): + out.append(len(v)) + out.extend(bytearray(v)) + def parse(self, s, pos): + l = s[pos] + return str(bytearray(s[pos+1:pos+l+1])), pos+l+1 +class PT_progmem_buffer(PT_string): + pass +class PT_buffer(PT_string): + pass + +MessageTypes = { + '%u': PT_uint32(), '%i': PT_int32(), + '%hu': PT_uint16(), '%hi': PT_int16(), + '%c': PT_byte(), + '%s': PT_string(), '%.*s': PT_progmem_buffer(), '%*s': PT_buffer(), +} + +# Update the message format to be compatible with python's % operator +def convert_msg_format(msgformat): + mf = msgformat.replace('%c', '%u') + mf = mf.replace('%.*s', '%s').replace('%*s', '%s') + return mf + +class MessageFormat: + def __init__(self, msgid, msgformat): + self.msgid = msgid + self.msgformat = msgformat + self.debugformat = convert_msg_format(msgformat) + parts = msgformat.split() + self.name = parts[0] + argparts = [arg.split('=') for arg in parts[1:]] + self.param_types = [MessageTypes[fmt] for name, fmt in argparts] + self.param_names = [name for name, fmt in argparts] + self.name_to_type = dict(zip(self.param_names, self.param_types)) + def encode(self, *params): + out = [] + out.append(self.msgid) + for i, t in enumerate(self.param_types): + t.encode(out, params[i]) + return out + def encode_by_name(self, **params): + out = [] + out.append(self.msgid) + for name, t in zip(self.param_names, self.param_types): + t.encode(out, params[name]) + return out + def parse(self, s, pos): + pos += 1 + out = {} + for t, name in zip(self.param_types, self.param_names): + v, pos = t.parse(s, pos) + out[name] = v + return out, pos + def dump(self, s, pos): + pos += 1 + out = [] + for t in self.param_types: + v, pos = t.parse(s, pos) + if not t.is_int: + v = repr(v) + out.append(v) + outmsg = self.debugformat % tuple(out) + return outmsg, pos + +class OutputFormat: + name = '#output' + def __init__(self, msgid, msgformat): + self.msgid = msgid + self.msgformat = msgformat + self.debugformat = convert_msg_format(msgformat) + self.param_types = [] + args = msgformat + while 1: + pos = args.find('%') + if pos < 0: + break + if pos+1 >= len(args) or args[pos+1] != '%': + for i in range(4): + t = MessageTypes.get(args[pos:pos+1+i]) + if t is not None: + self.param_types.append(t) + break + else: + raise error("Invalid output format for '%s'" % (msg,)) + args = args[pos+1:] + def parse(self, s, pos): + pos += 1 + out = [] + for t in self.param_types: + v, pos = t.parse(s, pos) + out.append(v) + outmsg = self.debugformat % tuple(out) + return {'#msg': outmsg}, pos + def dump(self, s, pos): + pos += 1 + out = [] + for t in self.param_types: + v, pos = t.parse(s, pos) + out.append(v) + outmsg = self.debugformat % tuple(out) + return outmsg, pos + +class UnknownFormat: + name = '#unknown' + def parse(self, s, pos): + msgid = s[pos] + msg = str(bytearray(s)) + return {'#msgid': msgid, '#msg': msg}, len(s)-MESSAGE_TRAILER_SIZE + +class MessageParser: + def __init__(self): + self.unknown = UnknownFormat() + self.messages_by_id = {} + self.messages_by_name = {} + self.static_strings = [] + self.config = {} + self.version = "" + self.raw_identify_data = "" + self._init_messages(DefaultMessages, DefaultMessages.keys()) + def check_packet(self, s): + if len(s) < MESSAGE_MIN: + return 0 + msglen = ord(s[MESSAGE_POS_LEN]) + if msglen < MESSAGE_MIN or msglen > MESSAGE_MAX: + return -1 + msgseq = ord(s[MESSAGE_POS_SEQ]) + if (msgseq & ~MESSAGE_SEQ_MASK) != MESSAGE_DEST: + return -1 + if len(s) < msglen: + # Need more data + return 0 + if s[msglen-MESSAGE_TRAILER_SYNC] != MESSAGE_SYNC: + return -1 + msgcrc = s[msglen-MESSAGE_TRAILER_CRC:msglen-MESSAGE_TRAILER_CRC+2] + crc = crc16_ccitt(s[:msglen-MESSAGE_TRAILER_SIZE]) + if crc != msgcrc: + #logging.debug("got crc %s vs %s" % (repr(crc), repr(msgcrc))) + return -1 + return msglen + def dump(self, s): + msgseq = s[MESSAGE_POS_SEQ] + out = ["seq: %02x" % (msgseq,)] + pos = MESSAGE_HEADER_SIZE + while 1: + msgid = s[pos] + mid = self.messages_by_id.get(msgid, self.unknown) + params, pos = mid.dump(s, pos) + out.append("%s" % (params,)) + if pos >= len(s)-MESSAGE_TRAILER_SIZE: + break + return out + def parse(self, s): + msgid = s[MESSAGE_HEADER_SIZE] + mid = self.messages_by_id.get(msgid, self.unknown) + params, pos = mid.parse(s, MESSAGE_HEADER_SIZE) + if pos != len(s)-MESSAGE_TRAILER_SIZE: + raise error("Extra data at end of message") + params['#name'] = mid.name + static_string_id = params.get('static_string_id') + if static_string_id is not None: + params['#msg'] = self.static_strings[static_string_id] + return params + def encode(self, seq, cmd): + msglen = MESSAGE_MIN + len(cmd) + seq = (seq & MESSAGE_SEQ_MASK) | MESSAGE_DEST + out = [chr(msglen), chr(seq), cmd] + out.append(crc16_ccitt(''.join(out))) + out.append(MESSAGE_SYNC) + return ''.join(out) + def _parse_buffer(self, value): + tval = int(value, 16) + out = [] + for i in range(len(value)/2): + out.append(tval & 0xff) + tval >>= 8 + out.reverse() + return ''.join([chr(i) for i in out]) + def lookup_command(self, msgformat): + parts = msgformat.strip().split() + msgname = parts[0] + mp = self.messages_by_name.get(msgname) + if mp is None: + raise error("Unknown command: %s" % (msgname,)) + if msgformat != mp.msgformat: + raise error("Command format mismatch: %s vs %s" % ( + msgformat, mp.msgformat)) + return mp + def create_command(self, msg): + parts = msg.strip().split() + if not parts: + return "" + msgname = parts[0] + mp = self.messages_by_name.get(msgname) + if mp is None: + raise error("Unknown command: %s" % (msgname,)) + try: + argparts = dict(arg.split('=', 1) for arg in parts[1:]) + for name, value in argparts.items(): + t = mp.name_to_type[name] + if t.is_int: + tval = int(value, 0) + else: + tval = self._parse_buffer(value) + argparts[name] = tval + except: + #traceback.print_exc() + raise error("Unable to extract params from: %s" % (msgname,)) + try: + cmd = mp.encode_by_name(**argparts) + except: + #traceback.print_exc() + raise error("Unable to encode: %s" % (msgname,)) + return cmd + def _init_messages(self, messages, parsers): + for msgid, msgformat in messages.items(): + msgid = int(msgid) + if msgid not in parsers: + self.messages_by_id[msgid] = OutputFormat(msgid, msgformat) + continue + msg = MessageFormat(msgid, msgformat) + self.messages_by_id[msgid] = msg + self.messages_by_name[msg.name] = msg + def process_identify(self, data, decompress=True): + if decompress: + data = zlib.decompress(data) + self.raw_identify_data = data + data = json.loads(data) + messages = data.get('messages') + commands = data.get('commands') + responses = data.get('responses') + self._init_messages(messages, commands+responses) + self.static_strings = data.get('static_strings', []) + self.config.update(data.get('config', {})) + self.version = data.get('version', '') diff --git a/klippy/parsedump.py b/klippy/parsedump.py new file mode 100755 index 000000000..0f6c48b35 --- /dev/null +++ b/klippy/parsedump.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python +# Script to parse a serial port data dump +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import os, sys, logging +import msgproto + +def read_dictionary(filename): + dfile = open(filename, 'rb') + dictionary = dfile.read() + dfile.close() + return dictionary + +def main(): + dict_filename, data_filename = sys.argv[1:] + + dictionary = read_dictionary(dict_filename) + + mp = msgproto.MessageParser() + mp.process_identify(dictionary, decompress=False) + + f = open(data_filename, 'rb') + fd = f.fileno() + data = "" + while 1: + newdata = os.read(fd, 4096) + if not newdata: + break + data += newdata + while 1: + l = mp.check_packet(data) + if l == 0: + break + if l < 0: + logging.error("Invalid data") + data = data[-l:] + continue + msgs = mp.dump(bytearray(data[:l])) + sys.stdout.write('\n'.join(msgs[1:]) + '\n') + data = data[l:] + +if __name__ == '__main__': + main() diff --git a/klippy/pins.py b/klippy/pins.py new file mode 100644 index 000000000..fad41643a --- /dev/null +++ b/klippy/pins.py @@ -0,0 +1,88 @@ +# Pin name to pin number definitions +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +import re + +def avr_pins(port_count): + pins = {} + for port in range(port_count): + portchr = chr(65 + port) + if portchr == 'I': + continue + for portbit in range(8): + pins['P%c%d' % (portchr, portbit)] = port * 8 + portbit + return pins + +PINS_atmega164 = avr_pins(4) +PINS_atmega1280 = avr_pins(12) + +MCU_PINS = { + "atmega168": PINS_atmega164, "atmega644p": PINS_atmega164, + "atmega1280": PINS_atmega1280, "atmega2560": PINS_atmega1280, +} + +def mcu_to_pins(mcu): + return MCU_PINS.get(mcu, {}) + +re_pin = re.compile(r'(?P<prefix>[ _]pin=)(?P<name>[^ ]*)') +def update_command(cmd, pmap): + def fixup(m): + return m.group('prefix') + str(pmap[m.group('name')]) + return re_pin.sub(fixup, cmd) + + +###################################################################### +# Arduino mappings +###################################################################### + +Arduino_standard = [ + "PD0", "PD1", "PD2", "PD3", "PD4", "PD5", "PD6", "PD7", "PB0", "PB1", + "PB2", "PB3", "PB4", "PB5", "PC0", "PC1", "PC2", "PC3", "PC4", "PC5", +] +Arduino_analog_standard = [ + "PC0", "PC1", "PC2", "PC3", "PC4", "PC5", "PE0", "PE1", +] + +Arduino_mega = [ + "PE0", "PE1", "PE4", "PE5", "PG5", "PE3", "PH3", "PH4", "PH5", "PH6", + "PB4", "PB5", "PB6", "PB7", "PJ1", "PJ0", "PH1", "PH0", "PD3", "PD2", + "PD1", "PD0", "PA0", "PA1", "PA2", "PA3", "PA4", "PA5", "PA6", "PA7", + "PC7", "PC6", "PC5", "PC4", "PC3", "PC2", "PC1", "PC0", "PD7", "PG2", + "PG1", "PG0", "PL7", "PL6", "PL5", "PL4", "PL3", "PL2", "PL1", "PL0", + "PB3", "PB2", "PB1", "PB0", "PF0", "PF1", "PF2", "PF3", "PF4", "PF5", + "PF6", "PF7", "PK0", "PK1", "PK2", "PK3", "PK4", "PK5", "PK6", "PK7", +] +Arduino_analog_mega = [ + "PF0", "PF1", "PF2", "PF3", "PF4", "PF5", + "PF6", "PF7", "PK0", "PK1", "PK2", "PK3", "PK4", "PK5", "PK6", "PK7", +] + +Sanguino = [ + "PB0", "PB1", "PB2", "PB3", "PB4", "PB5", "PB6", "PB7", "PD0", "PD1", + "PD2", "PD3", "PD4", "PD5", "PD6", "PD7", "PC0", "PC1", "PC2", "PC3", + "PC4", "PC5", "PC6", "PC7", "PA0", "PA1", "PA2", "PA3", "PA4", "PA5", + "PA6", "PA7" +] +Sanguino_analog = [ + "PA0", "PA1", "PA2", "PA3", "PA4", "PA5", "PA6", "PA7" +] + +Arduino_from_mcu = { + "atmega168": (Arduino_standard, Arduino_analog_standard), + "atmega644p": (Sanguino, Sanguino_analog), + "atmega1280": (Arduino_mega, Arduino_analog_mega), + "atmega2560": (Arduino_mega, Arduino_analog_mega), +} + +def map_pins(name, mcu): + pins = MCU_PINS.get(mcu, {}) + if name == 'arduino': + dpins, apins = Arduino_from_mcu.get(mcu, []) + for i in range(len(dpins)): + pins['ar' + str(i)] = pins[dpins[i]] + for i in range(len(apins)): + pins['analog%d' % (i,)] = pins[apins[i]] + return pins diff --git a/klippy/reactor.py b/klippy/reactor.py new file mode 100644 index 000000000..e07c64402 --- /dev/null +++ b/klippy/reactor.py @@ -0,0 +1,142 @@ +# File descriptor and timer event helper +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +import select, time, math + +class ReactorTimer: + def __init__(self, callback, waketime): + self.callback = callback + self.waketime = waketime + +class ReactorFileHandler: + def __init__(self, fd, callback): + self.fd = fd + self.callback = callback + def fileno(self): + return self.fd + +class SelectReactor: + NOW = 0. + NEVER = 9999999999999999. + def __init__(self): + self._fds = [] + self._timers = [] + self._next_timer = self.NEVER + self._process = True + # Timers + def _note_time(self, t): + nexttime = t.waketime + if nexttime < self._next_timer: + self._next_timer = nexttime + def update_timer(self, t, nexttime): + t.waketime = nexttime + self._note_time(t) + def register_timer(self, callback, waketime = NEVER): + handler = ReactorTimer(callback, waketime) + timers = list(self._timers) + timers.append(handler) + self._timers = timers + self._note_time(handler) + return handler + def unregister_timer(self, handler): + timers = list(self._timers) + timers.pop(timers.index(handler)) + self._timers = timers + def _check_timers(self, eventtime): + if eventtime < self._next_timer: + return min(1., max(.001, self._next_timer - eventtime)) + self._next_timer = self.NEVER + for t in self._timers: + if eventtime >= t.waketime: + t.waketime = t.callback(eventtime) + self._note_time(t) + if eventtime >= self._next_timer: + return 0. + return min(1., max(.001, self._next_timer - time.time())) + # File descriptors + def register_fd(self, fd, callback): + handler = ReactorFileHandler(fd, callback) + self._fds.append(handler) + return handler + def unregister_fd(self, handler): + self._fds.pop(self._fds.index(handler)) + # Main loop + def run(self): + self._process = True + eventtime = time.time() + while self._process: + timeout = self._check_timers(eventtime) + res = select.select(self._fds, [], [], timeout) + eventtime = time.time() + for fd in res[0]: + fd.callback(eventtime) + def end(self): + self._process = False + +class PollReactor(SelectReactor): + def __init__(self): + SelectReactor.__init__(self) + self._poll = select.poll() + self._fds = {} + # File descriptors + def register_fd(self, fd, callback): + handler = ReactorFileHandler(fd, callback) + fds = self._fds.copy() + fds[fd] = callback + self._fds = fds + self._poll.register(handler, select.POLLIN | select.POLLHUP) + return handler + def unregister_fd(self, handler): + self._poll.unregister(handler) + fds = self._fds.copy() + del fds[handler.fd] + self._fds = fds + # Main loop + def run(self): + self._process = True + eventtime = time.time() + while self._process: + timeout = int(math.ceil(self._check_timers(eventtime) * 1000.)) + res = self._poll.poll(timeout) + eventtime = time.time() + for fd, event in res: + self._fds[fd](eventtime) + +class EPollReactor(SelectReactor): + def __init__(self): + SelectReactor.__init__(self) + self._epoll = select.epoll() + self._fds = {} + # File descriptors + def register_fd(self, fd, callback): + handler = ReactorFileHandler(fd, callback) + fds = self._fds.copy() + fds[fd] = callback + self._fds = fds + self._epoll.register(fd, select.EPOLLIN | select.EPOLLHUP) + return handler + def unregister_fd(self, handler): + self._epoll.unregister(handler.fd) + fds = self._fds.copy() + del fds[handler.fd] + self._fds = fds + # Main loop + def run(self): + self._process = True + eventtime = time.time() + while self._process: + timeout = self._check_timers(eventtime) + res = self._epoll.poll(timeout) + eventtime = time.time() + for fd, event in res: + self._fds[fd](eventtime) + +# Use the poll based reactor if it is available +try: + select.poll + Reactor = PollReactor +except: + Reactor = SelectReactor diff --git a/klippy/serialhdl.py b/klippy/serialhdl.py new file mode 100644 index 000000000..80e29b963 --- /dev/null +++ b/klippy/serialhdl.py @@ -0,0 +1,286 @@ +# Serial port management for firmware communication +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import time, logging, threading +import serial + +import msgproto, chelper + +class SerialReader: + BITS_PER_BYTE = 10 + def __init__(self, reactor, serialport, baud): + self.reactor = reactor + self.serialport = serialport + self.baud = baud + # Serial port + self.ser = None + self.msgparser = msgproto.MessageParser() + # C interface + self.ffi_main, self.ffi_lib = chelper.get_ffi() + self.serialqueue = None + self.default_cmd_queue = self.alloc_command_queue() + self.stats_buf = self.ffi_main.new('char[4096]') + # MCU time/clock tracking + self.last_ack_time = self.last_ack_rtt_time = 0. + self.last_ack_clock = self.last_ack_rtt_clock = 0 + self.est_clock = 0. + # Threading + self.lock = threading.Lock() + self.background_thread = None + # Message handlers + self.status_timer = self.reactor.register_timer( + self._status_event, self.reactor.NOW) + self.status_cmd = None + handlers = { + '#unknown': self.handle_unknown, '#state': self.handle_state, + '#output': self.handle_output, 'status': self.handle_status, + 'shutdown': self.handle_output, 'is_shutdown': self.handle_output + } + self.handlers = dict(((k, None), v) for k, v in handlers.items()) + def _bg_thread(self): + response = self.ffi_main.new('struct pull_queue_message *') + while 1: + self.ffi_lib.serialqueue_pull(self.serialqueue, response) + count = response.len + if count <= 0: + break + params = self.msgparser.parse(response.msg[0:count]) + params['#sent_time'] = response.sent_time + params['#receive_time'] = response.receive_time + with self.lock: + hdl = (params['#name'], params.get('oid')) + hdl = self.handlers.get(hdl, self.handle_default) + try: + hdl(params) + except: + logging.exception("Exception in serial callback") + def connect(self): + logging.info("Starting serial connect") + self.ser = serial.Serial(self.serialport, self.baud, timeout=0) + stk500v2_leave(self.ser) + baud_adjust = float(self.BITS_PER_BYTE) / self.baud + self.serialqueue = self.ffi_lib.serialqueue_alloc( + self.ser.fileno(), baud_adjust, 0) + SerialBootStrap(self) + self.background_thread = threading.Thread(target=self._bg_thread) + self.background_thread.start() + def connect_file(self, debugoutput, dictionary, pace=False): + self.ser = debugoutput + self.msgparser.process_identify(dictionary, decompress=False) + baud_adjust = 0. + est_clock = 1000000000000. + if pace: + baud_adjust = float(self.BITS_PER_BYTE) / self.baud + est_clock = self.msgparser.config['CLOCK_FREQ'] + self.serialqueue = self.ffi_lib.serialqueue_alloc( + self.ser.fileno(), baud_adjust, 1) + self.est_clock = est_clock + self.last_ack_time = time.time() + self.last_ack_clock = 0 + self.ffi_lib.serialqueue_set_clock_est( + self.serialqueue, self.est_clock, self.last_ack_time + , self.last_ack_clock) + def disconnect(self): + self.send_flush() + time.sleep(0.010) + if self.ffi_lib is not None: + self.ffi_lib.serialqueue_exit(self.serialqueue) + if self.background_thread is not None: + self.background_thread.join() + def stats(self, eventtime): + if self.serialqueue is None: + return "" + sqstats = self.ffi_lib.serialqueue_get_stats( + self.serialqueue, self.stats_buf, len(self.stats_buf)) + sqstats = self.ffi_main.string(self.stats_buf) + tstats = " est_clock=%.3f last_ack_time=%.3f last_ack_clock=%d" % ( + self.est_clock, self.last_ack_time, self.last_ack_clock) + return sqstats + tstats + def _status_event(self, eventtime): + if self.status_cmd is None: + return eventtime + 0.1 + self.send(self.status_cmd) + return eventtime + 1.0 + # Serial response callbacks + def register_callback(self, callback, name, oid=None): + with self.lock: + self.handlers[name, oid] = callback + def unregister_callback(self, name, oid=None): + with self.lock: + del self.handlers[name, oid] + # Clock tracking + def get_clock(self, eventtime): + with self.lock: + return int(self.last_ack_clock + + (eventtime - self.last_ack_time) * self.est_clock) + def translate_clock(self, raw_clock): + with self.lock: + last_ack_clock = self.last_ack_clock + clock_diff = (last_ack_clock - raw_clock) & 0xffffffff + if clock_diff & 0x80000000: + return last_ack_clock + 0x100000000 - clock_diff + return last_ack_clock - clock_diff + def get_last_clock(self): + with self.lock: + return self.last_ack_clock + # Command sending + def send(self, cmd, minclock=0, reqclock=0, cq=None): + if cq is None: + cq = self.default_cmd_queue + self.ffi_lib.serialqueue_send( + self.serialqueue, cq, cmd, len(cmd), minclock, reqclock) + def encode_and_send(self, data, minclock, reqclock, cq): + self.ffi_lib.serialqueue_encode_and_send( + self.serialqueue, cq, data, len(data), minclock, reqclock) + def send_with_response(self, cmd, callback, name): + SerialRetryCommand(self, cmd, callback, name) + def send_flush(self): + self.ffi_lib.serialqueue_flush_ready(self.serialqueue) + def alloc_command_queue(self): + return self.ffi_lib.serialqueue_alloc_commandqueue() + # Dumping debug lists + def dump_debug(self): + sdata = self.ffi_main.new('struct pull_queue_message[1024]') + rdata = self.ffi_main.new('struct pull_queue_message[1024]') + scount = self.ffi_lib.serialqueue_extract_old( + self.serialqueue, 1, sdata, len(sdata)) + rcount = self.ffi_lib.serialqueue_extract_old( + self.serialqueue, 0, rdata, len(rdata)) + logging.info("Dumping send queue %d messages" % (scount,)) + for i in range(scount): + msg = sdata[i] + cmds = self.msgparser.dump(msg.msg[0:msg.len]) + logging.info("Sent %d %f %f %d: %s" % ( + i, msg.receive_time, msg.sent_time, msg.len, ', '.join(cmds))) + logging.info("Dumping receive queue %d messages" % (rcount,)) + for i in range(rcount): + msg = rdata[i] + cmds = self.msgparser.dump(msg.msg[0:msg.len]) + logging.info("Receive: %d %f %f %d: %s" % ( + i, msg.receive_time, msg.sent_time, msg.len, ', '.join(cmds))) + # Default message handlers + def handle_status(self, params): + with self.lock: + # Update last_ack_time / last_ack_clock + ack_clock = (self.last_ack_clock & ~0xffffffff) | params['clock'] + if ack_clock < self.last_ack_clock: + ack_clock += 0x100000000 + sent_time = params['#sent_time'] + self.last_ack_time = receive_time = params['#receive_time'] + self.last_ack_clock = ack_clock + # Update est_clock (if applicable) + if receive_time > self.last_ack_rtt_time + 1. and sent_time: + if self.last_ack_rtt_time: + timedelta = receive_time - self.last_ack_rtt_time + clockdelta = ack_clock - self.last_ack_rtt_clock + estclock = clockdelta / timedelta + if estclock > self.est_clock and self.est_clock: + self.est_clock = (self.est_clock * 63. + estclock) / 64. + else: + self.est_clock = estclock + self.last_ack_rtt_time = sent_time + self.last_ack_rtt_clock = ack_clock + self.ffi_lib.serialqueue_set_clock_est( + self.serialqueue, self.est_clock, receive_time, ack_clock) + def handle_unknown(self, params): + logging.warn("Unknown message type %d: %s" % ( + params['#msgid'], repr(params['#msg']))) + def handle_output(self, params): + logging.info("%s: %s" % (params['#name'], params['#msg'])) + def handle_state(self, params): + state = params['#state'] + if state == 'connected': + logging.info("Loaded %d commands (%s)" % ( + len(self.msgparser.messages_by_id), self.msgparser.version)) + else: + logging.info("State: %s" % (state,)) + def handle_default(self, params): + logging.warn("got %s" % (params,)) + +# Class to retry sending of a query command until a given response is received +class SerialRetryCommand: + RETRY_TIME = 0.500 + def __init__(self, serial, cmd, callback, name): + self.serial = serial + self.cmd = cmd + self.callback = callback + self.name = name + self.serial.register_callback(self.handle_callback, self.name) + self.send_timer = self.serial.reactor.register_timer( + self.send_event, self.serial.reactor.NOW) + def send_event(self, eventtime): + if self.callback is None: + self.serial.reactor.unregister_timer(self.send_timer) + return self.serial.reactor.NEVER + self.serial.send(self.cmd) + return eventtime + self.RETRY_TIME + def handle_callback(self, params): + done = self.callback(params) + if done: + self.serial.unregister_callback(self.name) + self.callback = None + +# Code to start communication and download message type dictionary +class SerialBootStrap: + RETRY_TIME = 0.500 + def __init__(self, serial): + self.serial = serial + self.identify_data = "" + self.identify_cmd = self.serial.msgparser.lookup_command( + "identify offset=%u count=%c") + self.is_done = False + self.serial.register_callback(self.handle_identify, 'identify_response') + self.serial.register_callback(self.handle_unknown, '#unknown') + self.send_timer = self.serial.reactor.register_timer( + self.send_event, self.serial.reactor.NOW) + def finalize(self): + self.is_done = True + self.serial.msgparser.process_identify(self.identify_data) + logging.info("MCU version: %s" % (self.serial.msgparser.version,)) + self.serial.unregister_callback('identify_response') + self.serial.register_callback(self.serial.handle_unknown, '#unknown') + get_status = self.serial.msgparser.lookup_command('get_status') + self.serial.status_cmd = get_status.encode() + with self.serial.lock: + hdl = self.serial.handlers['#state', None] + statemsg = {'#name': '#state', '#state': 'connected'} + hdl(statemsg) + def handle_identify(self, params): + if self.is_done or params['offset'] != len(self.identify_data): + return + msgdata = params['data'] + if not msgdata: + self.finalize() + return + self.identify_data += msgdata + imsg = self.identify_cmd.encode(len(self.identify_data), 40) + self.serial.send(imsg) + def send_event(self, eventtime): + if self.is_done: + self.serial.reactor.unregister_timer(self.send_timer) + return self.serial.reactor.NEVER + imsg = self.identify_cmd.encode(len(self.identify_data), 40) + self.serial.send(imsg) + return eventtime + self.RETRY_TIME + def handle_unknown(self, params): + logging.debug("Unknown message %d (len %d) while identifying" % ( + params['#msgid'], len(params['#msg']))) + +# Attempt to place an AVR stk500v2 style programmer into normal mode +def stk500v2_leave(ser): + logging.debug("Starting stk500v2 leave programmer sequence") + origbaud = ser.baudrate + # Request a dummy speed first as this seems to help reset the port + ser.baudrate = 1200 + ser.read(1) + # Send stk500v2 leave programmer sequence + ser.baudrate = 115200 + time.sleep(0.100) + ser.read(4096) + ser.write('\x1b\x01\x00\x01\x0e\x11\x04') + time.sleep(0.050) + res = ser.read(4096) + logging.debug("Got %s from stk500v2" % (repr(res),)) + ser.baudrate = origbaud diff --git a/klippy/serialqueue.c b/klippy/serialqueue.c new file mode 100644 index 000000000..fa75701d1 --- /dev/null +++ b/klippy/serialqueue.c @@ -0,0 +1,1021 @@ +// Serial port command queuing +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. +// +// This goal of this code is to handle low-level serial port +// communications with a microcontroller (mcu). This code is written +// in C (instead of python) to reduce communication latencies and to +// reduce scheduling jitter. The code queues messages to be +// transmitted, schedules transmission of commands at specified mcu +// clock times, prioritizes commands, and handles retransmissions. A +// background thread is launched to do this work and minimize latency. + +#include <errno.h> // errno +#include <math.h> // ceil +#include <poll.h> // poll +#include <pthread.h> // pthread_mutex_lock +#include <stddef.h> // offsetof +#include <stdint.h> // uint64_t +#include <stdio.h> // snprintf +#include <stdlib.h> // malloc +#include <string.h> // memset +#include <sys/time.h> // gettimeofday +#include <time.h> // struct timespec +#include <termios.h> // tcflush +#include <unistd.h> // pipe +#include "list.h" // list_add_tail +#include "serialqueue.h" // struct queue_message + + +/**************************************************************** + * Helper functions + ****************************************************************/ + +// Return the current system time as a double +static double +get_time(void) +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return (double)tv.tv_sec + (double)tv.tv_usec / 1000000.; +} + +#if 0 +// Fill a 'struct timespec' with a system time stored in a double +struct timespec +fill_time(double time) +{ + time_t t = time; + return (struct timespec) {t, (time - t)*1000000000. }; +} +#endif + +// Report 'errno' in a message written to stderr +void +report_errno(char *where, int rc) +{ + int e = errno; + fprintf(stderr, "Got error %d in %s: (%d)%s\n", rc, where, e, strerror(e)); +} + + +/**************************************************************** + * Poll reactor + ****************************************************************/ + +// The 'poll reactor' code is a mechanism for dispatching timer and +// file descriptor events. + +#define PR_NOW 0. +#define PR_NEVER 9999999999999999. + +struct pollreactor_timer { + double waketime; + double (*callback)(void *data, double eventtime); +}; + +struct pollreactor { + int num_fds, num_timers, must_exit; + void *callback_data; + double next_timer; + struct pollfd *fds; + void (**fd_callbacks)(void *data, double eventtime); + struct pollreactor_timer *timers; +}; + +// Allocate a new 'struct pollreactor' object +static void +pollreactor_setup(struct pollreactor *pr, int num_fds, int num_timers + , void *callback_data) +{ + pr->num_fds = num_fds; + pr->num_timers = num_timers; + pr->must_exit = 0; + pr->callback_data = callback_data; + pr->next_timer = PR_NEVER; + pr->fds = malloc(num_fds * sizeof(*pr->fds)); + memset(pr->fds, 0, num_fds * sizeof(*pr->fds)); + pr->fd_callbacks = malloc(num_fds * sizeof(*pr->fd_callbacks)); + memset(pr->fd_callbacks, 0, num_fds * sizeof(*pr->fd_callbacks)); + pr->timers = malloc(num_timers * sizeof(*pr->timers)); + memset(pr->timers, 0, num_timers * sizeof(*pr->timers)); + int i; + for (i=0; i<num_timers; i++) + pr->timers[i].waketime = PR_NEVER; +} + +// Add a callback for when a file descriptor (fd) becomes readable +static void +pollreactor_add_fd(struct pollreactor *pr, int pos, int fd, void *callback) +{ + pr->fds[pos].fd = fd; + pr->fds[pos].events = POLLIN|POLLHUP; + pr->fds[pos].revents = 0; + pr->fd_callbacks[pos] = callback; +} + +// Add a timer callback +static void +pollreactor_add_timer(struct pollreactor *pr, int pos, void *callback) +{ + pr->timers[pos].callback = callback; + pr->timers[pos].waketime = PR_NEVER; +} + +#if 0 +// Return the last schedule wake-up time for a timer +static double +pollreactor_get_timer(struct pollreactor *pr, int pos) +{ + return pr->timers[pos].waketime; +} +#endif + +// Set the wake-up time for a given timer +static void +pollreactor_update_timer(struct pollreactor *pr, int pos, double waketime) +{ + pr->timers[pos].waketime = waketime; + if (waketime < pr->next_timer) + pr->next_timer = waketime; +} + +// Internal code to invoke timer callbacks +static int +pollreactor_check_timers(struct pollreactor *pr, double eventtime) +{ + if (eventtime >= pr->next_timer) { + pr->next_timer = PR_NEVER; + int i; + for (i=0; i<pr->num_timers; i++) { + struct pollreactor_timer *timer = &pr->timers[i]; + double t = timer->waketime; + if (eventtime >= t) { + t = timer->callback(pr->callback_data, eventtime); + timer->waketime = t; + } + if (t < pr->next_timer) + pr->next_timer = t; + } + if (eventtime >= pr->next_timer) + return 0; + } + double timeout = ceil((pr->next_timer - eventtime) * 1000.); + return timeout < 1. ? 1 : (timeout > 1000. ? 1000 : (int)timeout); +} + +// Repeatedly check for timer and fd events and invoke their callbacks +static void +pollreactor_run(struct pollreactor *pr) +{ + pr->must_exit = 0; + double eventtime = get_time(); + while (! pr->must_exit) { + int timeout = pollreactor_check_timers(pr, eventtime); + int ret = poll(pr->fds, pr->num_fds, timeout); + eventtime = get_time(); + if (ret > 0) { + int i; + for (i=0; i<pr->num_fds; i++) + if (pr->fds[i].revents) + pr->fd_callbacks[i](pr->callback_data, eventtime); + } else if (ret < 0) { + report_errno("poll", ret); + pr->must_exit = 1; + } + } +} + +// Request that a currently running pollreactor_run() loop exit +static void +pollreactor_do_exit(struct pollreactor *pr) +{ + pr->must_exit = 1; +} + +// Check if a pollreactor_run() loop has been requested to exit +static int +pollreactor_is_exit(struct pollreactor *pr) +{ + return pr->must_exit; +} + + +/**************************************************************** + * Serial protocol helpers + ****************************************************************/ + +// Implement the standard crc "ccitt" algorithm on the given buffer +static uint16_t +crc16_ccitt(uint8_t *buf, uint8_t len) +{ + uint16_t crc = 0xffff; + while (len--) { + uint8_t data = *buf++; + data ^= crc & 0xff; + data ^= data << 4; + crc = ((((uint16_t)data << 8) | (crc >> 8)) ^ (uint8_t)(data >> 4) + ^ ((uint16_t)data << 3)); + } + return crc; +} + +// Verify a buffer starts with a valid mcu message +static int +check_message(uint8_t *need_sync, uint8_t *buf, int buf_len) +{ + if (buf_len < MESSAGE_MIN) + // Need more data + return 0; + if (*need_sync) + goto error; + uint8_t msglen = buf[MESSAGE_POS_LEN]; + if (msglen < MESSAGE_MIN || msglen > MESSAGE_MAX) + goto error; + uint8_t msgseq = buf[MESSAGE_POS_SEQ]; + if ((msgseq & ~MESSAGE_SEQ_MASK) != MESSAGE_DEST) + goto error; + if (buf_len < msglen) + // Need more data + return 0; + if (buf[msglen-MESSAGE_TRAILER_SYNC] != MESSAGE_SYNC) + goto error; + uint16_t msgcrc = ((buf[msglen-MESSAGE_TRAILER_CRC] << 8) + | (uint8_t)buf[msglen-MESSAGE_TRAILER_CRC+1]); + uint16_t crc = crc16_ccitt(buf, msglen-MESSAGE_TRAILER_SIZE); + if (crc != msgcrc) + goto error; + return msglen; + +error: ; + // Discard bytes until next SYNC found + uint8_t *next_sync = memchr(buf, MESSAGE_SYNC, buf_len); + if (next_sync) { + *need_sync = 0; + return -(next_sync - buf + 1); + } + *need_sync = 1; + return -buf_len; +} + +// Encode an integer as a variable length quantity (vlq) +static uint8_t * +encode_int(uint8_t *p, uint32_t v) +{ + int32_t sv = v; + if (sv < (3L<<5) && sv >= -(1L<<5)) goto f4; + if (sv < (3L<<12) && sv >= -(1L<<12)) goto f3; + if (sv < (3L<<19) && sv >= -(1L<<19)) goto f2; + if (sv < (3L<<26) && sv >= -(1L<<26)) goto f1; + *p++ = (v>>28) | 0x80; +f1: *p++ = ((v>>21) & 0x7f) | 0x80; +f2: *p++ = ((v>>14) & 0x7f) | 0x80; +f3: *p++ = ((v>>7) & 0x7f) | 0x80; +f4: *p++ = v & 0x7f; + return p; +} + + +/**************************************************************** + * Command queues + ****************************************************************/ + +struct command_queue { + struct list_head stalled_queue, ready_queue; + struct list_node node; +}; + +// Allocate a 'struct queue_message' object +static struct queue_message * +message_alloc(void) +{ + struct queue_message *qm = malloc(sizeof(*qm)); + memset(qm, 0, sizeof(*qm)); + return qm; +} + +// Allocate a queue_message and fill it with the specified data +static struct queue_message * +message_fill(uint8_t *data, int len) +{ + struct queue_message *qm = message_alloc(); + memcpy(qm->msg, data, len); + qm->len = len; + return qm; +} + +// Allocate a queue_message and fill it with a series of encoded vlq integers +struct queue_message * +message_alloc_and_encode(uint32_t *data, int len) +{ + struct queue_message *qm = message_alloc(); + int i; + uint8_t *p = qm->msg; + for (i=0; i<len; i++) { + p = encode_int(p, data[i]); + if (p > &qm->msg[MESSAGE_PAYLOAD_MAX]) + goto fail; + } + qm->len = p - qm->msg; + return qm; + +fail: + fprintf(stderr, "Encode error\n"); + qm->len = 0; + return qm; +} + +// Free the storage from a previous message_alloc() call +static void +message_free(struct queue_message *qm) +{ + free(qm); +} + + +/**************************************************************** + * Serialqueue interface + ****************************************************************/ + +struct serialqueue { + // Input reading + struct pollreactor pr; + int serial_fd; + int pipe_fds[2]; + uint8_t input_buf[4096]; + uint8_t need_sync; + int input_pos; + // Threading + pthread_t tid; + pthread_mutex_t lock; // protects variables below + pthread_cond_t cond; + int receive_waiting; + // Baud / clock tracking + double baud_adjust, idle_time; + double est_clock, last_ack_time; + uint64_t last_ack_clock; + double last_receive_sent_time; + // Retransmit support + uint64_t send_seq, receive_seq; + uint64_t retransmit_seq, rtt_sample_seq; + struct list_head sent_queue; + double srtt, rttvar, rto; + // Pending transmission message queues + struct list_head pending_queues; + int ready_bytes, stalled_bytes; + uint64_t need_kick_clock; + int can_delay_writes; + // Received messages + struct list_head receive_queue; + // Debugging + struct list_head old_sent, old_receive; + // Stats + uint32_t bytes_write, bytes_read, bytes_retransmit, bytes_invalid; +}; + +#define SQPF_SERIAL 0 +#define SQPF_PIPE 1 +#define SQPF_NUM 2 + +#define SQPT_RETRANSMIT 0 +#define SQPT_COMMAND 1 +#define SQPT_NUM 2 + +#define MIN_RTO 0.025 +#define MAX_RTO 5.000 +#define MAX_SERIAL_BUFFER 0.050 +#define MIN_REQTIME_DELTA 0.250 +#define IDLE_QUERY_TIME 1.0 + +#define DEBUG_QUEUE_SENT 100 +#define DEBUG_QUEUE_RECEIVE 20 + +// Create a series of empty messages and add them to a list +static void +debug_queue_alloc(struct list_head *root, int count) +{ + int i; + for (i=0; i<count; i++) { + struct queue_message *qm = message_alloc(); + list_add_head(&qm->node, root); + } +} + +// Copy a message to a debug queue and free old debug messages +static void +debug_queue_add(struct list_head *root, struct queue_message *qm) +{ + list_add_tail(&qm->node, root); + struct queue_message *old = list_first_entry( + root, struct queue_message, node); + list_del(&old->node); + message_free(old); +} + +// Wake up the receiver thread if it is waiting +static void +check_wake_receive(struct serialqueue *sq) +{ + if (sq->receive_waiting) { + sq->receive_waiting = 0; + pthread_cond_signal(&sq->cond); + } +} + +// Update internal state when the receive sequence increases +static void +update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq) +{ + // Remove from sent queue + int ack_count = rseq - sq->receive_seq; + uint64_t sent_seq = sq->receive_seq; + while (!list_empty(&sq->sent_queue) && ack_count--) { + struct queue_message *sent = list_first_entry( + &sq->sent_queue, struct queue_message, node); + if (rseq == ++sent_seq) + sq->last_receive_sent_time = sent->receive_time; + list_del(&sent->node); + debug_queue_add(&sq->old_sent, sent); + } + sq->receive_seq = rseq; + if (rseq > sq->send_seq) + sq->send_seq = rseq; + pollreactor_update_timer(&sq->pr, SQPT_COMMAND, PR_NOW); + + // Update retransmit info + if (sq->rtt_sample_seq && rseq >= sq->rtt_sample_seq + && sq->last_receive_sent_time) { + // RFC6298 rtt calculations + double delta = eventtime - sq->last_receive_sent_time; + if (!sq->srtt) { + sq->rttvar = delta / 2.0; + sq->srtt = delta * 10.0; // use a higher start default + } else { + sq->rttvar = (3.0 * sq->rttvar + fabs(sq->srtt - delta)) / 4.0; + sq->srtt = (7.0 * sq->srtt + delta) / 8.0; + } + double rttvar4 = sq->rttvar * 4.0; + if (rttvar4 < 0.001) + rttvar4 = 0.001; + sq->rto = sq->srtt + rttvar4; + if (sq->rto < MIN_RTO) + sq->rto = MIN_RTO; + else if (sq->rto > MAX_RTO) + sq->rto = MAX_RTO; + sq->rtt_sample_seq = 0; + } + if (list_empty(&sq->sent_queue)) { + pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, PR_NEVER); + } else { + struct queue_message *sent = list_first_entry( + &sq->sent_queue, struct queue_message, node); + double nr = eventtime + sq->rto + sent->len * sq->baud_adjust; + pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, nr); + } +} + +// Process a well formed input message +static void +handle_message(struct serialqueue *sq, double eventtime, int len) +{ + // Calculate receive sequence number + uint64_t rseq = ((sq->receive_seq & ~MESSAGE_SEQ_MASK) + | (sq->input_buf[MESSAGE_POS_SEQ] & MESSAGE_SEQ_MASK)); + if (rseq < sq->receive_seq) + rseq += MESSAGE_SEQ_MASK+1; + + if (rseq != sq->receive_seq) + // New sequence number + update_receive_seq(sq, eventtime, rseq); + else if (len == MESSAGE_MIN && rseq > sq->retransmit_seq) + // Duplicate sequence number in an empty message is a nak + pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, PR_NOW); + + if (len > MESSAGE_MIN) { + // Add message to receive queue + struct queue_message *qm = message_fill(sq->input_buf, len); + qm->sent_time = sq->last_receive_sent_time; + qm->receive_time = eventtime; + list_add_tail(&qm->node, &sq->receive_queue); + check_wake_receive(sq); + } +} + +// Callback for input activity on the serial fd +static void +input_event(struct serialqueue *sq, double eventtime) +{ + int ret = read(sq->serial_fd, &sq->input_buf[sq->input_pos] + , sizeof(sq->input_buf) - sq->input_pos); + if (ret <= 0) { + report_errno("read", ret); + pollreactor_do_exit(&sq->pr); + return; + } + sq->input_pos += ret; + for (;;) { + ret = check_message(&sq->need_sync, sq->input_buf, sq->input_pos); + if (!ret) + // Need more data + return; + if (ret > 0) { + // Received a valid message + pthread_mutex_lock(&sq->lock); + handle_message(sq, eventtime, ret); + sq->bytes_read += ret; + pthread_mutex_unlock(&sq->lock); + } else { + // Skip bad data at beginning of input + ret = -ret; + pthread_mutex_lock(&sq->lock); + sq->bytes_invalid += ret; + pthread_mutex_unlock(&sq->lock); + } + sq->input_pos -= ret; + if (sq->input_pos) + memmove(sq->input_buf, &sq->input_buf[ret], sq->input_pos); + } +} + +// Callback for input activity on the pipe fd (wakes command_event) +static void +kick_event(struct serialqueue *sq, double eventtime) +{ + char dummy[4096]; + int ret = read(sq->pipe_fds[0], dummy, sizeof(dummy)); + if (ret < 0) + report_errno("pipe read", ret); + pollreactor_update_timer(&sq->pr, SQPT_COMMAND, PR_NOW); +} + +// Callback timer for when a retransmit should be done +static double +retransmit_event(struct serialqueue *sq, double eventtime) +{ + int ret = tcflush(sq->serial_fd, TCOFLUSH); + if (ret < 0) + report_errno("tcflush", ret); + + pthread_mutex_lock(&sq->lock); + + // Retransmit all pending messages + uint8_t buf[MESSAGE_MAX * MESSAGE_SEQ_MASK + 1]; + int buflen = 0; + buf[buflen++] = MESSAGE_SYNC; + struct queue_message *qm; + list_for_each_entry(qm, &sq->sent_queue, node) { + memcpy(&buf[buflen], qm->msg, qm->len); + buflen += qm->len; + } + ret = write(sq->serial_fd, buf, buflen); + if (ret < 0) + report_errno("retransmit write", ret); + sq->bytes_retransmit += buflen; + + // Update rto + sq->rto *= 2.0; + if (sq->rto > MAX_RTO) + sq->rto = MAX_RTO; + sq->retransmit_seq = sq->send_seq; + sq->rtt_sample_seq = 0; + sq->idle_time = eventtime + buflen * sq->baud_adjust; + double waketime = sq->idle_time + sq->rto; + + pthread_mutex_unlock(&sq->lock); + return waketime; +} + +// Construct a block of data and send to the serial port +static void +build_and_send_command(struct serialqueue *sq, double eventtime) +{ + struct queue_message *out = message_alloc(); + out->len = MESSAGE_HEADER_SIZE; + + while (sq->ready_bytes) { + // Find highest priority message (message with lowest req_clock) + uint64_t min_clock = MAX_CLOCK; + struct command_queue *q, *cq = NULL; + struct queue_message *qm = NULL; + list_for_each_entry(q, &sq->pending_queues, node) { + if (!list_empty(&q->ready_queue)) { + struct queue_message *m = list_first_entry( + &q->ready_queue, struct queue_message, node); + if (m->req_clock < min_clock) { + min_clock = m->req_clock; + cq = q; + qm = m; + } + } + } + // Append message to outgoing command + if (out->len + qm->len > sizeof(out->msg) - MESSAGE_TRAILER_SIZE) + break; + list_del(&qm->node); + if (list_empty(&cq->ready_queue) && list_empty(&cq->stalled_queue)) + list_del(&cq->node); + memcpy(&out->msg[out->len], qm->msg, qm->len); + out->len += qm->len; + sq->ready_bytes -= qm->len; + message_free(qm); + } + + // Fill header / trailer + out->len += MESSAGE_TRAILER_SIZE; + out->msg[MESSAGE_POS_LEN] = out->len; + out->msg[MESSAGE_POS_SEQ] = MESSAGE_DEST | (sq->send_seq & MESSAGE_SEQ_MASK); + uint16_t crc = crc16_ccitt(out->msg, out->len - MESSAGE_TRAILER_SIZE); + out->msg[out->len - MESSAGE_TRAILER_CRC] = crc >> 8; + out->msg[out->len - MESSAGE_TRAILER_CRC+1] = crc & 0xff; + out->msg[out->len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC; + + // Send message + int ret = write(sq->serial_fd, out->msg, out->len); + if (ret < 0) + report_errno("write", ret); + sq->bytes_write += out->len; + if (eventtime > sq->idle_time) + sq->idle_time = eventtime; + sq->idle_time += out->len * sq->baud_adjust; + out->sent_time = eventtime; + out->receive_time = sq->idle_time; + if (list_empty(&sq->sent_queue)) + pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT + , sq->idle_time + sq->rto); + sq->send_seq++; + if (!sq->rtt_sample_seq) + sq->rtt_sample_seq = sq->send_seq; + list_add_tail(&out->node, &sq->sent_queue); +} + +// Determine the time the next serial data should be sent +static double +check_send_command(struct serialqueue *sq, double eventtime) +{ + if (eventtime < sq->idle_time - MAX_SERIAL_BUFFER) + // Serial port already busy + return sq->idle_time - MAX_SERIAL_BUFFER; + if (sq->send_seq - sq->receive_seq >= MESSAGE_SEQ_MASK + && sq->receive_seq != (uint64_t)-1) + // Need an ack before more messages can be sent + return PR_NEVER; + + // Check for stalled messages now ready + double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time; + idletime += MESSAGE_MIN * sq->baud_adjust; + double timedelta = idletime - sq->last_ack_time; + uint64_t ack_clock = (uint64_t)(timedelta * sq->est_clock) + sq->last_ack_clock; + uint64_t min_stalled_clock = MAX_CLOCK, min_ready_clock = MAX_CLOCK; + struct command_queue *cq; + list_for_each_entry(cq, &sq->pending_queues, node) { + // Move messages from the stalled_queue to the ready_queue + while (!list_empty(&cq->stalled_queue)) { + struct queue_message *qm = list_first_entry( + &cq->stalled_queue, struct queue_message, node); + if (ack_clock < qm->min_clock) { + if (qm->min_clock < min_stalled_clock) + min_stalled_clock = qm->min_clock; + break; + } + list_del(&qm->node); + list_add_tail(&qm->node, &cq->ready_queue); + sq->stalled_bytes -= qm->len; + sq->ready_bytes += qm->len; + } + // Update min_ready_clock + if (!list_empty(&cq->ready_queue)) { + struct queue_message *qm = list_first_entry( + &cq->ready_queue, struct queue_message, node); + if (qm->req_clock < min_ready_clock) + min_ready_clock = qm->req_clock; + } + } + + // Check for messages to send + if (sq->ready_bytes >= MESSAGE_PAYLOAD_MAX) + return PR_NOW; + if (! sq->can_delay_writes) { + if (sq->ready_bytes) + return PR_NOW; + if (sq->est_clock) + sq->can_delay_writes = 1; + sq->need_kick_clock = MAX_CLOCK; + return PR_NEVER; + } + uint64_t reqclock_delta = MIN_REQTIME_DELTA * sq->est_clock; + if (min_ready_clock <= ack_clock + reqclock_delta) + return PR_NOW; + uint64_t wantclock = min_ready_clock - reqclock_delta; + if (min_stalled_clock < wantclock) + wantclock = min_stalled_clock; + sq->need_kick_clock = wantclock; + return idletime + (wantclock - ack_clock) / sq->est_clock; +} + +// Callback timer to send data to the serial port +static double +command_event(struct serialqueue *sq, double eventtime) +{ + pthread_mutex_lock(&sq->lock); + double waketime; + for (;;) { + waketime = check_send_command(sq, eventtime); + if (waketime != PR_NOW) + break; + build_and_send_command(sq, eventtime); + } + pthread_mutex_unlock(&sq->lock); + return waketime; +} + +// Main background thread for reading/writing to serial port +static void * +background_thread(void *data) +{ + struct serialqueue *sq = data; + pollreactor_run(&sq->pr); + + pthread_mutex_lock(&sq->lock); + check_wake_receive(sq); + pthread_mutex_unlock(&sq->lock); + + return NULL; +} + +// Create a new 'struct serialqueue' object +struct serialqueue * +serialqueue_alloc(int serial_fd, double baud_adjust, int write_only) +{ + struct serialqueue *sq = malloc(sizeof(*sq)); + memset(sq, 0, sizeof(*sq)); + sq->baud_adjust = baud_adjust; + + // Reactor setup + sq->serial_fd = serial_fd; + int ret = pipe(sq->pipe_fds); + if (ret) + goto fail; + pollreactor_setup(&sq->pr, SQPF_NUM, SQPT_NUM, sq); + if (!write_only) + pollreactor_add_fd(&sq->pr, SQPF_SERIAL, serial_fd, input_event); + pollreactor_add_fd(&sq->pr, SQPF_PIPE, sq->pipe_fds[0], kick_event); + pollreactor_add_timer(&sq->pr, SQPT_RETRANSMIT, retransmit_event); + pollreactor_add_timer(&sq->pr, SQPT_COMMAND, command_event); + + // Retransmit setup + sq->send_seq = 1; + if (write_only) { + sq->receive_seq = -1; + sq->rto = PR_NEVER; + } else { + sq->receive_seq = 1; + sq->rto = MIN_RTO; + } + + // Queues + sq->need_kick_clock = MAX_CLOCK; + list_init(&sq->pending_queues); + list_init(&sq->sent_queue); + list_init(&sq->receive_queue); + + // Debugging + list_init(&sq->old_sent); + list_init(&sq->old_receive); + debug_queue_alloc(&sq->old_sent, DEBUG_QUEUE_SENT); + debug_queue_alloc(&sq->old_receive, DEBUG_QUEUE_RECEIVE); + + // Thread setup + ret = pthread_mutex_init(&sq->lock, NULL); + if (ret) + goto fail; + ret = pthread_cond_init(&sq->cond, NULL); + if (ret) + goto fail; + ret = pthread_create(&sq->tid, NULL, background_thread, sq); + if (ret) + goto fail; + + return sq; + +fail: + report_errno("init", ret); + return NULL; +} + +// Request that the background thread exit +void +serialqueue_exit(struct serialqueue *sq) +{ + pollreactor_do_exit(&sq->pr); + int ret = pthread_join(sq->tid, NULL); + if (ret) + report_errno("pthread_join", ret); +} + +// Allocate a 'struct command_queue' +struct command_queue * +serialqueue_alloc_commandqueue(void) +{ + struct command_queue *cq = malloc(sizeof(*cq)); + memset(cq, 0, sizeof(*cq)); + list_init(&cq->ready_queue); + list_init(&cq->stalled_queue); + return cq; +} + +// Write to the internal pipe to wake the background thread if in poll +static void +kick_bg_thread(struct serialqueue *sq) +{ + int ret = write(sq->pipe_fds[1], ".", 1); + if (ret < 0) + report_errno("pipe write", ret); +} + +// Add a batch of messages to the given command_queue +void +serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq + , struct list_head *msgs) +{ + // Make sure min_clock is set in list and calculate total bytes + int len = 0; + struct queue_message *qm; + list_for_each_entry(qm, msgs, node) { + if (qm->min_clock + (1LL<<31) < qm->req_clock) + qm->min_clock = qm->req_clock - (1LL<<31); + len += qm->len; + } + if (! len) + return; + qm = list_first_entry(msgs, struct queue_message, node); + + // Add list to cq->stalled_queue + pthread_mutex_lock(&sq->lock); + if (list_empty(&cq->ready_queue) && list_empty(&cq->stalled_queue)) + list_add_tail(&cq->node, &sq->pending_queues); + list_join_tail(msgs, &cq->stalled_queue); + sq->stalled_bytes += len; + int mustwake = 0; + if (qm->min_clock < sq->need_kick_clock) { + sq->need_kick_clock = 0; + mustwake = 1; + } + pthread_mutex_unlock(&sq->lock); + + // Wake the background thread if necessary + if (mustwake) + kick_bg_thread(sq); +} + +// Schedule the transmission of a message on the serial port at a +// given time and priority. +void +serialqueue_send(struct serialqueue *sq, struct command_queue *cq + , uint8_t *msg, int len, uint64_t min_clock, uint64_t req_clock) +{ + struct queue_message *qm = message_fill(msg, len); + qm->min_clock = min_clock; + qm->req_clock = req_clock; + + struct list_head msgs; + list_init(&msgs); + list_add_tail(&qm->node, &msgs); + serialqueue_send_batch(sq, cq, &msgs); +} + +// Like serialqueue_send() but also builds the message to be sent +void +serialqueue_encode_and_send(struct serialqueue *sq, struct command_queue *cq + , uint32_t *data, int len + , uint64_t min_clock, uint64_t req_clock) +{ + struct queue_message *qm = message_alloc_and_encode(data, len); + qm->min_clock = min_clock; + qm->req_clock = req_clock; + + struct list_head msgs; + list_init(&msgs); + list_add_tail(&qm->node, &msgs); + serialqueue_send_batch(sq, cq, &msgs); +} + +// Return a message read from the serial port (or wait for one if none +// available) +void +serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm) +{ + pthread_mutex_lock(&sq->lock); + // Wait for message to be available + while (list_empty(&sq->receive_queue)) { + if (pollreactor_is_exit(&sq->pr)) + goto exit; + sq->receive_waiting = 1; + int ret = pthread_cond_wait(&sq->cond, &sq->lock); + if (ret) + report_errno("pthread_cond_wait", ret); + } + + // Remove message from queue + struct queue_message *qm = list_first_entry( + &sq->receive_queue, struct queue_message, node); + list_del(&qm->node); + + // Copy message + memcpy(pqm->msg, qm->msg, qm->len); + pqm->len = qm->len; + pqm->sent_time = qm->sent_time; + pqm->receive_time = qm->receive_time; + debug_queue_add(&sq->old_receive, qm); + + pthread_mutex_unlock(&sq->lock); + return; + +exit: + pqm->len = -1; + pthread_mutex_unlock(&sq->lock); +} + +// Set the estimated clock rate of the mcu on the other end of the +// serial port +void +serialqueue_set_clock_est(struct serialqueue *sq, double est_clock + , double last_ack_time, uint64_t last_ack_clock) +{ + pthread_mutex_lock(&sq->lock); + sq->est_clock = est_clock; + sq->last_ack_time = last_ack_time; + sq->last_ack_clock = last_ack_clock; + pthread_mutex_unlock(&sq->lock); +} + +// Flush all messages in a "ready" state +void +serialqueue_flush_ready(struct serialqueue *sq) +{ + pthread_mutex_lock(&sq->lock); + sq->can_delay_writes = 0; + pthread_mutex_unlock(&sq->lock); + kick_bg_thread(sq); +} + +// Return a string buffer containing statistics for the serial port +void +serialqueue_get_stats(struct serialqueue *sq, char *buf, int len) +{ + struct serialqueue stats; + pthread_mutex_lock(&sq->lock); + memcpy(&stats, sq, sizeof(stats)); + pthread_mutex_unlock(&sq->lock); + + snprintf(buf, len, "bytes_write=%u bytes_read=%u" + " bytes_retransmit=%u bytes_invalid=%u" + " send_seq=%u receive_seq=%u retransmit_seq=%u" + " srtt=%.3f rttvar=%.3f rto=%.3f" + " ready_bytes=%u stalled_bytes=%u" + , stats.bytes_write, stats.bytes_read + , stats.bytes_retransmit, stats.bytes_invalid + , (int)stats.send_seq, (int)stats.receive_seq + , (int)stats.retransmit_seq + , stats.srtt, stats.rttvar, stats.rto + , stats.ready_bytes, stats.stalled_bytes); +} + +// Extract old messages stored in the debug queues +int +serialqueue_extract_old(struct serialqueue *sq, int sentq + , struct pull_queue_message *q, int max) +{ + int count = sentq ? DEBUG_QUEUE_SENT : DEBUG_QUEUE_RECEIVE; + struct list_head *rootp = sentq ? &sq->old_sent : &sq->old_receive; + struct list_head replacement, current; + list_init(&replacement); + debug_queue_alloc(&replacement, count); + list_init(¤t); + + // Atomically replace existing debug list with new zero'd list + pthread_mutex_lock(&sq->lock); + list_join_tail(rootp, ¤t); + list_init(rootp); + list_join_tail(&replacement, rootp); + pthread_mutex_unlock(&sq->lock); + + // Walk the debug list + int pos = 0; + while (!list_empty(¤t) && pos < max) { + struct queue_message *qm = list_first_entry( + ¤t, struct queue_message, node); + if (qm->len) { + struct pull_queue_message *pqm = q++; + pos++; + memcpy(pqm->msg, qm->msg, qm->len); + pqm->len = qm->len; + pqm->sent_time = qm->sent_time; + pqm->receive_time = qm->receive_time; + } + list_del(&qm->node); + message_free(qm); + } + return pos; +} diff --git a/klippy/serialqueue.h b/klippy/serialqueue.h new file mode 100644 index 000000000..229fb2165 --- /dev/null +++ b/klippy/serialqueue.h @@ -0,0 +1,66 @@ +#ifndef SERIALQUEUE_H +#define SERIALQUEUE_H + +#include "list.h" // struct list_head + +#define MAX_CLOCK 0x7fffffffffffffff + +#define MESSAGE_MIN 5 +#define MESSAGE_MAX 64 +#define MESSAGE_HEADER_SIZE 2 +#define MESSAGE_TRAILER_SIZE 3 +#define MESSAGE_POS_LEN 0 +#define MESSAGE_POS_SEQ 1 +#define MESSAGE_TRAILER_CRC 3 +#define MESSAGE_TRAILER_SYNC 1 +#define MESSAGE_PAYLOAD_MAX (MESSAGE_MAX - MESSAGE_MIN) +#define MESSAGE_SEQ_MASK 0x0f +#define MESSAGE_DEST 0x10 +#define MESSAGE_SYNC 0x7E + +struct queue_message { + int len; + uint8_t msg[MESSAGE_MAX]; + union { + // Filled when on a command queue + struct { + uint64_t min_clock, req_clock; + }; + // Filled when in sent/receive queues + struct { + double sent_time, receive_time; + }; + }; + struct list_node node; +}; + +struct queue_message *message_alloc_and_encode(uint32_t *data, int len); + +struct pull_queue_message { + uint8_t msg[MESSAGE_MAX]; + int len; + double sent_time, receive_time; +}; + +struct serialqueue; +struct serialqueue *serialqueue_alloc(int serial_fd, double baud_adjust + , int write_only); +void serialqueue_exit(struct serialqueue *sq); +struct command_queue *serialqueue_alloc_commandqueue(void); +void serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq + , struct list_head *msgs); +void serialqueue_send(struct serialqueue *sq, struct command_queue *cq + , uint8_t *msg, int len + , uint64_t min_clock, uint64_t req_clock); +void serialqueue_encode_and_send(struct serialqueue *sq, struct command_queue *cq + , uint32_t *data, int len + , uint64_t min_clock, uint64_t req_clock); +void serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm); +void serialqueue_set_clock_est(struct serialqueue *sq, double est_clock + , double last_ack_time, uint64_t last_ack_clock); +void serialqueue_flush_ready(struct serialqueue *sq); +void serialqueue_get_stats(struct serialqueue *sq, char *buf, int len); +int serialqueue_extract_old(struct serialqueue *sq, int sentq + , struct pull_queue_message *q, int max); + +#endif // serialqueue.h diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c new file mode 100644 index 000000000..d39bbc324 --- /dev/null +++ b/klippy/stepcompress.c @@ -0,0 +1,498 @@ +// Stepper pulse schedule compression +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. +// +// The goal of this code is to take a series of scheduled stepper +// pulse times and compress them into a handful of commands that can +// be efficiently transmitted and executed on a microcontroller (mcu). +// The mcu accepts step pulse commands that take interval, count, and +// add parameters such that 'count' pulses occur, with each step event +// calculating the next step event time using: +// next_wake_time = last_wake_time + interval; interval += add +// This code is writtin in C (instead of python) for processing +// efficiency - the repetitive integer math is vastly faster in C. + +#include <math.h> // sqrt +#include <stddef.h> // offsetof +#include <stdint.h> // uint32_t +#include <stdio.h> // fprintf +#include <stdlib.h> // malloc +#include <string.h> // memset +#include "serialqueue.h" // struct queue_message + +#define CHECK_LINES 1 +#define QUEUE_START_SIZE 1024 + +struct stepcompress { + // Buffer management + uint32_t *queue, *queue_end, *queue_pos, *queue_next; + // Internal tracking + uint32_t relclock, max_error; + // Error checking + uint32_t errors; + // Message generation + uint64_t last_step_clock; + struct list_head msg_queue; + uint32_t queue_step_msgid, oid; +}; + + +/**************************************************************** + * Queue management + ****************************************************************/ + +// Shuffle the internal queue to avoid having to allocate more ram +static void +clean_queue(struct stepcompress *sc) +{ + uint32_t *src = sc->queue_pos, *dest = sc->queue; + while (src < sc->queue_next) + *dest++ = *src++ - sc->relclock; + sc->queue_pos = sc->queue; + sc->queue_next = dest; + sc->relclock = 0; +} + +// Expand the internal queue of step times +static void +expand_queue(struct stepcompress *sc, int count) +{ + if (sc->queue + count <= sc->queue_end) { + clean_queue(sc); + return; + } + int alloc = sc->queue_end - sc->queue; + int pos = sc->queue_pos - sc->queue; + int next = sc->queue_next - sc->queue; + if (!alloc) + alloc = QUEUE_START_SIZE; + while (next + count > alloc) + alloc *= 2; + sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue)); + sc->queue_end = sc->queue + alloc; + sc->queue_pos = sc->queue + pos; + sc->queue_next = sc->queue + next; +} + +// Check if the internal queue needs to be expanded, and expand if so +static inline void +check_expand(struct stepcompress *sc, int count) +{ + if (sc->queue_next + count > sc->queue_end) + expand_queue(sc, count); +} + + +/**************************************************************** + * Step compression + ****************************************************************/ + +#define DIV_UP(n,d) (((n) + (d) - 1) / (d)) + +static inline int32_t +idiv_up(int32_t n, int32_t d) +{ + return (n>=0) ? DIV_UP(n,d) : (n/d); +} + +static inline int32_t +idiv_down(int32_t n, int32_t d) +{ + return (n>=0) ? (n/d) : (n - d + 1) / d; +} + +struct points { + int32_t minp, maxp; +}; + +// Given a requested step time, return the minimum and maximum +// acceptable times +static struct points +minmax_point(struct stepcompress *sc, uint32_t *pos) +{ + uint32_t prevpoint = pos > sc->queue_pos ? *(pos-1) - sc->relclock : 0; + uint32_t point = *pos - sc->relclock; + uint32_t max_error = (point - prevpoint) / 2; + if (max_error > sc->max_error) + max_error = sc->max_error; + return (struct points){ point - max_error, point }; +} + +// The maximum add delta between two valid quadratic sequences of the +// form "add*count*(count-1)/2 + interval*count" is "(6 + 4*sqrt(2)) * +// maxerror / (count*count)". The "6 + 4*sqrt(2)" is 11.65685, but +// using 11 and rounding up when dividing works well in practice. +#define QUADRATIC_DEV 11 + +struct step_move { + uint32_t interval; + uint16_t count; + int16_t add; +}; + +// Find a 'step_move' that covers a series of step times +static struct step_move +compress_bisect_add(struct stepcompress *sc) +{ + uint32_t *last = sc->queue_next; + if (last > sc->queue_pos + 65535) + last = sc->queue_pos + 65535; + struct points point = minmax_point(sc, sc->queue_pos); + int32_t origmininterval = point.minp, origmaxinterval = point.maxp; + int32_t add = 0, minadd=-0x8001, maxadd=0x8000; + int32_t bestadd=0, bestcount=0, bestinterval=0; + + for (;;) { + // Find longest valid sequence with the given 'add' + int32_t mininterval = origmininterval, maxinterval = origmaxinterval; + int32_t count = 1, addfactor = 0; + for (;;) { + if (sc->queue_pos + count >= last) + return (struct step_move){ maxinterval, count, add }; + point = minmax_point(sc, sc->queue_pos + count); + addfactor += count; + int32_t c = add*addfactor; + int32_t nextmininterval = mininterval; + if (c + nextmininterval*(count+1) < point.minp) + nextmininterval = DIV_UP(point.minp - c, count+1); + int32_t nextmaxinterval = maxinterval; + if (c + nextmaxinterval*(count+1) > point.maxp) + nextmaxinterval = (point.maxp - c) / (count+1); + if (nextmininterval > nextmaxinterval) + break; + count += 1; + mininterval = nextmininterval; + maxinterval = nextmaxinterval; + } + if (count > bestcount || (count == bestcount && add > bestadd)) { + bestcount = count; + bestadd = add; + bestinterval = maxinterval; + } + + // Check if a greater or lesser add could extend the sequence + int32_t maxreach = add*addfactor + maxinterval*(count+1); + if (maxreach < point.minp) + origmaxinterval = maxinterval; + else + origmininterval = mininterval; + + if ((minadd+1)*addfactor + origmaxinterval*(count+1) < point.minp) + minadd = idiv_up(point.minp - origmaxinterval*(count+1) + , addfactor) - 1; + if ((maxadd-1)*addfactor + origmininterval*(count+1) > point.maxp) + maxadd = idiv_down(point.maxp - origmininterval*(count+1) + , addfactor) + 1; + + // The maximum valid deviation between two quadratic sequences + // can be calculated and used to further limit the add range. + if (count > 1) { + int32_t errdelta = DIV_UP(sc->max_error*QUADRATIC_DEV, count*count); + if (minadd < add - errdelta) + minadd = add - errdelta; + if (maxadd > add + errdelta) + maxadd = add + errdelta; + } + + // Bisect valid add range and try again with new 'add' + add = (maxadd + minadd) / 2; + if (add <= minadd || add >= maxadd) + break; + } + if (bestcount < 2) + bestadd = 0; + return (struct step_move){ bestinterval, bestcount, bestadd }; +} + + +/**************************************************************** + * Step compress checking + ****************************************************************/ + +// Verify that a given 'step_move' matches the actual step times +static void +check_line(struct stepcompress *sc, struct step_move move) +{ + if (!CHECK_LINES) + return; + int err = 0; + if (!move.count || !move.interval || move.interval >= 0x80000000) { + fprintf(stderr, "ERROR: Point out of range: %d %d %d\n" + , move.interval, move.count, move.add); + err++; + } + uint32_t interval = move.interval, p = interval; + uint16_t i; + for (i=0; i<move.count; i++) { + struct points point = minmax_point(sc, sc->queue_pos + i); + if (p < point.minp || p > point.maxp) { + fprintf(stderr, "ERROR: Point %d of %d: %d not in %d:%d\n" + , i+1, move.count, p, point.minp, point.maxp); + err++; + } + interval += move.add; + p += interval; + } + sc->errors += err; +} + + +/**************************************************************** + * Step compress interface + ****************************************************************/ + +// Allocate a new 'stepcompress' object +struct stepcompress * +stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid, uint32_t oid) +{ + struct stepcompress *sc = malloc(sizeof(*sc)); + memset(sc, 0, sizeof(*sc)); + sc->max_error = max_error; + list_init(&sc->msg_queue); + sc->queue_step_msgid = queue_step_msgid; + sc->oid = oid; + return sc; +} + +// Schedule a step event at the specified step_clock time +void +stepcompress_push(struct stepcompress *sc, double step_clock) +{ + check_expand(sc, 1); + step_clock += 0.5 + sc->relclock - sc->last_step_clock; + *sc->queue_next++ = step_clock; +} + +// Schedule 'steps' number of steps with a constant time between steps +// using the formula: step_clock = clock_offset + step_num*factor +double +stepcompress_push_factor(struct stepcompress *sc + , double steps, double step_offset + , double clock_offset, double factor) +{ + // Calculate number of steps to take + double ceil_steps = ceil(steps - step_offset); + double next_step_offset = ceil_steps - (steps - step_offset); + int count = ceil_steps; + check_expand(sc, count); + + // Calculate each step time + uint32_t *qn = sc->queue_next, *end = &qn[count]; + clock_offset += 0.5 + sc->relclock - sc->last_step_clock; + double pos = step_offset; + while (qn < end) { + *qn++ = clock_offset + pos*factor; + pos += 1.0; + } + sc->queue_next = qn; + return next_step_offset; +} + +// Schedule 'steps' number of steps using the formula: +// step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset) +double +stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset + , double clock_offset, double sqrt_offset, double factor) +{ + // Calculate number of steps to take + double ceil_steps = ceil(steps - step_offset); + double next_step_offset = ceil_steps - (steps - step_offset); + int count = ceil_steps; + check_expand(sc, count); + + // Calculate each step time + uint32_t *qn = sc->queue_next, *end = &qn[count]; + clock_offset += 0.5 + sc->relclock - sc->last_step_clock; + double pos = step_offset + sqrt_offset/factor; + if (factor >= 0.0) + while (qn < end) { + *qn++ = clock_offset + sqrt(pos*factor); + pos += 1.0; + } + else + while (qn < end) { + *qn++ = clock_offset - sqrt(pos*factor); + pos += 1.0; + } + sc->queue_next = end; + return next_step_offset; +} + +// Convert previously scheduled steps into commands for the mcu +static void +stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) +{ + if (sc->queue_pos >= sc->queue_next) + return; + while (move_clock > sc->last_step_clock) { + struct step_move move = compress_bisect_add(sc); + check_line(sc, move); + + uint32_t msg[5] = { + sc->queue_step_msgid, sc->oid, move.interval, move.count, move.add + }; + struct queue_message *qm = message_alloc_and_encode(msg, 5); + qm->req_clock = sc->last_step_clock; + list_add_tail(&qm->node, &sc->msg_queue); + + uint32_t addfactor = move.count*(move.count-1)/2; + uint32_t ticks = move.add*addfactor + move.interval*move.count; + sc->last_step_clock += ticks; + if (sc->queue_pos + move.count >= sc->queue_next) { + sc->queue_pos = sc->queue_next = sc->queue; + sc->relclock = 0; + break; + } + sc->queue_pos += move.count; + sc->relclock += ticks; + } +} + +// Reset the internal state of the stepcompress object +void +stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock) +{ + stepcompress_flush(sc, UINT64_MAX); + sc->last_step_clock = last_step_clock; +} + +// Queue an mcu command to go out in order with stepper commands +void +stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len) +{ + stepcompress_flush(sc, UINT64_MAX); + + struct queue_message *qm = message_alloc_and_encode(data, len); + qm->min_clock = -1; + qm->req_clock = sc->last_step_clock; + list_add_tail(&qm->node, &sc->msg_queue); +} + +// Return the count of internal errors found +uint32_t +stepcompress_get_errors(struct stepcompress *sc) +{ + return sc->errors; +} + + +/**************************************************************** + * Step compress synchronization + ****************************************************************/ + +// The steppersync object is used to synchronize the output of mcu +// step commands. The mcu can only queue a limited number of step +// commands - this code tracks when items on the mcu step queue become +// free so that new commands can be transmitted. It also ensures the +// mcu step queue is ordered between steppers so that no stepper +// starves the other steppers of space in the mcu step queue. + +struct steppersync { + // Serial port + struct serialqueue *sq; + struct command_queue *cq; + // Storage for associated stepcompress objects + struct stepcompress **sc_list; + int sc_num; + // Storage for list of pending move clocks + uint64_t *move_clocks; + int num_move_clocks; +}; + +// Allocate a new 'stepperysync' object +struct steppersync * +steppersync_alloc(struct serialqueue *sq, struct stepcompress **sc_list + , int sc_num, int move_num) +{ + struct steppersync *ss = malloc(sizeof(*ss)); + memset(ss, 0, sizeof(*ss)); + ss->sq = sq; + ss->cq = serialqueue_alloc_commandqueue(); + + ss->sc_list = malloc(sizeof(*sc_list)*sc_num); + memcpy(ss->sc_list, sc_list, sizeof(*sc_list)*sc_num); + ss->sc_num = sc_num; + + ss->move_clocks = malloc(sizeof(*ss->move_clocks)*move_num); + memset(ss->move_clocks, 0, sizeof(*ss->move_clocks)*move_num); + ss->num_move_clocks = move_num; + + return ss; +} + +// Implement a binary heap algorithm to track when the next available +// 'struct move' in the mcu will be available +static void +heap_replace(struct steppersync *ss, uint64_t req_clock) +{ + uint64_t *mc = ss->move_clocks; + int nmc = ss->num_move_clocks, pos = 0; + for (;;) { + int child1_pos = 2*pos+1, child2_pos = 2*pos+2; + uint64_t child2_clock = child2_pos < nmc ? mc[child2_pos] : UINT64_MAX; + uint64_t child1_clock = child1_pos < nmc ? mc[child1_pos] : UINT64_MAX; + if (req_clock <= child1_clock && req_clock <= child2_clock) { + mc[pos] = req_clock; + break; + } + if (child1_clock < child2_clock) { + mc[pos] = child1_clock; + pos = child1_pos; + } else { + mc[pos] = child2_clock; + pos = child2_pos; + } + } +} + +// Find and transmit any scheduled steps prior to the given 'move_clock' +void +steppersync_flush(struct steppersync *ss, uint64_t move_clock) +{ + // Flush each stepcompress to the specified move_clock + int i; + for (i=0; i<ss->sc_num; i++) + stepcompress_flush(ss->sc_list[i], move_clock); + + // Order commands by the reqclock of each pending command + struct list_head msgs; + list_init(&msgs); + uint64_t min_clock = ss->move_clocks[0]; + for (;;) { + // Find message with lowest reqclock + uint64_t req_clock = MAX_CLOCK; + struct queue_message *qm = NULL; + for (i=0; i<ss->sc_num; i++) { + struct stepcompress *sc = ss->sc_list[i]; + if (!list_empty(&sc->msg_queue)) { + struct queue_message *m = list_first_entry( + &sc->msg_queue, struct queue_message, node); + if (m->req_clock < req_clock) { + qm = m; + req_clock = m->req_clock; + } + } + } + if (!qm || (!qm->min_clock && req_clock > move_clock)) + break; + + // Set the min_clock for this command + if (!qm->min_clock) { + qm->min_clock = min_clock; + heap_replace(ss, req_clock); + min_clock = ss->move_clocks[0]; + } else { + qm->min_clock = min_clock; + } + + // Batch this command + list_del(&qm->node); + list_add_tail(&qm->node, &msgs); + } + + // Transmit commands + if (!list_empty(&msgs)) + serialqueue_send_batch(ss->sq, ss->cq, &msgs); +} diff --git a/klippy/stepper.py b/klippy/stepper.py new file mode 100644 index 000000000..61faaee07 --- /dev/null +++ b/klippy/stepper.py @@ -0,0 +1,67 @@ +# Printer stepper support +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import math, logging + +class PrinterStepper: + def __init__(self, printer, config): + self.printer = printer + self.config = config + self.mcu_stepper = self.mcu_enable = self.mcu_endstop = None + + self.step_dist = config.getfloat('step_distance') + self.inv_step_dist = 1. / self.step_dist + max_velocity = config.getfloat('max_velocity') + self.max_step_velocity = max_velocity * self.inv_step_dist + max_accel = config.getfloat('max_accel') + self.max_step_accel = max_accel * self.inv_step_dist + + self.homing_speed = config.getfloat('homing_speed', 5.0) + self.homing_positive_dir = config.getboolean( + 'homing_positive_dir', False) + self.homing_retract_dist = config.getfloat('homing_retract_dist', 5.) + self.position_min = config.getfloat('position_min', 0.) + self.position_endstop = config.getfloat('position_endstop') + self.position_max = config.getfloat('position_max') + + self.clock_ticks = None + self.need_motor_enable = True + def build_config(self): + self.clock_ticks = self.printer.mcu.get_mcu_freq() + max_error = self.config.getfloat('max_error', 0.000050) + max_error = int(max_error * self.clock_ticks) + + step_pin = self.config.get('step_pin') + dir_pin = self.config.get('dir_pin') + jc = 0.005 # XXX + min_stop_interval = int((math.sqrt(1./self.max_step_accel + jc**2) - jc) + * self.clock_ticks) - max_error + min_stop_interval = max(0, min_stop_interval) + mcu = self.printer.mcu + self.mcu_stepper = mcu.create_stepper( + step_pin, dir_pin, min_stop_interval, max_error) + enable_pin = self.config.get('enable_pin') + if enable_pin is not None: + self.mcu_enable = mcu.create_digital_out(enable_pin, 0) + endstop_pin = self.config.get('endstop_pin') + if endstop_pin is not None: + self.mcu_endstop = mcu.create_endstop(endstop_pin, self.mcu_stepper) + def motor_enable(self, move_time, enable=0): + if (self.mcu_enable is not None + and self.mcu_enable.get_last_setting() != enable): + mc = int(self.mcu_enable.get_print_clock(move_time)) + self.mcu_enable.set_digital(mc + 1, enable) + self.need_motor_enable = True + def prep_move(self, sdir, move_time): + move_clock = self.mcu_stepper.get_print_clock(move_time) + self.mcu_stepper.set_next_step_dir(sdir, int(move_clock)) + if self.need_motor_enable: + self.motor_enable(move_time, 1) + self.need_motor_enable = False + return (move_clock, self.clock_ticks, self.mcu_stepper) + def enable_endstop_checking(self, move_time, hz): + move_clock = int(self.mcu_endstop.get_print_clock(move_time)) + self.mcu_endstop.home(move_clock, int(self.clock_ticks / hz)) + return self.mcu_endstop diff --git a/klippy/util.py b/klippy/util.py new file mode 100644 index 000000000..caac827e0 --- /dev/null +++ b/klippy/util.py @@ -0,0 +1,32 @@ +# Low level unix utility functions +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +import os, pty, fcntl, termios, signal + +# Return the SIGINT interrupt handler back to the OS default +def fix_sigint(): + signal.signal(signal.SIGINT, signal.SIG_DFL) +fix_sigint() + +# Set a file-descriptor as non-blocking +def set_nonblock(fd): + fcntl.fcntl(fd, fcntl.F_SETFL + , fcntl.fcntl(fd, fcntl.F_GETFL) | os.O_NONBLOCK) + +# Support for creating a pseudo-tty for emulating a serial port +def create_pty(ptyname): + mfd, sfd = pty.openpty() + try: + os.unlink(ptyname) + except os.error: + pass + os.symlink(os.ttyname(sfd), ptyname) + fcntl.fcntl(mfd, fcntl.F_SETFL + , fcntl.fcntl(mfd, fcntl.F_GETFL) | os.O_NONBLOCK) + old = termios.tcgetattr(mfd) + old[3] = old[3] & ~termios.ECHO + termios.tcsetattr(mfd, termios.TCSADRAIN, old) + return mfd diff --git a/scripts/avrsim.py b/scripts/avrsim.py new file mode 100755 index 000000000..bbcc20b7f --- /dev/null +++ b/scripts/avrsim.py @@ -0,0 +1,212 @@ +#!/usr/bin/env python +# Script to interact with simulavr by simulating a serial port. +# +# Copyright (C) 2015 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +import sys, optparse, os, pty, select, fcntl, termios, traceback, errno +import pysimulavr + +SERIALBITS = 10 # 8N1 = 1 start, 8 data, 1 stop + +# Class to read serial data from AVR serial transmit pin. +class SerialRxPin(pysimulavr.PySimulationMember, pysimulavr.Pin): + def __init__(self, baud): + pysimulavr.Pin.__init__(self) + pysimulavr.PySimulationMember.__init__(self) + self.sc = pysimulavr.SystemClock.Instance() + self.delay = 10**9 / baud + self.current = 0 + self.pos = -1 + self.queue = "" + def SetInState(self, pin): + pysimulavr.Pin.SetInState(self, pin) + self.state = pin.outState + if self.pos < 0 and pin.outState == pin.LOW: + self.pos = 0 + self.sc.Add(self) + def DoStep(self, trueHwStep): + ishigh = self.state == self.HIGH + self.current |= ishigh << self.pos + self.pos += 1 + if self.pos == 1: + return int(self.delay * 1.5) + if self.pos >= SERIALBITS: + self.queue += chr((self.current >> 1) & 0xff) + self.pos = -1 + self.current = 0 + return -1 + return self.delay + def popChars(self): + d = self.queue + self.queue = "" + return d + +# Class to send serial data to AVR serial receive pin. +class SerialTxPin(pysimulavr.PySimulationMember, pysimulavr.Pin): + MAX_QUEUE = 64 + def __init__(self, baud): + pysimulavr.Pin.__init__(self) + pysimulavr.PySimulationMember.__init__(self) + self.SetPin('H') + self.sc = pysimulavr.SystemClock.Instance() + self.delay = 10**9 / baud + self.current = 0 + self.pos = 0 + self.queue = "" + def DoStep(self, trueHwStep): + if not self.pos: + if not self.queue: + return -1 + self.current = (ord(self.queue[0]) << 1) | 0x200 + self.queue = self.queue[1:] + newstate = 'L' + if self.current & (1 << self.pos): + newstate = 'H' + self.SetPin(newstate) + self.pos += 1 + if self.pos >= SERIALBITS: + self.pos = 0 + return self.delay + def needChars(self): + if len(self.queue) > self.MAX_QUEUE / 2: + return 0 + return self.MAX_QUEUE - len(self.queue) + def pushChars(self, c): + queueEmpty = not self.queue + self.queue += c + if queueEmpty: + self.sc.Add(self) + +# Support for creating VCD trace files +class Tracing: + def __init__(self, filename, signals): + self.filename = filename + self.signals = signals + if not signals: + self.dman = None + return + self.dman = pysimulavr.DumpManager.Instance() + self.dman.SetSingleDeviceApp() + def show_help(self): + ostr = pysimulavr.ostringstream() + self.dman.save(ostr) + sys.stdout.write(ostr.str()) + sys.exit(1) + def load_options(self): + if self.dman is None: + return + if self.signals.strip() == '?': + self.show_help() + sigs = "\n".join(["+ " + s for s in self.signals.split(',')]) + self.dman.addDumpVCD(self.filename, sigs, "ns", False, False) + def start(self): + if self.dman is not None: + self.dman.start() + def finish(self): + if self.dman is not None: + self.dman.stopApplication() + +# Support for creating a pseudo-tty for emulating a serial port +def create_pty(ptyname): + mfd, sfd = pty.openpty() + try: + os.unlink(ptyname) + except os.error: + pass + os.symlink(os.ttyname(sfd), ptyname) + fcntl.fcntl(mfd, fcntl.F_SETFL + , fcntl.fcntl(mfd, fcntl.F_GETFL) | os.O_NONBLOCK) + old = termios.tcgetattr(mfd) + old[3] = old[3] & ~termios.ECHO + termios.tcsetattr(mfd, termios.TCSADRAIN, old) + return mfd + +def main(): + usage = "%prog [options] <program.elf>" + opts = optparse.OptionParser(usage) + opts.add_option("-m", "--machine", type="string", dest="machine", + default="atmega644", help="type of AVR machine to simulate") + opts.add_option("-s", "--speed", type="int", dest="speed", default=8000000, + help="machine speed") + opts.add_option("-b", "--baud", type="int", dest="baud", default=38400, + help="baud rate of the emulated serial port") + opts.add_option("-t", "--trace", type="string", dest="trace", + help="signals to trace (? for help)") + opts.add_option("-p", "--port", type="string", dest="port", + default="/tmp/pseudoserial", + help="pseudo-tty device to create for serial port") + deffile = os.path.splitext(os.path.basename(sys.argv[0]))[0] + ".vcd" + opts.add_option("-f", "--tracefile", type="string", dest="tracefile", + default=deffile, help="filename to write signal trace to") + options, args = opts.parse_args() + if len(args) != 1: + opts.error("Incorrect number of arguments") + elffile = args[0] + proc = options.machine + ptyname = options.port + speed = options.speed + baud = options.baud + + # launch simulator + sc = pysimulavr.SystemClock.Instance() + trace = Tracing(options.tracefile, options.trace) + dev = pysimulavr.AvrFactory.instance().makeDevice(proc) + dev.Load(elffile) + dev.SetClockFreq(10**9 / speed) + sc.Add(dev) + trace.load_options() + + # Setup rx pin + rxpin = SerialRxPin(baud) + net = pysimulavr.Net() + net.Add(rxpin) + net.Add(dev.GetPin("D1")) + + # Setup tx pin + txpin = SerialTxPin(baud) + net2 = pysimulavr.Net() + net2.Add(dev.GetPin("D0")) + net2.Add(txpin) + + # Display start banner + msg = "Starting AVR simulation: machine=%s speed=%d\n" % (proc, speed) + msg += "Serial: port=%s baud=%d\n" % (ptyname, baud) + if options.trace: + msg += "Trace file: %s\n" % (options.tracefile,) + sys.stdout.write(msg) + sys.stdout.flush() + + # Create terminal device + fd = create_pty(ptyname) + + # Run loop + try: + trace.start() + while 1: + starttime = sc.GetCurrentTime() + r = sc.RunTimeRange(speed/1000) + endtime = sc.GetCurrentTime() + if starttime == endtime: + break + d = rxpin.popChars() + if d: + os.write(fd, d) + txsize = txpin.needChars() + if txsize: + res = select.select([fd], [], [], 0) + if res[0]: + try: + d = os.read(fd, txsize) + except os.error, e: + if e.errno in (errno.EAGAIN, errno.EWOULDBLOCK): + continue + break + txpin.pushChars(d) + trace.finish() + finally: + os.unlink(ptyname) + +if __name__ == '__main__': + main() diff --git a/scripts/buildcommands.py b/scripts/buildcommands.py new file mode 100644 index 000000000..8b9cc9d8b --- /dev/null +++ b/scripts/buildcommands.py @@ -0,0 +1,313 @@ +#!/usr/bin/env python +# Script to handle build time requests embedded in C code. +# +# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +import sys, os, subprocess, optparse, logging, shlex, socket, time +import json, zlib +sys.path.append('./klippy') +import msgproto + +FILEHEADER = """ +/* DO NOT EDIT! This is an autogenerated file. See scripts/buildcommands.py. */ + +#include "board/pgm.h" +#include "command.h" +""" + +def error(msg): + sys.stderr.write(msg + "\n") + sys.exit(-1) + +# Parser for constants in simple C header files. +def scan_config(file): + f = open(file, 'r') + opts = {} + for l in f.readlines(): + parts = l.split() + if len(parts) != 3: + continue + if parts[0] != '#define': + continue + value = parts[2] + if value.isdigit() or (value.startswith('0x') and value[2:].isdigit()): + value = int(value, 0) + elif value.startswith('"'): + value = value[1:-1] + opts[parts[1]] = value + f.close() + return opts + + +###################################################################### +# Command and output parser generation +###################################################################### + +def build_parser(parser, iscmd, all_param_types): + if parser.name == "#empty": + return "\n // Empty message\n .max_size=0," + if parser.name == "#output": + comment = "Output: " + parser.msgformat + else: + comment = parser.msgformat + params = '0' + types = tuple([t.__class__.__name__ for t in parser.param_types]) + if types: + paramid = all_param_types.get(types) + if paramid is None: + paramid = len(all_param_types) + all_param_types[types] = paramid + params = 'command_parameters%d' % (paramid,) + out = """ + // %s + .msg_id=%d, + .num_params=%d, + .param_types = %s, +""" % (comment, parser.msgid, len(types), params) + if iscmd: + num_args = (len(types) + types.count('PT_progmem_buffer') + + types.count('PT_buffer')) + out += " .num_args=%d," % (num_args,) + else: + max_size = min(msgproto.MESSAGE_MAX + , 1 + sum([t.max_length for t in parser.param_types])) + out += " .max_size=%d," % (max_size,) + return out + +def build_parsers(parsers, msg_to_id, all_param_types): + pcode = [] + for msgname, msg in parsers: + msgid = msg_to_id[msg] + if msgname is None: + parser = msgproto.OutputFormat(msgid, msg) + else: + parser = msgproto.MessageFormat(msgid, msg) + parsercode = build_parser(parser, 0, all_param_types) + pcode.append("{%s\n}, " % (parsercode,)) + fmt = """ +const struct command_encoder command_encoders[] PROGMEM = { +%s +}; +""" + return fmt % ("".join(pcode).strip(),) + +def build_param_types(all_param_types): + sorted_param_types = sorted([(i, a) for a, i in all_param_types.items()]) + params = [''] + for paramid, argtypes in sorted_param_types: + params.append( + 'static const uint8_t command_parameters%d[] PROGMEM = {\n' + ' %s };' % ( + paramid, ', '.join(argtypes),)) + params.append('') + return "\n".join(params) + +def build_commands(cmd_by_id, messages_by_name, all_param_types): + max_cmd_msgid = max(cmd_by_id.keys()) + index = [] + parsers = [] + externs = {} + for msgid in range(max_cmd_msgid+1): + if msgid not in cmd_by_id: + index.append(" 0,") + continue + funcname, flags, msgname = cmd_by_id[msgid] + msg = messages_by_name[msgname] + externs[funcname] = 1 + parsername = 'parser_%s' % (funcname,) + index.append(" &%s," % (parsername,)) + parser = msgproto.MessageFormat(msgid, msg) + parsercode = build_parser(parser, 1, all_param_types) + parsers.append("const struct command_parser %s PROGMEM = {" + " %s\n .flags=%s,\n .func=%s\n};" % ( + parsername, parsercode, flags, funcname)) + index = "\n".join(index) + externs = "\n".join(["extern void "+funcname+"(uint32_t*);" + for funcname in sorted(externs)]) + fmt = """ +%s + +%s + +const struct command_parser * const command_index[] PROGMEM = { +%s +}; + +const uint8_t command_index_size PROGMEM = ARRAY_SIZE(command_index); +""" + return fmt % (externs, '\n'.join(parsers), index) + + +###################################################################### +# Identify data dictionary generation +###################################################################### + +def build_identify(cmd_by_id, msg_to_id, responses, static_strings + , config, version): + #commands, messages, static_strings + messages = dict((msgid, msg) for msg, msgid in msg_to_id.items()) + data = {} + data['messages'] = messages + data['commands'] = sorted(cmd_by_id.keys()) + data['responses'] = sorted(responses) + data['static_strings'] = static_strings + configlist = ['MCU', 'CLOCK_FREQ'] + data['config'] = dict((i, config['CONFIG_'+i]) for i in configlist + if 'CONFIG_'+i in config) + data['version'] = version + + # Format compressed info into C code + data = json.dumps(data) + zdata = zlib.compress(data, 9) + out = [] + for i in range(len(zdata)): + if i % 8 == 0: + out.append('\n ') + out.append(" 0x%02x," % (ord(zdata[i]),)) + fmt = """ +const uint8_t command_identify_data[] PROGMEM = {%s +}; + +// Identify size = %d (%d uncompressed) +const uint32_t command_identify_size PROGMEM = ARRAY_SIZE(command_identify_data); +""" + return fmt % (''.join(out), len(zdata), len(data)) + + +###################################################################### +# Version generation +###################################################################### + +# Run program and return the specified output +def check_output(prog): + logging.debug("Running %s" % (repr(prog),)) + try: + process = subprocess.Popen(shlex.split(prog), stdout=subprocess.PIPE) + output = process.communicate()[0] + retcode = process.poll() + except OSError: + logging.debug("Exception on run: %s" % (traceback.format_exc(),)) + return "" + logging.debug("Got (code=%s): %s" % (retcode, repr(output))) + if retcode: + return "" + try: + return output.decode() + except UnicodeError: + logging.debug("Exception on decode: %s" % (traceback.format_exc(),)) + return "" + +# Obtain version info from "git" program +def git_version(): + if not os.path.exists('.git'): + logging.debug("No '.git' file/directory found") + return "" + ver = check_output("git describe --tags --long --dirty").strip() + logging.debug("Got git version: %s" % (repr(ver),)) + return ver + +def build_version(extra): + version = git_version() + if not version: + version = "?" + btime = time.strftime("%Y%m%d_%H%M%S") + hostname = socket.gethostname() + version = "%s-%s-%s%s" % (version, btime, hostname, extra) + return version + + +###################################################################### +# Main code +###################################################################### + +def main(): + usage = "%prog [options] <cmd section file> <autoconf.h> <output.c>" + opts = optparse.OptionParser(usage) + opts.add_option("-e", "--extra", dest="extra", default="", + help="extra version string to append to version") + opts.add_option("-v", action="store_true", dest="verbose", + help="enable debug messages") + + options, args = opts.parse_args() + if len(args) != 3: + opts.error("Incorrect arguments") + incmdfile, inheader, outcfile = args + if options.verbose: + logging.basicConfig(level=logging.DEBUG) + + # Setup + commands = {} + messages_by_name = dict((m.split()[0], m) + for m in msgproto.DefaultMessages.values()) + parsers = [] + static_strings = [] + # Parse request file + f = open(incmdfile, 'rb') + data = f.read() + f.close() + for req in data.split('\0'): + req = req.lstrip() + parts = req.split() + if not parts: + continue + cmd = parts[0] + msg = req[len(cmd)+1:] + if cmd == '_DECL_COMMAND': + funcname, flags, msgname = parts[1:4] + if msgname in commands: + error("Multiple definitions for command '%s'" % msgname) + commands[msgname] = (funcname, flags, msgname) + msg = req.split(None, 3)[3] + m = messages_by_name.get(msgname) + if m is not None and m != msg: + error("Conflicting definition for command '%s'" % msgname) + messages_by_name[msgname] = msg + elif cmd == '_DECL_PARSER': + if len(parts) == 1: + msgname = msg = "#empty" + else: + msgname = parts[1] + m = messages_by_name.get(msgname) + if m is not None and m != msg: + error("Conflicting definition for message '%s'" % msgname) + messages_by_name[msgname] = msg + parsers.append((msgname, msg)) + elif cmd == '_DECL_OUTPUT': + parsers.append((None, msg)) + elif cmd == '_DECL_STATIC_STR': + static_strings.append(req[17:]) + else: + error("Unknown build time command '%s'" % cmd) + # Create unique ids for each message type + msgid = max(msgproto.DefaultMessages.keys()) + msg_to_id = dict((m, i) for i, m in msgproto.DefaultMessages.items()) + for msgname in commands.keys() + [m for n, m in parsers]: + msg = messages_by_name.get(msgname, msgname) + if msg not in msg_to_id: + msgid += 1 + msg_to_id[msg] = msgid + # Create message definitions + all_param_types = {} + parsercode = build_parsers(parsers, msg_to_id, all_param_types) + # Create command definitions + cmd_by_id = dict((msg_to_id[messages_by_name.get(msgname, msgname)], cmd) + for msgname, cmd in commands.items()) + cmdcode = build_commands(cmd_by_id, messages_by_name, all_param_types) + paramcode = build_param_types(all_param_types) + # Create identify information + config = scan_config(inheader) + version = build_version(options.extra) + sys.stdout.write("Version: %s\n" % (version,)) + responses = [msg_to_id[msg] for msgname, msg in messages_by_name.items() + if msgname not in commands] + icode = build_identify(cmd_by_id, msg_to_id, responses, static_strings + , config, version) + # Write output + f = open(outcfile, 'wb') + f.write(FILEHEADER + paramcode + parsercode + cmdcode + icode) + f.close() + +if __name__ == '__main__': + main() diff --git a/scripts/checkstack.py b/scripts/checkstack.py new file mode 100755 index 000000000..d4f58cf39 --- /dev/null +++ b/scripts/checkstack.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python +# Script that tries to find how much stack space each function in an +# object is using. +# +# Copyright (C) 2015 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +# Usage: +# avr-objdump -d out/klipper.elf | scripts/checkstack.py + +import sys +import re + +# Functions that change stacks +STACKHOP = [] +# List of functions we can assume are never called. +IGNORE = [] + +OUTPUTDESC = """ +#funcname1[preamble_stack_usage,max_usage_with_callers]: +# insn_addr:called_function [usage_at_call_point+caller_preamble,total_usage] +# +#funcname2[p,m,max_usage_to_yield_point]: +# insn_addr:called_function [u+c,t,usage_to_yield_point] +""" + +class function: + def __init__(self, funcaddr, funcname): + self.funcaddr = funcaddr + self.funcname = funcname + self.basic_stack_usage = 0 + self.max_stack_usage = None + self.yield_usage = -1 + self.max_yield_usage = None + self.total_calls = 0 + # called_funcs = [(insnaddr, calladdr, stackusage), ...] + self.called_funcs = [] + self.subfuncs = {} + # Update function info with a found "yield" point. + def noteYield(self, stackusage): + if self.yield_usage < stackusage: + self.yield_usage = stackusage + # Update function info with a found "call" point. + def noteCall(self, insnaddr, calladdr, stackusage): + if (calladdr, stackusage) in self.subfuncs: + # Already noted a nearly identical call - ignore this one. + return + self.called_funcs.append((insnaddr, calladdr, stackusage)) + self.subfuncs[(calladdr, stackusage)] = 1 + +# Find out maximum stack usage for a function +def calcmaxstack(info, funcs): + if info.max_stack_usage is not None: + return + info.max_stack_usage = max_stack_usage = info.basic_stack_usage + info.max_yield_usage = max_yield_usage = info.yield_usage + total_calls = 0 + seenbefore = {} + # Find max of all nested calls. + for insnaddr, calladdr, usage in info.called_funcs: + callinfo = funcs.get(calladdr) + if callinfo is None: + continue + calcmaxstack(callinfo, funcs) + if callinfo.funcname not in seenbefore: + seenbefore[callinfo.funcname] = 1 + total_calls += callinfo.total_calls + 1 + funcnameroot = callinfo.funcname.split('.')[0] + if funcnameroot in IGNORE: + # This called function is ignored - don't contribute it to + # the max stack. + continue + totusage = usage + callinfo.max_stack_usage + totyieldusage = usage + callinfo.max_yield_usage + if funcnameroot in STACKHOP: + # Don't count children of this function + totusage = totyieldusage = usage + if totusage > max_stack_usage: + max_stack_usage = totusage + if callinfo.max_yield_usage >= 0 and totyieldusage > max_yield_usage: + max_yield_usage = totyieldusage + info.max_stack_usage = max_stack_usage + info.max_yield_usage = max_yield_usage + info.total_calls = total_calls + +# Try to arrange output so that functions that call each other are +# near each other. +def orderfuncs(funcaddrs, availfuncs): + l = [(availfuncs[funcaddr].total_calls + , availfuncs[funcaddr].funcname, funcaddr) + for funcaddr in funcaddrs if funcaddr in availfuncs] + l.sort() + l.reverse() + out = [] + while l: + count, name, funcaddr = l.pop(0) + info = availfuncs.get(funcaddr) + if info is None: + continue + calladdrs = [calls[1] for calls in info.called_funcs] + del availfuncs[funcaddr] + out = out + orderfuncs(calladdrs, availfuncs) + [info] + return out + +hex_s = r'[0-9a-f]+' +re_func = re.compile(r'^(?P<funcaddr>' + hex_s + r') <(?P<func>.*)>:$') +re_asm = re.compile( + r'^[ ]*(?P<insnaddr>' + hex_s + + r'):\t[^\t]*\t(?P<insn>[^\t]+?)(?P<params>\t[^;]*)?' + + r'[ ]*(; (?P<calladdr>0x' + hex_s + + r') <(?P<ref>.*)>)?$') + +def main(): + unknownfunc = function(None, "<unknown>") + indirectfunc = function(-1, '<indirect>') + unknownfunc.max_stack_usage = indirectfunc.max_stack_usage = 0 + unknownfunc.max_yield_usage = indirectfunc.max_yield_usage = -1 + funcs = {-1: indirectfunc} + funcaddr = None + datalines = {} + cur = None + atstart = 0 + stackusage = 0 + + # Parse input lines + for line in sys.stdin.readlines(): + m = re_func.match(line) + if m is not None: + # Found function + funcaddr = int(m.group('funcaddr'), 16) + funcs[funcaddr] = cur = function(funcaddr, m.group('func')) + stackusage = 0 + atstart = 1 + continue + m = re_asm.match(line) + if m is None: + if funcaddr not in datalines: + datalines[funcaddr] = line.split() + #print("other", repr(line)) + continue + insn = m.group('insn') + + if insn == 'push': + stackusage += 1 + continue + if insn == 'rcall' and m.group('params').strip() == '.+0': + stackusage += 2 + continue + + if atstart: + if insn in ['in', 'eor']: + continue + cur.basic_stack_usage = stackusage + atstart = 0 + + insnaddr = m.group('insnaddr') + calladdr = m.group('calladdr') + if calladdr is None: + if insn == 'ijmp': + # Indirect tail call + cur.noteCall(insnaddr, -1, 0) + elif insn == 'icall': + cur.noteCall(insnaddr, -1, stackusage + 2) + else: + # misc instruction + continue + else: + # Jump or call insn + calladdr = int(calladdr, 16) + ref = m.group('ref') + if '+' in ref: + # Inter-function jump. + pass + elif insn in ('rjmp', 'jmp', 'brne', 'brcs'): + # Tail call + cur.noteCall(insnaddr, calladdr, 0) + elif insn in ('rcall', 'call'): + cur.noteCall(insnaddr, calladdr, stackusage + 2) + else: + print("unknown call", ref) + cur.noteCall(insnaddr, calladdr, stackusage) + # Reset stack usage to preamble usage + stackusage = cur.basic_stack_usage + + # Update for known indirect functions + funcsbyname = {} + for info in funcs.values(): + funcnameroot = info.funcname.split('.')[0] + funcsbyname[funcnameroot] = info + mainfunc = funcsbyname.get('sched_main') + cmdfunc = funcsbyname.get('command_task') + eventfunc = funcsbyname.get('__vector_13') + for funcnameroot, info in funcsbyname.items(): + if (funcnameroot.startswith('_DECL_taskfuncs_') + or funcnameroot.startswith('_DECL_initfuncs_') + or funcnameroot.startswith('_DECL_shutdownfuncs_')): + funcname = funcnameroot[funcnameroot.index('_', 7)+1:] + f = funcsbyname[funcname] + mainfunc.noteCall(0, f.funcaddr, mainfunc.basic_stack_usage + 2) + if funcnameroot.startswith('parser_'): + f = funcsbyname.get(funcnameroot[7:]) + if f is not None: + numparams = int(datalines[info.funcaddr][2], 16) + stackusage = cmdfunc.basic_stack_usage + 2 + numparams * 4 + cmdfunc.noteCall(0, f.funcaddr, stackusage) + if funcnameroot.endswith('_event'): + eventfunc.noteCall(0, info.funcaddr, eventfunc.basic_stack_usage + 2) + + # Calculate maxstackusage + for info in funcs.values(): + calcmaxstack(info, funcs) + + # Sort functions for output + funcinfos = orderfuncs(funcs.keys(), funcs.copy()) + + # Show all functions + print(OUTPUTDESC) + for info in funcinfos: + if info.max_stack_usage == 0 and info.max_yield_usage < 0: + continue + yieldstr = "" + if info.max_yield_usage >= 0: + yieldstr = ",%d" % info.max_yield_usage + print("\n%s[%d,%d%s]:" % (info.funcname, info.basic_stack_usage + , info.max_stack_usage, yieldstr)) + for insnaddr, calladdr, stackusage in info.called_funcs: + callinfo = funcs.get(calladdr, unknownfunc) + yieldstr = "" + if callinfo.max_yield_usage >= 0: + yieldstr = ",%d" % (stackusage + callinfo.max_yield_usage) + print(" %04s:%-40s [%d+%d,%d%s]" % ( + insnaddr, callinfo.funcname, stackusage + , callinfo.basic_stack_usage + , stackusage+callinfo.max_stack_usage, yieldstr)) + +if __name__ == '__main__': + main() diff --git a/src/Kconfig b/src/Kconfig new file mode 100644 index 000000000..e7460cce3 --- /dev/null +++ b/src/Kconfig @@ -0,0 +1,20 @@ +# Main Kconfig settings + +mainmenu "Klipper Firmware Configuration" + +choice + prompt "Micro-controller Architecture" + config MACH_AVR + bool "Atmega AVR" + config MACH_SIMU + bool "Host simulator" +endchoice + +source "src/avr/Kconfig" +source "src/simulator/Kconfig" + +config INLINE_STEPPER_HACK + # Enables gcc to inline stepper_event() into the main timer irq handler + bool + default y if MACH_AVR + default n diff --git a/src/avr/Kconfig b/src/avr/Kconfig new file mode 100644 index 000000000..e09646760 --- /dev/null +++ b/src/avr/Kconfig @@ -0,0 +1,64 @@ +# Kconfig settings for AVR processors + +if MACH_AVR + +config BOARD_DIRECTORY + string + default "avr" + +choice + prompt "Processor model" + config MACH_atmega168 + bool "atmega168" + config MACH_atmega644p + bool "atmega644p" + config MACH_atmega1280 + bool "atmega1280" + config MACH_atmega2560 + bool "atmega2560" +endchoice + +config MCU + string + default "atmega168" if MACH_atmega168 + default "atmega644p" if MACH_atmega644p + default "atmega1280" if MACH_atmega1280 + default "atmega2560" if MACH_atmega2560 + +choice + prompt "Processor speed" + config AVR_FREQ_8000000 + bool "8Mhz" + config AVR_FREQ_16000000 + bool "16Mhz" + config AVR_FREQ_20000000 + bool "20Mhz" +endchoice + +config CLOCK_FREQ + int + default 8000000 if AVR_FREQ_8000000 + default 16000000 if AVR_FREQ_16000000 + default 20000000 if AVR_FREQ_20000000 + +config AVR_STACK_SIZE + int + default 256 if MACH_atmega2560 + default 128 + +config AVR_WATCHDOG + bool "Support for automated reset on watchdog timeout" + default y +config AVR_SERIAL + bool + default y +config SERIAL_BAUD + depends on AVR_SERIAL + int "Baud rate for serial port" + default 250000 +config SERIAL_BAUD_U2X + depends on AVR_SERIAL + bool "Use AVR Baud 2X mode" + default y + +endif diff --git a/src/avr/Makefile b/src/avr/Makefile new file mode 100644 index 000000000..758443dab --- /dev/null +++ b/src/avr/Makefile @@ -0,0 +1,19 @@ +# Additional avr build rules + +# Use the avr toolchain +CROSS_PREFIX=avr- + +CFLAGS-y += -mmcu=$(CONFIG_MCU) -DF_CPU=$(CONFIG_CLOCK_FREQ) +LDFLAGS-y += -Wl,--relax + +# Add avr source files +src-y += avr/main.c avr/timer.c avr/gpio.c avr/alloc.c +src-$(CONFIG_AVR_WATCHDOG) += avr/watchdog.c +src-$(CONFIG_AVR_SERIAL) += avr/serial.c + +# Build the additional hex output file +target-y += $(OUT)klipper.elf.hex + +$(OUT)klipper.elf.hex: $(OUT)klipper.elf + @echo " Creating hex file $@" + $(Q)$(OBJCOPY) -j .text -j .data -O ihex $< $@ diff --git a/src/avr/alloc.c b/src/avr/alloc.c new file mode 100644 index 000000000..aaa671bc7 --- /dev/null +++ b/src/avr/alloc.c @@ -0,0 +1,25 @@ +// AVR allocation checking code. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <avr/io.h> // AVR_STACK_POINTER_REG +#include <stdlib.h> // __malloc_heap_end +#include "autoconf.h" // CONFIG_AVR_STACK_SIZE +#include "compiler.h" // ALIGN +#include "misc.h" // alloc_maxsize + +size_t +alloc_maxsize(size_t reqsize) +{ + uint16_t memend = ALIGN(AVR_STACK_POINTER_REG, 256); + __malloc_heap_end = (void*)memend - CONFIG_AVR_STACK_SIZE; + extern char *__brkval; + int16_t maxsize = __malloc_heap_end - __brkval - 2; + if (maxsize < 0) + return 0; + if (reqsize < maxsize) + return reqsize; + return maxsize; +} diff --git a/src/avr/gpio.c b/src/avr/gpio.c new file mode 100644 index 000000000..d2cdfbd89 --- /dev/null +++ b/src/avr/gpio.c @@ -0,0 +1,337 @@ +// GPIO functions on AVR. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <stddef.h> // NULL +#include "autoconf.h" // CONFIG_MACH_atmega644p +#include "command.h" // shutdown +#include "gpio.h" // gpio_out_write +#include "irq.h" // irq_save +#include "pgm.h" // PROGMEM +#include "sched.h" // DECL_INIT + + +/**************************************************************** + * AVR chip definitions + ****************************************************************/ + +#define GPIO(PORT, NUM) (((PORT)-'A') * 8 + (NUM)) +#define GPIO2PORT(PIN) ((PIN) / 8) +#define GPIO2BIT(PIN) (1<<((PIN) % 8)) + +static volatile uint8_t * const digital_regs[] PROGMEM = { +#ifdef PINA + &PINA, +#else + NULL, +#endif + &PINB, &PINC, &PIND, +#ifdef PINE + &PINE, &PINF, &PING, &PINH, NULL, &PINJ, &PINK, &PINL +#endif +}; + +struct gpio_digital_regs { + // gcc (pre v6) does better optimization when uint8_t are bitfields + volatile uint8_t in : 8, mode : 8, out : 8; +}; + +#define GPIO2REGS(pin) \ + ((struct gpio_digital_regs*)READP(digital_regs[GPIO2PORT(pin)])) + +struct gpio_pwm_info { + volatile void *ocr; + volatile uint8_t *rega, *regb; + uint8_t en_bit, pin, flags; +}; + +enum { GP_8BIT=1, GP_AFMT=2 }; + +static const struct gpio_pwm_info pwm_regs[] PROGMEM = { +#if CONFIG_MACH_atmega168 + { &OCR0A, &TCCR0A, &TCCR0B, 1<<COM0A1, GPIO('D', 6), GP_8BIT }, + { &OCR0B, &TCCR0A, &TCCR0B, 1<<COM0B1, GPIO('D', 5), GP_8BIT }, +// { &OCR1A, &TCCR1A, &TCCR1B, 1<<COM1A1, GPIO('B', 1), 0 }, +// { &OCR1B, &TCCR1A, &TCCR1B, 1<<COM1B1, GPIO('B', 2), 0 }, + { &OCR2A, &TCCR2A, &TCCR2B, 1<<COM2A1, GPIO('B', 3), GP_8BIT|GP_AFMT }, + { &OCR2B, &TCCR2A, &TCCR2B, 1<<COM2B1, GPIO('D', 3), GP_8BIT|GP_AFMT }, +#elif CONFIG_MACH_atmega644p + { &OCR0A, &TCCR0A, &TCCR0B, 1<<COM0A1, GPIO('B', 3), GP_8BIT }, + { &OCR0B, &TCCR0A, &TCCR0B, 1<<COM0B1, GPIO('B', 4), GP_8BIT }, +// { &OCR1A, &TCCR1A, &TCCR1B, 1<<COM1A1, GPIO('D', 5), 0 }, +// { &OCR1B, &TCCR1A, &TCCR1B, 1<<COM1B1, GPIO('D', 4), 0 }, + { &OCR2A, &TCCR2A, &TCCR2B, 1<<COM2A1, GPIO('D', 7), GP_8BIT|GP_AFMT }, + { &OCR2B, &TCCR2A, &TCCR2B, 1<<COM2B1, GPIO('D', 6), GP_8BIT|GP_AFMT }, +#elif CONFIG_MACH_atmega1280 || CONFIG_MACH_atmega2560 + { &OCR0A, &TCCR0A, &TCCR0B, 1<<COM0A1, GPIO('B', 7), GP_8BIT }, + { &OCR0B, &TCCR0A, &TCCR0B, 1<<COM0B1, GPIO('G', 5), GP_8BIT }, +// { &OCR1A, &TCCR1A, &TCCR1B, 1<<COM1A1, GPIO('B', 5), 0 }, +// { &OCR1B, &TCCR1A, &TCCR1B, 1<<COM1B1, GPIO('B', 6), 0 }, +// { &OCR1C, &TCCR1A, &TCCR1B, 1<<COM1C1, GPIO('B', 7), 0 }, + { &OCR2A, &TCCR2A, &TCCR2B, 1<<COM2A1, GPIO('B', 4), GP_8BIT|GP_AFMT }, + { &OCR2B, &TCCR2A, &TCCR2B, 1<<COM2B1, GPIO('H', 6), GP_8BIT|GP_AFMT }, + { &OCR3A, &TCCR3A, &TCCR3B, 1<<COM3A1, GPIO('E', 3), 0 }, + { &OCR3B, &TCCR3A, &TCCR3B, 1<<COM3B1, GPIO('E', 4), 0 }, + { &OCR3C, &TCCR3A, &TCCR3B, 1<<COM3C1, GPIO('E', 5), 0 }, + { &OCR4A, &TCCR4A, &TCCR4B, 1<<COM4A1, GPIO('H', 3), 0 }, + { &OCR4B, &TCCR4A, &TCCR4B, 1<<COM4B1, GPIO('H', 4), 0 }, + { &OCR4C, &TCCR4A, &TCCR4B, 1<<COM4C1, GPIO('H', 5), 0 }, + { &OCR5A, &TCCR5A, &TCCR5B, 1<<COM5A1, GPIO('L', 3), 0 }, + { &OCR5B, &TCCR5A, &TCCR5B, 1<<COM5B1, GPIO('L', 4), 0 }, + { &OCR5C, &TCCR5A, &TCCR5B, 1<<COM5C1, GPIO('L', 5), 0 }, +#endif +}; + +struct gpio_adc_info { + uint8_t pin; +}; + +static const struct gpio_adc_info adc_pins[] PROGMEM = { +#if CONFIG_MACH_atmega168 + { GPIO('C', 0) }, { GPIO('C', 1) }, { GPIO('C', 2) }, { GPIO('C', 3) }, + { GPIO('C', 4) }, { GPIO('C', 5) }, { GPIO('E', 0) }, { GPIO('E', 1) }, +#elif CONFIG_MACH_atmega644p + { GPIO('A', 0) }, { GPIO('A', 1) }, { GPIO('A', 2) }, { GPIO('A', 3) }, + { GPIO('A', 4) }, { GPIO('A', 5) }, { GPIO('A', 6) }, { GPIO('A', 7) }, +#elif CONFIG_MACH_atmega1280 || CONFIG_MACH_atmega2560 + { GPIO('F', 0) }, { GPIO('F', 1) }, { GPIO('F', 2) }, { GPIO('F', 3) }, + { GPIO('F', 4) }, { GPIO('F', 5) }, { GPIO('F', 6) }, { GPIO('F', 7) }, + { GPIO('K', 0) }, { GPIO('K', 1) }, { GPIO('K', 2) }, { GPIO('K', 3) }, + { GPIO('K', 4) }, { GPIO('K', 5) }, { GPIO('K', 6) }, { GPIO('K', 7) }, +#endif +}; + +#if CONFIG_MACH_atmega168 +static const uint8_t SS = GPIO('B', 2), SCK = GPIO('B', 5), MOSI = GPIO('B', 3); +#elif CONFIG_MACH_atmega644p +static const uint8_t SS = GPIO('B', 4), SCK = GPIO('B', 7), MOSI = GPIO('B', 5); +#elif CONFIG_MACH_atmega1280 || CONFIG_MACH_atmega2560 +static const uint8_t SS = GPIO('B', 0), SCK = GPIO('B', 1), MOSI = GPIO('B', 2); +#endif + +static const uint8_t ADMUX_DEFAULT = 0x40; + + +/**************************************************************** + * gpio functions + ****************************************************************/ + +struct gpio_out +gpio_out_setup(uint8_t pin, uint8_t val) +{ + if (GPIO2PORT(pin) > ARRAY_SIZE(digital_regs)) + goto fail; + struct gpio_digital_regs *regs = GPIO2REGS(pin); + if (! regs) + goto fail; + uint8_t bit = GPIO2BIT(pin); + uint8_t flag = irq_save(); + regs->out = val ? (regs->out | bit) : (regs->out & ~bit); + regs->mode |= bit; + irq_restore(flag); + return (struct gpio_out){ .regs=regs, .bit=bit }; +fail: + shutdown("Not an output pin"); +} + +void gpio_out_toggle(struct gpio_out g) +{ + g.regs->in = g.bit; +} + +void +gpio_out_write(struct gpio_out g, uint8_t val) +{ + uint8_t flag = irq_save(); + g.regs->out = val ? (g.regs->out | g.bit) : (g.regs->out & ~g.bit); + irq_restore(flag); +} + +struct gpio_in +gpio_in_setup(uint8_t pin, int8_t pull_up) +{ + if (GPIO2PORT(pin) > ARRAY_SIZE(digital_regs)) + goto fail; + struct gpio_digital_regs *regs = GPIO2REGS(pin); + if (! regs) + goto fail; + uint8_t bit = GPIO2BIT(pin); + uint8_t flag = irq_save(); + regs->out = pull_up > 0 ? (regs->out | bit) : (regs->out & ~bit); + regs->mode &= ~bit; + irq_restore(flag); + return (struct gpio_in){ .regs=regs, .bit=bit }; +fail: + shutdown("Not an input pin"); +} + +uint8_t +gpio_in_read(struct gpio_in g) +{ + return !!(g.regs->in & g.bit); +} + + +void +gpio_pwm_write(struct gpio_pwm g, uint8_t val) +{ + if (g.size8) { + *(volatile uint8_t*)g.reg = val; + } else { + uint8_t flag = irq_save(); + *(volatile uint16_t*)g.reg = val; + irq_restore(flag); + } +} + +struct gpio_pwm +gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint8_t val) +{ + uint8_t chan; + for (chan=0; chan<ARRAY_SIZE(pwm_regs); chan++) { + const struct gpio_pwm_info *p = &pwm_regs[chan]; + if (READP(p->pin) != pin) + continue; + uint8_t flags = READP(p->flags), cs; + if (flags & GP_AFMT) { + switch (cycle_time) { + case 0 ... 8*510L - 1: cs = 1; break; + case 8*510L ... 32*510L - 1: cs = 2; break; + case 32*510L ... 64*510L - 1: cs = 3; break; + case 64*510L ... 128*510L - 1: cs = 4; break; + case 128*510L ... 256*510L - 1: cs = 5; break; + case 256*510L ... 1024*510L - 1: cs = 6; break; + default: cs = 7; break; + } + } else { + switch (cycle_time) { + case 0 ... 8*510L - 1: cs = 1; break; + case 8*510L ... 64*510L - 1: cs = 2; break; + case 64*510L ... 256*510L - 1: cs = 3; break; + case 256*510L ... 1024*510L - 1: cs = 4; break; + default: cs = 5; break; + } + } + volatile uint8_t *rega = READP(p->rega), *regb = READP(p->regb); + uint8_t en_bit = READP(p->en_bit); + struct gpio_digital_regs *regs = GPIO2REGS(pin); + uint8_t bit = GPIO2BIT(pin); + struct gpio_pwm g = (struct gpio_pwm) { + (void*)READP(p->ocr), flags & GP_8BIT }; + + // Setup PWM timer + uint8_t flag = irq_save(); + uint8_t old_cs = *regb & 0x07; + if (old_cs && old_cs != cs) + shutdown("PWM already programmed at different speed"); + *regb = cs; + + // Set default value and enable output + gpio_pwm_write(g, val); + *rega |= (1<<WGM00) | en_bit; + regs->mode |= bit; + irq_restore(flag); + + return g; + } + shutdown("Not a valid PWM pin"); +} + + +struct gpio_adc +gpio_adc_setup(uint8_t pin) +{ + uint8_t chan; + for (chan=0; chan<ARRAY_SIZE(adc_pins); chan++) { + const struct gpio_adc_info *a = &adc_pins[chan]; + if (READP(a->pin) != pin) + continue; + + // Enable ADC + ADCSRA |= (1<<ADPS0)|(1<<ADPS1)|(1<<ADPS2)|(1<<ADEN); + + // Disable digital input for this pin +#ifdef DIDR2 + if (chan >= 8) + DIDR2 |= 1 << (chan & 0x07); + else +#endif + DIDR0 |= 1 << chan; + + return (struct gpio_adc){ chan }; + } + shutdown("Not a valid ADC pin"); +} + +uint32_t +gpio_adc_sample_time(void) +{ + return (13 + 1) * 128 + 200; +} + +enum { ADC_DUMMY=0xff }; +static uint8_t last_analog_read = ADC_DUMMY; + +uint8_t +gpio_adc_sample(struct gpio_adc g) +{ + if (ADCSRA & (1<<ADSC)) + // Busy + return 1; + if (last_analog_read == g.chan) + // Sample now ready + return 0; + if (last_analog_read != ADC_DUMMY) + // Sample on another channel in progress + return 1; + last_analog_read = g.chan; + +#if defined(ADCSRB) && defined(MUX5) + // the MUX5 bit of ADCSRB selects whether we're reading from channels + // 0 to 7 (MUX5 low) or 8 to 15 (MUX5 high). + ADCSRB = (ADCSRB & ~(1 << MUX5)) | (((g.chan >> 3) & 0x01) << MUX5); +#endif + + ADMUX = ADMUX_DEFAULT | (g.chan & 0x07); + + // start the conversion + ADCSRA |= 1<<ADSC; + return 1; +} + +void +gpio_adc_clear_sample(struct gpio_adc g) +{ + if (last_analog_read == g.chan) + last_analog_read = ADC_DUMMY; +} + +uint16_t +gpio_adc_read(struct gpio_adc g) +{ + last_analog_read = ADC_DUMMY; + return ADC; +} + + +void +spi_config(void) +{ + gpio_out_setup(SS, 1); + gpio_out_setup(SCK, 0); + gpio_out_setup(MOSI, 0); + SPCR = (1<<MSTR) | (1<<SPE); +} + +void +spi_transfer(char *data, uint8_t len) +{ + while (len--) { + SPDR = *data; + while (!(SPSR & (1<<SPIF))) + ; + *data++ = SPDR; + } +} diff --git a/src/avr/gpio.h b/src/avr/gpio.h new file mode 100644 index 000000000..e6cbbd329 --- /dev/null +++ b/src/avr/gpio.h @@ -0,0 +1,42 @@ +#ifndef __AVR_GPIO_H +#define __AVR_GPIO_H + +#include <stdint.h> +#include "compiler.h" // __always_inline + +struct gpio_out { + struct gpio_digital_regs *regs; + // gcc (pre v6) does better optimization when uint8_t are bitfields + uint8_t bit : 8; +}; +struct gpio_out gpio_out_setup(uint8_t pin, uint8_t val); +void gpio_out_toggle(struct gpio_out g); +void gpio_out_write(struct gpio_out g, uint8_t val); + +struct gpio_in { + struct gpio_digital_regs *regs; + uint8_t bit; +}; +struct gpio_in gpio_in_setup(uint8_t pin, int8_t pull_up); +uint8_t gpio_in_read(struct gpio_in g); + +struct gpio_pwm { + void *reg; + uint8_t size8; +}; +struct gpio_pwm gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint8_t val); +void gpio_pwm_write(struct gpio_pwm g, uint8_t val); + +struct gpio_adc { + uint8_t chan; +}; +struct gpio_adc gpio_adc_setup(uint8_t pin); +uint32_t gpio_adc_sample_time(void); +uint8_t gpio_adc_sample(struct gpio_adc g); +void gpio_adc_clear_sample(struct gpio_adc g); +uint16_t gpio_adc_read(struct gpio_adc g); + +void spi_config(void); +void spi_transfer(char *data, uint8_t len); + +#endif // gpio.h diff --git a/src/avr/irq.h b/src/avr/irq.h new file mode 100644 index 000000000..bfc6cb511 --- /dev/null +++ b/src/avr/irq.h @@ -0,0 +1,29 @@ +#ifndef __AVR_IRQ_H +#define __AVR_IRQ_H +// Definitions for irq enable/disable on AVR + +#include <avr/interrupt.h> // cli +#include "compiler.h" // barrier + +static inline void irq_disable(void) { + cli(); + barrier(); +} + +static inline void irq_enable(void) { + barrier(); + sei(); +} + +static inline uint8_t irq_save(void) { + uint8_t flag = SREG; + irq_disable(); + return flag; +} + +static inline void irq_restore(uint8_t flag) { + barrier(); + SREG = flag; +} + +#endif // irq.h diff --git a/src/avr/main.c b/src/avr/main.c new file mode 100644 index 000000000..7651ab4a4 --- /dev/null +++ b/src/avr/main.c @@ -0,0 +1,17 @@ +// Main starting point for AVR boards. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "irq.h" // irq_enable +#include "sched.h" // sched_main + +// Main entry point for avr code. +int +main(void) +{ + irq_enable(); + sched_main(); + return 0; +} diff --git a/src/avr/misc.h b/src/avr/misc.h new file mode 100644 index 000000000..244cd5bc8 --- /dev/null +++ b/src/avr/misc.h @@ -0,0 +1,25 @@ +#ifndef __AVR_MISC_H +#define __AVR_MISC_H + +#include <stdint.h> +#include <util/crc16.h> + +// alloc.c +size_t alloc_maxsize(size_t reqsize); + +// console.c +char *console_get_input(uint8_t *plen); +void console_pop_input(uint8_t len); +char *console_get_output(uint8_t len); +void console_push_output(uint8_t len); + +// Optimized crc16_ccitt for the avr processor +#define HAVE_OPTIMIZED_CRC 1 +static inline uint16_t _crc16_ccitt(char *buf, uint8_t len) { + uint16_t crc = 0xFFFF; + while (len--) + crc = _crc_ccitt_update(crc, *buf++); + return crc; +} + +#endif // misc.h diff --git a/src/avr/pgm.h b/src/avr/pgm.h new file mode 100644 index 000000000..ba68d8f9c --- /dev/null +++ b/src/avr/pgm.h @@ -0,0 +1,25 @@ +#ifndef __AVR_PGM_H +#define __AVR_PGM_H +// This header provides the avr/pgmspace.h definitions for "PROGMEM" +// on AVR platforms. + +#include <avr/pgmspace.h> + +#define READP(VAR) ({ \ + _Pragma("GCC diagnostic push"); \ + _Pragma("GCC diagnostic ignored \"-Wint-to-pointer-cast\""); \ + typeof(VAR) __val = \ + __builtin_choose_expr(sizeof(VAR) == 1, \ + (typeof(VAR))pgm_read_byte(&(VAR)), \ + __builtin_choose_expr(sizeof(VAR) == 2, \ + (typeof(VAR))pgm_read_word(&(VAR)), \ + __builtin_choose_expr(sizeof(VAR) == 4, \ + (typeof(VAR))pgm_read_dword(&(VAR)), \ + __force_link_error__unknown_type))); \ + _Pragma("GCC diagnostic pop"); \ + __val; \ + }) + +extern void __force_link_error__unknown_type(void); + +#endif // pgm.h diff --git a/src/avr/serial.c b/src/avr/serial.c new file mode 100644 index 000000000..c73890dd6 --- /dev/null +++ b/src/avr/serial.c @@ -0,0 +1,137 @@ +// AVR serial port code. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <avr/interrupt.h> // USART0_RX_vect +#include <string.h> // memmove +#include "autoconf.h" // CONFIG_SERIAL_BAUD +#include "sched.h" // DECL_INIT +#include "irq.h" // irq_save +#include "misc.h" // console_get_input + +#define SERIAL_BUFFER_SIZE 96 +static char receive_buf[SERIAL_BUFFER_SIZE]; +static uint8_t receive_pos; +static char transmit_buf[SERIAL_BUFFER_SIZE]; +static uint8_t transmit_pos, transmit_max; + + +/**************************************************************** + * Serial hardware + ****************************************************************/ + +static void +serial_init(void) +{ + if (CONFIG_SERIAL_BAUD_U2X) { + UCSR0A = 1<<U2X0; + UBRR0 = DIV_ROUND_CLOSEST(F_CPU, 8UL * CONFIG_SERIAL_BAUD) - 1UL; + } else { + UCSR0A = 0; + UBRR0 = DIV_ROUND_CLOSEST(F_CPU, 16UL * CONFIG_SERIAL_BAUD) - 1UL; + } + + UCSR0C = (1<<UCSZ01) | (1<<UCSZ00); + UCSR0B = (1<<RXEN0) | (1<<TXEN0) | (1<<RXCIE0) | (1<<UDRIE0); +} +DECL_INIT(serial_init); + +#ifdef USART_RX_vect +#define USART0_RX_vect USART_RX_vect +#define USART0_UDRE_vect USART_UDRE_vect +#endif + +// Rx interrupt - data available to be read. +ISR(USART0_RX_vect) +{ + uint8_t data = UDR0; + if (receive_pos >= sizeof(receive_buf)) + // Serial overflow - ignore it as crc error will force retransmit + return; + receive_buf[receive_pos++] = data; +} + +// Tx interrupt - data can be written to serial. +ISR(USART0_UDRE_vect) +{ + if (transmit_pos >= transmit_max) + UCSR0B &= ~(1<<UDRIE0); + else + UDR0 = transmit_buf[transmit_pos++]; +} + + +/**************************************************************** + * Console access functions + ****************************************************************/ + +// Return a buffer (and length) containing any incoming messages +char * +console_get_input(uint8_t *plen) +{ + *plen = readb(&receive_pos); + return receive_buf; +} + +// Remove from the receive buffer the given number of bytes +void +console_pop_input(uint8_t len) +{ + uint8_t copied = 0; + for (;;) { + uint8_t rpos = readb(&receive_pos); + uint8_t needcopy = rpos - len; + if (needcopy) { + memmove(&receive_buf[copied], &receive_buf[copied + len] + , needcopy - copied); + copied = needcopy; + } + uint8_t flag = irq_save(); + if (rpos != readb(&receive_pos)) { + // Raced with irq handler - retry + irq_restore(flag); + continue; + } + receive_pos = needcopy; + irq_restore(flag); + break; + } +} + +// Return an output buffer that the caller may fill with transmit messages +char * +console_get_output(uint8_t len) +{ + uint8_t tpos = readb(&transmit_pos), tmax = readb(&transmit_max); + if (tpos == tmax) { + tpos = tmax = 0; + writeb(&transmit_max, 0); + writeb(&transmit_pos, 0); + } + if (tmax + len <= sizeof(transmit_buf)) + return &transmit_buf[tmax]; + if (tmax - tpos + len > sizeof(transmit_buf)) + return NULL; + // Disable TX irq and move buffer + writeb(&transmit_max, 0); + barrier(); + tpos = readb(&transmit_pos); + tmax -= tpos; + memmove(&transmit_buf[0], &transmit_buf[tpos], tmax); + writeb(&transmit_pos, 0); + barrier(); + writeb(&transmit_max, tmax); + UCSR0B |= 1<<UDRIE0; + return &transmit_buf[tmax]; +} + +// Accept the given number of bytes added to the transmit buffer +void +console_push_output(uint8_t len) +{ + writeb(&transmit_max, readb(&transmit_max) + len); + // enable TX interrupt + UCSR0B |= 1<<UDRIE0; +} diff --git a/src/avr/timer.c b/src/avr/timer.c new file mode 100644 index 000000000..0b2485521 --- /dev/null +++ b/src/avr/timer.c @@ -0,0 +1,171 @@ +// AVR timer interrupt scheduling code. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <avr/interrupt.h> // TCNT1 +#include "command.h" // shutdown +#include "irq.h" // irq_save +#include "sched.h" // sched_timer_kick +#include "timer.h" // timer_from_ms + + +/**************************************************************** + * Low level timer code + ****************************************************************/ + +// Return the number of clock ticks for a given number of milliseconds +uint32_t +timer_from_ms(uint32_t ms) +{ + return ms * (F_CPU / 1000); +} + +static inline uint16_t +timer_get(void) +{ + return TCNT1; +} + +static inline void +timer_set(uint16_t next) +{ + OCR1A = next; +} + +static inline void +timer_set_clear(uint16_t next) +{ + OCR1A = next; + TIFR1 = 1<<OCF1A; +} + +ISR(TIMER1_COMPA_vect) +{ + sched_timer_kick(); +} + +static void +timer_init(void) +{ + // no outputs + TCCR1A = 0; + // Normal Mode + TCCR1B = 1<<CS10; + // enable interrupt + TIMSK1 = 1<<OCIE1A; +} +DECL_INIT(timer_init); + + +/**************************************************************** + * 32bit timer wrappers + ****************************************************************/ + +static uint32_t timer_last; + +// Return the 32bit current time given the 16bit current time. +static __always_inline uint32_t +calc_time(uint32_t last, uint16_t cur) +{ + union u32_u16_u calc; + calc.val = last; + if (cur < calc.lo) + calc.hi++; + calc.lo = cur; + return calc.val; +} + +// Called by main code once every millisecond. (IRQs disabled.) +void +timer_periodic(void) +{ + timer_last = calc_time(timer_last, timer_get()); +} + +// Return the current time (in absolute clock ticks). +uint32_t +timer_read_time(void) +{ + uint8_t flag = irq_save(); + uint16_t cur = timer_get(); + uint32_t last = timer_last; + irq_restore(flag); + return calc_time(last, cur); +} + +#define TIMER_MIN_TICKS 100 + +// Set the next timer wake time (in absolute clock ticks). Caller +// must disable irqs. The caller should not schedule a time more than +// a few milliseconds in the future. +uint8_t +timer_set_next(uint32_t next) +{ + uint16_t cur = timer_get(); + if ((int16_t)(OCR1A - cur) < 0 && !(TIFR1 & (1<<OCF1A))) + // Already processing timer irqs + try_shutdown("timer_set_next called during timer dispatch"); + uint32_t mintime = calc_time(timer_last, cur + TIMER_MIN_TICKS); + if (sched_is_before(mintime, next)) { + timer_set_clear(next); + return 0; + } + timer_set_clear(mintime); + return 1; +} + +static uint8_t timer_repeat; +#define TIMER_MAX_REPEAT 40 +#define TIMER_MAX_NEXT_REPEAT 15 + +#define TIMER_MIN_TRY_TICKS 60 // 40 ticks to exit irq; 20 ticks of progress +#define TIMER_DEFER_REPEAT_TICKS 200 + +// Similar to timer_set_next(), but wait for the given time if it is +// in the near future. +uint8_t +timer_try_set_next(uint32_t target) +{ + uint16_t next = target, now = timer_get(); + int16_t diff = next - now; + if (diff > TIMER_MIN_TRY_TICKS) + // Schedule next timer normally. + goto done; + + // Next timer is in the past or near future - can't reschedule to it + uint8_t tr = timer_repeat-1; + if (likely(tr)) { + irq_enable(); + timer_repeat = tr; + irq_disable(); + while (diff >= 0) { + // Next timer is in the near future - wait for time to occur + now = timer_get(); + irq_enable(); + diff = next - now; + irq_disable(); + } + return 0; + } + + // Too many repeat timers from a single interrupt - force a pause + timer_repeat = TIMER_MAX_NEXT_REPEAT; + next = now + TIMER_DEFER_REPEAT_TICKS; + if (diff < (int16_t)(-timer_from_ms(1))) + goto fail; + +done: + timer_set(next); + return 1; +fail: + shutdown("Rescheduled timer in the past"); +} + +static void +timer_task(void) +{ + timer_repeat = TIMER_MAX_REPEAT; +} +DECL_TASK(timer_task); diff --git a/src/avr/timer.h b/src/avr/timer.h new file mode 100644 index 000000000..2165fecd5 --- /dev/null +++ b/src/avr/timer.h @@ -0,0 +1,12 @@ +#ifndef __AVR_TIMER_H +#define __AVR_TIMER_H + +#include <stdint.h> + +uint32_t timer_from_ms(uint32_t ms); +void timer_periodic(void); +uint32_t timer_read_time(void); +uint8_t timer_set_next(uint32_t next); +uint8_t timer_try_set_next(uint32_t next); + +#endif // timer.h diff --git a/src/avr/watchdog.c b/src/avr/watchdog.c new file mode 100644 index 000000000..f925a8d5e --- /dev/null +++ b/src/avr/watchdog.c @@ -0,0 +1,38 @@ +// Initialization of AVR watchdog timer. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <avr/interrupt.h> // WDT_vect +#include <avr/wdt.h> // wdt_enable +#include "command.h" // shutdown +#include "sched.h" // DECL_TASK + +static uint8_t watchdog_shutdown; + +ISR(WDT_vect) +{ + watchdog_shutdown = 1; + shutdown("Watchdog timer!"); +} + +static void +watchdog_reset(void) +{ + wdt_reset(); + if (watchdog_shutdown) { + WDTCSR |= 1<<WDIE; + watchdog_shutdown = 0; + } +} +DECL_TASK(watchdog_reset); + +static void +watchdog_init(void) +{ + // 0.5s timeout, interrupt and system reset + wdt_enable(WDTO_500MS); + WDTCSR |= 1<<WDIE; +} +DECL_INIT(watchdog_init); diff --git a/src/basecmd.c b/src/basecmd.c new file mode 100644 index 000000000..b94c820a5 --- /dev/null +++ b/src/basecmd.c @@ -0,0 +1,301 @@ +// Basic infrastructure commands. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <stdlib.h> // malloc +#include <string.h> // memcpy +#include "basecmd.h" // lookup_oid +#include "board/irq.h" // irq_save +#include "board/misc.h" // alloc_maxsize +#include "command.h" // DECL_COMMAND +#include "sched.h" // sched_clear_shutdown + + +/**************************************************************** + * Move queue + ****************************************************************/ + +static struct move *move_list, *move_free_list; +static uint16_t move_count; + +void +move_free(struct move *m) +{ + m->next = move_free_list; + move_free_list = m; +} + +struct move * +move_alloc(void) +{ + uint8_t flag = irq_save(); + struct move *m = move_free_list; + if (!m) + shutdown("Move queue empty"); + move_free_list = m->next; + irq_restore(flag); + return m; +} + +static void +move_reset(void) +{ + if (!move_count) + return; + // Add everything in move_list to the free list. + uint32_t i; + for (i=0; i<move_count-1; i++) + move_list[i].next = &move_list[i+1]; + move_list[move_count-1].next = NULL; + move_free_list = &move_list[0]; +} +DECL_SHUTDOWN(move_reset); + + +/**************************************************************** + * Generic object ids (oid) + ****************************************************************/ + +struct oid_s { + void *type, *data; +}; + +static struct oid_s *oids; +static uint8_t num_oid; +static uint32_t config_crc; +static uint8_t config_finalized; + +void * +lookup_oid(uint8_t oid, void *type) +{ + if (oid >= num_oid || type != oids[oid].type) + shutdown("Invalid oid type"); + return oids[oid].data; +} + +static void +assign_oid(uint8_t oid, void *type, void *data) +{ + if (oid >= num_oid || oids[oid].type || config_finalized) + shutdown("Can't assign oid"); + oids[oid].type = type; + oids[oid].data = data; +} + +void * +alloc_oid(uint8_t oid, void *type, uint16_t size) +{ + void *data = malloc(size); + if (!data) + shutdown("malloc failed"); + memset(data, 0, size); + assign_oid(oid, type, data); + return data; +} + +void * +next_oid(uint8_t *i, void *type) +{ + uint8_t oid = *i; + for (;;) { + oid++; + if (oid >= num_oid) + return NULL; + if (oids[oid].type == type) { + *i = oid; + return oids[oid].data; + } + } +} + +void +command_allocate_oids(uint32_t *args) +{ + if (oids) + shutdown("oids already allocated"); + uint8_t count = args[0]; + oids = malloc(sizeof(oids[0]) * count); + if (!oids) + shutdown("malloc failed"); + memset(oids, 0, sizeof(oids[0]) * count); + num_oid = count; +} +DECL_COMMAND(command_allocate_oids, "allocate_oids count=%c"); + +void +command_get_config(uint32_t *args) +{ + sendf("config is_config=%c crc=%u move_count=%hu" + , config_finalized, config_crc, move_count); +} +DECL_COMMAND_FLAGS(command_get_config, HF_IN_SHUTDOWN, "get_config"); + +void +command_finalize_config(uint32_t *args) +{ + if (!oids || config_finalized) + shutdown("Can't finalize"); + uint16_t count = alloc_maxsize(sizeof(*move_list)*1024) / sizeof(*move_list); + move_list = malloc(count * sizeof(*move_list)); + if (!count || !move_list) + shutdown("malloc failed"); + move_count = count; + move_reset(); + config_crc = args[0]; + config_finalized = 1; + command_get_config(NULL); +} +DECL_COMMAND(command_finalize_config, "finalize_config crc=%u"); + + +/**************************************************************** + * Group commands + ****************************************************************/ + +static struct timer group_timer; + +static uint8_t +group_end_event(struct timer *timer) +{ + shutdown("Missed scheduling of next event"); +} + +void +command_start_group(uint32_t *args) +{ + sched_del_timer(&group_timer); + group_timer.func = group_end_event; + group_timer.waketime = args[0]; + sched_timer(&group_timer); +} +DECL_COMMAND(command_start_group, "start_group clock=%u"); + +void +command_end_group(uint32_t *args) +{ + sched_del_timer(&group_timer); +} +DECL_COMMAND(command_end_group, "end_group"); + + +/**************************************************************** + * Timing and load stats + ****************************************************************/ + +void +command_get_status(uint32_t *args) +{ + sendf("status clock=%u status=%c", sched_read_time(), sched_is_shutdown()); +} +DECL_COMMAND_FLAGS(command_get_status, HF_IN_SHUTDOWN, "get_status"); + +static void +stats_task(void) +{ + static uint32_t last, count, sumsq; + uint32_t cur = sched_read_time(); + uint32_t diff = (cur - last) >> 8; + last = cur; + count++; + uint32_t nextsumsq = sumsq + diff*diff; + if (nextsumsq < sumsq) + nextsumsq = 0xffffffff; + sumsq = nextsumsq; + + static uint32_t prev; + if (sched_is_before(cur, prev + sched_from_ms(5000))) + return; + sendf("stats count=%u sum=%u sumsq=%u", count, cur - prev, sumsq); + prev = cur; + count = sumsq = 0; +} +DECL_TASK(stats_task); + + +/**************************************************************** + * Register debug commands + ****************************************************************/ + +void +command_debug_read8(uint32_t *args) +{ + uint8_t *ptr = (void*)(size_t)args[0]; + uint16_t v = *ptr; + sendf("debug_result val=%hu", v); +} +DECL_COMMAND_FLAGS(command_debug_read8, HF_IN_SHUTDOWN, "debug_read8 addr=%u"); + +void +command_debug_read16(uint32_t *args) +{ + uint16_t *ptr = (void*)(size_t)args[0]; + uint8_t flag = irq_save(); + uint16_t v = *ptr; + irq_restore(flag); + sendf("debug_result val=%hu", v); +} +DECL_COMMAND_FLAGS(command_debug_read16, HF_IN_SHUTDOWN, "debug_read16 addr=%u"); + +void +command_debug_write8(uint32_t *args) +{ + uint8_t *ptr = (void*)(size_t)args[0]; + *ptr = args[1]; +} +DECL_COMMAND_FLAGS(command_debug_write8, HF_IN_SHUTDOWN, + "debug_write8 addr=%u val=%u"); + +void +command_debug_write16(uint32_t *args) +{ + uint16_t *ptr = (void*)(size_t)args[0]; + uint8_t flag = irq_save(); + *ptr = args[1]; + irq_restore(flag); +} +DECL_COMMAND_FLAGS(command_debug_write16, HF_IN_SHUTDOWN, + "debug_write16 addr=%u val=%u"); + + +/**************************************************************** + * Misc commands + ****************************************************************/ + +void +command_reset(uint32_t *args) +{ + // XXX - implement reset +} +DECL_COMMAND_FLAGS(command_reset, HF_IN_SHUTDOWN, "msg_reset"); + +void +command_emergency_stop(uint32_t *args) +{ + shutdown("command request"); +} +DECL_COMMAND_FLAGS(command_emergency_stop, HF_IN_SHUTDOWN, "emergency_stop"); + +void +command_clear_shutdown(uint32_t *args) +{ + sched_clear_shutdown(); +} +DECL_COMMAND_FLAGS(command_clear_shutdown, HF_IN_SHUTDOWN, "clear_shutdown"); + +void +command_identify(uint32_t *args) +{ + uint32_t offset = args[0]; + uint8_t count = args[1]; + uint32_t isize = READP(command_identify_size); + if (offset >= isize) + count = 0; + else if (offset + count > isize) + count = isize - offset; + sendf("identify_response offset=%u data=%.*s" + , offset, count, &command_identify_data[offset]); +} +DECL_COMMAND_FLAGS(command_identify, HF_IN_SHUTDOWN, + "identify offset=%u count=%c"); diff --git a/src/basecmd.h b/src/basecmd.h new file mode 100644 index 000000000..e5719c6fd --- /dev/null +++ b/src/basecmd.h @@ -0,0 +1,23 @@ +#ifndef __BASECMD_H +#define __BASECMD_H + +#include <stdint.h> // uint8_t + +struct move { + uint32_t interval; + int16_t add; + uint16_t count; + struct move *next; + uint8_t flags; +}; + +void move_free(struct move *m); +struct move *move_alloc(void); +void *lookup_oid(uint8_t oid, void *type); +void *alloc_oid(uint8_t oid, void *type, uint16_t size); +void *next_oid(uint8_t *i, void *type); + +#define foreach_oid(pos,data,oidtype) \ + for (pos=-1; (data=next_oid(&pos, oidtype)); ) + +#endif // basecmd.h diff --git a/src/command.c b/src/command.c new file mode 100644 index 000000000..8608fbdf4 --- /dev/null +++ b/src/command.c @@ -0,0 +1,315 @@ +// Code for parsing incoming commands and encoding outgoing messages +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <ctype.h> // isspace +#include <stdarg.h> // va_start +#include <stdio.h> // vsnprintf +#include <stdlib.h> // strtod +#include <string.h> // strcasecmp +#include "board/irq.h" // irq_disable +#include "board/misc.h" // HAVE_OPTIMIZED_CRC +#include "board/pgm.h" // READP +#include "command.h" // output_P +#include "sched.h" // DECL_TASK + +#define MESSAGE_MIN 5 +#define MESSAGE_MAX 64 +#define MESSAGE_HEADER_SIZE 2 +#define MESSAGE_TRAILER_SIZE 3 +#define MESSAGE_POS_LEN 0 +#define MESSAGE_POS_SEQ 1 +#define MESSAGE_TRAILER_CRC 3 +#define MESSAGE_TRAILER_SYNC 1 +#define MESSAGE_PAYLOAD_MAX (MESSAGE_MAX - MESSAGE_MIN) +#define MESSAGE_SEQ_MASK 0x0f +#define MESSAGE_DEST 0x10 +#define MESSAGE_SYNC 0x7E + +static uint8_t next_sequence = MESSAGE_DEST; + + +/**************************************************************** + * Binary message parsing + ****************************************************************/ + +// Implement the standard crc "ccitt" algorithm on the given buffer +static uint16_t +crc16_ccitt(char *buf, uint8_t len) +{ + if (HAVE_OPTIMIZED_CRC) + return _crc16_ccitt(buf, len); + uint16_t crc = 0xffff; + while (len--) { + uint8_t data = *buf++; + data ^= crc & 0xff; + data ^= data << 4; + crc = ((((uint16_t)data << 8) | (crc >> 8)) ^ (uint8_t)(data >> 4) + ^ ((uint16_t)data << 3)); + } + return crc; +} + +// Encode an integer as a variable length quantity (vlq) +static char * +encode_int(char *p, uint32_t v) +{ + int32_t sv = v; + if (sv < (3L<<5) && sv >= -(1L<<5)) goto f4; + if (sv < (3L<<12) && sv >= -(1L<<12)) goto f3; + if (sv < (3L<<19) && sv >= -(1L<<19)) goto f2; + if (sv < (3L<<26) && sv >= -(1L<<26)) goto f1; + *p++ = (v>>28) | 0x80; +f1: *p++ = ((v>>21) & 0x7f) | 0x80; +f2: *p++ = ((v>>14) & 0x7f) | 0x80; +f3: *p++ = ((v>>7) & 0x7f) | 0x80; +f4: *p++ = v & 0x7f; + return p; +} + +// Parse an integer that was encoded as a "variable length quantity" +static uint32_t +parse_int(char **pp) +{ + char *p = *pp; + uint8_t c = *p++; + uint32_t v = c & 0x7f; + if ((c & 0x60) == 0x60) + v |= -0x20; + while (c & 0x80) { + c = *p++; + v = (v<<7) | (c & 0x7f); + } + *pp = p; + return v; +} + +// Parse an incoming command into 'args' +static noinline char * +parsef(char *p, char *maxend, const struct command_parser *cp, uint32_t *args) +{ + if (sched_is_shutdown() && !(READP(cp->flags) & HF_IN_SHUTDOWN)) { + sendf("is_shutdown static_string_id=%hu", sched_shutdown_reason()); + return NULL; + } + uint8_t num_params = READP(cp->num_params); + const uint8_t *param_types = READP(cp->param_types); + while (num_params--) { + if (p > maxend) + goto error; + uint8_t t = READP(*param_types); + param_types++; + switch (t) { + case PT_uint32: + case PT_int32: + case PT_uint16: + case PT_int16: + case PT_byte: + *args++ = parse_int(&p); + break; + case PT_buffer: { + uint8_t len = *p++; + if (p + len > maxend) + goto error; + *args++ = len; + *args++ = (size_t)p; + p += len; + break; + } + default: + goto error; + } + } + return p; +error: + shutdown("Command parser error"); +} + +// Encode a message and transmit it +void +_sendf(uint8_t parserid, ...) +{ + const struct command_encoder *cp = &command_encoders[parserid]; + uint8_t max_size = READP(cp->max_size); + char *buf = console_get_output(max_size + MESSAGE_MIN); + if (!buf) + return; + char *p = &buf[MESSAGE_HEADER_SIZE]; + if (max_size) { + char *maxend = &p[max_size]; + va_list args; + va_start(args, parserid); + uint8_t num_params = READP(cp->num_params); + const uint8_t *param_types = READP(cp->param_types); + *p++ = READP(cp->msg_id); + while (num_params--) { + if (p > maxend) + goto error; + uint8_t t = READP(*param_types); + param_types++; + uint32_t v; + switch (t) { + case PT_uint32: + case PT_int32: + case PT_uint16: + case PT_int16: + case PT_byte: + if (t >= PT_uint16) + v = va_arg(args, int) & 0xffff; + else + v = va_arg(args, uint32_t); + p = encode_int(p, v); + break; + case PT_string: { + char *s = va_arg(args, char*), *lenp = p++; + while (*s && p<maxend) + *p++ = *s++; + *lenp = p-lenp-1; + break; + } + case PT_progmem_buffer: + case PT_buffer: { + v = va_arg(args, int); + if (v > maxend-p) + v = maxend-p; + *p++ = v; + char *s = va_arg(args, char*); + if (t == PT_progmem_buffer) + memcpy_P(p, s, v); + else + memcpy(p, s, v); + p += v; + break; + } + default: + goto error; + } + } + va_end(args); + } + + // Send message to serial port + uint8_t msglen = p+MESSAGE_TRAILER_SIZE - buf; + buf[MESSAGE_POS_LEN] = msglen; + buf[MESSAGE_POS_SEQ] = next_sequence; + uint16_t crc = crc16_ccitt(buf, p-buf); + *p++ = crc>>8; + *p++ = crc; + *p++ = MESSAGE_SYNC; + console_push_output(msglen); + return; +error: + shutdown("Message encode error"); +} + + +/**************************************************************** + * Command routing + ****************************************************************/ + +// Find the command handler associated with a command +static const struct command_parser * +command_get_handler(uint8_t cmdid) +{ + if (cmdid >= READP(command_index_size)) + goto error; + const struct command_parser *cp = READP(command_index[cmdid]); + if (!cp) + goto error; + return cp; +error: + shutdown("Invalid command"); +} + +enum { CF_NEED_SYNC=1<<0, CF_NEED_VALID=1<<1 }; + +// Find the next complete message. +static char * +command_get_message(void) +{ + static uint8_t sync_state; + uint8_t buf_len; + char *buf = console_get_input(&buf_len); + if (buf_len && sync_state & CF_NEED_SYNC) + goto need_sync; + if (buf_len < MESSAGE_MIN) + // Not ready to run. + return NULL; + uint8_t msglen = buf[MESSAGE_POS_LEN]; + if (msglen < MESSAGE_MIN || msglen > MESSAGE_MAX) + goto error; + uint8_t msgseq = buf[MESSAGE_POS_SEQ]; + if ((msgseq & ~MESSAGE_SEQ_MASK) != MESSAGE_DEST) + goto error; + if (buf_len < msglen) + // Need more data + return NULL; + if (buf[msglen-MESSAGE_TRAILER_SYNC] != MESSAGE_SYNC) + goto error; + uint16_t msgcrc = ((buf[msglen-MESSAGE_TRAILER_CRC] << 8) + | (uint8_t)buf[msglen-MESSAGE_TRAILER_CRC+1]); + uint16_t crc = crc16_ccitt(buf, msglen-MESSAGE_TRAILER_SIZE); + if (crc != msgcrc) + goto error; + sync_state &= ~CF_NEED_VALID; + // Check sequence number + if (msgseq != next_sequence) { + // Lost message - discard messages until it is retransmitted + console_pop_input(msglen); + goto nak; + } + next_sequence = ((msgseq + 1) & MESSAGE_SEQ_MASK) | MESSAGE_DEST; + sendf(""); // An empty message with a new sequence number is an ack + return buf; + +error: + if (buf[0] == MESSAGE_SYNC) { + // Ignore (do not nak) leading SYNC bytes + console_pop_input(1); + return NULL; + } + sync_state |= CF_NEED_SYNC; +need_sync: ; + // Discard bytes until next SYNC found + char *next_sync = memchr(buf, MESSAGE_SYNC, buf_len); + if (next_sync) { + sync_state &= ~CF_NEED_SYNC; + console_pop_input(next_sync - buf + 1); + } else { + console_pop_input(buf_len); + } + if (sync_state & CF_NEED_VALID) + return NULL; + sync_state |= CF_NEED_VALID; +nak: + sendf(""); // An empty message with a duplicate sequence number is a nak + return NULL; +} + +// Background task that reads commands from the board serial port +static void +command_task(void) +{ + // Process commands. + char *buf = command_get_message(); + if (!buf) + return; + uint8_t msglen = buf[MESSAGE_POS_LEN]; + char *p = &buf[MESSAGE_HEADER_SIZE]; + char *msgend = &buf[msglen-MESSAGE_TRAILER_SIZE]; + while (p < msgend) { + uint8_t cmdid = *p++; + const struct command_parser *cp = command_get_handler(cmdid); + uint32_t args[READP(cp->num_args)]; + p = parsef(p, msgend, cp, args); + if (!p) + break; + void (*func)(uint32_t*) = READP(cp->func); + func(args); + } + console_pop_input(msglen); + return; +} +DECL_TASK(command_task); diff --git a/src/command.h b/src/command.h new file mode 100644 index 000000000..2bbe37f82 --- /dev/null +++ b/src/command.h @@ -0,0 +1,81 @@ +#ifndef __COMMAND_H +#define __COMMAND_H + +#include <stdarg.h> // va_list +#include <stddef.h> // size_t +#include <stdint.h> // uint8_t +#include "compiler.h" // __section + +// Declare a function to run when the specified command is received +#define DECL_COMMAND(FUNC, MSG) \ + _DECL_COMMAND(FUNC, 0, MSG) +#define DECL_COMMAND_FLAGS(FUNC, FLAGS, MSG) \ + _DECL_COMMAND(FUNC, FLAGS, MSG) + +// Flags for command handler declarations. +#define HF_IN_SHUTDOWN 0x01 // Handler can run even when in emergency stop + +// Send an output message (and declare a static message type for it) +#define output(FMT, args...) \ + _sendf(_DECL_OUTPUT(FMT) , ##args ) + +// Declare a message type and transmit it. +#define sendf(FMT, args...) \ + _sendf(_DECL_PARSER(FMT) , ##args) + +// Shut down the machine (also declares a static string to transmit) +#define shutdown(msg) \ + sched_shutdown(_DECL_STATIC_STR(msg)) +#define try_shutdown(msg) \ + sched_try_shutdown(_DECL_STATIC_STR(msg)) + +// command.c +void _sendf(uint8_t parserid, ...); + +// out/compile_time_request.c (auto generated file) +struct command_encoder { + uint8_t msg_id, max_size, num_params; + const uint8_t *param_types; +}; +struct command_parser { + uint8_t msg_id, num_args, flags, num_params; + const uint8_t *param_types; + void (*func)(uint32_t *args); +}; +enum { + PT_uint32, PT_int32, PT_uint16, PT_int16, PT_byte, + PT_string, PT_progmem_buffer, PT_buffer, +}; +extern const struct command_encoder command_encoders[]; +extern const struct command_parser * const command_index[]; +extern const uint8_t command_index_size; +extern const uint8_t command_identify_data[]; +extern const uint32_t command_identify_size; + +// Compiler glue for DECL_COMMAND macros above. +#define _DECL_COMMAND(FUNC, FLAGS, MSG) \ + char __PASTE(_DECLS_ ## FUNC ## _, __LINE__) [] \ + __visible __section(".compile_time_request") \ + = "_DECL_COMMAND " __stringify(FUNC) " " __stringify(FLAGS) " " MSG; \ + void __visible FUNC(uint32_t*) + +// Create a compile time request and return a unique (incrementing id) +// for that request. +#define _DECL_REQUEST_ID(REQUEST, ID_SECTION) ({ \ + static char __PASTE(_DECLS_, __LINE__)[] \ + __section(".compile_time_request") = REQUEST; \ + asm volatile("" : : "m"(__PASTE(_DECLS_, __LINE__))); \ + static char __PASTE(_DECLI_, __LINE__) \ + __section(".compile_time_request." ID_SECTION); \ + (size_t)&__PASTE(_DECLI_, __LINE__); }) + +#define _DECL_PARSER(FMT) \ + _DECL_REQUEST_ID("_DECL_PARSER " FMT, "parsers") + +#define _DECL_OUTPUT(FMT) \ + _DECL_REQUEST_ID("_DECL_OUTPUT " FMT, "parsers") + +#define _DECL_STATIC_STR(FMT) \ + _DECL_REQUEST_ID("_DECL_STATIC_STR " FMT, "static_strings") + +#endif // command.h diff --git a/src/compiler.h b/src/compiler.h new file mode 100644 index 000000000..ba4b83d5c --- /dev/null +++ b/src/compiler.h @@ -0,0 +1,66 @@ +#ifndef __COMPILER_H +#define __COMPILER_H +// Low level definitions for C languange and gcc compiler. + +#define barrier() __asm__ __volatile__("": : :"memory") + +#define likely(x) __builtin_expect(!!(x), 1) +#define unlikely(x) __builtin_expect(!!(x), 0) + +#define noinline __attribute__((noinline)) +#ifndef __always_inline +#define __always_inline inline __attribute__((always_inline)) +#endif +#define __visible __attribute__((externally_visible)) +#define __noreturn __attribute__((noreturn)) + +#define PACKED __attribute__((packed)) +#define __aligned(x) __attribute__((aligned(x))) +#define __section(S) __attribute__((section(S))) + +#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) +#define ALIGN(x,a) __ALIGN_MASK(x,(typeof(x))(a)-1) +#define __ALIGN_MASK(x,mask) (((x)+(mask))&~(mask)) +#define ALIGN_DOWN(x,a) ((x) & ~((typeof(x))(a)-1)) + +#define container_of(ptr, type, member) ({ \ + const typeof( ((type *)0)->member ) *__mptr = (ptr); \ + (type *)( (char *)__mptr - offsetof(type,member) );}) + +#define __stringify_1(x) #x +#define __stringify(x) __stringify_1(x) + +#define ___PASTE(a,b) a##b +#define __PASTE(a,b) ___PASTE(a,b) + +#define DIV_ROUND_UP(n,d) (((n) + (d) - 1) / (d)) +#define DIV_ROUND_CLOSEST(x, divisor)({ \ + typeof(divisor) __divisor = divisor; \ + (((x) + ((__divisor) / 2)) / (__divisor)); \ + }) + +union u32_u16_u { + struct { uint16_t lo, hi; }; + uint32_t val; +}; + +static inline void writel(void *addr, uint32_t val) { + *(volatile uint32_t *)addr = val; +} +static inline void writew(void *addr, uint16_t val) { + *(volatile uint16_t *)addr = val; +} +static inline void writeb(void *addr, uint8_t val) { + *(volatile uint8_t *)addr = val; +} +static inline uint32_t readl(const void *addr) { + return *(volatile const uint32_t *)addr; +} +static inline uint16_t readw(const void *addr) { + return *(volatile const uint16_t *)addr; +} +static inline uint8_t readb(const void *addr) { + return *(volatile const uint8_t *)addr; +} + +#endif // compiler.h diff --git a/src/declfunc.lds.S b/src/declfunc.lds.S new file mode 100644 index 000000000..9bb5c8ad8 --- /dev/null +++ b/src/declfunc.lds.S @@ -0,0 +1,26 @@ +// Linker script that defines symbols around sections. The DECL_X() +// macros need this linker script to place _start and _end symbols +// around the list of declared items. + +#define DECLWRAPPER(NAME) \ + .progmem.data. ## NAME : SUBALIGN(1) { \ + NAME ## _start = . ; \ + *( .progmem.data. ## NAME ##.pre* ) \ + *( .progmem.data. ## NAME ##* ) \ + *( .progmem.data. ## NAME ##.post* ) \ + NAME ## _end = . ; \ + } + +SECTIONS +{ + DECLWRAPPER(taskfuncs) + DECLWRAPPER(initfuncs) + DECLWRAPPER(shutdownfuncs) + + .compile_time_request.static_strings 0 (INFO) : { + *( .compile_time_request.static_strings ) + } + .compile_time_request.parsers 0 (INFO) : { + *( .compile_time_request.parsers ) + } +} diff --git a/src/endstop.c b/src/endstop.c new file mode 100644 index 000000000..bc1776059 --- /dev/null +++ b/src/endstop.c @@ -0,0 +1,110 @@ +// Handling of end stops. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <stddef.h> // offsetof +#include "basecmd.h" // alloc_oid +#include "board/gpio.h" // struct gpio +#include "board/irq.h" // irq_save +#include "command.h" // DECL_COMMAND +#include "sched.h" // struct timer +#include "stepper.h" // stepper_stop + +struct end_stop { + struct timer time; + uint32_t rest_time; + struct stepper *stepper; + struct gpio_in pin; + uint8_t pin_value, flags; +}; + +enum { ESF_HOMING=1, ESF_REPORT=2 }; + +// Timer callback for an end stop +static uint8_t +end_stop_event(struct timer *t) +{ + struct end_stop *e = container_of(t, struct end_stop, time); + uint8_t val = gpio_in_read(e->pin); + if (val != e->pin_value) { + e->time.waketime += e->rest_time; + return SF_RESCHEDULE; + } + // Stop stepper + e->flags = ESF_REPORT; + stepper_stop(e->stepper); + return SF_DONE; +} + +void +command_config_end_stop(uint32_t *args) +{ + struct end_stop *e = alloc_oid(args[0], command_config_end_stop, sizeof(*e)); + struct stepper *s = lookup_oid(args[3], command_config_stepper); + e->time.func = end_stop_event; + e->stepper = s; + e->pin = gpio_in_setup(args[1], args[2]); +} +DECL_COMMAND(command_config_end_stop, + "config_end_stop oid=%c pin=%c pull_up=%c stepper_oid=%c"); + +// Home an axis +void +command_end_stop_home(uint32_t *args) +{ + struct end_stop *e = lookup_oid(args[0], command_config_end_stop); + sched_del_timer(&e->time); + e->time.waketime = args[1]; + e->rest_time = args[2]; + if (!e->rest_time) { + // Disable end stop checking + e->flags = 0; + return; + } + e->pin_value = args[3]; + e->flags = ESF_HOMING; + sched_timer(&e->time); +} +DECL_COMMAND(command_end_stop_home, + "end_stop_home oid=%c clock=%u rest_ticks=%u pin_value=%c"); + +static void +end_stop_report(uint8_t oid, struct end_stop *e) +{ + uint8_t flag = irq_save(); + uint32_t position = stepper_get_position(e->stepper); + uint8_t eflags = e->flags; + e->flags &= ~ESF_REPORT; + irq_restore(flag); + + sendf("end_stop_state oid=%c homing=%c pin=%c pos=%i" + , oid, !!(eflags & ESF_HOMING), gpio_in_read(e->pin) + , position - STEPPER_POSITION_BIAS); +} + +void +command_end_stop_query(uint32_t *args) +{ + uint8_t oid = args[0]; + struct end_stop *e = lookup_oid(oid, command_config_end_stop); + end_stop_report(oid, e); +} +DECL_COMMAND(command_end_stop_query, "end_stop_query oid=%c"); + +static void +end_stop_task(void) +{ + static uint16_t next; + if (!sched_check_periodic(50, &next)) + return; + uint8_t oid; + struct end_stop *e; + foreach_oid(oid, e, command_config_end_stop) { + if (!(e->flags & ESF_REPORT)) + continue; + end_stop_report(oid, e); + } +} +DECL_TASK(end_stop_task); diff --git a/src/gpiocmds.c b/src/gpiocmds.c new file mode 100644 index 000000000..ff03dfd17 --- /dev/null +++ b/src/gpiocmds.c @@ -0,0 +1,401 @@ +// Commands for controlling GPIO pins +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <stddef.h> // offsetof +#include "basecmd.h" // alloc_oid +#include "board/gpio.h" // struct gpio +#include "board/irq.h" // irq_save +#include "command.h" // DECL_COMMAND +#include "sched.h" // DECL_TASK + + +/**************************************************************** + * Digital out pins + ****************************************************************/ + +struct digital_out_s { + struct timer timer; + struct gpio_out pin; + uint32_t max_duration; + uint8_t value, default_value; +}; + +static uint8_t +digital_end_event(struct timer *timer) +{ + shutdown("Missed scheduling of next pin event"); +} + +static uint8_t +digital_out_event(struct timer *timer) +{ + struct digital_out_s *d = container_of(timer, struct digital_out_s, timer); + gpio_out_write(d->pin, d->value); + if (d->value == d->default_value || !d->max_duration) + return SF_DONE; + d->timer.waketime += d->max_duration; + d->timer.func = digital_end_event; + return SF_RESCHEDULE; +} + +void +command_config_digital_out(uint32_t *args) +{ + struct digital_out_s *d = alloc_oid(args[0], command_config_digital_out + , sizeof(*d)); + d->default_value = args[2]; + d->pin = gpio_out_setup(args[1], d->default_value); + d->max_duration = args[3]; +} +DECL_COMMAND(command_config_digital_out, + "config_digital_out oid=%c pin=%u default_value=%c" + " max_duration=%u"); + +void +command_schedule_digital_out(uint32_t *args) +{ + struct digital_out_s *d = lookup_oid(args[0], command_config_digital_out); + sched_del_timer(&d->timer); + d->timer.func = digital_out_event; + d->timer.waketime = args[1]; + d->value = args[2]; + sched_timer(&d->timer); +} +DECL_COMMAND(command_schedule_digital_out, + "schedule_digital_out oid=%c clock=%u value=%c"); + +static void +digital_out_shutdown(void) +{ + uint8_t i; + struct digital_out_s *d; + foreach_oid(i, d, command_config_digital_out) { + gpio_out_write(d->pin, d->default_value); + } +} +DECL_SHUTDOWN(digital_out_shutdown); + +void +command_set_digital_out(uint32_t *args) +{ + gpio_out_setup(args[0], args[1]); +} +DECL_COMMAND(command_set_digital_out, "set_digital_out pin=%u value=%c"); + + +/**************************************************************** + * Hardware PWM pins + ****************************************************************/ + +struct pwm_out_s { + struct timer timer; + struct gpio_pwm pin; + uint32_t max_duration; + uint8_t value, default_value; +}; + +static uint8_t +pwm_event(struct timer *timer) +{ + struct pwm_out_s *p = container_of(timer, struct pwm_out_s, timer); + gpio_pwm_write(p->pin, p->value); + if (p->value == p->default_value || !p->max_duration) + return SF_DONE; + p->timer.waketime += p->max_duration; + p->timer.func = digital_end_event; + return SF_RESCHEDULE; +} + +void +command_config_pwm_out(uint32_t *args) +{ + struct pwm_out_s *p = alloc_oid(args[0], command_config_pwm_out, sizeof(*p)); + p->default_value = args[3]; + p->pin = gpio_pwm_setup(args[1], args[2], p->default_value); + p->max_duration = args[4]; +} +DECL_COMMAND(command_config_pwm_out, + "config_pwm_out oid=%c pin=%u cycle_ticks=%u default_value=%c" + " max_duration=%u"); + +void +command_schedule_pwm_out(uint32_t *args) +{ + struct pwm_out_s *p = lookup_oid(args[0], command_config_pwm_out); + sched_del_timer(&p->timer); + p->timer.func = pwm_event; + p->timer.waketime = args[1]; + p->value = args[2]; + sched_timer(&p->timer); +} +DECL_COMMAND(command_schedule_pwm_out, + "schedule_pwm_out oid=%c clock=%u value=%c"); + +static void +pwm_shutdown(void) +{ + uint8_t i; + struct pwm_out_s *p; + foreach_oid(i, p, command_config_pwm_out) { + gpio_pwm_write(p->pin, p->default_value); + } +} +DECL_SHUTDOWN(pwm_shutdown); + +void +command_set_pwm_out(uint32_t *args) +{ + gpio_pwm_setup(args[0], args[1], args[2]); +} +DECL_COMMAND(command_set_pwm_out, "set_pwm_out pin=%u cycle_ticks=%u value=%c"); + + +/**************************************************************** + * Soft PWM output pins + ****************************************************************/ + +struct soft_pwm_s { + struct timer timer; + uint32_t on_duration, off_duration, end_time; + uint32_t next_on_duration, next_off_duration; + uint32_t max_duration, cycle_time, pulse_time; + struct gpio_out pin; + uint8_t default_value, flags; +}; + +enum { + SPF_ON=1<<0, SPF_TOGGLING=1<<1, SPF_CHECK_END=1<<2, SPF_HAVE_NEXT=1<<3, + SPF_NEXT_ON=1<<4, SPF_NEXT_TOGGLING=1<<5, SPF_NEXT_CHECK_END=1<<6, +}; + +static uint8_t soft_pwm_load_event(struct timer *timer); + +// Normal pulse change event +static uint8_t +soft_pwm_toggle_event(struct timer *timer) +{ + struct soft_pwm_s *s = container_of(timer, struct soft_pwm_s, timer); + gpio_out_toggle(s->pin); + s->flags ^= SPF_ON; + uint32_t waketime = s->timer.waketime; + if (s->flags & SPF_ON) + waketime += s->on_duration; + else + waketime += s->off_duration; + if (s->flags & SPF_CHECK_END && !sched_is_before(waketime, s->end_time)) { + // End of normal pulsing - next event loads new pwm settings + s->timer.func = soft_pwm_load_event; + waketime = s->end_time; + } + s->timer.waketime = waketime; + return SF_RESCHEDULE; +} + +// Load next pwm settings +static uint8_t +soft_pwm_load_event(struct timer *timer) +{ + struct soft_pwm_s *s = container_of(timer, struct soft_pwm_s, timer); + if (!(s->flags & SPF_HAVE_NEXT)) + shutdown("Missed scheduling of next pwm event"); + uint8_t flags = s->flags >> 4; + s->flags = flags; + gpio_out_write(s->pin, flags & SPF_ON); + if (!(flags & SPF_TOGGLING)) { + // Pin is in an always on (value=255) or always off (value=0) state + if (!(flags & SPF_CHECK_END)) + return SF_DONE; + s->timer.waketime = s->end_time = s->end_time + s->max_duration; + return SF_RESCHEDULE; + } + // Schedule normal pin toggle timer events + s->timer.func = soft_pwm_toggle_event; + s->off_duration = s->next_off_duration; + s->on_duration = s->next_on_duration; + s->timer.waketime = s->end_time + s->on_duration; + s->end_time += s->max_duration; + return SF_RESCHEDULE; +} + +void +command_config_soft_pwm_out(uint32_t *args) +{ + struct soft_pwm_s *s = alloc_oid(args[0], command_config_soft_pwm_out + , sizeof(*s)); + s->cycle_time = args[2]; + s->pulse_time = s->cycle_time / 255; + s->default_value = !!args[3]; + s->max_duration = args[4]; + s->flags = s->default_value ? SPF_ON : 0; + s->pin = gpio_out_setup(args[1], s->default_value); +} +DECL_COMMAND(command_config_soft_pwm_out, + "config_soft_pwm_out oid=%c pin=%u cycle_ticks=%u default_value=%c" + " max_duration=%u"); + +void +command_schedule_soft_pwm_out(uint32_t *args) +{ + struct soft_pwm_s *s = lookup_oid(args[0], command_config_soft_pwm_out); + uint32_t time = args[1]; + uint8_t value = args[2]; + uint8_t next_flags = SPF_CHECK_END | SPF_HAVE_NEXT; + uint32_t next_on_duration, next_off_duration; + if (value == 0 || value == 255) { + next_on_duration = next_off_duration = 0; + next_flags |= value ? SPF_NEXT_ON : 0; + if (!!value != s->default_value && s->max_duration) + next_flags |= SPF_NEXT_CHECK_END; + } else { + next_on_duration = s->pulse_time * value; + next_off_duration = s->cycle_time - next_on_duration; + next_flags |= SPF_NEXT_ON | SPF_NEXT_TOGGLING; + if (s->max_duration) + next_flags |= SPF_NEXT_CHECK_END; + } + uint8_t flag = irq_save(); + if (s->flags & SPF_CHECK_END && sched_is_before(s->end_time, time)) + shutdown("next soft pwm extends existing pwm"); + s->end_time = time; + s->next_on_duration = next_on_duration; + s->next_off_duration = next_off_duration; + s->flags |= next_flags; + if (s->flags & SPF_TOGGLING && sched_is_before(s->timer.waketime, time)) { + // soft_pwm_toggle_event() will schedule a load event when ready + } else { + // Schedule the loading of the pwm parameters at the requested time + sched_del_timer(&s->timer); + s->timer.waketime = time; + s->timer.func = soft_pwm_load_event; + sched_timer(&s->timer); + } + irq_restore(flag); +} +DECL_COMMAND(command_schedule_soft_pwm_out, + "schedule_soft_pwm_out oid=%c clock=%u value=%c"); + +static void +soft_pwm_shutdown(void) +{ + uint8_t i; + struct soft_pwm_s *s; + foreach_oid(i, s, command_config_soft_pwm_out) { + gpio_out_write(s->pin, s->default_value); + s->flags = s->default_value ? SPF_ON : 0; + } +} +DECL_SHUTDOWN(soft_pwm_shutdown); + + +/**************************************************************** + * Analog input pins + ****************************************************************/ + +struct analog_in { + struct timer timer; + uint32_t rest_time, sample_time, next_begin_time; + uint16_t value, min_value, max_value; + struct gpio_adc pin; + uint8_t state, sample_count; +}; + +static uint8_t +analog_in_event(struct timer *timer) +{ + struct analog_in *a = container_of(timer, struct analog_in, timer); + if (gpio_adc_sample(a->pin)) { + a->timer.waketime += gpio_adc_sample_time(); + return SF_RESCHEDULE; + } + uint16_t value = gpio_adc_read(a->pin); + uint8_t state = a->state; + if (state >= a->sample_count) { + state = 0; + } else { + value += a->value; + } + a->value = value; + a->state = state+1; + if (a->state < a->sample_count) { + a->timer.waketime += a->sample_time; + return SF_RESCHEDULE; + } + if (a->value < a->min_value || a->value > a->max_value) + shutdown("adc out of range"); + a->next_begin_time += a->rest_time; + a->timer.waketime = a->next_begin_time; + return SF_RESCHEDULE; +} + +void +command_config_analog_in(uint32_t *args) +{ + struct analog_in *a = alloc_oid( + args[0], command_config_analog_in, sizeof(*a)); + a->timer.func = analog_in_event; + a->pin = gpio_adc_setup(args[1]); + a->state = 1; +} +DECL_COMMAND(command_config_analog_in, "config_analog_in oid=%c pin=%u"); + +void +command_query_analog_in(uint32_t *args) +{ + struct analog_in *a = lookup_oid(args[0], command_config_analog_in); + sched_del_timer(&a->timer); + gpio_adc_clear_sample(a->pin); + a->next_begin_time = args[1]; + a->timer.waketime = a->next_begin_time; + a->sample_time = args[2]; + a->sample_count = args[3]; + a->state = a->sample_count + 1; + a->rest_time = args[4]; + a->min_value = args[5]; + a->max_value = args[6]; + if (! a->sample_count) + return; + sched_timer(&a->timer); +} +DECL_COMMAND(command_query_analog_in, + "query_analog_in oid=%c clock=%u sample_ticks=%u sample_count=%c" + " rest_ticks=%u min_value=%hu max_value=%hu"); + +static void +analog_in_task(void) +{ + static uint16_t next; + if (!sched_check_periodic(3, &next)) + return; + uint8_t oid; + struct analog_in *a; + foreach_oid(oid, a, command_config_analog_in) { + if (a->state != a->sample_count) + continue; + uint8_t flag = irq_save(); + if (a->state != a->sample_count) { + irq_restore(flag); + continue; + } + uint16_t value = a->value; + uint32_t next_begin_time = a->next_begin_time; + a->state++; + irq_restore(flag); + sendf("analog_in_state oid=%c next_clock=%u value=%hu" + , oid, next_begin_time, value); + } +} +DECL_TASK(analog_in_task); + +static void +analog_in_shutdown(void) +{ + uint8_t i; + struct analog_in *a; + foreach_oid(i, a, command_config_analog_in) { + gpio_adc_clear_sample(a->pin); + } +} +DECL_SHUTDOWN(analog_in_shutdown); diff --git a/src/sched.c b/src/sched.c new file mode 100644 index 000000000..2f51515c0 --- /dev/null +++ b/src/sched.c @@ -0,0 +1,282 @@ +// Basic scheduling functions and startup/shutdown code. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <setjmp.h> // setjmp +#include <stdarg.h> // va_list +#include <stddef.h> // NULL +#include "autoconf.h" // CONFIG_* +#include "board/irq.h" // irq_save +#include "board/timer.h" // timer_from_ms +#include "command.h" // shutdown +#include "sched.h" // sched_from_ms +#include "stepper.h" // stepper_event + + +/**************************************************************** + * Timers + ****************************************************************/ + +static uint16_t millis; + +// Default millisecond timer. This timer counts milliseconds. It +// also simplifies the timer code by ensuring there is always at least +// one timer on the timer list and that there is always a timer not +// more than 1 ms in the future. +static uint8_t +ms_event(struct timer *t) +{ + millis++; + timer_periodic(); + t->waketime += sched_from_ms(1); + return SF_RESCHEDULE; +} + +static struct timer ms_timer = { + .func = ms_event +}; + +// Check if ready for a recurring periodic event +uint8_t +sched_check_periodic(uint16_t time, uint16_t *pnext) +{ + uint16_t next = *pnext, cur; + uint8_t flag = irq_save(); + cur = millis; + irq_restore(flag); + if ((int16_t)(cur - next) < 0) + return 0; + *pnext = cur + time; + return 1; +} + +// Return the number of clock ticks for a given number of milliseconds +uint32_t +sched_from_ms(uint32_t ms) +{ + return timer_from_ms(ms); +} + +// Return the current time (in clock ticks) +uint32_t +sched_read_time(void) +{ + return timer_read_time(); +} + +// Return true if time1 is before time2. Always use this function to +// compare times as regular C comparisons can fail if the counter +// rolls over. +uint8_t +sched_is_before(uint32_t time1, uint32_t time2) +{ + return (int32_t)(time1 - time2) < 0; +} + +static struct timer *timer_list = &ms_timer; + +// Add a timer to timer list. +static __always_inline void +add_timer(struct timer *add) +{ + struct timer **timep = &timer_list, *t = timer_list; + while (t && !sched_is_before(add->waketime, t->waketime)) { + timep = &t->next; + t = t->next; + } + add->next = t; + *timep = add; +} + +// Schedule a function call at a supplied time. +void +sched_timer(struct timer *add) +{ + uint8_t flag = irq_save(); + add_timer(add); + + // Reschedule timer if necessary. + if (timer_list == add) { + uint8_t ret = timer_set_next(add->waketime); + if (ret) + shutdown("Timer too close"); + } + + irq_restore(flag); +} + +// Remove a timer that may be live. +void +sched_del_timer(struct timer *del) +{ + uint8_t flag = irq_save(); + + if (timer_list == del) { + timer_list = del->next; + timer_set_next(timer_list->waketime); + irq_restore(flag); + return; + } + + // Find and remove from timer list. + struct timer *prev = timer_list; + for (;;) { + struct timer *t = prev->next; + if (!t) + break; + if (t == del) { + prev->next = del->next; + break; + } + prev = t; + } + + irq_restore(flag); +} + +// Invoke timers - called from board timer irq code. +void +sched_timer_kick(void) +{ + struct timer *t = timer_list; + for (;;) { + // Invoke timer callback + uint8_t res; + if (CONFIG_INLINE_STEPPER_HACK && likely(!t->func)) + res = stepper_event(t); + else + res = t->func(t); + + // Update timer_list (rescheduling current timer if necessary) + timer_list = t->next; + if (likely(res)) + add_timer(t); + t = timer_list; + + // Schedule next timer event (or run next timer if it's ready) + res = timer_try_set_next(t->waketime); + if (res) + break; + } +} + +// Shutdown all user timers on an emergency stop. +static void +timer_shutdown(void) +{ + timer_list = &ms_timer; + ms_timer.next = NULL; + timer_set_next(timer_list->waketime); +} +DECL_SHUTDOWN(timer_shutdown); + + +/**************************************************************** + * Shutdown processing + ****************************************************************/ + +static uint16_t shutdown_reason; +static uint8_t shutdown_status; + +// Return true if the machine is in an emergency stop state +uint8_t +sched_is_shutdown(void) +{ + return !!shutdown_status; +} + +uint16_t +sched_shutdown_reason(void) +{ + return shutdown_reason; +} + +// Transition out of shutdown state +void +sched_clear_shutdown(void) +{ + if (!shutdown_status) + shutdown("Shutdown cleared when not shutdown"); + if (shutdown_status == 2) + // Ignore attempt to clear shutdown if still processing shutdown + return; + shutdown_status = 0; +} + +// Invoke all shutdown functions (as declared by DECL_SHUTDOWN) +static void +run_shutdown(void) +{ + shutdown_status = 2; + struct callback_handler *p; + foreachdecl(p, shutdownfuncs) { + void (*func)(void) = READP(p->func); + func(); + } + shutdown_status = 1; + irq_enable(); + + sendf("shutdown static_string_id=%hu", shutdown_reason); +} + +// Shutdown the machine if not already in the process of shutting down +void +sched_try_shutdown(uint16_t reason) +{ + if (shutdown_status != 2) + sched_shutdown(reason); +} + +static jmp_buf shutdown_jmp; + +// Force the machine to immediately run the shutdown handlers +void +sched_shutdown(uint16_t reason) +{ + irq_disable(); + shutdown_reason = reason; + longjmp(shutdown_jmp, 1); +} + + +/**************************************************************** + * Startup and background task processing + ****************************************************************/ + +// Invoke all init functions (as declared by DECL_INIT) +static void +run_init(void) +{ + struct callback_handler *p; + foreachdecl(p, initfuncs) { + void (*func)(void) = READP(p->func); + func(); + } +} + +// Invoke all background task functions (as declared by DECL_TASK) +static void +run_task(void) +{ + struct callback_handler *p; + foreachdecl(p, taskfuncs) { + void (*func)(void) = READP(p->func); + func(); + } +} + +// Main loop of program +void +sched_main(void) +{ + run_init(); + + int ret = setjmp(shutdown_jmp); + if (ret) + run_shutdown(); + + for (;;) + run_task(); +} diff --git a/src/sched.h b/src/sched.h new file mode 100644 index 000000000..047ac1ce6 --- /dev/null +++ b/src/sched.h @@ -0,0 +1,51 @@ +#ifndef __SCHED_H +#define __SCHED_H + +#include <stdint.h> +#include "board/pgm.h" // PSTR +#include "compiler.h" // __section + +// Declare an init function (called at firmware startup) +#define DECL_INIT(FUNC) _DECL_CALLBACK(initfuncs, FUNC) +// Declare a task function (called periodically during normal runtime) +#define DECL_TASK(FUNC) _DECL_CALLBACK(taskfuncs, FUNC) +// Declare a shutdown function (called on an emergency stop) +#define DECL_SHUTDOWN(FUNC) _DECL_CALLBACK(shutdownfuncs, FUNC) + +// Timer structure for scheduling timed events (see sched_timer() ) +struct timer { + struct timer *next; + uint8_t (*func)(struct timer*); + uint32_t waketime; +}; + +enum { SF_DONE=0, SF_RESCHEDULE=1 }; + +// sched.c +uint8_t sched_check_periodic(uint16_t time, uint16_t *pnext); +uint32_t sched_from_ms(uint32_t ms); +uint32_t sched_read_time(void); +uint8_t sched_is_before(uint32_t time1, uint32_t time2); +void sched_timer(struct timer*); +void sched_del_timer(struct timer *del); +void sched_timer_kick(void); +uint8_t sched_is_shutdown(void); +uint16_t sched_shutdown_reason(void); +void sched_clear_shutdown(void); +void sched_try_shutdown(uint16_t reason); +void sched_shutdown(uint16_t reason) __noreturn; +void sched_main(void); + +// Compiler glue for DECL_X macros above. +struct callback_handler { + void (*func)(void); +}; +#define _DECL_CALLBACK(NAME, FUNC) \ + const struct callback_handler _DECL_ ## NAME ## _ ## FUNC __visible \ + __section(".progmem.data." __stringify(NAME) ) = { .func = FUNC } + +#define foreachdecl(ITER, NAME) \ + extern typeof(*ITER) NAME ## _start[], NAME ## _end[]; \ + for (ITER = NAME ## _start ; ITER < NAME ## _end ; ITER ++) + +#endif // sched.h diff --git a/src/simulator/Kconfig b/src/simulator/Kconfig new file mode 100644 index 000000000..539cb0aa8 --- /dev/null +++ b/src/simulator/Kconfig @@ -0,0 +1,10 @@ +# Kconfig settings for compiling and running the firmware on the host +# processor for simulation purposes. + +if MACH_SIMU + +config BOARD_DIRECTORY + string + default "simulator" + +endif diff --git a/src/simulator/Makefile b/src/simulator/Makefile new file mode 100644 index 000000000..0dbcdab44 --- /dev/null +++ b/src/simulator/Makefile @@ -0,0 +1,3 @@ +# Additional simulator build rules + +src-y += simulator/main.c simulator/gpio.c diff --git a/src/simulator/gpio.c b/src/simulator/gpio.c new file mode 100644 index 000000000..90d684e93 --- /dev/null +++ b/src/simulator/gpio.c @@ -0,0 +1,45 @@ +// GPIO functions on simulator. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "gpio.h" // gpio_out_write + +struct gpio_out gpio_out_setup(uint8_t pin, uint8_t val) { + return (struct gpio_out){.pin=pin}; +} +void gpio_out_toggle(struct gpio_out g) { +} +void gpio_out_write(struct gpio_out g, uint8_t val) { +} +struct gpio_in gpio_in_setup(uint8_t pin, int8_t pull_up) { + return (struct gpio_in){.pin=pin}; +} +uint8_t gpio_in_read(struct gpio_in g) { + return 0; +} +struct gpio_pwm gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint8_t val) { + return (struct gpio_pwm){.pin=pin}; +} +void gpio_pwm_write(struct gpio_pwm g, uint8_t val) { +} +struct gpio_adc gpio_adc_setup(uint8_t pin) { + return (struct gpio_adc){.pin=pin}; +} +uint32_t gpio_adc_sample_time(void) { + return 0; +} +uint8_t gpio_adc_sample(struct gpio_adc g) { + return 0; +} +void gpio_adc_clear_sample(struct gpio_adc g) { +} +uint16_t gpio_adc_read(struct gpio_adc g) { + return 0; +} + +void spi_config(void) { +} +void spi_transfer(char *data, uint8_t len) { +} diff --git a/src/simulator/gpio.h b/src/simulator/gpio.h new file mode 100644 index 000000000..84deb7ef8 --- /dev/null +++ b/src/simulator/gpio.h @@ -0,0 +1,37 @@ +#ifndef __SIMU_GPIO_H +#define __SIMU_GPIO_H + +#include <stdint.h> + +struct gpio_out { + uint8_t pin; +}; +struct gpio_out gpio_out_setup(uint8_t pin, uint8_t val); +void gpio_out_toggle(struct gpio_out g); +void gpio_out_write(struct gpio_out g, uint8_t val); + +struct gpio_in { + uint8_t pin; +}; +struct gpio_in gpio_in_setup(uint8_t pin, int8_t pull_up); +uint8_t gpio_in_read(struct gpio_in g); + +struct gpio_pwm { + uint8_t pin; +}; +struct gpio_pwm gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint8_t val); +void gpio_pwm_write(struct gpio_pwm g, uint8_t val); + +struct gpio_adc { + uint8_t pin; +}; +struct gpio_adc gpio_adc_setup(uint8_t pin); +uint32_t gpio_adc_sample_time(void); +uint8_t gpio_adc_sample(struct gpio_adc g); +void gpio_adc_clear_sample(struct gpio_adc g); +uint16_t gpio_adc_read(struct gpio_adc g); + +void spi_config(void); +void spi_transfer(char *data, uint8_t len); + +#endif // gpio.h diff --git a/src/simulator/irq.h b/src/simulator/irq.h new file mode 100644 index 000000000..63f512908 --- /dev/null +++ b/src/simulator/irq.h @@ -0,0 +1,31 @@ +#ifndef __SIMU_IRQ_H +#define __SIMU_IRQ_H +// Definitions for irq enable/disable on host simulator + +#include <stdint.h> +#include "compiler.h" // barrier + +extern uint8_t Interrupt_off; + +static inline void irq_disable(void) { + Interrupt_off = 1; + barrier(); +} + +static inline void irq_enable(void) { + barrier(); + Interrupt_off = 0; +} + +static inline uint8_t irq_save(void) { + uint8_t flag = Interrupt_off; + irq_disable(); + return flag; +} + +static inline void irq_restore(uint8_t flag) { + barrier(); + Interrupt_off = flag; +} + +#endif // irq.h diff --git a/src/simulator/main.c b/src/simulator/main.c new file mode 100644 index 000000000..2d43a138b --- /dev/null +++ b/src/simulator/main.c @@ -0,0 +1,102 @@ +// Main starting point for host simulator. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <fcntl.h> +#include <stdio.h> +#include <unistd.h> +#include "sched.h" // sched_main + +uint8_t Interrupt_off; + + +/**************************************************************** + * Timers + ****************************************************************/ + +uint32_t +timer_from_ms(uint32_t ms) +{ + return 0; // XXX +} + +void +timer_periodic(void) +{ +} + +uint32_t +timer_read_time(void) +{ + return 0; // XXX +} + +uint8_t +timer_set_next(uint32_t next) +{ + return 0; +} + +uint8_t +timer_try_set_next(uint32_t next) +{ + return 1; +} + + +/**************************************************************** + * Turn stdin/stdout into serial console + ****************************************************************/ + +// XXX +char * +console_get_input(uint8_t *plen) +{ + *plen = 0; + return NULL; +} + +void +console_pop_input(uint8_t len) +{ +} + +// Return an output buffer that the caller may fill with transmit messages +char * +console_get_output(uint8_t len) +{ + return NULL; +} + +// Accept the given number of bytes added to the transmit buffer +void +console_push_output(uint8_t len) +{ +} + + +/**************************************************************** + * Startup + ****************************************************************/ + +// Periodically sleep so we don't consume all CPU +static void +simu_pause(void) +{ + // XXX - should check that no timers are present. + usleep(1); +} +DECL_TASK(simu_pause); + +// Main entry point for simulator. +int +main(void) +{ + // Make stdin non-blocking + fcntl(STDIN_FILENO, F_SETFL, fcntl(STDIN_FILENO, F_GETFL, 0) | O_NONBLOCK); + + sched_main(); + return 0; +} diff --git a/src/simulator/misc.h b/src/simulator/misc.h new file mode 100644 index 000000000..c279794e3 --- /dev/null +++ b/src/simulator/misc.h @@ -0,0 +1,21 @@ +#ifndef __SIMU_MISC_H +#define __SIMU_MISC_H + +#include <stdint.h> + +// main.c +char *console_get_input(uint8_t *plen); +void console_pop_input(uint8_t len); +char *console_get_output(uint8_t len); +void console_push_output(uint8_t len); + +static inline size_t alloc_maxsize(size_t reqsize) { + return reqsize; +} + +#define HAVE_OPTIMIZED_CRC 0 +static inline uint16_t _crc16_ccitt(char *buf, uint8_t len) { + return 0; +} + +#endif // misc.h diff --git a/src/simulator/pgm.h b/src/simulator/pgm.h new file mode 100644 index 000000000..e5f3787d5 --- /dev/null +++ b/src/simulator/pgm.h @@ -0,0 +1,13 @@ +#ifndef __SIMU_PGM_H +#define __SIMU_PGM_H +// This header provides wrappers for the AVR specific "PROGMEM" +// declarations. + +#define PROGMEM +#define PSTR(S) S +#define READP(VAR) VAR +#define vsnprintf_P(D, S, F, A) vsnprintf(D, S, F, A) +#define strcasecmp_P(S1, S2) strcasecmp(S1, S2) +#define memcpy_P(DST, SRC, SIZE) memcpy((DST), (SRC), (SIZE)) + +#endif // pgm.h diff --git a/src/simulator/timer.h b/src/simulator/timer.h new file mode 100644 index 000000000..f35b82785 --- /dev/null +++ b/src/simulator/timer.h @@ -0,0 +1,12 @@ +#ifndef __SIMU_TIMER_H +#define __SIMU_TIMER_H + +#include <stdint.h> + +uint32_t timer_from_ms(uint32_t ms); +void timer_periodic(void); +uint32_t timer_read_time(void); +uint8_t timer_set_next(uint32_t next); +uint8_t timer_try_set_next(uint32_t next); + +#endif // timer.h diff --git a/src/spicmds.c b/src/spicmds.c new file mode 100644 index 000000000..bb37be2bf --- /dev/null +++ b/src/spicmds.c @@ -0,0 +1,22 @@ +// Commands for sending messages on an SPI bus +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "board/gpio.h" // gpio_out_write +#include "command.h" // DECL_COMMAND + +void +command_send_spi_message(uint32_t *args) +{ + // For now, this only implements enough to program an ad5206 digipot + uint8_t len = args[1]; + char *msg = (void*)(size_t)args[2]; + spi_config(); + struct gpio_out pin = gpio_out_setup(args[0], 0); + spi_transfer(msg, len); + gpio_out_write(pin, 1); + sendf("spi_response response=%*s", len, msg); +} +DECL_COMMAND(command_send_spi_message, "send_spi_message pin=%u msg=%*s"); diff --git a/src/stepper.c b/src/stepper.c new file mode 100644 index 000000000..4679bf709 --- /dev/null +++ b/src/stepper.c @@ -0,0 +1,202 @@ +// Handling of stepper drivers. +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <stddef.h> // NULL +#include "autoconf.h" // CONFIG_* +#include "basecmd.h" // alloc_oid +#include "board/gpio.h" // gpio_out_write +#include "board/irq.h" // irq_save +#include "command.h" // DECL_COMMAND +#include "sched.h" // struct timer +#include "stepper.h" // command_config_stepper + + +/**************************************************************** + * Steppers + ****************************************************************/ + +struct stepper { + struct timer time; + uint32_t interval; + int16_t add; + uint16_t count; + struct gpio_out step_pin, dir_pin; + uint32_t position; + struct move *first, **plast; + uint32_t min_stop_interval; + // gcc (pre v6) does better optimization when uint8_t are bitfields + uint8_t flags : 8; +}; + +enum { MF_DIR=1 }; +enum { SF_LAST_DIR=1, SF_NEXT_DIR=2, SF_INVERT_STEP=4 }; + +// Setup a stepper for the next move in its queue +static uint8_t +stepper_load_next(struct stepper *s) +{ + struct move *m = s->first; + if (!m) { + if (s->interval - s->add < s->min_stop_interval) + shutdown("No next step"); + s->count = 0; + return SF_DONE; + } + + s->interval = m->interval; + s->time.waketime += s->interval; + s->add = m->add; + s->interval += s->add; + s->count = m->count; + if (m->flags & MF_DIR) { + s->position = -s->position + s->count; + gpio_out_toggle(s->dir_pin); + } else { + s->position += s->count; + } + + s->first = m->next; + move_free(m); + return SF_RESCHEDULE; +} + +// Timer callback - step the given stepper. +uint8_t +stepper_event(struct timer *t) +{ + struct stepper *s = container_of(t, struct stepper, time); + gpio_out_toggle(s->step_pin); + uint16_t count = s->count - 1; + if (likely(count)) { + s->count = count; + s->time.waketime += s->interval; + s->interval += s->add; + gpio_out_toggle(s->step_pin); + return SF_RESCHEDULE; + } + uint8_t ret = stepper_load_next(s); + gpio_out_toggle(s->step_pin); + return ret; +} + +void +command_config_stepper(uint32_t *args) +{ + struct stepper *s = alloc_oid(args[0], command_config_stepper, sizeof(*s)); + if (!CONFIG_INLINE_STEPPER_HACK) + s->time.func = stepper_event; + s->flags = args[4] ? SF_INVERT_STEP : 0; + s->step_pin = gpio_out_setup(args[1], s->flags & SF_INVERT_STEP ? 1 : 0); + s->dir_pin = gpio_out_setup(args[2], 0); + s->min_stop_interval = args[3]; + s->position = STEPPER_POSITION_BIAS; +} +DECL_COMMAND(command_config_stepper, + "config_stepper oid=%c step_pin=%c dir_pin=%c" + " min_stop_interval=%u invert_step=%c"); + +// Schedule a set of steps with a given timing +void +command_queue_step(uint32_t *args) +{ + struct stepper *s = lookup_oid(args[0], command_config_stepper); + struct move *m = move_alloc(); + m->flags = 0; + if (!!(s->flags & SF_LAST_DIR) != !!(s->flags & SF_NEXT_DIR)) { + s->flags ^= SF_LAST_DIR; + m->flags |= MF_DIR; + } + m->interval = args[1]; + m->count = args[2]; + if (!m->count) + shutdown("Invalid count parameter"); + m->add = args[3]; + m->next = NULL; + + uint8_t flag = irq_save(); + if (s->count) { + if (s->first) + *s->plast = m; + else + s->first = m; + s->plast = &m->next; + } else { + s->first = m; + stepper_load_next(s); + sched_timer(&s->time); + } + irq_restore(flag); +} +DECL_COMMAND(command_queue_step, + "queue_step oid=%c interval=%u count=%hu add=%hi"); + +// Set the direction of the next queued step +void +command_set_next_step_dir(uint32_t *args) +{ + struct stepper *s = lookup_oid(args[0], command_config_stepper); + s->flags = (s->flags & ~SF_NEXT_DIR) | (args[1] ? SF_NEXT_DIR : 0); +} +DECL_COMMAND(command_set_next_step_dir, "set_next_step_dir oid=%c dir=%c"); + +// Set an absolute time that the next step will be relative to +void +command_reset_step_clock(uint32_t *args) +{ + struct stepper *s = lookup_oid(args[0], command_config_stepper); + uint32_t waketime = args[1]; + if (s->count) + shutdown("Can't reset time when stepper active"); + s->time.waketime = waketime; +} +DECL_COMMAND(command_reset_step_clock, "reset_step_clock oid=%c clock=%u"); + +// Return the current stepper position. Caller must disable irqs. +uint32_t +stepper_get_position(struct stepper *s) +{ + uint32_t position = s->position - s->count; + if (position & 0x80000000) + return -position; + return position; +} + +// Reset the internal state of a 'struct stepper' +static void +stepper_reset(struct stepper *s) +{ + s->position = stepper_get_position(s); + s->count = 0; + s->flags &= SF_INVERT_STEP; + gpio_out_write(s->dir_pin, 0); +} + +// Stop all moves for a given stepper (used in end stop homing). IRQs +// must be off. +void +stepper_stop(struct stepper *s) +{ + sched_del_timer(&s->time); + stepper_reset(s); + while (s->first) { + struct move *next = s->first->next; + move_free(s->first); + s->first = next; + } +} + +static void +stepper_shutdown(void) +{ + uint8_t i; + struct stepper *s; + foreach_oid(i, s, command_config_stepper) { + stepper_reset(s); + s->first = NULL; + gpio_out_write(s->step_pin, s->flags & SF_INVERT_STEP ? 1 : 0); + } +} +DECL_SHUTDOWN(stepper_shutdown); diff --git a/src/stepper.h b/src/stepper.h new file mode 100644 index 000000000..9a81b56e2 --- /dev/null +++ b/src/stepper.h @@ -0,0 +1,14 @@ +#ifndef __STEPPER_H +#define __STEPPER_H + +#include <stdint.h> // uint8_t + +enum { STEPPER_POSITION_BIAS=0x40000000 }; + +uint8_t stepper_event(struct timer *t); +void command_config_stepper(uint32_t *args); +struct stepper; +uint32_t stepper_get_position(struct stepper *s); +void stepper_stop(struct stepper *s); + +#endif // stepper.h |