Add GDB stub code

10 files changed, 2084 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
index e69de29..33a8b1f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -0,0 +1,4 @@
+*.bin
+*.elf
+*.ld
+*.o
diff --git a/LICENSE.txt b/LICENSE.txt
new file mode 100644
index 0000000..92919f4
--- /dev/null
+++ b/LICENSE.txt
@@ -0,0 +1,339 @@
+  GNU GENERAL PUBLIC LICENSE
+                       Version 2, June 1991
+
+ Copyright (C) 1989, 1991 Free Software Foundation, Inc., <http://fsf.org/>
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+License is intended to guarantee your freedom to share and change free
+software--to make sure the software is free for all its users.  This
+General Public License applies to most of the Free Software
+Foundation's software and to any other program whose authors commit to
+using it.  (Some other Free Software Foundation software is covered by
+the GNU Lesser General Public License instead.)  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+this service if you wish), that you receive source code or can get it
+if you want it, that you can change the software or use pieces of it
+in new free programs; and that you know you can do these things.
+
+  To protect your rights, we need to make restrictions that forbid
+anyone to deny you these rights or to ask you to surrender the rights.
+These restrictions translate to certain responsibilities for you if you
+distribute copies of the software, or if you modify it.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must give the recipients all the rights that
+you have.  You must make sure that they, too, receive or can get the
+source code.  And you must show them these terms so they know their
+rights.
+
+  We protect your rights with two steps: (1) copyright the software, and
+(2) offer you this license which gives you legal permission to copy,
+distribute and/or modify the software.
+
+  Also, for each author's protection and ours, we want to make certain
+that everyone understands that there is no warranty for this free
+software.  If the software is modified by someone else and passed on, we
+want its recipients to know that what they have is not the original, so
+that any problems introduced by others will not reflect on the original
+authors' reputations.
+
+  Finally, any free program is threatened constantly by software
+patents.  We wish to avoid the danger that redistributors of a free
+program will individually obtain patent licenses, in effect making the
+program proprietary.  To prevent this, we have made it clear that any
+patent must be licensed for everyone's free use or not licensed at all.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                    GNU GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License applies to any program or other work which contains
+a notice placed by the copyright holder saying it may be distributed
+under the terms of this General Public License.  The "Program", below,
+refers to any such program or work, and a "work based on the Program"
+means either the Program or any derivative work under copyright law:
+that is to say, a work containing the Program or a portion of it,
+either verbatim or with modifications and/or translated into another
+language.  (Hereinafter, translation is included without limitation in
+the term "modification".)  Each licensee is addressed as "you".
+
+Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running the Program is not restricted, and the output from the Program
+is covered only if its contents constitute a work based on the
+Program (independent of having been made by running the Program).
+Whether that is true depends on what the Program does.
+
+  1. You may copy and distribute verbatim copies of the Program's
+source code as you receive it, in any medium, provided that you
+conspicuously and appropriately publish on each copy an appropriate
+copyright notice and disclaimer of warranty; keep intact all the
+notices that refer to this License and to the absence of any warranty;
+and give any other recipients of the Program a copy of this License
+along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and
+you may at your option offer warranty protection in exchange for a fee.
+
+  2. You may modify your copy or copies of the Program or any portion
+of it, thus forming a work based on the Program, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) You must cause the modified files to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    b) You must cause any work that you distribute or publish, that in
+    whole or in part contains or is derived from the Program or any
+    part thereof, to be licensed as a whole at no charge to all third
+    parties under the terms of this License.
+
+    c) If the modified program normally reads commands interactively
+    when run, you must cause it, when started running for such
+    interactive use in the most ordinary way, to print or display an
+    announcement including an appropriate copyright notice and a
+    notice that there is no warranty (or else, saying that you provide
+    a warranty) and that users may redistribute the program under
+    these conditions, and telling the user how to view a copy of this
+    License.  (Exception: if the Program itself is interactive but
+    does not normally print such an announcement, your work based on
+    the Program is not required to print an announcement.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Program,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Program, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Program.
+
+In addition, mere aggregation of another work not based on the Program
+with the Program (or with a work based on the Program) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may copy and distribute the Program (or a work based on it,
+under Section 2) in object code or executable form under the terms of
+Sections 1 and 2 above provided that you also do one of the following:
+
+    a) Accompany it with the complete corresponding machine-readable
+    source code, which must be distributed under the terms of Sections
+    1 and 2 above on a medium customarily used for software interchange; or,
+
+    b) Accompany it with a written offer, valid for at least three
+    years, to give any third party, for a charge no more than your
+    cost of physically performing source distribution, a complete
+    machine-readable copy of the corresponding source code, to be
+    distributed under the terms of Sections 1 and 2 above on a medium
+    customarily used for software interchange; or,
+
+    c) Accompany it with the information you received as to the offer
+    to distribute corresponding source code.  (This alternative is
+    allowed only for noncommercial distribution and only if you
+    received the program in object code or executable form with such
+    an offer, in accord with Subsection b above.)
+
+The source code for a work means the preferred form of the work for
+making modifications to it.  For an executable work, complete source
+code means all the source code for all modules it contains, plus any
+associated interface definition files, plus the scripts used to
+control compilation and installation of the executable.  However, as a
+special exception, the source code distributed need not include
+anything that is normally distributed (in either source or binary
+form) with the major components (compiler, kernel, and so on) of the
+operating system on which the executable runs, unless that component
+itself accompanies the executable.
+
+If distribution of executable or object code is made by offering
+access to copy from a designated place, then offering equivalent
+access to copy the source code from the same place counts as
+distribution of the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  4. You may not copy, modify, sublicense, or distribute the Program
+except as expressly provided under this License.  Any attempt
+otherwise to copy, modify, sublicense or distribute the Program is
+void, and will automatically terminate your rights under this License.
+However, parties who have received copies, or rights, from you under
+this License will not have their licenses terminated so long as such
+parties remain in full compliance.
+
+  5. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Program or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Program (or any work based on the
+Program), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Program or works based on it.
+
+  6. Each time you redistribute the Program (or any work based on the
+Program), the recipient automatically receives a license from the
+original licensor to copy, distribute or modify the Program subject to
+these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties to
+this License.
+
+  7. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Program at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Program by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under
+any particular circumstance, the balance of the section is intended to
+apply and the section as a whole is intended to apply in other
+circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system, which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  8. If the distribution and/or use of the Program is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Program under this License
+may add an explicit geographical distribution limitation excluding
+those countries, so that distribution is permitted only in or among
+countries not thus excluded.  In such case, this License incorporates
+the limitation as if written in the body of this License.
+
+  9. The Free Software Foundation may publish revised and/or new versions
+of the General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Program
+specifies a version number of this License which applies to it and "any
+later version", you have the option of following the terms and conditions
+either of that version or of any later version published by the Free
+Software Foundation.  If the Program does not specify a version number of
+this License, you may choose any version ever published by the Free Software
+Foundation.
+
+  10. If you wish to incorporate parts of the Program into other free
+programs whose distribution conditions are different, write to the author
+to ask for permission.  For software which is copyrighted by the Free
+Software Foundation, write to the Free Software Foundation; we sometimes
+make exceptions for this.  Our decision will be guided by the two goals
+of preserving the free status of all derivatives of our free software and
+of promoting the sharing and reuse of software generally.
+
+                            NO WARRANTY
+
+  11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
+FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
+OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
+PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
+OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
+TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
+PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
+REPAIR OR CORRECTION.
+
+  12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
+REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
+OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
+TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
+YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
+PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGES.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    {description}
+    Copyright (C) {year}  {fullname}
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License along
+    with this program; if not, write to the Free Software Foundation, Inc.,
+    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+
+Also add information on how to contact you by electronic and paper mail.
+
+If the program is interactive, make it output a short notice like this
+when it starts in an interactive mode:
+
+    Gnomovision version 69, Copyright (C) year name of author
+    Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, the commands you use may
+be called something other than `show w' and `show c'; they could even be
+mouse-clicks or menu items--whatever suits your program.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the program, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the program
+  `Gnomovision' (which makes passes at compilers) written by James Hacker.
+
+  {signature of Ty Coon}, 1 April 1989
+  Ty Coon, President of Vice
+
+This General Public License does not permit incorporating your program into
+proprietary programs.  If your program is a subroutine library, you may
+consider it more useful to permit linking proprietary applications with the
+library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..27316db
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,57 @@
+#
+# Copyright (C) 2016  Matt Borgerson
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with this program; if not, write to the Free Software Foundation, Inc.,
+# 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+#
+
+CC           := gcc
+CFLAGS       := -Werror -ansi -Os -m32 -g -ffunction-sections -fno-stack-protector
+LD           := ld
+LDFLAGS      := --script=gdbstub.ld -m elf_i386 --gc-sections
+NASM         := nasm
+NASM_FLAGS   := -felf
+OBJCOPY      := objcopy
+OBJCOPYFLAGS := --output-target=binary
+TARGET       := gdbstub.bin
+BASE_ADDRESS := 0x500000
+OBJECTS      := gdbstub_rsp.o \
+                gdbstub_int.o \
+                gdbstub_sys.o
+
+all: $(TARGET)
+.PRECIOUS: %.elf
+
+%.bin: %.elf
+	$(OBJCOPY) $(OBJCOPYFLAGS) $^ $@
+
+%.elf: $(OBJECTS) gdbstub.ld
+	$(LD) $(LDFLAGS) -o $@ $(OBJECTS)
+
+gdbstub.ld: gdbstub.ld.in
+	$(shell sed -e "s/\$$BASE_ADDRESS/$(BASE_ADDRESS)/" gdbstub.ld.in > gdbstub.ld)
+
+%.o: %.c
+	$(CC) $(CFLAGS) -o $@ -c $<
+
+%.o: %.nasm
+	$(NASM) -o $@ $(NASM_FLAGS) $<
+
+.PHONY: clean
+clean:
+	rm -f \
+		$(TARGET) \
+		$(TARGET:.bin=.elf) \
+		$(OBJECTS) \
+		gdbstub.ld
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..59a6c01
--- /dev/null
+++ b/README.md
@@ -0,0 +1,20 @@
+gdbstub
+=======
+This is a simple gdb stub that can be easily dropped in to your project. It
+has no external dependencies and requires just standard tools to build.
+
+Protocol
+--------
+Communication between the stub and the debugger takes place via the [gdb Remote Serial Protocol](https://sourceware.org/gdb/onlinedocs/gdb/Remote-Protocol.html).
+
+Porting
+-------
+This was developed for x86 systems but it's fairly modular. So, with a little
+effort, it should work on other platforms. You will need to modify
+`gdbstub_sys.h` and `gdbstub_sys.c` to fit your platform's needs accordingly.
+
+License
+-------
+Licensed under GPL v2. See `LICENSE.txt` for full license.
+
+Matt Borgerson, 2016
diff --git a/gdbstub.h b/gdbstub.h
new file mode 100644
index 0000000..6506311
--- /dev/null
+++ b/gdbstub.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2016  Matt Borgerson
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#ifndef _GDBSTUB_H_
+#define _GDBSTUB_H_
+
+/* Enable debug statements (printf) */
+#define DEBUG 0
+
+/* Include platform specific definitions */
+#include "gdbstub_sys.h"
+
+/*****************************************************************************
+ * Macros
+ ****************************************************************************/
+
+#if DEBUG
+#define DEBUG_PRINT(...) fprintf(stderr, __VA_ARGS__)
+#else
+#define DEBUG_PRINT(...)
+#endif
+
+#ifndef EOF
+#define EOF (-1)
+#endif
+
+#ifndef NULL
+#define NULL ((void*)0)
+#endif
+
+#ifndef ASSERT
+#if DEBUG
+#define ASSERT(x) { \
+	if (!(x)) { \
+		fprintf(stderr, "ASSERTION FAILED\n"); \
+		fprintf(stderr, "  Assertion: %s\n", #x); \
+		fprintf(stderr, "  Location:  %s @ %s:%d\n", __func__, \
+		                                             __FILE__, __LINE__); \
+		exit(1); \
+	} \
+}
+#else
+#define ASSERT(x) { \
+ 	while(1); \
+}
+#endif
+#endif
+
+/*****************************************************************************
+ * Prototypes
+ ****************************************************************************/
+
+int dbg_main(struct dbg_state *state);
+
+/* System functions, supported by all stubs */
+int dbg_sys_getc(void);
+int dbg_sys_putchar(int ch);
+int dbg_sys_mem_readb(address addr, char *val);
+int dbg_sys_mem_writeb(address addr, char val);
+int dbg_sys_continue();
+int dbg_sys_step();
+
+#endif
\ No newline at end of file
diff --git a/gdbstub.ld.in b/gdbstub.ld.in
new file mode 100644
index 0000000..4c15643
--- /dev/null
+++ b/gdbstub.ld.in
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2016  Matt Borgerson
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+ENTRY(dbg_start)
+
+MEMORY
+{
+	RAM (WX) : ORIGIN = $BASE_ADDRESS, LENGTH = 0x10000
+}
+
+SECTIONS
+{
+	.text : {
+		*(.text.dbg_start)
+		*(.text)
+		*(.rodata)
+	} > RAM
+
+	.data : {
+		*(.data)
+		*(.bss)
+	} > RAM
+
+	/DISCARD/ : {
+		*(.comment)
+		*(.note.GNU-stack)
+		*(.eh_frame)
+		*(.rela.eh_frame)
+		*(.shstrtab)
+		*(.symtab)
+		*(.strtab)
+	}
+}
\ No newline at end of file
diff --git a/gdbstub_int.nasm b/gdbstub_int.nasm
new file mode 100644
index 0000000..836f865
--- /dev/null
+++ b/gdbstub_int.nasm
@@ -0,0 +1,106 @@
+;
+; Copyright (C) 2016  Matt Borgerson
+;
+; This program is free software; you can redistribute it and/or modify
+; it under the terms of the GNU General Public License as published by
+; the Free Software Foundation; either version 2 of the License, or
+; (at your option) any later version.
+;
+; This program is distributed in the hope that it will be useful,
+; but WITHOUT ANY WARRANTY; without even the implied warranty of
+; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+; GNU General Public License for more details.
+;
+; You should have received a copy of the GNU General Public License along
+; with this program; if not, write to the Free Software Foundation, Inc.,
+; 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+;
+
+bits 32
+
+%define NUM_HANDLERS 32
+
+section .data
+global dbg_int_handlers
+
+; Generate table of handlers
+dbg_int_handlers:
+%macro handler_addr 1
+	dd dbg_int_handler_%1
+%endmacro
+%assign i 0
+%rep NUM_HANDLERS
+	handler_addr i
+	%assign i i+1
+%endrep
+
+section .text
+extern dbg_int_handler
+
+%macro int 1
+dbg_int_handler_%1:
+	push    0  ; Dummy Error code
+	push    %1 ; Interrupt Vector
+	jmp     dbg_int_handler_common
+%endmacro
+
+%macro inte 1
+dbg_int_handler_%1:
+	; Error code already on stack
+	push    %1 ; Interrupt Vector
+	jmp     dbg_int_handler_common
+%endmacro
+
+; Generate Interrupt Handlers
+%assign i 0
+%rep NUM_HANDLERS
+	%if (i == 8) || ((i >= 10) && (i <= 14)) || (i == 17)
+		inte i
+	%else
+		int i
+	%endif
+%assign i i+1
+%endrep
+
+; Common Interrupt Handler
+dbg_int_handler_common:
+	pushad
+	push    ds
+	push    es
+	push    fs
+	push    gs
+	push    ss
+	mov     ebp, esp
+	
+	; Stack:
+	; - EFLAGS
+	; - CS
+	; - EIP
+	; - ERROR CODE
+	; - VECTOR
+	; - EAX
+	; - ECX
+	; - EDX
+	; - EBX
+	; - ESP
+	; - EBP
+	; - ESI
+	; - EDI
+	; - DS
+	; - ES
+	; - FS
+	; - GS
+	; - SS
+
+	push    ebp
+	call    dbg_int_handler
+
+	mov     esp, ebp
+	pop     ss
+	pop     gs
+	pop     fs
+	pop     es
+	pop     ds
+	popad
+	add     esp, 8 ; Pop error & vector
+	iret
diff --git a/gdbstub_rsp.c b/gdbstub_rsp.c
new file mode 100644
index 0000000..9894fa9
--- /dev/null
+++ b/gdbstub_rsp.c
@@ -0,0 +1,962 @@
+/*
+ * Copyright (C) 2016  Matt Borgerson
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include "gdbstub.h"
+
+/*****************************************************************************
+ * Types
+ ****************************************************************************/
+
+typedef int (*dbg_enc_func)(char *buf, size_t buf_len, const char *data, size_t data_len);
+typedef int (*dbg_dec_func)(const char *buf, size_t buf_len, char *data, size_t data_len);
+
+/*****************************************************************************
+ * Const Data
+ ****************************************************************************/
+
+const char digits[] = "0123456789abcdef";
+
+/*****************************************************************************
+ * Prototypes
+ ****************************************************************************/
+
+/* Communication functions */
+int dbg_write(const char *buf, size_t len);
+int dbg_read(char *buf, size_t buf_len, size_t len);
+
+/* String processing helper functions */
+int dbg_strlen(const char *ch);
+int dbg_is_printable_char(char ch);
+char dbg_get_digit(int val);
+int dbg_get_val(char digit, int base);
+int dbg_strtol(const char *str, size_t len, int base, const char **endptr);
+
+/* Packet functions */
+int dbg_send_packet(const char *pkt, size_t pkt_len);
+int dbg_recv_packet(char *pkt_buf, size_t pkt_buf_len, size_t *pkt_len);
+int dbg_checksum(const char *buf, size_t len);
+int dbg_recv_ack(void);
+
+/* Data encoding/decoding */
+int dbg_enc_hex(char *buf, size_t buf_len, const char *data, size_t data_len);
+int dbg_dec_hex(const char *buf, size_t buf_len, char *data, size_t data_len);
+int dbg_enc_bin(char *buf, size_t buf_len, const char *data, size_t data_len);
+int dbg_dec_bin(const char *buf, size_t buf_len, char *data, size_t data_len);
+
+/* Packet creation helpers */
+int dbg_send_ok_packet(char *buf, size_t buf_len);
+int dbg_send_conmsg_packet(char *buf, size_t buf_len, const char *msg);
+int dbg_send_signal_packet(char *buf, size_t buf_len, char signal);
+int dbg_send_error_packet(char *buf, size_t buf_len, char error);
+
+/* Command functions */
+int dbg_mem_read(char *buf, size_t buf_len, address addr, size_t len, dbg_enc_func enc);
+int dbg_mem_write(const char *buf, size_t buf_len, address addr, size_t len, dbg_dec_func dec);
+int dbg_continue(void);
+int dbg_step(void);
+
+/*****************************************************************************
+ * String Processing Helper Functions
+ ****************************************************************************/
+
+/*
+ * Get null-terminated string length.
+ */
+int dbg_strlen(const char *ch)
+{
+	int len;
+
+	len = 0;
+	while (*ch++) {
+		len += 1;
+	}
+
+	return len;
+}
+
+/*
+ * Get integer value for a string representation.
+ *
+ * If the string starts with + or -, it will be signed accordingly.
+ *
+ * If base == 0, the base will be determined:
+ *   base 16 if the string starts with 0x or 0X,
+ *   base 10 otherwise
+ *
+ * If endptr is specified, it will point to the last non-digit in the
+ * string. If there are no digits in the string, it will be set to NULL.
+ */
+int dbg_strtol(const char *str, size_t len, int base, const char **endptr)
+{
+	size_t pos;
+	int sign, tmp, value, valid;
+
+	value = 0;
+	pos   = 0;
+	sign  = 1;
+	valid = 0;
+
+	if (endptr) {
+		*endptr = NULL;
+	}
+
+	if (len < 1) {
+		return 0;
+	}
+
+	/* Detect negative numbers */
+	if (str[pos] == '-') {
+		sign = -1;
+		pos += 1;
+	} else if (str[pos] == '+') {
+		sign = 1;
+		pos += 1;
+	}
+
+	/* Detect '0x' hex prefix */
+	if ((pos + 2 < len) && (str[pos] == '0') &&
+		((str[pos+1] == 'x') || (str[pos+1] == 'X'))) {
+		base = 16;
+		pos += 2;
+	}
+
+	if (base == 0) {
+		base = 10;
+	}
+
+	for (; (pos < len) && (str[pos] != '\x00'); pos++) {
+		tmp = dbg_get_val(str[pos], base);
+		if (tmp == EOF) {
+			break;
+		}
+
+		value = value*base + tmp;
+		valid = 1; /* At least one digit is valid */
+	}
+
+	if (!valid) {
+		return 0;
+	}
+
+	if (endptr) {
+		*endptr = str+pos;
+	}
+
+	value *= sign;
+
+	return value;
+}
+
+/*
+ * Get the corresponding ASCII hex digit character for a value.
+ */
+char dbg_get_digit(int val)
+{
+	if ((val >= 0) && (val <= 0xf)) {
+		return digits[val];
+	} else {
+		return EOF;
+	}
+}
+
+/*
+ * Get the corresponding value for a ASCII digit character.
+ *
+ * Supports bases 2-16.
+ */
+int dbg_get_val(char digit, int base)
+{
+	int value;
+
+	if ((digit >= '0') && (digit <= '9')) {
+		value = digit-'0';
+	} else if ((digit >= 'a') && (digit <= 'f')) {
+		value = digit-'a'+0xa;
+	} else if ((digit >= 'A') && (digit <= 'F')) {
+		value = digit-'A'+0xa;
+	} else {
+		return EOF;
+	}
+
+	return (value < base) ? value : EOF;
+}
+
+/*
+ * Determine if this is a printable ASCII character.
+ */
+int dbg_is_printable_char(char ch)
+{
+	return (ch >= 0x20 && ch <= 0x7e);
+}
+
+/*****************************************************************************
+ * Packet Functions
+ ****************************************************************************/
+
+/*
+ * Receive a packet acknowledgment
+ *
+ * Returns:
+ *    0   if an ACK (+) was received
+ *    1   if a NACK (-) was received
+ *    EOF otherwise
+ */
+int dbg_recv_ack(void)
+{
+	int response;
+
+	/* Wait for packet ack */
+	switch (response = dbg_sys_getc()) {
+	case '+':
+		/* Packet acknowledged */
+		return 0;
+	case '-':
+		/* Packet negative acknowledged */
+		return 1;
+	default:
+		/* Bad response! */
+		DEBUG_PRINT("received bad packet response: 0x%2x\n", response);
+		return EOF;
+	}
+}
+
+/*
+ * Calculate 8-bit checksum of a buffer.
+ *
+ * Returns:
+ *    8-bit checksum.
+ */
+int dbg_checksum(const char *buf, size_t len)
+{
+	unsigned char csum;
+
+	csum = 0;
+
+	while (len--) {
+		csum += *buf++;
+	}
+
+	return csum;
+}
+
+/*
+ * Transmits a packet of data.
+ * Packets are of the form: $<packet-data>#<checksum>
+ *
+ * Returns:
+ *    0   if the packet was transmitted and acknowledged
+ *    1   if the packet was transmitted but not acknowledged
+ *    EOF otherwise
+ */
+int dbg_send_packet(const char *pkt_data, size_t pkt_len)
+{
+	char buf[3];
+	char csum;
+
+	/* Send packet start */
+	if (dbg_sys_putchar('$') == EOF) {
+		return EOF;
+	}
+
+#if DEBUG
+	{
+		size_t p;
+		DEBUG_PRINT("-> ");
+		for (p = 0; p < pkt_len; p++) {
+			if (dbg_is_printable_char(pkt_data[p])) {
+				DEBUG_PRINT("%c", pkt_data[p]);
+			} else {
+				DEBUG_PRINT("\\x%02x", pkt_data[p]&0xff);
+			}
+		}
+		DEBUG_PRINT("\n");
+	}
+#endif
+
+	/* Send packet data */
+	if (dbg_write(pkt_data, pkt_len) == EOF) {
+		return EOF;
+	}
+
+	/* Send the checksum */
+	buf[0] = '#';
+	csum = dbg_checksum(pkt_data, pkt_len);
+	if ((dbg_enc_hex(buf+1, sizeof(buf)-1, &csum, 1) == EOF) ||
+		(dbg_write(buf, sizeof(buf)) == EOF)) {
+		return EOF;
+	}
+
+	return dbg_recv_ack();
+}
+
+/*
+ * Receives a packet of data, assuming a 7-bit clean connection.
+ *
+ * Returns:
+ *    0   if the packet was received
+ *    EOF otherwise
+ */
+int dbg_recv_packet(char *pkt_buf, size_t pkt_buf_len, size_t *pkt_len)
+{
+	int data;
+	char expected_csum, actual_csum;
+	char buf[2];
+
+	/* Wait for packet start */
+	actual_csum = 0;
+
+	while (1) {
+		data = dbg_sys_getc();
+		if (data == '$') {
+			/* Detected start of packet. */
+			break;
+		}
+	}
+
+	/* Read until checksum */
+	*pkt_len = 0;
+	while (1) {
+		data = dbg_sys_getc();
+
+		if (data == EOF) {
+			/* Error receiving character */
+			return EOF;
+		} else if (data == '#') {
+			/* End of packet */
+			break;
+		} else {
+			/* Check for space */
+			if (*pkt_len >= pkt_buf_len) {
+				DEBUG_PRINT("packet buffer overflow\n");
+				return EOF;
+			}
+
+			/* Store character and update checksum */
+			pkt_buf[(*pkt_len)++] = (char) data;
+		}
+	}
+
+#if DEBUG
+	{
+		size_t p;
+		DEBUG_PRINT("<- ");
+		for (p = 0; p < *pkt_len; p++) {
+			if (dbg_is_printable_char(pkt_buf[p])) {
+				DEBUG_PRINT("%c", pkt_buf[p]);
+			} else {
+				DEBUG_PRINT("\\x%02x", pkt_buf[p] & 0xff);
+			}
+		}
+		DEBUG_PRINT("\n");
+	}
+#endif
+
+	/* Receive the checksum */
+	if ((dbg_read(buf, sizeof(buf), 2) == EOF) ||
+		(dbg_dec_hex(buf, 2, &expected_csum, 1) == EOF)) {
+		return EOF;
+	}
+
+	/* Verify checksum */
+	actual_csum = dbg_checksum(pkt_buf, *pkt_len);
+	if (actual_csum != expected_csum) {
+		/* Send packet nack */
+		DEBUG_PRINT("received packet with bad checksum\n");
+		dbg_sys_putchar('-');
+		return EOF;
+	}
+
+	/* Send packet ack */
+	dbg_sys_putchar('+');
+	return 0;
+}
+
+/*****************************************************************************
+ * Data Encoding/Decoding
+ ****************************************************************************/
+
+/*
+ * Encode data to its hex-value representation in a buffer.
+ *
+ * Returns:
+ *    0+  number of bytes written to buf
+ *    EOF if the buffer is too small
+ */
+int dbg_enc_hex(char *buf, size_t buf_len, const char *data, size_t data_len)
+{
+	size_t pos;
+
+	if (buf_len < data_len*2) {
+		/* Buffer too small */
+		return EOF;
+	}
+
+	for (pos = 0; pos < data_len; pos++) {
+		*buf++ = dbg_get_digit((data[pos] >> 4) & 0xf);
+		*buf++ = dbg_get_digit((data[pos]     ) & 0xf);
+	}
+
+	return data_len*2;
+}
+
+/*
+ * Decode data from its hex-value representation to a buffer.
+ *
+ * Returns:
+ *    0   if successful
+ *    EOF if the buffer is too small
+ */
+int dbg_dec_hex(const char *buf, size_t buf_len, char *data, size_t data_len)
+{
+	size_t pos;
+	int tmp;
+
+	if (buf_len != data_len*2) {
+		/* Buffer too small */
+		return EOF;
+	}
+
+	for (pos = 0; pos < data_len; pos++) {
+		/* Decode high nibble */
+		tmp = dbg_get_val(*buf++, 16);
+		if (tmp == EOF) {
+			/* Buffer contained junk. */
+			ASSERT(0);
+			return EOF;
+		}
+
+		data[pos] = tmp << 4;
+
+		/* Decode low nibble */
+		tmp = dbg_get_val(*buf++, 16);
+		if (tmp == EOF) {
+			/* Buffer contained junk. */
+			ASSERT(0);
+			return EOF;
+		}
+		data[pos] |= tmp;
+	}
+
+	return 0;
+}
+
+/*
+ * Encode data to its binary representation in a buffer.
+ *
+ * Returns:
+ *    0+  number of bytes written to buf
+ *    EOF if the buffer is too small
+ */
+int dbg_enc_bin(char *buf, size_t buf_len, const char *data, size_t data_len)
+{
+	size_t buf_pos, data_pos;
+
+	for (buf_pos = 0, data_pos = 0; data_pos < data_len; data_pos++) {
+		if (data[data_pos] == '$' ||
+			data[data_pos] == '#' ||
+			data[data_pos] == '}' ||
+			data[data_pos] == '*') {
+			if (buf_pos+1 >= buf_len) {
+				ASSERT(0);
+				return EOF;
+			}
+			buf[buf_pos++] = '}';
+			buf[buf_pos++] = data[data_pos] ^ 0x20;
+		} else {
+			if (buf_pos >= buf_len) {
+				ASSERT(0);
+				return EOF;
+			}
+			buf[buf_pos++] = data[data_pos];
+		}
+	}
+
+	return buf_pos;
+}
+
+/*
+ * Decode data from its bin-value representation to a buffer.
+ *
+ * Returns:
+ *    0+  if successful, number of bytes decoded
+ *    EOF if the buffer is too small
+ */
+int dbg_dec_bin(const char *buf, size_t buf_len, char *data, size_t data_len)
+{
+	size_t buf_pos, data_pos;
+
+	for (buf_pos = 0, data_pos = 0; buf_pos < buf_len; buf_pos++) {
+		if (data_pos >= data_len) {
+			/* Output buffer overflow */
+			ASSERT(0);
+			return EOF;
+		}
+		if (buf[buf_pos] == '}') {
+			/* The next byte is escaped! */
+			if (buf_pos+1 >= buf_len) {
+				/* There's an escape character, but no escaped character
+				 * following the escape character. */
+				ASSERT(0);
+				return EOF;
+			}
+			buf_pos += 1;
+			data[data_pos++] = buf[buf_pos] ^ 0x20;
+		} else {
+			data[data_pos++] = buf[buf_pos];
+		}
+	}
+
+	return data_pos;
+}
+
+/*****************************************************************************
+ * Command Functions
+ ****************************************************************************/
+
+/*
+ * Read from memory and encode into buf.
+ *
+ * Returns:
+ *    0+  number of bytes written to buf
+ *    EOF if the buffer is too small
+ */
+int dbg_mem_read(char *buf, size_t buf_len, address addr, size_t len, dbg_enc_func enc)
+{
+	char data[64];
+	size_t pos;
+
+	if (len > sizeof(data)) {
+		return EOF;
+	}
+
+	/* Read from system memory */
+	for (pos = 0; pos < len; pos++) {
+		if (dbg_sys_mem_readb(addr+pos, &data[pos])) {
+			/* Failed to read */
+			return EOF;
+		}
+	}
+
+	/* Encode data */
+	return enc(buf, buf_len, data, len);
+}
+
+/*
+ * Write to memory from encoded buf.
+ */
+int dbg_mem_write(const char *buf, size_t buf_len, address addr, size_t len, dbg_dec_func dec)
+{
+	char data[64];
+	size_t pos;
+
+	if (len > sizeof(data)) {
+		return EOF;
+	}
+
+	/* Decode data */
+	if (dec(buf, buf_len, data, len) == EOF) {
+		return EOF;
+	}
+
+	/* Write to system memory */
+	for (pos = 0; pos < len; pos++) {
+		if (dbg_sys_mem_writeb(addr+pos, data[pos])) {
+			/* Failed to write */
+			return EOF;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Continue program execution at PC.
+ */
+int dbg_continue(void)
+{
+	dbg_sys_continue();
+	return 0;
+}
+
+/*
+ * Step one instruction.
+ */
+int dbg_step(void)
+{
+	dbg_sys_step();
+	return 0;
+}
+
+/*****************************************************************************
+ * Packet Creation Helpers
+ ****************************************************************************/
+
+/*
+ * Send OK packet
+ */
+int dbg_send_ok_packet(char *buf, size_t buf_len)
+{
+	return dbg_send_packet("OK", 2);
+}
+
+/*
+ * Send a message to the debugging console (via O XX... packet)
+ */
+int dbg_send_conmsg_packet(char *buf, size_t buf_len, const char *msg)
+{
+	size_t size;
+	int status;
+
+	if (buf_len < 2) {
+		/* Buffer too small */
+		return EOF;
+	}
+
+	buf[0] = 'O';
+	status = dbg_enc_hex(&buf[1], buf_len-1, msg, dbg_strlen(msg));
+	if (status == EOF) {
+		return EOF;
+	}
+	size = 1 + status;
+	return dbg_send_packet(buf, size);
+}
+
+/*
+ * Send a signal packet (S AA).
+ */
+int dbg_send_signal_packet(char *buf, size_t buf_len, char signal)
+{
+	size_t size;
+	int status;
+
+	if (buf_len < 4) {
+		/* Buffer too small */
+		return EOF;
+	}
+
+	buf[0] = 'S';
+	status = dbg_enc_hex(&buf[1], buf_len-1, &signal, 1);
+	if (status == EOF) {
+		return EOF;
+	}
+	size = 1 + status;
+	return dbg_send_packet(buf, size);
+}
+
+/*
+ * Send a error packet (E AA).
+ */
+int dbg_send_error_packet(char *buf, size_t buf_len, char error)
+{
+	size_t size;
+	int status;
+
+	if (buf_len < 4) {
+		/* Buffer too small */
+		return EOF;
+	}
+
+	buf[0] = 'E';
+	status = dbg_enc_hex(&buf[1], buf_len-1, &error, 1);
+	if (status == EOF) {
+		return EOF;
+	}
+	size = 1 + status;
+	return dbg_send_packet(buf, size);
+}
+
+/*****************************************************************************
+ * Communication Functions
+ ****************************************************************************/
+
+/*
+ * Write a sequence of bytes.
+ *
+ * Returns:
+ *    0   if successful
+ *    EOF if failed to write all bytes
+ */
+int dbg_write(const char *buf, size_t len)
+{
+	while (len--) {
+		if (dbg_sys_putchar(*buf++) == EOF) {
+			return EOF;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Read a sequence of bytes.
+ *
+ * Returns:
+ *    0   if successfully read len bytes
+ *    EOF if failed to read all bytes
+ */
+int dbg_read(char *buf, size_t buf_len, size_t len)
+{
+	char c;
+
+	if (buf_len < len) {
+		/* Buffer too small */
+		return EOF;
+	}
+
+	while (len--) {
+		if ((c = dbg_sys_getc()) == EOF) {
+			return EOF;
+		}
+		*buf++ = c;
+	}
+
+	return 0;
+}
+
+/*****************************************************************************
+ * Main Loop
+ ****************************************************************************/
+
+/*
+ * Main debug loop. Handles commands.
+ */
+int dbg_main(struct dbg_state *state)
+{
+	address     addr;
+	char        pkt_buf[256];
+	int         status;
+	size_t      length;
+	size_t      pkt_len;
+	const char *ptr_next;
+
+	dbg_send_signal_packet(pkt_buf, sizeof(pkt_buf), 0);
+
+	while (1) {
+		/* Receive the next packet */
+		status = dbg_recv_packet(pkt_buf, sizeof(pkt_buf), &pkt_len);
+		if (status == EOF) {
+			break;
+		}
+
+		if (pkt_len == 0) {
+			/* Received empty packet.. */
+			continue;
+		}
+
+		ptr_next = pkt_buf;
+
+		/*
+		 * Handle one letter commands
+		 */
+		switch (pkt_buf[0]) {
+
+		/* Calculate remaining space in packet from ptr_next position. */
+		#define token_remaining_buf (pkt_len-(ptr_next-pkt_buf))
+
+		/* Expecting a seperator. If not present, go to error */
+		#define token_expect_seperator(c) \
+			{ \
+				if (!ptr_next || *ptr_next != c) { \
+					goto error; \
+				} else { \
+					ptr_next += 1; \
+				} \
+			}
+
+		/* Expecting an integer argument. If not present, go to error */
+		#define token_expect_integer_arg(arg) \
+			{ \
+				arg = dbg_strtol(ptr_next, token_remaining_buf, \
+				                 16, &ptr_next); \
+				if (!ptr_next) { \
+					goto error; \
+				} \
+			}
+
+		/*
+		 * Read Registers
+		 * Command Format: g
+		 */
+		case 'g':
+			/* Encode registers */
+			status = dbg_enc_hex(pkt_buf, sizeof(pkt_buf),
+			                     (char *)&(state->registers),
+			                     sizeof(state->registers));
+			if (status == EOF) {
+				goto error;
+			}
+			pkt_len = status;
+			dbg_send_packet(pkt_buf, pkt_len);
+			break;
+		
+		/*
+		 * Write Registers
+		 * Command Format: G XX...
+		 */
+		case 'G':
+			status = dbg_dec_hex(pkt_buf+1, pkt_len-1,
+			                     (char *)&(state->registers),
+			                     sizeof(state->registers));
+			if (status == EOF) {
+				goto error;
+			}
+			dbg_send_ok_packet(pkt_buf, sizeof(pkt_buf));
+			break;
+
+		/*
+		 * Read a Register
+		 * Command Format: p n
+		 */
+		case 'p':
+			ptr_next += 1;
+			token_expect_integer_arg(addr);
+
+			if (addr >= DBG_CPU_I386_NUM_REGISTERS) {
+				goto error;
+			}
+
+			/* Read Register */
+			status = dbg_enc_hex(pkt_buf, sizeof(pkt_buf),
+			                     (char *)&(state->registers[addr]),
+			                     sizeof(state->registers[addr]));
+			if (status == EOF) {
+				goto error;
+			}
+			dbg_send_packet(pkt_buf, status);
+			break;
+		
+		/*
+		 * Write a Register
+		 * Command Format: P n...=r...
+		 */
+		case 'P':
+			ptr_next += 1;
+			token_expect_integer_arg(addr);
+			token_expect_seperator('=');
+
+			if (addr >= DBG_CPU_I386_NUM_REGISTERS) {
+				goto error;
+			}
+
+			status = dbg_dec_hex(ptr_next, token_remaining_buf,
+			                     (char *)&(state->registers[addr]),
+			                     sizeof(state->registers[addr]));
+			if (status == EOF) {
+				goto error;
+			}
+			dbg_send_ok_packet(pkt_buf, sizeof(pkt_buf));
+			break;
+		
+		/*
+		 * Read Memory
+		 * Command Format: m addr,length
+		 */
+		case 'm':
+			ptr_next += 1;
+			token_expect_integer_arg(addr);
+			token_expect_seperator(',');
+			token_expect_integer_arg(length);
+
+			/* Read Memory */
+			status = dbg_mem_read(pkt_buf, sizeof(pkt_buf),
+			                      addr, length, dbg_enc_hex);
+			if (status == EOF) {
+				goto error;
+			}
+			dbg_send_packet(pkt_buf, status);
+			break;
+		
+		/*
+		 * Write Memory
+		 * Command Format: M addr,length:XX..
+		 */
+		case 'M':
+			ptr_next += 1;
+			token_expect_integer_arg(addr);
+			token_expect_seperator(',');
+			token_expect_integer_arg(length);
+			token_expect_seperator(':');
+
+			/* Write Memory */
+			status = dbg_mem_write(ptr_next, token_remaining_buf,
+			                       addr, length, dbg_dec_hex);
+			if (status == EOF) {
+				goto error;
+			}
+			dbg_send_ok_packet(pkt_buf, sizeof(pkt_buf));
+			break;
+
+		/*
+		 * Write Memory (Binary)
+		 * Command Format: X addr,length:XX..
+		 */
+		case 'X':
+			ptr_next += 1;
+			token_expect_integer_arg(addr);
+			token_expect_seperator(',');
+			token_expect_integer_arg(length);
+			token_expect_seperator(':');
+
+			/* Write Memory */
+			status = dbg_mem_write(ptr_next, token_remaining_buf,
+			                       addr, length, dbg_dec_bin);
+			if (status == EOF) {
+				goto error;
+			}
+			dbg_send_ok_packet(pkt_buf, sizeof(pkt_buf));
+			break;
+
+		/* 
+		 * Continue
+		 * Command Format: c [addr]
+		 */
+		case 'c':
+			dbg_continue();
+			return 0;
+
+		/*
+		 * Single-step
+		 * Command Format: s [addr]
+		 */
+		case 's':
+			dbg_step();
+			return 0;
+
+		case '?':
+			dbg_send_signal_packet(pkt_buf, sizeof(pkt_buf), 0);
+			break;
+
+		/*
+		 * Unsupported Command
+		 */
+		default:
+			dbg_send_packet(NULL, 0);
+		}
+
+		continue;
+
+	error:
+		dbg_send_error_packet(pkt_buf, sizeof(pkt_buf), 0x00);
+
+		#undef token_remaining_buf
+		#undef token_expect_seperator
+		#undef token_expect_integer_arg
+	}
+
+	return 0;
+}
diff --git a/gdbstub_sys.c b/gdbstub_sys.c
new file mode 100644
index 0000000..2b17702
--- /dev/null
+++ b/gdbstub_sys.c
@@ -0,0 +1,341 @@
+/*
+ * Copyright (C) 2016  Matt Borgerson
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include "gdbstub.h"
+
+#ifdef __STRICT_ANSI__
+#define asm __asm__
+#endif
+
+#define SERIAL_COM1 0x3f8
+#define SERIAL_COM2 0x2f8
+#define SERIAL_PORT SERIAL_COM1
+
+#define NUM_IDT_ENTRIES 32
+
+/*****************************************************************************
+ * BSS Data
+ ****************************************************************************/
+
+static struct dbg_idt_gate dbg_idt_gates[NUM_IDT_ENTRIES];
+static struct dbg_state    dbg_state;
+
+/*****************************************************************************
+ * Misc. Functions
+ ****************************************************************************/
+
+void *dbg_sys_memset(void *ptr, int data, size_t len)
+{
+	char *p = ptr;
+
+	while (len--) {
+		*p++ = (char)data;
+	}
+
+	return ptr;
+}
+
+/*
+ * Get current code segment (CS register).
+ */
+uint32_t dbg_get_cs()
+{
+	uint32_t cs;
+
+	asm(
+		"push    %%cs;"
+		"pop     %%eax;"
+		/* Outputs  */ : "=a" (cs)
+		/* Inputs   */ : /* None */
+		/* Clobbers */ : /* None */
+		);
+
+	return cs;
+}
+
+/*****************************************************************************
+ * Interrupt Management Functions
+ ****************************************************************************/
+
+/*
+ * Initialize idt_gates with the interrupt handlers.
+ */
+int dbg_init_gates()
+{
+	size_t   i;
+	uint16_t cs;
+
+	cs = dbg_get_cs();
+	for (i = 0; i < NUM_IDT_ENTRIES; i++) {
+		dbg_idt_gates[i].flags       = 0x8E00;
+		dbg_idt_gates[i].segment     = cs;
+		dbg_idt_gates[i].offset_low  =
+			((uint32_t)dbg_int_handlers[i]      ) & 0xffff;
+		dbg_idt_gates[i].offset_high =
+			((uint32_t)dbg_int_handlers[i] >> 16) & 0xffff;
+	}
+
+	return 0;
+}
+
+/*
+ * Load a new IDT.
+ */
+int dbg_load_idt(struct dbg_idtr *idtr)
+{
+	asm(
+		"lidt    %0"
+		/* Outputs  */ : /* None */
+		/* Inputs   */ : "m" (*idtr)
+		/* Clobbers */ : /* None */
+		);
+
+	return 0;
+}
+
+/*
+ * Get current IDT.
+ */
+int dbg_store_idt(struct dbg_idtr *idtr)
+{
+	asm(
+		"sidt    %0"
+		/* Outputs  */ : "=m" (*idtr)
+		/* Inputs   */ : /* None */
+		/* Clobbers */ : /* None */
+		);
+
+	return 0;
+}
+
+/*
+ * Hook a vector of the current IDT.
+ */
+int dbg_hook_idt(uint8_t vector, const void *function)
+{
+	struct dbg_idtr      idtr;
+	struct dbg_idt_gate *gates;
+
+	dbg_store_idt(&idtr);
+	gates = (struct dbg_idt_gate *)idtr.offset;
+	gates[vector].flags       = 0x8E00;
+	gates[vector].segment     = dbg_get_cs();
+	gates[vector].offset_low  = (((uint32_t)function)      ) & 0xffff;
+	gates[vector].offset_high = (((uint32_t)function) >> 16) & 0xffff;
+
+	return 0;
+}
+
+/*
+ * Initialize IDT gates and load the new IDT.
+ */
+int dbg_init_idt(void)
+{
+	struct dbg_idtr idtr;
+
+	dbg_init_gates();
+	idtr.len = sizeof(dbg_idt_gates)-1;
+	idtr.offset = (uint32_t)dbg_idt_gates;
+	dbg_load_idt(&idtr);
+
+	return 0;
+}
+
+/*
+ * Common interrupt handler routine.
+ */
+void dbg_int_handler(struct dbg_interrupt_state *istate)
+{
+	dbg_interrupt(istate);
+}
+
+/*
+ * Debug interrupt handler.
+ */
+void dbg_interrupt(struct dbg_interrupt_state *istate)
+{
+	dbg_sys_memset(&dbg_state.registers, 0, sizeof(dbg_state.registers));
+
+	dbg_state.signum = istate->vector;
+
+	/* Load Registers */
+	dbg_state.registers[DBG_CPU_I386_REG_EAX] = istate->eax;
+	dbg_state.registers[DBG_CPU_I386_REG_ECX] = istate->ecx;
+	dbg_state.registers[DBG_CPU_I386_REG_EDX] = istate->edx;
+	dbg_state.registers[DBG_CPU_I386_REG_EBX] = istate->ebx;
+	dbg_state.registers[DBG_CPU_I386_REG_ESP] = istate->esp;
+	dbg_state.registers[DBG_CPU_I386_REG_EBP] = istate->ebp;
+	dbg_state.registers[DBG_CPU_I386_REG_ESI] = istate->esi;
+	dbg_state.registers[DBG_CPU_I386_REG_EDI] = istate->edi;
+	dbg_state.registers[DBG_CPU_I386_REG_PC]  = istate->eip;
+	dbg_state.registers[DBG_CPU_I386_REG_CS]  = istate->cs;
+	dbg_state.registers[DBG_CPU_I386_REG_PS]  = istate->eflags;
+	dbg_state.registers[DBG_CPU_I386_REG_SS]  = istate->ss;
+	dbg_state.registers[DBG_CPU_I386_REG_DS]  = istate->ds;
+	dbg_state.registers[DBG_CPU_I386_REG_ES]  = istate->es;
+	dbg_state.registers[DBG_CPU_I386_REG_FS]  = istate->fs;
+	dbg_state.registers[DBG_CPU_I386_REG_GS]  = istate->gs;
+
+	dbg_main(&dbg_state);
+
+	/* Restore Registers */
+	istate->eax    = dbg_state.registers[DBG_CPU_I386_REG_EAX];
+	istate->ecx    = dbg_state.registers[DBG_CPU_I386_REG_ECX];
+	istate->edx    = dbg_state.registers[DBG_CPU_I386_REG_EDX];
+	istate->ebx    = dbg_state.registers[DBG_CPU_I386_REG_EBX];
+	istate->esp    = dbg_state.registers[DBG_CPU_I386_REG_ESP];
+	istate->ebp    = dbg_state.registers[DBG_CPU_I386_REG_EBP];
+	istate->esi    = dbg_state.registers[DBG_CPU_I386_REG_ESI];
+	istate->edi    = dbg_state.registers[DBG_CPU_I386_REG_EDI];
+	istate->eip    = dbg_state.registers[DBG_CPU_I386_REG_PC];
+	istate->cs     = dbg_state.registers[DBG_CPU_I386_REG_CS];
+	istate->eflags = dbg_state.registers[DBG_CPU_I386_REG_PS];
+	istate->ss     = dbg_state.registers[DBG_CPU_I386_REG_SS];
+	istate->ds     = dbg_state.registers[DBG_CPU_I386_REG_DS];
+	istate->es     = dbg_state.registers[DBG_CPU_I386_REG_ES];
+	istate->fs     = dbg_state.registers[DBG_CPU_I386_REG_FS];
+	istate->gs     = dbg_state.registers[DBG_CPU_I386_REG_GS];
+}
+
+/*****************************************************************************
+ * I/O Functions
+ ****************************************************************************/
+
+/*
+ * Write to I/O port.
+ */
+void dbg_io_write_8(uint16_t port, uint8_t val)
+{
+	asm(
+		"outb    %%al, %%dx;"
+		/* Outputs  */ : /* None */
+		/* Inputs   */ : "a" (val), "d" (port)
+		/* Clobbers */ : /* None */
+		);
+}
+
+/*
+ * Read from I/O port.
+ */
+uint8_t dbg_io_read_8(uint16_t port)
+{
+	uint8_t val;
+
+	asm(
+		"inb     %%dx, %%al;"
+		/* Outputs  */ : "=a" (val)
+		/* Inputs   */ : "d" (port)
+		/* Clobbers */ : /* None */
+		);
+
+	return val;
+}
+
+/*****************************************************************************
+ * NS16550 Serial Port (IO)
+ ****************************************************************************/
+
+#define SERIAL_THR 0
+#define SERIAL_RBR 0
+#define SERIAL_LSR 5
+
+int dbg_serial_getc(void)
+{
+	/* Wait for data */
+	while ((dbg_io_read_8(SERIAL_PORT + SERIAL_LSR) & 1) == 0);
+	return dbg_io_read_8(SERIAL_PORT + SERIAL_RBR);
+}
+
+int dbg_serial_putchar(int ch)
+{
+	/* Wait for THRE (bit 5) to be high */
+	while ((dbg_io_read_8(SERIAL_PORT + SERIAL_LSR) & (1<<5)) == 0);
+	dbg_io_write_8(SERIAL_PORT + SERIAL_THR, ch);
+	return ch;
+}
+
+/*****************************************************************************
+ * Debugging System Functions
+ ****************************************************************************/
+
+/*
+ * Write one character to the debugging stream.
+ */
+int dbg_sys_putchar(int ch)
+{
+	return dbg_serial_putchar(ch);
+}
+
+/*
+ * Read one character from the debugging stream.
+ */
+int dbg_sys_getc(void)
+{
+	return dbg_serial_getc() & 0xff;
+}
+
+/*
+ * Read one byte from memory.
+ */
+int dbg_sys_mem_readb(address addr, char *val)
+{
+	*val = *(volatile char *)addr;
+	return 0;
+}
+
+/*
+ * Write one byte to memory.
+ */
+int dbg_sys_mem_writeb(address addr, char val)
+{
+	*(volatile char *)addr = val;
+	return 0;
+}
+
+/*
+ * Continue program execution.
+ */
+int dbg_sys_continue()
+{
+	dbg_state.registers[DBG_CPU_I386_REG_PS] &= ~(1<<8);
+	return 0;
+}
+
+/*
+ * Single step the next instruction.
+ */
+int dbg_sys_step()
+{
+	dbg_state.registers[DBG_CPU_I386_REG_PS] |= 1<<8;
+	return 0;
+}
+
+/*
+ * Debugger init function.
+ *
+ * Hooks the IDT to enable debugging.
+ */
+void dbg_start(void)
+{
+	/* Hook current IDT. */
+	dbg_hook_idt(1, dbg_int_handlers[1]);
+	dbg_hook_idt(3, dbg_int_handlers[3]);
+
+	/* Interrupt to start debugging. */
+	asm("int3");
+}
diff --git a/gdbstub_sys.h b/gdbstub_sys.h
new file mode 100644
index 0000000..dd82b1f
--- /dev/null
+++ b/gdbstub_sys.h
@@ -0,0 +1,129 @@
+/*
+ * Copyright (C) 2016  Matt Borgerson
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+/* Define the size_t type */
+#define DBG_DEFINE_SIZET 1
+
+/* Define required standard integer types (e.g. uint16_t) */
+#define DBG_DEFINE_STDINT 1
+
+/*****************************************************************************
+ * Types
+ ****************************************************************************/
+
+#if DBG_DEFINE_STDINT
+typedef unsigned char  uint8_t;
+typedef unsigned short uint16_t;
+typedef unsigned long  uint32_t;
+#endif
+
+#if DBG_DEFINE_SIZET
+typedef unsigned int size_t;
+#endif
+
+typedef unsigned int address;
+typedef unsigned int reg;
+
+#pragma pack(1)
+struct dbg_interrupt_state {
+	uint32_t ss;
+	uint32_t gs;
+	uint32_t fs;
+	uint32_t es;
+	uint32_t ds;
+	uint32_t edi;
+	uint32_t esi;
+	uint32_t ebp;
+	uint32_t esp;
+	uint32_t ebx;
+	uint32_t edx;
+	uint32_t ecx;
+	uint32_t eax;
+	uint32_t vector;
+	uint32_t error_code;
+	uint32_t eip;
+	uint32_t cs;
+	uint32_t eflags;
+};
+#pragma pack()
+
+#pragma pack(1)
+struct dbg_idtr
+{
+	uint16_t len;
+	uint32_t offset;
+};
+#pragma pack()
+
+#pragma pack(1)
+struct dbg_idt_gate
+{
+	uint16_t offset_low;
+	uint16_t segment;
+	uint16_t flags;
+	uint16_t offset_high;
+};
+#pragma pack()
+
+enum DBG_REGISTER {
+	DBG_CPU_I386_REG_EAX       = 0,
+	DBG_CPU_I386_REG_ECX       = 1,
+	DBG_CPU_I386_REG_EDX       = 2,
+	DBG_CPU_I386_REG_EBX       = 3,
+	DBG_CPU_I386_REG_ESP       = 4,
+	DBG_CPU_I386_REG_EBP       = 5,
+	DBG_CPU_I386_REG_ESI       = 6,
+	DBG_CPU_I386_REG_EDI       = 7,
+	DBG_CPU_I386_REG_PC        = 8,
+	DBG_CPU_I386_REG_PS        = 9,
+	DBG_CPU_I386_REG_CS        = 10,
+	DBG_CPU_I386_REG_SS        = 11,
+	DBG_CPU_I386_REG_DS        = 12,
+	DBG_CPU_I386_REG_ES        = 13,
+	DBG_CPU_I386_REG_FS        = 14,
+	DBG_CPU_I386_REG_GS        = 15,
+	DBG_CPU_I386_NUM_REGISTERS = 16
+};
+
+struct dbg_state {
+	int signum;
+	reg registers[DBG_CPU_I386_NUM_REGISTERS];
+};
+
+/*****************************************************************************
+ * Const Data
+ ****************************************************************************/
+
+extern void const * const dbg_int_handlers[];
+
+/*****************************************************************************
+ * Prototypes
+ ****************************************************************************/
+
+int dbg_hook_idt(uint8_t vector, const void *function);
+int dbg_init_gates();
+int dbg_init_idt(void);
+int dbg_load_idt(struct dbg_idtr *idtr);
+int dbg_store_idt(struct dbg_idtr *idtr);
+uint32_t dbg_get_cs();
+void dbg_int_handler(struct dbg_interrupt_state *istate);
+void dbg_interrupt(struct dbg_interrupt_state *istate);
+void dbg_start(void);
+void dbg_io_write_8(uint16_t port, uint8_t val);
+uint8_t dbg_io_read_8(uint16_t port);
+void *dbg_sys_memset(void *ptr, int data, size_t len);
\ No newline at end of file