diff --git a/bluetooth/tracing/BluetoothStack.wprp b/bluetooth/tracing/BluetoothStack.wprp
new file mode 100644
index 0000000..ed54769
Binary files /dev/null and b/bluetooth/tracing/BluetoothStack.wprp differ
diff --git a/bluetooth/tracing/readme.md b/bluetooth/tracing/readme.md
new file mode 100644
index 0000000..0436dd3
--- /dev/null
+++ b/bluetooth/tracing/readme.md
@@ -0,0 +1,28 @@
+# How to collect bluetooth logs
+
+## More info
+wpr.exe is available on all version of windows. More details can be found [here.](https://docs.microsoft.com/en-us/previous-versions/windows/it-pro/windows-8.1-and-8/hh448229%28v%3dwin.10%29)
+
+wpr.exe /? will also give you more information.
+
+## Collecting logs
+
+From an administrator command prompt: 
+1. Run as admin: "wpr.exe -start BluetoothStack.wprp"
+2. Toggle the Bluetooth radio off-on via the quick action menu or force a power cycle of the remote device (we want the connection information).
+3. Reproduce the issue.
+4. Run as admin: "wpr.exe -stop bth_tracing.etl"
+
+*Note: This will not continue tracing after a reboot*
+
+## Collecting logs across reboots
+From an administrator command prompt: 
+1. Run as admin: "wpr.exe -boottrace -addboot BluetoothStack.wprp"
+2. Reboot machine 
+  * Trace is not running until you reboot the machine
+3. Reproduce the issue - log will keep running across reboots until you stop it manually
+4. Run as admin: "wpr.exe -boottrace -stopboot bth_tracing.etl"
+
+## Collecting logs for driver or setup issues
+* Attach c:\windows\inf\setupapi.*.log and c:\Windows\Panther\setupact.log and c:\windows\logs\windowsupdate\* to the bug.
+* Attach Microsoft-Windows-Kernel-PnP%4Configuration.evtx (In event viewer as Microsoft-Windows-Kernel-PnP\Configuration).
\ No newline at end of file
diff --git a/usb/tools/ConnectionExerciser/README.md b/usb/tools/ConnectionExerciser/README.md
new file mode 100644
index 0000000..8212187
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/README.md
@@ -0,0 +1,21 @@
+Building the code:
+
+1) Clone the code for the ConnectionExerciser project.
+2) Clone the following projects into the libraries directory:
+
+    https://github.com/PFroese/ArduinoSerialCommand
+    https://github.com/mattnichols/Timer
+    https://github.com/adafruit/Adafruit_TCS34725
+    https://github.com/todbot/ServoEaser
+    
+3) Make the following minor modification to the ArduinoSerialCommand library:
+    - Open SerialCommand.h
+    - Locate the #define for MAXSERIALCOMMANDS
+    - Change the value from 16 to 32
+4) Install the Arduino IDE.
+5) Under "File | Preferences" change the "Sketchbook location" to the path to the ConnectionExerciser project.
+6) Select "File | Sketchbook | Shield" to open the project.
+7) Select "Tools | Board | Mega 2560"
+8) Select "Sketch | Compile" to build the device image, and "Sketch | Upload" to deploy it to an attached Connection Exerciser device.
+
+    
diff --git a/usb/tools/ConnectionExerciser/Schematics/3101_schematic.pdf b/usb/tools/ConnectionExerciser/Schematics/3101_schematic.pdf
new file mode 100644
index 0000000..6ed4ecf
Binary files /dev/null and b/usb/tools/ConnectionExerciser/Schematics/3101_schematic.pdf differ
diff --git a/usb/tools/ConnectionExerciser/Shield/Shield.ino b/usb/tools/ConnectionExerciser/Shield/Shield.ino
new file mode 100644
index 0000000..f3db01a
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/Shield/Shield.ino
@@ -0,0 +1,228 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#include <SoftwareSerial.h>
+#include <SerialCommand.h>
+#include <LiquidCrystal.h>
+#include <Event.h>
+#include <Timer.h>
+#include <Wire.h>
+#include <DualRoleConnectionExerciser.h>
+#include <USBCExerciser.h>
+#include <HMDExerciser.h>
+#include <HDMIExerciser.h>
+#include <DTMF.h>
+#include <ServoShield.h>
+#include <Model_3201.h>
+
+#define DBGPRINT(A)    if (dbg) {Serial.print(A);}
+#define DBGPRINTLN(A)  if (dbg) {Serial.println(A);}
+
+byte VERSION = 1;
+byte shield = 0;
+bool hmdShield = false;
+
+void AddSerCommand(const char* cmd, void (*function)(SerialCommand*));
+void AddSerDefaultHandler(void (*function)(SerialCommand*));
+
+SerialCommand sCmd(Serial);
+SerialCommand s1Cmd(Serial1);
+Timer timer;
+LiquidCrystal *lcd;
+bool dbg = false;
+uint8_t Data[10];
+uint32_t cmdDelay = 0; // Seconds
+uint32_t disconnectTimeout = 0; // miliseconds
+float (*ReadVoltage)() = NULL;
+float (*ReadCurrent)() = NULL;
+void (*SetPort)(byte) = NULL;
+char (*GetPort)() = NULL;
+void (*ShieldLoop)() = NULL;
+void (*SuperSpeed)(bool) = NULL;
+extern bool gHmdPresenceTrigger;
+
+void setup() {
+  Serial1.begin(115200);
+  
+  //
+  // PORTL[7:4] and PORTB[6] are used to identify the shield.
+  // Pull-ups are enable to detect the case where
+  // no shield is attached.
+  //
+
+  // HMD Shield and DTMF share the same pins and shouldn't be used together
+  // Check if HMD Shield is present and run the appropriate setup
+
+  // Enable pullups on PORTB[6] and PORTL[7:4]
+  PORTB = 0x40;
+  PORTL = 0xF0;
+
+  //
+  // Look for stacked shields -- but not on the 3201
+  //
+  if ( (PINL >> 4) != MODEL_3201 )
+  {     
+      Serial.begin(9600);
+      shield = (PINB >> 6) & 1;
+      
+      DBGPRINT( F("// Shield type: ") );
+      DBGPRINTLN( shield );
+      if(!shield)
+      {
+        HMD_setup();
+        hmdShield = true;
+      }
+      else
+      {
+        DTMF_setup();
+      }
+  }
+  else
+  {
+      //
+      // The 3201 communicates at 115200 to handle programming the
+      // FPGA image into flash.
+      //
+      Serial.begin(115200);
+  }
+
+  shield = PINL >> 4;
+
+  DBGPRINT( F("// Shield type: ") );
+  DBGPRINTLN( shield );
+  switch (shield)
+  {
+    case DRCE_SHIELD:
+      DRCE_setup();
+      break;
+    case USBC_SHIELD:
+      USBC_setup();
+      HDMI_setup();
+      break;
+    case MODEL_3201:
+      DTMF_setup();
+      USBC_setup();
+      Model_3201_setup();
+      break;
+  }
+
+  AddSerCommand("debug", SetDebugCB);
+  AddSerCommand("version", GetVersionCB);
+  AddSerCommand("put", PutDataCB);
+  AddSerCommand("get", GetDataCB);
+  AddSerDefaultHandler(Usage);
+}
+
+void AddSerCommand( const char* cmd, void (*function)(SerialCommand*))
+{
+  sCmd.addCommand(cmd, function);
+  s1Cmd.addCommand(cmd, function);
+}
+
+void AddSerDefaultHandler(void (*function)(SerialCommand*))
+{
+  sCmd.addDefaultHandler(function);
+  s1Cmd.addDefaultHandler(function);
+}
+
+void loop() {
+  sCmd.readSerial();
+  s1Cmd.readSerial();
+  timer.update();
+  if ( ShieldLoop )
+  {
+    ShieldLoop();
+  }
+  if(gHmdPresenceTrigger)
+  {
+    HMD_PulseLed();
+    delay(20);
+    gHmdPresenceTrigger = false;
+  }
+  if(hmdShield)
+  {
+    SERVO_Loop();
+  }
+}
+
+void Usage(SerialCommand*)
+{
+  DBGPRINTLN(F("debug [options]"));
+  DBGPRINTLN(F("    on, enable debug output"));
+  DBGPRINTLN(F("    off (or other), disable debug output"));
+  DBGPRINTLN(F("version"));
+  DBGPRINTLN(F("    ABTT:<firmare version>:<shield type>"));
+  if(USBC_SHIELD)
+  {
+    USBC_Usage();
+    HDMI_Usage();
+    HMD_Usage();
+  }
+  if(DRCE_SHIELD)
+  {
+    DRCE_Usage();
+  }
+}
+
+void PutDataCB(SerialCommand* cmd)
+{
+  uint8_t index;
+  char *arg = cmd->next();
+
+  if ( arg )
+  {
+    index = atoi(arg);
+    arg = cmd->next();
+    if ( index <= 9 && arg )
+    {
+      Data[index] = atoi(arg);
+      cmd->GetHardwareSerial()->print( F("put ") );
+      cmd->GetHardwareSerial()->print( index );
+      cmd->GetHardwareSerial()->print( F(" ") );
+      cmd->GetHardwareSerial()->println( arg );
+    }
+  }
+}
+
+void GetDataCB(SerialCommand* cmd)
+{
+  char Buffer[3];
+  uint8_t index;
+  char *arg = cmd->next();
+
+  if ( arg )
+  {
+    index = atoi(arg);
+    if ( index <= 9 )
+    {
+      Buffer[0] = (Data[index] / 10) + '0';
+      Buffer[1] = (Data[index] % 10) + '0';
+      Buffer[2] = '\0';
+      cmd->GetHardwareSerial()->println(Buffer);
+    }
+  }
+}
+
+void GetVersionCB(SerialCommand* cmd)
+{
+  char Buffer[8];
+
+  sprintf_P( Buffer, PSTR("%02d%02d"), VERSION, shield );
+  cmd->GetHardwareSerial()->println( Buffer );
+}
+
+void SetDebugCB(SerialCommand* cmd)
+{
+  char *arg = cmd->next();
+
+  if (arg != NULL) {
+    if ( strcmp(arg, "on" ) == 0) {
+      dbg = true;
+      DBGPRINTLN(F("Turn on debug"));
+    }
+    if ( strcmp(arg, "off" ) == 0) {
+      DBGPRINTLN(F("Turn off debug"));
+      dbg = false;
+    }
+  }
+}
diff --git a/usb/tools/ConnectionExerciser/libraries/DTMF/DTMF.cpp b/usb/tools/ConnectionExerciser/libraries/DTMF/DTMF.cpp
new file mode 100644
index 0000000..197978f
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/DTMF/DTMF.cpp
@@ -0,0 +1,471 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#include <SoftwareSerial.h>
+#include <SerialCommand.h>
+#include <Event.h>
+#include <Timer.h>
+#include "DTMF.h"
+
+void AddSerCommand(const char* cmd, void(*function)(SerialCommand*));
+extern Timer timer;
+extern bool dbg;
+extern uint32_t cmdDelay;
+extern uint32_t disconnectTimeout;
+extern void Usage();
+
+#define DBGPRINT(A)    if (dbg) {Serial.print(A);}
+#define DBGPRINTLN(A)  if (dbg) {Serial.println(A);}
+
+#define kD0           22 // PA[0]
+#define kD1           23 // PA[1]
+#define kD2           24 // PA[2]
+#define kD3           25 // PA[3]
+#define kRDN          26 // PA[4]
+#define kIRQN         27 // PA[5]
+#define kRS0          28 // PA[6]
+#define kWRN          29 // PA[7]
+
+extern float (*ReadVoltage)();
+extern float (*ReadCurrent)();
+extern void (*SetPort)(byte);
+extern char (*GetPort)();
+extern void (*SuperSpeed)(bool);
+extern byte VERSION;
+extern byte shield;
+
+
+void DTMF_loop();
+void DTMF_SoftwareReset();
+byte DTMF_ReadStatusRegister();
+void DTMF_WriteControlRegister( byte value );
+void DTMF_TransmitTone( byte data );
+char DTMF_ReadData();
+void DTMF_GenerateTone(SerialCommand* cmd);
+byte DTMF_ToneToByte( char tone );
+void DTMF_BuildCommandString( char c );
+bool DTMF_HandleCommand( void );
+
+bool TxReady = false;
+char TxBuffer[8];
+byte TxIndex = 0;
+bool TxSendBuffer = false;
+bool TxLastCharacter = false;
+char RxBuffer[8];
+byte RxIndex = 0;
+extern uint8_t Data[10];
+
+void DTMF_setup()
+{
+    //
+    // Enable pull ups
+    //
+    PORTA = 0xD0;
+
+    //
+    // RS0 is pulled low on the DTMF board
+    //
+    if ( PINA & 0x40 == 0x40 )
+    {
+        //
+        // No DTMF board
+        //
+        DBGPRINTLN( F("No DTMF present") );
+        return;
+    }
+
+    AddSerCommand("tone", DTMF_GenerateTone);
+
+    //
+    // Configure outputs
+    //
+    DDRA = 0xD0;
+
+
+    // Initialize
+    DTMF_SoftwareReset();
+    DTMF_WriteControlRegister( 0x0D );
+    DTMF_WriteControlRegister( 0x00 );
+
+    timer.every( 5, DTMF_loop );
+    TxReady = true;
+}
+
+void DTMF_loop()
+{
+    byte status;
+
+    if ( digitalRead( kIRQN ) == 0 )
+    {
+        status = DTMF_ReadStatusRegister();
+        // DBGPRINT( F("// DTMF Status: ") );
+        // DBGPRINTLN( status );
+
+        if ((status & 0x02) == 0x02) {
+            TxReady = true;
+            if ( TxLastCharacter == true ) {
+                //
+                // Turn off tone output at end of last character
+                //
+                DTMF_WriteControlRegister( 0x04 );
+                TxLastCharacter = false;
+            }
+        }
+        if ((status & 0x04) == 0x04) {
+            DTMF_BuildCommandString( DTMF_ReadData() );
+        }
+    }
+    if ( TxSendBuffer && TxReady ) {
+        if ( TxIndex == 0 ) {
+            //
+            // Turn on tone output at start of first character
+            //
+            DTMF_WriteControlRegister( 0x05 );
+        }
+        DTMF_TransmitTone( TxBuffer[TxIndex] );
+        if ( TxBuffer[TxIndex] == '#' ) {
+            DBGPRINTLN( TxBuffer[TxIndex] );
+            TxLastCharacter = true;
+            TxSendBuffer = false;
+            TxIndex = 0;
+        } else {
+            DBGPRINT( TxBuffer[TxIndex] );
+            TxIndex++;
+        }
+    }
+}
+
+void DTMF_BuildCommandString( char c )
+{
+    RxBuffer[RxIndex] = c;
+    if ( c == '#' )
+    {
+        if ( DTMF_HandleCommand() )
+        {
+            delay(100);
+            TxIndex = 0;
+            TxSendBuffer = true;
+        }
+    } else {
+        RxIndex++;
+    }
+}
+
+bool DTMF_HandleCommand( void )
+{
+    bool SendData = false;
+    float f;
+    int i;
+    uint8_t id;
+    uint16_t data;
+
+    //
+    // A well formed command is terminated with a #
+    //
+    if ( RxIndex >= sizeof( RxBuffer) || RxBuffer[RxIndex] != '#' )
+    {
+        goto Exit;
+    }
+
+    //
+    // Just a # was recieved
+    //
+    if ( RxIndex == 0 )
+    {
+        TxBuffer[0] = RxBuffer[0];
+        SendData = true;
+        goto Exit;
+    }
+
+    //
+    // Handle single character commands
+    //
+    if ( RxIndex == 1 )
+    {
+        switch (RxBuffer[0])
+        {
+            case '0':
+            case '1':
+            case '2':
+            case '3':
+            case '4':
+                if ( SetPort )
+                {
+                    SetPort( RxBuffer[0] - '0' );
+                    TxBuffer[0] = RxBuffer[0];
+                    TxBuffer[1] = RxBuffer[1];
+                    if ( RxBuffer[0] == '0' )
+                    {
+                        DBGPRINTLN( F("// Turn off ports") );
+                    } else {
+                        DBGPRINT( F("// Set port " ) );
+                        DBGPRINTLN( RxBuffer[0] );
+                    }
+                    SendData = true;
+                }
+                break;
+            case '5':
+            case '6':
+                if ( SuperSpeed )
+                {
+                    if ( RxBuffer[0] == '5' )
+                    {
+                        SuperSpeed(false);
+                    } else {
+                        SuperSpeed(true);
+                    }
+                    TxBuffer[0] = RxBuffer[0];
+                    TxBuffer[1] = RxBuffer[1];
+                    SendData = true;
+                }
+                break;
+            case 'A':
+                if ( GetPort )
+                {
+                    TxBuffer[0] = GetPort();
+                    TxBuffer[1] = RxBuffer[1];
+                    DBGPRINT( F("// Get Port : ") );
+                    DBGPRINTLN( TxBuffer[0] );
+                    SendData = true;
+                }
+                break;
+            case 'B':
+                if ( ReadVoltage )
+                {
+                    f = ReadVoltage();
+                    i = (int)f;
+                    sprintf_P( TxBuffer, PSTR("%02d%02d#"), i, abs((int)((f - (float)i) * 100.00)) );
+                    DBGPRINT( F("// Volts: "));
+                    DBGPRINTLN( TxBuffer );
+                    SendData = true;
+                }
+                break;
+            case 'C':
+                if ( ReadCurrent )
+                {
+                    f = ReadCurrent();
+                    i = (int)f;
+                    if ( f < 0.0 )
+                        sprintf_P( TxBuffer, PSTR("1%d%02d#"), -1 * i, abs((int)((f - (float)i) * 100.00)) );
+                    else
+                        sprintf_P( TxBuffer, PSTR("%02d%02d#"), i, abs((int)((f - (float)i) * 100.00)) );
+                    DBGPRINT( F("// Current: "));
+                    DBGPRINTLN( TxBuffer );
+                    SendData = true;
+                }
+                break;
+            case 'D':
+                sprintf_P( TxBuffer, PSTR("%02d%02d#"), VERSION, shield );
+                DBGPRINT( F("// Version : ") );
+                DBGPRINTLN( TxBuffer );
+                SendData = true;
+                break;
+        }
+        goto Exit;
+    }
+
+    if ( RxBuffer[1] == '*' )
+    {
+        switch ( RxBuffer[0] )
+        {
+            case '0':
+                id = RxBuffer[2] - '0';
+                data = (RxBuffer[3] - '0') * 10 + (RxBuffer[4] - '0');
+                if ( id <= 9 )
+                {
+                    DBGPRINT( F("// Put Data:") );
+                    DBGPRINT( id );
+                    DBGPRINT( F(",") );
+                    DBGPRINTLN( data );
+                    Data[id] = data;
+                    for ( TxIndex = 0; TxIndex <= RxIndex; TxIndex++ )
+                    {
+                        TxBuffer[TxIndex] = RxBuffer[TxIndex];
+                    }
+                    SendData = true;
+                }
+                break;
+            case '1':
+                id = RxBuffer[2] - '0';
+                if ( id <= 9 )
+                {
+                    DBGPRINT( F("// Get data:") );
+                    DBGPRINT( id );
+                    DBGPRINT( F(",") );
+                    DBGPRINTLN( Data[id] );
+                    TxBuffer[0] = (Data[id] / 10) + '0';
+                    TxBuffer[1] = (Data[id] % 10) + '0';
+                    TxBuffer[2] = '#';
+                    SendData = true;
+                }
+                break;
+            case '2':
+                // Command Delay in seconds.  Must transmit two digits: 1 second = 01
+                cmdDelay = (RxBuffer[2] - '0') * 10 + \
+                           (RxBuffer[3] - '0');
+                cmdDelay *= 1000;
+
+                DBGPRINT(F("// Command Delay: "));
+                DBGPRINTLN(cmdDelay);
+                for (TxIndex = 0; TxIndex <= RxIndex; TxIndex++)
+                {
+                    TxBuffer[TxIndex] = RxBuffer[TxIndex];
+                }
+                SendData = true;
+                break;
+            case '3':
+                // Disconnect Timeout in miliseconds.  Must transmit 4 digits: 1 ms = 0001
+                disconnectTimeout = (RxBuffer[2] - '0') * 1000 + \
+                                    (RxBuffer[3] - '0') * 100 + \
+                                    (RxBuffer[4] - '0') * 10 + \
+                                    (RxBuffer[5] - '0');
+
+                DBGPRINT(F("// Disconnect Timeout: "));
+                DBGPRINTLN(disconnectTimeout);
+                for (TxIndex = 0; TxIndex <= RxIndex; TxIndex++)
+                {
+                    TxBuffer[TxIndex] = RxBuffer[TxIndex];
+                }
+                SendData = true;
+                break;
+        }
+        goto Exit;
+    }
+
+Exit:
+    RxIndex = 0;
+    for ( i = 0; i < sizeof(RxBuffer); i++ )
+        RxBuffer[i] = 0;
+    return SendData;
+}
+
+
+void DTMF_SoftwareReset()
+{
+    DTMF_ReadStatusRegister();
+    DTMF_WriteControlRegister( 0x00 );
+    DTMF_WriteControlRegister( 0x00 );
+    DTMF_WriteControlRegister( 0x08 );
+    DTMF_WriteControlRegister( 0x00 );
+    DTMF_ReadStatusRegister();
+}
+
+byte DTMF_ReadStatusRegister()
+{
+    byte status;
+
+    noInterrupts();
+    DDRA  = 0xD0;           // configure the port
+    PORTA = 0xD0;           // {WRN, RSO, 0, RDN}
+    PORTA = 0xC0;           // {WRN, RS0, 0, RDN#}
+    PORTA = 0xC0;           // {WRN, RS0, 0, RDN#}
+    PORTA = 0xC0;           // {WRN, RS0, 0, RDN#}
+    status = PINA & 0x0F;   // Read the status
+    PORTA = 0xD0;           // {WRN, RS0, 0, RDN}
+    interrupts();
+    return status;
+}
+
+void DTMF_WriteControlRegister( byte data )
+{
+    noInterrupts();
+    data  = data & 0x0F;
+    DDRA  = 0xDF;           // configure the port
+    PORTA = 0xD0 | data;    // { WRN, RS0, 0, RDN}
+    PORTA = 0x50 | data;    // {WRN#, RS0, 0, RDN}
+    PORTA = 0x50 | data;    // {WRN#, RS0, 0, RDN}
+    PORTA = 0x50 | data;    // {WRN#, RS0, 0, RDN}
+    PORTA = 0xD0 | data;    // { WRN, RS0, 0, RDN}
+    PORTA = 0xD0;
+    DDRA  = 0xD0;
+    interrupts();
+}
+
+void DTMF_TransmitTone( byte tone )
+{
+    byte data = DTMF_ToneToByte( tone );
+    noInterrupts();
+    data = data & 0x0F;
+    DDRA  = 0xDF;           // configure the port
+    PORTA = 0x90 | data;    // { WRN, RS0#, 0, RDN}
+    PORTA = 0x10 | data;    // {WRN#, RS0#, 0, RDN}
+    PORTA = 0x10 | data;    // {WRN#, RS0#, 0, RDN}
+    PORTA = 0x10 | data;    // {WRN#, RS0#, 0, RDN}
+    PORTA = 0x90 | data;    // { WRN, RS0#, 0, RDN}
+    PORTA = 0xD0 | data;    // { WRN, RS0,  0, RDN}
+    PORTA = 0xD0;
+    DDRA  = 0xD0;
+    TxReady = false;
+    interrupts();
+}
+
+char DTMF_ReadData()
+{
+    byte data;
+    byte value[16] = { 'D', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '*', '#', 'A', 'B', 'C' };
+
+    noInterrupts();
+    DDRA  = 0xD0;           // configure the port
+    PORTA = 0x90;           // {WRN, RS0#, 0, RDN}
+    PORTA = 0x80;           // {WRN, RS0#, 0, RDN#}
+    PORTA = 0x80;           // {WRN, RS0#, 0, RDN#}
+    PORTA = 0x80;           // {WRN, RS0#, 0, RDN#}
+    data  = PINA & 0x0F;    // Read the data
+    PORTA = 0x80;           // {WRN, RS0#, 0, RDN}
+    PORTA = 0xD0;           // {WRN, RS0, 0, RDN}
+    DDRA  = 0xD0;
+    interrupts();
+
+    return value[data];
+}
+
+void DTMF_GenerateTone(SerialCommand* cmd)
+{
+    char *arg = cmd->next();
+    byte index = 0;
+
+    DBGPRINTLN( F("Generate Tone" ) );
+
+    if ( arg == NULL ) {
+        DBGPRINTLN( F("Tone not specified") );
+    } else {
+        while ( arg[index] ) {
+            TxBuffer[TxIndex] = arg[index];
+            TxIndex++;
+            if ( arg[index] == '#' ) {
+                DBGPRINTLN( F("EOT character found") );
+                TxIndex = 0;
+                TxSendBuffer = true;
+                return;
+            }
+            index++;
+        }
+    }
+}
+
+byte DTMF_ToneToByte( char tone )
+{
+    byte retVal = 0xFF;
+
+    switch (tone)
+    {
+        case '0': retVal = 0x0A; break;
+        case '1': retVal = 0x01; break;
+        case '2': retVal = 0x02; break;
+        case '3': retVal = 0x03; break;
+        case '4': retVal = 0x04; break;
+        case '5': retVal = 0x05; break;
+        case '6': retVal = 0x06; break;
+        case '7': retVal = 0x07; break;
+        case '8': retVal = 0x08; break;
+        case '9': retVal = 0x09; break;
+        case '*': retVal = 0x0B; break;
+        case '#': retVal = 0x0C; break;
+        case 'a': retVal = 0x0D; break;
+        case 'b': retVal = 0x0E; break;
+        case 'c': retVal = 0x0F; break;
+        case 'd': retVal = 0x00; break;
+    }
+    return retVal;
+}
+
diff --git a/usb/tools/ConnectionExerciser/libraries/DTMF/DTMF.h b/usb/tools/ConnectionExerciser/libraries/DTMF/DTMF.h
new file mode 100644
index 0000000..874090e
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/DTMF/DTMF.h
@@ -0,0 +1,9 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#ifndef DTMF_H
+#define DTMF_H
+
+void DTMF_setup();
+
+#endif
diff --git a/usb/tools/ConnectionExerciser/libraries/DTMF/keywords.txt b/usb/tools/ConnectionExerciser/libraries/DTMF/keywords.txt
new file mode 100644
index 0000000..594986d
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/DTMF/keywords.txt
@@ -0,0 +1,23 @@
+#######################################
+# Syntax Coloring Map For DualRoleConnectionExercisor
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+DTMF_setup	KEYWORD2
+
+#######################################
+# Instances (KEYWORD2)
+#######################################
+
+
+#######################################
+# Constants (LITERAL1)
+#######################################
diff --git a/usb/tools/ConnectionExerciser/libraries/DualRoleConnectionExerciser/DualRoleConnectionExerciser.cpp b/usb/tools/ConnectionExerciser/libraries/DualRoleConnectionExerciser/DualRoleConnectionExerciser.cpp
new file mode 100644
index 0000000..fcf0b8c
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/DualRoleConnectionExerciser/DualRoleConnectionExerciser.cpp
@@ -0,0 +1,371 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#include <SoftwareSerial.h>
+#include <SerialCommand.h>
+#include <LiquidCrystal.h>
+#include <Event.h>
+#include <Timer.h>
+#include "DualRoleConnectionExerciser.h"
+
+void AddSerCommand(const char* cmd, void(*function)(SerialCommand*));
+extern Timer timer;
+extern LiquidCrystal *lcd;
+extern bool dbg;
+
+#define DBGPRINT(A)    if (dbg) {Serial.print(A);}
+#define DBGPRINTLN(A)  if (dbg) {Serial.println(A);}
+
+#define kIdEnable     5 // DUT USB port kIdEnable select (0 = floating, 1 = ground)
+#define kUsbEnableN   6 // USB mux output enable (0 = connect, 1 = disconnect)
+#define kSELA         7 // USB mux/PMIC select line A
+#define kSELB         8 // USB mux/PMIC select line B
+#define kPowerEnable  9 // Output enable for VBUS mux
+#define kGroundIdPin 10 // Switch to ground the ID pin (0 = floating, 1 = ground)
+#define USBMUX(A) PORTH= (PINH & 0xCF) | (A) << 4
+
+#define kLCDROWS      2 // Number of rows on the LCD module
+#define kLCDCOLS      8 // Number of columns on the LCD module
+#define kRS          41 // LCD module register select line (0 = command, 1 = data input)
+#define kRW          38 // Read not write (0 = write, 1 = read)
+#define kEN          39 // LCD module enable
+
+#define kVREF 2.56       // Internal reference voltage
+#define kCURROFFSET 0.05 // Voltage offset of differential ADC for current measurement
+#define kVOLTAGESCALEFACTOR 5.5 // see description of PrintSummary()
+#define kCURRENTSCALEFACTOR 10 // see description of PrintSummary()
+#define kADCSCALAR (kVREF * ((10.0 + 5.0)/5.0) / 1024.0) // see description of PrintSummary()
+#define kDIFFADCSCALAR (kVREF * kCURRENTSCALEFACTOR / 512.0) // see description of PrintSummary()
+
+//
+// Function pointers used by DTMF routines.  These are populated
+// in the setup routine.
+//
+extern float (*ReadVoltage)();
+extern float (*ReadCurrent)();
+extern void (*SetPort)(byte);
+extern char (*GetPort)();
+extern void (*ShieldLoop)();
+
+void DRCE_PrintVoltsCB(SerialCommand* cmd);
+void DRCE_PrintAmpsCB(SerialCommand* cmd);
+void DRCE_ChangePortCB(SerialCommand* cmd);
+void DRCE_SetPort(byte port);
+char DRCE_GetPort(void);
+void DRCE_UpdateDisplayCB(void);
+void DRCE_Usage();
+float DRCE_ReadVoltage(void);
+float DRCE_ReadCurrent(void);
+void DRCE_DisconnectUSB(void);
+void DRCE_CyclePorts(void);
+void DRCE_UpdateVoltageString(float volts, char *strVolts );
+void DRCE_UpdateCurrentString(float amps, char *strAmps );
+void DRCE_Loop(void);
+
+uint16_t *VoltageReadings;
+int16_t *CurrentReadings;
+uint8_t  ReadingsIndex;
+
+
+void DRCE_setup()
+{
+    //
+    // Hook up function pointers
+    //
+    ReadVoltage = &DRCE_ReadVoltage;
+    ReadCurrent = &DRCE_ReadCurrent;
+    SetPort = &DRCE_SetPort;
+    GetPort = &DRCE_GetPort;
+    ShieldLoop = &DRCE_Loop;
+
+    VoltageReadings = new uint16_t[128];
+    CurrentReadings = new int16_t[128];
+    for ( ReadingsIndex = 0; ReadingsIndex < 128; ReadingsIndex++ )
+    {
+        VoltageReadings[ReadingsIndex] = 0;
+        CurrentReadings[ReadingsIndex] = 0;
+    }
+    ReadingsIndex = 0;
+
+    //
+    // Register serial command callbacks
+    //
+    AddSerCommand("volts", DRCE_PrintVoltsCB);
+    AddSerCommand("amps", DRCE_PrintAmpsCB);
+    AddSerCommand("port", DRCE_ChangePortCB);
+
+    // 1602A LCD module
+    pinMode(kRS, OUTPUT);
+    pinMode(kRW, OUTPUT);
+    pinMode(kEN, OUTPUT);
+
+    // ADC
+    pinMode(A0, INPUT);
+    pinMode(A1, INPUT);
+    pinMode(A2, INPUT);
+
+    // PMIC and USB MUX
+    pinMode(kIdEnable, OUTPUT);
+    pinMode(kGroundIdPin, OUTPUT);
+    pinMode(kUsbEnableN, OUTPUT);
+    pinMode(kSELA, OUTPUT);
+    pinMode(kSELB, OUTPUT);
+    pinMode(kPowerEnable, OUTPUT);
+
+    //
+    // Initialize pins
+    //
+    digitalWrite(kRS, LOW);
+    digitalWrite(kRW, LOW);
+    digitalWrite(kEN, LOW);
+    digitalWrite(A3, LOW);
+    digitalWrite(A4, HIGH);
+    digitalWrite(kIdEnable, LOW);   // Initialize ID pin floating, DUT is peripheral.
+    digitalWrite(kGroundIdPin, LOW );
+    digitalWrite(kUsbEnableN, HIGH);  // Initialize USB mux disabled, no ports connected.
+    digitalWrite(kSELA, LOW);
+    digitalWrite(kSELB, LOW);
+    digitalWrite(kPowerEnable, LOW);
+    analogReference(INTERNAL2V56);
+
+    lcd = new LiquidCrystal(kRS, kRW, kEN, 37, 36, 35, 34, 33, 32, 31, 30);
+    lcd->begin(kLCDCOLS, kLCDROWS);
+    timer.every( 500, DRCE_UpdateDisplayCB);
+
+    if ( PINL & 0x01 )
+        timer.every( 3000, DRCE_CyclePorts);
+}
+
+void DRCE_Loop()
+{
+    VoltageReadings[ReadingsIndex] = analogRead(A2);
+    CurrentReadings[ReadingsIndex] = analogRead(A0) - analogRead(A1);
+    ReadingsIndex = (ReadingsIndex + 1) % 128;
+}
+
+void DRCE_PrintVoltsCB(SerialCommand* cmd)
+{
+    char Buffer[8];
+    float f;
+    int i;
+
+    f = DRCE_ReadVoltage();
+    i = (int)f;
+
+    sprintf_P(Buffer, PSTR("%02d%02d"), i, abs((int)((f - (float)i) * 100.00)));
+    cmd->GetHardwareSerial()->println(Buffer);
+}
+
+void DRCE_PrintAmpsCB(SerialCommand* cmd)
+{
+    char Buffer[8];
+    float f;
+    int i;
+
+    f = DRCE_ReadCurrent();
+    i = (int)f;
+    if (f < 0.0)
+        sprintf_P(Buffer, PSTR("1%d%02d"), -1 * i, abs((int)((f - (float)i) * 100.00)));
+    else
+        sprintf_P(Buffer, PSTR("%02d%02d"), i, abs((int)((f - (float)i) * 100.00)));
+    cmd->GetHardwareSerial()->println(Buffer);
+}
+
+void DRCE_CyclePorts()
+{
+    byte mux[4] = {0x3, 0x2, 0x0, 0x1};
+
+    DRCE_DisconnectUSB();
+    delay(100);
+
+
+    switch ((PINH & 0x30) >> 4)
+    {
+        case 0x3: USBMUX( 0x2 ); break;
+        case 0x2: USBMUX( 0x0 ); break;
+        case 0x0: USBMUX( 0x1 ); break;
+        case 0x1: USBMUX( 0x3 ); break;
+    }
+    digitalWrite(kPowerEnable, HIGH );
+    delay(5);
+
+    if ( ((PINH & 0x30) >> 4) >= 0x2 ) {
+        digitalWrite( kGroundIdPin, HIGH );
+    } else {
+        digitalWrite( kGroundIdPin, LOW );
+    }
+    digitalWrite(kIdEnable, LOW);
+    digitalWrite(kUsbEnableN, LOW);
+
+    switch ((PINH & 0x30) >> 4)
+    {
+        case 0x0: DBGPRINTLN( F("PORT 3") ); break;
+        case 0x1: DBGPRINTLN( F("PORT 4") ); break;
+        case 0x2: DBGPRINTLN( F("PORT 2") ); break;
+        case 0x3: DBGPRINTLN( F("PORT 1") ); break;
+    }
+}
+
+void DRCE_ChangePortCB(SerialCommand* cmd)
+{
+    char *arg = cmd->next();
+
+    if ( arg == NULL ) {
+        cmd->GetHardwareSerial()->println(DRCE_GetPort());
+        return;
+    }
+
+    cmd->GetHardwareSerial()->print(F("port "));
+    cmd->GetHardwareSerial()->println(atoi(arg));
+
+    DRCE_SetPort( atoi(arg) );
+}
+
+void DRCE_SetPort( byte port )
+{
+    byte mux[4] = {0x3, 0x2, 0x0, 0x1};
+
+    DRCE_DisconnectUSB();
+    delay(100);
+    
+    if ( port >= 1 && port <= 4 ) {
+        digitalWrite(kPowerEnable, HIGH);
+    
+        DBGPRINT(F("DRCE_SetPort() : "));
+        DBGPRINTLN(port);
+      
+        USBMUX( mux[port-1] );
+
+        delay(5);
+        if ( port == 1 || port == 2 ) {
+            digitalWrite( kGroundIdPin, HIGH );
+        } else {
+            digitalWrite( kGroundIdPin, LOW );
+        }
+        digitalWrite(kIdEnable, LOW);
+        digitalWrite(kUsbEnableN, LOW);
+    } else {
+        DBGPRINTLN(F("USB ports are all off"));
+    }
+    
+}
+
+char DRCE_GetPort()
+{
+    char table[4] = {'3', '4', '2', '1'};
+
+    DBGPRINTLN( F("DRCE_GetPort()") );
+
+    if ( digitalRead( kUsbEnableN ) == HIGH ) {
+        return '0';
+    }
+
+    return table[ (PINH & 0x30) >> 4 ];
+}
+
+void DRCE_Usage()
+{
+    DBGPRINTLN(F("volts"));
+    DBGPRINTLN(F("    shows voltage. no parameters."));
+    DBGPRINTLN(F("amps"));
+    DBGPRINTLN(F("    shows amperage. no parameters."));
+    DBGPRINTLN(F("port [options]"));
+    DBGPRINTLN(F("    <empty>, shows current USB port."));
+    DBGPRINTLN(F("    1, connects to USB port 1"));
+    DBGPRINTLN(F("    2, connects to USB port 2"));
+    DBGPRINTLN(F("    3, connects to USB port 3"));
+    DBGPRINTLN(F("    0 (or other), disconnect all ports"));
+}
+
+void DRCE_UpdateDisplayCB()
+{
+    char tmpStr[10];
+  
+    lcd->home();
+    DRCE_UpdateVoltageString( DRCE_ReadVoltage(), tmpStr );
+    lcd->print( tmpStr );
+    lcd->setCursor(1,1);
+    DRCE_UpdateCurrentString( DRCE_ReadCurrent(), tmpStr );
+    lcd->print( tmpStr );
+}
+
+
+float DRCE_ReadVoltage()
+{
+    uint32_t v;
+    uint8_t i;
+    float volts;
+
+    v = 0;
+    for ( i = 0; i < 128; i++ )
+    {
+        v += VoltageReadings[i];
+    }
+    v = v / 128;
+  
+    // v = analogRead(A2);
+
+    volts = v * 2.56 / 1024.0 * (100.0 + 49.9)/49.9;
+    //
+    // The circuit will cause VBUS to be less than 0.5 volts when nothing is connected.
+    // Return zero when this condition is detected.
+    //
+    if ( volts < 0.75 )
+        volts = 0.0;
+    return volts;
+}
+
+float DRCE_ReadCurrent()
+{
+    int32_t i;
+    uint8_t index;
+    float amps;
+  
+    i = 0;
+    for ( index = 0; index < 128; index++ )
+    {
+        i += CurrentReadings[index];
+    }
+    i = i / 128;
+
+    // i = analogRead(A0) - analogRead(A1);
+    amps =  -1.0 * i * 2.56 / 1024.0;
+
+    return amps;
+}
+
+void DRCE_DisconnectUSB()
+{
+    digitalWrite(kUsbEnableN, HIGH);
+    digitalWrite(kIdEnable, LOW);
+    digitalWrite(kGroundIdPin, LOW );
+    delay(5);
+    digitalWrite(kPowerEnable, LOW);
+    delay(50);
+}
+
+void DRCE_UpdateVoltageString(float volts, char *strVolts)
+{
+    int i, f;
+  
+    i = (int)volts;
+    f = abs((int)((volts - (float)i) * 100.00));  
+    sprintf_P( strVolts, PSTR("  %d.%02d V"), i, f );
+}
+
+void DRCE_UpdateCurrentString(float amps, char *strAmps)
+{
+    int i, f;
+
+    i = (int)amps;
+    f = abs((int)((amps - (float)i) * 1000.00)); 
+    if ( amps < 0.0 )
+        *strAmps = '-';
+    else if ( amps > 0.0 )
+        *strAmps = '+';
+    else
+        *strAmps = ' ';
+
+    sprintf_P( strAmps+1, PSTR("%d.%03dA"), i, f );
+}
+
diff --git a/usb/tools/ConnectionExerciser/libraries/DualRoleConnectionExerciser/DualRoleConnectionExerciser.h b/usb/tools/ConnectionExerciser/libraries/DualRoleConnectionExerciser/DualRoleConnectionExerciser.h
new file mode 100644
index 0000000..7c00a03
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/DualRoleConnectionExerciser/DualRoleConnectionExerciser.h
@@ -0,0 +1,12 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#ifndef DualRoleConnectionExerciser_h
+#define DualRoleConnectionExerciser_h
+
+#define DRCE_SHIELD 0x0
+
+void DRCE_setup();
+void DRCE_Usage();
+
+#endif
diff --git a/usb/tools/ConnectionExerciser/libraries/DualRoleConnectionExerciser/keywords.txt b/usb/tools/ConnectionExerciser/libraries/DualRoleConnectionExerciser/keywords.txt
new file mode 100644
index 0000000..833f56d
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/DualRoleConnectionExerciser/keywords.txt
@@ -0,0 +1,24 @@
+#######################################
+# Syntax Coloring Map For DualRoleConnectionExercisor
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+DRCE_setup	KEYWORD2
+
+#######################################
+# Instances (KEYWORD2)
+#######################################
+
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+DRCE_SHIELD	LITERAL1
diff --git a/usb/tools/ConnectionExerciser/libraries/HDMIExerciser/HDMIExerciser.cpp b/usb/tools/ConnectionExerciser/libraries/HDMIExerciser/HDMIExerciser.cpp
new file mode 100644
index 0000000..f45af4e
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/HDMIExerciser/HDMIExerciser.cpp
@@ -0,0 +1,92 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#include <SerialCommand.h>
+#include "HDMIExerciser.h"
+
+#define DBGPRINT(A)    if (dbg) {Serial.print(A);}
+#define DBGPRINTLN(A)  if (dbg) {Serial.println(A);}
+
+// Pins
+#define HdmiEnable 52
+#define HdmiSelect 53
+#define HdmiHpd 40
+
+void HDMI_SetPortCB(SerialCommand* cmd);
+void HDMI_GetPortCB(SerialCommand* cmd);
+void HDMI_Usage();
+void AddSerCommand(const char* cmd, void(*function)(SerialCommand*));
+extern bool dbg;
+
+unsigned int gPort;
+
+void HDMI_setup()
+{
+	DBGPRINT(F("// HDMI Exerciser Initialized "));
+	AddSerCommand("sethdmiport", HDMI_SetPortCB);
+	AddSerCommand("gethdmiport", HDMI_GetPortCB);
+
+	pinMode(HdmiEnable, OUTPUT);
+	pinMode(HdmiSelect, OUTPUT);
+    pinMode(HdmiHpd, INPUT);
+
+    // Initialize with Port 2 (J3)
+	digitalWrite(HdmiEnable, HIGH);
+	digitalWrite(HdmiSelect, HIGH);
+    gPort = 2;
+}
+
+void HDMI_SetPortCB(SerialCommand* cmd)
+{
+	DBGPRINTLN(F("// SetHdmiPort "));
+	char *arg = cmd->next();
+
+	if (arg == NULL)
+	{
+		return;
+	}
+
+	gPort = atoi(arg);
+
+	switch (gPort)
+	{
+    case 0:
+        digitalWrite(HdmiEnable, LOW);
+        DBGPRINTLN(F("// Hdmi Disconnected "));
+        break;
+	case 1:
+        digitalWrite(HdmiEnable, HIGH);
+		digitalWrite(HdmiSelect, LOW);
+		DBGPRINTLN(F("// J2 Connected "));
+		break;
+	case 2:
+        digitalWrite(HdmiEnable, HIGH);
+		digitalWrite(HdmiSelect, HIGH);
+		DBGPRINTLN(F("// J3 Connected "));
+		break;
+	default:
+		return;
+	}
+
+    char port = '0' + gPort;
+	cmd->GetHardwareSerial()->println(port);
+}
+
+void HDMI_GetPortCB(SerialCommand* cmd)
+{
+	DBGPRINTLN(F("// GetHdmiPort "));
+	//DBGPRINTLN(F('0' + gPort));
+    char port = '0' + gPort;
+
+	cmd->GetHardwareSerial()->println(port);
+}
+
+void HDMI_Usage()
+{
+    DBGPRINTLN(F("sethdmiport [options]"));
+    DBGPRINTLN(F("    1, connects to HDMI port J2"));
+    DBGPRINTLN(F("    2, connects to HDMI port J3"));
+    DBGPRINTLN(F("    0, disconnect all ports"));
+    DBGPRINTLN(F("gethdmiport"));
+    DBGPRINTLN(F("    returns the currently connected HDMI port"));
+}
diff --git a/usb/tools/ConnectionExerciser/libraries/HDMIExerciser/HDMIExerciser.h b/usb/tools/ConnectionExerciser/libraries/HDMIExerciser/HDMIExerciser.h
new file mode 100644
index 0000000..5d43714
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/HDMIExerciser/HDMIExerciser.h
@@ -0,0 +1,10 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#ifndef HDMIExerciser_H
+#define HDMIExerciser_H
+
+void HDMI_setup();
+void HDMI_Usage();
+
+#endif
diff --git a/usb/tools/ConnectionExerciser/libraries/HDMIExerciser/keywords.txt b/usb/tools/ConnectionExerciser/libraries/HDMIExerciser/keywords.txt
new file mode 100644
index 0000000..e6f4cc6
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/HDMIExerciser/keywords.txt
@@ -0,0 +1,24 @@
+#######################################
+# Syntax Coloring Map For DualRoleConnectionExercisor
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+HDMI_setup	KEYWORD2
+
+#######################################
+# Instances (KEYWORD2)
+#######################################
+
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+HDMI_SHIELD	LITERAL1
diff --git a/usb/tools/ConnectionExerciser/libraries/HmdExerciser/HMDExerciser.cpp b/usb/tools/ConnectionExerciser/libraries/HmdExerciser/HMDExerciser.cpp
new file mode 100644
index 0000000..4472d3a
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/HmdExerciser/HMDExerciser.cpp
@@ -0,0 +1,484 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#include <SerialCommand.h>
+#include "HMDExerciser.h"
+#include "Adafruit_TCS34725.h"
+#include "ServoShield.h"
+#include "limits.h"
+
+#define DBGPRINT(A)    if (dbg) {Serial.print(A);}
+#define DBGPRINTLN(A)  if (dbg) {Serial.println(A);}
+#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+
+// Delays
+#define RetryCount 4
+#define StartupDelay 200
+#define ReadingDelay 750
+
+// Audio
+#define PeakAmplitude (1024 / 2)
+#define MaxSampleCount ULONG_MAX
+
+//Pins
+#define PresenceSpoof 23
+#define ColorSensorInt 24
+#define PresenceIrSensorHmd1 2
+#define ColorSensorPower0Hmd1 22
+#define ColorSensorPower1Hmd1 25
+#define PresenceIrSensorHmd2 3
+#define ColorSensorPower0Hmd2 26
+#define ColorSensorPower1Hmd2 27
+
+
+struct _HmdPins
+{
+    int Hmd = 1;
+    int PresenceSpoofer = PresenceSpoof;
+    int PresenceIrSensor = PresenceIrSensorHmd1;
+    int ColorSensorPower0 = ColorSensorPower0Hmd1;
+    int ColorSensorPower1 = ColorSensorPower1Hmd1;
+    int ColorSensorInterrupt = ColorSensorInt;
+}HmdPins;
+
+
+Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_700MS, TCS34725_GAIN_60X);
+
+void HMD_SetHmdCB(SerialCommand* cmd);
+void HMD_GetDisplayBrightnessCB(SerialCommand* cmd);
+void HMD_GetDisplayColorCB(SerialCommand* cmd);
+void HMD_SetPresenceCB(SerialCommand* cmd);
+void HMD_GetVolumeRmsCB(SerialCommand* cmd);
+void HMD_GetVolumeAvgCB(SerialCommand* cmd);
+void HMD_GetVolumePeakCB(SerialCommand* cmd);
+void HMD_SetVolumeSampleCountCB(SerialCommand* cmd);
+bool HMD_SetHmd(int hmd);
+void HMD_PresenceOn();
+void HMD_PresenceOff();
+void HMD_IrIsr();
+void HMD_GetVolume();
+void HMD_Usage();
+bool HMD_UpdateColorSensorValues(int displayIndex);
+void AddSerCommand(const char* cmd, void(*function)(SerialCommand*));
+extern bool dbg;
+
+bool gHmdPresenceTrigger = false;
+unsigned long gSampleCount = 2048;
+uint64_t gVolumeAvg = 0, gVolumeRms = 0, gVolumeMax = 0;
+uint16_t gRed, gGreen, gBlue, gClear = 0;
+
+void HMD_setup()
+{
+    DBGPRINTLN(F("// HMD Exerciser Initialized "));
+    AddSerCommand("presence", HMD_SetPresenceCB);
+    AddSerCommand("brightness", HMD_GetDisplayBrightnessCB);
+    AddSerCommand("color", HMD_GetDisplayColorCB);
+    AddSerCommand("getvolumerms", HMD_GetVolumeRmsCB);
+    AddSerCommand("getvolumeavg", HMD_GetVolumeAvgCB);
+    AddSerCommand("getvolumepeak", HMD_GetVolumePeakCB);
+    AddSerCommand("svsc", HMD_SetVolumeSampleCountCB);
+    AddSerCommand("sethmd", HMD_SetHmdCB);
+
+    for (int i = 2; i > 0; i--)
+    {
+        HMD_SetHmd(i);
+        // Color Sensor
+        pinMode(HmdPins.ColorSensorPower0, OUTPUT);
+        pinMode(HmdPins.ColorSensorPower1, OUTPUT);
+        pinMode(HmdPins.ColorSensorInterrupt, INPUT);
+        digitalWrite(HmdPins.ColorSensorPower0, LOW);
+        digitalWrite(HmdPins.ColorSensorPower1, LOW);
+
+        // Presence Spoofing
+        pinMode(HmdPins.PresenceSpoofer, OUTPUT);
+        digitalWrite(HmdPins.PresenceSpoofer, HIGH);
+        pinMode(HmdPins.PresenceIrSensor, INPUT);
+    }
+    
+    // Audio
+    DIDR0 = 0x01; // Disable digital in for ADC 0
+    ADCSRA = 0xE7; // ADC Enabled, Start Conversion, Auto Trigger, Clear Interrupt Flag, Interrupt Disable, Prescaler 32 (38.4khz sample rate)
+    ADMUX = 0x40;
+
+    SERVO_setup();
+}
+
+void HMD_SetPresenceCB(SerialCommand* cmd)
+{
+    char buffer[2];
+    DBGPRINTLN(F("// SetPresence "));
+    char *arg = cmd->next();
+    int presence = 0;
+
+    if (arg == NULL)
+    {
+        return;
+    }
+
+    presence = atoi(arg);
+
+    switch (presence)
+    {
+    case 0:
+        HMD_PresenceOff();
+        DBGPRINTLN(F("// Presence Spoofer Off "));
+        break;
+    case 1:
+        HMD_PresenceOn();
+        DBGPRINTLN(F("// Presence Spoofer On "));
+        break;
+    default:
+        return;
+    }
+
+    sprintf_P(buffer, PSTR("%u"), presence);
+    cmd->GetHardwareSerial()->println(buffer);
+}
+
+void HMD_SetHmdCB(SerialCommand* cmd)
+{
+    char buffer[2];
+    DBGPRINTLN(F("// SetHmd "));
+    char *arg = cmd->next();
+    int hmd = 0;
+
+    if (arg == NULL)
+    {
+        return;
+    }
+
+    hmd = atoi(arg);
+    if(!HMD_SetHmd(hmd))
+    {
+        DBGPRINTLN(F("// Invalid HMD index. Valid values are 1 or 2 "));
+        cmd->GetHardwareSerial()->println('0');
+        return;
+    }
+
+    sprintf_P(buffer, PSTR("%u"), hmd);
+    cmd->GetHardwareSerial()->println(buffer);
+}
+
+bool HMD_SetHmd(int hmd)
+{
+    HMD_PresenceOff();
+    if (hmd == 1)
+    {
+        HmdPins.Hmd = 1;
+		HmdPins.PresenceIrSensor = PresenceIrSensorHmd1;
+		HmdPins.ColorSensorPower0 = ColorSensorPower0Hmd1;
+		HmdPins.ColorSensorPower1 = ColorSensorPower1Hmd1;
+    }
+    else if (hmd == 2)
+    {
+        HmdPins.Hmd = 2;
+		HmdPins.PresenceIrSensor = PresenceIrSensorHmd2;
+		HmdPins.ColorSensorPower0 = ColorSensorPower0Hmd2;
+		HmdPins.ColorSensorPower1 = ColorSensorPower1Hmd2;
+    }
+    else
+    {
+        return false;
+    }
+
+    return true;
+}
+
+void HMD_GetDisplayBrightnessCB(SerialCommand* cmd)
+{
+    char *arg = cmd->next();
+    char buffer[8];
+    int display;
+    DBGPRINTLN(F("// GetDisplayBrightness "));
+
+    if (arg == NULL)
+    {
+        display = 0;
+    }
+    else
+    {
+        display = atoi(arg);
+    }
+
+    display = atoi(arg);
+
+    DBGPRINT(F("// Display: "));
+    DBGPRINTLN(display);
+
+    if (HMD_UpdateColorSensorValues(display))
+    {
+        // Send gClear value response to serial
+        sprintf_P(buffer, PSTR("%7u"), gClear);
+        cmd->GetHardwareSerial()->println(buffer);
+    }
+}
+
+void HMD_GetDisplayColorCB(SerialCommand* cmd)
+{
+    char *arg = cmd->next();
+    char buffer[24];
+    int display;
+    DBGPRINTLN(F("// GetDisplayColor "));
+
+    if (arg == NULL)
+    {
+        display = 0;
+    }
+    else
+    {
+        display = atoi(arg);
+    }
+
+    if (HMD_UpdateColorSensorValues(display))
+    {
+        // Send RGB value response to serial
+        sprintf_P(buffer, PSTR("%7u %7u %7u"), gRed, gGreen, gBlue);
+        cmd->GetHardwareSerial()->println(buffer);
+    }
+}
+
+void HMD_GetVolumeRmsCB(SerialCommand* cmd)
+{
+    DBGPRINTLN(F("// GetVolumeRms "));
+    char buffer[5] = "";
+    
+    HMD_GetVolume();
+    
+    // Send volume response to serial
+    sprintf_P(buffer, PSTR("%3u"), gVolumeRms);
+    cmd->GetHardwareSerial()->println(buffer);
+}
+
+void HMD_GetVolumePeakCB(SerialCommand* cmd)
+{
+    DBGPRINTLN(F("// GetVolumePeak "));
+    char buffer[5] = "";
+
+    HMD_GetVolume();
+
+    // Send volume response to serial
+    sprintf_P(buffer, PSTR("%3u"), gVolumeMax);
+    cmd->GetHardwareSerial()->println(buffer);
+}
+
+void HMD_GetVolumeAvgCB(SerialCommand* cmd)
+{
+    DBGPRINTLN(F("// GetVolumeAvg "));
+    char buffer[5] = "";
+
+    HMD_GetVolume();
+
+    // Send volume response to serial
+    sprintf_P(buffer, PSTR("%3u"), gVolumeAvg);
+    cmd->GetHardwareSerial()->println(buffer);
+}
+
+void HMD_SetVolumeSampleCountCB(SerialCommand* cmd)
+{
+    char *arg = cmd->next();
+    char buffer[12];
+    unsigned long sampleCount;
+    DBGPRINTLN(F("// SetVolumeSampleCount "));
+
+    if (arg == NULL)
+    {
+        return;
+    }
+    else
+    {
+        sampleCount = strtoul(arg, NULL, 10);
+        DBGPRINT(F("// Received Sample Count: "));
+        DBGPRINTLN(arg);
+        if (sampleCount < 1)
+        {
+            cmd->GetHardwareSerial()->println("-1");
+        }
+        else if(sampleCount > MaxSampleCount)
+        {
+            DBGPRINT(F("// Sample count "));
+            DBGPRINTLN(arg);
+            DBGPRINT(F("is greater than max sample count "));
+            DBGPRINT(MaxSampleCount);
+            cmd->GetHardwareSerial()->println("-1");
+        }
+        else
+        {
+            gSampleCount = sampleCount;
+
+            // Respond with sample count
+            sprintf_P(buffer, PSTR("%10lu"), gSampleCount);
+            cmd->GetHardwareSerial()->println(buffer);
+        }
+    }
+}
+
+bool HMD_UpdateColorSensorValues(int displayIndex)
+{
+    char buffer[8];
+    bool sensorRead = false;
+
+    // Set default readings to zero
+    gRed = 0;
+    gGreen = 0;
+    gBlue = 0;
+    gClear = 0;
+
+    DBGPRINTLN(F("// Turning on display sensor "));
+
+    // Turn on appropriate color sensor
+    switch (displayIndex)
+    {
+    case 0:
+        digitalWrite(HmdPins.ColorSensorPower0, HIGH);
+        break;
+    case 1:
+        digitalWrite(HmdPins.ColorSensorPower1, HIGH);
+        break;
+    default:
+        DBGPRINT(F("// No display for that display index "));
+        return false;
+    }
+
+    DBGPRINTLN(F("// Waiting for sensor to power up "));
+
+    // Wait for sensor to power up
+    delay(StartupDelay);
+
+    DBGPRINTLN(F("// Reading sensor value "));
+
+    // Attempt to talk to the sensor
+    for (int i = 0; i < RetryCount; i++)
+    {
+        // Read value if sensor is present
+        if (tcs.begin())
+        {
+            tcs.setIntegrationTime(TCS34725_INTEGRATIONTIME_700MS);
+            tcs.setGain(TCS34725_GAIN_60X);
+
+            //Since we've just powered up, we need to wait for a reading to complete
+            delay(ReadingDelay);
+
+            DBGPRINTLN(F("// Getting raw data from sensor "));
+
+            // Grab the RGBC values
+            tcs.getRawData(&gRed, &gGreen, &gBlue, &gClear);
+            sensorRead = true;
+
+            DBGPRINT(F("// R: "));
+            DBGPRINTLN(utoa(gRed, buffer, 10));
+            DBGPRINT(F("// G: "));
+            DBGPRINTLN(utoa(gGreen, buffer, 10));
+            DBGPRINT(F("// B: "));
+            DBGPRINTLN(utoa(gBlue, buffer, 10));
+            DBGPRINT(F("// gClear: "));
+            DBGPRINTLN(utoa(gClear, buffer, 10));
+            break;
+        }
+    }
+
+    // Turn off color sensor
+    switch (displayIndex)
+    {
+    case 0:
+        digitalWrite(HmdPins.ColorSensorPower0, LOW);
+        break;
+    case 1:
+        digitalWrite(HmdPins.ColorSensorPower1, LOW);
+        break;
+    }
+    
+    return sensorRead;
+}
+
+void HMD_PresenceOn()
+{
+    // Attach an interrupt to the IR sensor to control presence spoofing time synchronization
+    attachInterrupt(digitalPinToInterrupt(HmdPins.PresenceIrSensor), HMD_IrIsr, RISING);
+    digitalWrite(HmdPins.PresenceSpoofer, LOW);
+}
+
+void HMD_PresenceOff()
+{
+    // Disable the interrupt and set the presence LED full on to drown out the presence sensor and fake non-presence
+    detachInterrupt(digitalPinToInterrupt(HmdPins.PresenceIrSensor));
+    digitalWrite(HmdPins.PresenceSpoofer, HIGH);
+}
+
+void HMD_IrIsr()
+{
+    gHmdPresenceTrigger = true;
+}
+
+void HMD_PulseLed()
+{
+    // Fake the blinking sequence of the HMD presence sensor
+    PORTA |= B00000010;
+    delayMicroseconds(40);
+    PORTA &= ~B00000010;
+    delayMicroseconds(4686);
+    PORTA |= B00000010;
+    delayMicroseconds(40);
+    PORTA &= ~B00000010;
+}
+
+void HMD_GetVolume()
+{
+    char buffer[8];
+    gVolumeMax = 0;
+    gVolumeAvg = 0;
+    gVolumeRms = 0;
+
+    for (uint64_t i = 0; i < gSampleCount; i++)
+    {
+        while (!(ADCSRA & /*0x10*/_BV(ADIF))); // Wait for the ADIF bit of the ADC status register to be signaled
+        sbi(ADCSRA, ADIF); // Restart the ADC
+        
+        // Grab the low and high bytes and concatenate them
+        byte l = ADCL;
+        byte h = ADCH;
+        int reading = ((int)h << 8) | l;
+        unsigned int amplitude = abs(reading - PeakAmplitude);
+
+        // Compare with previous entry for max
+        gVolumeMax = max(gVolumeMax, amplitude);
+        // Sum total for avg
+        gVolumeAvg += amplitude;
+        // Sum squares for RMS
+        gVolumeRms += ((unsigned long)amplitude * amplitude);
+    }
+    gVolumeMax = 100 * gVolumeMax / PeakAmplitude;
+    gVolumeAvg = ((gVolumeAvg / gSampleCount) * 100) / PeakAmplitude;
+    gVolumeRms = (sqrt(gVolumeRms / gSampleCount) * 100) / PeakAmplitude;
+    gVolumeRms = gVolumeRms * 0.7; // Approximate peak using .7 for sine wave
+    DBGPRINT(F("Volume Rms: "));
+    DBGPRINTLN(utoa(gVolumeRms, buffer, 10));
+    DBGPRINT(F("Volume Avg: "));
+    DBGPRINTLN(utoa(gVolumeAvg, buffer, 10));
+    DBGPRINT(F("Volume Peak: "));
+    DBGPRINTLN(utoa(gVolumeMax, buffer, 10));
+}
+
+void HMD_Usage()
+{
+    DBGPRINTLN(F("sethmd [hmd]"));
+    DBGPRINTLN(F("    selects the HMD to target for brightness, color, and presence commands"));
+    DBGPRINTLN(F("brightness [display]"));
+    DBGPRINTLN(F("    returns the brightness of the specified display"));
+    DBGPRINTLN(F("color [display]"));
+    DBGPRINTLN(F("    returns the RGB color of the specified display"));
+    DBGPRINTLN(F("presence [option]"));
+    DBGPRINTLN(F("    0, turns the presence sensor off"));
+    DBGPRINTLN(F("    1, turns the presence sensor on"));
+    DBGPRINTLN(F("getvolumerms"));
+    DBGPRINTLN(F("    returns the rms volume of the audio waveform"));
+    DBGPRINTLN(F("getvolumepeak"));
+    DBGPRINTLN(F("    returns the absolute value peak value of the waveform"));
+    DBGPRINTLN(F("getvolumeavg"));
+    DBGPRINTLN(F("    returns the avg volume of the absolute value of the audio waveform"));
+    DBGPRINTLN(F("svsc [sample count]"));
+    DBGPRINT(F("    sets the number of samples to take from the waveform. Default is 2048, max is "));
+    DBGPRINTLN(MaxSampleCount);
+
+    SERVO_Usage();
+}
diff --git a/usb/tools/ConnectionExerciser/libraries/HmdExerciser/HMDExerciser.h b/usb/tools/ConnectionExerciser/libraries/HmdExerciser/HMDExerciser.h
new file mode 100644
index 0000000..292b61f
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/HmdExerciser/HMDExerciser.h
@@ -0,0 +1,11 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#ifndef HMDExerciser_H
+#define HMDExerciser_H
+
+void HMD_setup();
+void HMD_Usage();
+void HMD_PulseLed();
+
+#endif
diff --git a/usb/tools/ConnectionExerciser/libraries/HmdExerciser/keywords.txt b/usb/tools/ConnectionExerciser/libraries/HmdExerciser/keywords.txt
new file mode 100644
index 0000000..fe302ff
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/HmdExerciser/keywords.txt
@@ -0,0 +1,24 @@
+#######################################
+# Syntax Coloring Map For HMDExerciser
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+HMD_setup	KEYWORD2
+
+#######################################
+# Instances (KEYWORD2)
+#######################################
+
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+
diff --git a/usb/tools/ConnectionExerciser/libraries/Model_3201/Model_3201.cpp b/usb/tools/ConnectionExerciser/libraries/Model_3201/Model_3201.cpp
new file mode 100644
index 0000000..6394b6b
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/Model_3201/Model_3201.cpp
@@ -0,0 +1,21 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#include "Model_3201.h"
+
+#define EEPROM_SELECT   A0 // PF[0]
+#define CDONE           A1 // PF[1]
+#define CRESET_B        A2 // PF[2]
+#define INIT_DONE       A3 // PF[3]
+#define EEPROM_CS       53 // PB[0]
+
+void Model_3201_setup()
+{
+    digitalWrite( CRESET_B, LOW );
+    pinMode( CRESET_B, OUTPUT );
+    
+	//
+	// TODO: Placeholder for FPGA Interface
+	//
+}
+
diff --git a/usb/tools/ConnectionExerciser/libraries/Model_3201/Model_3201.h b/usb/tools/ConnectionExerciser/libraries/Model_3201/Model_3201.h
new file mode 100644
index 0000000..840b1f4
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/Model_3201/Model_3201.h
@@ -0,0 +1,12 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#ifndef MODEL_3201_H
+#define MODEL_3201_H
+
+#include <SPI.h>
+#define MODEL_3201 0xC
+
+void Model_3201_setup();
+
+#endif
diff --git a/usb/tools/ConnectionExerciser/libraries/Servo/ServoShield.cpp b/usb/tools/ConnectionExerciser/libraries/Servo/ServoShield.cpp
new file mode 100644
index 0000000..70be132
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/Servo/ServoShield.cpp
@@ -0,0 +1,137 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#include <SerialCommand.h>
+#include <Servo.h>
+#include <ServoEaser.h>
+#include "ServoShield.h"
+
+#define DBGPRINT(A)    if (dbg) {Serial.print(A);}
+#define DBGPRINTLN(A)  if (dbg) {Serial.println(A);}
+
+// Pins
+#define ServoPin1 11
+#define ServoPin2 12
+
+ServoEaser Servo1Easer;
+ServoEaser Servo2Easer;
+
+int servoFrameMillis = 20;
+int msPerDegree = 5;
+int easingMultipler = 3;
+int defaultEaseTime = msPerDegree * easingMultipler;
+
+struct Servos
+{
+    Servo servo;
+    ServoEaser servoEaser;
+    int angle;
+    int pin;
+};
+Servos ServoArray[2];
+
+void SERVO_SetAngleCB(SerialCommand* cmd);
+void SERVO_Usage();
+void AddSerCommand(const char* cmd, void(*function)(SerialCommand*));
+bool SERVO_Overshoot(int angle, int servo);
+void SERVO_Loop();
+extern bool dbg;
+
+void SERVO_setup()
+{
+    DBGPRINTLN(F("// Servo Shield Initialized "));
+    AddSerCommand("setangle", SERVO_SetAngleCB);
+    ServoArray[0].pin = ServoPin1;
+    ServoArray[1].pin = ServoPin2;
+    ServoArray[0].servo.attach(ServoArray[0].pin, 800, 2200);
+    ServoArray[1].servo.attach(ServoArray[1].pin, 800, 2200);
+    ServoArray[0].angle = 90;
+    ServoArray[1].angle = 90;
+    ServoArray[0].servoEaser.begin( ServoArray[0].servo, servoFrameMillis );
+    ServoArray[1].servoEaser.begin( ServoArray[1].servo, servoFrameMillis );
+}
+
+void SERVO_SetAngleCB(SerialCommand* cmd)
+{
+    char buffer[2];
+    char *arg = cmd->next();
+    int servoIndex = 0;
+    int angle = 0;
+
+    DBGPRINTLN(F("// Set Servo Angle "));
+
+    if (arg == NULL)
+    {
+        return;
+    }
+
+    servoIndex = atoi(arg);
+    arg = cmd->next();
+
+    if (arg == NULL)
+    {
+        return;
+    }
+
+    angle = atoi(arg);
+    
+    if ((servoIndex > 0) && (servoIndex <= (sizeof(ServoArray) / sizeof(ServoArray[0]))))
+    {
+        servoIndex = servoIndex - 1;
+    }
+    else
+    {
+        DBGPRINT(F("// Servo \""));
+        DBGPRINT(servoIndex);
+        DBGPRINTLN(F("\" not valid"));
+        sprintf_P(buffer, PSTR("%u"), 0);
+        cmd->GetHardwareSerial()->println(buffer);
+        return false;
+    }
+
+    if (angle < 0 || angle > 180)
+    {
+        DBGPRINT(F("// Angle \""));
+        DBGPRINT(angle);
+        DBGPRINTLN(F("\" not valid"));
+        sprintf_P(buffer, PSTR("%u"), 0);
+        cmd->GetHardwareSerial()->println(buffer);
+        return false;
+    }
+
+    
+    int angleChange = (angle < ServoArray[servoIndex].angle) ? (ServoArray[servoIndex].angle - angle) : (angle - ServoArray[servoIndex].angle);
+    int moveTime = 125 + (angleChange * defaultEaseTime);
+    if(moveTime < 200)
+    {
+        moveTime = 200;
+    }
+    else
+    {
+        ServoArray[servoIndex].servoEaser.easeTo(angle, moveTime);
+        
+        DBGPRINT(F("// Moving "));
+        DBGPRINT(angle);
+        DBGPRINT(F(" degrees over "));
+        DBGPRINT(moveTime);
+        DBGPRINTLN(F(" ms "));
+    }
+    ServoArray[servoIndex].angle = angle;
+    
+    sprintf_P(buffer, PSTR("%u"), servoIndex+1);
+    cmd->GetHardwareSerial()->println(buffer);
+}
+
+void SERVO_Usage()
+{
+    DBGPRINTLN(F("setAngle [servo] [angle]"));
+    DBGPRINTLN(F("    sets specified servo to specified angle"));
+}
+
+void SERVO_Loop()
+{
+    for(int i = 0; i < 2; i++)
+    {
+        ServoArray[i].servoEaser.update();
+    }
+}
diff --git a/usb/tools/ConnectionExerciser/libraries/Servo/ServoShield.h b/usb/tools/ConnectionExerciser/libraries/Servo/ServoShield.h
new file mode 100644
index 0000000..ead7514
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/Servo/ServoShield.h
@@ -0,0 +1,11 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#ifndef SERVOShield_H
+#define SERVOShield_H
+
+void SERVO_setup();
+void SERVO_Usage();
+void SERVO_Loop();
+
+#endif
diff --git a/usb/tools/ConnectionExerciser/libraries/USBCExerciser/USBCExerciser.cpp b/usb/tools/ConnectionExerciser/libraries/USBCExerciser/USBCExerciser.cpp
new file mode 100644
index 0000000..507ba48
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/USBCExerciser/USBCExerciser.cpp
@@ -0,0 +1,463 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#include <SoftwareSerial.h>
+#include <SerialCommand.h>
+#include <LiquidCrystal.h>
+#include <Event.h>
+#include <Timer.h>
+#include <Wire.h>
+#include "DualRoleConnectionExerciser.h"
+
+void AddSerCommand(const char* cmd, void(*function)(SerialCommand*));
+extern Timer timer;
+extern LiquidCrystal *lcd;
+extern bool dbg;
+extern uint32_t cmdDelay;
+extern uint32_t disconnectTimeout;
+
+byte switchToPortNum = 0;
+bool enableSuperSpeed = true;
+
+#define DBGPRINT(A)    if (dbg) {Serial.print(A);}
+#define DBGPRINTLN(A)  if (dbg) {Serial.println(A);}
+
+                            // PK[0]
+#define kOEN          A9    // PK[1]
+#define kSEL1         A10   // PK[2]
+#define kSEL0         A11   // PK[3]
+#define kALERT        A12   // PK[4]
+                            // PK[5]
+#define kSS_OEN       A14   // PK[6]
+#define kSS_SEL       A15   // PK[7]
+
+/*
+#define kCCSEL0       A8    // PK[0]
+#define kCCSEL1       A9    // PK[1]
+#define kUSB1SEL0     A11   // PK[2]
+#define kUSB1SEL1     A10   // PK[3]
+#define kOEN          A11   // PK[4]
+#define kSEL          A12   // PK[5]
+#define kUSB2SEL1     A13   // PK[6]
+#define kUSB2SEL0     A14   // PK[7]
+*/
+
+#define kPOWER_SELA   7     // PH[4]
+#define kPOWER_SELB   8     // PH[5]
+#define kPOWER_EN     9     // PH[6]
+
+#define kLCDROWS      2 // Number of rows on the LCD module
+#define kLCDCOLS      8 // Number of columns on the LCD module
+#define kRS          41 // LCD module register select line (0 = command, 1 = data input)
+#define kRW          38 // Read not write (0 = write, 1 = read)
+#define kEN          39 // LCD module enable
+
+#define kVREF 2.56       // Internal reference voltage
+#define kCURROFFSET 0.05 // Voltage offset of differential ADC for current measurement
+#define kVOLTAGESCALEFACTOR 5.5 // see description of PrintSummary()
+#define kCURRENTSCALEFACTOR 10 // see description of PrintSummary()
+#define kADCSCALAR (kVREF * ((10.0 + 5.0)/5.0) / 1024.0) // see description of PrintSummary()
+#define kDIFFADCSCALAR (kVREF * kCURRENTSCALEFACTOR / 512.0) // see description of PrintSummary()
+
+//
+// Function pointers used by DTMF routines.  These are populated
+// in the setup routine.
+//
+extern float (*ReadVoltage)();
+extern float (*ReadCurrent)();
+extern void (*SetPort)(byte);
+extern char (*GetPort)();
+extern void (*SuperSpeed)(bool);
+
+void USBC_PrintVoltsCB(SerialCommand* cmd);
+void USBC_PrintAmpsCB(SerialCommand* cmd);
+void USBC_ChangePortCB(SerialCommand* cmd);
+void USBC_SetPort(byte port);
+char USBC_GetPort(void);
+void USBC_UpdateDisplayCB(void);
+void USBC_Usage();
+float USBC_ReadVoltage(void);
+float USBC_ReadCurrent(void);
+void USBC_DisconnectUSB(void);
+void USBC_CyclePorts(void);
+void USBC_UpdateVoltageString(float volts, char *strVolts );
+void USBC_UpdateCurrentString(float amps, char *strAmps );
+void USBC_SuperSpeedCB(SerialCommand* cmd);
+void USBC_SuperSpeed( bool enable );
+void USBC_SetCmdDelayCB(SerialCommand* cmd);
+void USBC_SetDisconnectTimeoutCB(SerialCommand* cmd);
+void USBC_SetPort_Internal(void);
+
+void USBC_setup()
+{
+    //
+    // Hook up function pointers
+    //
+    ReadVoltage = &USBC_ReadVoltage;
+    ReadCurrent = &USBC_ReadCurrent;
+    SetPort = &USBC_SetPort;
+    GetPort = &USBC_GetPort;
+    SuperSpeed = &USBC_SuperSpeed;
+
+    //
+    // Register serial command callbacks
+    //
+    AddSerCommand("volts", USBC_PrintVoltsCB);
+    AddSerCommand("amps", USBC_PrintAmpsCB);
+    AddSerCommand("port", USBC_ChangePortCB);
+    AddSerCommand("delay", USBC_SetCmdDelayCB);
+    AddSerCommand("timeout", USBC_SetDisconnectTimeoutCB);
+    AddSerCommand("superspeed", USBC_SuperSpeedCB);
+
+    // 1602A LCD module
+    pinMode(kRS, OUTPUT);
+    pinMode(kRW, OUTPUT);
+    pinMode(kEN, OUTPUT);
+
+    // ADC
+    pinMode(A0, INPUT);
+    pinMode(A1, INPUT);
+    pinMode(A2, INPUT);
+
+    // Port K
+    pinMode(   kOEN, OUTPUT);
+    pinMode(  kSEL0, OUTPUT);
+    pinMode(  kSEL1, OUTPUT);
+    pinMode( kALERT, INPUT);
+    pinMode(kSS_OEN, OUTPUT);
+    pinMode(kSS_SEL, OUTPUT);
+    DDRK = 0xFF;
+    PORTK = 0x10;
+
+    pinMode(kPOWER_SELA, OUTPUT);
+    pinMode(kPOWER_SELB, OUTPUT);
+    pinMode(kPOWER_EN, OUTPUT);
+
+
+    // Initialize
+    digitalWrite(kRS, LOW);
+    digitalWrite(kRW, LOW);
+    digitalWrite(kEN, LOW);
+    digitalWrite(kOEN, HIGH);
+    digitalWrite(kSS_OEN, HIGH);
+    digitalWrite(kPOWER_SELA, LOW);
+    digitalWrite(kPOWER_SELB, LOW);
+    digitalWrite(kPOWER_EN, LOW);
+    analogReference(INTERNAL2V56);
+
+    Wire.begin();
+    Wire.setClock( 400000 );
+
+    Wire.beginTransmission( 0x40 );
+    Wire.write( 0x05 );
+    Wire.write( 0x02 );
+    Wire.write( 0x00 );
+    Wire.endTransmission( true );
+
+    Wire.beginTransmission( 0x40 );
+    Wire.write( 0x00 );
+    Wire.write( 0x48 );
+    Wire.write( 0x07 );
+    Wire.endTransmission( true );
+
+
+    lcd = new LiquidCrystal(kRS, kRW, kEN, 37, 36, 35, 34, 33, 32, 31, 30);
+    lcd->begin(kLCDCOLS, kLCDROWS);
+    timer.every( 500, USBC_UpdateDisplayCB);
+
+    if ( PINL & 0x01 )
+        timer.every( 3000, USBC_CyclePorts);
+
+    SetPort(3);
+}
+
+void USBC_PrintVoltsCB(SerialCommand* cmd)
+{
+    char Buffer[8];
+    float f;
+    int i;
+
+    f = USBC_ReadVoltage();
+    i = (int)f;
+
+    sprintf_P( Buffer, PSTR("%02d%02d"), i, abs((int)((f - (float)i) * 100.00)) );
+    cmd->GetHardwareSerial()->println(Buffer);
+}
+
+void USBC_PrintAmpsCB(SerialCommand* cmd)
+{
+    char Buffer[8];
+    float f;
+    int i;
+
+    f = ReadCurrent();
+    i = (int)f;
+    if ( f < 0.0 )
+        sprintf_P( Buffer, PSTR("1%d%02d"), -1 * i, abs((int)((f - (float)i) * 100.00)) );
+    else
+        sprintf_P( Buffer, PSTR("%02d%02d"), i, abs((int)((f - (float)i) * 100.00)) );
+    cmd->GetHardwareSerial()->println(Buffer);
+}
+
+void USBC_CyclePorts()
+{
+}
+
+void USBC_SuperSpeedCB(SerialCommand* cmd)
+{
+    char *arg = cmd->next();
+
+    if ( arg != NULL )
+    {
+        cmd->GetHardwareSerial()->print(F("superspeed "));
+        cmd->GetHardwareSerial()->println(arg);
+
+        USBC_SuperSpeed( atoi(arg) );
+    }
+}
+
+void USBC_SuperSpeed( bool enable )
+{
+    if (enable)
+    {
+        DBGPRINTLN(F("// Enable superspeed switches"));
+    }
+    else
+    {
+        DBGPRINTLN(F("// Disable superspeed switches"));
+    }
+
+    if (enable != enableSuperSpeed)
+    {
+        // Set the enable state no matter what port is current connected
+        enableSuperSpeed = enable;
+
+        byte port = (((PINH & 0x30) >> 4) + 1);
+
+        if (1 == port || 2 == port)
+        {
+            // Re-connect to the same port only if on SuperSpeed port
+            USBC_SetPort(port);
+        }
+    }
+}
+
+void USBC_SetCmdDelayCB(SerialCommand* cmd)
+{
+    char* arg = cmd->next();
+
+    cmdDelay = atoi(arg);
+
+    cmd->GetHardwareSerial()->print(F("delay "));
+    cmd->GetHardwareSerial()->println(cmdDelay);
+
+    cmdDelay *= 1000; // Convert to ms
+}
+
+void USBC_SetDisconnectTimeoutCB(SerialCommand* cmd)
+{
+    char* arg = cmd->next();
+
+    disconnectTimeout = atoi(arg);
+
+    cmd->GetHardwareSerial()->print(F("timeout "));
+    cmd->GetHardwareSerial()->println(disconnectTimeout);
+}
+
+void USBC_ChangePortCB(SerialCommand* cmd)
+{
+    char *arg = cmd->next();
+
+    if ( arg == NULL ) {
+        cmd->GetHardwareSerial()->println(USBC_GetPort());
+        return;
+    }
+
+    cmd->GetHardwareSerial()->print(F("port "));
+    cmd->GetHardwareSerial()->println(atoi(arg));
+
+    USBC_SetPort( atoi(arg) );
+}
+
+void USBC_SetPort( byte port )
+{
+    switchToPortNum = port;
+
+    timer.after(cmdDelay, USBC_SetPort_Internal);
+    cmdDelay = 0;
+}
+
+void USBC_SetPort_Internal()
+{
+    byte mux[4] = {0x3, 0x2, 0x0, 0x1};
+
+    USBC_DisconnectUSB();
+
+    // NOTE: see PK[] defines above to interpret PORTK
+    if (switchToPortNum >= 1 && switchToPortNum <= 4) {
+        switch (switchToPortNum){
+        case 1:
+            digitalWrite( kPOWER_SELA, LOW );
+            digitalWrite( kPOWER_SELB, LOW );
+            digitalWrite( kPOWER_EN, HIGH );
+
+            PORTK = (enableSuperSpeed) ? 0x0C : 0x4C;
+            break;
+        case 2:
+            digitalWrite( kPOWER_SELA, HIGH );
+            digitalWrite( kPOWER_SELB, LOW );
+            digitalWrite( kPOWER_EN, HIGH );
+
+            PORTK = (enableSuperSpeed) ? 0x84 : 0xC4;
+            break;
+        case 3:
+            digitalWrite( kPOWER_SELA, HIGH );
+            digitalWrite( kPOWER_SELB, HIGH );
+            digitalWrite( kPOWER_EN, HIGH );
+
+            PORTK = 0x08;
+            break;
+        case 4:
+            digitalWrite( kPOWER_SELA, LOW );
+            digitalWrite( kPOWER_SELB, HIGH );
+            digitalWrite( kPOWER_EN, HIGH );
+
+            PORTK = 0x00;
+        }
+
+        if (0 != disconnectTimeout)
+        {
+            timer.after(disconnectTimeout, USBC_DisconnectUSB);
+            disconnectTimeout = 0;
+        }
+    }
+}
+
+char USBC_GetPort()
+{
+    char port = '0';
+    uint8_t tmp;
+
+    if ( digitalRead( kPOWER_EN ) )
+    {
+        // port = ((PINH & 0x30) >> 4) + '1';
+        tmp = (PINH & 0x30) >> 4;
+        switch (tmp)
+        {
+        case 0: port = '1'; break;
+        case 1: port = '2'; break;
+        case 2: port = '4'; break;
+        case 3: port = '3'; break;
+        }
+    }
+
+    return port;
+}
+
+void USBC_Usage()
+{
+    DBGPRINTLN(F("volts"));
+    DBGPRINTLN(F("    shows voltage. no parameters."));
+    DBGPRINTLN(F("amps"));
+    DBGPRINTLN(F("    shows amperage. no parameters."));
+    DBGPRINTLN(F("port [options]"));
+    DBGPRINTLN(F("    <empty>, shows current USB port."));
+    DBGPRINTLN(F("    1, connects to USB port 1"));
+    DBGPRINTLN(F("    2, connects to USB port 2"));
+    DBGPRINTLN(F("    3, connects to USB port 3"));
+    DBGPRINTLN(F("    0 (or other), disconnect all ports"));
+    DBGPRINTLN(F("delay [seconds]"));
+    DBGPRINTLN(F("    seconds to delay the next port change."));
+    DBGPRINTLN(F("timeout [miliseconds]"));
+    DBGPRINTLN(F("    next port change will disconnect after specified number of ms."));
+}
+
+void USBC_UpdateDisplayCB()
+{
+    char tmpStr[10];
+  
+    lcd->home();
+    USBC_UpdateVoltageString( USBC_ReadVoltage(), tmpStr );
+    lcd->print( tmpStr );
+    lcd->setCursor(1,1);
+    USBC_UpdateCurrentString( USBC_ReadCurrent(), tmpStr );
+    lcd->print( tmpStr );
+}
+
+
+float USBC_ReadVoltage()
+{
+    int v;
+    float volts;
+
+    Wire.beginTransmission( 0x40 );
+    Wire.write( 0x02 );
+    Wire.endTransmission( true );
+    Wire.requestFrom( 0x40, 2 );
+    if ( Wire.available() == 2 )
+    {
+        v = Wire.read();
+        v = (v << 8) | Wire.read();
+        volts = v * 0.00125;
+    }
+    return volts;
+}
+
+float USBC_ReadCurrent()
+{
+    uint16_t raw;
+    int i;
+    float amps;
+
+    Wire.beginTransmission( 0x40 );
+    Wire.write( 0x04 );
+    Wire.endTransmission( true );
+    Wire.requestFrom( 0x40, 2 );
+    if ( Wire.available() == 2 )
+    {
+        raw = Wire.read();
+        raw = raw << 8 | Wire.read();
+        i = (int16_t)raw;
+        amps = i * 0.001;
+    }
+    return amps;
+}
+
+void USBC_DisconnectUSB()
+{
+    // Disconnect data signals
+    digitalWrite(kOEN, HIGH);
+    digitalWrite(kSS_OEN, HIGH);
+
+    // Disconnect power
+    digitalWrite(kPOWER_EN, LOW);
+
+    // Stay in the disconnected state for at least 200 ms to allow
+    // any high voltage contract to be disconnected by the supply
+    delay(200);
+}
+
+void USBC_UpdateVoltageString(float volts, char *strVolts)
+{
+    int i, f;
+  
+    i = (int)volts;
+    f = abs((int)((volts - (float)i) * 100.00));  
+    sprintf_P( strVolts, PSTR(" %2d.%02d V"), i, f );
+}
+
+void USBC_UpdateCurrentString(float amps, char *strAmps)
+{
+    int i, f;
+  
+    i = abs((int)amps);
+    f = (int)((abs(amps) - (float)i) * 100.00);  
+
+    if ( amps < 0.0 )
+    {
+        sprintf_P( strAmps, PSTR("-%d.%02d A"), i, f );
+    } else {
+        sprintf_P( strAmps, PSTR(" %d.%02d A"), i, f );
+    }
+}
+
diff --git a/usb/tools/ConnectionExerciser/libraries/USBCExerciser/USBCExerciser.h b/usb/tools/ConnectionExerciser/libraries/USBCExerciser/USBCExerciser.h
new file mode 100644
index 0000000..30f3843
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/USBCExerciser/USBCExerciser.h
@@ -0,0 +1,12 @@
+/* Copyright (c) Microsoft Corporation. All rights reserved.
+   Licensed under the MIT License. */
+
+#ifndef USBCExerciser_H
+#define USBCExerciser_H
+
+#define USBC_SHIELD 0x8
+
+void USBC_setup();
+void USBC_Usage();
+
+#endif
diff --git a/usb/tools/ConnectionExerciser/libraries/USBCExerciser/keywords.txt b/usb/tools/ConnectionExerciser/libraries/USBCExerciser/keywords.txt
new file mode 100644
index 0000000..a4f616b
--- /dev/null
+++ b/usb/tools/ConnectionExerciser/libraries/USBCExerciser/keywords.txt
@@ -0,0 +1,24 @@
+#######################################
+# Syntax Coloring Map For DualRoleConnectionExercisor
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+USBC_setup	KEYWORD2
+
+#######################################
+# Instances (KEYWORD2)
+#######################################
+
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+USBC_SHIELD	LITERAL1
diff --git a/usb/tools/README.md b/usb/tools/README.md
new file mode 100644
index 0000000..f861b49
--- /dev/null
+++ b/usb/tools/README.md
@@ -0,0 +1 @@
+ConnectionExerciser: contains code for the Connection Exerciser device.