11 files changed, 1573 insertions, 194 deletions

diff --git a/tmk_core/protocol/lufa.mk b/tmk_core/protocol/lufa.mk
index 5b1e3d19d..151d26cbc 100644
--- a/tmk_core/protocol/lufa.mk
+++ b/tmk_core/protocol/lufa.mk
@@ -21,6 +21,10 @@ ifeq ($(strip $(MIDI_ENABLE)), yes)
         include $(TMK_PATH)/protocol/midi.mk
 endif
 
+ifeq ($(strip $(ADAFRUIT_BLE_ENABLE)), yes)
+        LUFA_SRC += $(LUFA_DIR)/adafruit_ble.cpp
+endif
+
 ifeq ($(strip $(BLUETOOTH_ENABLE)), yes)
         LUFA_SRC += $(LUFA_DIR)/bluetooth.c \
         $(TMK_DIR)/protocol/serial_uart.c
diff --git a/tmk_core/protocol/lufa/adafruit_ble.cpp b/tmk_core/protocol/lufa/adafruit_ble.cpp
new file mode 100644
index 000000000..37194e77a
--- /dev/null
+++ b/tmk_core/protocol/lufa/adafruit_ble.cpp
@@ -0,0 +1,805 @@
+#include "adafruit_ble.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <alloca.h>
+#include <util/delay.h>
+#include <util/atomic.h>
+#include "debug.h"
+#include "pincontrol.h"
+#include "timer.h"
+#include "action_util.h"
+#include "ringbuffer.hpp"
+#include <string.h>
+
+// These are the pin assignments for the 32u4 boards.
+// You may define them to something else in your config.h
+// if yours is wired up differently.
+#ifndef AdafruitBleResetPin
+#define AdafruitBleResetPin D4
+#endif
+
+#ifndef AdafruitBleCSPin
+#define AdafruitBleCSPin    B4
+#endif
+
+#ifndef AdafruitBleIRQPin
+#define AdafruitBleIRQPin   E6
+#endif
+
+
+#define SAMPLE_BATTERY
+#define ConnectionUpdateInterval 1000 /* milliseconds */
+
+static struct {
+  bool is_connected;
+  bool initialized;
+  bool configured;
+
+#define ProbedEvents 1
+#define UsingEvents 2
+  bool event_flags;
+
+#ifdef SAMPLE_BATTERY
+  uint16_t last_battery_update;
+  uint32_t vbat;
+#endif
+  uint16_t last_connection_update;
+} state;
+
+// Commands are encoded using SDEP and sent via SPI
+// https://github.com/adafruit/Adafruit_BluefruitLE_nRF51/blob/master/SDEP.md
+
+#define SdepMaxPayload 16
+struct sdep_msg {
+  uint8_t type;
+  uint8_t cmd_low;
+  uint8_t cmd_high;
+  struct __attribute__((packed)) {
+    uint8_t len:7;
+    uint8_t more:1;
+  };
+  uint8_t payload[SdepMaxPayload];
+} __attribute__((packed));
+
+// The recv latency is relatively high, so when we're hammering keys quickly,
+// we want to avoid waiting for the responses in the matrix loop.  We maintain
+// a short queue for that.  Since there is quite a lot of space overhead for
+// the AT command representation wrapped up in SDEP, we queue the minimal
+// information here.
+
+enum queue_type {
+  QTKeyReport, // 1-byte modifier + 6-byte key report
+  QTConsumer,  // 16-bit key code
+#ifdef MOUSE_ENABLE
+  QTMouseMove, // 4-byte mouse report
+#endif
+};
+
+struct queue_item {
+  enum queue_type queue_type;
+  uint16_t added;
+  union __attribute__((packed)) {
+    struct __attribute__((packed)) {
+      uint8_t modifier;
+      uint8_t keys[6];
+    } key;
+
+    uint16_t consumer;
+    struct __attribute__((packed)) {
+      uint8_t x, y, scroll, pan;
+    } mousemove;
+  };
+};
+
+// Items that we wish to send
+static RingBuffer<queue_item, 40> send_buf;
+// Pending response; while pending, we can't send any more requests.
+// This records the time at which we sent the command for which we
+// are expecting a response.
+static RingBuffer<uint16_t, 2> resp_buf;
+
+static bool process_queue_item(struct queue_item *item, uint16_t timeout);
+
+enum sdep_type {
+  SdepCommand = 0x10,
+  SdepResponse = 0x20,
+  SdepAlert = 0x40,
+  SdepError = 0x80,
+  SdepSlaveNotReady = 0xfe, // Try again later
+  SdepSlaveOverflow = 0xff, // You read more data than is available
+};
+
+enum ble_cmd {
+  BleInitialize = 0xbeef,
+  BleAtWrapper = 0x0a00,
+  BleUartTx = 0x0a01,
+  BleUartRx = 0x0a02,
+};
+
+enum ble_system_event_bits {
+  BleSystemConnected = 0,
+  BleSystemDisconnected = 1,
+  BleSystemUartRx = 8,
+  BleSystemMidiRx = 10,
+};
+
+// The SDEP.md file says 2MHz but the web page and the sample driver
+// both use 4MHz
+#define SpiBusSpeed 4000000
+
+#define SdepTimeout 150 /* milliseconds */
+#define SdepShortTimeout 10 /* milliseconds */
+#define SdepBackOff 25 /* microseconds */
+#define BatteryUpdateInterval 10000 /* milliseconds */
+
+static bool at_command(const char *cmd, char *resp, uint16_t resplen,
+                       bool verbose, uint16_t timeout = SdepTimeout);
+static bool at_command_P(const char *cmd, char *resp, uint16_t resplen,
+                         bool verbose = false);
+
+struct SPI_Settings {
+  uint8_t spcr, spsr;
+};
+
+static struct SPI_Settings spi;
+
+// Initialize 4Mhz MSBFIRST MODE0
+void SPI_init(struct SPI_Settings *spi) {
+  spi->spcr = _BV(SPE) | _BV(MSTR);
+  spi->spsr = _BV(SPI2X);
+
+  static_assert(SpiBusSpeed == F_CPU / 2, "hard coded at 4Mhz");
+
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+    // Ensure that SS is OUTPUT High
+    digitalWrite(B0, PinLevelHigh);
+    pinMode(B0, PinDirectionOutput);
+
+    SPCR |= _BV(MSTR);
+    SPCR |= _BV(SPE);
+    pinMode(B1 /* SCK */, PinDirectionOutput);
+    pinMode(B2 /* MOSI */, PinDirectionOutput);
+  }
+}
+
+static inline void SPI_begin(struct SPI_Settings*spi) {
+  SPCR = spi->spcr;
+  SPSR = spi->spsr;
+}
+
+static inline uint8_t SPI_TransferByte(uint8_t data) {
+  SPDR = data;
+  asm volatile("nop");
+  while (!(SPSR & _BV(SPIF))) {
+    ; // wait
+  }
+  return SPDR;
+}
+
+static inline void spi_send_bytes(const uint8_t *buf, uint8_t len) {
+  if (len == 0) return;
+  const uint8_t *end = buf + len;
+  while (buf < end) {
+    SPDR = *buf;
+    while (!(SPSR & _BV(SPIF))) {
+      ; // wait
+    }
+    ++buf;
+  }
+}
+
+static inline uint16_t spi_read_byte(void) {
+  return SPI_TransferByte(0x00 /* dummy */);
+}
+
+static inline void spi_recv_bytes(uint8_t *buf, uint8_t len) {
+  const uint8_t *end = buf + len;
+  if (len == 0) return;
+  while (buf < end) {
+    SPDR = 0; // write a dummy to initiate read
+    while (!(SPSR & _BV(SPIF))) {
+      ; // wait
+    }
+    *buf = SPDR;
+    ++buf;
+  }
+}
+
+#if 0
+static void dump_pkt(const struct sdep_msg *msg) {
+  print("pkt: type=");
+  print_hex8(msg->type);
+  print(" cmd=");
+  print_hex8(msg->cmd_high);
+  print_hex8(msg->cmd_low);
+  print(" len=");
+  print_hex8(msg->len);
+  print(" more=");
+  print_hex8(msg->more);
+  print("\n");
+}
+#endif
+
+// Send a single SDEP packet
+static bool sdep_send_pkt(const struct sdep_msg *msg, uint16_t timeout) {
+  SPI_begin(&spi);
+
+  digitalWrite(AdafruitBleCSPin, PinLevelLow);
+  uint16_t timerStart = timer_read();
+  bool success = false;
+  bool ready = false;
+
+  do {
+    ready = SPI_TransferByte(msg->type) != SdepSlaveNotReady;
+    if (ready) {
+      break;
+    }
+
+    // Release it and let it initialize
+    digitalWrite(AdafruitBleCSPin, PinLevelHigh);
+    _delay_us(SdepBackOff);
+    digitalWrite(AdafruitBleCSPin, PinLevelLow);
+  } while (timer_elapsed(timerStart) < timeout);
+
+  if (ready) {
+    // Slave is ready; send the rest of the packet
+    spi_send_bytes(&msg->cmd_low,
+                   sizeof(*msg) - (1 + sizeof(msg->payload)) + msg->len);
+    success = true;
+  }
+
+  digitalWrite(AdafruitBleCSPin, PinLevelHigh);
+
+  return success;
+}
+
+static inline void sdep_build_pkt(struct sdep_msg *msg, uint16_t command,
+                                  const uint8_t *payload, uint8_t len,
+                                  bool moredata) {
+  msg->type = SdepCommand;
+  msg->cmd_low = command & 0xff;
+  msg->cmd_high = command >> 8;
+  msg->len = len;
+  msg->more = (moredata && len == SdepMaxPayload) ? 1 : 0;
+
+  static_assert(sizeof(*msg) == 20, "msg is correctly packed");
+
+  memcpy(msg->payload, payload, len);
+}
+
+// Read a single SDEP packet
+static bool sdep_recv_pkt(struct sdep_msg *msg, uint16_t timeout) {
+  bool success = false;
+  uint16_t timerStart = timer_read();
+  bool ready = false;
+
+  do {
+    ready = digitalRead(AdafruitBleIRQPin);
+    if (ready) {
+      break;
+    }
+    _delay_us(1);
+  } while (timer_elapsed(timerStart) < timeout);
+
+  if (ready) {
+    SPI_begin(&spi);
+
+    digitalWrite(AdafruitBleCSPin, PinLevelLow);
+
+    do {
+      // Read the command type, waiting for the data to be ready
+      msg->type = spi_read_byte();
+      if (msg->type == SdepSlaveNotReady || msg->type == SdepSlaveOverflow) {
+        // Release it and let it initialize
+        digitalWrite(AdafruitBleCSPin, PinLevelHigh);
+        _delay_us(SdepBackOff);
+        digitalWrite(AdafruitBleCSPin, PinLevelLow);
+        continue;
+      }
+
+      // Read the rest of the header
+      spi_recv_bytes(&msg->cmd_low, sizeof(*msg) - (1 + sizeof(msg->payload)));
+
+      // and get the payload if there is any
+      if (msg->len <= SdepMaxPayload) {
+        spi_recv_bytes(msg->payload, msg->len);
+      }
+      success = true;
+      break;
+    } while (timer_elapsed(timerStart) < timeout);
+
+    digitalWrite(AdafruitBleCSPin, PinLevelHigh);
+  }
+  return success;
+}
+
+static void resp_buf_read_one(bool greedy) {
+  uint16_t last_send;
+  if (!resp_buf.peek(last_send)) {
+    return;
+  }
+
+  if (digitalRead(AdafruitBleIRQPin)) {
+    struct sdep_msg msg;
+
+again:
+    if (sdep_recv_pkt(&msg, SdepTimeout)) {
+      if (!msg.more) {
+        // We got it; consume this entry
+        resp_buf.get(last_send);
+        dprintf("recv latency %dms\n", TIMER_DIFF_16(timer_read(), last_send));
+      }
+
+      if (greedy && resp_buf.peek(last_send) && digitalRead(AdafruitBleIRQPin)) {
+        goto again;
+      }
+    }
+
+  } else if (timer_elapsed(last_send) > SdepTimeout * 2) {
+    dprintf("waiting_for_result: timeout, resp_buf size %d\n",
+            (int)resp_buf.size());
+
+    // Timed out: consume this entry
+    resp_buf.get(last_send);
+  }
+}
+
+static void send_buf_send_one(uint16_t timeout = SdepTimeout) {
+  struct queue_item item;
+
+  // Don't send anything more until we get an ACK
+  if (!resp_buf.empty()) {
+    return;
+  }
+
+  if (!send_buf.peek(item)) {
+    return;
+  }
+  if (process_queue_item(&item, timeout)) {
+    // commit that peek
+    send_buf.get(item);
+    dprintf("send_buf_send_one: have %d remaining\n", (int)send_buf.size());
+  } else {
+    dprint("failed to send, will retry\n");
+    _delay_ms(SdepTimeout);
+    resp_buf_read_one(true);
+  }
+}
+
+static void resp_buf_wait(const char *cmd) {
+  bool didPrint = false;
+  while (!resp_buf.empty()) {
+    if (!didPrint) {
+      dprintf("wait on buf for %s\n", cmd);
+      didPrint = true;
+    }
+    resp_buf_read_one(true);
+  }
+}
+
+static bool ble_init(void) {
+  state.initialized = false;
+  state.configured = false;
+  state.is_connected = false;
+
+  pinMode(AdafruitBleIRQPin, PinDirectionInput);
+  pinMode(AdafruitBleCSPin, PinDirectionOutput);
+  digitalWrite(AdafruitBleCSPin, PinLevelHigh);
+
+  SPI_init(&spi);
+
+  // Perform a hardware reset
+  pinMode(AdafruitBleResetPin, PinDirectionOutput);
+  digitalWrite(AdafruitBleResetPin, PinLevelHigh);
+  digitalWrite(AdafruitBleResetPin, PinLevelLow);
+  _delay_ms(10);
+  digitalWrite(AdafruitBleResetPin, PinLevelHigh);
+
+  _delay_ms(1000); // Give it a second to initialize
+
+  state.initialized = true;
+  return state.initialized;
+}
+
+static inline uint8_t min(uint8_t a, uint8_t b) {
+  return a < b ? a : b;
+}
+
+static bool read_response(char *resp, uint16_t resplen, bool verbose) {
+  char *dest = resp;
+  char *end = dest + resplen;
+
+  while (true) {
+    struct sdep_msg msg;
+
+    if (!sdep_recv_pkt(&msg, 2 * SdepTimeout)) {
+      dprint("sdep_recv_pkt failed\n");
+      return false;
+    }
+
+    if (msg.type != SdepResponse) {
+      *resp = 0;
+      return false;
+    }
+
+    uint8_t len = min(msg.len, end - dest);
+    if (len > 0) {
+      memcpy(dest, msg.payload, len);
+      dest += len;
+    }
+
+    if (!msg.more) {
+      // No more data is expected!
+      break;
+    }
+  }
+
+  // Ensure the response is NUL terminated
+  *dest = 0;
+
+  // "Parse" the result text; we want to snip off the trailing OK or ERROR line
+  // Rewind past the possible trailing CRLF so that we can strip it
+  --dest;
+  while (dest > resp && (dest[0] == '\n' || dest[0] == '\r')) {
+    *dest = 0;
+    --dest;
+  }
+
+  // Look back for start of preceeding line
+  char *last_line = strrchr(resp, '\n');
+  if (last_line) {
+    ++last_line;
+  } else {
+    last_line = resp;
+  }
+
+  bool success = false;
+  static const char kOK[] PROGMEM = "OK";
+
+  success = !strcmp_P(last_line, kOK );
+
+  if (verbose || !success) {
+    dprintf("result: %s\n", resp);
+  }
+  return success;
+}
+
+static bool at_command(const char *cmd, char *resp, uint16_t resplen,
+                       bool verbose, uint16_t timeout) {
+  const char *end = cmd + strlen(cmd);
+  struct sdep_msg msg;
+
+  if (verbose) {
+    dprintf("ble send: %s\n", cmd);
+  }
+
+  if (resp) {
+    // They want to decode the response, so we need to flush and wait
+    // for all pending I/O to finish before we start this one, so
+    // that we don't confuse the results
+    resp_buf_wait(cmd);
+    *resp = 0;
+  }
+
+  // Fragment the command into a series of SDEP packets
+  while (end - cmd > SdepMaxPayload) {
+    sdep_build_pkt(&msg, BleAtWrapper, (uint8_t *)cmd, SdepMaxPayload, true);
+    if (!sdep_send_pkt(&msg, timeout)) {
+      return false;
+    }
+    cmd += SdepMaxPayload;
+  }
+
+  sdep_build_pkt(&msg, BleAtWrapper, (uint8_t *)cmd, end - cmd, false);
+  if (!sdep_send_pkt(&msg, timeout)) {
+    return false;
+  }
+
+  if (resp == NULL) {
+    auto now = timer_read();
+    while (!resp_buf.enqueue(now)) {
+      resp_buf_read_one(false);
+    }
+    auto later = timer_read();
+    if (TIMER_DIFF_16(later, now) > 0) {
+      dprintf("waited %dms for resp_buf\n", TIMER_DIFF_16(later, now));
+    }
+    return true;
+  }
+
+  return read_response(resp, resplen, verbose);
+}
+
+bool at_command_P(const char *cmd, char *resp, uint16_t resplen, bool verbose) {
+  auto cmdbuf = (char *)alloca(strlen_P(cmd) + 1);
+  strcpy_P(cmdbuf, cmd);
+  return at_command(cmdbuf, resp, resplen, verbose);
+}
+
+bool adafruit_ble_is_connected(void) {
+  return state.is_connected;
+}
+
+bool adafruit_ble_enable_keyboard(void) {
+  char resbuf[128];
+
+  if (!state.initialized && !ble_init()) {
+    return false;
+  }
+
+  state.configured = false;
+
+  // Disable command echo
+  static const char kEcho[] PROGMEM = "ATE=0";
+  // Make the advertised name match the keyboard
+  static const char kGapDevName[] PROGMEM =
+      "AT+GAPDEVNAME=" STR(PRODUCT) " " STR(DESCRIPTION);
+  // Turn on keyboard support
+  static const char kHidEnOn[] PROGMEM = "AT+BLEHIDEN=1";
+
+  // Adjust intervals to improve latency.  This causes the "central"
+  // system (computer/tablet) to poll us every 10-30 ms.  We can't
+  // set a smaller value than 10ms, and 30ms seems to be the natural
+  // processing time on my macbook.  Keeping it constrained to that
+  // feels reasonable to type to.
+  static const char kGapIntervals[] PROGMEM = "AT+GAPINTERVALS=10,30,,";
+
+  // Reset the device so that it picks up the above changes
+  static const char kATZ[] PROGMEM = "ATZ";
+
+  // Turn down the power level a bit
+  static const char kPower[] PROGMEM = "AT+BLEPOWERLEVEL=-12";
+  static PGM_P const configure_commands[] PROGMEM = {
+    kEcho,
+    kGapIntervals,
+    kGapDevName,
+    kHidEnOn,
+    kPower,
+    kATZ,
+  };
+
+  uint8_t i;
+  for (i = 0; i < sizeof(configure_commands) / sizeof(configure_commands[0]);
+       ++i) {
+    PGM_P cmd;
+    memcpy_P(&cmd, configure_commands + i, sizeof(cmd));
+
+    if (!at_command_P(cmd, resbuf, sizeof(resbuf))) {
+      dprintf("failed BLE command: %S: %s\n", cmd, resbuf);
+      goto fail;
+    }
+  }
+
+  state.configured = true;
+
+  // Check connection status in a little while; allow the ATZ time
+  // to kick in.
+  state.last_connection_update = timer_read();
+fail:
+  return state.configured;
+}
+
+static void set_connected(bool connected) {
+  if (connected != state.is_connected) {
+    if (connected) {
+      print("****** BLE CONNECT!!!!\n");
+    } else {
+      print("****** BLE DISCONNECT!!!!\n");
+    }
+    state.is_connected = connected;
+
+    // TODO: if modifiers are down on the USB interface and
+    // we cut over to BLE or vice versa, they will remain stuck.
+    // This feels like a good point to do something like clearing
+    // the keyboard and/or generating a fake all keys up message.
+    // However, I've noticed that it takes a couple of seconds
+    // for macOS to to start recognizing key presses after BLE
+    // is in the connected state, so I worry that doing that
+    // here may not be good enough.
+  }
+}
+
+void adafruit_ble_task(void) {
+  char resbuf[48];
+
+  if (!state.configured && !adafruit_ble_enable_keyboard()) {
+    return;
+  }
+  resp_buf_read_one(true);
+  send_buf_send_one(SdepShortTimeout);
+
+  if (resp_buf.empty() && (state.event_flags & UsingEvents) &&
+      digitalRead(AdafruitBleIRQPin)) {
+    // Must be an event update
+    if (at_command_P(PSTR("AT+EVENTSTATUS"), resbuf, sizeof(resbuf))) {
+      uint32_t mask = strtoul(resbuf, NULL, 16);
+
+      if (mask & BleSystemConnected) {
+        set_connected(true);
+      } else if (mask & BleSystemDisconnected) {
+        set_connected(false);
+      }
+    }
+  }
+
+  if (timer_elapsed(state.last_connection_update) > ConnectionUpdateInterval) {
+    bool shouldPoll = true;
+    if (!(state.event_flags & ProbedEvents)) {
+      // Request notifications about connection status changes.
+      // This only works in SPIFRIEND firmware > 0.6.7, which is why
+      // we check for this conditionally here.
+      // Note that at the time of writing, HID reports only work correctly
+      // with Apple products on firmware version 0.6.7!
+      // https://forums.adafruit.com/viewtopic.php?f=8&t=104052
+      if (at_command_P(PSTR("AT+EVENTENABLE=0x1"), resbuf, sizeof(resbuf))) {
+        at_command_P(PSTR("AT+EVENTENABLE=0x2"), resbuf, sizeof(resbuf));
+        state.event_flags |= UsingEvents;
+      }
+      state.event_flags |= ProbedEvents;
+
+      // leave shouldPoll == true so that we check at least once
+      // before relying solely on events
+    } else {
+      shouldPoll = false;
+    }
+
+    static const char kGetConn[] PROGMEM = "AT+GAPGETCONN";
+    state.last_connection_update = timer_read();
+
+    if (at_command_P(kGetConn, resbuf, sizeof(resbuf))) {
+      set_connected(atoi(resbuf));
+    }
+  }
+
+#ifdef SAMPLE_BATTERY
+  // I don't know if this really does anything useful yet; the reported
+  // voltage level always seems to be around 3200mV.  We may want to just rip
+  // this code out.
+  if (timer_elapsed(state.last_battery_update) > BatteryUpdateInterval &&
+      resp_buf.empty()) {
+    state.last_battery_update = timer_read();
+
+    if (at_command_P(PSTR("AT+HWVBAT"), resbuf, sizeof(resbuf))) {
+      state.vbat = atoi(resbuf);
+    }
+  }
+#endif
+}
+
+static bool process_queue_item(struct queue_item *item, uint16_t timeout) {
+  char cmdbuf[48];
+  char fmtbuf[64];
+
+  // Arrange to re-check connection after keys have settled
+  state.last_connection_update = timer_read();
+
+#if 1
+  if (TIMER_DIFF_16(state.last_connection_update, item->added) > 0) {
+    dprintf("send latency %dms\n",
+            TIMER_DIFF_16(state.last_connection_update, item->added));
+  }
+#endif
+
+  switch (item->queue_type) {
+    case QTKeyReport:
+      strcpy_P(fmtbuf,
+          PSTR("AT+BLEKEYBOARDCODE=%02x-00-%02x-%02x-%02x-%02x-%02x-%02x"));
+      snprintf(cmdbuf, sizeof(cmdbuf), fmtbuf, item->key.modifier,
+               item->key.keys[0], item->key.keys[1], item->key.keys[2],
+               item->key.keys[3], item->key.keys[4], item->key.keys[5]);
+      return at_command(cmdbuf, NULL, 0, true, timeout);
+
+    case QTConsumer:
+      strcpy_P(fmtbuf, PSTR("AT+BLEHIDCONTROLKEY=0x%04x"));
+      snprintf(cmdbuf, sizeof(cmdbuf), fmtbuf, item->consumer);
+      return at_command(cmdbuf, NULL, 0, true, timeout);
+
+#ifdef MOUSE_ENABLE
+    case QTMouseMove:
+      strcpy_P(fmtbuf, PSTR("AT+BLEHIDMOUSEMOVE=%d,%d,%d,%d"));
+      snprintf(cmdbuf, sizeof(cmdbuf), fmtbuf, item->mousemove.x,
+          item->mousemove.y, item->mousemove.scroll, item->mousemove.pan);
+      return at_command(cmdbuf, NULL, 0, true, timeout);
+#endif
+    default:
+      return true;
+  }
+}
+
+bool adafruit_ble_send_keys(uint8_t hid_modifier_mask, uint8_t *keys,
+                            uint8_t nkeys) {
+  struct queue_item item;
+  bool didWait = false;
+
+  item.queue_type = QTKeyReport;
+  item.key.modifier = hid_modifier_mask;
+  item.added = timer_read();
+
+  while (nkeys >= 0) {
+    item.key.keys[0] = keys[0];
+    item.key.keys[1] = nkeys >= 1 ? keys[1] : 0;
+    item.key.keys[2] = nkeys >= 2 ? keys[2] : 0;
+    item.key.keys[3] = nkeys >= 3 ? keys[3] : 0;
+    item.key.keys[4] = nkeys >= 4 ? keys[4] : 0;
+    item.key.keys[5] = nkeys >= 5 ? keys[5] : 0;
+
+    if (!send_buf.enqueue(item)) {
+      if (!didWait) {
+        dprint("wait for buf space\n");
+        didWait = true;
+      }
+      send_buf_send_one();
+      continue;
+    }
+
+    if (nkeys <= 6) {
+      return true;
+    }
+
+    nkeys -= 6;
+    keys += 6;
+  }
+
+  return true;
+}
+
+bool adafruit_ble_send_consumer_key(uint16_t keycode, int hold_duration) {
+  struct queue_item item;
+
+  item.queue_type = QTConsumer;
+  item.consumer = keycode;
+
+  while (!send_buf.enqueue(item)) {
+    send_buf_send_one();
+  }
+  return true;
+}
+
+#ifdef MOUSE_ENABLE
+bool adafruit_ble_send_mouse_move(int8_t x, int8_t y, int8_t scroll,
+                                  int8_t pan) {
+  struct queue_item item;
+
+  item.queue_type = QTMouseMove;
+  item.mousemove.x = x;
+  item.mousemove.y = y;
+  item.mousemove.scroll = scroll;
+  item.mousemove.pan = pan;
+
+  while (!send_buf.enqueue(item)) {
+    send_buf_send_one();
+  }
+  return true;
+}
+#endif
+
+uint32_t adafruit_ble_read_battery_voltage(void) {
+  return state.vbat;
+}
+
+bool adafruit_ble_set_mode_leds(bool on) {
+  if (!state.configured) {
+    return false;
+  }
+
+  // The "mode" led is the red blinky one
+  at_command_P(on ? PSTR("AT+HWMODELED=1") : PSTR("AT+HWMODELED=0"), NULL, 0);
+
+  // Pin 19 is the blue "connected" LED; turn that off too.
+  // When turning LEDs back on, don't turn that LED on if we're
+  // not connected, as that would be confusing.
+  at_command_P(on && state.is_connected ? PSTR("AT+HWGPIO=19,1")
+                                        : PSTR("AT+HWGPIO=19,0"),
+               NULL, 0);
+  return true;
+}
+
+// https://learn.adafruit.com/adafruit-feather-32u4-bluefruit-le/ble-generic#at-plus-blepowerlevel
+bool adafruit_ble_set_power_level(int8_t level) {
+  char cmd[46];
+  if (!state.configured) {
+    return false;
+  }
+  snprintf(cmd, sizeof(cmd), "AT+BLEPOWERLEVEL=%d", level);
+  return at_command(cmd, NULL, 0, false);
+}
diff --git a/tmk_core/protocol/lufa/adafruit_ble.h b/tmk_core/protocol/lufa/adafruit_ble.h
new file mode 100644
index 000000000..351fd55ae
--- /dev/null
+++ b/tmk_core/protocol/lufa/adafruit_ble.h
@@ -0,0 +1,60 @@
+/* Bluetooth Low Energy Protocol for QMK.
+ * Author: Wez Furlong, 2016
+ * Supports the Adafruit BLE board built around the nRF51822 chip.
+ */
+#pragma once
+#ifdef ADAFRUIT_BLE_ENABLE
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Instruct the module to enable HID keyboard support and reset */
+extern bool adafruit_ble_enable_keyboard(void);
+
+/* Query to see if the BLE module is connected */
+extern bool adafruit_ble_query_is_connected(void);
+
+/* Returns true if we believe that the BLE module is connected.
+ * This uses our cached understanding that is maintained by
+ * calling ble_task() periodically. */
+extern bool adafruit_ble_is_connected(void);
+
+/* Call this periodically to process BLE-originated things */
+extern void adafruit_ble_task(void);
+
+/* Generates keypress events for a set of keys.
+ * The hid modifier mask specifies the state of the modifier keys for
+ * this set of keys.
+ * Also sends a key release indicator, so that the keys do not remain
+ * held down. */
+extern bool adafruit_ble_send_keys(uint8_t hid_modifier_mask, uint8_t *keys,
+                                   uint8_t nkeys);
+
+/* Send a consumer keycode, holding it down for the specified duration
+ * (milliseconds) */
+extern bool adafruit_ble_send_consumer_key(uint16_t keycode, int hold_duration);
+
+#ifdef MOUSE_ENABLE
+/* Send a mouse/wheel movement report.
+ * The parameters are signed and indicate positive of negative direction
+ * change. */
+extern bool adafruit_ble_send_mouse_move(int8_t x, int8_t y, int8_t scroll,
+                                         int8_t pan);
+#endif
+
+/* Compute battery voltage by reading an analog pin.
+ * Returns the integer number of millivolts */
+extern uint32_t adafruit_ble_read_battery_voltage(void);
+
+extern bool adafruit_ble_set_mode_leds(bool on);
+extern bool adafruit_ble_set_power_level(int8_t level);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // ADAFRUIT_BLE_ENABLE
diff --git a/tmk_core/protocol/lufa/descriptor.c b/tmk_core/protocol/lufa/descriptor.c
index 6f2407f58..bf47787d2 100644
--- a/tmk_core/protocol/lufa/descriptor.c
+++ b/tmk_core/protocol/lufa/descriptor.c
@@ -164,6 +164,28 @@ const USB_Descriptor_HIDReport_Datatype_t PROGMEM ExtrakeyReport[] =
 };
 #endif
 
+#ifdef RAW_ENABLE
+const USB_Descriptor_HIDReport_Datatype_t PROGMEM RawReport[] =
+{
+    HID_RI_USAGE_PAGE(16, 0xFF60), /* Vendor Page 0xFF60 */
+    HID_RI_USAGE(8, 0x61), /* Vendor Usage 0x61 */
+    HID_RI_COLLECTION(8, 0x01), /* Application */
+        HID_RI_USAGE(8, 0x62), /* Vendor Usage 0x62 */
+        HID_RI_LOGICAL_MINIMUM(8, 0x00),
+        HID_RI_LOGICAL_MAXIMUM(16, 0x00FF),
+        HID_RI_REPORT_COUNT(8, RAW_EPSIZE),
+        HID_RI_REPORT_SIZE(8, 0x08),
+        HID_RI_INPUT(8, HID_IOF_DATA | HID_IOF_VARIABLE | HID_IOF_ABSOLUTE),
+        HID_RI_USAGE(8, 0x63), /* Vendor Usage 0x63 */
+        HID_RI_LOGICAL_MINIMUM(8, 0x00),
+        HID_RI_LOGICAL_MAXIMUM(16, 0x00FF),
+        HID_RI_REPORT_COUNT(8, RAW_EPSIZE),
+        HID_RI_REPORT_SIZE(8, 0x08),
+        HID_RI_OUTPUT(8, HID_IOF_DATA | HID_IOF_VARIABLE | HID_IOF_ABSOLUTE | HID_IOF_NON_VOLATILE),
+    HID_RI_END_COLLECTION(0),
+};
+#endif
+
 #ifdef CONSOLE_ENABLE
 const USB_Descriptor_HIDReport_Datatype_t PROGMEM ConsoleReport[] =
 {
@@ -399,6 +421,58 @@ const USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor =
         },
 #endif
 
+                /*
+             * Raw
+             */
+        #ifdef RAW_ENABLE
+            .Raw_Interface =
+                {
+                    .Header                 = {.Size = sizeof(USB_Descriptor_Interface_t), .Type = DTYPE_Interface},
+
+                    .InterfaceNumber        = RAW_INTERFACE,
+                    .AlternateSetting       = 0x00,
+
+                    .TotalEndpoints         = 2,
+
+                    .Class                  = HID_CSCP_HIDClass,
+                    .SubClass               = HID_CSCP_NonBootSubclass,
+                    .Protocol               = HID_CSCP_NonBootProtocol,
+
+                    .InterfaceStrIndex      = NO_DESCRIPTOR
+                },
+
+            .Raw_HID =
+                {
+                    .Header                 = {.Size = sizeof(USB_HID_Descriptor_HID_t), .Type = HID_DTYPE_HID},
+
+                    .HIDSpec                = VERSION_BCD(1,1,1),
+                    .CountryCode            = 0x00,
+                    .TotalReportDescriptors = 1,
+                    .HIDReportType          = HID_DTYPE_Report,
+                    .HIDReportLength        = sizeof(RawReport)
+                },
+
+            .Raw_INEndpoint =
+                {
+                    .Header                 = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint},
+
+                    .EndpointAddress        = (ENDPOINT_DIR_IN | RAW_IN_EPNUM),
+                    .Attributes             = (EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA),
+                    .EndpointSize           = RAW_EPSIZE,
+                    .PollingIntervalMS      = 0x01
+                },
+
+            .Raw_OUTEndpoint =
+                {
+                    .Header                 = {.Size = sizeof(USB_Descriptor_Endpoint_t), .Type = DTYPE_Endpoint},
+
+                    .EndpointAddress        = (ENDPOINT_DIR_OUT | RAW_OUT_EPNUM),
+                    .Attributes             = (EP_TYPE_INTERRUPT | ENDPOINT_ATTR_NO_SYNC | ENDPOINT_USAGE_DATA),
+                    .EndpointSize           = RAW_EPSIZE,
+                    .PollingIntervalMS      = 0x01
+                },
+        #endif
+
     /*
      * Console
      */
@@ -754,7 +828,6 @@ const USB_Descriptor_Configuration_t PROGMEM ConfigurationDescriptor =
                     .PollingIntervalMS      = 0x05
             },
 #endif
-
 };
 
 
@@ -846,6 +919,12 @@ uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue,
                 Size    = sizeof(USB_HID_Descriptor_HID_t);
                 break;
 #endif
+#ifdef RAW_ENABLE
+            case RAW_INTERFACE:
+                Address = &ConfigurationDescriptor.Raw_HID;
+                Size    = sizeof(USB_HID_Descriptor_HID_t);
+                break;
+#endif
 #ifdef CONSOLE_ENABLE
             case CONSOLE_INTERFACE:
                 Address = &ConfigurationDescriptor.Console_HID;
@@ -878,6 +957,12 @@ uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue,
                 Size    = sizeof(ExtrakeyReport);
                 break;
 #endif
+#ifdef RAW_ENABLE
+            case RAW_INTERFACE:
+                Address = &RawReport;
+                Size    = sizeof(RawReport);
+                break;
+#endif
 #ifdef CONSOLE_ENABLE
             case CONSOLE_INTERFACE:
                 Address = &ConsoleReport;
diff --git a/tmk_core/protocol/lufa/descriptor.h b/tmk_core/protocol/lufa/descriptor.h
index c6c94e361..24ce420e6 100644
--- a/tmk_core/protocol/lufa/descriptor.h
+++ b/tmk_core/protocol/lufa/descriptor.h
@@ -71,6 +71,14 @@ typedef struct
     USB_Descriptor_Endpoint_t             Extrakey_INEndpoint;
 #endif
 
+#ifdef RAW_ENABLE
+    // Raw HID Interface
+    USB_Descriptor_Interface_t            Raw_Interface;
+    USB_HID_Descriptor_HID_t              Raw_HID;
+    USB_Descriptor_Endpoint_t             Raw_INEndpoint;
+    USB_Descriptor_Endpoint_t             Raw_OUTEndpoint;
+#endif
+
 #ifdef CONSOLE_ENABLE
     // Console HID Interface
     USB_Descriptor_Interface_t            Console_Interface;
@@ -137,10 +145,16 @@ typedef struct
 #   define EXTRAKEY_INTERFACE       MOUSE_INTERFACE
 #endif
 
+#ifdef RAW_ENABLE
+#   define RAW_INTERFACE                (EXTRAKEY_INTERFACE + 1)
+#else
+#   define RAW_INTERFACE                EXTRAKEY_INTERFACE
+#endif
+
 #ifdef CONSOLE_ENABLE
-#   define CONSOLE_INTERFACE        (EXTRAKEY_INTERFACE + 1)
+#   define CONSOLE_INTERFACE        (RAW_INTERFACE + 1)
 #else
-#   define CONSOLE_INTERFACE        EXTRAKEY_INTERFACE
+#   define CONSOLE_INTERFACE        RAW_INTERFACE
 #endif
 
 #ifdef NKRO_ENABLE
@@ -182,12 +196,19 @@ typedef struct
 #   define EXTRAKEY_IN_EPNUM        MOUSE_IN_EPNUM
 #endif
 
+#ifdef RAW_ENABLE
+#   define RAW_IN_EPNUM         (EXTRAKEY_IN_EPNUM + 1)
+#   define RAW_OUT_EPNUM        (EXTRAKEY_IN_EPNUM + 2)
+#else
+#   define RAW_OUT_EPNUM        EXTRAKEY_IN_EPNUM
+#endif
+
 #ifdef CONSOLE_ENABLE
-#   define CONSOLE_IN_EPNUM         (EXTRAKEY_IN_EPNUM + 1)
-#   define CONSOLE_OUT_EPNUM        (EXTRAKEY_IN_EPNUM + 1)
-//#   define CONSOLE_OUT_EPNUM        (EXTRAKEY_IN_EPNUM + 2)
+#   define CONSOLE_IN_EPNUM         (RAW_OUT_EPNUM + 1)
+//#   define CONSOLE_OUT_EPNUM        (RAW_OUT_EPNUM + 2)
+#   define CONSOLE_OUT_EPNUM        (RAW_OUT_EPNUM + 1)
 #else
-#   define CONSOLE_OUT_EPNUM        EXTRAKEY_IN_EPNUM
+#   define CONSOLE_OUT_EPNUM        RAW_OUT_EPNUM
 #endif
 
 #ifdef NKRO_ENABLE
@@ -217,7 +238,6 @@ typedef struct
 #   define CDC_OUT_EPNUM        MIDI_STREAM_OUT_EPNUM
 #endif
 
-
 #if defined(__AVR_ATmega32U2__) && CDC_OUT_EPNUM > 4
 # error "Endpoints are not available enough to support all functions. Remove some in Makefile.(MOUSEKEY, EXTRAKEY, CONSOLE, NKRO, MIDI, SERIAL)"
 #endif
@@ -225,6 +245,7 @@ typedef struct
 #define KEYBOARD_EPSIZE             8
 #define MOUSE_EPSIZE                8
 #define EXTRAKEY_EPSIZE             8
+#define RAW_EPSIZE                      32
 #define CONSOLE_EPSIZE              32
 #define NKRO_EPSIZE                 32
 #define MIDI_STREAM_EPSIZE          64
diff --git a/tmk_core/protocol/lufa/lufa.c b/tmk_core/protocol/lufa/lufa.c
index 01c0e45b0..dd78fe621 100644
--- a/tmk_core/protocol/lufa/lufa.c
+++ b/tmk_core/protocol/lufa/lufa.c
@@ -51,6 +51,8 @@
 
 #include "descriptor.h"
 #include "lufa.h"
+#include "quantum.h"
+#include <util/atomic.h>
 
 #ifdef NKRO_ENABLE
   #include "keycode_config.h"
@@ -66,11 +68,26 @@
 #ifdef BLUETOOTH_ENABLE
     #include "bluetooth.h"
 #endif
+#ifdef ADAFRUIT_BLE_ENABLE
+    #include "adafruit_ble.h"
+#endif
 
 #ifdef VIRTSER_ENABLE
     #include "virtser.h"
 #endif
 
+#if (defined(RGB_MIDI) | defined(RGBLIGHT_ANIMATIONS)) & defined(RGBLIGHT_ENABLE)
+    #include "rgblight.h"
+#endif
+
+#ifdef MIDI_ENABLE
+  #include "sysex_tools.h"
+#endif
+
+#ifdef RAW_ENABLE
+        #include "raw_hid.h"
+#endif
+
 uint8_t keyboard_idle = 0;
 /* 0: Boot Protocol, 1: Report Protocol(default) */
 uint8_t keyboard_protocol = 1;
@@ -79,9 +96,9 @@ static uint8_t keyboard_led_stats = 0;
 static report_keyboard_t keyboard_report_sent;
 
 #ifdef MIDI_ENABLE
-void usb_send_func(MidiDevice * device, uint16_t cnt, uint8_t byte0, uint8_t byte1, uint8_t byte2);
-void usb_get_midi(MidiDevice * device);
-void midi_usb_init(MidiDevice * device);
+static void usb_send_func(MidiDevice * device, uint16_t cnt, uint8_t byte0, uint8_t byte1, uint8_t byte2);
+static void usb_get_midi(MidiDevice * device);
+static void midi_usb_init(MidiDevice * device);
 #endif
 
 /* Host driver */
@@ -166,6 +183,80 @@ USB_ClassInfo_CDC_Device_t cdc_device =
 };
 #endif
 
+#ifdef RAW_ENABLE
+
+void raw_hid_send( uint8_t *data, uint8_t length )
+{
+        // TODO: implement variable size packet
+        if ( length != RAW_EPSIZE )
+        {
+                return;
+        }
+
+        if (USB_DeviceState != DEVICE_STATE_Configured)
+        {
+                return;
+        }
+
+        // TODO: decide if we allow calls to raw_hid_send() in the middle
+        // of other endpoint usage.
+        uint8_t ep = Endpoint_GetCurrentEndpoint();
+
+        Endpoint_SelectEndpoint(RAW_IN_EPNUM);
+
+        // Check to see if the host is ready to accept another packet
+        if (Endpoint_IsINReady())
+        {
+                // Write data
+                Endpoint_Write_Stream_LE(data, RAW_EPSIZE, NULL);
+                // Finalize the stream transfer to send the last packet
+                Endpoint_ClearIN();
+        }
+
+        Endpoint_SelectEndpoint(ep);
+}
+
+__attribute__ ((weak))
+void raw_hid_receive( uint8_t *data, uint8_t length )
+{
+        // Users should #include "raw_hid.h" in their own code
+        // and implement this function there. Leave this as weak linkage
+        // so users can opt to not handle data coming in.
+}
+
+static void raw_hid_task(void)
+{
+        // Create a temporary buffer to hold the read in data from the host
+        uint8_t data[RAW_EPSIZE];
+        bool data_read = false;
+
+        // Device must be connected and configured for the task to run
+        if (USB_DeviceState != DEVICE_STATE_Configured)
+        return;
+
+        Endpoint_SelectEndpoint(RAW_OUT_EPNUM);
+
+        // Check to see if a packet has been sent from the host
+        if (Endpoint_IsOUTReceived())
+        {
+                // Check to see if the packet contains data
+                if (Endpoint_IsReadWriteAllowed())
+                {
+                        /* Read data */
+                        Endpoint_Read_Stream_LE(data, sizeof(data), NULL);
+                        data_read = true;
+                }
+
+                // Finalize the stream transfer to receive the last packet
+                Endpoint_ClearOUT();
+
+                if ( data_read )
+                {
+                        raw_hid_receive( data, sizeof(data) );
+                }
+        }
+}
+#endif
 
 /*******************************************************************************
  * Console
@@ -285,10 +376,14 @@ void EVENT_USB_Device_WakeUp()
 #endif
 }
 
+
+
 #ifdef CONSOLE_ENABLE
 static bool console_flush = false;
 #define CONSOLE_FLUSH_SET(b)   do { \
-    uint8_t sreg = SREG; cli(); console_flush = b; SREG = sreg; \
+  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {\
+    console_flush = b; \
+  } \
 } while (0)
 
 // called every 1ms
@@ -302,6 +397,7 @@ void EVENT_USB_Device_StartOfFrame(void)
     Console_Task();
     console_flush = false;
 }
+
 #endif
 
 /** Event handler for the USB_ConfigurationChanged event.
@@ -330,6 +426,14 @@ void EVENT_USB_Device_ConfigurationChanged(void)
                                      EXTRAKEY_EPSIZE, ENDPOINT_BANK_SINGLE);
 #endif
 
+#ifdef RAW_ENABLE
+    /* Setup Raw HID Report Endpoints */
+    ConfigSuccess &= ENDPOINT_CONFIG(RAW_IN_EPNUM, EP_TYPE_INTERRUPT, ENDPOINT_DIR_IN,
+                                                                         RAW_EPSIZE, ENDPOINT_BANK_SINGLE);
+    ConfigSuccess &= ENDPOINT_CONFIG(RAW_OUT_EPNUM, EP_TYPE_INTERRUPT, ENDPOINT_DIR_OUT,
+                                                                         RAW_EPSIZE, ENDPOINT_BANK_SINGLE);
+#endif
+
 #ifdef CONSOLE_ENABLE
     /* Setup Console HID Report Endpoints */
     ConfigSuccess &= ENDPOINT_CONFIG(CONSOLE_IN_EPNUM, EP_TYPE_INTERRUPT, ENDPOINT_DIR_IN,
@@ -492,9 +596,35 @@ static uint8_t keyboard_leds(void)
     return keyboard_led_stats;
 }
 
+#define SendToUSB 1
+#define SendToBT  2
+#define SendToBLE 4
+
+static inline uint8_t where_to_send(void) {
+#ifdef ADAFRUIT_BLE_ENABLE
+#if 0
+  if (adafruit_ble_is_connected()) {
+    // For testing, send to BLE as a priority
+    return SendToBLE;
+  }
+#endif
+
+  // This is the real policy
+  if (USB_DeviceState != DEVICE_STATE_Configured) {
+    if (adafruit_ble_is_connected()) {
+      return SendToBLE;
+    }
+  }
+#endif
+  return ((USB_DeviceState == DEVICE_STATE_Configured) ? SendToUSB : 0)
+#ifdef BLUETOOTH_ENABLE
+    || SendToBT
+#endif
+    ;
+}
+
 static void send_keyboard(report_keyboard_t *report)
 {
-
 #ifdef BLUETOOTH_ENABLE
     bluefruit_serial_send(0xFD);
     for (uint8_t i = 0; i < KEYBOARD_EPSIZE; i++) {
@@ -503,9 +633,17 @@ static void send_keyboard(report_keyboard_t *report)
 #endif
 
     uint8_t timeout = 255;
+    uint8_t where = where_to_send();
 
-    if (USB_DeviceState != DEVICE_STATE_Configured)
-        return;
+#ifdef ADAFRUIT_BLE_ENABLE
+    if (where & SendToBLE) {
+      adafruit_ble_send_keys(report->mods, report->keys, sizeof(report->keys));
+    }
+#endif
+
+    if (!(where & SendToUSB)) {
+      return;
+    }
 
     /* Select the Keyboard Report Endpoint */
 #ifdef NKRO_ENABLE
@@ -558,8 +696,17 @@ static void send_mouse(report_mouse_t *report)
 
     uint8_t timeout = 255;
 
-    if (USB_DeviceState != DEVICE_STATE_Configured)
-        return;
+    uint8_t where = where_to_send();
+
+#ifdef ADAFRUIT_BLE_ENABLE
+    if (where & SendToBLE) {
+      // FIXME: mouse buttons
+      adafruit_ble_send_mouse_move(report->x, report->y, report->v, report->h);
+    }
+#endif
+    if (!(where & SendToUSB)) {
+      return;
+    }
 
     /* Select the Mouse Report Endpoint */
     Endpoint_SelectEndpoint(MOUSE_IN_EPNUM);
@@ -617,9 +764,16 @@ static void send_consumer(uint16_t data)
 #endif
 
     uint8_t timeout = 255;
+    uint8_t where = where_to_send();
 
-    if (USB_DeviceState != DEVICE_STATE_Configured)
-        return;
+#ifdef ADAFRUIT_BLE_ENABLE
+    if (where & SendToBLE) {
+      adafruit_ble_send_consumer_key(data, 0);
+    }
+#endif
+    if (!(where & SendToUSB)) {
+      return;
+    }
 
     report_extra_t r = {
         .report_id = REPORT_ID_CONSUMER,
@@ -709,7 +863,7 @@ int8_t sendchar(uint8_t c)
  ******************************************************************************/
 
 #ifdef MIDI_ENABLE
-void usb_send_func(MidiDevice * device, uint16_t cnt, uint8_t byte0, uint8_t byte1, uint8_t byte2) {
+static void usb_send_func(MidiDevice * device, uint16_t cnt, uint8_t byte0, uint8_t byte1, uint8_t byte2) {
   MIDI_EventPacket_t event;
   event.Data1 = byte0;
   event.Data2 = byte1;
@@ -769,7 +923,7 @@ void usb_send_func(MidiDevice * device, uint16_t cnt, uint8_t byte0, uint8_t byt
   USB_USBTask();
 }
 
-void usb_get_midi(MidiDevice * device) {
+static void usb_get_midi(MidiDevice * device) {
   MIDI_EventPacket_t event;
   while (MIDI_Device_ReceiveEventPacket(&USB_MIDI_Interface, &event)) {
 
@@ -799,12 +953,12 @@ void usb_get_midi(MidiDevice * device) {
   USB_USBTask();
 }
 
-void midi_usb_init(MidiDevice * device){
+static void midi_usb_init(MidiDevice * device){
   midi_device_init(device);
   midi_device_set_send_func(device, usb_send_func);
   midi_device_set_pre_input_process_func(device, usb_get_midi);
 
-  SetupHardware();
+  // SetupHardware();
   sei();
 }
 
@@ -1029,7 +1183,7 @@ int main(void)
 
     print("Keyboard start.\n");
     while (1) {
-        #ifndef BLUETOOTH_ENABLE
+        #if !defined(BLUETOOTH_ENABLE) && !defined(ADAFRUIT_BLE_ENABLE)
         while (USB_DeviceState == DEVICE_STATE_Suspended) {
             print("[s]");
             suspend_power_down();
@@ -1039,20 +1193,34 @@ int main(void)
         }
         #endif
 
+        keyboard_task();
+
 #ifdef MIDI_ENABLE
         midi_device_process(&midi_device);
         // MIDI_Task();
 #endif
-        keyboard_task();
+
+#if defined(RGBLIGHT_ANIMATIONS) & defined(RGBLIGHT_ENABLE)
+        rgblight_task();
+#endif
+
+#ifdef ADAFRUIT_BLE_ENABLE
+        adafruit_ble_task();
+#endif
 
 #ifdef VIRTSER_ENABLE
         virtser_task();
         CDC_Device_USBTask(&cdc_device);
 #endif
 
+#ifdef RAW_ENABLE
+        raw_hid_task();
+#endif
+
 #if !defined(INTERRUPT_CONTROL_ENDPOINT)
         USB_USBTask();
 #endif
+
     }
 }
 
@@ -1077,15 +1245,38 @@ void fallthrough_callback(MidiDevice * device,
 #endif
 }
 
+
 void cc_callback(MidiDevice * device,
     uint8_t chan, uint8_t num, uint8_t val) {
   //sending it back on the next channel
-  midi_send_cc(device, (chan + 1) % 16, num, val);
+  // midi_send_cc(device, (chan + 1) % 16, num, val);
 }
 
-void sysex_callback(MidiDevice * device,
-    uint16_t start, uint8_t length, uint8_t * data) {
-  for (int i = 0; i < length; i++)
-    midi_send_cc(device, 15, 0x7F & data[i], 0x7F & (start + i));
+uint8_t midi_buffer[MIDI_SYSEX_BUFFER] = {0};
+
+void sysex_callback(MidiDevice * device, uint16_t start, uint8_t length, uint8_t * data) {
+    #ifdef API_SYSEX_ENABLE
+        // SEND_STRING("\n");
+        // send_word(start);
+        // SEND_STRING(": ");
+        for (uint8_t place = 0; place < length; place++) {
+            // send_byte(*data);
+            midi_buffer[start + place] = *data;
+            if (*data == 0xF7) {
+                // SEND_STRING("\nRD: ");
+                // for (uint8_t i = 0; i < start + place + 1; i++){
+                //     send_byte(midi_buffer[i]);
+                // SEND_STRING(" ");
+                // }
+                uint8_t * decoded = malloc(sizeof(uint8_t) * (sysex_decoded_length(start + place - 4)));
+                uint16_t decode_length = sysex_decode(decoded, midi_buffer + 4, start + place - 4);
+                process_api(decode_length, decoded);
+            }
+            // SEND_STRING(" ");
+            data++;
+        }
+    #endif
 }
+
+
 #endif
diff --git a/tmk_core/protocol/lufa/lufa.h b/tmk_core/protocol/lufa/lufa.h
index aad08d640..b11854101 100644
--- a/tmk_core/protocol/lufa/lufa.h
+++ b/tmk_core/protocol/lufa/lufa.h
@@ -68,8 +68,17 @@ typedef struct {
 } __attribute__ ((packed)) report_extra_t;
 
 #ifdef MIDI_ENABLE
-void MIDI_Task(void);
-MidiDevice midi_device;
+  void MIDI_Task(void);
+  MidiDevice midi_device;
+  #define MIDI_SYSEX_BUFFER 32 
+#endif
+
+#ifdef API_ENABLE
+  #include "api.h"
+#endif
+
+#ifdef API_SYSEX_ENABLE
+  #include "api_sysex.h"
 #endif
 
 // #if LUFA_VERSION_INTEGER < 0x120730
diff --git a/tmk_core/protocol/lufa/ringbuffer.hpp b/tmk_core/protocol/lufa/ringbuffer.hpp
new file mode 100644
index 000000000..70a3c4881
--- /dev/null
+++ b/tmk_core/protocol/lufa/ringbuffer.hpp
@@ -0,0 +1,66 @@
+#pragma once
+// A simple ringbuffer holding Size elements of type T
+template <typename T, uint8_t Size>
+class RingBuffer {
+ protected:
+  T buf_[Size];
+  uint8_t head_{0}, tail_{0};
+ public:
+  inline uint8_t nextPosition(uint8_t position) {
+    return (position + 1) % Size;
+  }
+
+  inline uint8_t prevPosition(uint8_t position) {
+    if (position == 0) {
+      return Size - 1;
+    }
+    return position - 1;
+  }
+
+  inline bool enqueue(const T &item) {
+    static_assert(Size > 1, "RingBuffer size must be > 1");
+    uint8_t next = nextPosition(head_);
+    if (next == tail_) {
+      // Full
+      return false;
+    }
+
+    buf_[head_] = item;
+    head_ = next;
+    return true;
+  }
+
+  inline bool get(T &dest, bool commit = true) {
+    auto tail = tail_;
+    if (tail == head_) {
+      // No more data
+      return false;
+    }
+
+    dest = buf_[tail];
+    tail = nextPosition(tail);
+
+    if (commit) {
+      tail_ = tail;
+    }
+    return true;
+  }
+
+  inline bool empty() const { return head_ == tail_; }
+
+  inline uint8_t size() const {
+    int diff = head_ - tail_;
+    if (diff >= 0) {
+      return diff;
+    }
+    return Size + diff;
+  }
+
+  inline T& front() {
+    return buf_[tail_];
+  }
+
+  inline bool peek(T &item) {
+    return get(item, false);
+  }
+};
diff --git a/tmk_core/protocol/midi.mk b/tmk_core/protocol/midi.mk
index c85ae42ff..4855b23d3 100644
--- a/tmk_core/protocol/midi.mk
+++ b/tmk_core/protocol/midi.mk
@@ -4,6 +4,7 @@ SRC += midi.c \
            midi_device.c \
            bytequeue/bytequeue.c \
            bytequeue/interrupt_setting.c \
+           sysex_tools.c \
            $(LUFA_SRC_USBCLASS)
 
 VPATH += $(TMK_PATH)/$(MIDI_DIR)
\ No newline at end of file
diff --git a/tmk_core/protocol/ps2_mouse.c b/tmk_core/protocol/ps2_mouse.c
index c3e8b3c1c..d9ccbecb4 100644
--- a/tmk_core/protocol/ps2_mouse.c
+++ b/tmk_core/protocol/ps2_mouse.c
@@ -18,60 +18,59 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include <stdbool.h>
 #include<avr/io.h>
 #include<util/delay.h>
-#include "ps2.h"
 #include "ps2_mouse.h"
-#include "report.h"
 #include "host.h"
 #include "timer.h"
 #include "print.h"
+#include "report.h"
 #include "debug.h"
+#include "ps2.h"
 
+/* ============================= MACROS ============================ */
 
 static report_mouse_t mouse_report = {};
 
+static inline void ps2_mouse_print_report(report_mouse_t *mouse_report);
+static inline void ps2_mouse_convert_report_to_hid(report_mouse_t *mouse_report);
+static inline void ps2_mouse_clear_report(report_mouse_t *mouse_report);
+static inline void ps2_mouse_enable_scrolling(void);
+static inline void ps2_mouse_scroll_button_task(report_mouse_t *mouse_report);
 
-static void print_usb_data(void);
-
+/* ============================= IMPLEMENTATION ============================ */
 
 /* supports only 3 button mouse at this time */
-uint8_t ps2_mouse_init(void) {
-    uint8_t rcv;
-
+void ps2_mouse_init(void) {
     ps2_host_init();
 
-    _delay_ms(1000);    // wait for powering up
+    _delay_ms(PS2_MOUSE_INIT_DELAY);    // wait for powering up
 
-    // send Reset
-    rcv = ps2_host_send(0xFF);
-    print("ps2_mouse_init: send Reset: ");
-    phex(rcv); phex(ps2_error); print("\n");
+    PS2_MOUSE_SEND(PS2_MOUSE_RESET, "ps2_mouse_init: sending reset");
 
-    // read completion code of BAT
-    rcv = ps2_host_recv_response();
-    print("ps2_mouse_init: read BAT: ");
-    phex(rcv); phex(ps2_error); print("\n");
+    PS2_MOUSE_RECEIVE("ps2_mouse_init: read BAT");
+    PS2_MOUSE_RECEIVE("ps2_mouse_init: read DevID");
 
-    // read Device ID
-    rcv = ps2_host_recv_response();
-    print("ps2_mouse_init: read DevID: ");
-    phex(rcv); phex(ps2_error); print("\n");
+#ifdef PS2_MOUSE_USE_REMOTE_MODE
+    ps2_mouse_set_remote_mode();
+#else
+    ps2_mouse_enable_data_reporting();
+#endif
+
+#ifdef PS2_MOUSE_ENABLE_SCROLLING
+    ps2_mouse_enable_scrolling();
+#endif
 
-    // send Set Remote mode
-    rcv = ps2_host_send(0xF0);
-    print("ps2_mouse_init: send 0xF0: ");
-    phex(rcv); phex(ps2_error); print("\n");
+#ifdef PS2_MOUSE_USE_2_1_SCALING
+    ps2_mouse_set_scaling_2_1();
+#endif
 
-    return 0;
+    ps2_mouse_init_user();
 }
 
-#define X_IS_NEG  (mouse_report.buttons & (1<<PS2_MOUSE_X_SIGN))
-#define Y_IS_NEG  (mouse_report.buttons & (1<<PS2_MOUSE_Y_SIGN))
-#define X_IS_OVF  (mouse_report.buttons & (1<<PS2_MOUSE_X_OVFLW))
-#define Y_IS_OVF  (mouse_report.buttons & (1<<PS2_MOUSE_Y_OVFLW))
-void ps2_mouse_task(void)
-{
-    enum { SCROLL_NONE, SCROLL_BTN, SCROLL_SENT };
-    static uint8_t scroll_state = SCROLL_NONE;
+__attribute__((weak))
+void ps2_mouse_init_user(void) {
+}
+
+void ps2_mouse_task(void) {
     static uint8_t buttons_prev = 0;
 
     /* receives packet from mouse */
@@ -79,142 +78,169 @@ void ps2_mouse_task(void)
     rcv = ps2_host_send(PS2_MOUSE_READ_DATA);
     if (rcv == PS2_ACK) {
         mouse_report.buttons = ps2_host_recv_response();
-        mouse_report.x = ps2_host_recv_response();
-        mouse_report.y = ps2_host_recv_response();
+        mouse_report.x = ps2_host_recv_response() * PS2_MOUSE_X_MULTIPLIER;
+        mouse_report.y = ps2_host_recv_response() * PS2_MOUSE_Y_MULTIPLIER;
+#ifdef PS2_MOUSE_ENABLE_SCROLLING
+        mouse_report.v = -(ps2_host_recv_response() & PS2_MOUSE_SCROLL_MASK) * PS2_MOUSE_V_MULTIPLIER;
+#endif
     } else {
         if (debug_mouse) print("ps2_mouse: fail to get mouse packet\n");
         return;
     }
-        xprintf("%ud ", timer_read());
-        print("ps2_mouse raw: [");
-        phex(mouse_report.buttons); print("|");
-        print_hex8((uint8_t)mouse_report.x); print(" ");
-        print_hex8((uint8_t)mouse_report.y); print("]\n");
 
     /* if mouse moves or buttons state changes */
-    if (mouse_report.x || mouse_report.y ||
+    if (mouse_report.x || mouse_report.y || mouse_report.v ||
             ((mouse_report.buttons ^ buttons_prev) & PS2_MOUSE_BTN_MASK)) {
-
-#ifdef PS2_MOUSE_DEBUG
-        print("ps2_mouse raw: [");
-        phex(mouse_report.buttons); print("|");
-        print_hex8((uint8_t)mouse_report.x); print(" ");
-        print_hex8((uint8_t)mouse_report.y); print("]\n");
+#ifdef PS2_MOUSE_DEBUG_RAW
+        // Used to debug raw ps2 bytes from mouse
+        ps2_mouse_print_report(&mouse_report);
 #endif
-
         buttons_prev = mouse_report.buttons;
+        ps2_mouse_convert_report_to_hid(&mouse_report);
+#if PS2_MOUSE_SCROLL_BTN_MASK
+        ps2_mouse_scroll_button_task(&mouse_report);
+#endif
+#ifdef PS2_MOUSE_DEBUG_HID
+        // Used to debug the bytes sent to the host
+        ps2_mouse_print_report(&mouse_report);
+#endif
+        host_mouse_send(&mouse_report);
+    }
+    
+    ps2_mouse_clear_report(&mouse_report);
+}
 
-        // PS/2 mouse data is '9-bit integer'(-256 to 255) which is comprised of sign-bit and 8-bit value.
-        // bit: 8    7 ... 0
-        //      sign \8-bit/
-        //
-        // Meanwhile USB HID mouse indicates 8bit data(-127 to 127), note that -128 is not used.
-        //
-        // This converts PS/2 data into HID value. Use only -127-127 out of PS/2 9-bit.
-        mouse_report.x = X_IS_NEG ?
-                          ((!X_IS_OVF && -127 <= mouse_report.x && mouse_report.x <= -1) ?  mouse_report.x : -127) :
-                          ((!X_IS_OVF && 0 <= mouse_report.x && mouse_report.x <= 127) ? mouse_report.x : 127);
-        mouse_report.y = Y_IS_NEG ?
-                          ((!Y_IS_OVF && -127 <= mouse_report.y && mouse_report.y <= -1) ?  mouse_report.y : -127) :
-                          ((!Y_IS_OVF && 0 <= mouse_report.y && mouse_report.y <= 127) ? mouse_report.y : 127);
+void ps2_mouse_disable_data_reporting(void) {
+    PS2_MOUSE_SEND(PS2_MOUSE_DISABLE_DATA_REPORTING, "ps2 mouse disable data reporting"); 
+}
 
-        // remove sign and overflow flags
-        mouse_report.buttons &= PS2_MOUSE_BTN_MASK;
+void ps2_mouse_enable_data_reporting(void) {
+    PS2_MOUSE_SEND(PS2_MOUSE_ENABLE_DATA_REPORTING, "ps2 mouse enable data reporting");
+}
 
-        // invert coordinate of y to conform to USB HID mouse
-        mouse_report.y = -mouse_report.y;
+void ps2_mouse_set_remote_mode(void) { 
+    PS2_MOUSE_SEND_SAFE(PS2_MOUSE_SET_REMOTE_MODE, "ps2 mouse set remote mode"); 
+    ps2_mouse_mode = PS2_MOUSE_REMOTE_MODE;
+}
 
+void ps2_mouse_set_stream_mode(void) { 
+    PS2_MOUSE_SEND_SAFE(PS2_MOUSE_SET_STREAM_MODE, "ps2 mouse set stream mode"); 
+    ps2_mouse_mode = PS2_MOUSE_STREAM_MODE;
+}
 
-#if PS2_MOUSE_SCROLL_BTN_MASK
-        static uint16_t scroll_button_time = 0;
-        if ((mouse_report.buttons & (PS2_MOUSE_SCROLL_BTN_MASK)) == (PS2_MOUSE_SCROLL_BTN_MASK)) {
-            if (scroll_state == SCROLL_NONE) {
-                scroll_button_time = timer_read();
-                scroll_state = SCROLL_BTN;
-            }
-
-            // doesn't send Scroll Button
-            //mouse_report.buttons &= ~(PS2_MOUSE_SCROLL_BTN_MASK);
-
-            if (mouse_report.x || mouse_report.y) {
-                scroll_state = SCROLL_SENT;
-
-                mouse_report.v = -mouse_report.y/(PS2_MOUSE_SCROLL_DIVISOR_V);
-                mouse_report.h =  mouse_report.x/(PS2_MOUSE_SCROLL_DIVISOR_H);
-                mouse_report.x = 0;
-                mouse_report.y = 0;
-                //host_mouse_send(&mouse_report);
-            }
+void ps2_mouse_set_scaling_2_1(void) {
+    PS2_MOUSE_SEND_SAFE(PS2_MOUSE_SET_SCALING_2_1, "ps2 mouse set scaling 2:1"); 
+}
+
+void ps2_mouse_set_scaling_1_1(void) {
+    PS2_MOUSE_SEND_SAFE(PS2_MOUSE_SET_SCALING_1_1, "ps2 mouse set scaling 1:1"); 
+}
+
+void ps2_mouse_set_resolution(ps2_mouse_resolution_t resolution) {
+    PS2_MOUSE_SET_SAFE(PS2_MOUSE_SET_RESOLUTION, resolution, "ps2 mouse set resolution");
+}
+
+void ps2_mouse_set_sample_rate(ps2_mouse_sample_rate_t sample_rate) {
+    PS2_MOUSE_SET_SAFE(PS2_MOUSE_SET_SAMPLE_RATE, sample_rate, "ps2 mouse set sample rate");
+}
+
+/* ============================= HELPERS ============================ */
+
+#define X_IS_NEG  (mouse_report->buttons & (1<<PS2_MOUSE_X_SIGN))
+#define Y_IS_NEG  (mouse_report->buttons & (1<<PS2_MOUSE_Y_SIGN))
+#define X_IS_OVF  (mouse_report->buttons & (1<<PS2_MOUSE_X_OVFLW))
+#define Y_IS_OVF  (mouse_report->buttons & (1<<PS2_MOUSE_Y_OVFLW))
+static inline void ps2_mouse_convert_report_to_hid(report_mouse_t *mouse_report) {
+    // PS/2 mouse data is '9-bit integer'(-256 to 255) which is comprised of sign-bit and 8-bit value.
+    // bit: 8    7 ... 0
+    //      sign \8-bit/
+    //
+    // Meanwhile USB HID mouse indicates 8bit data(-127 to 127), note that -128 is not used.
+    //
+    // This converts PS/2 data into HID value. Use only -127-127 out of PS/2 9-bit.
+    mouse_report->x = X_IS_NEG ?
+        ((!X_IS_OVF && -127 <= mouse_report->x && mouse_report->x <= -1) ?  mouse_report->x : -127) :
+        ((!X_IS_OVF && 0 <= mouse_report->x && mouse_report->x <= 127) ? mouse_report->x : 127);
+    mouse_report->y = Y_IS_NEG ?
+        ((!Y_IS_OVF && -127 <= mouse_report->y && mouse_report->y <= -1) ?  mouse_report->y : -127) :
+        ((!Y_IS_OVF && 0 <= mouse_report->y && mouse_report->y <= 127) ? mouse_report->y : 127);
+
+    // remove sign and overflow flags
+    mouse_report->buttons &= PS2_MOUSE_BTN_MASK;
+
+    // invert coordinate of y to conform to USB HID mouse
+    mouse_report->y = -mouse_report->y;
+}
+
+static inline void ps2_mouse_clear_report(report_mouse_t *mouse_report) {
+    mouse_report->x = 0;
+    mouse_report->y = 0;
+    mouse_report->v = 0;
+    mouse_report->h = 0;
+    mouse_report->buttons = 0;
+}
+
+static inline void ps2_mouse_print_report(report_mouse_t *mouse_report) {
+    if (!debug_mouse) return;
+    print("ps2_mouse: [");
+    phex(mouse_report->buttons); print("|");
+    print_hex8((uint8_t)mouse_report->x); print(" ");
+    print_hex8((uint8_t)mouse_report->y); print(" ");
+    print_hex8((uint8_t)mouse_report->v); print(" ");
+    print_hex8((uint8_t)mouse_report->h); print("]\n");
+}
+
+static inline void ps2_mouse_enable_scrolling(void) {
+    PS2_MOUSE_SEND(PS2_MOUSE_SET_SAMPLE_RATE, "Initiaing scroll wheel enable: Set sample rate");
+    PS2_MOUSE_SEND(200, "200");
+    PS2_MOUSE_SEND(PS2_MOUSE_SET_SAMPLE_RATE, "Set sample rate");
+    PS2_MOUSE_SEND(100, "100");
+    PS2_MOUSE_SEND(PS2_MOUSE_SET_SAMPLE_RATE, "Set sample rate");
+    PS2_MOUSE_SEND(80, "80");
+    PS2_MOUSE_SEND(PS2_MOUSE_GET_DEVICE_ID, "Finished enabling scroll wheel");
+    _delay_ms(20);
+}
+
+#define PRESS_SCROLL_BUTTONS    mouse_report->buttons |= (PS2_MOUSE_SCROLL_BTN_MASK)
+#define RELEASE_SCROLL_BUTTONS  mouse_report->buttons &= ~(PS2_MOUSE_SCROLL_BTN_MASK)
+static inline void ps2_mouse_scroll_button_task(report_mouse_t *mouse_report) {
+    static enum { 
+        SCROLL_NONE, 
+        SCROLL_BTN, 
+        SCROLL_SENT,
+    } scroll_state = SCROLL_NONE;
+    static uint16_t scroll_button_time = 0;
+
+    if (PS2_MOUSE_SCROLL_BTN_MASK == (mouse_report->buttons & (PS2_MOUSE_SCROLL_BTN_MASK))) {
+        // All scroll buttons are pressed
+
+        if (scroll_state == SCROLL_NONE) {
+            scroll_button_time = timer_read();
+            scroll_state = SCROLL_BTN;
         }
-        else if ((mouse_report.buttons & (PS2_MOUSE_SCROLL_BTN_MASK)) == 0) {
+
+        // If the mouse has moved, update the report to scroll instead of move the mouse
+        if (mouse_report->x || mouse_report->y) {
+            scroll_state = SCROLL_SENT;
+            mouse_report->v = -mouse_report->y/(PS2_MOUSE_SCROLL_DIVISOR_V);
+            mouse_report->h =  mouse_report->x/(PS2_MOUSE_SCROLL_DIVISOR_H);
+            mouse_report->x = 0;
+            mouse_report->y = 0;
+        }
+    } else if (0 == (PS2_MOUSE_SCROLL_BTN_MASK & mouse_report->buttons)) {
+        // None of the scroll buttons are pressed 
+
 #if PS2_MOUSE_SCROLL_BTN_SEND
-            if (scroll_state == SCROLL_BTN &&
-                    TIMER_DIFF_16(timer_read(), scroll_button_time) < PS2_MOUSE_SCROLL_BTN_SEND) {
-                // send Scroll Button(down and up at once) when not scrolled
-                mouse_report.buttons |= (PS2_MOUSE_SCROLL_BTN_MASK);
-                host_mouse_send(&mouse_report);
-                _delay_ms(100);
-                mouse_report.buttons &= ~(PS2_MOUSE_SCROLL_BTN_MASK);
-            }
-#endif
-            scroll_state = SCROLL_NONE;
+        if (scroll_state == SCROLL_BTN 
+                && timer_elapsed(scroll_button_time) < PS2_MOUSE_SCROLL_BTN_SEND) {
+            PRESS_SCROLL_BUTTONS;
+            host_mouse_send(mouse_report);
+            _delay_ms(100);
+            RELEASE_SCROLL_BUTTONS;
         }
-        // doesn't send Scroll Button
-        mouse_report.buttons &= ~(PS2_MOUSE_SCROLL_BTN_MASK);
 #endif
-
-
-        host_mouse_send(&mouse_report);
-        print_usb_data();
+        scroll_state = SCROLL_NONE;
     }
-    // clear report
-    mouse_report.x = 0;
-    mouse_report.y = 0;
-    mouse_report.v = 0;
-    mouse_report.h = 0;
-    mouse_report.buttons = 0;
-}
 
-static void print_usb_data(void)
-{
-    if (!debug_mouse) return;
-    print("ps2_mouse usb: [");
-    phex(mouse_report.buttons); print("|");
-    print_hex8((uint8_t)mouse_report.x); print(" ");
-    print_hex8((uint8_t)mouse_report.y); print(" ");
-    print_hex8((uint8_t)mouse_report.v); print(" ");
-    print_hex8((uint8_t)mouse_report.h); print("]\n");
-}
-
-
-/* PS/2 Mouse Synopsis
- * http://www.computer-engineering.org/ps2mouse/
- *
- * Command:
- * 0xFF: Reset
- * 0xF6: Set Defaults Sampling; rate=100, resolution=4cnt/mm, scaling=1:1, reporting=disabled
- * 0xF5: Disable Data Reporting
- * 0xF4: Enable Data Reporting
- * 0xF3: Set Sample Rate
- * 0xF2: Get Device ID
- * 0xF0: Set Remote Mode
- * 0xEB: Read Data
- * 0xEA: Set Stream Mode
- * 0xE9: Status Request
- * 0xE8: Set Resolution
- * 0xE7: Set Scaling 2:1
- * 0xE6: Set Scaling 1:1
- *
- * Mode:
- * Stream Mode: devices sends the data when it changs its state
- * Remote Mode: host polls the data periodically
- *
- * This code uses Remote Mode and polls the data with Read Data(0xEB).
- *
- * Data format:
- * byte|7       6       5       4       3       2       1       0
- * ----+--------------------------------------------------------------
- *    0|Yovflw  Xovflw  Ysign   Xsign   1       Middle  Right   Left
- *    1|                    X movement
- *    2|                    Y movement
- */
+    RELEASE_SCROLL_BUTTONS;
+}
diff --git a/tmk_core/protocol/ps2_mouse.h b/tmk_core/protocol/ps2_mouse.h
index 27d9790d4..3c93a4634 100644
--- a/tmk_core/protocol/ps2_mouse.h
+++ b/tmk_core/protocol/ps2_mouse.h
@@ -19,16 +19,61 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define  PS2_MOUSE_H
 
 #include <stdbool.h>
+#include "debug.h"
 
-#define PS2_MOUSE_READ_DATA     0xEB
+#define PS2_MOUSE_SEND(command, message) \
+do { \
+   uint8_t rcv = ps2_host_send(command); \
+   if (debug_mouse) { \
+        print((message)); \
+        xprintf(" command: %X, result: %X, error: %X \n", command, rcv, ps2_error); \
+    } \
+} while(0)
+
+#define PS2_MOUSE_SEND_SAFE(command, message) \
+do { \
+    if (PS2_MOUSE_STREAM_MODE == ps2_mouse_mode) { \
+        ps2_mouse_disable_data_reporting(); \
+    } \
+    PS2_MOUSE_SEND(command, message); \
+    if (PS2_MOUSE_STREAM_MODE == ps2_mouse_mode) { \
+        ps2_mouse_enable_data_reporting(); \
+    } \
+} while(0)
+
+#define PS2_MOUSE_SET_SAFE(command, value, message) \
+do { \
+    if (PS2_MOUSE_STREAM_MODE == ps2_mouse_mode) { \
+        ps2_mouse_disable_data_reporting(); \
+    } \
+    PS2_MOUSE_SEND(command, message); \
+    PS2_MOUSE_SEND(value, "Sending value"); \
+    if (PS2_MOUSE_STREAM_MODE == ps2_mouse_mode) { \
+        ps2_mouse_enable_data_reporting(); \
+    } \
+} while(0)
+
+#define PS2_MOUSE_RECEIVE(message) \
+do { \
+   uint8_t rcv = ps2_host_recv_response(); \
+   if (debug_mouse) { \
+        print((message)); \
+        xprintf(" result: %X, error: %X \n", rcv, ps2_error); \
+    } \
+} while(0)
+
+static enum ps2_mouse_mode_e {
+    PS2_MOUSE_STREAM_MODE,
+    PS2_MOUSE_REMOTE_MODE,
+} ps2_mouse_mode = PS2_MOUSE_STREAM_MODE;
 
 /*
  * Data format:
  * byte|7       6       5       4       3       2       1       0
- * ----+--------------------------------------------------------------
- *    0|Yovflw  Xovflw  Ysign   Xsign   1       Middle  Right   Left
- *    1|                    X movement(0-255)
- *    2|                    Y movement(0-255)
+ * ----+----------------------------------------------------------------
+ *    0|[Yovflw][Xovflw][Ysign ][Xsign ][ 1    ][Middle][Right ][Left  ]
+ *    1|[                    X movement(0-255)                         ]
+ *    2|[                    Y movement(0-255)                         ]
  */
 #define PS2_MOUSE_BTN_MASK      0x07
 #define PS2_MOUSE_BTN_LEFT      0
@@ -39,10 +84,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define PS2_MOUSE_X_OVFLW       6
 #define PS2_MOUSE_Y_OVFLW       7
 
-
-/*
- * Scroll by mouse move with pressing button
- */
 /* mouse button to start scrolling; set 0 to disable scroll */
 #ifndef PS2_MOUSE_SCROLL_BTN_MASK
 #define PS2_MOUSE_SCROLL_BTN_MASK       (1<<PS2_MOUSE_BTN_MIDDLE)
@@ -58,9 +99,79 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #ifndef PS2_MOUSE_SCROLL_DIVISOR_H
 #define PS2_MOUSE_SCROLL_DIVISOR_H      2
 #endif
+/* multiply reported mouse values by these */
+#ifndef PS2_MOUSE_X_MULTIPLIER
+#define PS2_MOUSE_X_MULTIPLIER          1
+#endif
+#ifndef PS2_MOUSE_Y_MULTIPLIER
+#define PS2_MOUSE_Y_MULTIPLIER          1
+#endif
+#ifndef PS2_MOUSE_V_MULTIPLIER
+#define PS2_MOUSE_V_MULTIPLIER          1
+#endif
+/* For some mice this will need to be 0x0F */
+#ifndef PS2_MOUSE_SCROLL_MASK       
+#define PS2_MOUSE_SCROLL_MASK           0xFF 
+#endif
+#ifndef PS2_MOUSE_INIT_DELAY
+#define PS2_MOUSE_INIT_DELAY            1000
+#endif
 
+enum ps2_mouse_command_e {
+    PS2_MOUSE_RESET = 0xFF,
+    PS2_MOUSE_RESEND = 0xFE,
+    PS2_MOSUE_SET_DEFAULTS = 0xF6,
+    PS2_MOUSE_DISABLE_DATA_REPORTING = 0xF5,
+    PS2_MOUSE_ENABLE_DATA_REPORTING = 0xF4,
+    PS2_MOUSE_SET_SAMPLE_RATE = 0xF3,
+    PS2_MOUSE_GET_DEVICE_ID = 0xF2,
+    PS2_MOUSE_SET_REMOTE_MODE = 0xF0,
+    PS2_MOUSE_SET_WRAP_MODE = 0xEC,
+    PS2_MOUSE_READ_DATA = 0xEB,
+    PS2_MOUSE_SET_STREAM_MODE = 0xEA,
+    PS2_MOUSE_STATUS_REQUEST = 0xE9,
+    PS2_MOUSE_SET_RESOLUTION = 0xE8,
+    PS2_MOUSE_SET_SCALING_2_1 = 0xE7,
+    PS2_MOUSE_SET_SCALING_1_1 = 0xE6,
+};
+
+typedef enum ps2_mouse_resolution_e {
+    PS2_MOUSE_1_COUNT_MM,
+    PS2_MOUSE_2_COUNT_MM,
+    PS2_MOUSE_4_COUNT_MM,
+    PS2_MOUSE_8_COUNT_MM,   
+} ps2_mouse_resolution_t;
+
+typedef enum ps2_mouse_sample_rate_e {
+    PS2_MOUSE_10_SAMPLES_SEC = 10,
+    PS2_MOUSE_20_SAMPLES_SEC = 20,
+    PS2_MOUSE_40_SAMPLES_SEC = 40,
+    PS2_MOUSE_60_SAMPLES_SEC = 60,
+    PS2_MOUSE_80_SAMPLES_SEC = 80,
+    PS2_MOUSE_100_SAMPLES_SEC = 100,
+    PS2_MOUSE_200_SAMPLES_SEC = 200,
+} ps2_mouse_sample_rate_t;
+
+void ps2_mouse_init(void);
+
+void ps2_mouse_init_user(void);
 
-uint8_t ps2_mouse_init(void);
 void ps2_mouse_task(void);
 
+void ps2_mouse_disable_data_reporting(void);
+
+void ps2_mouse_enable_data_reporting(void);
+
+void ps2_mouse_set_remote_mode(void);
+
+void ps2_mouse_set_stream_mode(void);
+
+void ps2_mouse_set_scaling_2_1(void);
+
+void ps2_mouse_set_scaling_1_1(void);
+
+void ps2_mouse_set_resolution(ps2_mouse_resolution_t resolution);
+
+void ps2_mouse_set_sample_rate(ps2_mouse_sample_rate_t sample_rate);
+
 #endif