Initial

.gitignore

@@ -0,0 +1,212 @@
+.idea
+### Python template
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+
+# C extensions
+*.so
+*.bin
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+### C++ template
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### C template
+# Prerequisites
+
+# Object files
+*.ko
+*.elf
+
+# Linker output
+*.ilk
+*.map
+*.exp
+
+
+# Shared objects (inc. Windows DLLs)
+*.so.*
+
+# Executables
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+*.su
+*.idb
+*.pdb
+
+# Kernel Module Compile Results
+*.mod*
+*.cmd
+.tmp_versions/
+modules.order
+Module.symvers
+Mkfile.old
+dkms.conf
+

emulator.py

@@ -0,0 +1,240 @@
+import sys
+# from http.server import BaseHTTPRequestHandler, HTTPServer
+# from subprocess import PIPE, Popen
+from threading import Thread
+from time import sleep
+import websocket
+from os import path, chdir
+import ctypes
+
+import wx
+
+pwd = path.dirname(path.realpath(__file__))
+chdir(pwd)
+libmsp430 = ctypes.cdll.LoadLibrary(path.join(pwd, "libmsp430.so"))
+run_emu = libmsp430.run
+run_emu.restype = ctypes.c_int
+run_emu.argtypes = [ctypes.c_uint]
+
+
+class Emulator:
+    EVENT_CONSOLE = 0
+    EVENT_SERIAL = 1
+
+    def __init__(self, emu_dir, ws_port=59981, load=None, callback=None):
+        # self.process = Popen([path.join(emu_dir, 'MSP430'), str(ws_port)], stdout=PIPE, stderr=PIPE)
+        self.process = Thread(target=run_emu, args=(ws_port, ))
+        self.process.start()
+        sleep(3)
+        self.ws = websocket.WebSocketApp(f"ws://127.0.0.1:{ws_port}",
+                                         subprotocols={"emu-protocol"},
+                                         on_open=self._ws_open,
+                                         on_data=self._ws_msg,
+                                         on_error=self._ws_err,
+                                         on_close=self._ws_close
+                                        )
+        self.load = load
+        self.started = False
+        self.start_errors = 0
+        self.callback = callback
+        Thread(target=self.ws.run_forever).start()
+
+    def wait(self):
+        self.process.wait()
+
+    def load_file(self, fname):
+        with open(fname, 'rb') as f:
+            fdata = f.read()
+            name = path.basename(fname)
+            payload = b'\x00'  # opcode
+            payload += len(fdata).to_bytes(2, byteorder='big')
+            payload += len(name).to_bytes(2, byteorder='big')
+            payload += name.encode() + fdata
+            self.ws.send(payload, websocket.ABNF.OPCODE_BINARY)
+
+    def _ws_open(self):
+        self.started = True
+        if self.load is not None:
+            self.load_file(self.load)
+
+    def _ws_msg(self, data, frame, x):
+        opcode = data[0]
+        if opcode == 0:
+            return
+        elif opcode == 1:
+            message = data[1:-1].decode()
+            if "Type 'h' for" in message:
+                return
+            if callable(self.callback):
+                self.callback(self.EVENT_CONSOLE, message)
+            print(message, end=None)
+            return
+        elif opcode == 2:
+            message = data[1:-1].decode()
+            if callable(self.callback):
+                self.callback(self.EVENT_SERIAL, message)
+            return
+        else:
+            pass
+
+    def _ws_err(self, err):
+        if not self.started:
+            self.start_errors += 1
+            if self.start_errors < 5:
+                print(f"Failed to connect to emulator backend attempt {self.start_errors}")
+                sleep(1)
+                self.ws.run_forever()
+            raise ConnectionError("Failed to connect to emulation backend after 5 tries")
+        raise err
+
+    def _ws_close(self):
+        if not self.started:
+            return
+        pass
+
+    def __del__(self):
+        self.close()
+
+    def emulation_pause(self):
+        self.ws.send(b'\x02', websocket.ABNF.OPCODE_BINARY)
+
+    def emulation_start(self):
+        self.ws.send(b'\x01', websocket.ABNF.OPCODE_BINARY)
+
+    def close(self):
+        self.ws.close()
+        self.process.join()
+
+
+class EmulatorWindow(wx.Frame):
+    def __init__(self, parent, title):
+        wx.Frame.__init__(self, parent, title=title)
+        self.control = wx.TextCtrl(self, size=wx.Size(400, 450),
+                                   style=wx.TE_MULTILINE | wx.TE_READONLY | wx.TE_DONTWRAP)
+        self.serial = wx.TextCtrl(self, size=wx.Size(400, 450), style=wx.TE_MULTILINE | wx.TE_READONLY | wx.TE_DONTWRAP)
+        self.serial_input = wx.TextCtrl(self, style=wx.TE_DONTWRAP)
+
+        self.CreateStatusBar()  # A Statusbar in the bottom of the window
+
+        filemenu = wx.Menu()
+
+        menuFile = filemenu.Append(wx.ID_OPEN, "&Firmware", " Open firmware")
+        menuReset = filemenu.Append(wx.ID_CLOSE_ALL, "&Reset", " Reset Emulator")
+        menuExit = filemenu.Append(wx.ID_EXIT, "E&xit", " Terminate the program")
+        self.Bind(wx.EVT_MENU, self.OnOpen, menuFile)
+        self.Bind(wx.EVT_MENU, self.RestartEmulator, menuReset)
+        self.Bind(wx.EVT_MENU, self.OnExit, menuExit)
+
+        menuBar = wx.MenuBar()
+        menuBar.Append(filemenu, "&File")
+        self.SetMenuBar(menuBar)
+
+        self.sizer2 = wx.BoxSizer(wx.HORIZONTAL)
+        btn_start_emu = wx.Button(self, -1, "S&tart")
+        self.Bind(wx.EVT_BUTTON, self.OnStart, btn_start_emu)
+        btn_stop_emu = wx.Button(self, -1, "P&ause")
+        self.Bind(wx.EVT_BUTTON, self.OnPause, btn_stop_emu)
+        self.sizer2.Add(btn_start_emu, 1, wx.EXPAND)
+        self.sizer2.Add(btn_stop_emu, 1, wx.EXPAND)
+
+        self.sizer = wx.BoxSizer(wx.VERTICAL)
+
+        self.sizer3 = wx.BoxSizer(wx.HORIZONTAL)
+        btn_start_emu = wx.Button(self, -1, "Send")
+        self.sizer3.Add(self.serial_input, 1, wx.EXPAND)
+        self.sizer3.Add(btn_start_emu, 0)
+
+        self.sizer0 = wx.BoxSizer(wx.VERTICAL)
+        self.sizer0.Add(self.serial, 1, wx.EXPAND)
+        self.sizer0.Add(self.sizer3, 0, wx.EXPAND)
+
+        panel = wx.Panel(self, size=wx.Size(275, 375))
+        img = wx.Bitmap("msp430.png", wx.BITMAP_TYPE_PNG)
+        wx.StaticBitmap(panel, -1, img, (0, 0), (img.GetWidth(), img.GetHeight()))
+        self.diagram = DrawRect(panel, -1, size=wx.Size(275, 375))
+        #
+        # dc = wx.WindowDC(panel)
+        # dc.SetPen(wx.WHITE_PEN)
+        # dc.SetBrush(wx.WHITE_BRUSH)
+        # dc.DrawRectangle(50, 50, 500, 500)
+
+
+        self.sizer1 = wx.BoxSizer(wx.HORIZONTAL)
+        self.sizer1.Add(panel, 0, wx.ALIGN_CENTER_HORIZONTAL | wx.ALIGN_CENTER_VERTICAL)
+        self.sizer1.Add(self.control, 1, wx.EXPAND)
+        self.sizer1.Add(self.sizer0, 1, wx.EXPAND)
+
+        self.sizer.Add(self.sizer1, 1, wx.EXPAND)
+        self.sizer.Add(self.sizer2, 0, wx.EXPAND)
+
+        self.SetSizer(self.sizer)
+        self.SetAutoLayout(1)
+        self.sizer.Fit(self)
+        self.Show()
+
+        self.control.WriteText("Initialising Emulator..\n")
+        self.load = None
+        if len(sys.argv) >= 2:
+            if path.exists(sys.argv[1]):
+                self.load = sys.argv[1]
+        self.emu = Emulator('emulator', load=self.load, callback=self.callback)
+
+    def callback(self, event, data):
+        if event == Emulator.EVENT_CONSOLE:
+            wx.CallAfter(self.control.AppendText, data)
+        if event == Emulator.EVENT_SERIAL:
+            wx.CallAfter(self.serial.AppendText, data)
+
+    def RestartEmulator(self, e):
+        self.control.AppendText("Stopping Emulator..")
+        self.emu.close()
+        self.control.Clear()
+        self.serial.Clear()
+        self.control.WriteText("Initialising Emulator..\n")
+        self.emu = Emulator('emulator', load=self.load, callback=self.callback)
+
+    def OnOpen(self, e):
+        with wx.FileDialog(self, "Open Firmware", wildcard="ELF files (*.elf)|*.elf",
+                           style=wx.FD_OPEN | wx.FD_FILE_MUST_EXIST) as fileDialog:
+            if fileDialog.ShowModal() == wx.ID_CANCEL:
+                return
+            self.load = fileDialog.GetPath()
+            self.RestartEmulator(None)
+
+    def OnPause(self, e):
+        self.emu.emulation_pause()
+
+    def OnStart(self, e):
+        self.emu.emulation_start()
+
+    def OnExit(self, e):
+        self.emu.close()
+        self.Close(True)
+
+
+class DrawRect(wx.Panel):
+    """ class MyPanel creates a panel to draw on, inherits wx.Panel """
+    def __init__(self, parent, id, **kwargs):
+        # create a panel
+        wx.Panel.__init__(self, parent, id, **kwargs)
+        # self.SetBackgroundColour("white")
+        self.Bind(wx.EVT_PAINT, self.OnPaint)
+
+    def OnPaint(self, evt):
+        """set up the device context (DC) for painting"""
+        self.dc = wx.PaintDC(self)
+        self.dc.SetPen(wx.Pen("green",style=wx.TRANSPARENT))
+        self.dc.SetBrush(wx.Brush("green", wx.SOLID))
+        # set x, y, w, h for rectangle
+        self.dc.DrawRectangle(83, 356, 8, 15)
+
+        self.dc.SetPen(wx.Pen("red",style=wx.TRANSPARENT))
+        self.dc.SetBrush(wx.Brush("red", wx.SOLID))
+        self.dc.DrawRectangle(70, 356, 8, 15)
+        del self.dc
+
+
+if __name__ == '__main__':
+    app = wx.App(False)
+    frame = EmulatorWindow(None, "MSP430 Emulator")
+    app.MainLoop()

emulator/Makefile

@@ -0,0 +1,86 @@
+all: MSP430 SERVER
+
+MSP430 : main.o utilities.o emu_server.o registers.o memspace.o debugger.o disassembler.o \
+	register_display.o decoder.o flag_handler.o formatI.o formatII.o formatIII.o \
+	usci.o port1.o packet_queue.o bcm.o timer_a.o
+
+	g++ -o MSP430 main.o emu_server.o utilities.o registers.o memspace.o debugger.o disassembler.o \
+	register_display.o decoder.o flag_handler.o formatI.o formatII.o formatIII.o usci.o port1.o bcm.o timer_a.o packet_queue.o -lreadline -lwebsockets -lpthread -lrt -lssl -lcrypto;
+
+libmsp430.so: main.o utilities.o emu_server.o registers.o memspace.o debugger.o disassembler.o \
+	register_display.o decoder.o flag_handler.o formatI.o formatII.o formatIII.o \
+	usci.o port1.o packet_queue.o bcm.o timer_a.o
+	g++ main.o emu_server.o utilities.o registers.o memspace.o debugger.o disassembler.o \
+	register_display.o decoder.o flag_handler.o formatI.o formatII.o formatIII.o usci.o port1.o bcm.o timer_a.o packet_queue.o -lreadline -lwebsockets -lpthread -lrt -lssl -lcrypto -shared -o libmsp430.so;
+
+main.o : main.cpp
+	g++ -c -fPIC main.cpp
+
+utilities.o : devices/utilities.c
+	g++ -c -fPIC devices/utilities.c
+
+registers.o : devices/cpu/registers.c
+	g++ -c -fPIC devices/cpu/registers.c
+
+memspace.o : devices/memory/memspace.c
+	g++ -c -fPIC devices/memory/memspace.c
+
+debugger.o : debugger/debugger.c
+	g++ -c -fPIC debugger/debugger.c
+
+disassembler.o : debugger/disassembler.c
+	g++ -c -fPIC debugger/disassembler.c
+
+register_display.o : debugger/register_display.c
+	g++ -c -fPIC debugger/register_display.c
+
+decoder.o : devices/cpu/decoder.c
+	g++ -c -fPIC devices/cpu/decoder.c
+
+flag_handler.o : devices/cpu/flag_handler.c
+	g++ -c -fPIC devices/cpu/flag_handler.c
+
+formatI.o : devices/cpu/formatI.c
+	g++ -c -fPIC devices/cpu/formatI.c
+
+formatII.o : devices/cpu/formatII.c
+	g++ -c -fPIC devices/cpu/formatII.c
+
+formatIII.o : devices/cpu/formatIII.c
+	g++ -c -fPIC devices/cpu/formatIII.c
+
+bcm.o : devices/peripherals/bcm.c
+	g++ -c -fPIC devices/peripherals/bcm.c
+
+timer_a.o : devices/peripherals/timer_a.c
+	g++ -c -fPIC devices/peripherals/timer_a.c
+
+usci.o : devices/peripherals/usci.c
+	g++ -c -fPIC devices/peripherals/usci.c
+
+port1.o : devices/peripherals/port1.c
+	g++ -c -fPIC devices/peripherals/port1.c
+
+emu_server.o : debugger/websockets/emu_server.cpp
+	g++ -c -fPIC debugger/websockets/emu_server.cpp
+
+packet_queue.o : debugger/websockets/packet_queue.c
+	g++ -c -fPIC debugger/websockets/packet_queue.c
+
+# Server Program
+
+SERVER : server.o
+	cc -o server server.o -lrt -lpthread -lwebsockets -lssl -lcrypto;
+
+server.o : debugger/server/server.c
+	cc -c debugger/server/server.c
+
+
+clean :
+	rm server.o main.o utilities.o emu_server.o registers.o \
+	memspace.o debugger.o disassembler.o \
+	register_display.o decoder.o flag_handler.o formatI.o \
+	formatII.o formatIII.o \
+	usci.o port1.o packet_queue.o bcm.o timer_a.o \
+	*.bin *.tmp *.elf \
+	MSP430 server;

emulator/debugger/debugger.c

@@ -0,0 +1,558 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "debugger.h"
+extern uint8_t* MEMSPACE;
+
+Emulator *local_emu = NULL;
+
+bool exec_cmd (Emulator *emu, char *line, int len) 
+{
+  Cpu *cpu = emu->cpu;
+  Debugger *deb = emu->debugger;
+
+  char cmd[100] = {0};
+  unsigned int op1 = 0, op2 = 0, op3 = 0;
+  int ops;
+
+  char bogus1[100] = {0};
+  uint32_t bogus2 = 0, bogus3 = 0;
+
+  ops = sscanf(line, "%s %u %u", cmd, &op1, &op2);
+  //printf("Got %s, %u, %u - ops %d\n", cmd, op1, op2, ops);
+
+  /* RESET / RESTART
+     
+     Resets the entire virtual machine to it's starting state.
+     Puts the starting address back into Program Counter
+   */
+  if ( !strncasecmp("reset", cmd, sizeof "reset") ||
+       !strncasecmp("restart", cmd, sizeof "restart")) 
+    {      
+      cpu->pc = 0xC000;
+
+      display_registers(emu);
+      disassemble(emu, cpu->pc, 1);
+    }
+  
+  // s [NUM], step NUM instructions forward, defaults to 1 // 
+  else if ( !strncasecmp("s", cmd, sizeof "s") ||
+	    !strncasecmp("step", cmd, sizeof "step")) 
+    {
+      int steps = 1; // 1 step by default
+      uint32_t i;
+
+      if (ops == 2) {
+	steps = (int) op1;
+      }
+
+      for (i = 0;i < steps;i++) {
+	decode(emu, fetch(emu), EXECUTE);
+
+        // Handle Peripherals     
+	handle_bcm(emu);
+	handle_timer_a(emu);
+	handle_port_1(emu);
+	handle_usci(emu); 
+      }
+
+      display_registers(emu);
+      disassemble(emu, cpu->pc, 1);
+  }                                
+  
+  // Quit program //
+  else if ( !strncasecmp("quit", cmd, sizeof "quit") ||
+	    !strncasecmp("q", cmd, sizeof "q")) 
+    {    
+      // This flag stops the main loop in main.c
+      deb->quit = true;
+    }
+  
+  // run the program until a breakpoint is hit //
+  else if ( !strncasecmp("run", cmd, sizeof "run") ||
+	    !strncasecmp("r", cmd, sizeof "r")) 
+    {
+      cpu->running = true;
+      deb->debug_mode = false;
+
+      update_register_display(emu);
+    }
+
+  // Display disassembly of N at HEX_ADDR: dis [N] [HEX_ADDR] //
+  else if ( !strncasecmp("disas", cmd, sizeof "disas") ||
+	    !strncasecmp("dis", cmd, sizeof "dis") ||
+	    !strncasecmp("disassemble", cmd, sizeof "disassemble")) 
+    {
+      uint16_t start_addr = cpu->pc;
+      uint32_t num = 10; 
+      
+      ops = sscanf(line, "%s %u %X", bogus1, &bogus2, &bogus3);
+      
+      if (ops == 2) {
+	sscanf(line, "%s %u", bogus1, &num);
+      }
+      else if (ops == 3) {
+	sscanf(line, "%s %u %X", bogus1, &num, 
+		     (unsigned int *)&start_addr);
+      }
+      
+      disassemble(emu, start_addr, num);
+    }
+
+  else if ( !strncasecmp("dump", cmd, sizeof "dump" )) 
+    {
+      char str[100] = {0};
+      uint16_t start_addr = cpu->pc;
+      uint32_t stride;
+      
+      sscanf(line, "%s %s", bogus1, str);
+      
+      // Is it a direct address or an adress in a register being spec'd 
+      if (str[0] >= '0' && str[0] <= '9') {
+	sscanf(str, "%X", (unsigned int *) &start_addr);
+      }
+      else if (str[0] == '%' || str[0] == 'r' || str[0] == 'R')
+      {
+        uint16_t *p = (uint16_t *)get_reg_ptr(emu, reg_name_to_num(str));
+	    start_addr = *p;
+      }
+      
+      stride = BYTE_STRIDE;
+      dump_memory(emu, MEMSPACE, 0x0, start_addr, stride);	
+    }
+
+  // Set REG/LOC VALUE 
+  else if ( !strncasecmp("set", cmd, sizeof "set") ) 
+    {
+      uint16_t value = 0;
+      char reg_name_or_addr[50] = {0};
+      char *reg_name = NULL;
+      char *addr_str = NULL;
+
+      //print_console(emu, "Not yet implemented.\n");
+      
+      sscanf(line, "%s %s %X", bogus1, reg_name_or_addr, 
+	     (unsigned int*)&value);
+      
+      //printf("Got %s and %X\n", reg_name_or_addr, value);
+
+      // Figure out if the value given is a reg name or addr
+      int res = reg_name_to_num(reg_name_or_addr);
+
+      if (res != -1) {  // If its a reg name
+	reg_name = reg_name_or_addr;
+	printf("In reg part...\n");
+	
+	if (res == 2) { // SR (R2)
+	  set_sr_value(emu, value);
+	}
+	else { // All others
+	  uint16_t *p = (uint16_t*)get_reg_ptr(emu, res);
+	  *p = value;
+	}
+
+	display_registers(emu);
+	disassemble(emu, cpu->pc, 1);
+      }
+      else {
+	addr_str = reg_name_or_addr;
+	printf("In addr part...\n");
+
+	uint16_t virtual_addr = (uint16_t) strtol(addr_str, NULL, 0);
+
+	uint16_t *p = get_addr_ptr(virtual_addr);
+	*p = value;
+      }
+    }
+
+  // break BREAKPOINT_ADDRESS - set breakpoint //
+  else if ( !strncasecmp("break", cmd, sizeof "break") ) 
+    {
+      if (deb->num_bps >= MAX_BREAKPOINTS) {
+	//printf("Breakpoints are full.\n");
+	print_console(emu, "Breakpoints are full.\n");
+
+	return true;
+      }
+    
+      ops = sscanf(line, "%s %X", bogus1, &bogus2);
+      char entry[100] = {0};
+
+      if (ops == 2) {
+	sscanf(line, "%s %X", bogus1, (unsigned int *) 
+	       &deb->bp_addresses[deb->num_bps]);
+      
+	sprintf(entry, "\n\t[Breakpoint [%d] Set]\n", deb->num_bps + 1);
+	//printf("%s", entry);
+	print_console(emu, entry);
+
+	++deb->num_bps;
+      }
+      else {
+	//printf("error\n");
+	print_console(emu, "error\n");
+      }
+    }
+
+  // Display all breakpoints //
+  else if ( !strncasecmp("bps", cmd, sizeof "bps" )) 
+    {
+      char entry[100] = {0};
+
+      if (deb->num_bps > 0) {
+	deb->current_bp = 0;
+
+	while (deb->current_bp < deb->num_bps) {
+	  sprintf(entry, "\t[%d] 0x%04X\n", 
+		  deb->current_bp+1, deb->bp_addresses[deb->current_bp]);
+
+	  //printf("%s", entry);
+	  print_console(emu, entry);
+
+	  ++deb->current_bp;
+	}
+      }
+      else {
+	//printf("You have not set any breakpoints!\n");
+	print_console(emu, "You have not set any breakpoints!\n");
+      }
+    }
+
+  // Display registers //
+  else if ( !strncasecmp("regs", cmd, sizeof "regs")) 
+    {
+      display_registers(emu);
+      disassemble(emu, cpu->pc, 1);
+    }
+
+  // help, display a list of debugger cmds //
+  else if ( !strncasecmp("help", cmd, sizeof "help") ||
+	    !strncasecmp("h", cmd, sizeof "h") ) 
+    {
+      display_help(emu);
+    }
+
+  // End the line loop, next instruction
+  else 
+    {
+      print_console(emu, "\t[Invalid command, type \"help\".]\n");
+    }
+
+  return true;
+}
+
+/* Main command loop */
+bool command_loop (Emulator *emu, char *buf, int len)
+{
+  /*
+  Cpu *cpu = emu->cpu;
+  Debugger *debugger = emu->debugger;
+
+  static uint16_t breakpoint_addresses[MAX_BREAKPOINTS];
+  static uint8_t cur_bp_number = 0;
+
+  char cmd[512];
+  char *line;
+
+  // Check for breakpoints //
+  int i;
+  for (i = 0;i < cur_bp_number;i++) {
+    if (cpu->pc == breakpoint_addresses[i]) {
+      debugger->run = 0; // Stop fast execution //
+      debugger->debug_mode = true;
+      printf("\n\t[Breakpoint %d hit]\n\n", i + 1);
+      break;
+    }
+  }
+
+  if (!debugger->disassemble_mode && debugger->debug_mode) {
+    display_registers(emu);
+    disassemble(emu, cpu->pc, 1);
+  }
+  
+  while (!debugger->run) {
+    bzero(cmd, sizeof cmd);    
+    line = readline("\n>> ");
+
+    if ( strlen(line) >= 1 ) {
+      add_history(line);
+      sscanf(line, "%s", cmd);
+      line += strlen(cmd) + 1;
+    }
+    else {
+      continue;
+    }
+    
+    // reset the virtual machine
+    if ( !strncasecmp("reset", cmd, sizeof "reset") ||
+	 !strncasecmp("restart", cmd, sizeof "restart")) {
+      
+      cpu->pc = 0xC000;
+      break;
+    }
+    // s NUM_STEPS, step X instructions forward, defaults to 1 
+    else if ( !strncasecmp("s", cmd, sizeof "s") ||
+	      !strncasecmp("step", cmd, sizeof "step")) {
+      
+      unsigned int num_of_steps = 0;
+
+      if (line[1] == ' ') {
+	sscanf(line, "%u", &num_of_steps);
+	printf("TODO:Stepping %u\n", num_of_steps);
+      }
+
+      break;
+    }                                 
+
+    // run, run the program until a breakpoint is hit 
+    else if ( !strncasecmp("quit", cmd, sizeof "quit") ||
+	      !strncasecmp("q", cmd, sizeof "q")) {
+      return false;
+    }
+
+    // run, run the program until a breakpoint is hit 
+    else if ( !strncasecmp("run", cmd, sizeof "run") ||
+	      !strncasecmp("r", cmd, sizeof "r")) {
+      debugger->run = true;
+      debugger->debug_mode = false;
+      
+      break;
+    }
+
+    // Display disassembly 
+    else if ( !strncasecmp("disas", cmd, sizeof "disas") ||
+	      !strncasecmp("dis", cmd, sizeof "dis") ||
+	      !strncasecmp("disassemble", cmd, sizeof "disassemble")) {
+
+      uint16_t start_addr;
+      uint32_t num;
+      int res;
+
+      res = sscanf(line, "%X%u", (unsigned int *) &start_addr, &num);
+
+      if (res <= 0) {
+	start_addr = cpu->pc;
+	num = 10;
+      } 
+      else if (res == 1) {
+	num = 10;
+      }
+
+      if (num > 0) 
+	disassemble(emu, start_addr, num);
+
+      continue;
+    }
+
+    // Display all 16 registers 
+    else if ( !strncasecmp("regs", cmd, sizeof "regs")) {
+      display_registers(emu);
+      continue;
+    }
+    
+    // Display all breakpoints 
+    else if ( !strncasecmp("bps", cmd, sizeof "bps" )) {
+      if (cur_bp_number > 0) {
+	int i;
+	for (i = 0;i < cur_bp_number;i++) {
+	  printf("\t[%d] 0x%04X\n", i + 1, breakpoint_addresses[i]);
+	}
+      }
+      else {
+	puts("You have not set any breakpoints!\n");
+      }
+      
+      continue;
+    }
+
+    else if ( !strncasecmp("dump", cmd, sizeof "dump" )) {
+      char param1[33] = {0};
+      uint16_t start_addr;
+      uint32_t stride;
+      sscanf(line, "%s", param1);
+      
+      // Is it a direct address or a an address in a register being spec'd 
+      if (param1[0] >= '0' && param1[0] <= '9') {
+	sscanf(param1, "%X", (unsigned int *) &start_addr);
+      }
+      else if (param1[0] == '%' || param1[0] == 'r' || param1[0] == 'R') {   
+	uint16_t *p = (uint16_t *)get_reg_ptr(emu, reg_name_to_num(param1));
+	start_addr = *p;
+      }
+      
+      stride = BYTE_STRIDE;
+      dump_memory(MEMSPACE, 0x0, start_addr, stride);	
+    }
+
+    // Set REG/LOC VALUE 
+    else if ( !strncasecmp("set", cmd, sizeof "set") ) {
+      int value = 0;
+      char reg_name_or_addr[33];
+      
+      sscanf(line, "%s %X", reg_name_or_addr, &value);
+
+      if ( reg_name_to_num(reg_name_or_addr) != -1 ) {
+	uint16_t *p = (uint16_t *)get_reg_ptr(emu, reg_name_to_num(reg_name_or_addr) );
+	*p = value;
+
+	display_registers(emu);
+	disassemble(emu, cpu->pc, 1);
+      }
+      else {
+	uint16_t virtual_addr = (uint16_t) strtol(reg_name_or_addr, NULL, 0);
+
+	uint16_t *p = get_addr_ptr(virtual_addr);
+	*p = value;
+      }
+
+      continue;
+    }
+
+    // break BREAKPOINT_ADDRESS 
+    else if ( !strncasecmp("break", cmd, sizeof "break") ) {
+      if (cur_bp_number >= MAX_BREAKPOINTS) {
+	printf("Too many breakpoints.\n");
+      }
+      else {
+	sscanf(line, "%X", (unsigned int *) 
+	       &breakpoint_addresses[cur_bp_number]);
+	printf("\n\t[Breakpoint [%d] Set]\n", cur_bp_number + 1);
+	
+	++cur_bp_number;
+      }
+      
+      continue;
+    }
+
+    // help, display a list of debugger cmds 
+    else if ( !strncasecmp("help", cmd, sizeof "help") ||
+	      !strncasecmp("h", cmd, sizeof "h") ) {
+      display_help(emu);
+    }
+
+    // End the line loop, next instruction 
+    else {
+      puts("\t[Invalid command, type \"help\".]");
+      continue;
+    }
+  }
+  */
+  return true;
+}
+
+//##########+++ Dump Memory Function +++##########
+void dump_memory ( Emulator *emu, uint8_t *MEM, uint32_t size, uint32_t start_addr, uint8_t stride)
+{
+  uint32_t i, msp_addr = start_addr;
+  MEM += start_addr;
+  char str[100] = {0};
+
+  puts("");
+  print_console(emu, "\n");
+
+  for (i = 0; i < 32; i += 8) {
+    sprintf(str, "0x%04X:\t", msp_addr);
+
+    printf("%s", str);
+    print_console(emu, str);
+
+    if ( stride == BYTE_STRIDE ) {
+      sprintf(str, "0x%02X  0x%02X  0x%02X  0x%02X  "\
+	      "0x%02X  0x%02X  0x%02X  0x%02X\n",
+	      *(MEM+0),*(MEM+1),*(MEM+2),*(MEM+3),
+	      *(MEM+4),*(MEM+5),*(MEM+6),*(MEM+7));
+
+      printf("%s", str);
+      print_console(emu, str);
+    }
+    else if ( stride == WORD_STRIDE ) {
+      printf("0x%02X%02X  0x%02X%02X  0x%02X%02X  0x%02X%02X\n",
+             *(MEM+0),*(MEM+1),*(MEM+2),*(MEM+3),*(MEM+4),
+	     *(MEM+5),*(MEM+6),*(MEM+7));
+    }
+    else if ( stride == DWORD_STRIDE ) {
+      printf("0x%02X%02X%02X%02X  0x%02X%02X%02X%02X\n",
+             *(MEM+0),*(MEM+1),*(MEM+2),*(MEM+3),*(MEM+4),
+	     *(MEM+5),*(MEM+6),*(MEM+7));
+    }
+    
+    MEM += 8;        // Increase character by 4
+    msp_addr += 8;   // Increase msp_addr by 4
+  }
+  
+  puts("");
+}
+
+void setup_debugger(Emulator *emu)
+{
+  local_emu = emu;
+  Debugger *deb = emu->debugger;
+
+  deb->debug_mode = true;
+  deb->disassemble_mode = false;
+  deb->quit = false;
+
+  deb->web_interface = true;
+  deb->web_server_ready = false;
+  deb->web_firmware_uploaded = false;
+
+  deb->console_interface = false;
+
+  memset(deb->bp_addresses, 0, sizeof(deb->bp_addresses));
+  deb->num_bps = 0;
+  deb->current_bp = 0;
+}
+
+void handle_sigint(int sig)
+{
+  if (local_emu == NULL) return;
+
+  local_emu->cpu->running = false;
+  local_emu->debugger->debug_mode = true;
+}
+
+void register_signal(int sig)
+{
+  signal(sig, handle_sigint);
+}
+
+void handle_breakpoints (Emulator *emu)
+{
+  int i;
+  Cpu *cpu = emu->cpu;
+  Debugger *deb = emu->debugger;
+  char str[100] = {0};
+
+  for (i = 0;i < deb->num_bps;i++) {
+
+    if (cpu->pc == deb->bp_addresses[i]) {
+      cpu->running = false;
+      deb->debug_mode = true;
+      
+      sprintf(str, "\n\t[Breakpoint %d hit]\n\n", i + 1);
+      printf("%s", str);
+      print_console(emu, str);
+      
+      display_registers(emu);
+      disassemble(emu, cpu->pc, 1);
+      
+      return;
+    }
+
+  }
+}

emulator/debugger/debugger.h

@@ -0,0 +1,67 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _DEBUGGER_H_
+#define _DEBUGGER_H_
+
+#include "../main.h"
+#include "../devices/cpu/registers.h"
+#include "../devices/memory/memspace.h"
+
+typedef enum { BYTE_STRIDE, WORD_STRIDE, DWORD_STRIDE } Stride;
+enum { MAX_BREAKPOINTS = 100 };
+
+typedef struct Debugger 
+{
+  bool disassemble_mode;
+  bool debug_mode;
+  bool web_interface;
+  bool console_interface;
+  bool quit;
+
+  unsigned int ws_port;
+
+  pthread_t web_server_thread;
+  bool web_server_ready;
+  bool web_firmware_uploaded;
+
+  pthread_t gui_thread;
+
+  char mnemonic[50];
+
+  uint16_t bp_addresses[MAX_BREAKPOINTS];
+  uint16_t current_bp;
+  uint32_t num_bps;
+
+  // debug server for web interface
+  Server *server;
+} Debugger;
+
+void setup_debugger(Emulator *emu);
+
+void dump_memory(Emulator *emu, uint8_t *MEM, uint32_t size, 
+		 uint32_t start_addr, uint8_t stride);
+
+void handle_sigint(int signal);
+
+bool command_loop(Emulator *emu, char *buf, int len);
+bool exec_cmd (Emulator *emu, char *buf, int len);
+
+void handle_breakpoints (Emulator *emu);
+
+#endif

emulator/debugger/disassembler.c

@@ -0,0 +1,48 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "disassembler.h"
+
+void disassemble(Emulator *emu, uint16_t start_addr, uint8_t times)
+{
+  Cpu *cpu = emu->cpu;
+  Debugger *debugger = emu->debugger;
+
+  uint16_t saved_pc = cpu->pc, opcode;
+  uint32_t i;
+
+  debugger->disassemble_mode = true;
+  cpu->pc = start_addr;
+  
+  for (i = 0;i < times;i++)
+  {
+    char addr_str[32] = {0};
+
+    sprintf(addr_str, "0x%04X:\t", cpu->pc);
+
+    printf("%s", addr_str);
+    if (debugger->web_interface)
+        print_console(emu, addr_str);
+
+    opcode = fetch(emu);    
+    decode(emu, opcode, DISASSEMBLE);
+  }
+
+  debugger->disassemble_mode = false;
+  cpu->pc = saved_pc; // Restore PC
+}

emulator/debugger/disassembler.h

@@ -0,0 +1,28 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _DISASSEMBLER_H_
+#define _DISASSEMBLER_H_
+
+#include "debugger.h"
+#include "../devices/cpu/decoder.h"
+#include "../devices/cpu/registers.h"
+
+void disassemble(Emulator *emu, uint16_t start_addr, uint8_t times);
+
+#endif

emulator/debugger/register_display.c

@@ -0,0 +1,157 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "register_display.h"
+
+/*   Display all 16 registers                                                  
+ *   - Toggle between Common mode and Specific mode                         
+ *     (Common: Display all as R0 - R15)                                   
+ *     (Specific: Display Register usages like PC, SR, etc.)               
+ */
+
+void display_registers(Emulator *emu)
+{
+  Cpu *cpu = emu->cpu;
+  Debugger *debugger = emu->debugger;
+
+  typedef enum {UNDEF, LINUX, WINDOWS} System_t;
+  System_t this_system;
+  uint16_t r2 = sr_to_value(emu);
+
+  char full[1000] = {0};
+
+#ifdef __linux
+  this_system = LINUX;
+#else
+  this_system = UNDEF;
+#endif
+
+  char *v_flag, *n_flag, *z_flag, *c_flag;
+  char *red, *green, *cyan, *clear, *blue, *white, *yellow;
+  char *reg_col, *spec_reg_col, *decor_col, *value_col;
+
+  const char *r0_name = "PC";
+  const char *r1_name = "SP";
+  const char *r2_name = "SR";
+  const char *r3_name = "CG2";
+  const char *r4_name = "R4";
+  const char *r5_name = "R5";
+  const char *r6_name = "R6";
+  const char *r7_name = "R7";
+  const char *r8_name = "R8";
+  const char *r9_name = "R9";
+  const char *r10_name = "R10";
+  const char *r11_name = "R11";
+  const char *r12_name = "R12";
+  const char *r13_name = "R13";
+  const char *r14_name = "R14";
+  const char *r15_name = "R15";    
+
+
+  if (debugger->console_interface) {
+    red = (char*)"\x1b[31;1m";
+    green = (char*)"\x1b[32;1m";
+    cyan = (char*)"\x1b[36;1m";
+    blue = (char*)"\x1b[34;1m";
+    white = (char*)"\x1b[37;1m";
+    yellow = (char*)"\x1b[33;1m";
+    clear = (char*)"";
+
+    reg_col = blue;
+    value_col = white;
+    spec_reg_col = red;
+    decor_col = green;
+
+    v_flag = (char*)"V\x1b[0m";
+    n_flag = (char*)"\x1b[36;1mN\x1b[0m";
+    z_flag = (char*)"\x1b[36;1mZ\x1b[0m";
+    c_flag = (char*)"\x1b[36;1mC\x1b[0m";
+  }
+  else {
+    red = (char*)"";
+    green = (char*)"";
+    cyan = (char*)"";
+    blue = (char*)"";
+    white = (char*)"";
+    yellow = (char*)"";
+    clear = (char*)"";
+
+    reg_col = (char*)"";
+    value_col = (char*)"";
+    spec_reg_col = (char*)"";
+    decor_col = (char*)"";
+
+    v_flag = (char*)"V";
+    n_flag = (char*)"N";
+    z_flag = (char*)"Z";
+    c_flag = (char*)"C";
+  }
+  
+  sprintf(full,
+	  "\n%s%s%s:   %s%04X  "\
+	  "%s%s%s:   %s%04X  "	\
+	  "%s%s%s:   %s%04X  "\
+	  "%s%s%s:  %s%04X  "\
+	  "%s%s%s: %s%d\n"\
+	  "%s%%%s%s%s:  %s%04X  "\
+	  "%s%%%s%s%s:  %s%04X  "\
+	  "%s%%%s%s%s:  %s%04X  "\
+	  "%s%%%s%s%s:  %s%04X  "\
+	  "%s%s%s: %s%d\n"\
+	  "%s%%%s%s%s:  %s%04X  "\
+	  "%s%%%s%s%s:  %s%04X  "\
+	  "%s%%%s%s%s: %s%04X  "\
+	  "%s%%%s%s%s: %s%04X  "\
+	  "%s%s%s: %s%d\n"\
+	  "%s%%%s%s%s: %s%04X  "\
+	  "%s%%%s%s%s: %s%04X  "\
+	  "%s%%%s%s%s: %s%04X  "\
+	  "%s%%%s%s%s: %s%04X  "\
+	  "%s%s%s: %s%d\n\n",
+	 
+	  red, r0_name, decor_col, value_col, (uint16_t)cpu->pc, 
+	  red, r1_name, decor_col, value_col, (uint16_t)cpu->sp, 
+	  red, r2_name, decor_col, value_col, (uint16_t)r2, 
+	  red, r3_name, decor_col, value_col, (uint16_t)cpu->cg2, 
+
+	  cyan, c_flag, decor_col, value_col, cpu->sr.carry,
+
+	  decor_col, reg_col, r4_name, decor_col, value_col, (uint16_t)cpu->r4, 
+	  decor_col, reg_col, r5_name, decor_col, value_col, (uint16_t)cpu->r5, 
+	  decor_col, reg_col, r6_name, decor_col, value_col, (uint16_t)cpu->r6,
+	  decor_col, reg_col, r7_name, decor_col, value_col, (uint16_t)cpu->r7, 
+
+	  cyan, z_flag, decor_col, value_col, cpu->sr.zero,
+
+	  decor_col, reg_col, r8_name, decor_col,value_col, (uint16_t)cpu->r8, 
+	  decor_col, reg_col, r9_name, decor_col,value_col, (uint16_t)cpu->r9, 
+	  decor_col, reg_col, r10_name, decor_col,value_col,(uint16_t)cpu->r10,
+	  decor_col, reg_col, r11_name, decor_col,value_col,(uint16_t)cpu->r11, 
+
+	  cyan, n_flag, decor_col, value_col, cpu->sr.negative, 
+
+	  decor_col, reg_col, r12_name, decor_col,value_col,(uint16_t)cpu->r12, 
+	  decor_col, reg_col, r13_name, decor_col,value_col,(uint16_t)cpu->r13, 
+	  decor_col, reg_col, r14_name, decor_col,value_col,(uint16_t)cpu->r14,
+	  decor_col, reg_col, r15_name, decor_col,value_col,(uint16_t)cpu->r15,
+
+	  cyan, v_flag, decor_col, value_col, cpu->sr.overflow);
+
+  printf("%s", full);
+  print_console(emu, full);
+}

emulator/debugger/register_display.h

@@ -0,0 +1,28 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _REGISTER_DISPLAY_H_
+#define _REGISTER_DISPLAY_H_
+
+#include <stdio.h>
+
+#include "../devices/cpu/registers.h"
+
+void display_registers(Emulator *emu);
+
+#endif

emulator/debugger/server/server.c

@@ -0,0 +1,192 @@
+/*
+    MSP430 Emulator
+    Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+    "MSP430 Emulator" 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.
+
+    "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "server.h"
+
+struct lws *ws = NULL;
+struct lws_context *context;
+pid_t emulator_pids[1000] = {0};
+
+void handle_sigchld(int sig)
+{
+    int saved_errno = errno;
+
+    while (waitpid((pid_t)(-1), 0, WNOHANG) > 0) {}
+    errno = saved_errno;
+}
+
+void signal_callback_handler(int signum)
+{
+    printf("Caught signal %d\n", signum);
+    // Cleanup and close up stuff here
+    int saved_errno = errno;
+
+    while (waitpid((pid_t)(-1), 0, WNOHANG) > 0) {}
+    errno = saved_errno;
+
+    // Terminate program
+    exit(signum);
+}
+
+void web_send (char *buf)
+{
+    int len;
+    char *msg, *begin;
+
+    // send message 
+    len = strlen(buf);
+
+    msg = (char *) malloc(len + LWS_SEND_BUFFER_PRE_PADDING
+			+ LWS_SEND_BUFFER_POST_PADDING);
+
+    begin = msg + LWS_SEND_BUFFER_PRE_PADDING;
+
+    strncpy(begin, buf, len);
+    int n = lws_write(ws, begin, len, LWS_WRITE_TEXT);
+    printf("sent %d bytes of %d len\n", n, len);
+    
+    free(msg);
+}
+
+
+int callback_http (
+		     struct lws *wsi,
+		     enum lws_callback_reasons reason,
+		     void *user, void *in, size_t len)
+{
+    ws = wsi;
+    return 0;
+}
+
+int callback_emu ( struct lws *wsi,
+		     enum lws_callback_reasons reason,
+		     void *user, void *in, size_t len )
+{
+    static unsigned new_ws_port = 9001;
+    char port_str[100] = {0};
+    
+    switch (reason) {
+        case LWS_CALLBACK_PROTOCOL_INIT: {
+            ws = wsi;
+            break;
+        };
+
+        case LWS_CALLBACK_SERVER_WRITEABLE: {
+            pid_t pid;
+            sprintf(port_str, "%u", new_ws_port);
+    
+            // Child (pty)         
+            if( !(pid = fork()) ) {         
+                printf("Child: Got pid #%u\n", pid);
+
+                char * const args[] = {
+                    "nice", "-20", "./MSP430", port_str,
+	    //"./MSP430", port_str,
+	    //"gdb", "-ex", "run", "--args", "./MSP430", port_str,
+                    NULL
+                };
+
+	setpgid(0, 0);         
+	execvp(args[0], args);
+	exit(1);                                
+            }
+
+            // Parent 
+            printf("Parent: Got pid #%u\n", pid);
+            usleep(1000);
+        
+            web_send(port_str);
+            ++new_ws_port;
+
+            lws_close_reason(wsi, 0, NULL, 0);
+
+            break;
+        };
+
+        case LWS_CALLBACK_ESTABLISHED: {
+            puts("connection established");
+
+            lws_callback_on_writable(wsi);
+            break;
+        }
+
+        default: {
+            printf("Some other thing: %d\n", reason); 
+            break;
+            
+            ws = wsi;
+        }
+    }
+    
+    return 0;
+}
+
+int main (int argc, char **argv)
+{
+    int port = 9000;
+
+    struct lws_context_creation_info context_info =
+    {
+        .port = port,
+        .iface = NULL,
+        .protocols = protocols,
+        .ssl_cert_filepath = NULL,
+        .ssl_private_key_filepath = NULL,
+        .ssl_ca_filepath = NULL,
+
+        .gid = -1,
+        .uid = -1,
+
+        .max_http_header_pool = 1,
+        .options = 0,
+    };
+    
+    // create lws context representing this server
+    context = lws_create_context(&context_info);
+    
+    if (context == NULL)
+    {
+        puts("lws init failed\n");
+        return -1;
+    }
+
+    signal(SIGINT, signal_callback_handler);
+
+    // Setup child reaping handler
+    struct sigaction sa;
+    sa.sa_handler = &handle_sigchld;
+    sigemptyset(&sa.sa_mask);
+    sa.sa_flags = SA_RESTART | SA_NOCLDSTOP;
+    if (sigaction(SIGCHLD, &sa, 0) == -1)
+    {
+        perror(0);
+        exit(1);
+    }
+
+    printf("starting server...\n");
+
+    while (true)
+    {
+        lws_service(context, 10); // ms
+        //lws_callback_on_writable_all_protocol(context, &protocols[1]);
+        usleep(1000);
+    }
+
+    lws_context_destroy(context);    
+    return 0;
+}

emulator/debugger/server/server.h

@@ -0,0 +1,63 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _SERVER_H_
+#define _SERVER_H_
+
+#include <string.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <signal.h>
+#include <sys/wait.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <libwebsockets.h>
+
+extern int callback_http (
+                   struct lws *wsi,
+                   enum lws_callback_reasons reason,
+                   void *user, void *in, size_t len);
+
+extern int callback_emu (
+		  struct lws *wsi,
+                  enum lws_callback_reasons reason,
+                  void *user, void *in, size_t len);
+
+static struct lws_protocols protocols [] = 
+{
+    {
+        "http-only",
+        callback_http,
+        0
+    },
+
+    {
+        "emu-protocol",
+        callback_emu,
+        0,
+        1024 * 4,
+        0,
+    },
+
+    {
+        NULL, NULL, 0, 0
+    }
+};
+
+#endif

emulator/debugger/websockets/emu_server.cpp

@@ -0,0 +1,578 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include <string>
+#include "emu_server.h"
+
+#define TXIFG 0x02
+#define RXIFG 0x01
+#define HIGH  1
+#define LOW   0
+#define OUT   1
+#define IN    0
+
+//
+#define MAX_UPLOAD_FILENAME_SIZE 0xFFFF+1
+char UploadFileName[MAX_UPLOAD_FILENAME_SIZE] = {0};
+
+Emulator *emu = NULL;
+uint8_t *data;
+int lent;
+
+void *thrd (void *ctxt) 
+{
+  Usci *usci = (Usci *)ctxt;
+  int i, j = 0;
+  int len = lent;
+  uint8_t *bytes = data;
+
+  //printf("len is %d\nstr is %s\n", len, bytes);
+
+  while (true) {
+    usleep(333);
+    while (*usci->IFG2 & RXIFG);
+    uint8_t thing = *(bytes);
+
+    if (thing == '\n') {                  
+      thing = '\r';
+    }                          
+    
+    //printf("Got 0x%02X '%c'\n", thing, thing);
+    
+    if (*bytes == '\\') {
+      ++bytes;
+      if (*bytes == 'h' || *bytes == 'H') {
+	++bytes;
+	
+	char buf[3] = {*((char*)bytes), *((char*)bytes+1), 0};
+
+	//printf("%s - %s\n", buf, bytes);
+	thing = (uint8_t) strtoul(buf, NULL, 16);
+
+	++bytes;
+      }  
+    }	  
+
+    *usci->UCA0RXBUF = thing;    
+    *usci->IFG2 |= RXIFG;
+	    
+    //printf("\n0x%04X in UCA0RXBUF\n", (uint8_t)*usci->UCA0RXBUF);
+
+    //puts("waiting..");
+    while (*usci->IFG2 & RXIFG);
+    //puts("done");
+    //*usci->IFG2 |= RXIFG;
+    
+    if (*usci->UCA0RXBUF == '\r') break;
+    ++bytes;
+  }
+
+  return NULL;
+}
+
+int callback_emu (struct lws *wsi, enum lws_callback_reasons reason, void *user, void *in, size_t len)
+{
+  Cpu *cpu = emu->cpu;
+  Port_1 *p1 = emu->cpu->p1;
+  Debugger *deb = emu->debugger;
+
+  switch (reason)
+  {
+    case LWS_CALLBACK_ESTABLISHED:
+    {
+      puts("connection established");
+      
+      // Flip ready flag for the emulator to begin
+      deb->web_server_ready = true;
+
+      // get the ball rolling
+      //libwebsocket_callback_on_writable(this, wsi);
+      lws_callback_on_writable(wsi);
+      
+      break;
+    }
+      
+    case LWS_CALLBACK_SERVER_WRITEABLE:
+    {
+        if ( !packet_queue_empty(emu) )
+        {
+            Packet p = packet_dequeue(emu);
+	  
+            void *msg = p.message;
+            size_t msg_len = p.length;
+            uint8_t op = p.opcode;
+	
+            //printf("[%s] of len %u, opcode: %u\n\n",
+            //(char *)msg, (unsigned int)msg_len, op);
+	
+            // Leave room for websock header/trailer & opcode
+            size_t pack_len = msg_len + sizeof(op) + LWS_SEND_BUFFER_PRE_PADDING + LWS_SEND_BUFFER_POST_PADDING;
+	
+            void *packet = malloc(pack_len);
+	
+            // Zero out our packet
+            memset( ((uint8_t*)packet + LWS_SEND_BUFFER_PRE_PADDING), 0, msg_len + sizeof(op) );
+
+            // Place opcode into packet
+            *((uint8_t *)((uint8_t*)packet + LWS_SEND_BUFFER_PRE_PADDING)) = op;
+	
+            // Place message into packet
+            memcpy( ((uint8_t*)packet + LWS_SEND_BUFFER_PRE_PADDING + sizeof(op)), (const void *)p.message, msg_len);
+
+            /*
+            int i;
+            for (i = 0;i < pack_len;i++){
+            printf( "%02X (%c) | ", *((uint8_t *)(packet+i)),
+	            *((char *)(packet+i)) );
+            }
+            puts("\n");
+            */
+
+            lws_write(wsi, ((unsigned char*)(packet))+LWS_SEND_BUFFER_PRE_PADDING, msg_len + sizeof(op), LWS_WRITE_BINARY);
+	  
+            free(p.message);
+            free(packet);
+        }
+
+        static bool p1_0_on = false;
+        static bool p1_1_on = false;
+        static bool p1_2_on = false;
+        static bool p1_3_on = false;
+        static bool p1_4_on = false;
+        static bool p1_5_on = false;
+        static bool p1_6_on = false;
+        static bool p1_7_on = false;
+      
+        // P1.0 ON/OFF
+        if (p1->DIR_0 == OUT)
+        {
+            if (p1->OUT_0 == HIGH)
+            {
+                if (p1_0_on == false)
+                {
+                    send_control(emu, P1_0_ON_PACKET, NULL, 0);
+                    p1_0_on = true;
+                }
+            }
+            else if (p1->OUT_0 == LOW)
+            {
+                if (p1_0_on == true)
+                {
+                    send_control(emu, P1_0_OFF_PACKET, NULL, 0);
+                    p1_0_on = false;
+                }
+            }
+        }
+
+      // P1.1 ON/OFF
+      if (p1->DIR_1 == OUT) {
+	if (p1->OUT_1 == HIGH) {	  
+	  if (p1_1_on == false) {
+	    send_control(emu, P1_1_ON_PACKET, NULL, 0);
+	    p1_1_on = true;
+	  }
+	}
+	else if (p1->OUT_1 == LOW) {
+	  if (p1_1_on == true) {
+	    send_control(emu, P1_1_OFF_PACKET, NULL, 0);
+	    p1_1_on = false;
+	  }
+	}
+      }
+
+      // P1.2 ON/OFF
+      if (p1->DIR_2 == OUT) {
+	if (p1->OUT_2 == HIGH) {	  
+	  if (p1_2_on == false) {
+	    send_control(emu, P1_2_ON_PACKET, NULL, 0);
+        puts("p2 on");
+	    p1_2_on = true;
+	  }
+	}
+	else if (p1->OUT_2 == LOW) {
+	  if (p1_2_on == true) {
+	    send_control(emu, P1_2_OFF_PACKET, NULL, 0);
+	    p1_2_on = false;
+	  }
+	}
+      }
+
+      // P1.3 ON/OFF
+      if (p1->DIR_3 == OUT) {
+	if (p1->OUT_3 == HIGH) {	  
+	  if (p1_3_on == false) {
+	    send_control(emu, P1_3_ON_PACKET, NULL, 0);
+	    p1_3_on = true;
+	  }
+	}
+	else if (p1->OUT_3 == LOW) {
+	  if (p1_3_on == true) {
+	    send_control(emu, P1_3_OFF_PACKET, NULL, 0);
+	    p1_3_on = false;
+	  }
+	}
+      }
+
+      // P1.4 ON/OFF
+      if (p1->DIR_4 == OUT) {
+	if (p1->OUT_4 == HIGH) {	  
+	  if (p1_4_on == false) {
+	    send_control(emu, P1_4_ON_PACKET, NULL, 0);
+	    p1_4_on = true;
+	  }
+	}
+	else if (p1->OUT_4 == LOW) {
+	  if (p1_4_on == true) {
+	    send_control(emu, P1_4_OFF_PACKET, NULL, 0);
+	    p1_4_on = false;
+	  }
+	}
+      }
+
+      // P1.5 ON/OFF
+      if (p1->DIR_5 == OUT) {
+	if (p1->OUT_5 == HIGH) {	  
+	  if (p1_5_on == false) {
+	    send_control(emu, P1_5_ON_PACKET, NULL, 0);
+	    p1_5_on = true;
+	  }
+	}
+	else if (p1->OUT_5 == LOW) {
+	  if (p1_5_on == true) {
+	    send_control(emu, P1_5_OFF_PACKET, NULL, 0);
+	    p1_5_on = false;
+	  }
+	}
+      }
+      
+      // P1.6 ON/OFF
+      if (p1->DIR_6 == OUT) {
+	if (p1->OUT_6 == HIGH) {	  
+	  if (p1_6_on == false) {
+	    send_control(emu, P1_6_ON_PACKET, NULL, 0);
+	    p1_6_on = true;
+	  }
+	}
+	else if (p1->OUT_6 == LOW) {
+	  if (p1_6_on == true) {
+	    send_control(emu, P1_6_OFF_PACKET, NULL, 0);
+	    p1_6_on = false;
+	  }
+	}
+      }
+
+      // P1.7 ON/OFF
+      if (p1->DIR_7 == OUT) {
+	if (p1->OUT_7 == HIGH) {	  
+	  if (p1_7_on == false) {
+	    send_control(emu, P1_7_ON_PACKET, NULL, 0);
+	    p1_7_on = true;
+	  }
+	}
+	else if (p1->OUT_7 == LOW) {
+	  if (p1_7_on == true) {
+	    send_control(emu, P1_7_OFF_PACKET, NULL, 0);
+	    p1_7_on = false;
+	  }
+	}
+      }      
+
+      lws_callback_on_writable( wsi);
+      break;
+    }
+
+    case LWS_CALLBACK_CLIENT_CONNECTION_ERROR: {
+      puts("Connection Error");
+      break;
+    }
+
+    case LWS_CALLBACK_CLOSED: {
+      puts("Connection Closed");
+      exit(0);
+      break;
+    }
+
+    case LWS_CALLBACK_CLIENT_WRITEABLE: {
+      puts("cli writable");
+      break;
+    }
+
+    case LWS_CALLBACK_RECEIVE:
+    {
+        static FILE *fp = NULL;
+        static bool upload_in_progress = false;
+        static uint32_t uploaded_bytes;
+
+        static uint16_t FileSize = 0;
+
+        char *buf = (char *)in;      
+      
+        if (upload_in_progress)
+        { // Continue transaction of upload
+            int i;
+
+            for (i = 0;i < len;i++)
+            {
+                fwrite(&buf[i], 1, 1, fp);
+                uploaded_bytes++;
+
+                if (uploaded_bytes >= FileSize)
+                {
+                    puts("met bytes");
+                    fclose(fp);
+                   // system("msp430-objcopy -O binary tmp.elf tmp.bin");
+                    std::string ObjCopyLine = "objcopy -I elf32-little -O binary \"" + std::string(UploadFileName) + "\" tmp.bin";
+                    system(ObjCopyLine.c_str());
+
+                    deb->web_firmware_uploaded = true;
+                    upload_in_progress = false;
+                    return 0;
+                }
+            }
+	
+            return 0;
+        }
+      
+        unsigned char opcode = buf[0];
+        printf("opcode: %02X\n", *(uint8_t*)in);
+      
+        switch (opcode)
+        {
+            case 0x00: // Upload File
+            {
+                // Get the 32-bit length of the file
+                FileSize = ntohs( *((uint16_t*)((uint8_t*)in + 1)) );
+
+                // Get the 8-bit length of the ASCII filename
+                uint16_t FileNameSize = ntohs(*((uint16_t*)((uint8_t*)in + 3)));
+
+	            printf(" Got file size %u, got data len %u for first callback.\n", (unsigned int)FileSize, (unsigned int)len);
+
+                // Ensure that the file isn't too big, otherwise just quit on the user for now
+	            if (FileSize >= 0xFFFF)
+                {
+                    print_console(emu, "File Size Not Supported. Quitting.\n");
+                    deb->quit = true;
+                    exit(1);
+                }
+
+                // for now assume the entire filename is uploaded, and has no 0 at the end so add one
+                memset(UploadFileName, 0, MAX_UPLOAD_FILENAME_SIZE);
+                memmove(&UploadFileName[0], &buf[5], FileNameSize);
+
+                printf(" Got name size %u, name %s.\n", FileNameSize, UploadFileName);
+
+                upload_in_progress = true;
+	            uploaded_bytes = 0;
+	            fp = fopen(UploadFileName, "wb");
+
+	            // Get Any Bytes that are in with this packet
+	            for (uint32_t i = 5+FileNameSize;i < len;i++)
+                {	         
+	                fwrite(&buf[i], 1, 1, fp);
+	                uploaded_bytes++;
+
+	                if (uploaded_bytes >= FileSize)
+                    {
+	                    puts("met bytes");
+	                    fclose(fp);
+
+	                    //system("msp430-objcopy -O binary tmp.elf tmp.bin");
+                        std::string ObjCopyLine = "objcopy -I elf32-little -O binary \"" + std::string(UploadFileName) + "\" tmp.bin";
+                        system(ObjCopyLine.c_str());
+
+	                    deb->web_firmware_uploaded = true;
+	                    upload_in_progress = false;
+	                    return 0;
+	                }
+	            }
+
+	            break;
+	        }
+
+                 case 0x01: { // PLAY
+	     printf("Got play\n");
+	     cpu->running = true;
+	     deb->debug_mode = false;
+	     update_register_display(emu);
+
+	     return 0;
+	 }
+            
+                 case 0x02: { // PAUSE
+	     printf("Got pause\n");
+
+	     if (cpu->running) {
+	         cpu->running = false;
+	         deb->debug_mode = true;
+
+	         // display first round of registers
+	         display_registers(emu);
+	         disassemble(emu, cpu->pc, 1);
+	         update_register_display(emu);
+	     }
+	     
+	     return 0;
+	 }
+
+                 case 0x03: { // SERIAL DATA
+	     if (len > 1000) exit(1);
+
+	     lent = len - 1;
+	     data = ((uint8_t*)in + 1);
+	     
+	     pthread_t t;
+	     if( pthread_create(&t, NULL, thrd, (void *)cpu->usci ) ) {
+	         fprintf(stderr, "Error creating thread\n");                                        
+	     }
+	     
+	     //printf("Got serial data %s ... %d bytes long\n", 
+	     //(char *)(in + 1), (unsigned int)len - 1);
+	     
+	     return 0;
+	 }
+
+                 case 0x04: { // Console Input Data
+	     if (len > 1000) exit(1);
+
+	     buf = buf + 1;	     
+	     printf("%s\n", buf);
+
+	     if (!cpu->running && deb->debug_mode) {
+	         exec_cmd(emu, buf, len);	    
+	         update_register_display(emu);
+	     }
+
+	     return 0;
+                 }
+            
+                 default: break;
+            }
+        }
+    
+    default: {
+        break;
+    }
+}
+        
+    return 0;
+}
+
+void *web_server (void *ctxt)
+{
+  emu = (Emulator *) ctxt;
+  Debugger *deb = emu->debugger;
+
+    int port = 9001;
+    //struct libwebsocket_context *context;
+    struct lws_context *context;
+
+    struct lws_context_creation_info context_info = {0};
+
+    context_info.port = deb->ws_port;
+    context_info.iface = NULL;
+    context_info.protocols = protocols;
+    context_info.extensions = NULL;
+    context_info.ssl_cert_filepath = NULL;
+    context_info.ssl_private_key_filepath = NULL;
+    context_info.ssl_ca_filepath = NULL;
+    context_info.gid = -1;
+    context_info.uid = -1;
+    context_info.options = 0;
+    context_info.ka_time = false;
+    context_info.ka_probes = false;
+    context_info.ka_interval = false;
+
+  // Initialize packet queuing system
+  init_packet_queue(emu);
+  
+  // create libwebsocket context representing this server
+  context = lws_create_context(&context_info);
+
+  if (context == NULL) {
+    fprintf(stderr, "libwebsocket init failed\n");
+    deb->quit = true;
+    exit(1);
+  }
+    
+  printf("starting server...\n");
+    
+  while (true) {
+    lws_service(context, 10); // ms
+    usleep(1000);
+  }
+
+  lws_context_destroy(context);
+  return NULL;
+}
+
+void send_control(Emulator *emu, uint8_t opcode, void *data, size_t data_size)
+{
+  const static uint8_t NUM_OPCODE_BYTES   = 1;
+  const static uint8_t NUM_DATA_LEN_BYTES = 1;
+
+  if (data == NULL) { // Simple case
+    packet_enqueue(emu, (void *)&opcode, 1, CONTROL_PACKET_OPCODE);
+  }
+  else { // data case ( OP DL DATADATADATA )
+    uint8_t full_packet[NUM_OPCODE_BYTES + NUM_DATA_LEN_BYTES + data_size];
+
+    // First part of packet is the opcode for specific control type
+    memcpy(full_packet, (void *)&opcode, NUM_OPCODE_BYTES);
+
+    // Second part is the data length field for following data
+    memcpy(full_packet + NUM_OPCODE_BYTES, &data_size, 
+	   NUM_DATA_LEN_BYTES);
+
+    // Third part is the data itself.
+    memcpy(full_packet + NUM_OPCODE_BYTES + NUM_DATA_LEN_BYTES, 
+	   data, data_size);
+    
+    /*
+    int i;
+    for (i = 0;i < 1+size;i++) {
+      printf("%02X ", *(uint8_t *)(full_packet + i));
+    }
+    puts(" .");
+    */
+
+    packet_enqueue(emu, (void *)&full_packet, 
+		   NUM_OPCODE_BYTES + NUM_DATA_LEN_BYTES + data_size, 
+		   CONTROL_PACKET_OPCODE);
+    
+  }
+}
+
+void print_serial (Emulator *emu, char *buf) 
+{
+  packet_enqueue(emu, buf, strlen(buf) + 1, SERIAL_PACKET_OPCODE);
+}
+
+void print_console (Emulator *emu, const char *buf)
+{
+    packet_enqueue(emu, (void*)buf, strlen(buf) + 1, CONSOLE_PACKET_OPCODE);
+}
+
+int callback_http (
+		   struct lws *wsi,
+		   //enum libwebsocket_callback_reasons reason, 
+		   enum lws_callback_reasons reason, 
+		   void *user, void *in, size_t len)
+{
+  return 0;
+}

emulator/debugger/websockets/emu_server.h

@@ -0,0 +1,137 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _EMU_SERVER_H_
+#define _EMU_SERVER_H_
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <string.h>
+#include <stdint.h>
+#include <signal.h>
+#include <stdbool.h>
+#include <pthread.h>
+#include <libwebsockets.h>
+
+#include "../../main.h"
+#include "packet_queue.h"
+
+enum {
+  CONTROL_PACKET_OPCODE  = 0x00,
+  CONSOLE_PACKET_OPCODE  = 0x01,
+  SERIAL_PACKET_OPCODE   = 0x02,
+};
+
+enum {
+  P1_0_ON_PACKET  = 0x00,
+  P1_0_OFF_PACKET = 0x01,
+
+  P1_1_ON_PACKET  = 0x02,
+  P1_1_OFF_PACKET = 0x03,
+
+  P1_2_ON_PACKET  = 0x04,
+  P1_2_OFF_PACKET = 0x05,
+
+  P1_3_ON_PACKET  = 0x06,
+  P1_3_OFF_PACKET = 0x07,
+
+  P1_4_ON_PACKET  = 0x08,
+  P1_4_OFF_PACKET = 0x09,
+
+  P1_5_ON_PACKET  = 0x0A,
+  P1_5_OFF_PACKET = 0x0B,
+
+  P1_6_ON_PACKET  = 0x0C,
+  P1_6_OFF_PACKET = 0x0D,
+
+  P1_7_ON_PACKET  = 0x0E,
+  P1_7_OFF_PACKET = 0x0F,
+
+  UPDATE_REG_R0_PACKET = 0x10,
+  UPDATE_REG_R1_PACKET = 0x11,
+  UPDATE_REG_R2_PACKET = 0x12,
+  UPDATE_REG_R3_PACKET = 0x13,
+  UPDATE_REG_R4_PACKET = 0x14,
+  UPDATE_REG_R5_PACKET = 0x15,
+  UPDATE_REG_R6_PACKET = 0x16,
+  UPDATE_REG_R7_PACKET = 0x17,
+  UPDATE_REG_R8_PACKET = 0x18,
+  UPDATE_REG_R9_PACKET = 0x19,
+  UPDATE_REG_R10_PACKET = 0x1A,
+  UPDATE_REG_R11_PACKET = 0x1B,
+  UPDATE_REG_R12_PACKET = 0x1C,
+  UPDATE_REG_R13_PACKET = 0x1D,
+  UPDATE_REG_R14_PACKET = 0x1E,
+  UPDATE_REG_R15_PACKET = 0x1F,
+  UPDATE_ALL_REGS_PACKET = 0x20,
+
+  SERVO_MOTOR = 0x21,
+};
+
+int callback_http (
+                   struct lws *wsi,
+                   enum lws_callback_reasons reason,
+                   void *user, void *in, size_t len);
+
+int callback_emu (
+		  struct lws *wsi,
+		  //enum lws_callback_reasons reason,
+		  enum lws_callback_reasons reason,
+		  void *user, void *in, size_t len);
+
+//static struct libwebsocket_protocols protocols[] = {
+static struct lws_protocols protocols [] = {
+  /* first protocol must always be HTTP handler */
+  {
+    "http-only",   // name
+    callback_http, // callback
+    0              // per_session_data_size
+  },
+
+  {
+    "emu-protocol",
+    callback_emu,
+    0,
+    1024 * 4, // rx buffer size
+    0,        // id
+    0         // user context data
+  },
+
+  {
+    NULL, NULL, 0   /* End of list */
+  }
+};
+
+struct Server {
+  // Pending Packets Queue  
+  Packet *pending_packets_head;
+  Packet *pending_packets_tail;
+  uint32_t packets_queued;
+
+  // Other  
+  bool spin_lock;
+};
+
+void print_console (Emulator *emu, const char *buf);
+void print_serial (Emulator *emu, char *buf);
+void send_control (Emulator *emu, uint8_t opcode, 
+		   void *data, size_t size);
+void *web_server (void *ctxt);
+
+#endif

emulator/debugger/websockets/packet_queue.c

@@ -0,0 +1,154 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "packet_queue.h"
+
+void init_packet_queue (Emulator *emu) 
+{
+  Server *s = emu->debugger->server;
+
+  s->pending_packets_head = NULL;
+  s->pending_packets_tail = NULL;
+  s->packets_queued = 0;
+  s->spin_lock = false;
+}
+
+void destroy_packet_queue (Emulator *emu)
+{
+
+}
+
+bool packet_queue_full (Emulator *emu) 
+{
+  Server *s = emu->debugger->server;
+
+  return false;
+}
+
+bool packet_queue_empty (Emulator *emu) 
+{
+  Server *s = emu->debugger->server;
+
+  if (s->packets_queued > 0 && s->pending_packets_head != NULL) {
+    return false;
+  }
+  else {
+    return true;
+  }
+}
+
+void packet_enqueue (Emulator *emu, void *item, size_t size, 
+		     uint8_t opcode)
+{  
+  Server *s = emu->debugger->server;
+  Packet *head, *tail, *cur;
+  void *heap_item;
+  
+  while (s->spin_lock);
+  s->spin_lock = true;
+
+  if (s->pending_packets_head == NULL) {
+    head = tail = s->pending_packets_head = s->pending_packets_tail = 
+      (Packet *) calloc(1, sizeof(Packet));        
+
+    // copy item data onto the heap so it won't go out of scope
+    heap_item = calloc(1, size);
+    memcpy(heap_item, (const void *)item, size);
+    
+    head->message = heap_item;
+    head->length = size;
+    head->opcode = opcode;
+    head->next = NULL;
+  }
+  else {
+    //printf("IN ELSE OF PACKET_ENQUEUE\n");
+    cur = s->pending_packets_head;
+    while (cur->next != NULL) {
+      cur = cur->next;
+    }
+  
+    cur->next = tail = (Packet *) calloc(1, sizeof(Packet));
+
+    // copy item data onto the heap so it won't go out of scope
+    heap_item = calloc(1, size);
+    memcpy(heap_item, (const void *)item, size);
+
+    cur->next->message = heap_item;
+    cur->next->length = size;
+    cur->next->opcode = opcode;
+    cur->next->next = NULL;
+  }
+
+  s->packets_queued++;
+  s->spin_lock = false;
+  return;
+}
+
+void print_packet_queue (Emulator *emu) 
+{
+  Server *s = emu->debugger->server;
+  Packet *cur = s->pending_packets_head;
+  uint32_t packet = 0;
+
+  printf("packets: %u\n", s->packets_queued);
+
+  while (cur != NULL) {
+    printf("I have %s, len %u, PACKET #%u\n", 
+	   (char *)cur->message, (unsigned int)cur->length, packet++);
+    
+    cur = cur->next;
+  }
+}
+
+Packet packet_dequeue (Emulator *emu) 
+{
+  Server *s = emu->debugger->server;
+  Packet *head, *tail, *saved, ret;
+  
+  while (s->spin_lock);
+  s->spin_lock = true;
+
+  //printf("Count:%d\n", s->packets_queued);
+  //printf("in packet_dequeue(), head = s->pending_packets_head = %p\n",
+  //s->pending_packets_head);
+  head = s->pending_packets_head;
+  //printf("head = %p\n", head);
+
+  if ( packet_queue_empty(emu) ) {
+    printf("EMPTY QUEUE...\n");
+    return ret;
+  }
+  
+  s->pending_packets_head = head->next;
+  
+  //if (s->packets_queued == 2) {
+    //sleep(2);
+  //}
+
+  ret.message = head->message;
+  ret.length  = head->length;
+  ret.opcode  = head->opcode;
+  ret.next    = NULL;
+  
+  //free(head->message);
+  free(head);
+  s->packets_queued--;
+
+  s->spin_lock = false;
+  return ret;
+}

emulator/debugger/websockets/packet_queue.h

@@ -0,0 +1,36 @@
+#pragma once
+
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "../../main.h"
+
+struct Packet {
+  void *message;
+  size_t length;
+  uint8_t opcode;
+
+  Packet *next;
+};
+
+void init_packet_queue (Emulator *emu);
+bool packet_queue_empty (Emulator *emu);
+
+void packet_enqueue (Emulator *emu, void *info, size_t len, uint8_t opcode);
+Packet packet_dequeue (Emulator *emu);
+void print_packet_queue (Emulator *emu);

emulator/devices/cpu/decoder.c

@@ -0,0 +1,148 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "decoder.h"
+
+// ##########+++ CPU Fetch Cycle  +++##########
+uint16_t fetch(Emulator *emu)
+{
+    Cpu *cpu = emu->cpu;
+    uint16_t word, *p;
+    
+    p = (get_addr_ptr(cpu->pc));
+    word = *p;
+
+    cpu->pc += 2;
+    
+    return word;
+}
+
+// ##########+++ CPU Decode Cycle +++##########
+void decode(Emulator *emu, uint16_t instruction, bool disassemble)
+{    
+    Cpu *cpu = emu->cpu;
+    Debugger *debugger = emu->debugger;
+
+    int done = 0;
+    uint8_t FormatId;
+    memset(debugger->mnemonic, 0, sizeof debugger->mnemonic);
+
+    FormatId = (uint8_t)(instruction >> 12);
+
+    if (FormatId == 0x1)
+    {
+        // format II (single operand) instruction
+        decode_formatII(emu, instruction, disassemble);    
+    }        
+    else if (FormatId >= 0x2 && FormatId <= 3)
+    {
+        // format III (jump) instruction
+        decode_formatIII(emu, instruction, disassemble);
+    }
+    else if (FormatId >= 0x4)
+    {
+        // format I (two operand) instruction
+        decode_formatI(emu, instruction, disassemble);
+    }
+    else 
+    {
+        char inv[100] = {0};
+
+        sprintf(inv, "%04X\t[INVALID INSTRUCTION]\n", instruction);
+        print_console(emu, inv);
+        printf("%s", inv);
+        
+        //cpu->pc -= 2;
+        cpu->running = false;
+        debugger->debug_mode = true;
+    }
+}
+
+// Constant Generator
+int16_t run_constant_generator(uint8_t source, uint8_t as_flag) 
+{
+    int16_t generated_constant = 0;
+
+    switch (source)
+    {
+        case 2:
+        {     /* Register R2/SR/CG1 */
+            switch (as_flag)
+            {
+                case 0b10:
+                {     /* +4, bit processing */
+	                generated_constant = 4;
+	                break;
+                }
+                case 0b11:
+                {     /* +8, bit processing */
+	                generated_constant = 8;
+	                break;
+	            }
+                default:
+                {
+	                printf("Invalid as_flag for CG1\n");
+                }
+            }
+            
+            break;
+        }
+            
+        // Register R3/CG2
+        case 3:
+        {
+
+            switch (as_flag)
+            {
+                case 0b00:
+                {     /* 0, word processing */
+	                generated_constant = 0;
+	                break;
+                }
+                case 0b01:
+                {     /* +1 */
+	                generated_constant = 1;
+	                break;
+                }
+                case 0b10:
+                {     /* +2, bit processing */
+	                generated_constant = 2;
+	                break;
+                }
+                case 0b11:
+                {     /* -1, word processing */
+	                generated_constant = -1;
+	                break;
+                }
+                default:
+                {
+	                printf("Invalid as_flag for CG2\n");
+                }
+            }
+
+            break;
+        }
+            
+        default:
+        {
+            printf("Invalid source register for constant generation.\n");
+        }    
+    }
+
+    return generated_constant;
+}

emulator/devices/cpu/decoder.h

@@ -0,0 +1,48 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _DECODER_H_
+#define _DECODER_H_
+
+#include <string.h>
+#include <stdio.h>
+#include <stdint.h>
+
+#include "registers.h"
+#include "../utilities.h"
+#include "formatI.h"
+#include "formatII.h"
+#include "formatIII.h"
+
+int16_t run_constant_generator(uint8_t source, uint8_t as_flag);
+
+void decode(Emulator *emu, uint16_t instruction, bool disassemble);
+
+uint16_t fetch(Emulator *emu);
+
+enum { 
+  WORD, 
+  BYTE 
+};
+
+enum {
+  EXECUTE = 0,
+  DISASSEMBLE,
+};
+
+#endif

emulator/devices/cpu/flag_handler.c

@@ -0,0 +1,139 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "decoder.h"
+#include "flag_handler.h"
+
+/**
+ * @brief Test if the result of the asm instruction is zero
+ * @param result_addr The address of the operation's result
+ * @param bw_flag Byte or Word flag
+ * @return true if zero, false otherwise
+ */
+uint8_t is_zero (uint16_t *result_addr, uint8_t bw_flag)
+{
+  if (bw_flag == WORD) {
+    if (*result_addr == 0 ) {
+      return 1;
+    }
+
+    return 0;
+  }
+  else if (bw_flag == BYTE) {
+    if (*(int8_t *) result_addr == 0) {
+      return 1;
+    }
+
+    return 0;
+  }
+
+  return false;
+}
+
+/**
+ * @brief Test if the result of the asm instruction is negative
+ * @param result_addr The address of the operation's result
+ * @param bw_flag Byte or Word flag
+ * @return true if zero, false otherwise
+ */
+uint8_t is_negative(int16_t *result_addr, uint8_t bw_flag)
+{
+  if (bw_flag == WORD) {
+    if (*result_addr < 0) {
+      return 1;
+    }
+
+    return 0;
+  }
+  else if (bw_flag == BYTE) {
+    if (*((int8_t *) result_addr) < 0) {
+      return 1;
+    }
+
+    return 0;
+  }
+
+  return false;
+}
+
+/**
+ * @brief Test if the result of the asm instruction WILL carry
+ * @param original_dst_value The original value at the destination
+ * @param source_value The value at the source location
+ * @param bw_flag Byte or Word flag
+ * @return true if zero, false otherwise
+ */
+uint8_t is_carried(uint32_t original_dst_value, uint32_t source_value,
+                   uint8_t bw_flag)
+{
+  if (bw_flag == WORD) {
+    if ((65535 - (uint16_t)source_value) < (uint16_t)original_dst_value ||
+        ((original_dst_value + source_value) >> 16) != 0) {
+      return 1;
+    }
+
+    return 0;
+  }
+  else if (bw_flag == BYTE) {
+    if ((255 - (uint8_t)source_value) < (uint8_t)original_dst_value ||
+        ((original_dst_value + source_value) >> 8) != 0) {
+      return 1;
+    }
+
+    return 0;
+  }
+
+  return false;
+}
+
+/**
+ * @brief Test if the result of the asm instruction is overflowed
+ * @param source_value The value at the source operand
+ * @param destination_value The value at the destination operand
+ * @param result A pointer to the result of the operation
+ * @param bw_flag Byte or Word flag
+ * @return true if zero, false otherwise
+ */
+uint8_t is_overflowed(uint16_t source_value, uint16_t destination_value,
+                      uint16_t *result, uint8_t bw_flag)
+{
+  if (bw_flag == WORD) {
+    if ( (source_value >> 15) == (destination_value >> 15) &&
+         (*result >> 15) != (destination_value >> 15) ) {
+      return 1;
+    }
+
+    return 0;
+  }
+  else if (bw_flag == BYTE) {
+    uint8_t dst_prev_value = (uint8_t) destination_value;
+    uint8_t src_value = (uint8_t) source_value;
+
+    if ( (src_value >> 7) == (dst_prev_value >> 7) &&
+         (*(uint8_t *)result >> 7) != (dst_prev_value >> 7)) {
+      return 1;
+    }
+
+    return 0;
+  }
+  else {
+    printf("Error, overflowed function: invalid bw_flag\n");
+  }
+
+  return false;
+}

emulator/devices/cpu/flag_handler.h

@@ -0,0 +1,36 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _FLAG_HANDLER_H_
+#define _FLAG_HANDLER_H_
+
+#include <stdint.h>
+#include <stdlib.h>
+#include "decoder.h"
+
+uint8_t is_overflowed (uint16_t source, uint16_t original_destination,
+                       uint16_t *result_addr, uint8_t bw_flag);
+
+uint8_t is_negative (int16_t *result_addr, uint8_t bw_flag);
+
+uint8_t is_zero (uint16_t *result_addr, uint8_t bw_flag);
+
+uint8_t is_carried (uint32_t original_dst_value, uint32_t source_value,
+                    uint8_t bw_flag);
+
+#endif

emulator/devices/cpu/formatI.c

@@ -0,0 +1,890 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+//##########+++ Decode Format I Instructions +++##########
+//# Format I are double operand of the form:
+//# [CCCC][SSSS][ABaa][DDDD]
+//#
+//# Where C = Opcode, B = Byte/Word flag,
+//# A = Addressing mode for destination
+//# a = Addressing mode for s_reg_name
+//# S = S_Reg_Name, D = Destination
+//########################################################
+
+#include "formatI.h"
+
+void decode_formatI(Emulator *emu, uint16_t instruction, bool disassemble)
+{
+  Cpu *cpu = emu->cpu;
+  Debugger *debugger = emu->debugger;
+
+  uint8_t opcode = (instruction & 0xF000) >> 12;
+  uint8_t source = (instruction & 0x0F00) >> 8;
+  uint8_t as_flag = (instruction & 0x0030) >> 4;
+  uint8_t destination = (instruction & 0x000F);
+  uint8_t ad_flag = (instruction & 0x0080) >> 7;
+  uint8_t bw_flag = (instruction & 0x0040) >> 6;
+
+  char s_reg_name[10], d_reg_name[10];
+
+  char mnemonic[100] = {0};
+  /* String to show hex value of instruction */
+  char hex_str[100] = {0};
+  char hex_str_part[10] = {0};
+
+  sprintf(hex_str, "%04X", instruction);
+
+  /* Source Register pointer */
+  int16_t *s_reg = get_reg_ptr(emu, source);
+  
+  /* Destination Register pointer */
+  int16_t *d_reg = get_reg_ptr(emu, destination);
+
+  reg_num_to_name(source, s_reg_name);      /* Get source register name */
+  reg_num_to_name(destination, d_reg_name); /* Get destination register name */
+
+  uint8_t constant_generator_active = 0;    /* Specifies if CG1/CG2 active */
+  int16_t immediate_constant = 0;           /* Generated Constant */  
+
+  /* Spot CG1 and CG2 Constant generator instructions */
+  if ( (source == 2 && as_flag > 1) || source == 3 ) {
+    constant_generator_active = 1;
+    immediate_constant = run_constant_generator(source, as_flag);
+  }
+  else {
+    constant_generator_active = 0;
+  }
+
+  /* Identify the nature of instruction operand addressing modes */
+  int16_t source_value, source_offset;
+  int16_t destination_offset;
+  uint16_t *destination_addr;
+  char asm_operands[20] = {0}, asm_op2[20] = {0};
+
+  /* Register - Register;     Ex: MOV Rs, Rd */
+  /* Constant Gen - Register; Ex: MOV #C, Rd */ /* 0 */
+  if (as_flag == 0 && ad_flag == 0) {
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = immediate_constant;
+
+      sprintf(asm_operands, "#0x%04X, %s", 
+	      (uint16_t) source_value, d_reg_name);
+    }
+    else {                             /* Source register */
+      source_value = *s_reg;
+      sprintf(asm_operands, "%s, %s", s_reg_name, d_reg_name);
+    }
+
+    destination_addr = (uint16_t *)d_reg;         // Destination Register
+
+    if (!disassemble) {
+      bw_flag == BYTE ? *d_reg &= 0x00FF : 0;
+    }
+  }
+  
+  /* Register - Indexed;      Ex: MOV Rs, 0x0(Rd) */
+  /* Register - Symbolic;     Ex: MOV Rs, 0xD     */
+  /* Register - Absolute;     Ex: MOV Rs, &0xD    */
+  /* Constant Gen - Indexed;  Ex: MOV #C, 0x0(Rd) */ /* 0 */
+  /* Constant Gen - Symbolic; Ex: MOV #C, 0xD     */ /* 0 */
+  /* Constant Gen - Absolute; Ex: MOV #C, &0xD    */ /* 0 */
+  else if (as_flag == 0 && ad_flag == 1) {
+    destination_offset = fetch(emu);
+    
+    sprintf(hex_str_part, "%04X", (uint16_t) destination_offset);
+    strncat(hex_str, hex_str_part, sizeof hex_str);
+
+    destination_addr = get_addr_ptr(*d_reg + destination_offset);
+    
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = immediate_constant;
+      sprintf(asm_operands, "#0x%04X, ", source_value);
+    }
+    else {                             /* Source from register */
+      source_value = *s_reg;
+      sprintf(asm_operands, "%s, ", s_reg_name);
+    }
+
+    if (destination == 0) {            /* Destination Symbolic */
+      uint16_t virtual_addr = *d_reg + destination_offset - 2;
+      destination_addr = get_addr_ptr(virtual_addr);
+
+      sprintf(asm_op2, "0x%04X", (uint16_t) virtual_addr);
+    }
+    else if (destination == 2) {       /* Destination Absolute */
+      destination_addr = get_addr_ptr(destination_offset);
+      sprintf(asm_op2, "&0x%04X", (uint16_t) destination_offset);
+    }
+    else {                             /* Destination Indexed */
+      sprintf(asm_op2, "0x%04X(%s)", 
+	      (uint16_t) destination_offset, d_reg_name);
+    }
+
+    strncat(asm_operands, asm_op2, sizeof asm_op2);
+  }    
+
+  /* Indexed - Register;      Ex: MOV 0x0(Rs), Rd */
+  /* Symbolic - Register;     Ex: MOV 0xS, Rd     */
+  /* Absolute - Register;     Ex: MOV &0xS, Rd    */
+  /* Constant Gen - Register; Ex: MOV #C, Rd      */ /* 1 */
+  else if (as_flag == 1 && ad_flag == 0) {
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = immediate_constant;
+      sprintf(asm_operands, "#0x%04X, %s", source_value, d_reg_name);
+    }
+    else if (source == 0) {            /* Source Symbolic */
+      source_offset = fetch(emu);
+      uint16_t virtual_addr = *s_reg + source_offset - 2;
+
+      source_value = *get_addr_ptr(virtual_addr);	
+
+      sprintf(hex_str_part, "%04X", (uint16_t) source_offset);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      sprintf(asm_operands, "0x%04X, %s", virtual_addr, d_reg_name);
+    }
+    else if (source == 2) {            /* Source Absolute */
+      source_offset = fetch(emu);
+      source_value = *get_addr_ptr(source_offset);
+
+      sprintf(hex_str_part, "%04X", (uint16_t) source_offset);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      sprintf(asm_operands, "&0x%04X, %s", 
+	      (uint16_t) source_offset, d_reg_name);
+    }
+    else {                             /* Source Indexed */
+      source_offset = fetch(emu);
+      source_value = *get_addr_ptr(*s_reg + source_offset);
+
+      sprintf(hex_str_part, "%04X", (uint16_t) source_offset);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      sprintf(asm_operands, "0x%04X(%s), %s", 
+	      (uint16_t) source_offset, s_reg_name, d_reg_name);  
+    }
+
+    destination_addr = (uint16_t *)d_reg;          /* Destination register */
+    
+    if (!disassemble) {
+      bw_flag == BYTE ? *d_reg &= 0x00FF : 0;
+    }
+  }
+
+  /* Indexed - Indexed;       Ex: MOV 0x0(Rs), 0x0(Rd) */
+  /* Symbolic - Indexed;      Ex: MOV 0xS, 0x0(Rd)     */
+  /* Indexed - Symbolic;      Ex: MOV 0x0(Rd), 0xD     */
+  /* Symbolic - Symbolic;     Ex: MOV 0xS, 0xD         */
+  /* Absolute - Indexed;      Ex: MOV &0xS, 0x0(Rd)    */
+  /* Indexed - Absolute;      Ex: MOV 0x0(Rs), &0xD    */
+  /* Absolute - Absolute;     Ex: MOV &0xS, &0xD       */
+  /* Absolute - Symbolic;     Ex: MOV &0xS, 0xD        */
+  /* Symbolic - Absolute;     Ex: MOV 0xS, &0xD        */
+  /* Constant Gen - Indexed;  Ex: MOV #C, 0x0(Rd)      */ /* 1 */
+  /* Constant Gen - Symbolic; Ex: MOV #C, 0xD          */ /* 1 */
+  /* Constant Gen - Absolute; Ex: MOV #C, &0xD         */ /* 1 */
+  else if (as_flag == 1 && ad_flag == 1) {
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = immediate_constant;
+      sprintf(asm_operands, "#0x%04X, ", source_value);
+    }
+    else if (source == 0) {            /* Source Symbolic */
+      source_offset = fetch(emu);
+      uint16_t virtual_addr = cpu->pc + source_offset - 4;
+
+      source_value = *get_addr_ptr(virtual_addr);
+
+      sprintf(hex_str_part, "%04X", (uint16_t) source_offset);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      sprintf(asm_operands, "0x%04X, ", virtual_addr);
+    }
+    else if (source == 2) {            /* Source Absolute */
+      source_offset = fetch(emu);
+      source_value = *get_addr_ptr(source_offset);
+
+      sprintf(hex_str_part, "%04X", (uint16_t) source_offset);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      sprintf(asm_operands, "&0x%04X, ", (uint16_t) source_offset);
+    }
+    else {                             /* Source Indexed */
+      source_offset = fetch(emu);
+      source_value = *get_addr_ptr(*s_reg + source_offset);
+
+      sprintf(hex_str_part, "%04X", (uint16_t)source_offset);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      sprintf(asm_operands, "0x%04X(%s), ", 
+	      (uint16_t) source_offset, s_reg_name);
+    }
+      
+    destination_offset = fetch(emu);
+
+    sprintf(hex_str_part, "%04X", (uint16_t) destination_offset);
+    strncat(hex_str, hex_str_part, sizeof hex_str);
+
+    if (destination == 0) {        /* Destination Symbolic */
+      uint16_t virtual_addr = cpu->pc + destination_offset - 2;
+
+      destination_addr = get_addr_ptr(virtual_addr);
+      sprintf(asm_op2, "0x%04X", virtual_addr);
+    }
+    else if (destination == 2) {   /* Destination Absolute */
+      destination_addr = get_addr_ptr(destination_offset);
+      sprintf(asm_op2, "&0x%04X", (uint16_t) destination_offset);
+    }
+    else {                         /* Destination indexed */
+      destination_addr = get_addr_ptr(*d_reg + destination_offset);
+      sprintf(asm_op2, "0x%04X(%s)", (uint16_t)destination_offset, d_reg_name);
+    }
+
+    strncat(asm_operands, asm_op2, sizeof asm_op2);
+  }
+
+  /* Indirect - Register;     Ex: MOV @Rs, Rd */
+  /* Constant Gen - Register; Ex: MOV #C, Rd  */ /* 2, 4 */
+  else if (as_flag == 2 && ad_flag == 0) {
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = immediate_constant;
+      sprintf(asm_operands, "#0x%04X, %s", immediate_constant, d_reg_name);
+    }
+    else {                             /* Source Indirect */
+      source_value = *get_addr_ptr(*s_reg);
+      sprintf(asm_operands, "@%s, %s", s_reg_name, d_reg_name);
+    }
+
+    destination_addr = (uint16_t*)d_reg;          /* Destination Register */
+
+    if (!disassemble) {
+      bw_flag == BYTE ? *d_reg &= 0x00FF : 0;
+    }
+  }
+
+  /* Indirect - Indexed;      Ex: MOV @Rs, 0x0(Rd)   */
+  /* Indirect - Symbolic;     Ex: MOV @Rs, 0xD       */
+  /* Indirect - Absolute;     Ex: MOV @Rs, &0xD      */
+  /* Constant Gen - Indexed;  Ex: MOV #C, 0x0(Rd)    */ /* 2, 4 */
+  /* Constant Gen - Symbolic; Ex: MOV #C, 0xD        */ /* 2, 4 */
+  /* Constant Gen - Absolute; Ex: MOV #C, &0xD       */ /* 2, 4 */
+  else if (as_flag == 2 && ad_flag == 1) {
+    destination_offset = fetch(emu);
+
+    sprintf(hex_str_part, "%04X", (uint16_t) destination_offset);
+    strncat(hex_str, hex_str_part, sizeof hex_str);
+
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = immediate_constant;
+      sprintf(asm_operands, "#0x%04X, ", source_value);
+    }
+    else {                             /* Source Indirect */
+      source_value = *get_addr_ptr(*s_reg);
+      sprintf(asm_operands, "@%s, ", s_reg_name);
+    }
+
+    if (destination == 0) {        /* Destination Symbolic */
+      uint16_t virtual_addr = cpu->pc + destination_offset - 2;
+
+      destination_addr = get_addr_ptr(virtual_addr);
+      sprintf(asm_op2, "0x%04X", virtual_addr);
+    }
+    else if (destination == 2) {   /* Destination Absolute */
+      destination_addr = get_addr_ptr(destination_offset);
+      sprintf(asm_op2, "&0x%04X", destination_offset);
+    }
+    else {                         /* Destination Indexed */
+      destination_addr = get_addr_ptr(*d_reg + destination_offset);
+      sprintf(asm_op2, "0x%04X(%s)", (uint16_t)destination_offset, d_reg_name);
+    }
+
+    strncat(asm_operands, asm_op2, sizeof asm_op2);
+  }
+
+  /* Indirect Inc - Register; Ex: MOV @Rs+, Rd */
+  /* Immediate - Register;    Ex: MOV #S, Rd   */
+  /* Constant Gen - Register; Ex: MOV #C, Rd   */ /* -1, 8 */
+  else if (as_flag == 3 && ad_flag == 0) {  
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = immediate_constant;
+      sprintf(asm_operands, "#0x%04X, %s", 
+	      (uint16_t) source_value, d_reg_name);
+    }
+    else if (source == 0) {            /* Source Immediate */
+      source_value = fetch(emu);
+	
+      sprintf(hex_str_part, "%04X", (uint16_t) source_value);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      if (bw_flag == WORD) {
+	sprintf(asm_operands, "#0x%04X, %s", 
+		(uint16_t) source_value, d_reg_name);
+      }
+      else if (bw_flag == BYTE) {
+	sprintf(asm_operands, "#0x%04X, %s",
+		(uint8_t) source_value, d_reg_name);
+      }
+    }
+    else {                              /* Source Indirect AutoIncrement */
+      source_value = *get_addr_ptr(*s_reg);
+
+      sprintf(asm_operands, "@%s+, %s", s_reg_name, d_reg_name);
+      
+      if (!disassemble) {
+	bw_flag == WORD ? *s_reg += 2 : (*s_reg += 1);
+      }
+    }
+
+    destination_addr = (uint16_t*)d_reg;           /* Destination Register */
+
+    if (!disassemble) {
+      bw_flag == BYTE ? *d_reg &= 0x00FF : 0;
+    }
+  }
+
+  /* Indirect Inc - Indexed;  Ex: MOV @Rs+, 0x0(Rd) */
+  /* Indirect Inc - Symbolic; Ex: MOV @Rs+, 0xD     */
+  /* Indirect Inc - Absolute; Ex: MOV @Rs+, &0xD    */
+  /* Immediate - Indexed;     Ex: MOV #S, 0x0(Rd)   */
+  /* Immediate - Symbolic;    Ex: MOV #S, 0xD       */
+  /* Immediate - Absolute;    Ex: MOV #S, &0xD      */
+  /* Constant Gen - Indexed;  Ex: MOV #C, 0x0(Rd)   */ /* -1, 8 */
+  /* Constant Gen - Symbolic; Ex: MOV #C, 0xD       */ /* -1, 8 */
+  /* Constant Gen - Absolute; Ex: MOV #C, &0xD      */ /* -1, 8 */
+  else if (as_flag == 3 && ad_flag == 1) {
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = immediate_constant;
+      sprintf(asm_operands, "#0x%04X, ", (uint16_t)source_value);
+    }
+    else if (source == 0) {            /* Source Immediate */
+      source_value = fetch(emu);
+
+      sprintf(hex_str_part, "%04X", (uint16_t) source_value);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      sprintf(asm_operands, "#0x%04X, ", (uint16_t)source_value);
+    }
+    else {                             /* Source Indirect Auto Increment */
+      source_value = *get_addr_ptr(*s_reg); 
+
+      sprintf(asm_operands, "@%s+, ", s_reg_name);	
+
+      if (!disassemble) {
+	bw_flag == WORD ? *s_reg += 2 : (*s_reg += 1);
+      }
+    }
+
+    destination_offset = fetch(emu);
+
+    sprintf(hex_str_part, "%04X", (uint16_t) destination_offset);
+    strncat(hex_str, hex_str_part, sizeof hex_str);
+
+    if (destination == 0) {        /* Destination Symbolic */
+      uint16_t virtual_addr = cpu->pc + destination_offset - 2;
+	
+      destination_addr = get_addr_ptr(virtual_addr);
+      sprintf(asm_op2, "0x%04X", virtual_addr);
+    }
+    else if (destination == 2) {   /* Destination Absolute */
+      destination_addr = get_addr_ptr(destination_offset);
+      sprintf(asm_op2, "&0x%04X", (uint16_t) destination_offset);
+    }
+    else {                         /* Destination Indexed */
+      destination_addr = get_addr_ptr(*d_reg + destination_offset);
+
+      sprintf(asm_op2, "0x%04X(%s)", 
+	      (uint16_t) destination_offset, d_reg_name);
+    }
+
+    strncat(asm_operands, asm_op2, sizeof asm_op2);
+  }
+  
+
+  if (!disassemble) {
+    switch (opcode) {
+      
+      /* MOV SOURCE, DESTINATION
+       *   Ex: MOV #4, R6
+       *
+       * SOURCE = DESTINATION
+       *
+       * The source operand is moved to the destination. The source operand is 
+       * not affected. The previous contents of the destination are lost.
+       * 
+       */
+      case 0x4: {
+
+        if (bw_flag == WORD) {		
+          *destination_addr = source_value;
+        }
+        else if (bw_flag == BYTE) {
+          *((uint8_t *) destination_addr) = (uint8_t) source_value;
+        }
+        
+        break;
+      }
+
+      /* ADD SOURCE, DESTINATION 
+       *   Ex: ADD R5, R4
+       * 
+       * The source operand is added to the destination operand. The source op
+       * is not affected. The previous contents of the destination are lost.
+       *
+       * DESTINATION = SOURCE + DESTINATION
+       *   
+       * N: Set if result is negative, reset if positive
+       * Z: Set if result is zero, reset otherwise
+       * C: Set if there is a carry from the result, cleared if not
+       * V: Set if an arithmetic overflow occurs, otherwise reset  
+       *
+       */
+      case 0x5:{
+
+        uint16_t original_dst_value = *destination_addr;
+
+        if (bw_flag == WORD) {
+          *destination_addr += source_value;
+        }
+        else if (bw_flag == BYTE) {
+          *((uint8_t *) destination_addr) += (uint8_t) source_value;
+        }
+
+        cpu->sr.zero = is_zero(destination_addr, bw_flag);
+        cpu->sr.negative = is_negative((int16_t*)destination_addr, bw_flag);
+
+        cpu->sr.carry = is_carried(original_dst_value, source_value, bw_flag);
+
+        cpu->sr.overflow = is_overflowed(source_value, original_dst_value, 
+          			  destination_addr, bw_flag);
+
+        break;
+      }
+
+      /* ADDC SOURCE, DESTINATION 
+       *   Ex: ADDC R5, R4
+       *
+       * DESTINATION += (SOURCE + C)
+       *
+       * N: Set if result is negative, reset if positive
+       * Z: Set if result is zero, reset otherwise
+       * C: Set if there is a carry from the result, cleared if not
+       * V: Set if an arithmetic overflow occurs, otherwise reset  
+       *
+       */
+      case 0x6:{
+
+        uint16_t original_dst_value = *destination_addr;
+
+        if (bw_flag == WORD) {
+          *destination_addr += source_value + cpu->sr.carry;
+        }
+        else if (bw_flag == BYTE) {
+          *((uint8_t *) destination_addr) += (uint8_t) source_value + cpu->sr.carry;
+        }
+        
+        cpu->sr.zero = is_zero(destination_addr, bw_flag);
+        cpu->sr.negative = is_negative((int16_t*)destination_addr, bw_flag);
+
+        cpu->sr.carry = is_carried(original_dst_value, source_value, bw_flag);
+
+        cpu->sr.overflow = is_overflowed(source_value, original_dst_value, 
+          			  destination_addr, bw_flag);
+        break;
+      }
+ 
+      /* SUBC SOURCE, DESTINATION
+       *   Ex: SUB R4, R5
+       *
+       *   DST += ~SRC + C
+       *
+       *  N: Set if result is negative, reset if positive
+       *  Z: Set if result is zero, reset otherwise
+       *  C: Set if there is a carry from the MSB of the result, reset otherwise.
+       *     Set to 1 if no borrow, reset if borrow.
+       *  V: Set if an arithmetic overflow occurs, otherwise reset
+       *
+       *
+       */
+      case 0x7:{
+        
+        int16_t original_dst_value = *destination_addr;
+        source_value = ~source_value; /* 1's comp */
+        
+        if (bw_flag == WORD) {
+          *(int16_t *)destination_addr += source_value + cpu->sr.carry; 
+        }
+        else if (bw_flag == BYTE) {
+          *(int8_t *)destination_addr += (int8_t) source_value + cpu->sr.carry;
+        }
+
+        cpu->sr.zero = is_zero(destination_addr, bw_flag);
+        cpu->sr.negative = is_negative((int16_t*)destination_addr, bw_flag);
+
+        cpu->sr.carry = is_carried(original_dst_value, source_value, bw_flag);
+
+        cpu->sr.overflow = is_overflowed(source_value, original_dst_value, 
+          			  destination_addr, bw_flag);
+        break;
+      }
+
+      /* SUB SOURCE, DESTINATION
+       *   Ex: SUB R4, R5
+       *
+       *   DST -= SRC
+       *
+       *  N: Set if result is negative, reset if positive
+       *  Z: Set if result is zero, reset otherwise
+       *  C: Set if there is a carry from the MSB of the result, reset otherwise.
+       *     Set to 1 if no borrow, reset if borrow.
+       *  V: Set if an arithmetic overflow occurs, otherwise reset
+       *  TODO: SUBTRACTION OVERFLOW FLAG ERROR
+       */  
+
+      case 0x8:{
+
+        int16_t original_dst_value = *destination_addr;
+        source_value = ~source_value + 1;
+ 
+        if (bw_flag == WORD) {
+          *(uint16_t *)destination_addr += source_value; 
+        }
+        else if (bw_flag == BYTE) {
+          *(uint8_t *)destination_addr += (uint8_t) source_value;
+        }
+
+        cpu->sr.zero = is_zero(destination_addr, bw_flag);
+        cpu->sr.negative = is_negative((int16_t*)destination_addr, bw_flag);
+        
+        if ( is_carried(~source_value, 1, bw_flag) ||
+             is_carried(original_dst_value, source_value, bw_flag) ) {
+
+          cpu->sr.carry = true;
+        }
+
+        cpu->sr.overflow = is_overflowed(source_value, original_dst_value, 
+          			  destination_addr, bw_flag);
+        break;
+      }
+
+      /* CMP SOURCE, DESTINATION
+       *
+       * N: Set if result is negative, reset if positive (src ≥ dst)
+       * Z: Set if result is zero, reset otherwise (src = dst)
+       * C: Set if there is a carry from the MSB of the result, reset otherwise
+       * V: Set if an arithmetic overflow occurs, otherwise reset   
+       * TODO: Fix overflow error
+       */
+      case 0x9:{
+        
+        int16_t original_dst_value = *destination_addr;
+        uint16_t unsigned_source_value = ((uint16_t)~source_value + 1);      
+        int16_t result;
+
+        bool early_carry = is_carried((uint16_t)~source_value, 1, bw_flag);
+
+        if (bw_flag == WORD) {
+          result = *destination_addr + (uint16_t) unsigned_source_value; 
+        }
+        else if (bw_flag == BYTE) {
+          result = *(uint8_t *)destination_addr + (uint8_t)unsigned_source_value;
+        }
+        
+        cpu->sr.negative = is_negative(&result, bw_flag);      
+        cpu->sr.zero = is_zero((uint16_t*)&result, bw_flag);
+
+        /* Check if the carry happens durring conversion to 2's comp */
+        if (! early_carry) {
+          cpu->sr.carry = is_carried(original_dst_value, unsigned_source_value, bw_flag);
+        }
+        else {
+          cpu->sr.carry = true;
+        }
+
+        cpu->sr.overflow = is_overflowed(unsigned_source_value, original_dst_value, (uint16_t*)&result, bw_flag);
+        break;
+      }
+
+      /* DADD SOURCE, DESTINATION
+       *
+       */
+      case 0xA:{
+
+        if (bw_flag == WORD) {
+          
+        }
+        else if (bw_flag == BYTE) {
+          
+        }
+        
+        break;
+      }
+
+      /* BIT SOURCE, DESTINATION
+       *
+       * N: Set if MSB of result is set, reset otherwise
+       * Z: Set if result is zero, reset otherwise
+       * C: Set if result is not zero, reset otherwise (.NOT. Zero)
+       * V: Reset
+      */
+      case 0xB:{
+
+        if (bw_flag == WORD) {
+          uint16_t result = ((uint16_t) source_value) & (*destination_addr);
+
+          cpu->sr.zero = (result == 0);
+          cpu->sr.negative = result >> 15;
+          cpu->sr.carry = (result != 0);
+        }
+        else if (bw_flag == BYTE) {
+          uint8_t result = 
+            ((uint8_t) source_value) & (*(uint8_t *) destination_addr);
+
+          cpu->sr.zero = (result == 0);
+          cpu->sr.negative = result >> 7;
+          cpu->sr.carry = (result != 0);
+        }
+
+        cpu->sr.overflow = false;
+
+        break;
+      }     
+
+      /* BIC SOURCE, DESTINATION
+       *
+       * No status bits affected
+       */
+      case 0xC:{
+
+        if (bw_flag == WORD) {
+          *destination_addr &= (uint16_t) ~source_value;
+        }
+        else if (bw_flag == BYTE) {
+          *(uint8_t *) destination_addr &= (uint8_t) ~source_value;	
+        }
+        
+        break;
+      }
+
+      /* BIS SOURCE, DESTINATION
+       *
+       */
+      case 0xD:{
+
+        if (bw_flag == WORD) {
+          *destination_addr |= (uint16_t) source_value;
+        }
+        else if (bw_flag == BYTE) {
+          *(uint8_t *) destination_addr |= (uint8_t) source_value;	
+        }
+        
+        break;
+      }
+      
+      /* XOR SOURCE, DESTINATION
+       *
+       * N: Set if result MSB is set, reset if not set
+       * Z: Set if result is zero, reset otherwise
+       * C: Set if result is not zero, reset otherwise ( = .NOT. Zero)
+       * V: Set if both operands are negative
+       */
+      case 0xE:{
+
+        if (bw_flag == WORD) {
+          cpu->sr.overflow = 
+            (*destination_addr >> 15) && ((uint16_t)source_value >> 15);
+
+          *destination_addr ^= (uint16_t)source_value;	
+
+          cpu->sr.negative = (*destination_addr) >> 15;
+          cpu->sr.zero = (*destination_addr == 0);
+          cpu->sr.carry = (*destination_addr != 0);
+        }
+        else if (bw_flag == BYTE) {
+          cpu->sr.overflow = 
+            (*(uint8_t *)destination_addr >> 7) && ((uint8_t)source_value >> 7);
+
+          *(uint8_t *) destination_addr ^= (uint8_t) source_value;	
+
+          cpu->sr.negative = (*(uint8_t *) destination_addr) >> 7;
+          cpu->sr.zero = (*(uint8_t *)destination_addr == 0);
+          cpu->sr.carry = (*(uint8_t *)destination_addr != 0);
+        }
+        
+        break;
+      }
+
+      /* AND SOURCE, DESTINATION
+       *
+       *  N: Set if result MSB is set, reset if not set
+       *  Z: Set if result is zero, reset otherwise
+       *  C: Set if result is not zero, reset otherwise ( = .NOT. Zero)
+       *  V: Reset
+       */
+      case 0xF:{
+
+        if (bw_flag == WORD) {
+          *destination_addr &= (uint16_t)source_value;	
+
+          cpu->sr.negative = (*destination_addr) >> 15;
+          cpu->sr.zero = (*destination_addr == 0);
+          cpu->sr.carry = (*destination_addr != 0);
+        }
+        else if (bw_flag == BYTE) {
+          *(uint8_t *) destination_addr &= (uint8_t) source_value;	
+
+          cpu->sr.negative = (*(uint8_t *) destination_addr) >> 7;
+          cpu->sr.zero = (*(uint8_t *)destination_addr == 0);
+          cpu->sr.carry = (*(uint8_t *)destination_addr != 0);
+        }
+
+        cpu->sr.overflow = false;
+        
+        break;
+      }
+
+    } //# End of switch
+
+  } // End of if
+
+
+
+  // DISASSEMBLY MODE
+  else {
+    switch (opcode) {      
+      case 0x4: {
+	bw_flag == WORD ? 
+          strncpy(mnemonic, "MOV", sizeof mnemonic) :
+          strncpy(mnemonic, "MOV.B", sizeof mnemonic);
+
+        break;
+      }
+      case 0x5: {
+        bw_flag == WORD ? 
+          strncpy(mnemonic, "ADD", sizeof mnemonic) :
+          strncpy(mnemonic, "ADD.B", sizeof mnemonic);
+
+        break;
+      }
+      case 0x6: {
+        bw_flag == WORD ? 
+          strncpy(mnemonic, "ADDC", sizeof mnemonic) :
+          strncpy(mnemonic, "ADDC.B", sizeof mnemonic);
+	
+        break;
+      }
+      case 0x7: {
+        bw_flag == WORD ? 
+          strncpy(mnemonic, "SUBC", sizeof mnemonic) :
+          strncpy(mnemonic, "SUBC.B", sizeof mnemonic);
+        
+        break;
+      }
+      case 0x8: {
+        bw_flag == WORD ? 
+          strncpy(mnemonic, "SUB", sizeof mnemonic) :
+          strncpy(mnemonic, "SUB.B", sizeof mnemonic);
+
+        break;
+      }
+      case 0x9: {
+        bw_flag == WORD ? 
+          strncpy(mnemonic, "CMP", sizeof mnemonic) :
+          strncpy(mnemonic, "CMP.B", sizeof mnemonic);
+
+        break;
+      }
+      case 0xA: {
+	bw_flag == WORD ? 
+          strncpy(mnemonic, "DADD", sizeof mnemonic) :
+          strncpy(mnemonic, "DADD.B", sizeof mnemonic);
+        
+        break;
+      }
+      case 0xB: {
+        bw_flag == WORD ? 
+          strncpy(mnemonic, "BIT", sizeof mnemonic) :
+          strncpy(mnemonic, "BIT.B", sizeof mnemonic);
+
+        break;
+      }     
+      case 0xC: {
+        bw_flag == WORD ? 
+          strncpy(mnemonic, "BIC", sizeof mnemonic) :
+          strncpy(mnemonic, "BIC.B", sizeof mnemonic);
+                
+        break;
+      }
+      case 0xD: {
+        bw_flag == WORD ? 
+          strncpy(mnemonic, "BIS", sizeof mnemonic) :
+          strncpy(mnemonic, "BIS.B", sizeof mnemonic);
+                
+        break;
+      }
+      case 0xE: {
+        bw_flag == WORD ? 
+          strncpy(mnemonic, "XOR", sizeof mnemonic) :
+          strncpy(mnemonic, "XOR.B", sizeof mnemonic);
+        
+        break;
+      }
+      case 0xF: {
+        bw_flag == WORD ? 
+          strncpy(mnemonic, "AND", sizeof mnemonic) :
+          strncpy(mnemonic, "AND.B", sizeof mnemonic);
+        
+        break;
+      }
+
+    } //# End of switch
+
+    strncat(mnemonic, "\t", sizeof mnemonic);
+    strncat(mnemonic, asm_operands, sizeof mnemonic);
+    strncat(mnemonic, "\n", sizeof mnemonic);
+
+    if (disassemble && emu->debugger->debug_mode) { 
+      int i;
+      char one = 0, two = 0;
+
+      // Make little endian big endian
+      for (i = 0;i < strlen(hex_str);i += 4) {
+	one = hex_str[i];
+	two = hex_str[i + 1];
+	
+	hex_str[i] = hex_str[i + 2];
+	hex_str[i + 1] = hex_str[i + 3];
+
+	hex_str[i + 2] = one;
+	hex_str[i + 3] = two;
+      }
+
+      printf("%s", hex_str);
+      print_console(emu, hex_str);
+
+      for (i = strlen(hex_str);i < 12;i++) {
+	printf(" ");
+	print_console(emu, " ");
+      }
+      
+      printf("\t%s", mnemonic);
+      
+      print_console(emu, "\t");
+      print_console(emu, mnemonic);
+    }
+
+  }
+};

emulator/devices/cpu/formatI.h

@@ -0,0 +1,27 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _DECODE_FORMATI_H
+#define _DECODE_FORMATI_H
+
+#include "flag_handler.h"
+#include "../utilities.h"
+
+void decode_formatI(Emulator *emu, uint16_t instruction, bool disassemble);
+
+#endif

emulator/devices/cpu/formatII.c

@@ -0,0 +1,431 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+//##########+++ Decode Format II Instructions +++#########
+//# Format II are single operand of the form:
+//#   [0001][00CC][CBAA][SSSS]
+//# 
+//# Where C = Opcode, B = Byte/Word flag, 
+//#       A = Addressing mode for source
+//#       S = Source 
+//########################################################
+
+#include "formatII.h"
+#include "decoder.h"
+#include "../utilities.h"
+
+void decode_formatII(Emulator *emu, uint16_t instruction, bool disassemble)
+{
+  Cpu *cpu = emu->cpu;
+  Debugger *debugger = emu->debugger;
+
+  uint8_t opcode = (instruction & 0x0380) >> 7;
+  uint8_t bw_flag = (instruction & 0x0040) >> 6;
+  uint8_t as_flag = (instruction & 0x0030) >> 4;
+  uint8_t source = (instruction & 0x000F);
+  
+  char reg_name[10];
+  reg_num_to_name(source, reg_name);
+  
+  uint16_t *reg = (uint16_t * )get_reg_ptr(emu, source);
+  uint16_t bogus_reg; /* For immediate values to be operated on */
+
+  uint8_t constant_generator_active = 0;    /* Specifies if CG1/CG2 active */
+  int16_t immediate_constant = 0;           /* Generated Constant */
+
+  char mnemonic[100] = {0};
+  /* String to show hex value of instruction */
+  char hex_str[100] = {0};
+  char hex_str_part[10] = {0};
+
+  sprintf(hex_str, "%04X", instruction);
+
+  /*
+  printf("Opcode: 0x%01X  Source bits: 0x%01X\nAS_Flag: 0x%01X  "\
+	 "BW_Flag: 0x%01X\n",
+         opcode, source, as_flag, bw_flag);
+  */
+
+  /* Spot CG1 and CG2 Constant generator instructions */
+  if ( (source == 2 && as_flag > 1) || source == 3 ) {
+    constant_generator_active = 1;
+    immediate_constant = run_constant_generator(source, as_flag);
+  }
+  else {
+    constant_generator_active = 0;
+  }
+
+  /* Identify the nature of instruction operand addressing modes */
+  int16_t source_value, source_offset;
+  uint16_t *source_address;
+  char asm_operand[50] = {0};
+
+  /* Register;     Ex: PUSH Rd */
+  /* Constant Gen; Ex: PUSH #C */   /* 0 */
+  if (as_flag == 0) {
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = bogus_reg = immediate_constant;
+      source_address = &bogus_reg;
+
+      sprintf(asm_operand, "#0x%04X", (uint16_t) source_value);
+    }
+    else {                             /* Source Register */
+      source_value = *reg;
+      source_address = reg;
+
+      sprintf(asm_operand, "%s", reg_name);
+    }
+
+    bw_flag == BYTE ? *reg &= 0x00FF : 0;
+  }
+
+  /* Indexed;      Ex: PUSH 0x0(Rs) */
+  /* Symbolic;     Ex: PUSH 0xS     */
+  /* Absolute:     Ex: PUSH &0xS    */
+  /* Constant Gen; Ex: PUSH #C      */ /* 1 */
+  else if (as_flag == 1) {
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = bogus_reg = immediate_constant;
+      source_address = &bogus_reg;
+
+      sprintf(asm_operand, "#0x%04X", source_value);
+    }
+    else if (source == 0) {            /* Source Symbolic */
+      source_offset = fetch(emu);
+      uint16_t virtual_addr = cpu->pc + source_offset;
+
+      sprintf(hex_str_part, "%04X", (uint16_t) source_offset);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      source_address = get_addr_ptr(virtual_addr);
+
+      sprintf(asm_operand, "0x%04X", virtual_addr);
+    }
+    else if (source == 2) {            /* Source Absolute */
+      source_offset = fetch(emu);
+      source_address = get_addr_ptr(source_offset);
+      source_value = *source_address;
+
+      sprintf(hex_str_part, "%04X", (uint16_t) source_value);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      sprintf(asm_operand, "&0x%04X", (uint16_t) source_offset);
+    }
+    else {                             /* Source Indexed */
+      source_offset = fetch(emu);
+      source_address = get_addr_ptr(*reg + source_offset);
+      source_value = *source_address;
+
+      sprintf(hex_str_part, "%04X", (uint16_t) source_offset);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      sprintf(asm_operand, "0x%04X(%s)", (uint16_t) source_offset, reg_name);
+    }
+  }
+
+  /* Indirect;     Ex: PUSH @Rs */
+  /* Constant Gen; Ex: PUSH #C */ /* 2, 4 */
+  else if (as_flag == 2) {
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = bogus_reg = immediate_constant;
+      source_address = &bogus_reg;
+
+      sprintf(asm_operand, "#0x%04X", immediate_constant);
+    }
+    else {                             /* Source Indirect */
+      source_address = get_addr_ptr(*reg);
+      source_value = *source_address;
+
+      sprintf(asm_operand, "@%s", reg_name);
+    }
+  }
+
+  /* Indirect AutoIncrement; Ex: PUSH @Rs+ */
+  /* Immediate;              Ex: PUSH #S   */
+  /* Constant Gen;           Ex: PUSH #C   */ /* -1, 8 */
+  else if (as_flag == 3) {
+    if (constant_generator_active) {   /* Source Constant */
+      source_value = bogus_reg = immediate_constant;
+      source_address = &bogus_reg;
+
+      sprintf(asm_operand, "#0x%04X", (uint16_t) source_value);
+    }
+    else if (source == 0) {            /* Source Immediate */
+      source_value = bogus_reg = fetch(emu);
+      source_address = &bogus_reg;
+
+      sprintf(hex_str_part, "%04X", (uint16_t) source_value);
+      strncat(hex_str, hex_str_part, sizeof hex_str);
+
+      if (bw_flag == WORD) {
+        sprintf(asm_operand, "#0x%04X", (uint16_t) source_value);
+      }
+      else if (bw_flag == BYTE) {
+        sprintf(asm_operand, "#0x%04X", (uint8_t) source_value);
+      }
+    }
+    else {                              /* Source Indirect AutoIncrement */
+      source_address = get_addr_ptr(*reg);
+      source_value = *source_address;
+
+      sprintf(asm_operand, "@%s+", reg_name);
+      bw_flag == WORD ? *reg += 2 : (*reg += 1);
+    }
+  }
+
+
+  if (!disassemble) {
+    switch (opcode) {
+        
+      /*  RRC Rotate right through carry
+       *    C → MSB → MSB-1 .... LSB+1 → LSB → C
+       *  
+       *  Description The destination operand is shifted right one position 
+       *  as shown in Figure 3-18. The carry bit (C) is shifted into the MSB, 
+       *  the LSB is shifted into the carry bit (C).
+       *
+       * N: Set if result is negative, reset if positive
+       * Z: Set if result is zero, reset otherwise
+       * C: Loaded from the LSB
+       * V: Reset
+       * TODO: UNDEFINED BEHAVIOR DURRING CONSTANT MANIPULATION, BROKEN
+       */
+    case 0x0:{
+      bool CF = cpu->sr.carry;
+
+      if (bw_flag == WORD) {
+	cpu->sr.carry = *source_address & 0x0001;  /* Set CF from LSB */
+	*source_address >>= 1;                /* Shift one right */
+	CF ? *source_address |= 0x8000 : 0;   /* Set MSB from prev CF */
+      }
+      else if (bw_flag == BYTE){
+	cpu->sr.carry = *(uint8_t *) source_address & 0x01;
+	*(uint8_t *) source_address >>= 1;
+	CF ? *(uint8_t *) source_address |= 0x80 : 0;
+      }
+
+      cpu->sr.zero = is_zero(source_address, bw_flag);
+      cpu->sr.negative = is_negative((int16_t*)source_address, bw_flag);
+      cpu->sr.overflow = false;
+
+      break;
+    }
+    
+      /* SWPB Swap bytes
+       * bw flag always 0 (word)
+       * Bits 15 to 8 ↔ bits 7 to 0
+       */
+    case 0x1:{	
+      uint8_t upper_nibble, lower_nibble;
+      upper_nibble = (*source_address & 0xFF00) >> 8;
+      lower_nibble = *source_address & 0x00FF;
+    
+      *source_address = ((uint16_t)0|(lower_nibble << 8)) | upper_nibble;
+
+      break;
+    }
+    
+      /* RRA Rotate right arithmetic 
+       *   MSB → MSB, MSB → MSB-1, ... LSB+1 → LSB, LSB → C
+       * 
+       * N: Set if result is negative, reset if positive
+       * Z: Set if result is zero, reset otherwise
+       * C: Loaded from the LSB
+       * V: Reset
+       */
+    case 0x2:{
+      if (bw_flag == WORD) {
+	cpu->sr.carry = *source_address & 0x0001;
+	bool msb = *source_address >> 15;
+	*source_address >>= 1;
+	msb ? *source_address |= 0x8000 : 0; /* Extend Sign */
+      }
+      else if (bw_flag == BYTE) {
+	cpu->sr.carry = *source_address & 0x0001;
+	bool msb = *source_address >> 7;
+	*source_address >>= 1;
+	msb ? *source_address |= 0x0080 : 0;
+      }
+
+      cpu->sr.zero = is_zero(source_address, bw_flag);
+      cpu->sr.negative = is_negative((int16_t*)source_address, bw_flag);
+      cpu->sr.overflow = false;
+      break;
+    }
+
+      /* SXT Sign extend byte to word
+       *   bw flag always 0 (word)
+       *
+       * Bit 7 → Bit 8 ......... Bit 15
+       * 
+       * N: Set if result is negative, reset if positive
+       * Z: Set if result is zero, reset otherwise
+       * C: Set if result is not zero, reset otherwise (.NOT. Zero)
+       * V: Reset
+       */
+
+    case 0x3:{
+      if (*source_address & 0x0080) {
+	*source_address |= 0xFF00;
+      }
+      else {
+	*source_address &= 0x00FF;
+      }
+    
+      cpu->sr.negative = is_negative((int16_t*)source_address, WORD);
+      cpu->sr.zero = is_zero(source_address, WORD);
+      cpu->sr.carry = ! cpu->sr.zero;
+      cpu->sr.overflow = false;
+
+      break;
+    }
+  
+      /* PUSH push value on to the stack
+       *   
+       *   SP - 2 → SP
+       *   src → @SP
+       *
+       */
+    case 0x4:{
+
+      cpu->sp -= 2; /* Yes, even for BYTE Instructions */
+      uint16_t *stack_address = get_stack_ptr(emu);
+    
+      if (bw_flag == WORD) {
+	*stack_address = source_value;
+      }
+      else if (bw_flag == BYTE) {
+	*stack_address &= 0xFF00; /* Zero out bottom half for pushed byte */
+	*stack_address |= (uint8_t) source_value;
+      }
+
+      break;
+    }
+
+      /* CALL SUBROUTINE: 
+       *     PUSH PC and PC = SRC
+       *     
+       *     This is always a word instruction. Supporting all addressing modes
+       */
+    
+    case 0x5:{
+    
+      cpu->sp -= 2;
+      uint16_t *stack_address = get_stack_ptr(emu);
+      *stack_address = cpu->pc;
+      cpu->pc = *source_address;
+
+      break;
+    }
+  
+      //# RETI Return from interrupt: Pop SR then pop PC
+    case 0x6:{
+       
+      break;
+    }
+    default:{
+      printf("Unknown Single operand instruction.\n");
+    }
+
+    } //# End of Switch
+  } //# end if
+  
+
+  else {    
+    switch (opcode) {
+    case 0x0: {
+      bw_flag == WORD ?
+	strncpy(mnemonic, "RRC", sizeof mnemonic) :
+	strncpy(mnemonic, "RRC.B", sizeof mnemonic);    
+
+      break;
+    }
+    case 0x1: {
+      strncpy(mnemonic, "SWPB", sizeof mnemonic);    
+      break;
+    }
+    case 0x2: {
+      bw_flag == WORD ?
+	strncpy(mnemonic, "RRA", sizeof mnemonic) :
+	strncpy(mnemonic, "RRA.B", sizeof mnemonic);     
+
+      break;
+    }
+    case 0x3: {
+      strncpy(mnemonic, "SXT", sizeof mnemonic);    
+      break;
+    }
+    case 0x4: {
+      bw_flag == WORD ?
+	strncpy(mnemonic, "PUSH", sizeof mnemonic) :
+	strncpy(mnemonic, "PUSH.B", sizeof mnemonic);    
+
+      break;
+    }
+    case 0x5: {
+      strncpy(mnemonic, "CALL", sizeof mnemonic);
+      break;
+    }
+    case 0x6: {
+      strncpy(mnemonic, "RETI", sizeof mnemonic);           
+      break;
+    }
+    default: {
+      printf("Unknown Single operand instruction.\n");
+    }
+
+    } //# End of Switch
+
+    strncat(mnemonic, "\t", sizeof mnemonic);
+    strncat(mnemonic, asm_operand, sizeof mnemonic);
+    strncat(mnemonic, "\n", sizeof mnemonic);
+    
+    if (disassemble && emu->debugger->debug_mode) {
+      int i;
+      char one = 0, two = 0;
+
+      // Make little endian big endian
+      for (i = 0;i < strlen(hex_str);i += 4) {
+	one = hex_str[i];
+	two = hex_str[i + 1];
+
+	hex_str[i] = hex_str[i + 2];
+        hex_str[i + 1] = hex_str[i + 3];
+
+	hex_str[i + 2] = one;
+	hex_str[i + 3] = two;
+      }
+
+      printf("%s", hex_str);
+      print_console(emu, hex_str);
+
+      for (i = strlen(hex_str);i < 12;i++) {
+	printf(" ");
+	print_console(emu, " ");
+      }
+
+      printf("\t%s", mnemonic);
+
+      print_console(emu, "\t");
+      print_console(emu, mnemonic);
+    }
+
+  } //# end else
+
+}
+

emulator/devices/cpu/formatII.h

@@ -0,0 +1,26 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _DECODE_FORMATII_H
+#define _DECODE_FORMATII_H
+
+#include "flag_handler.h"
+
+void decode_formatII(Emulator *emu, uint16_t instruction, bool disassemble);
+
+#endif

emulator/devices/cpu/formatIII.c

@@ -0,0 +1,249 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+//##########+++ Decode Format III Instructions +++#########
+//# Format III are jump instructions of the form:
+//#   [001C][CCXX][XXXX][XXXX]
+//# 
+//# Where C = Condition, X = 10-bit signed offset 
+//# 
+//########################################################
+
+#include "decoder.h"
+
+void decode_formatIII(Emulator *emu, uint16_t instruction, bool disassemble)
+{
+  Cpu *cpu = emu->cpu;
+  Debugger *debugger = emu->debugger;
+
+  uint8_t condition = (instruction & 0x1C00) >> 10;
+  int16_t signed_offset = (instruction & 0x03FF) * 2;
+  bool negative = signed_offset >> 9;
+  
+  char value[20];
+
+  char mnemonic[100] = {0};
+  /* String to show hex value of instruction */
+  char hex_str[100] = {0};
+
+  sprintf(hex_str, "%04X", instruction);
+
+  if (negative) { /* Sign Extend for Arithmetic Operations */
+    signed_offset |= 0xF800;
+  }
+
+  if (!disassemble) {
+  switch(condition){
+  
+  /* JNE/JNZ Jump if not equal/zero             
+  *
+  * If Z = 0: PC + 2 offset → PC
+  * If Z = 1: execute following instruction
+  */
+  case 0x0:{
+    if (cpu->sr.zero == false) {
+      cpu->pc += signed_offset;
+    }
+
+    break;
+  }
+  
+  /* JEQ/JZ Jump is equal/zero
+   * If Z = 1: PC + 2 offset → PC
+   * If Z = 0: execute following instruction
+  */
+  case 0x1:{
+    if (cpu->sr.zero == true) {
+      cpu->pc += signed_offset;
+    }
+
+    break;
+  }
+  
+  /* JNC/JLO Jump if no carry/lower
+  *
+  *  if C = 0: PC + 2 offset → PC
+  *  if C = 1: execute following instruction
+  */
+  case 0x2:{
+    if (cpu->sr.carry == false) {
+      cpu->pc += signed_offset;
+    }    
+    
+    break;
+  }
+
+  /* JC/JHS Jump if carry/higher or same
+  *
+  * If C = 1: PC + 2 offset → PC
+  * If C = 0: execute following instruction
+  */
+  case 0x3:{
+    if (cpu->sr.carry == true) {
+      cpu->pc += signed_offset;
+    }    
+
+    break;
+  }
+  
+  /* JN Jump if negative
+  *
+  *  if N = 1: PC + 2 ×offset → PC
+  *  if N = 0: execute following instruction
+  */
+  case 0x4:{
+    if (cpu->sr.negative == true) {
+      cpu->pc += signed_offset;
+    }    
+
+    break;
+  }
+   
+  /* JGE Jump if greater or equal (N == V)
+  *
+  *  If (N .XOR. V) = 0 then jump to label: PC + 2 P offset → PC
+  *  If (N .XOR. V) = 1 then execute the following instruction
+  */
+  case 0x5:{
+    if ((cpu->sr.negative ^ cpu->sr.overflow) == false) {
+      cpu->pc += signed_offset;
+    }    
+    
+    break;
+  }
+    
+  /* JL Jump if less (N != V)  
+  *
+  *  If (N .XOR. V) = 1 then jump to label: PC + 2 offset → PC
+  *  If (N .XOR. V) = 0 then execute following instruction
+  */
+  case 0x6:{
+    if ((cpu->sr.negative ^ cpu->sr.overflow) == true) {
+      cpu->pc += signed_offset;
+    }    
+    
+    break;
+  }
+ 
+  /* JMP Jump Unconditionally
+   *   
+   *  PC + 2 × offset → PC
+   *
+   */
+  case 0x7:{
+    cpu->pc += signed_offset;
+    break;
+  }
+
+  default:{
+    puts("Undefined Jump operation!\n");
+    return;
+  }
+  
+  } //# End of Switch
+  } //# end if
+
+
+  else {
+    switch(condition){
+
+    case 0x0:{
+      sprintf(mnemonic, "JNZ"); 
+      sprintf(value, "0x%04X", cpu->pc + signed_offset);
+      break;
+    }
+    case 0x1:{
+      sprintf(mnemonic, "JZ");
+      sprintf(value, "0x%04X", cpu->pc + signed_offset);
+      break;
+    }
+    case 0x2:{
+      sprintf(mnemonic, "JNC");
+      sprintf(value, "0x%04X", cpu->pc + signed_offset);
+      break;
+    }
+    case 0x3:{
+      sprintf(mnemonic, "JC");
+      sprintf(value, "0x%04X", cpu->pc + signed_offset);
+      break;
+    }
+    case 0x4:{
+      sprintf(mnemonic, "JN");
+      sprintf(value, "0x%04X", cpu->pc + signed_offset);
+      break;
+    }
+    case 0x5:{
+      sprintf(mnemonic, "JGE");    
+      sprintf(value, "0x%04X", cpu->pc + signed_offset);
+    
+      break;
+    }
+    case 0x6:{
+      sprintf(mnemonic, "JL");
+      sprintf(value, "0x%04X", cpu->pc + signed_offset);
+    
+      break;
+    }
+    case 0x7:{
+      sprintf(mnemonic, "JMP");
+      sprintf(value, "0x%04X", cpu->pc + signed_offset);
+      break;
+    }
+    default:{
+      puts("Undefined Jump operation!\n");
+      return;
+    }  
+
+    } //# End of Switch
+
+    strncat(mnemonic, "\t", sizeof(mnemonic));
+    strncat(mnemonic, value, sizeof(mnemonic));
+    strncat(mnemonic, "\n", sizeof(mnemonic));
+  
+    if (disassemble && emu->debugger->debug_mode) {
+      int i;
+      char one = 0, two = 0;
+
+      // Make little endian big endian
+      for (i = 0;i < strlen(hex_str);i += 4) {
+	one = hex_str[i];
+	two = hex_str[i + 1];
+
+	hex_str[i] = hex_str[i + 2];
+	hex_str[i + 1] = hex_str[i + 3];
+
+	hex_str[i + 2] = one;
+	hex_str[i + 3] = two;
+      }
+
+      printf("%s", hex_str);
+      print_console(emu, hex_str);
+
+      for (i = strlen(hex_str);i < 12;i++) {
+	printf(" ");
+	print_console(emu, " ");
+      }
+      
+      printf("\t%s", mnemonic);
+
+      print_console(emu, "\t");
+      print_console(emu, mnemonic);
+    }
+
+  }
+}

emulator/devices/cpu/formatIII.h

@@ -0,0 +1,24 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _DECODE_FORMATIII_H_
+#define _DECODE_FORMATIII_H_
+
+void decode_formatIII(Emulator *emu, uint16_t instruction, bool disassemble);
+
+#endif

emulator/devices/cpu/registers.c

@@ -0,0 +1,201 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "registers.h"
+
+//##########+++ MSP430 Register initialization +++##########
+void initialize_msp_registers(Emulator *emu)
+{
+  Cpu *cpu = emu->cpu;
+  Debugger *debugger = emu->debugger;
+
+  /* Initialize PC to boot loader code on cold boot (COLD)*/
+  //cpu->pc = 0x0C00;
+
+  /* Initialize Program Counter to *0xFFFE at boot or reset (WARM)*/
+  cpu->pc = 0xC000;
+  
+  /* Stack pointer typically begins at the top of RAM after reset */
+  cpu->sp = 0x400;
+
+  /* Initialize the status register */
+  memset(&cpu->sr, 0, sizeof(Status_reg));
+
+  cpu->running = false;
+  cpu->cg2 = 0;
+
+  cpu->r4 = cpu->r5 = cpu->r6 = cpu->r7 = cpu->r8 = 
+    cpu->r9 = cpu->r10 = cpu->r11 = cpu->r12 = cpu->r13 = 
+    cpu->r14 = cpu->r15 = 0;
+}
+
+void update_register_display (Emulator *emu) 
+{
+  Cpu *cpu = emu->cpu;  
+  char thing[50] = "....";  
+
+  if (emu->cpu->running) {
+    send_control(emu, UPDATE_ALL_REGS_PACKET, (void *)thing, strlen(thing));
+    
+    return;
+  }
+
+  sprintf(thing, "%04X", cpu->pc);
+  send_control(emu, UPDATE_REG_R0_PACKET, (void *)thing, strlen(thing));
+
+  sprintf(thing, "%04X", cpu->sp);
+  send_control(emu, UPDATE_REG_R1_PACKET, (void *)thing, strlen(thing));
+
+  sprintf(thing, "%04X", sr_to_value(emu));
+  send_control(emu, UPDATE_REG_R2_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X",(uint16_t) cpu->cg2);
+  send_control(emu, UPDATE_REG_R3_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X",(uint16_t) cpu->r4);
+  send_control(emu, UPDATE_REG_R4_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X",(uint16_t) cpu->r5);
+  send_control(emu, UPDATE_REG_R5_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X",(uint16_t) cpu->r6);
+  send_control(emu, UPDATE_REG_R6_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X",(uint16_t) cpu->r7);
+  send_control(emu, UPDATE_REG_R7_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X",(uint16_t) cpu->r8);
+  send_control(emu, UPDATE_REG_R8_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X",(uint16_t) cpu->r9);
+  send_control(emu, UPDATE_REG_R9_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X", (uint16_t)cpu->r10);
+  send_control(emu, UPDATE_REG_R10_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X", (uint16_t)cpu->r11);
+  send_control(emu, UPDATE_REG_R11_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X", (uint16_t)cpu->r12);
+  send_control(emu, UPDATE_REG_R12_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X", (uint16_t)cpu->r13);
+  send_control(emu, UPDATE_REG_R13_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X", (uint16_t)cpu->r14);
+  send_control(emu, UPDATE_REG_R14_PACKET, (void *)thing, strlen(thing));  
+
+  sprintf(thing, "%04X", (uint16_t)cpu->r15);
+  send_control(emu, UPDATE_REG_R15_PACKET, (void *)thing, strlen(thing));
+}
+
+//##########+++ Set SR struct Value +++##########
+void set_sr_value (Emulator *emu, uint16_t value) 
+{
+  Cpu *cpu = emu->cpu;  
+
+  // reset SR to set it properly...
+  memset(&cpu->sr, 0, sizeof(Status_reg));
+  //memcpy(&cpu->sr, &value, 16);
+
+  if (value & 0x8000) cpu->sr.reserved |= 0x8000;
+  if (value & 0x4000) cpu->sr.reserved |= 0x4000;
+  if (value & 0x2000) cpu->sr.reserved |= 0x2000;
+  if (value & 0x1000) cpu->sr.reserved |= 0x1000;
+  if (value & 0x0800) cpu->sr.reserved |= 0x0800;
+  if (value & 0x0400) cpu->sr.reserved |= 0x0400;
+  if (value & 0x0200) cpu->sr.reserved |= 0x0200;
+
+  cpu->sr.overflow = (value & 0x0100) ? 1 : 0;
+  cpu->sr.SCG1 =     (value & 0x0080) ? 1 : 0;
+  cpu->sr.SCG0 =     (value & 0x0040) ? 1 : 0;
+  cpu->sr.OSCOFF =   (value & 0x0020) ? 1 : 0;
+  cpu->sr.CPUOFF =   (value & 0x0010) ? 1 : 0;
+  cpu->sr.GIE =      (value & 0x0008) ? 1 : 0;
+  cpu->sr.negative = (value & 0x0004) ? 1 : 0;
+  cpu->sr.zero =     (value & 0x0002) ? 1 : 0;
+  cpu->sr.carry =    (value & 0x0001) ? 1 : 0;
+}
+
+//##########+++ Return value from SR struct +++##########
+uint16_t sr_to_value(Emulator *emu)
+{ 
+  Cpu *cpu = emu->cpu;
+  uint16_t r2 = 0;
+
+  // reserved bits not working quite right yet
+  if (cpu->sr.reserved & 0b1000000) {
+    r2 |= 0x8000;
+  }
+  if (cpu->sr.reserved & 0b0100000) {
+    r2 |= 0x4000;
+  }
+  if (cpu->sr.reserved & 0b0010000) {
+    r2 |= 0x2000;
+  }
+  if (cpu->sr.reserved & 0b0001000) {
+    r2 |= 0x1000;
+  }
+  if (cpu->sr.reserved & 0b0000100) {
+    r2 |= 0x0800;
+  }
+  if (cpu->sr.reserved & 0b0000010) {
+    r2 |= 0x0400;
+  }
+  if (cpu->sr.reserved & 0b0000001) {
+    r2 |= 0x0200;
+  }
+
+  if (cpu->sr.overflow) {
+    r2 |= 0x0100; 
+  }
+  
+  if (cpu->sr.SCG1) {
+    r2 |= 0x0080;
+  }
+  
+  if (cpu->sr.SCG0) {
+    r2 |= 0x0040;
+  }
+
+  if (cpu->sr.OSCOFF) {
+    r2 |= 0x0020;
+  }
+
+  if (cpu->sr.CPUOFF) {
+    r2 |= 0x0010;
+  }
+
+  if (cpu->sr.GIE) {
+    r2 |= 0x0008;
+  }
+
+  if (cpu->sr.negative) {
+    r2 |= 0x0004;
+  }
+  
+  if (cpu->sr.zero) {
+    r2 |= 0x0002;
+  }
+  
+  if (cpu->sr.carry) {
+    r2 |= 0x0001;
+  }
+  
+  return r2;
+}

emulator/devices/cpu/registers.h

@@ -0,0 +1,66 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _REGISTERS_H_
+#define _REGISTERS_H_
+
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdbool.h>
+#include "../../main.h"
+
+/* r2 or SR, the status register */
+typedef struct Status_reg {
+  uint8_t reserved : 7;   // Reserved bits    
+  uint8_t overflow : 1;   // Overflow flag
+  uint8_t SCG1 : 1;  // System Clock Generator SMCLK; ON = 0; OFF = 1;
+  uint8_t SCG0 : 1;  // System Clock Generator DCOCLK DCO ON = 0; DCO OFF = 1;
+  uint8_t OSCOFF : 1;    // Oscillator Off. LFXT1CLK ON = 0; LFXT1CLK OFF = 1; 
+  uint8_t CPUOFF : 1;     // CPU off; CPU OFF = 1; CPU ON = 0;                
+  uint8_t GIE : 1;    // General Inter enabl; Enbl maskable ints = 1; 0 = dont 
+  uint8_t negative : 1;   // Negative flag                                  
+  uint8_t zero : 1;       // Zero flag                                     
+  uint8_t carry : 1;      // Carry flag; Set when result produces a carry   
+} Status_reg;
+
+// Main CPU structure //
+typedef struct Cpu {
+  bool running;      /* CPU running or not */
+
+  uint16_t pc, sp;   /* R0 and R1 respectively */
+  Status_reg sr;     /* Status register fields */
+  int16_t cg2;       /* R3 or Constant Generator #2 */
+  
+  int16_t r4, r5, r6, r7;   /* R4-R15 General Purpose Registers */
+  int16_t r8, r9, r10, r11;
+  int16_t r12, r13, r14, r15;
+
+  Port_1 *p1;
+  //Port_2 *p2;
+  Usci *usci;
+  Bcm *bcm;
+  Timer_a *timer_a;
+} Cpu;
+
+uint16_t sr_to_value (Emulator *emu);
+void set_sr_value (Emulator *emu, uint16_t value);
+void initialize_msp_registers (Emulator *emu);
+void update_register_display (Emulator *emu);
+
+#endif

emulator/devices/memory/memspace.c

@@ -0,0 +1,100 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "memspace.h"
+
+uint8_t* MEMSPACE;   /* Memory Space */
+uint8_t* CODE;       // Code Memory
+uint8_t* INFO;       // Info memory
+uint8_t* IVT;        /* Interrupt Vector Table {Within ROM} */
+uint8_t* ROM;        /* Flash/Read-Only memory */
+uint8_t* RAM;        /* Random Access Memory */
+uint8_t* PER16;      /* 16-bit peripherals */
+uint8_t* PER8;       /* 8-bit peripherals */
+uint8_t* SFRS;       /* Special Function Registers */
+
+/* Initialize Address Space Locations
+** 
+** Allocate and set MSP430 Memory space
+** Some of these locations vary by model
+*/
+void initialize_msp_memspace()
+{
+  // MSP430g2553 Device Specific ...
+  // 16 KB / 64 KB Addressable Space
+  MEMSPACE = (uint8_t *) calloc(1, 0x10000);
+  //MEMSPACE = (uint8_t*)calloc(1, 0x100000);
+
+  // (lower bounds, so increment upwards)
+
+  // Info memory from 0x10FF - 0x1000 (256 Bytes)
+  INFO = MEMSPACE + 0x1000;
+  // Set it all to 0xFF by default...
+  memset(INFO, 0xFF, 256);
+
+  // Code Memory 0xFFFF - 0xC000;
+  CODE = MEMSPACE + 0xC000;
+  // Set it all to 0xFF by default...
+  memset(CODE, 0xFF, 16384);
+  
+  // Interrupt Vector Table 0xFFFF - 0xFFC0
+  IVT = MEMSPACE + 0xFFC0;
+  
+  // ROM // 0x400 - 0x1FFFF
+  ROM = MEMSPACE + 0x0400;   
+
+  // RAM from 0x3FF - 0x200
+  RAM = MEMSPACE + 0x0200;   // 0x200 - 0x3FF
+  PER16 = MEMSPACE + 0x0100;   // 0x0100 - 0x01FF
+  PER8 = MEMSPACE + 0x0010;   // 0x0010 - 0x00FF
+  SFRS = MEMSPACE + 0x0;      // 0x0 - 0x0F
+  
+  // Setup the calibration data in info memory
+
+  const uint16_t CALDCO_16MHZ_VLOC = 0x10F8;
+  *((uint8_t *) (MEMSPACE + CALDCO_16MHZ_VLOC)) = 0x95;
+
+  const uint16_t CALBC1_16MHZ_VLOC = 0x10F9;
+  *((uint8_t *) (MEMSPACE + CALBC1_16MHZ_VLOC)) = 0x8F;
+  
+  const uint16_t CALDCO_12MHZ_VLOC = 0x10FA;
+  *((uint8_t *) (MEMSPACE + CALDCO_12MHZ_VLOC)) = 0x9E;
+  
+  const uint16_t CALBC1_12MHZ_VLOC = 0x10FB;
+  *((uint8_t *) (MEMSPACE + CALBC1_12MHZ_VLOC)) = 0x8E;
+  
+  const uint16_t CALDCO_8MHZ_VLOC = 0x10FC;
+  *((uint8_t *) (MEMSPACE + CALDCO_8MHZ_VLOC)) = 0x92;
+  
+  const uint16_t CALBC1_8MHZ_VLOC = 0x10FD;
+  *((uint8_t *) (MEMSPACE + CALBC1_8MHZ_VLOC)) = 0x8D;
+  
+  const uint16_t CALDCO_1MHZ_VLOC = 0x10FE;
+  *((uint8_t *) (MEMSPACE + CALDCO_1MHZ_VLOC)) = 0xD1;
+  
+  const uint16_t CALBC1_1MHZ_VLOC = 0x10FF;
+  *((uint8_t *) (MEMSPACE + CALBC1_1MHZ_VLOC)) = 0x86;  
+}
+
+/* 
+** Free MSP430 virtual memory
+*/
+void uninitialize_msp_memspace()
+{
+  free(MEMSPACE);
+}

emulator/devices/memory/memspace.h

@@ -0,0 +1,161 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#pragma once
+#ifndef _MEMSPACE_H_
+#define _MEMSPACE_H_
+
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+
+void initialize_msp_memspace();
+void uninitialize_msp_memspace();
+
+static uint8_t blc[] = {
+  0x06, 0x0c, 0x1e, 0x0c, 0xff, 0x3f, 0xb2, 0x40,
+  0x40, 0xa5, 0x2c, 0x01, 0xb2, 0x90, 0x55, 0xaa,
+  0xde, 0xff, 0xff, 0x27, 0x31, 0x40, 0x20, 0x02,
+  0x0b, 0x43, 0xc0, 0x43, 0xfa, 0xf5, 0x32, 0xc2,
+  0xf2, 0xc0, 0x32, 0x00, 0x00, 0x00, 0xb2, 0x40,
+  0x80, 0x5a, 0x20, 0x01, 0xf2, 0x40, 0x88, 0x00,
+  0x57, 0x00, 0xf2, 0x40, 0x80, 0x00, 0x56, 0x00,
+  0xe2, 0xd3, 0x21, 0x00, 0xe2, 0xd3, 0x22, 0x00,
+  0xf2, 0xc0, 0x22, 0x00, 0x26, 0x00, 0xf2, 0xc0,
+  0x20, 0x00, 0x22, 0x00, 0xb2, 0x40, 0x00, 0xa5,
+  0x28, 0x01, 0x3b, 0xc0, 0x7a, 0x00, 0xb0, 0x12,
+  0x7e, 0x0e, 0x82, 0x43, 0x12, 0x02, 0x09, 0x43,
+  0x36, 0x40, 0x0a, 0x02, 0x37, 0x42, 0xb0, 0x12,
+  0x52, 0x0f, 0xc6, 0x4c, 0x00, 0x00, 0x16, 0x53,
+  0x17, 0x83, 0xf9, 0x23, 0x2b, 0xb3, 0x26, 0x20,
+  0x55, 0x42, 0x0b, 0x02, 0x75, 0x90, 0x12, 0x00,
+  0x75, 0x24, 0x75, 0x90, 0x10, 0x00, 0x54, 0x24,
+  0xb0, 0x12, 0x50, 0x0e, 0x55, 0x42, 0x0b, 0x02,
+  0x75, 0x90, 0x18, 0x00, 0x2e, 0x24, 0x2b, 0xb2,
+  0x15, 0x24, 0x75, 0x90, 0x20, 0x00, 0x15, 0x24,
+  0x75, 0x90, 0x16, 0x00, 0x22, 0x24, 0x75, 0x90,
+  0x14, 0x00, 0x9d, 0x24, 0x75, 0x90, 0x1a, 0x00,
+  0x18, 0x24, 0x75, 0x90, 0x1c, 0x00, 0x2f, 0x24,
+  0x05, 0x3c, 0x2b, 0xd2, 0xb0, 0x12, 0xe4, 0x0e,
+  0xc5, 0x3f, 0x21, 0x53, 0xb0, 0x12, 0xea, 0x0e,
+  0xc1, 0x3f, 0xb0, 0x12, 0xe4, 0x0e, 0xd2, 0x42,
+  0x0e, 0x02, 0x56, 0x00, 0xd2, 0x42, 0x0f, 0x02,
+  0x57, 0x00, 0xd2, 0x42, 0x10, 0x02, 0x16, 0x02,
+  0xb5, 0x3f, 0xb0, 0x12, 0xe4, 0x0e, 0x10, 0x42,
+  0x0e, 0x02, 0x16, 0x42, 0x0e, 0x02, 0x15, 0x43,
+  0x07, 0x3c, 0x36, 0x40, 0xfe, 0xff, 0xb2, 0x40,
+  0x06, 0xa5, 0x10, 0x02, 0x35, 0x40, 0x0c, 0x00,
+  0x92, 0x42, 0x10, 0x02, 0x28, 0x01, 0xb6, 0x43,
+  0x00, 0x00, 0x92, 0xb3, 0x2c, 0x01, 0xfd, 0x23,
+  0x15, 0x83, 0xf6, 0x23, 0xd3, 0x3f, 0x16, 0x42,
+  0x0e, 0x02, 0x17, 0x42, 0x10, 0x02, 0x35, 0x43,
+  0x75, 0x96, 0x03, 0x20, 0x17, 0x83, 0xfc, 0x23,
+  0xc9, 0x3f, 0x82, 0x46, 0x00, 0x02, 0xca, 0x3f,
+  0x36, 0x40, 0xe0, 0xff, 0x37, 0x40, 0x20, 0x00,
+  0x0a, 0x12, 0x0a, 0x43, 0xb0, 0x12, 0x52, 0x0f,
+  0x7c, 0xe6, 0x4a, 0xdc, 0x17, 0x83, 0xfa, 0x23,
+  0xb0, 0x12, 0x50, 0x0e, 0x4a, 0x93, 0x3a, 0x41,
+  0xb4, 0x27, 0x82, 0x93, 0xde, 0xff, 0x07, 0x24,
+  0x31, 0x40, 0x00, 0x10, 0x04, 0x12, 0x31, 0x90,
+  0x22, 0x02, 0xfc, 0x23, 0xc6, 0x3f, 0xb0, 0x12,
+  0xea, 0x0e, 0x70, 0x3f, 0x16, 0x42, 0x0e, 0x02,
+  0x17, 0x42, 0x10, 0x02, 0x2b, 0xb2, 0x30, 0x24,
+  0x3b, 0xd0, 0x10, 0x00, 0xb0, 0x12, 0x52, 0x0f,
+  0x2b, 0xb3, 0x2c, 0x20, 0x36, 0x90, 0x00, 0x10,
+  0x06, 0x2c, 0x36, 0x90, 0x00, 0x01, 0x06, 0x2c,
+  0xc6, 0x4c, 0x00, 0x00, 0x1b, 0x3c, 0xb2, 0x40,
+  0x40, 0xa5, 0x28, 0x01, 0x16, 0xb3, 0x03, 0x20,
+  0xc2, 0x4c, 0x14, 0x02, 0x13, 0x3c, 0xc2, 0x4c,
+  0x15, 0x02, 0x86, 0x9a, 0xfd, 0xff, 0x01, 0x24,
+  0x2b, 0xd3, 0x36, 0x90, 0x01, 0x02, 0x04, 0x28,
+  0x3b, 0xd2, 0x3b, 0xb0, 0x10, 0x00, 0x02, 0x24,
+  0x3b, 0xc0, 0x32, 0x00, 0x1a, 0x42, 0x14, 0x02,
+  0x86, 0x4a, 0xff, 0xff, 0x16, 0x53, 0x17, 0x83,
+  0xd5, 0x23, 0xb0, 0x12, 0x50, 0x0e, 0x72, 0x3f,
+  0xb0, 0x12, 0x52, 0x0f, 0x17, 0x83, 0xfc, 0x23,
+  0xb0, 0x12, 0x50, 0x0e, 0x6f, 0x3f, 0xb2, 0x40,
+  0x80, 0x00, 0x0a, 0x02, 0xd2, 0x42, 0x10, 0x02,
+  0x0c, 0x02, 0xd2, 0x42, 0x10, 0x02, 0x0d, 0x02,
+  0x82, 0x43, 0x12, 0x02, 0x09, 0x43, 0x36, 0x40,
+  0x0a, 0x02, 0x27, 0x42, 0x7c, 0x46, 0xb0, 0x12,
+  0xee, 0x0e, 0x17, 0x83, 0xfb, 0x23, 0x16, 0x42,
+  0x0e, 0x02, 0x17, 0x42, 0x10, 0x02, 0x36, 0x90,
+  0x00, 0x01, 0x0a, 0x28, 0xb2, 0x46, 0x14, 0x02,
+  0x5c, 0x42, 0x14, 0x02, 0xb0, 0x12, 0xee, 0x0e,
+  0x17, 0x83, 0x5c, 0x42, 0x15, 0x02, 0x01, 0x3c,
+  0x7c, 0x46, 0xb0, 0x12, 0xee, 0x0e, 0x17, 0x83,
+  0xee, 0x23, 0xb2, 0xe3, 0x12, 0x02, 0x5c, 0x42,
+  0x12, 0x02, 0xb0, 0x12, 0xee, 0x0e, 0x5c, 0x42,
+  0x13, 0x02, 0xb0, 0x12, 0xee, 0x0e, 0xfe, 0x3e,
+  0x18, 0x42, 0x12, 0x02, 0xb0, 0x12, 0x52, 0x0f,
+  0xc2, 0x4c, 0x12, 0x02, 0xb0, 0x12, 0x52, 0x0f,
+  0xc2, 0x4c, 0x13, 0x02, 0x38, 0xe3, 0x3b, 0xb2,
+  0x04, 0x24, 0x86, 0x9a, 0xfe, 0xff, 0x01, 0x24,
+  0x2b, 0xd3, 0x18, 0x92, 0x12, 0x02, 0x25, 0x23,
+  0x2b, 0xb3, 0x23, 0x23, 0x30, 0x41, 0xf2, 0xb0,
+  0x20, 0x00, 0x20, 0x00, 0xfc, 0x27, 0xf2, 0xb0,
+  0x20, 0x00, 0x20, 0x00, 0xfc, 0x23, 0xb2, 0x40,
+  0x24, 0x02, 0x60, 0x01, 0xf2, 0xb0, 0x20, 0x00,
+  0x20, 0x00, 0xfc, 0x27, 0x15, 0x42, 0x70, 0x01,
+  0x05, 0x11, 0x05, 0x11, 0x05, 0x11, 0x82, 0x45,
+  0x02, 0x02, 0x05, 0x11, 0x82, 0x45, 0x04, 0x02,
+  0xb2, 0x80, 0x1e, 0x00, 0x04, 0x02, 0x57, 0x42,
+  0x16, 0x02, 0x37, 0x80, 0x03, 0x00, 0x05, 0x11,
+  0x05, 0x11, 0x17, 0x53, 0xfd, 0x23, 0x35, 0x50,
+  0x40, 0xa5, 0x82, 0x45, 0x2a, 0x01, 0x35, 0x42,
+  0xb2, 0x40, 0x24, 0x02, 0x60, 0x01, 0x92, 0x92,
+  0x70, 0x01, 0x02, 0x02, 0xfc, 0x2f, 0x15, 0x83,
+  0xf7, 0x23, 0x09, 0x43, 0x7c, 0x40, 0x90, 0x00,
+  0x02, 0x3c, 0x7c, 0x40, 0xa0, 0x00, 0x4d, 0x43,
+  0xc9, 0xec, 0x12, 0x02, 0x19, 0xe3, 0x1b, 0xc3,
+  0x45, 0x4d, 0x55, 0x45, 0x00, 0x0f, 0x00, 0x55,
+  0x0c, 0x2e, 0x2e, 0x2e, 0x2e, 0x2e, 0x2e, 0x2e,
+  0x2e, 0x1a, 0x34, 0x34, 0x92, 0x42, 0x70, 0x01,
+  0x72, 0x01, 0xb2, 0x50, 0x0c, 0x00, 0x72, 0x01,
+  0x07, 0x3c, 0x1b, 0xb3, 0x0b, 0x20, 0x82, 0x43,
+  0x62, 0x01, 0x92, 0xb3, 0x62, 0x01, 0xfd, 0x27,
+  0xe2, 0xc3, 0x21, 0x00, 0x0a, 0x3c, 0x4c, 0x11,
+  0xf6, 0x2b, 0x1b, 0xe3, 0x82, 0x43, 0x62, 0x01,
+  0x92, 0xb3, 0x62, 0x01, 0xfd, 0x27, 0xe2, 0xd3,
+  0x21, 0x00, 0x92, 0x52, 0x02, 0x02, 0x72, 0x01,
+  0x5d, 0x53, 0x7d, 0x90, 0x0c, 0x00, 0xd4, 0x23,
+  0x30, 0x41, 0x4e, 0x43, 0x1b, 0xc3, 0x45, 0x4e,
+  0x55, 0x45, 0x5e, 0x0f, 0x00, 0x55, 0x0c, 0x5c,
+  0x5c, 0x5c, 0x5c, 0x5c, 0x5c, 0x5c, 0x5c, 0x3a,
+  0x7c, 0x00, 0xf2, 0xb0, 0x20, 0x00, 0x20, 0x00,
+  0xfc, 0x23, 0x92, 0x42, 0x70, 0x01, 0x72, 0x01,
+  0x92, 0x52, 0x04, 0x02, 0x72, 0x01, 0x82, 0x43,
+  0x62, 0x01, 0x92, 0xb3, 0x62, 0x01, 0xfd, 0x27,
+  0xf2, 0xb0, 0x20, 0x00, 0x20, 0x00, 0x20, 0x28,
+  0x2b, 0xd3, 0x1e, 0x3c, 0x4c, 0x10, 0x1c, 0x3c,
+  0x82, 0x43, 0x62, 0x01, 0x92, 0xb3, 0x62, 0x01,
+  0xfd, 0x27, 0xf2, 0xb0, 0x20, 0x00, 0x20, 0x00,
+  0x01, 0x28, 0x1b, 0xe3, 0x1b, 0xb3, 0x01, 0x24,
+  0x2b, 0xd3, 0xc9, 0xec, 0x12, 0x02, 0x19, 0xe3,
+  0x0b, 0x3c, 0x82, 0x43, 0x62, 0x01, 0x92, 0xb3,
+  0x62, 0x01, 0xfd, 0x27, 0xf2, 0xb0, 0x20, 0x00,
+  0x20, 0x00, 0xe4, 0x2b, 0x4c, 0x10, 0x1b, 0xe3,
+  0x92, 0x52, 0x02, 0x02, 0x72, 0x01, 0x5e, 0x53,
+  0xbe, 0x3f, 0x82, 0x43, 0x62, 0x01, 0x92, 0xb3,
+  0x62, 0x01, 0xfd, 0x27, 0xf2, 0xb0, 0x20, 0x00,
+  0x20, 0x00, 0x01, 0x2c, 0x2b, 0xd3, 0x30, 0x41,
+  0x25, 0x53, 0x00, 0x60, 0x00, 0x00, 0x00, 0x00,
+  0x00, 0x00, 0x02, 0x03, 0x01, 0x00, 0x26, 0x30,
+};
+
+#endif

emulator/devices/peripherals/bcm.c

@@ -0,0 +1,288 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "bcm.h"
+
+void handle_bcm (Emulator *emu) 
+{
+  Cpu *cpu = emu->cpu;
+  Bcm *bcm = cpu->bcm;
+
+  uint8_t DCOCTL  = *bcm->DCOCTL;
+  uint8_t BCSCTL1 = *bcm->BCSCTL1;
+  uint8_t BCSCTL2 = *bcm->BCSCTL2;
+  uint8_t BCSCTL3 = *bcm->BCSCTL3;
+
+  // HANDLE MCLK -------------------
+  uint8_t SELMx = BCSCTL2 >> 6;
+  uint8_t DIVMx = (BCSCTL2 >> 4) & 0x03;
+
+  if (SELMx == 0b00 || SELMx == 0b01) { // source = DCOCLK
+    bcm->mclk_source = DCOCLK;
+    bcm->mclk_freq = (bcm->dco_freq*1.0) / bcm->mclk_div;
+  }
+  else if (SELMx == 0b10) { // XT2CLK
+    bcm->mclk_source = XT2CLK;    
+  }
+  else if (SELMx == 0b11) { // VLOCLK
+    bcm->mclk_source = VLOCLK;    
+  }
+
+  switch (DIVMx) {
+  case 0b00: bcm->mclk_div = 1; break;
+  case 0b01: bcm->mclk_div = 2; break;
+  case 0b10: bcm->mclk_div = 4; break;
+  case 0b11: bcm->mclk_div = 8; break;
+  default: break;
+  }
+
+  // HANDLE SMCLK -------------------
+  uint8_t SELS  = (BCSCTL2 >> 3) & 0x01;
+  uint8_t DIVSx = (BCSCTL2 >> 1) & 0x03;
+
+  // HANDLE ACLK -------------------
+  uint8_t DIVAx = (BCSCTL1 >> 4) & 0x03;
+  
+
+  // HANDLE LOW POWER MODES --------
+
+  // Active Mode (CPU is active, all enabled clocks are active)
+  if (!cpu->sr.SCG1 && !cpu->sr.SCG0 && !cpu->sr.OSCOFF && !cpu->sr.CPUOFF) {
+
+  }
+
+  // LPM0 (CPU, MCLK are disabled, SMCLK, ACLK are active)
+  else if (!cpu->sr.SCG1 && !cpu->sr.SCG0 && !cpu->sr.OSCOFF && cpu->sr.CPUOFF){
+
+  }
+
+  /* LPM1 (CPU, MCLK are disabled. DCO and DC generator are
+     disabled if the DCO is not used for SMCLK. ACLK is
+     active.)
+  */
+  else if (!cpu->sr.SCG1 && cpu->sr.SCG0 && !cpu->sr.OSCOFF && cpu->sr.CPUOFF){
+
+  }
+
+  /* LPM2 (CPU, MCLK, SMCLK, DCO are disabled. DC generator remains enabled. 
+     ACLK is active.) */
+  else if (cpu->sr.SCG1 && !cpu->sr.SCG0 && !cpu->sr.OSCOFF && cpu->sr.CPUOFF){
+
+  }
+
+  // LPM3 (CPU, MCLK, SMCLK, DCO are disabled. DC generatordisabled.ACLK active.
+  else if (cpu->sr.SCG1 && cpu->sr.SCG0 && !cpu->sr.OSCOFF && cpu->sr.CPUOFF){
+
+  }
+
+  // LPM4 (CPU and all clocks are disabled)
+  else if (cpu->sr.SCG1 && cpu->sr.SCG0 && cpu->sr.OSCOFF && cpu->sr.CPUOFF){
+    
+  }
+
+  // HANDLE DCO --------------------
+
+  uint8_t DCOx   = DCOCTL >> 5;
+  uint8_t MODx   = DCOCTL & 0x1F;
+  uint8_t RSELx  = BCSCTL1 & 0x0F;  
+
+  // Default state of BCM after reset ~1.03 MHz
+  if (DCOx == 0b011 && RSELx == 0b0111) {
+    bcm->dco_freq = 1030000;
+    bcm->dco_period = 971;    
+    bcm->dco_pulse_width = 485;
+  }
+  // 16 Mhz
+  else if (DCOx == 0b100 && RSELx == 0b1111) {
+    bcm->dco_freq = 16000000;
+    bcm->dco_period = 63;
+    bcm->dco_pulse_width = 31;    
+  }
+  // 12 MHz
+  else if (DCOx == 0b100 && RSELx == 0b1110) {
+    bcm->dco_freq = 12000000;
+    bcm->dco_period = 83;
+    bcm->dco_pulse_width = 42;            
+  }
+  // 8 Mhz
+  else if (DCOx == 0b100 && RSELx == 0b1101) {
+    bcm->dco_freq = 8000000;
+    bcm->dco_period = 125;
+    bcm->dco_pulse_width = 62;        
+  }
+  // 1 MHz
+  else if (DCOx == 0b110 && RSELx == 0b0110) {
+    bcm->dco_freq = 1000000;
+    bcm->dco_period = 1000;
+    bcm->dco_pulse_width = 500;        
+  }
+
+  // HANDLE LFXT1CLK -------------------
+  uint8_t XTS = (BCSCTL1 >> 6) & 0x01; // LFXT1CLK select (high/low)
+  
+}
+
+void setup_bcm (Emulator *emu) 
+{
+  Cpu *cpu = emu->cpu;
+  Bcm *bcm = cpu->bcm;
+
+  static const uint16_t DCOCTL_VLOC = 0x56;
+  static const uint16_t BCSCTL1_VLOC = 0x57;
+  static const uint16_t BCSCTL2_VLOC = 0x58;
+  static const uint16_t BCSCTL3_VLOC = 0x53;
+  static const uint16_t IE1_VLOC = 0x0;
+  static const uint16_t IFG1_VLOC = 0x2;
+
+  *(bcm->DCOCTL   = (uint8_t *) get_addr_ptr(DCOCTL_VLOC)) = 0x60;
+  *(bcm->BCSCTL1   = (uint8_t *) get_addr_ptr(BCSCTL1_VLOC)) = 0x87;
+  *(bcm->BCSCTL2   = (uint8_t *) get_addr_ptr(BCSCTL2_VLOC)) = 0;
+  *(bcm->BCSCTL3   = (uint8_t *) get_addr_ptr(BCSCTL3_VLOC)) = 0x5;
+  *(bcm->IE1   = (uint8_t *) get_addr_ptr(IE1_VLOC)) = 0;
+  *(bcm->IFG1   = (uint8_t *) get_addr_ptr(IFG1_VLOC)) = 0;
+
+  // 1.03 MHz
+  bcm->dco_freq = 1030000;
+  bcm->dco_period = 971;
+  bcm->dco_pulse_width = 970 / 2;
+}
+
+
+uint64_t nanosec_diff(struct timespec *timeA_p, struct timespec *timeB_p)
+{
+    return ((timeA_p->tv_sec * 1000000000) + timeA_p->tv_nsec) - ((timeB_p->tv_sec * 1000000000) + timeB_p->tv_nsec);
+}
+
+void mclk_wait_cycles (Emulator *emu, uint64_t cycles)
+{
+    Cpu *cpu = emu->cpu;
+    Bcm *bcm = cpu->bcm;  
+
+    struct timespec start, end;
+    uint64_t i, elapsed_nsecs;
+  
+    for (i = 0;i < cycles;i++)
+    {
+        clock_gettime(CLOCK_MONOTONIC, &start);
+
+        while (true)
+        {
+            clock_gettime(CLOCK_MONOTONIC, &end);
+            elapsed_nsecs = nanosec_diff(&end, &start);
+
+            // Choose timing based on clock source
+            if (bcm->mclk_source == DCOCLK)
+            {
+                double thing = (1.0/(bcm->dco_freq/bcm->mclk_div))*1000000000.0;
+
+                if (elapsed_nsecs >= (uint64_t)thing)
+                    break;
+            }
+            else
+            {
+                puts("Error, clock source");
+            }
+        }
+    }
+}
+
+void smclk_wait_cycles (Emulator *emu, uint64_t cycles)
+{
+  Cpu *cpu = emu->cpu;
+  Bcm *bcm = cpu->bcm;  
+  
+  struct timespec start, end;
+  uint64_t i, elapsed_nsecs;
+  
+  for (i = 0;i < cycles;i++) {
+    clock_gettime(CLOCK_MONOTONIC, &start);
+
+    while (true) {
+      clock_gettime(CLOCK_MONOTONIC, &end);
+      elapsed_nsecs = nanosec_diff(&end, &start);
+
+      // Choose timing based on clock source
+      if (bcm->mclk_source == DCOCLK) {
+	//printf("div: %llu\n", 
+	//(long long unsigned)(1/(bcm->dco_freq/bcm->mclk_div)));
+
+	double thing = (1.0/(bcm->dco_freq/bcm->mclk_div))*1000000000.0;
+
+	if (elapsed_nsecs >= (uint64_t)thing) {
+	  break;
+	}    
+      }
+      else {
+	puts("Error, clock source");
+      }
+
+    }
+  }
+  
+}
+/*
+  /*
+  // Start Sources DCO, etc
+  pthread_t pp;
+
+  if ( pthread_create(&pp, NULL, DCO_source, (void *)emu ) ) {
+    printf("Error creating DCO  thread\n");
+    exit(1);
+  }
+
+void *DCO_source (void *data) 
+{
+  Emulator *emu = (Emulator *)data;
+  Bcm *bcm = emu->cpu->bcm;
+  
+  printf("In source thread...\n");
+
+  struct timespec start, end;
+  uint64_t elapsed_nsecs;
+  uint64_t trimmer = 0;
+
+  while (true) {
+    clock_gettime(CLOCK_MONOTONIC, &start);    
+
+    while (true) {
+      clock_gettime(CLOCK_MONOTONIC, &end);
+      elapsed_nsecs = nanosec_diff(&end, &start);      
+      if (elapsed_nsecs >= bcm->dco_period) break;
+    }
+  }
+
+  /*
+  while (true) {
+    clock_gettime(CLOCK_MONOTONIC, &start);    
+    bcm->dco_high = true;
+
+    while (true) {
+      clock_gettime(CLOCK_MONOTONIC, &end);
+      elapsed_nsecs = nanosec_diff(&end, &start);
+      
+      if (elapsed_nsecs >= bcm->dco_pulse_width) {
+	bcm->dco_high = false;
+      }
+
+      if (elapsed_nsecs >= bcm->dco_period) break;
+    }
+  }
+
+  return NULL;
+}
+*/

emulator/devices/peripherals/bcm.h

@@ -0,0 +1,72 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _BCM_H_
+#define _BCM_H_
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <time.h>
+#include <pthread.h>
+
+#include "../cpu/registers.h"
+#include "../utilities.h"
+
+enum {DCOCLK = 0, XT2CLK, VLOCLK, TACLK, ACLK, SMCLK, MCLK, INCLK, NUM_CLOCKS};
+
+struct Bcm {
+  // Peripheral register pointers //
+
+  // DCO Control Register
+  uint8_t *DCOCTL;  // r/w, @0x56, PUC:0x60
+
+  // Basic Clock System Control 1
+  uint8_t *BCSCTL1; // r/w, @0x57, POR: 0x87
+
+  // Basic Clock System Control 2
+  uint8_t *BCSCTL2; // r/w, @0x58, PUC: 0x0
+
+  // Basic Clock System Control 3
+  uint8_t *BCSCTL3; // r/w, @0x53, PUC: 0x5
+
+  // SFR Interrupt Enable Register 1
+  uint8_t *IE1;     // r/w, @0x0, PUC: 0x0
+
+  // SFR Interrupt Flag Register 1
+  uint8_t *IFG1;    // r/w, @0x2, PUC: 0x0
+
+  // -----
+  uint64_t dco_freq; // In Hz
+  uint64_t dco_period; // In nanosecs  
+  uint64_t dco_pulse_width; // In nanosecs  
+
+  // ----
+  uint8_t mclk_source;
+  uint64_t mclk_div;
+  uint64_t mclk_freq;
+};
+
+uint64_t nanosec_diff(struct timespec *timeA_p, struct timespec *timeB_p);
+void mclk_wait_cycles (Emulator *emu, uint64_t cycles);
+void smclk_wait_cycles (Emulator *emu, uint64_t cycles);
+void aclk_wait_cycles (Emulator *emu, uint64_t cycles);
+//void *DCO_source (void *data);
+void setup_bcm (Emulator *emu);
+void handle_bcm (Emulator *emu);
+
+#endif

emulator/devices/peripherals/port1.c

@@ -0,0 +1,475 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "port1.h"
+
+/* Cheat Sheet:
+ *  PxIN :  0 = LOW input, 1 = HIGH input
+ *  PxOUT:  0 = LOW output, 1 = HIGH output
+ *  PxDIR:  0 = INPUT, 1 = OUTPUT
+ *  PxREN:  0 = Pull Up/Down DISABLED, 1 = Pull Up/Down ENABLED
+ *
+ *  PxSEL2 | PxSEL | Explaination
+ *       0 |     0 | I/O function selected 
+ *       0 |     1 | Primary Peripheral module function selected
+ *       1 |     0 | Reserved ?
+ *       1 |     1 | Secondary Peripheral module function selected
+ *  [NOTE: P1 and P2 port pin INTs are disabled when PxSEL = 1]
+ *
+ *  PxIFG:  0 = No interrupt pending, 1 = Interrupt Pending
+ *  PxIES:  PxIFG set with a [0 = LOW-HIGH, 1 = HIGH-LOW] transition 
+ *  PxIE :  0 = interrupt disabled, 1 = interrupt enabled
+ */
+
+void handle_port_1 (Emulator *emu)
+{
+    Cpu *cpu = emu->cpu;
+    Port_1 *p = cpu->p1;
+
+    //////////////////// P1.0 ////////////////////////
+
+    // Check Direction
+    if (*p->DIR & 0x01)
+    {
+        p->DIR_0 = true;         // Set P1DIR.0 flag
+
+        if (*p->OUT & 0x01)      // Check OUTPUT
+            p->OUT_0 = true;       // Set P1OUT.0 flag
+        else
+            p->OUT_0 = false;      // Reset P1OUT.0 flag
+    }
+
+    // Check INPUT 
+    else                 
+    {                                
+        p->DIR_0 = false;
+    }
+    
+    /// Check if Interrupt Enabled for pin 
+    if (*p->IE & 0x01)
+    {         
+        p->IE_0 = true;
+
+        // Check For Interrupt Pending 
+        if (*p->IFG & 0x01)
+        {    
+            // Set p->IFG.0 flag indicating INT 
+            p->IFG_0 = true;
+        }
+        else
+        {
+            p->IFG_0 = false;
+        }
+    }
+    else
+    {
+        p->IE_0 = false;
+    }    
+    
+    // Check primary select
+    if (*p->SEL & 0x01) {
+        if (p->SEL_0 == false) {
+            puts("P1_SEL_0 = 1");
+        }
+        
+        p->SEL_0 = true;
+    }
+    else {
+        if (p->SEL_0 == true) {
+            puts("P1_SEL_0 = 0");
+        }
+
+        p->SEL_0 = false;
+    }
+
+    // Check secondary select
+    if (*p->SEL2 & 0x01) {
+        if (p->SEL2_0 == false) {
+            puts("P1_SEL2_0 = 1");
+        }
+        
+        p->SEL2_0 = true;
+    }
+    else {
+        if (p->SEL2_0 == true) {
+            puts("P1_SEL2_0 = 0");
+        }
+
+        p->SEL2_0 = false;        
+    }
+
+    //////////////////// P1.1 ////////////////////////
+
+    // Check Direction and IN/OUT
+    if (*p->DIR & 0x02) {
+        p->DIR_1 = true;
+        if (*p->OUT & 0x02) {
+            p->OUT_1 = true;
+        }
+        else {
+            p->OUT_1 = false;
+        }
+    }
+    else {
+        p->DIR_1 = false;
+    }
+
+    // Check Interrupts
+    if (*p->IE & 0x02) {
+        p->IE_1 = true;
+
+        if (*p->IFG & 0x02) {
+            p->IFG_1 = true;
+        }
+        else {
+            p->IFG_1 = false;
+        }
+    }
+    else {
+        p->IE_1 = false;
+    }
+
+    // Check primary select
+    if (*p->SEL & 0x02) {
+        if (p->SEL_1 == false) {
+            puts("P1_SEL_1 = 1");
+        }
+
+        p->SEL_1 = true;
+    }
+    else {
+        if (p->SEL_1 == true) {
+            puts("P1_SEL_1 = 0");
+        }
+
+        p->SEL_1 = false;
+    }
+
+    // Check secondary select
+    if (*p->SEL2 & 0x02) {
+        if (p->SEL2_1 == false) {
+            p->SEL2_1 = true;
+            puts("P1_SEL2_1 = 1");
+        }
+    }
+    else {
+        if (p->SEL2_1 == true) {
+            p->SEL2_1 = false;        
+            puts("P1_SEL2_1 = 0");
+        }
+    }
+
+    //////////////////// P1.2 ////////////////////////
+
+    if (*p->DIR & 0x04)
+    {
+        p->DIR_2 = true;
+
+        if (*p->OUT & 0x04)
+        {
+            p->OUT_2 = true;
+        }
+        else
+        {
+            p->OUT_2 = false;
+        }
+    }
+    else
+    {
+        p->DIR_2 = false;
+    }
+
+    if (*p->IE & 0x04)
+    {
+        p->IE_2 = true;
+
+        if (*p->IFG & 0x04)
+        {
+            p->IFG_2 = true;
+        }
+        else
+        {
+            p->IFG_2 = false;
+        }
+    }
+    else
+    {
+        p->IE_2 = false;
+    }
+
+    // Check primary select
+    if (*p->SEL & 0x04)
+    {
+        if (p->SEL_2 == false) 
+        {
+            puts("P1_SEL_2 = 1");
+        }
+        
+        p->SEL_2 = true;
+    }
+    else
+    {
+        if (p->SEL_2 == true)
+        {
+            puts("P1_SEL_2 = 0");
+        }
+
+        p->SEL_2 = false;
+    }
+
+    // Check secondary select
+    if (*p->SEL2 & 0x04) {
+        if (p->SEL2_2 == false) {
+            puts("P1_SEL2_2 = 1");
+        }
+        
+        p->SEL2_2 = true;
+    }
+    else {
+        if (p->SEL2_2 == true) {
+            puts("P1_SEL2_2 = 0");
+        }
+
+        p->SEL2_2 = false;        
+    }
+
+    ////////////////////////////////////////////////
+
+    // Handler P1.3 
+    if (*p->DIR & 0x08) {
+        p->DIR_3 = true;
+        if (*p->OUT & 0x08) {
+            p->OUT_3 = true;
+        }
+        else {
+            p->OUT_3 = false;
+        }
+    }
+    else {
+        p->DIR_3 = false;
+    }
+
+    if (*p->IE & 0x08) {
+        p->IE_3 = true;
+
+        if (*p->IFG & 0x08) {
+            p->IFG_3 = true;
+        }
+        else {
+            p->IFG_3 = false;
+        }
+    }
+    else {
+        p->IE_3 = false;
+    }
+
+    ///////////////////////////////////////////////////////////////
+
+    // Handler P1.4 
+    if (*p->DIR & 0x10) {
+        p->DIR_4 = true;
+        if (*p->OUT & 0x10) {
+            p->OUT_4 = true;
+        }
+        else {
+            p->OUT_4 = false;
+        }
+    }
+    else {
+        p->DIR_4 = false;
+    }
+
+    if (*p->IE & 0x10) {
+        p->IE_4 = true;
+
+        if (*p->IFG & 0x10) {
+            p->IFG_4 = true;
+        }
+        else {
+            p->IFG_4 = false;
+        }
+    }
+    else {
+        p->IE_4 = false;
+    }
+
+    /////////////////////////////////////////////////
+
+    // Handler P1.5 
+    if (*p->DIR & 0x20) {
+        p->DIR_5 = true;
+        if (*p->OUT & 0x20) {
+            p->OUT_5 = true;
+        }
+        else {
+            p->OUT_5 = false;
+        }
+    }
+    else {
+        p->DIR_5 = false;
+    }
+
+    if (*p->IE & 0x20) {
+        p->IE_5 = true;
+
+        if (*p->IFG & 0x20) {
+            p->IFG_5 = true;
+        }
+        else {
+            p->IFG_5 = false;
+        }
+    }
+    else {
+        p->IE_5 = false;
+    }
+
+    ////////////////////////////////////////////////////
+
+    // Handler P1.6 
+    if (*p->DIR & 0x40)
+    {
+        p->DIR_6 = true;
+        if (*p->OUT & 0x40)
+        {
+            p->OUT_6 = true;
+        }
+        else
+        {
+            p->OUT_6 = false;
+        }
+    }
+    else
+    {
+        p->DIR_6 = false;
+    }
+
+    if (*p->IE & 0x40)
+    {
+        p->IE_6 = true;
+
+        if (*p->IFG & 0x40)
+        {
+            p->IFG_6 = true;
+        }
+        else
+        {
+            p->IFG_6 = false;
+        }
+    }
+    else
+    {
+        p->IE_6 = false;
+    }
+
+    ////////////////////////////////////////////////////
+
+    // Handler P1.7 
+    if (*p->DIR & 0x80) {
+        p->DIR_7 = true;
+        if (*p->OUT & 0x80) {
+            p->OUT_7 = true;
+        }
+        else {
+            p->OUT_7 = false;
+        }
+    }
+    else {
+        p->DIR_7 = false;
+    }
+
+    if (*p->IE & 0x80) {
+        p->IE_7 = true;
+
+        if (*p->IFG & 0x80) {
+            p->IFG_7 = true;
+        }
+        else {
+            p->IFG_7 = false;
+        }
+    }
+    else {
+        p->IE_7 = false;
+    }
+}
+
+void setup_port_1 (Emulator *emu)
+{
+    Cpu *cpu = emu->cpu;
+    Port_1 *p = cpu->p1;
+  
+    static const uint16_t IN_VLOC   = 0x20;   // Input
+    static const uint16_t OUT_VLOC  = 0x21;   // Output
+    static const uint16_t DIR_VLOC  = 0x22;   // Direction
+    static const uint16_t IFG_VLOC  = 0x23;   // Interrupt flag
+    static const uint16_t IES_VLOC  = 0x24;   // Interrupt Edge Select
+    static const uint16_t IE_VLOC   = 0x25;   // Interrupt Enable
+    static const uint16_t SEL_VLOC  = 0x26;   // Select
+    static const uint16_t SEL2_VLOC = 0x41;   // Select 2
+    static const uint16_t REN_VLOC  = 0x27;   // Resistor Enable
+  
+    *(p->IN   = (uint8_t *) get_addr_ptr(IN_VLOC))   = 0;
+    *(p->OUT  = (uint8_t *) get_addr_ptr(OUT_VLOC))  = 0;
+    *(p->DIR  = (uint8_t *) get_addr_ptr(DIR_VLOC))  = 0;
+    *(p->IFG  = (uint8_t *) get_addr_ptr(IFG_VLOC))  = 0;
+    *(p->IES  = (uint8_t *) get_addr_ptr(IES_VLOC))  = 0;
+    *(p->IE   = (uint8_t *) get_addr_ptr(IE_VLOC))   = 0;
+    *(p->SEL  = (uint8_t *) get_addr_ptr(SEL_VLOC))  = 0;
+    *(p->SEL2 = (uint8_t *) get_addr_ptr(SEL2_VLOC)) = 0;
+    *(p->REN  = (uint8_t *) get_addr_ptr(REN_VLOC))  = 0;
+
+  
+    p->DIR_0 = false; p->OUT_0 = false; p->IFG_0 = false; 
+    p->IE_0 = false; p->SEL_0 = false; p->SEL2_0 = false;
+    
+    p->DIR_1 = false; p->OUT_1 = false; p->IFG_1 = false; 
+    p->IE_1 = false; p->SEL_1 = false; p->SEL2_1 = false;
+    
+    p->DIR_2 = false; p->OUT_2 = false; p->IFG_2 = false; 
+    p->IE_2 = false; p->SEL_2 = false; p->SEL2_2 = false;
+    
+    p->DIR_3 = false; p->OUT_3 = false; p->IFG_3 = false; 
+    p->IE_3 = false; p->SEL_3 = false; p->SEL2_3 = false;
+    
+    p->DIR_4 = false; p->OUT_4 = false; p->IFG_4 = false; 
+    p->IE_4 = false; p->SEL_4 = false; p->SEL2_4 = false;
+    
+    p->DIR_5 = false; p->OUT_5 = false; p->IFG_5 = false; 
+    p->IE_5 = false; p->SEL_5 = false; p->SEL2_5 = false;
+    
+    p->DIR_6 = false; p->OUT_6 = false; p->IFG_6 = false; 
+    p->IE_6 = false; p->SEL_6 = false; p->SEL2_6 = false;
+    
+    p->DIR_7 = false; p->OUT_7 = false; p->IFG_7 = false; 
+    p->IE_7 = false; p->SEL_7 = false; p->SEL2_7 = false;
+}
+
+/* POWER UP CLEAR (PUC)      
+ *
+ * A PUC is always generated when a POR is generated, but a POR is not
+ * generated by a PUC. The following events trigger a PUC:  
+ *                                                
+ * A POR signal                             
+ * Watchdog timer expiration when in watchdog mode only
+ * Watchdog timer security key violation          
+ * A Flash memory security key violation        
+ * A CPU instruct fetch from the peripheral address range 0h to 01FFh
+
+void power_up_clear () {
+  *P1OUT = *P1DIR = *P1IFG = *P1IE = *P1SEL = *P1SEL2 = *P1REN = 0;
+}
+ */

emulator/devices/peripherals/port1.h

@@ -0,0 +1,54 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _PORT_1_
+#define _PORT_1_
+
+#include "../cpu/registers.h"
+#include "../utilities.h"
+
+struct Port_1 {
+  // Peripheral register pointers
+  
+  // Port 1        = r/w =   = reset? =
+  uint8_t *IN;   /* r          -      */
+  uint8_t *OUT;  /* r/w     unchanged */
+  uint8_t *DIR;  /* r/w     PUC reset */
+  uint8_t *IFG;  /* r/w     PUC reset */
+  uint8_t *IES;  /* r/w     unchanged */
+  uint8_t *IE;   /* r/w     PUC reset */
+  uint8_t *SEL;  /* r/w     PUC reset */
+  uint8_t *SEL2; /* r/w     PUC reset */
+  uint8_t *REN;  /* r/w     PUC reset */
+
+  // Peripherals activation flags (for emulator)
+  bool DIR_0, OUT_0, IFG_0, IE_0, SEL_0, SEL2_0;
+  bool DIR_1, OUT_1, IFG_1, IE_1, SEL_1, SEL2_1;
+  bool DIR_2, OUT_2, IFG_2, IE_2, SEL_2, SEL2_2;
+  bool DIR_3, OUT_3, IFG_3, IE_3, SEL_3, SEL2_3;
+  bool DIR_4, OUT_4, IFG_4, IE_4, SEL_4, SEL2_4;
+  bool DIR_5, OUT_5, IFG_5, IE_5, SEL_5, SEL2_5;
+  bool DIR_6, OUT_6, IFG_6, IE_6, SEL_6, SEL2_6;
+  bool DIR_7, OUT_7, IFG_7, IE_7, SEL_7, SEL2_7;
+
+};
+
+void setup_port_1(Emulator *emu);
+void handle_port_1(Emulator *emu);
+
+#endif

emulator/devices/peripherals/timer_a.c

@@ -0,0 +1,280 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "timer_a.h"
+
+void handle_timer_a (Emulator *emu)
+{
+  Cpu *cpu = emu->cpu;
+  Timer_a *timer = cpu->timer_a;
+
+  uint16_t TA0CTL = *timer->TA0CTL;
+
+  // --------------------------------------------------------------
+  // Handle Timer_A0 Control Register
+  uint8_t TASSEL0 = (TA0CTL >> 8) & 0x03;
+  uint8_t ID0     = (TA0CTL >> 6) & 0x03;
+  uint8_t MC0     = (TA0CTL >> 4) & 0x03;
+  uint8_t TA0CLR   = (TA0CTL >> 2) & 0x01;
+  uint8_t TA0IE    = (TA0CTL >> 1) & 0x01;
+  uint8_t TA0IFG   = TA0CTL & 0x01;
+
+  switch (TASSEL0) {
+  case 0b00: {timer->source_0 = TACLK; break;}
+  case 0b01: {timer->source_0 = ACLK; break;}
+  case 0b10: {timer->source_0 = SMCLK; break;}
+  case 0b11: {timer->source_0 = INCLK; break;}
+  default: break;
+  }
+
+  switch (ID0) {
+  case 0b00: {timer->idiv_0 = 1; break;}
+  case 0b01: {timer->idiv_0 = 2; break;}
+  case 0b10: {timer->idiv_0 = 4; break;}
+  case 0b11: {timer->idiv_0 = 8; break;}
+  default: break;
+  }
+
+  switch (MC0) {
+  case 0b00: {timer->mode_0 = STOP_MODE; break;}
+  case 0b01: {timer->mode_0 = UP_MODE; break;}
+  case 0b10: {timer->mode_0 = CONTINOUS_MODE; break;}
+  case 0b11: {timer->mode_0 = UP_DOWN_MODE; break;}
+  default: break;
+  }
+
+  /* Timer_A clear; setting this bit resets TAR, the clock divider,
+     and the count direction. The TACLR bit is automatically 
+     reset and is always read as zero. */
+  if (TA0CLR) {    
+    *timer->TA0R = 0;
+    *timer->TA0CTL &= 0xFF0B; // 0b00001011
+  }
+  // --------------------------------------------------------------
+
+  // --------------------------------------------------------------
+  // Handle Timer_A0 Capture/Compare Control Register
+  
+  uint16_t TA0CCTL1 = *timer->TA0CCTL1;
+ 
+  uint8_t OUTMOD1 = (TA0CCTL1 >> 5) & 0x07;  
+  uint8_t CAP     = (TA0CCTL1 >> 8) & 0x01;
+
+  switch (OUTMOD1) {
+  case 0b000: {break;}
+  case 0b001: {break;}
+  case 0b010: {break;}
+  case 0b011: {break;}
+  case 0b100: {break;}
+  case 0b101: {break;}
+  case 0b110: {break;}
+  case 0b111: {break;}
+  default: break;
+  }  
+
+  // CAP field (Capture or compare mode?)
+  timer->capture_mode_0 = CAP;
+  timer->compare_mode_0 = !CAP;
+
+  // --------------------------------------------------------------
+
+
+  static double last_period = 0;
+  static double last_pulse_width = 0;
+
+  // Figure Out Frequency in up mode
+  if (timer->compare_mode_0) {
+    if (timer->mode_0 == UP_MODE) {
+      uint16_t period_ct = *timer->TA0CCR0 + 1;       
+      uint64_t frequency = emu->cpu->bcm->mclk_freq;
+
+      double period = (1.0/frequency) * period_ct;// In seconds
+      double pulse_width = (1.0/frequency) * (*timer->TA0CCR1);
+      double duty_cycle  = pulse_width / period; 
+      
+      /*
+	printf("period: %lf\npulse_width: %lf\nduty: %lf%%\n", 
+	period, pulse_width, duty_cycle);
+      */
+      
+      if (last_period != period || last_pulse_width != pulse_width) {
+	if (period >= 0.015 && period <= 0.025) { // 0.020 is sweet spot 50 Hz
+	  //printf("period: %lf, last_period: %lf\n", period, last_period);
+	  //printf("pw: %lf, last_pw: %lf\n", pulse_width, last_pulse_width);
+
+	  if (pulse_width < 0.0009) {
+	    // Send Control for 0 degrees
+	    //print_console(emu, "0 degrres\n");
+	    
+	    uint8_t byte = 0;
+	    send_control(emu, SERVO_MOTOR, (void *)&byte, 1);
+	  }
+	  else if (pulse_width < 0.0012) {
+	    // Send Control for 30 Degrees
+	    //print_console(emu, "30 degrres\n");
+	    
+	    uint8_t byte = 30;
+	    send_control(emu, SERVO_MOTOR, (void *)&byte, 1);
+	  }
+	  else if (pulse_width < 0.0015) {
+	    // Send Control for 60 degrees
+	    uint8_t byte = 60;
+	    send_control(emu, SERVO_MOTOR, (void *)&byte, 1);
+	  }
+	  else if (pulse_width < 0.0018) {
+	    // Send Control for 90 degrees
+	    //print_console(emu, "90 degrres\n");
+	    
+	    uint8_t byte = 90;
+	    send_control(emu, SERVO_MOTOR, (void *)&byte, 1);
+	  }
+	  else if (pulse_width < 0.0021) {
+	    // Send Control for 120 degrees
+	    uint8_t byte = 120;
+	    send_control(emu, SERVO_MOTOR, (void *)&byte, 1);
+	  }
+	  else if (pulse_width >= 0.0021) {
+	    // Send Control for 150 degrees
+	    uint8_t byte = 150;
+	    send_control(emu, SERVO_MOTOR, (void *)&byte, 1);
+	  }
+	}
+      }
+
+      if (OUTMOD1 == 0b111) { // RESET/SET
+	uint16_t pulse_width = *timer->TA0CCR1;	
+      }
+
+      last_period = period;
+      last_pulse_width = pulse_width;
+    }
+  }
+
+  if (!timer->timer_0_running && MC0 != 0) {
+    //print_console(emu, "START TIMER\n");
+    timer->timer_0_running = true;
+
+    //pthread_t pp;
+    //if(pthread_create(&pp, NULL, timer_A0_thread , (void*)emu)){
+    //printf("ErrorcreatingDCOthread\n");
+    //exit(1);
+    //}
+  }
+}
+
+void *timer_A0_thread (void *data)
+{
+  Emulator *emu = (Emulator *) data;
+  Timer_a *timer = emu->cpu->timer_a;
+  uint64_t counter;
+
+  bool high = false;
+
+  while (true) {
+    high = true;
+    counter = 0;
+
+    while (true) {
+      if ( counter == (*timer->TA0CCR1) ) {
+	high = false;
+      }
+      else if ( counter == (*timer->TA0CCR0 + 1) ) {
+	break;
+      }
+      
+      mclk_wait_cycles(emu, 1);
+      counter++;
+
+      high ? printf("-") : printf("_");
+      fflush(stdout);
+    }
+  }
+
+  return NULL;
+}
+
+void setup_timer_a (Emulator *emu)
+{
+  Cpu *cpu = emu->cpu;
+  Timer_a *timer = cpu->timer_a;
+
+  // Configure Timer_A0 Registers
+  const uint16_t TA0CTL_VLOC  = 0x160;
+  const uint16_t TA0R_VLOC  = 0x170;
+  const uint16_t TA0CCTL0_VLOC = 0x162;
+  const uint16_t TA0CCR0_VLOC  = 0x172;
+  const uint16_t TA0CCTL1_VLOC  = 0x164;
+  const uint16_t TA0CCR1_VLOC  = 0x174;
+  const uint16_t TA0CCTL2_VLOC  = 0x166;
+  const uint16_t TA0CCR2_VLOC  = 0x176;
+  const uint16_t TA0IV_VLOC  = 0x12E;
+  
+  *(timer->TA0CTL  = (uint16_t *) get_addr_ptr(TA0CTL_VLOC)) = 0;
+  *(timer->TA0R  = (uint16_t *) get_addr_ptr(TA0R_VLOC)) = 0;
+  *(timer->TA0CCTL0  = (uint16_t *) get_addr_ptr(TA0CCTL0_VLOC)) = 0;
+  *(timer->TA0CCR0  = (uint16_t *) get_addr_ptr(TA0CCR0_VLOC)) = 0;
+  *(timer->TA0CCTL1  = (uint16_t *) get_addr_ptr(TA0CCTL1_VLOC)) = 0;
+  *(timer->TA0CCR1  = (uint16_t *) get_addr_ptr(TA0CCR1_VLOC)) = 0;
+  *(timer->TA0CCTL2  = (uint16_t *) get_addr_ptr(TA0CCTL2_VLOC)) = 0;
+  *(timer->TA0CCR2  = (uint16_t *) get_addr_ptr(TA0CCR2_VLOC)) = 0;
+  *(timer->TA0IV  = (uint16_t *) get_addr_ptr(TA0IV_VLOC)) = 0;
+
+  // Configure Timer_A1 Registers
+  const uint16_t TA1CTL_VLOC  = 0x180;
+  const uint16_t TA1R_VLOC  = 0x190;
+  const uint16_t TA1CCTL0_VLOC = 0x182;
+  const uint16_t TA1CCR0_VLOC  = 0x192;
+  const uint16_t TA1CCTL1_VLOC  = 0x184;
+  const uint16_t TA1CCR1_VLOC  = 0x194;
+  const uint16_t TA1CCTL2_VLOC  = 0x186;
+  const uint16_t TA1CCR2_VLOC  = 0x196;
+  const uint16_t TA1IV_VLOC  = 0x11E;
+  
+  *(timer->TA1CTL  = (uint16_t *) get_addr_ptr(TA1CTL_VLOC)) = 0;
+  *(timer->TA1R  = (uint16_t *) get_addr_ptr(TA1R_VLOC)) = 0;
+  *(timer->TA1CCTL0  = (uint16_t *) get_addr_ptr(TA1CCTL0_VLOC)) = 0;
+  *(timer->TA1CCR0  = (uint16_t *) get_addr_ptr(TA1CCR0_VLOC)) = 0;
+  *(timer->TA1CCTL1  = (uint16_t *) get_addr_ptr(TA1CCTL1_VLOC)) = 0;
+  *(timer->TA1CCR1  = (uint16_t *) get_addr_ptr(TA1CCR1_VLOC)) = 0;
+  *(timer->TA1CCTL2  = (uint16_t *) get_addr_ptr(TA1CCTL2_VLOC)) = 0;
+  *(timer->TA1CCR2  = (uint16_t *) get_addr_ptr(TA1CCR2_VLOC)) = 0;
+  *(timer->TA1IV  = (uint16_t *) get_addr_ptr(TA1IV_VLOC)) = 0;
+
+  // Configure other
+  timer->source_0 = 0b10;
+  timer->timer_0_running = false;
+
+  timer->source_1 = 0b10;
+  timer->timer_1_running = false;
+}
+
+/* POWER UP CLEAR (PUC)      
+ *
+ * A PUC is always generated when a POR is generated, but a POR is not
+ * generated by a PUC. The following events trigger a PUC:  
+ *                                                
+ * A POR signal                             
+ * Watchdog timer expiration when in watchdog mode only
+ * Watchdog timer security key violation          
+ * A Flash memory security key violation        
+ * A CPU instruct fetch from the peripheral address range 0h to 01FFh
+
+void power_up_clear () {
+  *P1OUT = *P1DIR = *P1IFG = *P1IE = *P1SEL = *P1SEL2 = *P1REN = 0;
+}
+ */

emulator/devices/peripherals/timer_a.h

@@ -0,0 +1,105 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _TIMER_A_H_
+#define _TIMER_A_H_
+
+#include "../cpu/registers.h"
+#include "../utilities.h"
+
+enum {STOP_MODE = 0, UP_MODE, CONTINOUS_MODE, UP_DOWN_MODE, NUM_MODES};
+
+struct Timer_a {
+  // (ALL RESET WITH POR)
+
+  // --- TIMER_A0 ----------
+  // Timer_A0 Control, @0x160
+  uint16_t *TA0CTL;
+
+  // Timer_A0 Counter, @0x170
+  uint16_t *TA0R;
+
+  // Timer_A0 Capture/Compare Control 0, @0x162
+  uint16_t *TA0CCTL0;
+
+  // Timer_A0 Capture/Compare 0, @0x172
+  uint16_t *TA0CCR0;
+
+  // Timer_A0 Capture/Compare Control 1, @0x164
+  uint16_t *TA0CCTL1;
+
+  // Timer_A0 Capture/Compare 1, @0x174
+  uint16_t *TA0CCR1;
+
+  // Timer_A0 Capture/Compare Control 2, @0x166
+  uint16_t *TA0CCTL2;
+
+  // Timer_A0 Capture/Compare 2, @0x176
+  uint16_t *TA0CCR2;
+
+  // Timer_A0 Interrupt Vector, @0x12E
+  uint16_t *TA0IV; // READ ONLY
+
+  // --- TIMER_A1
+  // Timer_A1 Control
+  uint16_t *TA1CTL;
+
+  // Timer_A1 Counter
+  uint16_t *TA1R;
+
+  // Timer_A1 Capture/Compare Control 0
+  uint16_t *TA1CCTL0;
+
+  // Timer_A1 Capture/Compare 0
+  uint16_t *TA1CCR0;
+
+  // Timer_A1 Capture/Compare Control 1
+  uint16_t *TA1CCTL1;
+
+  // Timer_A1 Capture/Compare 1
+  uint16_t *TA1CCR1;
+
+  // Timer_A1 Capture/Compare Control 2
+  uint16_t *TA1CCTL2;
+
+  // Timer_A1 Capture/Compare 2
+  uint16_t *TA1CCR2;
+
+  // Timer_A1 Interrupt Vector
+  uint16_t *TA1IV; // READ ONLY
+
+  bool timer_0_running;
+  bool capture_mode_0;
+  bool compare_mode_0;
+  uint8_t source_0;
+  uint8_t idiv_0;
+  uint8_t mode_0;
+
+  bool timer_1_running;
+  bool capture_mode_1;
+  bool compare_mode_1;
+  uint8_t source_1;
+  uint8_t idiv_1;
+  uint8_t mode_1;
+};
+
+void *timer_A0_thread (void *data);
+void setup_timer_a (Emulator *emu);
+void handle_timer_a (Emulator *emu);
+
+#endif

emulator/devices/peripherals/usci.c

@@ -0,0 +1,183 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#define TXIFG 0x02
+#define RXIFG 0x01
+
+#include "usci.h"
+
+/*
+int master;
+FILE *slave;
+int sp;
+char c;
+
+void *thrd (void *ctxt)
+{
+  Usci *usci = (Usci *)ctxt;
+  char buf[64] = {0};
+
+  while (true) {
+    usleep(333);
+    if ( read(sp, buf, 1) > 0 ) {
+      while (*usci->IFG2 & RXIFG);
+
+      if (*buf == '\n') {
+	*buf = '\r';
+      }
+      if (*buf == '\\') {
+	// Ah, escape sequence, what will I parse it as?
+	read(sp, buf, 1);
+	if (*buf == 'h') {
+	  read(sp, buf, 2);
+	  buf[2] = 0;
+	  *usci->UCA0RXBUF = (uint8_t) strtoul(buf, NULL, 16);
+	}
+      }
+      else {    
+	*usci->UCA0RXBUF = *(uint8_t *) buf;
+      }
+
+      *usci->IFG2 |= RXIFG;
+    }
+  }  
+
+  return NULL;
+}
+
+void open_pty (Emulator *emu) 
+{
+  Cpu *cpu = emu->cpu;
+
+  char slavename[64], buf[64];
+  struct termios termios_p;
+  
+  master = posix_openpt(O_RDWR);
+
+  grantpt(master);
+  unlockpt(master);
+  ptsname_r(master, slavename, sizeof slavename);
+  snprintf(buf, sizeof buf, "-S%s/%d", strrchr(slavename,'/')+1, master);
+  
+  // Child (pty)
+  if( !fork() ) {   
+    char * const args[] = {
+      "xterm", buf, 
+      NULL
+    };
+
+    setpgid(0, 0);
+    execvp(args[0], args);
+    exit(1);
+  }
+  // Parent                                                            
+
+  sp = open(slavename, O_RDWR, O_NONBLOCK);  
+  read(sp, buf, 100);
+
+  tcgetattr(sp, &termios_p);
+  termios_p.c_lflag |= ECHO;
+  tcsetattr(sp, 0, &termios_p);
+  
+  pthread_t t;
+  if( pthread_create(&t, NULL, thrd, (void *)cpu->usci ) ) {
+    fprintf(stderr, "Error creating thread\n");
+  }
+}
+*/
+
+void handle_usci (Emulator *emu) 
+{
+  Cpu *cpu = emu->cpu;
+  Debugger *deb = emu->debugger;
+  Usci *usci = cpu->usci;
+  Port_1 *p1 = cpu->p1;
+  
+  static bool uart_active = false;
+  
+  // Handle sending from TX pin (P1.2) 
+  if (p1->SEL_2 && p1->SEL2_2) {
+    if (uart_active == false) {
+      puts("UART TX pin activated on P1.2");
+      uart_active = true;
+    }
+
+    // UCAxTXIFG
+    if (*usci->IFG2 & TXIFG) {
+      uint8_t c = *usci->UCA0TXBUF;
+      unsigned char str[2];
+      str[0] = c;
+      str[1] = 0;
+
+      *usci->IFG2 &= ~TXIFG;
+
+      if (c & 0xFF) {
+	if (deb->web_interface) {
+	  print_serial(emu, (char*)&str[0]);
+	  //write(sp, usci->UCA0TXBUF, 1);
+	}
+	else if (deb->console_interface) {
+	  //write(sp, usci->UCA0TXBUF, 1);
+	}
+
+	*usci->UCA0TXBUF = 0;
+      }
+
+      //*usci->IFG2 &= TXIFG;
+      *usci->IFG2 |= TXIFG;
+    }
+  }
+
+  return;
+}
+
+void setup_usci (Emulator *emu) 
+{
+  Cpu *cpu = emu->cpu;
+  Usci *usci = cpu->usci;
+
+  static const uint16_t UCA0CTL0_VLOC = 0x60; // Control Register 0
+  static const uint16_t UCA0CTL1_VLOC = 0x61; // Control Register 1
+  static const uint16_t UCA0BR0_VLOC  = 0x62; // Baud Rate ctl Register 0
+  static const uint16_t UCA0BR1_VLOC  = 0x63; // Baud Rate ctl Register 1
+  static const uint16_t UCA0MCTL_VLOC = 0x64; // Modulation ctl Register
+  static const uint16_t UCA0STAT_VLOC = 0x65; // Status Register
+  static const uint16_t UCA0RXBUF_VLOC = 0x66; // RECV buffer register
+  static const uint16_t UCA0TXBUF_VLOC = 0x67; // Transmit buffer register
+  static const uint16_t UCA0ABCTL_VLOC = 0x5D; // Auto-Baud control register
+  static const uint16_t UCA0IRTCTL_VLOC = 0x5E; // IrDA transmit control reg
+  static const uint16_t UCA0IRRCTL_VLOC = 0x5F; // IrDA Receive control reg
+  static const uint16_t IFG2_VLOC       = 0x03; // Interrupt flag register 2
+  
+  // Set initial values
+  *(usci->UCA0CTL0   = (uint8_t *) get_addr_ptr(UCA0CTL0_VLOC))  = 0;
+  *(usci->UCA0CTL1  = (uint8_t *) get_addr_ptr(UCA0CTL1_VLOC))   = 0x01;
+  *(usci->UCA0BR0  = (uint8_t *) get_addr_ptr(UCA0BR0_VLOC))     = 0;
+  *(usci->UCA0BR1  = (uint8_t *) get_addr_ptr(UCA0BR1_VLOC))     = 0;
+  *(usci->UCA0MCTL  = (uint8_t *) get_addr_ptr(UCA0MCTL_VLOC))   = 0;
+  *(usci->UCA0STAT  = (uint8_t *) get_addr_ptr(UCA0STAT_VLOC))   = 0;
+  *(usci->UCA0RXBUF  = (uint8_t *) get_addr_ptr(UCA0RXBUF_VLOC)) = 0;
+  *(usci->UCA0TXBUF  = (uint8_t *) get_addr_ptr(UCA0TXBUF_VLOC)) = 0;
+  *(usci->UCA0ABCTL  = (uint8_t *) get_addr_ptr(UCA0ABCTL_VLOC))   = 0;
+  *(usci->UCA0IRTCTL  = (uint8_t *) get_addr_ptr(UCA0IRTCTL_VLOC)) = 0;
+  *(usci->UCA0IRRCTL  = (uint8_t *) get_addr_ptr(UCA0IRRCTL_VLOC)) = 0;  
+
+  usci->IFG2  = (uint8_t *) get_addr_ptr(IFG2_VLOC);
+  *usci->IFG2 |= TXIFG;
+  *usci->IFG2 &= ~RXIFG;
+}

emulator/devices/peripherals/usci.h

@@ -0,0 +1,100 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#pragma once
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <termios.h>
+#include <stdint.h>
+#include <fcntl.h>
+
+#include "../cpu/registers.h"
+#include "../utilities.h"
+
+// USCI_Ax Control Register 0
+typedef struct Ctl0 
+{
+  bool UCPEN;   // Parity Enable: Disabled (0) Enabled (1)
+  bool UCPAR;   // Parity Select: Odd (0) Even (1)
+  bool UCMSB;   // MSB first: LSB first (0) MSB first (1)
+  bool UC7BIT;  // Char Len: 8-bit (0) 7-bit (1)
+  bool UCSPB;   // Stop bit select: One STP bits (0) Two STP bits (1)
+ 
+  // USCI Mode - When UCSYNC = 0, its asynchronous mode
+  // UART Mode (00)  
+  // Idle-line multiprocessor mode (01)
+  // Address-bit multiprocessor mode (10)
+  // UART mode with automatic baud rate detection (11)
+  uint8_t UCMODE : 2;
+  
+  bool UCSYNC;       // Synchronous mode enable: Async (0) Sync (1)
+} Ctl0;
+
+// USCI_Ax Control Register 1
+typedef struct Ctl1 {
+  // USCI clock source select, these bits select the Baud rate source clock.
+  // UCLK: 00 | ACLK: 01 | SMCLK: 10 | SMCLK: 11
+  uint8_t UCSSEL : 2;  
+
+  bool UCRXEIE : 1; // Recv erroneous-character interrupt enable
+  bool UCBRKIE : 1; // receive break character interrupt enable
+
+  // dormant. Puts SCI into sleep mode: 
+  // 0 - Not dormant. All received chars will set UCAxRXIFG
+  // 1 - Dormant. Only chars that are preceded by an idle line or addr bit
+  bool UCDORM : 1;
+
+  // Transmit address, Next frame to be transmitted will be marked as address
+  // Depedning on the selected multiproc mode. Data (0) Address (1)
+  bool UCTXADDR : 1;
+
+  // Transmit break, transmits a break with the next write to the TX buffer
+  // Not a break (0) Transmit a break (1)
+  bool UCTXBRK : 1;
+  
+  // Software reset enable: 
+  // Disabled. USCI reset released for operation (0)
+  // Enabled. USCI logic held in reset start. (1)
+  bool UCSWRST : 1;  
+} Ctl1;
+
+typedef struct Usci 
+{
+  // USCI_A0 Module Register Pointers
+  uint8_t *UCA0CTL0;
+  uint8_t *UCA0CTL1;
+  uint8_t *UCA0BR0;
+  uint8_t *UCA0BR1;
+  uint8_t *UCA0MCTL;
+  uint8_t *UCA0STAT;
+  uint8_t *UCA0RXBUF;
+  uint8_t *UCA0TXBUF;
+  uint8_t *UCA0ABCTL;
+  uint8_t *UCA0IRTCTL;
+  uint8_t *UCA0IRRCTL;
+
+  uint8_t *IFG2;
+  
+  Ctl0 ctl0;
+  Ctl1 ctl1;
+}
+Usci;
+
+void setup_usci(Emulator *emu);
+void handle_usci(Emulator *emu);

emulator/devices/utilities.c

@@ -0,0 +1,394 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "utilities.h"
+
+extern uint8_t* MEMSPACE;
+
+/**
+ * @brief This function loads the default TI bootloader code into virtual mem
+ * @param virt_addr The location in virtual memory to load the bootloader
+ */
+void load_bootloader(uint16_t virt_addr)
+{
+    uint16_t *real_addr = get_addr_ptr(virt_addr);
+  
+    memmove(real_addr, blc, sizeof blc);
+    printf("Loaded booloader code into address 0x%04X\n", virt_addr);
+}
+
+/**
+ * @brief This function loads firmware from a binary file on disk into the
+ * virtual memory of the emulated device at base virt_loc
+ * @param file_name The file name of the binary to load into virtual memory
+ * @param virt_loc The location in virtual memory to load the firmware
+ */
+void load_firmware(Emulator *emu, char *file_name, uint16_t virt_addr)
+{
+    uint32_t size, result;
+    char str[100] = {0};
+
+    sprintf(str, "Loading firmware: ( %s )\n", file_name);
+  
+    printf("%s", str);
+    print_console(emu, str);
+
+    FILE *fd = fopen(file_name, "rb+");
+  
+    if (fd == NULL)
+    {
+        printf("Could not open %s, exiting.\n", file_name);
+        exit(1);
+    }
+
+    /* obtain file size */
+    fseek(fd, 0, SEEK_END);
+    size = ftell(fd);
+    rewind(fd);
+
+    // check size
+    if (size > (0x10000 - 0x0C000))
+    {
+        printf("SizeTooBig\n");
+        print_console(emu, "Flash Size too small to fit your binary. Quitting, please refresh to try again. Ensure you are compiling for the right MSP version.\n");
+        usleep(20000);
+        exit(1);
+    }
+
+    uint16_t *real_addr = get_addr_ptr(virt_addr);
+
+    result = fread(real_addr, 1, size, fd);
+
+    sprintf(str, "Placed %d bytes into flash\n\n", result);
+    printf("%s", str);
+    print_console(emu, str);
+
+    fclose(fd);
+}
+
+uint16_t *get_stack_ptr(Emulator *emu)
+{
+    Cpu *cpu = emu->cpu;
+  
+    return (uint16_t *) (MEMSPACE + cpu->sp);
+}
+
+/**
+ * @brief Get the host's pointer to the virtual address of the guest
+ * @param virt_addr The virtual address of the guest to translate to a useable
+ * one in context of the host
+ * @return Pointer to the host's location of the guest's memory address
+ */
+uint16_t *get_addr_ptr(uint16_t virt_addr)
+{
+    return (uint16_t *) (MEMSPACE + virt_addr);
+}
+
+/**
+ * @brief Get a pointer to the register specified by the numeric register value
+ * @param cpu A pointer to the CPU structure
+ * @param reg The numeric value of the register
+ * @return Pointer to the register in question, NULL if register doesn't exist
+ */
+int16_t *get_reg_ptr(Emulator *emu, uint8_t reg)
+{
+    Cpu *cpu = emu->cpu;
+
+    static int16_t r2 = 0;
+    
+    switch (reg)
+    {    
+        case 0x0: return (int16_t *) &cpu->pc;
+        case 0x1: return (int16_t *) &cpu->sp;
+
+        case 0x2:
+        {
+            r2 = sr_to_value(emu);
+            return &r2;
+        }
+            
+        case 0x3: return &cpu->cg2;
+        case 0x4: return &cpu->r4;
+        case 0x5: return &cpu->r5;
+        case 0x6: return &cpu->r6;
+        case 0x7: return &cpu->r7;
+        case 0x8: return &cpu->r8;
+        case 0x9: return &cpu->r9;
+        case 0xA: return &cpu->r10;
+        case 0xB: return &cpu->r11;
+        case 0xC: return &cpu->r12;
+        case 0xD: return &cpu->r13;
+        case 0xE: return &cpu->r14;
+        case 0xF: return &cpu->r15;
+            
+        default:
+        {
+            puts("Invalid Register Number");
+            return 0;
+        }
+    }
+}
+
+/**
+ * @brief Convert register ASCII name to it's respective numeric value
+ * @param name The register's ASCII name
+ * @return The numeric equivalent for the register on success, -1 if an 
+ * invalid name was supplied
+ */
+int8_t reg_name_to_num(char *name)
+{
+    if ( !strncasecmp("%r0", name, sizeof "%r0") ||
+         !strncasecmp("r0", name, sizeof "r0")   ||
+         !strncasecmp("%pc", name, sizeof "%pc") ||
+         !strncasecmp("pc", name, sizeof "pc") )
+    {
+        return 0;
+    }
+    else if ( !strncasecmp("%r1", name, sizeof "%r1")   ||
+	          !strncasecmp("r1", name, sizeof "r1")     ||
+	          !strncasecmp("%sp", name, sizeof "%sp")   ||
+	          !strncasecmp("sp", name, sizeof "sp") )
+    {
+        return 1;
+    }
+    else if ( !strncasecmp("%r2", name, sizeof "%r2") ||
+	        !strncasecmp("r2", name, sizeof "r2")     ||
+	        !strncasecmp("%sr", name, sizeof "%sr")     ||
+	        !strncasecmp("sr", name, sizeof "sr") ) {
+    
+        return 2;
+    }
+    else if ( !strncasecmp("%r3", name, sizeof "%r3") ||
+	        !strncasecmp("r3", name, sizeof "r3")     ||
+	        !strncasecmp("%cg2", name, sizeof "%cg2") ||
+	        !strncasecmp("cg2", name, sizeof "cg2") ) {                        
+        
+        return 3;
+    }
+    else if ( !strncasecmp("%r4", name, sizeof "%r4") ||
+	        !strncasecmp("r4", name, sizeof "r4") ) {
+    
+        return 4;
+    }
+    else if ( !strncasecmp("%r5", name, sizeof "%r5") ||
+	        !strncasecmp("r5", name, sizeof "r5") ) {
+    
+        return 5;
+    }
+    else if ( !strncasecmp("%r6", name, sizeof "%r6") ||
+	        !strncasecmp("r6", name, sizeof "r6") ) {
+    
+        return 6;
+    }
+    else if ( !strncasecmp("%r7", name, sizeof "%r7") ||
+	        !strncasecmp("r7", name, sizeof "r7") ) {
+    
+        return 7;
+    }
+    else if ( !strncasecmp("%r8", name, sizeof "%r8") ||
+	        !strncasecmp("r8", name, sizeof "r8") ) {
+    
+        return 8;
+    }
+    else if ( !strncasecmp("%r9", name, sizeof "%r9") ||
+	        !strncasecmp("r9", name, sizeof "r9") ) {
+    
+        return 9;
+    }
+    else if ( !strncasecmp("%r10", name, sizeof "%r10") ||
+	        !strncasecmp("r10", name, sizeof "r10") ) {
+    
+        return 10;
+    }
+    else if ( !strncasecmp("%r11", name, sizeof "%r11") ||
+	        !strncasecmp("r11", name, sizeof "r11") ) {
+    
+        return 11;
+    }
+    else if ( !strncasecmp("%r12", name, sizeof "%r12") ||
+	        !strncasecmp("r12", name, sizeof "r12") ) {
+    
+        return 12;
+    }
+    else if ( !strncasecmp("%r13", name, sizeof "%r13") ||
+	        !strncasecmp("r13", name, sizeof "r13") ) {
+    
+        return 13;
+    }
+    else if ( !strncasecmp("%r14", name, sizeof "%r14") ||
+	        !strncasecmp("r14", name, sizeof "r14") ) {
+    
+        return 14;
+    }
+    else if ( !strncasecmp("%r15", name, sizeof "%r15") ||
+	        !strncasecmp("r15", name, sizeof "r15") ) {
+    
+        return 15;
+    }
+    
+    return -1;
+}
+
+/**
+ * @brief Convert register number into its ASCII name
+ * @param number The register number (0, 1, 2, ...) associated with a 
+ * register's name like (R0, R1, R2, ...)
+ * @param name A pointer to an allocated character array to fill up with
+ * the register's ASCII name
+ */
+void reg_num_to_name(uint8_t number, char *name)
+{    
+    switch (number)
+    { 
+        case 0x0:
+        {
+            strncpy(name, "PC\0", 3);
+            return;
+        }
+        case 0x1:
+        {
+            strncpy(name, "SP\0", 3);
+            return;
+        }
+        case 0x2:
+        {
+            strncpy(name, "SR\0", 3);
+            return;
+        }
+        case 0x3:
+        {
+            strncpy(name, "R3\0", 3);
+            return;
+        }
+        case 0x4:
+        {
+            strncpy(name, "R4\0", 3);
+            return;
+        }
+        case 0x5:
+        {
+            strncpy(name, "R5\0", 3);
+            return;
+        }
+        case 0x6:
+        {
+            strncpy(name, "R6\0", 3);
+            return;
+        }
+        case 0x7:
+        {
+            strncpy(name, "R7\0", 3);
+            return;
+        }
+        case 0x8:
+        {
+            strncpy(name, "R8\0", 3);
+            return;
+        }
+        case 0x9:
+        {
+            strncpy(name, "R9\0", 3);
+            return;
+        }
+        case 0xA:
+        {
+            strncpy(name, "R10\0", 4);
+            return;
+        }
+        case 0xB:
+        {
+            strncpy(name, "R11\0", 4);
+            return;
+        }
+        case 0xC:
+        {
+            strncpy(name, "R12\0", 4);
+            return;
+        }
+        case 0xD:
+        {
+            strncpy(name, "R13\0", 4);
+            return;
+        }
+        case 0xE:
+        {
+            strncpy(name, "R14\0", 4);
+            return;
+        }
+        case 0xF:
+        {
+            strncpy(name, "R15\0", 4);
+            return;
+        }
+        default:
+        {
+            strncpy(name, "???\0", 4);
+            return;
+        }
+    }
+}
+
+/**
+ * @brief This function displays the help menu to the user if he (or - but in
+ * practice all too seldom - she) enters incorrect parameters or prompts the 
+ * help menu with "help" or "h"
+ */
+const char* LocalHelpStr =
+"**************************************************\n"\
+"*\t\tMSP430-Emulator\n*\n*\tUsage: ./msp430 BINARY_FIRMWARE\n*\n"\
+"* run\t\t\t[Run Program Until Breakpoint is Hit]\n"\
+"* step [N]\t\t[Step Into Instruction]\n"\
+"* dump [HEX_ADDR|Rn]\t[Dump Memory direct or at register value]\n"\
+"* set [HEX_ADDR|Rn]\t[Set Memory or Register Location]\n"\
+"* dis [N][HEX_ADDR]\t[Disassemble Instructions]\n"\
+"* break ADDR\t\t[Set a Breakpoint]\n"\
+"* bps\t\t\t[Display Breakpoints]\n"\
+"* regs\t\t\t[Display Registers]\n"\
+"* CTRL+C\t\t[Pause Execution]\n"\
+"* reset\t\t\t[Reset Machine]\n"\
+"* quit\t\t\t[Exit program]\n"\
+"**************************************************\n";
+
+const char* WebHelpStr =
+"--------------------------------------------------\n"\
+" \t\t[MSP430-Emulator]\n"\
+" run\t\t\t[Run Program Until Breakpoint is Hit]\n"\
+" step [N]\t\t[Step Into Instruction]\n"\
+" dump [HEX_ADDR|Rn]\t[Dump Memory direct or at register value]\n"\
+" set [HEX_ADDR|Rn]\t[Set Memory or Register Location]\n"\
+" dis [N][HEX_ADDR]\t[Disassemble Instructions]\n"\
+" break ADDR\t\t[Set a Breakpoint]\n"\
+" bps\t\t\t[Display Breakpoints]\n"\
+" regs\t\t\t[Display Registers]\n"\
+" reset\t\t\t[Reset Machine]\n"\
+" quit\t\t\t[Exit program]\n"\
+"--------------------------------------------------\n";
+
+void display_help(Emulator *emu)
+{
+    Debugger *deb = emu->debugger;
+
+    if (deb->web_interface)
+    {
+        print_console(emu, WebHelpStr);
+    }
+    else
+    {
+        puts(LocalHelpStr);
+    }
+}

emulator/devices/utilities.h

@@ -0,0 +1,39 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#ifndef _UTILITIES_H_
+#define _UTILITIES_H_
+
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include "cpu/registers.h"
+#include "memory/memspace.h"
+
+void reg_num_to_name(uint8_t source_reg, char *reg_name);
+int16_t *get_reg_ptr(Emulator *emu, uint8_t reg);
+uint16_t *get_stack_ptr(Emulator *emu);
+uint16_t *get_addr_ptr(uint16_t virt_addr);
+int8_t reg_name_to_num(char *name);
+void load_bootloader(uint16_t virt_addr);
+void load_firmware(Emulator *emu, char *file_name, uint16_t virt_addr);
+void display_help(Emulator *emu);
+
+#endif

emulator/main.cpp

@@ -0,0 +1,127 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+  
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#include "main.h"
+
+int run(unsigned int port) {
+
+    Emulator *emu = (Emulator *) calloc( 1, sizeof(Emulator) );
+    Cpu *cpu = NULL; Debugger *deb = NULL;
+
+    emu->cpu       = (Cpu *) calloc(1, sizeof(Cpu));
+    emu->cpu->bcm  = (Bcm *) calloc(1, sizeof(Bcm));
+    emu->cpu->timer_a  = (Timer_a *) calloc(1, sizeof(Timer_a));
+    emu->cpu->p1   = (Port_1 *) calloc(1, sizeof(Port_1));
+    emu->cpu->usci = (Usci *) calloc(1, sizeof(Usci));
+
+    emu->debugger  = (Debugger *) calloc(1, sizeof(Debugger));
+    setup_debugger(emu);
+
+    cpu = emu->cpu;
+    deb = emu->debugger;
+
+    deb->server = (Server *) calloc(1, sizeof(Server));
+
+    if (deb->web_interface == true)
+        {
+            pthread_t *t = &deb->web_server_thread;
+
+            if ( pthread_create(t, NULL, web_server, (void *)emu ) )
+            {
+                fprintf(stderr, "Error creating web server thread\n");
+                return 1;
+            }
+
+            while (!deb->web_server_ready)
+                usleep(10000);
+
+            print_console(emu, " [MSP430 Emulator]\n Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)\n");
+//            print_console(emu, " [!] Upload your firmware (ELF format only); Type 'h' for debugger options.\n\n");
+
+            while (!deb->web_firmware_uploaded)
+                usleep(10000);
+        }
+
+        //register_signal(SIGINT); // Register Callback for CONTROL-c
+
+        initialize_msp_memspace();
+        initialize_msp_registers(emu);
+
+        setup_bcm(emu);
+        setup_timer_a(emu);
+        setup_port_1(emu);
+        setup_usci(emu);
+
+        load_bootloader(0x0C00);
+
+        if (deb->web_interface)
+            load_firmware(emu, (char*)"tmp.bin", 0xC000);
+
+        // display first round of registers
+        display_registers(emu);
+        disassemble(emu, cpu->pc, 1);
+        update_register_display(emu);
+
+        // Fetch-Decode-Execute Cycle (run machine)
+        while (!deb->quit)
+        {
+            // Handle debugger when CPU is not running
+            if (!cpu->running)
+            {
+                usleep(10000);
+                continue;
+            }
+
+            // Handle Breakpoints
+            handle_breakpoints(emu);
+
+            // Instruction Decoder
+            decode(emu, fetch(emu), EXECUTE);
+
+            // Handle Peripherals
+            handle_bcm(emu);
+            handle_timer_a(emu);
+            handle_port_1(emu);
+            handle_usci(emu);
+
+            // Average of 4 cycles per instruction
+            mclk_wait_cycles(emu, 4);
+        }
+        uninitialize_msp_memspace();
+        free(cpu->timer_a);
+        free(cpu->bcm);
+        free(cpu->p1);
+        free(cpu->usci);
+        free(cpu);
+        free(deb->server);
+        free(deb);
+        free(emu);
+
+        return 0;
+}
+
+
+int main(int argc, char *argv[])
+{
+    if (argv[1] == NULL)
+    {
+        puts("Need port argument");
+        return(1);
+    }
+    return run(strtoul(argv[1], NULL, 10));
+}

emulator/main.h

@@ -0,0 +1,66 @@
+/*
+  MSP430 Emulator
+  Copyright (C) 2020 Rudolf Geosits (rgeosits@live.esu.edu)
+
+  "MSP430 Emulator" 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.
+
+  "MSP430 Emulator" 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 <https://www.gnu.org/licenses/>.
+*/
+
+#pragma once
+
+#include <stdio.h>
+#include <time.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <string.h>
+#include <stdint.h>
+#include <signal.h>
+#include <sys/wait.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <pthread.h>
+#include <libwebsockets.h>
+#include <readline/readline.h>
+#include <readline/history.h>
+
+typedef struct Emulator Emulator;
+
+typedef struct Cpu Cpu;
+typedef struct Port_1 Port_1;
+typedef struct Usci Usci;
+typedef struct Bcm Bcm;
+typedef struct Timer_a Timer_a;
+typedef struct Status_reg Status_reg;
+
+typedef struct Debugger Debugger;
+typedef struct Server Server;
+typedef struct Packet Packet;
+
+#include "devices/peripherals/bcm.h"
+#include "devices/peripherals/timer_a.h"
+#include "devices/peripherals/port1.h"
+#include "devices/peripherals/usci.h"
+#include "devices/cpu/registers.h"
+#include "devices/utilities.h"
+#include "devices/memory/memspace.h"
+#include "debugger/websockets/emu_server.h"
+#include "devices/cpu/decoder.h"
+#include "debugger/debugger.h"
+#include "debugger/register_display.h"
+#include "debugger/disassembler.h"
+
+struct Emulator
+{
+    Cpu *cpu;
+    Debugger *debugger;
+};

setup.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+
+from distutils.core import setup, Extension
+
+emulator_files = [
+    'emulator/main.cpp',
+    'emulator/devices/utilities.c',
+    'emulator/devices/cpu/registers.c',
+    'emulator/devices/memory/memspace.c',
+    'emulator/debugger/debugger.c',
+    'emulator/debugger/disassembler.c',
+    'emulator/debugger/register_display.c',
+    'emulator/devices/cpu/decoder.c',
+    'emulator/devices/cpu/flag_handler.c',
+    'emulator/devices/cpu/formatI.c',
+    'emulator/devices/cpu/formatII.c',
+    'emulator/devices/cpu/formatIII.c',
+    'emulator/devices/peripherals/bcm.c',
+    'emulator/devices/peripherals/timer_a.c',
+    'emulator/devices/peripherals/usci.c',
+    'emulator/devices/peripherals/port1.c',
+    'emulator/debugger/websockets/emu_server.cpp',
+    'emulator/debugger/websockets/packet_queue.c',
+]
+# emulator_files.reverse()
+
+setup(name='msp430emu',
+      version='1.0',
+      description='MSP 430 Emulator',
+      author_email='zceemja@ucl.ac.uk',
+      # packages=['msp430emu'],
+      # package_dir={'msp430emu': '', 'emulator': 'emulator'},
+      # libraries=['wx', 'websocket'],
+      ext_modules=[Extension('emulator', emulator_files)]
+      )