Revision 0:39b7f3158ebe, committed 2018-04-05

Comitter:

wninghj

Date:

Thu Apr 05 21:26:10 2018 +0000

Commit message:

TEAM12 DJ;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_FIFO.cpp	Thu Apr 05 21:26:10 2018 +0000
@@ -0,0 +1,224 @@
+/**************************************************************************/
+/*!
+    @file     Adafruit_FIFO.cpp
+    @author   hathach
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2015, Adafruit Industries (adafruit.com)
+    All rights reserved.
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+    1. Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+    2. Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+    3. Neither the name of the copyright holders nor the
+    names of its contributors may be used to endorse or promote products
+    derived from this software without specific prior written permission.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+/**************************************************************************/
+
+#include "Adafruit_FIFO.h"
+#include <string.h>
+
+/******************************************************************************/
+/*!
+    @brief  Constructor
+
+    @param[in] buffer
+               Memory location to store data
+    @param[in] depth
+               Maximum number of items can be hold in buffer
+    @param[in] item_size
+               Number of bytes of each item
+    @param[in] overwrite
+               Should the buffer is overwitten to the first item when it is full
+*/
+/******************************************************************************/
+Adafruit_FIFO::Adafruit_FIFO(void* buffer, uint16_t depth, uint8_t item_size, bool overwrite)
+{
+  m_buffer       = (uint8_t*) buffer;
+  m_depth        = depth;
+  m_item_size    = item_size;
+  m_overwritable = overwrite;
+
+  m_count = m_wr_idx = m_rd_idx = 0;
+}
+
+/******************************************************************************/
+/*!
+    @brief  Clear the FIFO
+*/
+/******************************************************************************/
+void Adafruit_FIFO::clear(void)
+{
+  m_rd_idx = m_wr_idx = m_count = 0;
+}
+
+/******************************************************************************/
+/*!
+    @brief  Write an item to the FIFO
+
+    @param[in] item
+               Memory address of the item
+*/
+/******************************************************************************/
+bool Adafruit_FIFO::write(void const* item)
+{
+  if ( full() && !m_overwritable ) return false;
+
+  memcpy( m_buffer + (m_wr_idx * m_item_size),
+          item,
+          m_item_size);
+
+  m_wr_idx = (m_wr_idx + 1) % m_depth;
+
+  if ( full() )
+  {
+    m_rd_idx = m_wr_idx; // keep the full state (rd == wr && len = size)
+  }
+  else
+  {
+    m_count++;
+  }
+
+  return true;
+}
+
+/******************************************************************************/
+/*!
+    @brief  Write array of items to the FIFO
+
+    @param[in] data
+               Memory address of the item's array
+    @param[in] n
+               Number of items to write
+
+    @return    Number of written items
+*/
+/******************************************************************************/
+uint16_t Adafruit_FIFO::write_n(void const * data, uint16_t n)
+{
+  if ( n == 0 ) return 0;
+
+  uint8_t* buf = (uint8_t*) data;
+
+  uint16_t len = 0;
+  while( (len < n) && write(buf) )
+  {
+    len++;
+    buf += m_item_size;
+  }
+
+  return len;
+}
+
+/******************************************************************************/
+/*!
+    @brief  Read an item from FIFO
+
+    @param[in] buffer
+               Memory address to store item
+*/
+/******************************************************************************/
+bool Adafruit_FIFO::read(void* buffer)
+{
+  if( empty() ) return false;
+
+  memcpy(buffer,
+         m_buffer + (m_rd_idx * m_item_size),
+         m_item_size);
+  m_rd_idx = (m_rd_idx + 1) % m_depth;
+  m_count--;
+
+  return true;
+}
+
+/******************************************************************************/
+/*!
+    @brief  Read multiple items to an array
+
+    @param[in] buffer
+               Memory address of the item's array
+    @param[in] n
+               Number of items to read
+
+    @return    Number of read items
+*/
+/******************************************************************************/
+
+uint16_t Adafruit_FIFO::read_n (void * buffer, uint16_t n)
+{
+  if( n == 0 ) return 0;
+
+  uint8_t* buf = (uint8_t*) buffer;
+
+  uint16_t len = 0;
+  while( (len < n) && read(buf) )
+  {
+    len++;
+    buf += m_item_size;
+  }
+
+  return len;
+}
+
+/******************************************************************************/
+/*!
+    @brief  Read an item without removing it from the FIFO
+
+    @param[in] buffer
+               Memory address to store item
+*/
+/******************************************************************************/
+bool Adafruit_FIFO::peek(void* buffer)
+{
+  if( empty() ) return false;
+
+  memcpy(buffer,
+         m_buffer + (m_rd_idx * m_item_size),
+         m_item_size);
+
+  return true;
+}
+
+
+/******************************************************************************/
+/*!
+    @brief  Read an item without removing it from the FIFO at the specific index
+
+    @param[in] position
+               Position to read from in the FIFO buffer
+
+    @param[in] buffer
+               Memory address to store item
+*/
+/******************************************************************************/
+bool Adafruit_FIFO::peekAt(uint16_t position, void * p_buffer)
+{
+  if( empty() || (position >= m_count) ) return false;
+
+  uint16_t index = (m_rd_idx + position) % m_depth; // rd_idx is position=0
+  memcpy(p_buffer,
+         m_buffer + (index * m_item_size),
+         m_item_size);
+
+  return true;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_FIFO.h	Thu Apr 05 21:26:10 2018 +0000
@@ -0,0 +1,74 @@
+/**************************************************************************/
+/*!
+    @file     Adafruit_FIFO.h
+    @author   hathach
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2015, Adafruit Industries (adafruit.com)
+    All rights reserved.
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+    1. Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+    2. Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+    3. Neither the name of the copyright holders nor the
+    names of its contributors may be used to endorse or promote products
+    derived from this software without specific prior written permission.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+/**************************************************************************/
+
+#ifndef _Adafruit_FIFO_H_
+#define _Adafruit_FIFO_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+class Adafruit_FIFO
+{
+  private:
+             uint8_t* m_buffer       ; ///< buffer pointer
+             uint16_t m_depth        ; ///< max items
+             uint8_t  m_item_size    ; ///< size of each item
+             bool     m_overwritable ;
+    volatile uint16_t m_count        ; ///< number of items in queue
+    volatile uint16_t m_wr_idx       ; ///< write pointer
+    volatile uint16_t m_rd_idx       ; ///< read pointer
+
+  public:
+    // Constructor
+    Adafruit_FIFO(void* buffer, uint16_t depth, uint8_t item_size, bool overwrite);
+
+    void clear(void);
+    bool peek(void* buffer);
+    bool peekAt(uint16_t position, void * p_buffer);
+
+    bool write(void const* item);
+    uint16_t write_n(void const * data, uint16_t n);
+
+    bool read(void* buffer);
+    uint16_t read_n (void * buffer, uint16_t n);
+
+    inline bool     empty(void)     { return m_count == 0; }
+    inline bool     full(void)      { return m_count == m_depth; }
+    inline uint16_t count(void)     { return m_count; }
+    inline uint16_t remaining(void) { return m_depth - m_count; }
+};
+
+#endif /* _Adafruit_FIFO_H_ */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/common_header.h	Thu Apr 05 21:26:10 2018 +0000
@@ -0,0 +1,79 @@
+/**************************************************************************/
+/*!
+    @file     common_header.h
+    @author   hathach
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2016, Adafruit Industries (adafruit.com)
+    All rights reserved.
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+    1. Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+    2. Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+    3. Neither the name of the copyright holders nor the
+    names of its contributors may be used to endorse or promote products
+    derived from this software without specific prior written permission.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+/**************************************************************************/
+
+#ifndef _COMMON_HEADER_H_
+#define _COMMON_HEADER_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+//--------------------------------------------------------------------+
+// COMPILER
+//--------------------------------------------------------------------+
+#define STRING_(x)                  #x                   // stringify without expand
+#define XSTRING_(x)                 STRING_(x)           // expand then stringify
+#define STRING_CONCAT_(a, b)        a##b                 // concat without expand
+#define XSTRING_CONCAT_(a, b)       STRING_CONCAT_(a, b) // expand then concat
+
+#define ATTR_PACKED               __attribute__ ((packed))
+
+//--------------------------------------------------------------------+
+// ASSERT & VERIFY
+//--------------------------------------------------------------------+
+//#define ASSERT(condition, err)    if ( !(condition) ) return err;
+
+//------------- Compile-time Assert -------------//
+#if defined __COUNTER__ && __COUNTER__ != __COUNTER__
+  #define _ASSERT_COUNTER __COUNTER__
+#else
+  #define _ASSERT_COUNTER __LINE__
+#endif
+
+#define ASSERT_STATIC_(const_expr) enum { XSTRING_CONCAT_(static_assert_, _ASSERT_COUNTER) = 1/(!!(const_expr)) }
+
+
+#define VERIFY_(condition)                if ( !(condition) ) return false;
+#define VERIFY_RETURN_(condition, error)  if ( !(condition) ) return error;
+
+//--------------------------------------------------------------------+
+// INLINE FUNCTION
+//--------------------------------------------------------------------+
+static inline bool is_within(uint32_t lower, uint32_t value, uint32_t upper)
+{
+  return (lower <= value) && (value <= upper);
+}
+
+#endif /* _COMMON_HEADER_H_ */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 05 21:26:10 2018 +0000
@@ -0,0 +1,596 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "sdep.h"
+#include "Adafruit_FIFO.h"
+#include <stdio.h>
+
+#define BLUEFRUIT_MODE_COMMAND   HIGH
+#define BLUEFRUIT_MODE_DATA      LOW
+#define BLE_BUFSIZE              4*SDEP_MAX_PACKETSIZE
+
+
+#define SPI_IGNORED_BYTE          0xFEu /**< SPI default character. Character clocked out in case of an ignored transaction. */
+#define SPI_OVERREAD_BYTE         0xFFu /**< SPI over-read character. Character clocked out after an over-read of the transmit buffer. */
+#define SPI_DEFAULT_DELAY_US      50
+
+DigitalOut myled(LED1);
+
+Serial pc(USBTX,USBRX);
+
+SPI spi(p5, p6, p7); // mosi, miso, sclk
+
+DigitalOut cs(p21);
+DigitalIn m_irq_pin(p22);
+DigitalOut ble_reset(p23);
+
+void enable_spi() {
+    cs = 0;
+}
+
+void disable_spi() {
+    cs = 1;
+}
+
+// TX
+uint8_t         m_tx_buffer[SDEP_MAX_PACKETSIZE] = {0};
+uint8_t         m_tx_count = 0;
+
+// RX
+uint8_t         m_rx_buffer[BLE_BUFSIZE * 2] = {0};
+Adafruit_FIFO   m_rx_fifo(m_rx_buffer, sizeof(m_rx_buffer), 1, true);
+
+enum BLE_MODE {
+    COMMAND = 0,
+    DATA
+};
+
+BLE_MODE bleMode = COMMAND;
+
+
+// prototypes
+void spixfer(void *buff, size_t len);
+uint8_t spixfer(uint8_t x);
+bool bleGetResponse(void);
+bool sendPacket(uint16_t command, const uint8_t* buf, uint8_t count, uint8_t more_data);
+size_t bleWriteChar(uint8_t c);
+void bleWrite(char *cmd);
+
+
+volatile unsigned long _millis = 0;
+unsigned long millis(void) {
+    return _millis;
+}
+
+
+class TimeoutTimer
+{
+  private:
+    uint32_t start;
+    uint32_t interval;
+
+  public:
+    TimeoutTimer()              { start = millis(); interval = 0; }
+    TimeoutTimer(uint32_t msec) { set(msec); }
+
+    void set(uint32_t msec)     { start = millis(); interval = msec; }
+    bool expired(void)  const   { return (millis() - start) >= interval; }
+    void restart(void)          { start = millis(); }
+    void reset(void)            { start += interval; } // used for periodic invoke to prevent drift
+};
+
+
+uint16_t word(uint8_t h, uint8_t l) {
+    uint16_t res = h;
+    res <<= 8;
+    res |= l;
+    return res;
+}
+
+
+uint32_t _timeout = 250;
+
+
+bool bleGetPacket(sdepMsgResponse_t* p_response)
+{
+  // Wait until IRQ is asserted, double timeout since some commands take long time to start responding
+  TimeoutTimer tt(2*_timeout);
+  
+  while ( !m_irq_pin ) {
+    if (tt.expired()) return false;
+  }
+  
+  sdepMsgHeader_t* p_header = &p_response->header;
+  
+  enable_spi();
+
+  tt.set(_timeout);
+
+  do {
+    if ( tt.expired() ) break;
+
+    p_header->msg_type = spixfer(0xff);
+
+    if (p_header->msg_type == SPI_IGNORED_BYTE)
+    {
+      // Bluefruit may not be ready
+      // Disable & Re-enable CS with a bit of delay for Bluefruit to ready itself
+      disable_spi();
+      wait_us(50);
+      enable_spi();
+    }
+    else if (p_header->msg_type == SPI_OVERREAD_BYTE)
+    {
+      // IRQ may not be pulled down by Bluefruit when returning all data in previous transfer.
+      // This could happen when Arduino MCU is running at fast rate comparing to Bluefruit's MCU,
+      // causing an SPI_OVERREAD_BYTE to be returned at stage.
+      //
+      // Walkaround: Disable & Re-enable CS with a bit of delay and keep waiting
+      // TODO IRQ is supposed to be OFF then ON, it is better to use GPIO trigger interrupt.
+
+      disable_spi();
+      // wait for the clock to be enabled..
+//      while (!digitalRead(m_irq_pin)) {
+//        if ( tt.expired() ) break;
+//      }
+//      if (!digitalRead(m_irq_pin)) break;
+      wait_us(50);
+      enable_spi();
+    }
+  }  while (p_header->msg_type == SPI_IGNORED_BYTE || p_header->msg_type == SPI_OVERREAD_BYTE);
+
+  bool result=false;
+
+  // Not a loop, just a way to avoid goto with error handling
+  do
+  {
+    // Look for the header
+    // note that we should always get the right header at this point, and not doing so will really mess up things.
+    while ( p_header->msg_type != SDEP_MSGTYPE_RESPONSE && p_header->msg_type != SDEP_MSGTYPE_ERROR && !tt.expired() )
+    {
+      p_header->msg_type = spixfer(0xff);
+    }
+    
+    if ( tt.expired() ) break;
+    
+    memset( (&p_header->msg_type)+1, 0xff, sizeof(sdepMsgHeader_t) - 1);
+    spixfer((&p_header->msg_type)+1, sizeof(sdepMsgHeader_t) - 1);
+
+    // Command is 16-bit at odd address, may have alignment issue with 32-bit chip
+    uint16_t cmd_id = word(p_header->cmd_id_high, p_header->cmd_id_low);
+
+    // Error Message Response
+    if ( p_header->msg_type == SDEP_MSGTYPE_ERROR ) break;
+
+    // Invalid command
+    if (!(cmd_id == SDEP_CMDTYPE_AT_WRAPPER ||
+          cmd_id == SDEP_CMDTYPE_BLE_UARTTX ||
+          cmd_id == SDEP_CMDTYPE_BLE_UARTRX) )
+    {
+      break;
+    }
+
+    // Invalid length
+    if(p_header->length > SDEP_MAX_PACKETSIZE) break;
+
+    // read payload
+    memset(p_response->payload, 0xff, p_header->length);
+    spixfer(p_response->payload, p_header->length);
+
+    result = true;
+  }while(0);
+
+  disable_spi();
+
+  return result;
+}
+
+
+/******************************************************************************/
+/*!
+
+*/
+/******************************************************************************/
+void spixfer(void *buff, size_t len) {
+  uint8_t *p = (uint8_t *)buff;
+  while (len--) {
+    p[0] = spixfer(p[0]);
+    p++;
+  }
+}
+
+/******************************************************************************/
+/*!
+
+*/
+/******************************************************************************/
+uint8_t spixfer(uint8_t x) {
+  return spi.write(x);
+}
+
+
+/******************************************************************************/
+/*!
+    @brief  Try to perform an full AT response transfer from Bluefruit, or execute
+            as many SPI transaction as internal FIFO can hold up.
+
+    @note   If verbose is enabled, all the received data will be print to Serial
+
+    @return
+      - true  : if succeeded
+      - false : if failed
+*/
+/******************************************************************************/
+bool bleGetResponse(void)
+{
+  // Try to read data from Bluefruit if there is enough room in the fifo
+  while ( m_rx_fifo.remaining() >= SDEP_MAX_PACKETSIZE )
+  {
+    // Get a SDEP packet
+    sdepMsgResponse_t msg_response;
+    memset(&msg_response, 0, sizeof(sdepMsgResponse_t));
+
+    if ( !bleGetPacket(&msg_response) ) return false;
+
+    // Write to fifo
+    if ( msg_response.header.length > 0)
+    {
+      m_rx_fifo.write_n(msg_response.payload, msg_response.header.length);
+    }
+
+    // No more packet data
+    if ( !msg_response.header.more_data ) break;
+
+    // It takes a bit since all Data received to IRQ to get LOW
+    // May need to delay a bit for it to be stable before the next try
+    wait_us(50);
+  }
+
+  return true;
+}
+
+
+void bleWrite(char *cmd) {
+    while (*cmd != '\0') {
+        bleWriteChar((uint8_t) *cmd);
+        cmd += 1;
+    }
+}
+
+
+/******************************************************************************/
+/*!
+    @brief Check if the response from the previous command is ready
+
+    @return 'true' if a response is ready, otherwise 'false'
+*/
+/******************************************************************************/
+int bleAvailable(void)
+{
+  if (! m_rx_fifo.empty() ) {
+    return m_rx_fifo.count();
+  }
+
+  if ( bleMode == DATA )
+  {
+    // DATA Mode: query for BLE UART data
+    sendPacket(SDEP_CMDTYPE_BLE_UARTRX, NULL, 0, 0);
+
+    // Waiting to get response from Bluefruit
+    bleGetResponse();
+
+    return m_rx_fifo.count();
+  }else{
+    return m_irq_pin;
+  }
+}
+
+/******************************************************************************/
+/*!
+    @brief Get a byte from response data, perform SPI transaction if needed
+
+    @return -1 if no data is available
+*/
+/******************************************************************************/
+int bleRead(void)
+{
+  uint8_t ch;
+
+  // try to grab from buffer first...
+  if (!m_rx_fifo.empty()) {
+    m_rx_fifo.read(&ch);
+    return (int)ch;
+  }
+
+  if ( bleMode == DATA )
+  {
+    // DATA Mode: query for BLE UART data
+    sendPacket(SDEP_CMDTYPE_BLE_UARTRX, NULL, 0, 0);
+
+    // Waiting to get response from Bluefruit
+    bleGetResponse();
+  }else
+  {
+    // COMMAND Mode: Only read data from Bluefruit if IRQ is raised
+    if ( m_irq_pin ) bleGetResponse();
+  }
+
+  return m_rx_fifo.read(&ch) ? ((int) ch) : EOF;
+
+}
+
+
+uint16_t bleReadLine(char * buf, uint16_t bufsize)
+{
+  uint16_t timeout = 1000 * 10;
+  bool multiline = false;
+  uint16_t replyidx = 0;
+
+  while (timeout--) {
+    while(bleAvailable()) {
+      //pc.printf("SPI: Available\n");
+      char c = bleRead();
+      //pc.printf("SPI: %d\n", (int) c);
+      //SerialDebug.println(c);
+
+      if (c == '\r') continue;
+
+      if (c == '\n') {
+        // the first '\n' is ignored
+        if (replyidx == 0) continue;
+
+        if (!multiline) {
+          timeout = 0;
+          break;
+        }
+      }
+      buf[replyidx] = c;
+      replyidx++;
+
+      // Buffer is full
+      if (replyidx >= bufsize) {
+        //if (_verbose) { SerialDebug.println("*overflow*"); }  // for my debuggin' only!
+        timeout = 0;
+        break;
+      }
+    }
+
+    // delay if needed
+    if (timeout) {
+        Thread::wait(1);
+    }
+  }
+
+  buf[replyidx] = 0;  // null term
+
+  return replyidx;
+}
+
+bool waitForOK(void)
+{
+    // Use temp buffer to avoid overwrite returned result if any
+    char tempbuf[BLE_BUFSIZE + 1];
+
+    while (bleReadLine(tempbuf, BLE_BUFSIZE)) {
+        // pc.printf("SPI: %s\n", tempbuf);
+        if (strcmp(tempbuf, "OK") == 0) return true;
+        if (strcmp(tempbuf, "ERROR") == 0) return false;
+    }
+    return false;
+}
+
+bool sendATCommand(char *cmd) {
+    bleMode = COMMAND;
+    bleWrite(cmd);
+    bleWrite("\r\n");
+    bool result = waitForOK();
+    bleMode = DATA;
+    return result;
+}
+
+bool sendCommandCheckOK(char *cmd) {
+    return sendATCommand(cmd);
+}
+
+// =============================================================================
+
+void flush(void)
+{
+    m_rx_fifo.clear();
+}
+
+uint8_t highByte(uint16_t x) {
+    return (uint8_t) (x >> 8);
+}
+
+uint8_t lowByte(uint16_t x) {
+    return (uint8_t) (x & 0xff);
+}
+
+bool sendPacket(uint16_t command, const uint8_t* buf, uint8_t count, uint8_t more_data) {
+  // flush old response before sending the new command
+  if (more_data == 0) flush();
+
+  sdepMsgCommand_t msgCmd;
+
+  msgCmd.header.msg_type    = SDEP_MSGTYPE_COMMAND;
+  msgCmd.header.cmd_id_high = highByte(command);
+  msgCmd.header.cmd_id_low  = lowByte(command);
+  msgCmd.header.length      = count;
+  msgCmd.header.more_data   = (count == SDEP_MAX_PACKETSIZE) ? more_data : 0;
+
+  // Copy payload
+  if ( buf != NULL && count > 0) memcpy(msgCmd.payload, buf, count);
+
+  enable_spi();
+
+  TimeoutTimer tt(_timeout);
+
+  // Bluefruit may not be ready
+  while ( ( spixfer(msgCmd.header.msg_type) == SPI_IGNORED_BYTE ) && !tt.expired() )
+  {
+    // Disable & Re-enable CS with a bit of delay for Bluefruit to ready itself
+    disable_spi();
+    wait_us(SPI_DEFAULT_DELAY_US);
+    enable_spi();
+  }
+
+  bool result = !tt.expired();
+  if ( result )
+  {
+    // transfer the rest of the data
+    spixfer((void*) (((uint8_t*)&msgCmd) +1), sizeof(sdepMsgHeader_t)+count-1);
+  }
+
+  disable_spi();
+
+  return result;
+}
+
+size_t bleWriteChar(uint8_t c) {
+  if (bleMode == DATA)
+  {
+    sendPacket(SDEP_CMDTYPE_BLE_UARTTX, &c, 1, 0);
+    bleGetResponse();
+    return 1;
+  }
+
+  // Following code handle BLUEFRUIT_MODE_COMMAND
+
+  // Final packet due to \r or \n terminator
+  if (c == '\r' || c == '\n')
+  {
+    if (m_tx_count > 0)
+    {
+      bool result = sendPacket(SDEP_CMDTYPE_AT_WRAPPER, m_tx_buffer, m_tx_count, 0);
+      m_tx_count = 0;
+      if (!result) {
+        return 0;
+      }
+    }
+  }
+  // More than max packet buffered --> send with more_data = 1
+  else if (m_tx_count == SDEP_MAX_PACKETSIZE)
+  {
+    bool result = sendPacket(SDEP_CMDTYPE_AT_WRAPPER, m_tx_buffer, m_tx_count, 1);
+
+    m_tx_buffer[0] = c;
+    m_tx_count = 1;
+    if (!result) {
+      return 0;
+    }
+  }
+  // Not enough data, continue to buffer
+  else
+  {
+    m_tx_buffer[m_tx_count++] = c;
+  }
+  return 1;
+}
+
+int32_t readline_parseInt(void)
+{
+  char buffer[101] = { 0 };
+  uint16_t len = bleReadLine(buffer, 100);
+  if (len == 0) return 0;
+
+  // also parsed hex number e.g 0xADAF
+  int32_t val = strtol(buffer, NULL, 0);
+
+  return val;
+}
+
+bool send_arg_get_resp(int32_t *reply) {
+    bleWrite("\r\n"); // execute command
+
+    // parse integer response if required
+    if (reply)
+    {
+        (*reply) = readline_parseInt();
+    }
+
+    // check OK or ERROR status
+    return waitForOK();
+}
+
+int32_t sendATCommandIntReply(char *cmd) {
+    int32_t reply = 0;
+    BLE_MODE current_mode = bleMode;
+
+    // switch mode if necessary to execute command
+    bleMode = COMMAND;
+
+    // Execute command with parameter and get response
+    bleWrite(cmd);
+    send_arg_get_resp(&reply);
+
+    // switch back if necessary
+    if ( current_mode == DATA ) bleMode = DATA;
+    return reply;
+}
+
+bool sendInitializePattern() {
+    return sendPacket(SDEP_CMDTYPE_INITIALIZE, NULL, 0, 0);
+}
+
+bool isBLEConnected() {
+    int32_t connected = 0;
+    connected = sendATCommandIntReply("AT+GAPGETCONN");
+    return connected;
+}
+
+ 
+void timer_function_millis(void const *n) {
+    _millis += 1;
+}
+
+int main() {
+    // timer to bump _millis
+    RtosTimer millis_timer(timer_function_millis, osTimerPeriodic, (void *)0);
+    millis_timer.start(1);
+    
+    pc.baud(115200);
+    spi.format(8, 0);
+    spi.frequency(4000000);
+    
+    pc.printf("Prepare to initiaize BLE\n");
+    
+    // initialize BLE
+    cs = 1;
+    if (!sendInitializePattern()) pc.printf("BLE Init Failed\n");
+    ble_reset = 1;
+    ble_reset = 0;
+    Thread::wait(10);
+    ble_reset = 1;
+    Thread::wait(500);
+    cs = 1;
+    
+    pc.printf("Set BLE Name\n");
+    
+    // change name
+    if (!sendCommandCheckOK("AT+GAPDEVNAME=AdafruitDJ")) {
+        pc.printf("Could not set device name.\n");
+    }
+    
+    // wait until connected
+    while (!isBLEConnected()) {
+        Thread::wait(100);
+    }
+    
+    pc.printf("Ready\n");
+    
+    Thread::wait(1000);
+    
+    int i = 0;
+    while (true) {
+        bleMode = DATA;
+        // send character
+        char str[40] = { 0 };
+        sprintf(str, "Hello DJ %d\r\n", i);
+        bleWrite(str);
+        Thread::wait(1000);
+        pc.printf("Send\n");
+        i += 1;
+    }
+    
+    Thread::wait(osWaitForever);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Thu Apr 05 21:26:10 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/mbed_official/code/mbed-rtos/#5713cbbdb706

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Apr 05 21:26:10 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/994bdf8177cb
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sdep.h	Thu Apr 05 21:26:10 2018 +0000
@@ -0,0 +1,122 @@
+/******************************************************************************/
+/*!
+    @file     sdep.h
+    @author   hathach
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2013, K. Townsend (microBuilder.eu)
+    Copyright (c) 2014, Adafruit Industries (adafruit.com)
+
+    All rights reserved.
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+    1. Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+    2. Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+    3. Neither the name of the copyright holders nor the
+    names of its contributors may be used to endorse or promote products
+    derived from this software without specific prior written permission.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+    THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+/******************************************************************************/
+#ifndef _SDEP_H_
+#define _SDEP_H_
+
+#include "common_header.h"
+
+#define SDEP_MAX_PACKETSIZE       16 // Maximum payload per packet
+
+/******************************************************************************/
+/*!
+    This enumeration is used to make sure that each command has a unique
+    ID, and is used to create the command lookup table enum further down
+*/
+/******************************************************************************/
+typedef enum
+{
+  SDEP_CMDTYPE_INITIALIZE     = 0xBEEF,   /**< Controls the on board LED(s) */
+  SDEP_CMDTYPE_AT_WRAPPER     = 0x0A00,
+  SDEP_CMDTYPE_BLE_UARTTX     = 0x0A01,
+  SDEP_CMDTYPE_BLE_UARTRX     = 0x0A02,
+} sdepCmdType_t;
+
+/******************************************************************************/
+/*!
+    The first byte of every transfer defines the message type
+*/
+/******************************************************************************/
+typedef enum
+{
+  SDEP_MSGTYPE_COMMAND          = 0x10,
+  SDEP_MSGTYPE_RESPONSE         = 0x20,
+  SDEP_MSGTYPE_ALERT            = 0x40,
+  SDEP_MSGTYPE_ERROR            = 0x80
+} sdepMsgType_t;
+
+/******************************************************************************/
+/*!
+    4-byte header for SDEP messages
+*/
+/******************************************************************************/
+typedef struct ATTR_PACKED {
+  uint8_t msg_type;           // 8-bit message type indicator (sdepMsgType_t)
+
+  union
+  {
+    uint16_t cmd_id;          // 16-bit command ID
+    struct
+    {
+      uint8_t cmd_id_low;
+      uint8_t cmd_id_high;
+    };
+  };
+
+  struct ATTR_PACKED
+  {
+    uint8_t length    : 7;    // Payload length (for this packet)
+    uint8_t more_data : 1;    // 'more' bit for multiple packet transfers
+  };
+} sdepMsgHeader_t;
+
+/******************************************************************************/
+/*!
+    SDEP command message
+*/
+/******************************************************************************/
+typedef struct ATTR_PACKED 
+{
+  sdepMsgHeader_t header;
+  uint8_t payload[SDEP_MAX_PACKETSIZE];
+} sdepMsgCommand_t;
+
+/******************************************************************************/
+/*!
+    Response message struct (same as sdepMsgCommand_t)
+*/
+/******************************************************************************/
+typedef sdepMsgCommand_t sdepMsgResponse_t;
+
+/******************************************************************************/
+/*!
+    Alert message struct
+*/
+/******************************************************************************/
+typedef sdepMsgCommand_t sdepMsgAlert_t;
+
+#endif