diff --git a/.gitignore b/.gitignore
new file mode 100644
index 00000000..bfb0690c
--- /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 00000000..94a9ed02
--- /dev/null
+++ b/COPYING
@@ -0,0 +1,674 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc.
+ 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.
+
+
+ Copyright (C)
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 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 .
+
+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:
+
+ Copyright (C)
+ 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
+.
+
+ 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
+.
diff --git a/Makefile b/Makefile
new file mode 100644
index 00000000..8c8967ea
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,123 @@
+# Klipper build system
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..f594c95d
--- /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 .
diff --git a/config/avrsim.cfg b/config/avrsim.cfg
new file mode 100644
index 00000000..5b1d0f74
--- /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 00000000..3090ba15
--- /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 00000000..7a3080d2
--- /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 00000000..34550e25
--- /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 00000000..ed2d92c9
--- /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 00000000..aae13511
--- /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 00000000..05311c27
--- /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 00000000..40840077
--- /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
+#
+# 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 00000000..99ec3f4f
--- /dev/null
+++ b/klippy/chelper.py
@@ -0,0 +1,95 @@
+# Wrapper around C helper code
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..4782702a
--- /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
+#
+# 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[^}]*)\}')
+
+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] "
+ 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 00000000..52374844
--- /dev/null
+++ b/klippy/fan.py
@@ -0,0 +1,39 @@
+# Printer fan support
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..fc697b98
--- /dev/null
+++ b/klippy/gcode.py
@@ -0,0 +1,315 @@
+# Parse gcode commands
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..d05035a0
--- /dev/null
+++ b/klippy/heater.py
@@ -0,0 +1,288 @@
+# Printer heater support
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..566cdf95
--- /dev/null
+++ b/klippy/homing.py
@@ -0,0 +1,82 @@
+# Code for state tracking during homing operations
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..143dd762
--- /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
+#
+# 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] "
+ 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 00000000..317a109c
--- /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 00000000..9e377463
--- /dev/null
+++ b/klippy/lookahead.py
@@ -0,0 +1,50 @@
+# Move queue look-ahead
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..728ec91d
--- /dev/null
+++ b/klippy/mcu.py
@@ -0,0 +1,510 @@
+# Multi-processor safe interface to micro-controller
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..73c8d2d5
--- /dev/null
+++ b/klippy/msgproto.py
@@ -0,0 +1,313 @@
+# Protocol definitions for firmware communication
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..0f6c48b3
--- /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
+#
+# 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 00000000..fad41643
--- /dev/null
+++ b/klippy/pins.py
@@ -0,0 +1,88 @@
+# Pin name to pin number definitions
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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[ _]pin=)(?P[^ ]*)')
+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 00000000..e07c6440
--- /dev/null
+++ b/klippy/reactor.py
@@ -0,0 +1,142 @@
+# File descriptor and timer event helper
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..80e29b96
--- /dev/null
+++ b/klippy/serialhdl.py
@@ -0,0 +1,286 @@
+# Serial port management for firmware communication
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..fa75701d
--- /dev/null
+++ b/klippy/serialqueue.c
@@ -0,0 +1,1021 @@
+// Serial port command queuing
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// 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
+#include // ceil
+#include // poll
+#include // pthread_mutex_lock
+#include // offsetof
+#include // uint64_t
+#include // snprintf
+#include // malloc
+#include // memset
+#include // gettimeofday
+#include // struct timespec
+#include // tcflush
+#include // 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; itimers[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; inum_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; inum_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 &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; inode, 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 00000000..229fb216
--- /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 00000000..d39bbc32
--- /dev/null
+++ b/klippy/stepcompress.c
@@ -0,0 +1,498 @@
+// Stepper pulse schedule compression
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// 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 // sqrt
+#include // offsetof
+#include // uint32_t
+#include // fprintf
+#include // malloc
+#include // 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; iqueue_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; isc_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; isc_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 00000000..61faaee0
--- /dev/null
+++ b/klippy/stepper.py
@@ -0,0 +1,67 @@
+# Printer stepper support
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..caac827e
--- /dev/null
+++ b/klippy/util.py
@@ -0,0 +1,32 @@
+# Low level unix utility functions
+#
+# Copyright (C) 2016 Kevin O'Connor
+#
+# 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 00000000..bbcc20b7
--- /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
+#
+# 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] "
+ 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 00000000..8b9cc9d8
--- /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
+#
+# 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] "
+ 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 00000000..d4f58cf3
--- /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
+#
+# 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' + hex_s + r') <(?P.*)>:$')
+re_asm = re.compile(
+ r'^[ ]*(?P' + hex_s
+ + r'):\t[^\t]*\t(?P[^\t]+?)(?P\t[^;]*)?'
+ + r'[ ]*(; (?P0x' + hex_s
+ + r') <(?P[.*)>)?$')
+
+def main():
+ unknownfunc = function(None, "")
+ indirectfunc = function(-1, '')
+ 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 00000000..e7460cce
--- /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 00000000..e0964676
--- /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 00000000..758443da
--- /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 00000000..aaa671bc
--- /dev/null
+++ b/src/avr/alloc.c
@@ -0,0 +1,25 @@
+// AVR allocation checking code.
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // AVR_STACK_POINTER_REG
+#include // __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 00000000..d2cdfbd8
--- /dev/null
+++ b/src/avr/gpio.c
@@ -0,0 +1,337 @@
+// GPIO functions on AVR.
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // 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< 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; chanpin) != 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<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; chanpin) != pin)
+ continue;
+
+ // Enable ADC
+ ADCSRA |= (1<= 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<> 3) & 0x01) << MUX5);
+#endif
+
+ ADMUX = ADMUX_DEFAULT | (g.chan & 0x07);
+
+ // start the conversion
+ ADCSRA |= 1<
+#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 00000000..bfc6cb51
--- /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 // 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 00000000..7651ab4a
--- /dev/null
+++ b/src/avr/main.c
@@ -0,0 +1,17 @@
+// Main starting point for AVR boards.
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// 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 00000000..244cd5bc
--- /dev/null
+++ b/src/avr/misc.h
@@ -0,0 +1,25 @@
+#ifndef __AVR_MISC_H
+#define __AVR_MISC_H
+
+#include
+#include
+
+// 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 00000000..ba68d8f9
--- /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
+
+#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 00000000..c73890dd
--- /dev/null
+++ b/src/avr/serial.c
@@ -0,0 +1,137 @@
+// AVR serial port code.
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // USART0_RX_vect
+#include // 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<= 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< 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<
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // 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< 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 00000000..2165fecd
--- /dev/null
+++ b/src/avr/timer.h
@@ -0,0 +1,12 @@
+#ifndef __AVR_TIMER_H
+#define __AVR_TIMER_H
+
+#include
+
+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 00000000..f925a8d5
--- /dev/null
+++ b/src/avr/watchdog.c
@@ -0,0 +1,38 @@
+// Initialization of AVR watchdog timer.
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // WDT_vect
+#include // 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<
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // malloc
+#include // 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= 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 00000000..e5719c6f
--- /dev/null
+++ b/src/basecmd.h
@@ -0,0 +1,23 @@
+#ifndef __BASECMD_H
+#define __BASECMD_H
+
+#include // 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 00000000..8608fbdf
--- /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
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // isspace
+#include // va_start
+#include // vsnprintf
+#include // strtod
+#include // 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)
+ 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 00000000..2bbe37f8
--- /dev/null
+++ b/src/command.h
@@ -0,0 +1,81 @@
+#ifndef __COMMAND_H
+#define __COMMAND_H
+
+#include // va_list
+#include // size_t
+#include // 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 00000000..ba4b83d5
--- /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 00000000..9bb5c8ad
--- /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 00000000..bc177605
--- /dev/null
+++ b/src/endstop.c
@@ -0,0 +1,110 @@
+// Handling of end stops.
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // 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 00000000..ff03dfd1
--- /dev/null
+++ b/src/gpiocmds.c
@@ -0,0 +1,401 @@
+// Commands for controlling GPIO pins
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // 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 00000000..2f51515c
--- /dev/null
+++ b/src/sched.c
@@ -0,0 +1,282 @@
+// Basic scheduling functions and startup/shutdown code.
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // setjmp
+#include // va_list
+#include // 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 00000000..047ac1ce
--- /dev/null
+++ b/src/sched.h
@@ -0,0 +1,51 @@
+#ifndef __SCHED_H
+#define __SCHED_H
+
+#include
+#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 00000000..539cb0aa
--- /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 00000000..0dbcdab4
--- /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 00000000..90d684e9
--- /dev/null
+++ b/src/simulator/gpio.c
@@ -0,0 +1,45 @@
+// GPIO functions on simulator.
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// 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 00000000..84deb7ef
--- /dev/null
+++ b/src/simulator/gpio.h
@@ -0,0 +1,37 @@
+#ifndef __SIMU_GPIO_H
+#define __SIMU_GPIO_H
+
+#include
+
+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 00000000..63f51290
--- /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
+#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 00000000..2d43a138
--- /dev/null
+++ b/src/simulator/main.c
@@ -0,0 +1,102 @@
+// Main starting point for host simulator.
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include
+#include
+#include
+#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 00000000..c279794e
--- /dev/null
+++ b/src/simulator/misc.h
@@ -0,0 +1,21 @@
+#ifndef __SIMU_MISC_H
+#define __SIMU_MISC_H
+
+#include
+
+// 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 00000000..e5f3787d
--- /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 00000000..f35b8278
--- /dev/null
+++ b/src/simulator/timer.h
@@ -0,0 +1,12 @@
+#ifndef __SIMU_TIMER_H
+#define __SIMU_TIMER_H
+
+#include
+
+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 00000000..bb37be2b
--- /dev/null
+++ b/src/spicmds.c
@@ -0,0 +1,22 @@
+// Commands for sending messages on an SPI bus
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// 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 00000000..4679bf70
--- /dev/null
+++ b/src/stepper.c
@@ -0,0 +1,202 @@
+// Handling of stepper drivers.
+//
+// Copyright (C) 2016 Kevin O'Connor
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include // 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 00000000..9a81b56e
--- /dev/null
+++ b/src/stepper.h
@@ -0,0 +1,14 @@
+#ifndef __STEPPER_H
+#define __STEPPER_H
+
+#include // 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
]