aboutsummaryrefslogtreecommitdiff
path: root/drive-sdk/examples
diff options
context:
space:
mode:
Diffstat (limited to 'drive-sdk/examples')
-rwxr-xr-xdrive-sdk/examples/CMakeLists.txt2
-rw-r--r--drive-sdk/examples/README.md25
-rw-r--r--drive-sdk/examples/vehicle-scan/.gitignore3
-rwxr-xr-xdrive-sdk/examples/vehicle-scan/CMakeLists.txt22
-rw-r--r--drive-sdk/examples/vehicle-scan/README.md13
-rw-r--r--drive-sdk/examples/vehicle-scan/uthash.h948
-rw-r--r--drive-sdk/examples/vehicle-scan/vehicle-scan.c471
-rw-r--r--drive-sdk/examples/vehicle-tool/.gitignore4
-rwxr-xr-xdrive-sdk/examples/vehicle-tool/CMakeLists.txt31
-rw-r--r--drive-sdk/examples/vehicle-tool/display.c64
-rw-r--r--drive-sdk/examples/vehicle-tool/display.h32
-rw-r--r--drive-sdk/examples/vehicle-tool/vehicle_cmd.c1155
-rw-r--r--drive-sdk/examples/vehicle-tool/vehicle_tool.c172
13 files changed, 2942 insertions, 0 deletions
diff --git a/drive-sdk/examples/CMakeLists.txt b/drive-sdk/examples/CMakeLists.txt
new file mode 100755
index 0000000..90b1e68
--- /dev/null
+++ b/drive-sdk/examples/CMakeLists.txt
@@ -0,0 +1,2 @@
+add_subdirectory(vehicle-scan)
+add_subdirectory(vehicle-tool)
diff --git a/drive-sdk/examples/README.md b/drive-sdk/examples/README.md
new file mode 100644
index 0000000..9f5e781
--- /dev/null
+++ b/drive-sdk/examples/README.md
@@ -0,0 +1,25 @@
+Anki Drive SDK Examples
+=======================
+
+These examples show how to use the Anki Drive SDK to detect, display, connect to and control vehicles.
+
+
+## vehicle-scan
+
+A linux command line utility to scan for Anki Drive vehicles using Bluetooth LE and display information advertised by vehicles.
+This utility shows how to use the data parsing methods to extract vehicle information from Bluetooth LE advertising data packets.
+`vehicle-scan` links to code distributed with the [bluez][] library and is licensed under the GNU Public License v3.
+
+## vehicle-tool
+
+An interactive command line shell for connecting and controlling Anki Drive vehicles.
+This program demonstrates the Bluetooth LE connection procedure and shows how to interact use the message protocol to interact with vehicles.
+`vehicle-tool` links to code distributed with the [bluez][] library and is licensed under the GNU Public License v3.
+
+[bluez]: http://www.bluez.org
+
+## Building and Running Examples
+
+For a detailed description of how to build and run the example programs, see the guide to [Getting Started on Ubuntu][ubuntu-getting-started].
+
+[ubuntu-getting-started]: https://github.com/anki/drive-sdk/wiki/Getting-Started-on-Ubuntu
diff --git a/drive-sdk/examples/vehicle-scan/.gitignore b/drive-sdk/examples/vehicle-scan/.gitignore
new file mode 100644
index 0000000..c3a4afe
--- /dev/null
+++ b/drive-sdk/examples/vehicle-scan/.gitignore
@@ -0,0 +1,3 @@
+.deps
+.dirstamp
+vehicle-scan
diff --git a/drive-sdk/examples/vehicle-scan/CMakeLists.txt b/drive-sdk/examples/vehicle-scan/CMakeLists.txt
new file mode 100755
index 0000000..f976cfb
--- /dev/null
+++ b/drive-sdk/examples/vehicle-scan/CMakeLists.txt
@@ -0,0 +1,22 @@
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+
+SET (CMAKE_C_FLAGS "")
+
+include_directories(${drivekit_SOURCE_DIR}/include
+ ${bzle_SOURCE_DIR}/include
+ )
+
+# Add sources
+set(vehiclescan_SOURCES
+ vehicle-scan.c
+)
+
+add_executable(vehicle-scan ${vehiclescan_SOURCES})
+target_link_libraries(vehicle-scan
+ ankidrive
+ bzle
+)
+
+install(TARGETS vehicle-scan
+ RUNTIME DESTINATION bin
+)
diff --git a/drive-sdk/examples/vehicle-scan/README.md b/drive-sdk/examples/vehicle-scan/README.md
new file mode 100644
index 0000000..9be8499
--- /dev/null
+++ b/drive-sdk/examples/vehicle-scan/README.md
@@ -0,0 +1,13 @@
+## Build
+
+ make
+
+## Run
+
+ # bring up bluetooth 4.0 driver
+ sudo hciconfig hci0 up
+
+ # run scanning tool
+ sudo ./vehicle-scan
+
+ # Ctrl+C to stop scanning
diff --git a/drive-sdk/examples/vehicle-scan/uthash.h b/drive-sdk/examples/vehicle-scan/uthash.h
new file mode 100644
index 0000000..b9bc7e9
--- /dev/null
+++ b/drive-sdk/examples/vehicle-scan/uthash.h
@@ -0,0 +1,948 @@
+/*
+Copyright (c) 2003-2013, Troy D. Hanson http://troydhanson.github.com/uthash/
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 OWNER
+OR CONTRIBUTORS 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 UTHASH_H
+#define UTHASH_H
+
+#include <string.h> /* memcmp,strlen */
+#include <stddef.h> /* ptrdiff_t */
+#include <stdlib.h> /* exit() */
+
+/* These macros use decltype or the earlier __typeof GNU extension.
+ As decltype is only available in newer compilers (VS2010 or gcc 4.3+
+ when compiling c++ source) this code uses whatever method is needed
+ or, for VS2008 where neither is available, uses casting workarounds. */
+#ifdef _MSC_VER /* MS compiler */
+#if _MSC_VER >= 1600 && defined(__cplusplus) /* VS2010 or newer in C++ mode */
+#define DECLTYPE(x) (decltype(x))
+#else /* VS2008 or older (or VS2010 in C mode) */
+#define NO_DECLTYPE
+#define DECLTYPE(x)
+#endif
+#else /* GNU, Sun and other compilers */
+#define DECLTYPE(x) (__typeof(x))
+#endif
+
+#ifdef NO_DECLTYPE
+#define DECLTYPE_ASSIGN(dst,src) \
+do { \
+ char **_da_dst = (char**)(&(dst)); \
+ *_da_dst = (char*)(src); \
+} while(0)
+#else
+#define DECLTYPE_ASSIGN(dst,src) \
+do { \
+ (dst) = DECLTYPE(dst)(src); \
+} while(0)
+#endif
+
+/* a number of the hash function use uint32_t which isn't defined on win32 */
+#ifdef _MSC_VER
+typedef unsigned int uint32_t;
+typedef unsigned char uint8_t;
+#else
+#include <inttypes.h> /* uint32_t */
+#endif
+
+#define UTHASH_VERSION 1.9.8
+
+#ifndef uthash_fatal
+#define uthash_fatal(msg) exit(-1) /* fatal error (out of memory,etc) */
+#endif
+#ifndef uthash_malloc
+#define uthash_malloc(sz) malloc(sz) /* malloc fcn */
+#endif
+#ifndef uthash_free
+#define uthash_free(ptr,sz) free(ptr) /* free fcn */
+#endif
+
+#ifndef uthash_noexpand_fyi
+#define uthash_noexpand_fyi(tbl) /* can be defined to log noexpand */
+#endif
+#ifndef uthash_expand_fyi
+#define uthash_expand_fyi(tbl) /* can be defined to log expands */
+#endif
+
+/* initial number of buckets */
+#define HASH_INITIAL_NUM_BUCKETS 32 /* initial number of buckets */
+#define HASH_INITIAL_NUM_BUCKETS_LOG2 5 /* lg2 of initial number of buckets */
+#define HASH_BKT_CAPACITY_THRESH 10 /* expand when bucket count reaches */
+
+/* calculate the element whose hash handle address is hhe */
+#define ELMT_FROM_HH(tbl,hhp) ((void*)(((char*)(hhp)) - ((tbl)->hho)))
+
+#define HASH_FIND(hh,head,keyptr,keylen,out) \
+do { \
+ unsigned _hf_bkt,_hf_hashv; \
+ out=NULL; \
+ if (head) { \
+ HASH_FCN(keyptr,keylen, (head)->hh.tbl->num_buckets, _hf_hashv, _hf_bkt); \
+ if (HASH_BLOOM_TEST((head)->hh.tbl, _hf_hashv)) { \
+ HASH_FIND_IN_BKT((head)->hh.tbl, hh, (head)->hh.tbl->buckets[ _hf_bkt ], \
+ keyptr,keylen,out); \
+ } \
+ } \
+} while (0)
+
+#ifdef HASH_BLOOM
+#define HASH_BLOOM_BITLEN (1ULL << HASH_BLOOM)
+#define HASH_BLOOM_BYTELEN (HASH_BLOOM_BITLEN/8) + ((HASH_BLOOM_BITLEN%8) ? 1:0)
+#define HASH_BLOOM_MAKE(tbl) \
+do { \
+ (tbl)->bloom_nbits = HASH_BLOOM; \
+ (tbl)->bloom_bv = (uint8_t*)uthash_malloc(HASH_BLOOM_BYTELEN); \
+ if (!((tbl)->bloom_bv)) { uthash_fatal( "out of memory"); } \
+ memset((tbl)->bloom_bv, 0, HASH_BLOOM_BYTELEN); \
+ (tbl)->bloom_sig = HASH_BLOOM_SIGNATURE; \
+} while (0)
+
+#define HASH_BLOOM_FREE(tbl) \
+do { \
+ uthash_free((tbl)->bloom_bv, HASH_BLOOM_BYTELEN); \
+} while (0)
+
+#define HASH_BLOOM_BITSET(bv,idx) (bv[(idx)/8] |= (1U << ((idx)%8)))
+#define HASH_BLOOM_BITTEST(bv,idx) (bv[(idx)/8] & (1U << ((idx)%8)))
+
+#define HASH_BLOOM_ADD(tbl,hashv) \
+ HASH_BLOOM_BITSET((tbl)->bloom_bv, (hashv & (uint32_t)((1ULL << (tbl)->bloom_nbits) - 1)))
+
+#define HASH_BLOOM_TEST(tbl,hashv) \
+ HASH_BLOOM_BITTEST((tbl)->bloom_bv, (hashv & (uint32_t)((1ULL << (tbl)->bloom_nbits) - 1)))
+
+#else
+#define HASH_BLOOM_MAKE(tbl)
+#define HASH_BLOOM_FREE(tbl)
+#define HASH_BLOOM_ADD(tbl,hashv)
+#define HASH_BLOOM_TEST(tbl,hashv) (1)
+#define HASH_BLOOM_BYTELEN 0
+#endif
+
+#define HASH_MAKE_TABLE(hh,head) \
+do { \
+ (head)->hh.tbl = (UT_hash_table*)uthash_malloc( \
+ sizeof(UT_hash_table)); \
+ if (!((head)->hh.tbl)) { uthash_fatal( "out of memory"); } \
+ memset((head)->hh.tbl, 0, sizeof(UT_hash_table)); \
+ (head)->hh.tbl->tail = &((head)->hh); \
+ (head)->hh.tbl->num_buckets = HASH_INITIAL_NUM_BUCKETS; \
+ (head)->hh.tbl->log2_num_buckets = HASH_INITIAL_NUM_BUCKETS_LOG2; \
+ (head)->hh.tbl->hho = (char*)(&(head)->hh) - (char*)(head); \
+ (head)->hh.tbl->buckets = (UT_hash_bucket*)uthash_malloc( \
+ HASH_INITIAL_NUM_BUCKETS*sizeof(struct UT_hash_bucket)); \
+ if (! (head)->hh.tbl->buckets) { uthash_fatal( "out of memory"); } \
+ memset((head)->hh.tbl->buckets, 0, \
+ HASH_INITIAL_NUM_BUCKETS*sizeof(struct UT_hash_bucket)); \
+ HASH_BLOOM_MAKE((head)->hh.tbl); \
+ (head)->hh.tbl->signature = HASH_SIGNATURE; \
+} while(0)
+
+#define HASH_ADD(hh,head,fieldname,keylen_in,add) \
+ HASH_ADD_KEYPTR(hh,head,&((add)->fieldname),keylen_in,add)
+
+#define HASH_REPLACE(hh,head,fieldname,keylen_in,add,replaced) \
+do { \
+ replaced=NULL; \
+ HASH_FIND(hh,head,&((add)->fieldname),keylen_in,replaced); \
+ if (replaced!=NULL) { \
+ HASH_DELETE(hh,head,replaced); \
+ }; \
+ HASH_ADD(hh,head,fieldname,keylen_in,add); \
+} while(0)
+
+#define HASH_ADD_KEYPTR(hh,head,keyptr,keylen_in,add) \
+do { \
+ unsigned _ha_bkt; \
+ (add)->hh.next = NULL; \
+ (add)->hh.key = (char*)(keyptr); \
+ (add)->hh.keylen = (unsigned)(keylen_in); \
+ if (!(head)) { \
+ head = (add); \
+ (head)->hh.prev = NULL; \
+ HASH_MAKE_TABLE(hh,head); \
+ } else { \
+ (head)->hh.tbl->tail->next = (add); \
+ (add)->hh.prev = ELMT_FROM_HH((head)->hh.tbl, (head)->hh.tbl->tail); \
+ (head)->hh.tbl->tail = &((add)->hh); \
+ } \
+ (head)->hh.tbl->num_items++; \
+ (add)->hh.tbl = (head)->hh.tbl; \
+ HASH_FCN(keyptr,keylen_in, (head)->hh.tbl->num_buckets, \
+ (add)->hh.hashv, _ha_bkt); \
+ HASH_ADD_TO_BKT((head)->hh.tbl->buckets[_ha_bkt],&(add)->hh); \
+ HASH_BLOOM_ADD((head)->hh.tbl,(add)->hh.hashv); \
+ HASH_EMIT_KEY(hh,head,keyptr,keylen_in); \
+ HASH_FSCK(hh,head); \
+} while(0)
+
+#define HASH_TO_BKT( hashv, num_bkts, bkt ) \
+do { \
+ bkt = ((hashv) & ((num_bkts) - 1)); \
+} while(0)
+
+/* delete "delptr" from the hash table.
+ * "the usual" patch-up process for the app-order doubly-linked-list.
+ * The use of _hd_hh_del below deserves special explanation.
+ * These used to be expressed using (delptr) but that led to a bug
+ * if someone used the same symbol for the head and deletee, like
+ * HASH_DELETE(hh,users,users);
+ * We want that to work, but by changing the head (users) below
+ * we were forfeiting our ability to further refer to the deletee (users)
+ * in the patch-up process. Solution: use scratch space to
+ * copy the deletee pointer, then the latter references are via that
+ * scratch pointer rather than through the repointed (users) symbol.
+ */
+#define HASH_DELETE(hh,head,delptr) \
+do { \
+ unsigned _hd_bkt; \
+ struct UT_hash_handle *_hd_hh_del; \
+ if ( ((delptr)->hh.prev == NULL) && ((delptr)->hh.next == NULL) ) { \
+ uthash_free((head)->hh.tbl->buckets, \
+ (head)->hh.tbl->num_buckets*sizeof(struct UT_hash_bucket) ); \
+ HASH_BLOOM_FREE((head)->hh.tbl); \
+ uthash_free((head)->hh.tbl, sizeof(UT_hash_table)); \
+ head = NULL; \
+ } else { \
+ _hd_hh_del = &((delptr)->hh); \
+ if ((delptr) == ELMT_FROM_HH((head)->hh.tbl,(head)->hh.tbl->tail)) { \
+ (head)->hh.tbl->tail = \
+ (UT_hash_handle*)((ptrdiff_t)((delptr)->hh.prev) + \
+ (head)->hh.tbl->hho); \
+ } \
+ if ((delptr)->hh.prev) { \
+ ((UT_hash_handle*)((ptrdiff_t)((delptr)->hh.prev) + \
+ (head)->hh.tbl->hho))->next = (delptr)->hh.next; \
+ } else { \
+ DECLTYPE_ASSIGN(head,(delptr)->hh.next); \
+ } \
+ if (_hd_hh_del->next) { \
+ ((UT_hash_handle*)((ptrdiff_t)_hd_hh_del->next + \
+ (head)->hh.tbl->hho))->prev = \
+ _hd_hh_del->prev; \
+ } \
+ HASH_TO_BKT( _hd_hh_del->hashv, (head)->hh.tbl->num_buckets, _hd_bkt); \
+ HASH_DEL_IN_BKT(hh,(head)->hh.tbl->buckets[_hd_bkt], _hd_hh_del); \
+ (head)->hh.tbl->num_items--; \
+ } \
+ HASH_FSCK(hh,head); \
+} while (0)
+
+
+/* convenience forms of HASH_FIND/HASH_ADD/HASH_DEL */
+#define HASH_FIND_STR(head,findstr,out) \
+ HASH_FIND(hh,head,findstr,strlen(findstr),out)
+#define HASH_ADD_STR(head,strfield,add) \
+ HASH_ADD(hh,head,strfield,strlen(add->strfield),add)
+#define HASH_REPLACE_STR(head,strfield,add,replaced) \
+ HASH_REPLACE(hh,head,strfield,strlen(add->strfield),add,replaced)
+#define HASH_FIND_INT(head,findint,out) \
+ HASH_FIND(hh,head,findint,sizeof(int),out)
+#define HASH_ADD_INT(head,intfield,add) \
+ HASH_ADD(hh,head,intfield,sizeof(int),add)
+#define HASH_REPLACE_INT(head,intfield,add,replaced) \
+ HASH_REPLACE(hh,head,intfield,sizeof(int),add,replaced)
+#define HASH_FIND_PTR(head,findptr,out) \
+ HASH_FIND(hh,head,findptr,sizeof(void *),out)
+#define HASH_ADD_PTR(head,ptrfield,add) \
+ HASH_ADD(hh,head,ptrfield,sizeof(void *),add)
+#define HASH_REPLACE_PTR(head,ptrfield,add,replaced) \
+ HASH_REPLACE(hh,head,ptrfield,sizeof(void *),add,replaced)
+#define HASH_DEL(head,delptr) \
+ HASH_DELETE(hh,head,delptr)
+
+/* HASH_FSCK checks hash integrity on every add/delete when HASH_DEBUG is defined.
+ * This is for uthash developer only; it compiles away if HASH_DEBUG isn't defined.
+ */
+#ifdef HASH_DEBUG
+#define HASH_OOPS(...) do { fprintf(stderr,__VA_ARGS__); exit(-1); } while (0)
+#define HASH_FSCK(hh,head) \
+do { \
+ unsigned _bkt_i; \
+ unsigned _count, _bkt_count; \
+ char *_prev; \
+ struct UT_hash_handle *_thh; \
+ if (head) { \
+ _count = 0; \
+ for( _bkt_i = 0; _bkt_i < (head)->hh.tbl->num_buckets; _bkt_i++) { \
+ _bkt_count = 0; \
+ _thh = (head)->hh.tbl->buckets[_bkt_i].hh_head; \
+ _prev = NULL; \
+ while (_thh) { \
+ if (_prev != (char*)(_thh->hh_prev)) { \
+ HASH_OOPS("invalid hh_prev %p, actual %p\n", \
+ _thh->hh_prev, _prev ); \
+ } \
+ _bkt_count++; \
+ _prev = (char*)(_thh); \
+ _thh = _thh->hh_next; \
+ } \
+ _count += _bkt_count; \
+ if ((head)->hh.tbl->buckets[_bkt_i].count != _bkt_count) { \
+ HASH_OOPS("invalid bucket count %d, actual %d\n", \
+ (head)->hh.tbl->buckets[_bkt_i].count, _bkt_count); \
+ } \
+ } \
+ if (_count != (head)->hh.tbl->num_items) { \
+ HASH_OOPS("invalid hh item count %d, actual %d\n", \
+ (head)->hh.tbl->num_items, _count ); \
+ } \
+ /* traverse hh in app order; check next/prev integrity, count */ \
+ _count = 0; \
+ _prev = NULL; \
+ _thh = &(head)->hh; \
+ while (_thh) { \
+ _count++; \
+ if (_prev !=(char*)(_thh->prev)) { \
+ HASH_OOPS("invalid prev %p, actual %p\n", \
+ _thh->prev, _prev ); \
+ } \
+ _prev = (char*)ELMT_FROM_HH((head)->hh.tbl, _thh); \
+ _thh = ( _thh->next ? (UT_hash_handle*)((char*)(_thh->next) + \
+ (head)->hh.tbl->hho) : NULL ); \
+ } \
+ if (_count != (head)->hh.tbl->num_items) { \
+ HASH_OOPS("invalid app item count %d, actual %d\n", \
+ (head)->hh.tbl->num_items, _count ); \
+ } \
+ } \
+} while (0)
+#else
+#define HASH_FSCK(hh,head)
+#endif
+
+/* When compiled with -DHASH_EMIT_KEYS, length-prefixed keys are emitted to
+ * the descriptor to which this macro is defined for tuning the hash function.
+ * The app can #include <unistd.h> to get the prototype for write(2). */
+#ifdef HASH_EMIT_KEYS
+#define HASH_EMIT_KEY(hh,head,keyptr,fieldlen) \
+do { \
+ unsigned _klen = fieldlen; \
+ write(HASH_EMIT_KEYS, &_klen, sizeof(_klen)); \
+ write(HASH_EMIT_KEYS, keyptr, fieldlen); \
+} while (0)
+#else
+#define HASH_EMIT_KEY(hh,head,keyptr,fieldlen)
+#endif
+
+/* default to Jenkin's hash unless overridden e.g. DHASH_FUNCTION=HASH_SAX */
+#ifdef HASH_FUNCTION
+#define HASH_FCN HASH_FUNCTION
+#else
+#define HASH_FCN HASH_JEN
+#endif
+
+/* The Bernstein hash function, used in Perl prior to v5.6 */
+#define HASH_BER(key,keylen,num_bkts,hashv,bkt) \
+do { \
+ unsigned _hb_keylen=keylen; \
+ char *_hb_key=(char*)(key); \
+ (hashv) = 0; \
+ while (_hb_keylen--) { (hashv) = ((hashv) * 33) + *_hb_key++; } \
+ bkt = (hashv) & (num_bkts-1); \
+} while (0)
+
+
+/* SAX/FNV/OAT/JEN hash functions are macro variants of those listed at
+ * http://eternallyconfuzzled.com/tuts/algorithms/jsw_tut_hashing.aspx */
+#define HASH_SAX(key,keylen,num_bkts,hashv,bkt) \
+do { \
+ unsigned _sx_i; \
+ char *_hs_key=(char*)(key); \
+ hashv = 0; \
+ for(_sx_i=0; _sx_i < keylen; _sx_i++) \
+ hashv ^= (hashv << 5) + (hashv >> 2) + _hs_key[_sx_i]; \
+ bkt = hashv & (num_bkts-1); \
+} while (0)
+
+#define HASH_FNV(key,keylen,num_bkts,hashv,bkt) \
+do { \
+ unsigned _fn_i; \
+ char *_hf_key=(char*)(key); \
+ hashv = 2166136261UL; \
+ for(_fn_i=0; _fn_i < keylen; _fn_i++) \
+ hashv = (hashv * 16777619) ^ _hf_key[_fn_i]; \
+ bkt = hashv & (num_bkts-1); \
+} while(0)
+
+#define HASH_OAT(key,keylen,num_bkts,hashv,bkt) \
+do { \
+ unsigned _ho_i; \
+ char *_ho_key=(char*)(key); \
+ hashv = 0; \
+ for(_ho_i=0; _ho_i < keylen; _ho_i++) { \
+ hashv += _ho_key[_ho_i]; \
+ hashv += (hashv << 10); \
+ hashv ^= (hashv >> 6); \
+ } \
+ hashv += (hashv << 3); \
+ hashv ^= (hashv >> 11); \
+ hashv += (hashv << 15); \
+ bkt = hashv & (num_bkts-1); \
+} while(0)
+
+#define HASH_JEN_MIX(a,b,c) \
+do { \
+ a -= b; a -= c; a ^= ( c >> 13 ); \
+ b -= c; b -= a; b ^= ( a << 8 ); \
+ c -= a; c -= b; c ^= ( b >> 13 ); \
+ a -= b; a -= c; a ^= ( c >> 12 ); \
+ b -= c; b -= a; b ^= ( a << 16 ); \
+ c -= a; c -= b; c ^= ( b >> 5 ); \
+ a -= b; a -= c; a ^= ( c >> 3 ); \
+ b -= c; b -= a; b ^= ( a << 10 ); \
+ c -= a; c -= b; c ^= ( b >> 15 ); \
+} while (0)
+
+#define HASH_JEN(key,keylen,num_bkts,hashv,bkt) \
+do { \
+ unsigned _hj_i,_hj_j,_hj_k; \
+ unsigned char *_hj_key=(unsigned char*)(key); \
+ hashv = 0xfeedbeef; \
+ _hj_i = _hj_j = 0x9e3779b9; \
+ _hj_k = (unsigned)(keylen); \
+ while (_hj_k >= 12) { \
+ _hj_i += (_hj_key[0] + ( (unsigned)_hj_key[1] << 8 ) \
+ + ( (unsigned)_hj_key[2] << 16 ) \
+ + ( (unsigned)_hj_key[3] << 24 ) ); \
+ _hj_j += (_hj_key[4] + ( (unsigned)_hj_key[5] << 8 ) \
+ + ( (unsigned)_hj_key[6] << 16 ) \
+ + ( (unsigned)_hj_key[7] << 24 ) ); \
+ hashv += (_hj_key[8] + ( (unsigned)_hj_key[9] << 8 ) \
+ + ( (unsigned)_hj_key[10] << 16 ) \
+ + ( (unsigned)_hj_key[11] << 24 ) ); \
+ \
+ HASH_JEN_MIX(_hj_i, _hj_j, hashv); \
+ \
+ _hj_key += 12; \
+ _hj_k -= 12; \
+ } \
+ hashv += keylen; \
+ switch ( _hj_k ) { \
+ case 11: hashv += ( (unsigned)_hj_key[10] << 24 ); \
+ case 10: hashv += ( (unsigned)_hj_key[9] << 16 ); \
+ case 9: hashv += ( (unsigned)_hj_key[8] << 8 ); \
+ case 8: _hj_j += ( (unsigned)_hj_key[7] << 24 ); \
+ case 7: _hj_j += ( (unsigned)_hj_key[6] << 16 ); \
+ case 6: _hj_j += ( (unsigned)_hj_key[5] << 8 ); \
+ case 5: _hj_j += _hj_key[4]; \
+ case 4: _hj_i += ( (unsigned)_hj_key[3] << 24 ); \
+ case 3: _hj_i += ( (unsigned)_hj_key[2] << 16 ); \
+ case 2: _hj_i += ( (unsigned)_hj_key[1] << 8 ); \
+ case 1: _hj_i += _hj_key[0]; \
+ } \
+ HASH_JEN_MIX(_hj_i, _hj_j, hashv); \
+ bkt = hashv & (num_bkts-1); \
+} while(0)
+
+/* The Paul Hsieh hash function */
+#undef get16bits
+#if (defined(__GNUC__) && defined(__i386__)) || defined(__WATCOMC__) \
+ || defined(_MSC_VER) || defined (__BORLANDC__) || defined (__TURBOC__)
+#define get16bits(d) (*((const uint16_t *) (d)))
+#endif
+
+#if !defined (get16bits)
+#define get16bits(d) ((((uint32_t)(((const uint8_t *)(d))[1])) << 8) \
+ +(uint32_t)(((const uint8_t *)(d))[0]) )
+#endif
+#define HASH_SFH(key,keylen,num_bkts,hashv,bkt) \
+do { \
+ unsigned char *_sfh_key=(unsigned char*)(key); \
+ uint32_t _sfh_tmp, _sfh_len = keylen; \
+ \
+ int _sfh_rem = _sfh_len & 3; \
+ _sfh_len >>= 2; \
+ hashv = 0xcafebabe; \
+ \
+ /* Main loop */ \
+ for (;_sfh_len > 0; _sfh_len--) { \
+ hashv += get16bits (_sfh_key); \
+ _sfh_tmp = (uint32_t)(get16bits (_sfh_key+2)) << 11 ^ hashv; \
+ hashv = (hashv << 16) ^ _sfh_tmp; \
+ _sfh_key += 2*sizeof (uint16_t); \
+ hashv += hashv >> 11; \
+ } \
+ \
+ /* Handle end cases */ \
+ switch (_sfh_rem) { \
+ case 3: hashv += get16bits (_sfh_key); \
+ hashv ^= hashv << 16; \
+ hashv ^= (uint32_t)(_sfh_key[sizeof (uint16_t)] << 18); \
+ hashv += hashv >> 11; \
+ break; \
+ case 2: hashv += get16bits (_sfh_key); \
+ hashv ^= hashv << 11; \
+ hashv += hashv >> 17; \
+ break; \
+ case 1: hashv += *_sfh_key; \
+ hashv ^= hashv << 10; \
+ hashv += hashv >> 1; \
+ } \
+ \
+ /* Force "avalanching" of final 127 bits */ \
+ hashv ^= hashv << 3; \
+ hashv += hashv >> 5; \
+ hashv ^= hashv << 4; \
+ hashv += hashv >> 17; \
+ hashv ^= hashv << 25; \
+ hashv += hashv >> 6; \
+ bkt = hashv & (num_bkts-1); \
+} while(0)
+
+#ifdef HASH_USING_NO_STRICT_ALIASING
+/* The MurmurHash exploits some CPU's (x86,x86_64) tolerance for unaligned reads.
+ * For other types of CPU's (e.g. Sparc) an unaligned read causes a bus error.
+ * MurmurHash uses the faster approach only on CPU's where we know it's safe.
+ *
+ * Note the preprocessor built-in defines can be emitted using:
+ *
+ * gcc -m64 -dM -E - < /dev/null (on gcc)
+ * cc -## a.c (where a.c is a simple test file) (Sun Studio)
+ */
+#if (defined(__i386__) || defined(__x86_64__) || defined(_M_IX86))
+#define MUR_GETBLOCK(p,i) p[i]
+#else /* non intel */
+#define MUR_PLUS0_ALIGNED(p) (((unsigned long)p & 0x3) == 0)
+#define MUR_PLUS1_ALIGNED(p) (((unsigned long)p & 0x3) == 1)
+#define MUR_PLUS2_ALIGNED(p) (((unsigned long)p & 0x3) == 2)
+#define MUR_PLUS3_ALIGNED(p) (((unsigned long)p & 0x3) == 3)
+#define WP(p) ((uint32_t*)((unsigned long)(p) & ~3UL))
+#if (defined(__BIG_ENDIAN__) || defined(SPARC) || defined(__ppc__) || defined(__ppc64__))
+#define MUR_THREE_ONE(p) ((((*WP(p))&0x00ffffff) << 8) | (((*(WP(p)+1))&0xff000000) >> 24))
+#define MUR_TWO_TWO(p) ((((*WP(p))&0x0000ffff) <<16) | (((*(WP(p)+1))&0xffff0000) >> 16))
+#define MUR_ONE_THREE(p) ((((*WP(p))&0x000000ff) <<24) | (((*(WP(p)+1))&0xffffff00) >> 8))
+#else /* assume little endian non-intel */
+#define MUR_THREE_ONE(p) ((((*WP(p))&0xffffff00) >> 8) | (((*(WP(p)+1))&0x000000ff) << 24))
+#define MUR_TWO_TWO(p) ((((*WP(p))&0xffff0000) >>16) | (((*(WP(p)+1))&0x0000ffff) << 16))
+#define MUR_ONE_THREE(p) ((((*WP(p))&0xff000000) >>24) | (((*(WP(p)+1))&0x00ffffff) << 8))
+#endif
+#define MUR_GETBLOCK(p,i) (MUR_PLUS0_ALIGNED(p) ? ((p)[i]) : \
+ (MUR_PLUS1_ALIGNED(p) ? MUR_THREE_ONE(p) : \
+ (MUR_PLUS2_ALIGNED(p) ? MUR_TWO_TWO(p) : \
+ MUR_ONE_THREE(p))))
+#endif
+#define MUR_ROTL32(x,r) (((x) << (r)) | ((x) >> (32 - (r))))
+#define MUR_FMIX(_h) \
+do { \
+ _h ^= _h >> 16; \
+ _h *= 0x85ebca6b; \
+ _h ^= _h >> 13; \
+ _h *= 0xc2b2ae35l; \
+ _h ^= _h >> 16; \
+} while(0)
+
+#define HASH_MUR(key,keylen,num_bkts,hashv,bkt) \
+do { \
+ const uint8_t *_mur_data = (const uint8_t*)(key); \
+ const int _mur_nblocks = (keylen) / 4; \
+ uint32_t _mur_h1 = 0xf88D5353; \
+ uint32_t _mur_c1 = 0xcc9e2d51; \
+ uint32_t _mur_c2 = 0x1b873593; \
+ uint32_t _mur_k1 = 0; \
+ const uint8_t *_mur_tail; \
+ const uint32_t *_mur_blocks = (const uint32_t*)(_mur_data+_mur_nblocks*4); \
+ int _mur_i; \
+ for(_mur_i = -_mur_nblocks; _mur_i; _mur_i++) { \
+ _mur_k1 = MUR_GETBLOCK(_mur_blocks,_mur_i); \
+ _mur_k1 *= _mur_c1; \
+ _mur_k1 = MUR_ROTL32(_mur_k1,15); \
+ _mur_k1 *= _mur_c2; \
+ \
+ _mur_h1 ^= _mur_k1; \
+ _mur_h1 = MUR_ROTL32(_mur_h1,13); \
+ _mur_h1 = _mur_h1*5+0xe6546b64; \
+ } \
+ _mur_tail = (const uint8_t*)(_mur_data + _mur_nblocks*4); \
+ _mur_k1=0; \
+ switch((keylen) & 3) { \
+ case 3: _mur_k1 ^= _mur_tail[2] << 16; \
+ case 2: _mur_k1 ^= _mur_tail[1] << 8; \
+ case 1: _mur_k1 ^= _mur_tail[0]; \
+ _mur_k1 *= _mur_c1; \
+ _mur_k1 = MUR_ROTL32(_mur_k1,15); \
+ _mur_k1 *= _mur_c2; \
+ _mur_h1 ^= _mur_k1; \
+ } \
+ _mur_h1 ^= (keylen); \
+ MUR_FMIX(_mur_h1); \
+ hashv = _mur_h1; \
+ bkt = hashv & (num_bkts-1); \
+} while(0)
+#endif /* HASH_USING_NO_STRICT_ALIASING */
+
+/* key comparison function; return 0 if keys equal */
+#define HASH_KEYCMP(a,b,len) memcmp(a,b,len)
+
+/* iterate over items in a known bucket to find desired item */
+#define HASH_FIND_IN_BKT(tbl,hh,head,keyptr,keylen_in,out) \
+do { \
+ if (head.hh_head) DECLTYPE_ASSIGN(out,ELMT_FROM_HH(tbl,head.hh_head)); \
+ else out=NULL; \
+ while (out) { \
+ if ((out)->hh.keylen == keylen_in) { \
+ if ((HASH_KEYCMP((out)->hh.key,keyptr,keylen_in)) == 0) break; \
+ } \
+ if ((out)->hh.hh_next) DECLTYPE_ASSIGN(out,ELMT_FROM_HH(tbl,(out)->hh.hh_next)); \
+ else out = NULL; \
+ } \
+} while(0)
+
+/* add an item to a bucket */
+#define HASH_ADD_TO_BKT(head,addhh) \
+do { \
+ head.count++; \
+ (addhh)->hh_next = head.hh_head; \
+ (addhh)->hh_prev = NULL; \
+ if (head.hh_head) { (head).hh_head->hh_prev = (addhh); } \
+ (head).hh_head=addhh; \
+ if (head.count >= ((head.expand_mult+1) * HASH_BKT_CAPACITY_THRESH) \
+ && (addhh)->tbl->noexpand != 1) { \
+ HASH_EXPAND_BUCKETS((addhh)->tbl); \
+ } \
+} while(0)
+
+/* remove an item from a given bucket */
+#define HASH_DEL_IN_BKT(hh,head,hh_del) \
+ (head).count--; \
+ if ((head).hh_head == hh_del) { \
+ (head).hh_head = hh_del->hh_next; \
+ } \
+ if (hh_del->hh_prev) { \
+ hh_del->hh_prev->hh_next = hh_del->hh_next; \
+ } \
+ if (hh_del->hh_next) { \
+ hh_del->hh_next->hh_prev = hh_del->hh_prev; \
+ }
+
+/* Bucket expansion has the effect of doubling the number of buckets
+ * and redistributing the items into the new buckets. Ideally the
+ * items will distribute more or less evenly into the new buckets
+ * (the extent to which this is true is a measure of the quality of
+ * the hash function as it applies to the key domain).
+ *
+ * With the items distributed into more buckets, the chain length
+ * (item count) in each bucket is reduced. Thus by expanding buckets
+ * the hash keeps a bound on the chain length. This bounded chain
+ * length is the essence of how a hash provides constant time lookup.
+ *
+ * The calculation of tbl->ideal_chain_maxlen below deserves some
+ * explanation. First, keep in mind that we're calculating the ideal
+ * maximum chain length based on the *new* (doubled) bucket count.
+ * In fractions this is just n/b (n=number of items,b=new num buckets).
+ * Since the ideal chain length is an integer, we want to calculate
+ * ceil(n/b). We don't depend on floating point arithmetic in this
+ * hash, so to calculate ceil(n/b) with integers we could write
+ *
+ * ceil(n/b) = (n/b) + ((n%b)?1:0)
+ *
+ * and in fact a previous version of this hash did just that.
+ * But now we have improved things a bit by recognizing that b is
+ * always a power of two. We keep its base 2 log handy (call it lb),
+ * so now we can write this with a bit shift and logical AND:
+ *
+ * ceil(n/b) = (n>>lb) + ( (n & (b-1)) ? 1:0)
+ *
+ */
+#define HASH_EXPAND_BUCKETS(tbl) \
+do { \
+ unsigned _he_bkt; \
+ unsigned _he_bkt_i; \
+ struct UT_hash_handle *_he_thh, *_he_hh_nxt; \
+ UT_hash_bucket *_he_new_buckets, *_he_newbkt; \
+ _he_new_buckets = (UT_hash_bucket*)uthash_malloc( \
+ 2 * tbl->num_buckets * sizeof(struct UT_hash_bucket)); \
+ if (!_he_new_buckets) { uthash_fatal( "out of memory"); } \
+ memset(_he_new_buckets, 0, \
+ 2 * tbl->num_buckets * sizeof(struct UT_hash_bucket)); \
+ tbl->ideal_chain_maxlen = \
+ (tbl->num_items >> (tbl->log2_num_buckets+1)) + \
+ ((tbl->num_items & ((tbl->num_buckets*2)-1)) ? 1 : 0); \
+ tbl->nonideal_items = 0; \
+ for(_he_bkt_i = 0; _he_bkt_i < tbl->num_buckets; _he_bkt_i++) \
+ { \
+ _he_thh = tbl->buckets[ _he_bkt_i ].hh_head; \
+ while (_he_thh) { \
+ _he_hh_nxt = _he_thh->hh_next; \
+ HASH_TO_BKT( _he_thh->hashv, tbl->num_buckets*2, _he_bkt); \
+ _he_newbkt = &(_he_new_buckets[ _he_bkt ]); \
+ if (++(_he_newbkt->count) > tbl->ideal_chain_maxlen) { \
+ tbl->nonideal_items++; \
+ _he_newbkt->expand_mult = _he_newbkt->count / \
+ tbl->ideal_chain_maxlen; \
+ } \
+ _he_thh->hh_prev = NULL; \
+ _he_thh->hh_next = _he_newbkt->hh_head; \
+ if (_he_newbkt->hh_head) _he_newbkt->hh_head->hh_prev = \
+ _he_thh; \
+ _he_newbkt->hh_head = _he_thh; \
+ _he_thh = _he_hh_nxt; \
+ } \
+ } \
+ uthash_free( tbl->buckets, tbl->num_buckets*sizeof(struct UT_hash_bucket) ); \
+ tbl->num_buckets *= 2; \
+ tbl->log2_num_buckets++; \
+ tbl->buckets = _he_new_buckets; \
+ tbl->ineff_expands = (tbl->nonideal_items > (tbl->num_items >> 1)) ? \
+ (tbl->ineff_expands+1) : 0; \
+ if (tbl->ineff_expands > 1) { \
+ tbl->noexpand=1; \
+ uthash_noexpand_fyi(tbl); \
+ } \
+ uthash_expand_fyi(tbl); \
+} while(0)
+
+
+/* This is an adaptation of Simon Tatham's O(n log(n)) mergesort */
+/* Note that HASH_SORT assumes the hash handle name to be hh.
+ * HASH_SRT was added to allow the hash handle name to be passed in. */
+#define HASH_SORT(head,cmpfcn) HASH_SRT(hh,head,cmpfcn)
+#define HASH_SRT(hh,head,cmpfcn) \
+do { \
+ unsigned _hs_i; \
+ unsigned _hs_looping,_hs_nmerges,_hs_insize,_hs_psize,_hs_qsize; \
+ struct UT_hash_handle *_hs_p, *_hs_q, *_hs_e, *_hs_list, *_hs_tail; \
+ if (head) { \
+ _hs_insize = 1; \
+ _hs_looping = 1; \
+ _hs_list = &((head)->hh); \
+ while (_hs_looping) { \
+ _hs_p = _hs_list; \
+ _hs_list = NULL; \
+ _hs_tail = NULL; \
+ _hs_nmerges = 0; \
+ while (_hs_p) { \
+ _hs_nmerges++; \
+ _hs_q = _hs_p; \
+ _hs_psize = 0; \
+ for ( _hs_i = 0; _hs_i < _hs_insize; _hs_i++ ) { \
+ _hs_psize++; \
+ _hs_q = (UT_hash_handle*)((_hs_q->next) ? \
+ ((void*)((char*)(_hs_q->next) + \
+ (head)->hh.tbl->hho)) : NULL); \
+ if (! (_hs_q) ) break; \
+ } \
+ _hs_qsize = _hs_insize; \
+ while ((_hs_psize > 0) || ((_hs_qsize > 0) && _hs_q )) { \
+ if (_hs_psize == 0) { \
+ _hs_e = _hs_q; \
+ _hs_q = (UT_hash_handle*)((_hs_q->next) ? \
+ ((void*)((char*)(_hs_q->next) + \
+ (head)->hh.tbl->hho)) : NULL); \
+ _hs_qsize--; \
+ } else if ( (_hs_qsize == 0) || !(_hs_q) ) { \
+ _hs_e = _hs_p; \
+ if (_hs_p){ \
+ _hs_p = (UT_hash_handle*)((_hs_p->next) ? \
+ ((void*)((char*)(_hs_p->next) + \
+ (head)->hh.tbl->hho)) : NULL); \
+ } \
+ _hs_psize--; \
+ } else if (( \
+ cmpfcn(DECLTYPE(head)(ELMT_FROM_HH((head)->hh.tbl,_hs_p)), \
+ DECLTYPE(head)(ELMT_FROM_HH((head)->hh.tbl,_hs_q))) \
+ ) <= 0) { \
+ _hs_e = _hs_p; \
+ if (_hs_p){ \
+ _hs_p = (UT_hash_handle*)((_hs_p->next) ? \
+ ((void*)((char*)(_hs_p->next) + \
+ (head)->hh.tbl->hho)) : NULL); \
+ } \
+ _hs_psize--; \
+ } else { \
+ _hs_e = _hs_q; \
+ _hs_q = (UT_hash_handle*)((_hs_q->next) ? \
+ ((void*)((char*)(_hs_q->next) + \
+ (head)->hh.tbl->hho)) : NULL); \
+ _hs_qsize--; \
+ } \
+ if ( _hs_tail ) { \
+ _hs_tail->next = ((_hs_e) ? \
+ ELMT_FROM_HH((head)->hh.tbl,_hs_e) : NULL); \
+ } else { \
+ _hs_list = _hs_e; \
+ } \
+ if (_hs_e) { \
+ _hs_e->prev = ((_hs_tail) ? \
+ ELMT_FROM_HH((head)->hh.tbl,_hs_tail) : NULL); \
+ } \
+ _hs_tail = _hs_e; \
+ } \
+ _hs_p = _hs_q; \
+ } \
+ if (_hs_tail){ \
+ _hs_tail->next = NULL; \
+ } \
+ if ( _hs_nmerges <= 1 ) { \
+ _hs_looping=0; \
+ (head)->hh.tbl->tail = _hs_tail; \
+ DECLTYPE_ASSIGN(head,ELMT_FROM_HH((head)->hh.tbl, _hs_list)); \
+ } \
+ _hs_insize *= 2; \
+ } \
+ HASH_FSCK(hh,head); \
+ } \
+} while (0)
+
+/* This function selects items from one hash into another hash.
+ * The end result is that the selected items have dual presence
+ * in both hashes. There is no copy of the items made; rather
+ * they are added into the new hash through a secondary hash
+ * hash handle that must be present in the structure. */
+#define HASH_SELECT(hh_dst, dst, hh_src, src, cond) \
+do { \
+ unsigned _src_bkt, _dst_bkt; \
+ void *_last_elt=NULL, *_elt; \
+ UT_hash_handle *_src_hh, *_dst_hh, *_last_elt_hh=NULL; \
+ ptrdiff_t _dst_hho = ((char*)(&(dst)->hh_dst) - (char*)(dst)); \
+ if (src) { \
+ for(_src_bkt=0; _src_bkt < (src)->hh_src.tbl->num_buckets; _src_bkt++) { \
+ for(_src_hh = (src)->hh_src.tbl->buckets[_src_bkt].hh_head; \
+ _src_hh; \
+ _src_hh = _src_hh->hh_next) { \
+ _elt = ELMT_FROM_HH((src)->hh_src.tbl, _src_hh); \
+ if (cond(_elt)) { \
+ _dst_hh = (UT_hash_handle*)(((char*)_elt) + _dst_hho); \
+ _dst_hh->key = _src_hh->key; \
+ _dst_hh->keylen = _src_hh->keylen; \
+ _dst_hh->hashv = _src_hh->hashv; \
+ _dst_hh->prev = _last_elt; \
+ _dst_hh->next = NULL; \
+ if (_last_elt_hh) { _last_elt_hh->next = _elt; } \
+ if (!dst) { \
+ DECLTYPE_ASSIGN(dst,_elt); \
+ HASH_MAKE_TABLE(hh_dst,dst); \
+ } else { \
+ _dst_hh->tbl = (dst)->hh_dst.tbl; \
+ } \
+ HASH_TO_BKT(_dst_hh->hashv, _dst_hh->tbl->num_buckets, _dst_bkt); \
+ HASH_ADD_TO_BKT(_dst_hh->tbl->buckets[_dst_bkt],_dst_hh); \
+ (dst)->hh_dst.tbl->num_items++; \
+ _last_elt = _elt; \
+ _last_elt_hh = _dst_hh; \
+ } \
+ } \
+ } \
+ } \
+ HASH_FSCK(hh_dst,dst); \
+} while (0)
+
+#define HASH_CLEAR(hh,head) \
+do { \
+ if (head) { \
+ uthash_free((head)->hh.tbl->buckets, \
+ (head)->hh.tbl->num_buckets*sizeof(struct UT_hash_bucket)); \
+ HASH_BLOOM_FREE((head)->hh.tbl); \
+ uthash_free((head)->hh.tbl, sizeof(UT_hash_table)); \
+ (head)=NULL; \
+ } \
+} while(0)
+
+#define HASH_OVERHEAD(hh,head) \
+ (size_t)((((head)->hh.tbl->num_items * sizeof(UT_hash_handle)) + \
+ ((head)->hh.tbl->num_buckets * sizeof(UT_hash_bucket)) + \
+ (sizeof(UT_hash_table)) + \
+ (HASH_BLOOM_BYTELEN)))
+
+#ifdef NO_DECLTYPE
+#define HASH_ITER(hh,head,el,tmp) \
+for((el)=(head), (*(char**)(&(tmp)))=(char*)((head)?(head)->hh.next:NULL); \
+ el; (el)=(tmp),(*(char**)(&(tmp)))=(char*)((tmp)?(tmp)->hh.next:NULL))
+#else
+#define HASH_ITER(hh,head,el,tmp) \
+for((el)=(head),(tmp)=DECLTYPE(el)((head)?(head)->hh.next:NULL); \
+ el; (el)=(tmp),(tmp)=DECLTYPE(el)((tmp)?(tmp)->hh.next:NULL))
+#endif
+
+/* obtain a count of items in the hash */
+#define HASH_COUNT(head) HASH_CNT(hh,head)
+#define HASH_CNT(hh,head) ((head)?((head)->hh.tbl->num_items):0)
+
+typedef struct UT_hash_bucket {
+ struct UT_hash_handle *hh_head;
+ unsigned count;
+
+ /* expand_mult is normally set to 0. In this situation, the max chain length
+ * threshold is enforced at its default value, HASH_BKT_CAPACITY_THRESH. (If
+ * the bucket's chain exceeds this length, bucket expansion is triggered).
+ * However, setting expand_mult to a non-zero value delays bucket expansion
+ * (that would be triggered by additions to this particular bucket)
+ * until its chain length reaches a *multiple* of HASH_BKT_CAPACITY_THRESH.
+ * (The multiplier is simply expand_mult+1). The whole idea of this
+ * multiplier is to reduce bucket expansions, since they are expensive, in
+ * situations where we know that a particular bucket tends to be overused.
+ * It is better to let its chain length grow to a longer yet-still-bounded
+ * value, than to do an O(n) bucket expansion too often.
+ */
+ unsigned expand_mult;
+
+} UT_hash_bucket;
+
+/* random signature used only to find hash tables in external analysis */
+#define HASH_SIGNATURE 0xa0111fe1
+#define HASH_BLOOM_SIGNATURE 0xb12220f2
+
+typedef struct UT_hash_table {
+ UT_hash_bucket *buckets;
+ unsigned num_buckets, log2_num_buckets;
+ unsigned num_items;
+ struct UT_hash_handle *tail; /* tail hh in app order, for fast append */
+ ptrdiff_t hho; /* hash handle offset (byte pos of hash handle in element */
+
+ /* in an ideal situation (all buckets used equally), no bucket would have
+ * more than ceil(#items/#buckets) items. that's the ideal chain length. */
+ unsigned ideal_chain_maxlen;
+
+ /* nonideal_items is the number of items in the hash whose chain position
+ * exceeds the ideal chain maxlen. these items pay the penalty for an uneven
+ * hash distribution; reaching them in a chain traversal takes >ideal steps */
+ unsigned nonideal_items;
+
+ /* ineffective expands occur when a bucket doubling was performed, but
+ * afterward, more than half the items in the hash had nonideal chain
+ * positions. If this happens on two consecutive expansions we inhibit any
+ * further expansion, as it's not helping; this happens when the hash
+ * function isn't a good fit for the key domain. When expansion is inhibited
+ * the hash will still work, albeit no longer in constant time. */
+ unsigned ineff_expands, noexpand;
+
+ uint32_t signature; /* used only to find hash tables in external analysis */
+#ifdef HASH_BLOOM
+ uint32_t bloom_sig; /* used only to test bloom exists in external analysis */
+ uint8_t *bloom_bv;
+ char bloom_nbits;
+#endif
+
+} UT_hash_table;
+
+typedef struct UT_hash_handle {
+ struct UT_hash_table *tbl;
+ void *prev; /* prev element in app order */
+ void *next; /* next element in app order */
+ struct UT_hash_handle *hh_prev; /* previous hh in bucket order */
+ struct UT_hash_handle *hh_next; /* next hh in bucket order */
+ void *key; /* ptr to enclosing struct's key */
+ unsigned keylen; /* enclosing struct's key len */
+ unsigned hashv; /* result of hash-fcn(key) */
+} UT_hash_handle;
+
+#endif /* UTHASH_H */
diff --git a/drive-sdk/examples/vehicle-scan/vehicle-scan.c b/drive-sdk/examples/vehicle-scan/vehicle-scan.c
new file mode 100644
index 0000000..4ecc7e2
--- /dev/null
+++ b/drive-sdk/examples/vehicle-scan/vehicle-scan.c
@@ -0,0 +1,471 @@
+/*
+ * vehicle-scan.c
+ *
+ * drive-sdk - Anki Drive SDK
+ *
+ * Copyright (C) 2014 Anki, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * Portions of this software are derived from BlueZ, a Bluetooth protocol stack for
+ * Linux. The license for BlueZ is included below.
+ */
+
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2000-2001 Qualcomm Incorporated
+ * Copyright (C) 2002-2003 Maxim Krasnyansky <maxk@qualcomm.com>
+ * Copyright (C) 2002-2010 Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#include <stdio.h>
+#include <errno.h>
+#include <ctype.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <sys/param.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <signal.h>
+
+#define for_each_opt(opt, long, short) while ((opt=getopt_long(argc, argv, short ? short:"+", long, NULL)) != -1)
+
+#include <bzle/bluetooth/bluetooth.h>
+#include <bzle/bluetooth/hci.h>
+#include <bzle/bluetooth/hci_lib.h>
+
+#include "uthash.h"
+
+#include <ankidrive/advertisement.h>
+
+/* Unofficial value, might still change */
+#define LE_LINK 0x03
+
+#define FLAGS_AD_TYPE 0x01
+#define FLAGS_LIMITED_MODE_BIT 0x01
+#define FLAGS_GENERAL_MODE_BIT 0x02
+
+#define EIR_FLAGS 0x01 /* flags */
+#define EIR_UUID16_SOME 0x02 /* 16-bit UUID, more available */
+#define EIR_UUID16_ALL 0x03 /* 16-bit UUID, all listed */
+#define EIR_UUID32_SOME 0x04 /* 32-bit UUID, more available */
+#define EIR_UUID32_ALL 0x05 /* 32-bit UUID, all listed */
+#define EIR_UUID128_SOME 0x06 /* 128-bit UUID, more available */
+#define EIR_UUID128_ALL 0x07 /* 128-bit UUID, all listed */
+#define EIR_NAME_SHORT 0x08 /* shortened local name */
+#define EIR_NAME_COMPLETE 0x09 /* complete local name */
+#define EIR_TX_POWER 0x0A /* transmit power level */
+#define EIR_DEVICE_ID 0x10 /* device ID */
+
+struct _vehicle {
+ bdaddr_t address;
+ anki_vehicle_adv_t adv;
+ uint8_t scan_complete;
+ UT_hash_handle hh;
+};
+typedef struct _vehicle vehicle_t;
+
+static volatile int signal_received = 0;
+
+static void hex_dump(char *pref, int width, unsigned char *buf, int len)
+{
+ register int i,n;
+
+ for (i = 0, n = 1; i < len; i++, n++) {
+ if (n == 1)
+ printf("%s", pref);
+ printf("%2.2X ", buf[i]);
+ if (n == width) {
+ printf("\n");
+ n = 0;
+ }
+ }
+ if (i && n!=1)
+ printf("\n");
+}
+
+static struct option lescan_options[] = {
+ { "help", 0, 0, 'h' },
+ { "privacy", 0, 0, 'p' },
+ { "passive", 0, 0, 'P' },
+ { "whitelist", 0, 0, 'w' },
+ { "discovery", 1, 0, 'd' },
+ { "duplicates", 0, 0, 'D' },
+ { 0, 0, 0, 0 }
+};
+
+static const char *lescan_help =
+ "Usage:\n"
+ "\tlescan [--privacy] enable privacy\n"
+ "\tlescan [--passive] set scan type passive (default active)\n"
+ "\tlescan [--whitelist] scan for address in the whitelist only\n"
+ "\tlescan [--discovery=g|l] enable general or limited discovery"
+ "procedure\n"
+ "\tlescan [--duplicates] don't filter duplicates\n";
+
+static void helper_arg(int min_num_arg, int max_num_arg, int *argc,
+ char ***argv, const char *usage)
+{
+ *argc -= optind;
+ /* too many arguments, but when "max_num_arg < min_num_arg" then no
+ limiting (prefer "max_num_arg=-1" to gen infinity)
+ */
+ if ( (*argc > max_num_arg) && (max_num_arg >= min_num_arg ) ) {
+ fprintf(stderr, "%s: too many arguments (maximal: %i)\n",
+ *argv[0], max_num_arg);
+ printf("%s", usage);
+ exit(1);
+ }
+
+ /* print usage */
+ if (*argc < min_num_arg) {
+ fprintf(stderr, "%s: too few arguments (minimal: %i)\n",
+ *argv[0], min_num_arg);
+ printf("%s", usage);
+ exit(0);
+ }
+
+ *argv += optind;
+}
+
+
+static void sigint_handler(int sig)
+{
+ signal_received = sig;
+}
+
+static int read_flags(uint8_t *flags, const uint8_t *data, size_t size)
+{
+ size_t offset;
+
+ if (!flags || !data)
+ return -EINVAL;
+
+ offset = 0;
+ while (offset < size) {
+ uint8_t len = data[offset];
+ uint8_t type;
+
+ /* Check if it is the end of the significant part */
+ if (len == 0)
+ break;
+
+ if (len + offset > size)
+ break;
+
+ type = data[offset + 1];
+
+ if (type == FLAGS_AD_TYPE) {
+ *flags = data[offset + 2];
+ return 0;
+ }
+
+ offset += 1 + len;
+ }
+
+ return -ENOENT;
+}
+
+static int check_report_filter(uint8_t procedure, le_advertising_info *info)
+{
+ uint8_t flags;
+
+ /* If no discovery procedure is set, all reports are treat as valid */
+ if (procedure == 0)
+ return 1;
+
+ /* Read flags AD type value from the advertising report if it exists */
+ if (read_flags(&flags, info->data, info->length))
+ return 0;
+
+ switch (procedure) {
+ case 'l': /* Limited Discovery Procedure */
+ if (flags & FLAGS_LIMITED_MODE_BIT)
+ return 1;
+ break;
+ case 'g': /* General Discovery Procedure */
+ if (flags & (FLAGS_LIMITED_MODE_BIT | FLAGS_GENERAL_MODE_BIT))
+ return 1;
+ break;
+ default:
+ fprintf(stderr, "Unknown discovery procedure\n");
+ }
+
+ return 0;
+}
+
+static void eir_parse_name(uint8_t *eir, size_t eir_len,
+ char *buf, size_t buf_len)
+{
+ size_t offset;
+
+ offset = 0;
+ while (offset < eir_len) {
+ uint8_t field_len = eir[0];
+ size_t name_len;
+
+ /* Check for the end of EIR */
+ if (field_len == 0)
+ break;
+
+ if (offset + field_len > eir_len)
+ goto failed;
+
+ printf("eir_type: %02x: ", eir[1]);
+ hex_dump("0x", 32, &eir[2], field_len-1);
+
+ switch (eir[1]) {
+ case EIR_NAME_SHORT:
+ case EIR_NAME_COMPLETE:
+ name_len = field_len - 1;
+ if (name_len > buf_len)
+ goto failed;
+
+ memcpy(buf, &eir[2], name_len);
+ return;
+ }
+
+ offset += field_len + 1;
+ eir += field_len + 1;
+ }
+
+failed:
+ snprintf(buf, buf_len, "(unknown)");
+}
+
+vehicle_t *vehicles = NULL;
+
+static int print_advertising_devices(int dd, uint8_t filter_type)
+{
+ unsigned char buf[HCI_MAX_EVENT_SIZE], *ptr;
+ struct hci_filter nf, of;
+ struct sigaction sa;
+ socklen_t olen;
+ int len;
+
+ olen = sizeof(of);
+ if (getsockopt(dd, SOL_HCI, HCI_FILTER, &of, &olen) < 0) {
+ printf("Could not get socket options\n");
+ return -1;
+ }
+
+ hci_filter_clear(&nf);
+ hci_filter_set_ptype(HCI_EVENT_PKT, &nf);
+ hci_filter_set_event(EVT_LE_META_EVENT, &nf);
+
+ if (setsockopt(dd, SOL_HCI, HCI_FILTER, &nf, sizeof(nf)) < 0) {
+ printf("Could not set socket options\n");
+ return -1;
+ }
+
+ memset(&sa, 0, sizeof(sa));
+ sa.sa_flags = SA_NOCLDSTOP;
+ sa.sa_handler = sigint_handler;
+ sigaction(SIGINT, &sa, NULL);
+
+ while (1) {
+ evt_le_meta_event *meta;
+ le_advertising_info *info;
+ char addr[18];
+
+ while ((len = read(dd, buf, sizeof(buf))) < 0) {
+ if (errno == EINTR && signal_received == SIGINT) {
+ len = 0;
+ goto done;
+ }
+
+ if (errno == EAGAIN || errno == EINTR)
+ continue;
+ goto done;
+ }
+
+ ptr = buf + (1 + HCI_EVENT_HDR_SIZE);
+ len -= (1 + HCI_EVENT_HDR_SIZE);
+
+ meta = (void *) ptr;
+
+ if (meta->subevent != 0x02)
+ goto done;
+
+ /* Ignoring multiple reports */
+ info = (le_advertising_info *) (meta->data + 1);
+ if (check_report_filter(filter_type, info)) {
+ char name[30];
+
+ memset(name, 0, sizeof(name));
+
+ ba2str(&info->bdaddr, addr);
+
+ vehicle_t vehicle;
+ vehicle_t *v = &vehicle;
+
+ HASH_FIND(hh, vehicles, &info->bdaddr, sizeof(bdaddr_t), v);
+ if (v == NULL) {
+ v = (vehicle_t *)malloc(sizeof(vehicle_t));
+ memset(&v->adv, 0, sizeof(anki_vehicle_adv_t));
+ memcpy(&v->address, &info->bdaddr, sizeof(bdaddr_t));
+ HASH_ADD(hh, vehicles, address, sizeof(bdaddr_t), v);
+ }
+
+ int err = anki_vehicle_parse_adv_record(info->data, info->length, &v->adv);
+
+ if (err == 0 && v->adv.mfg_data.identifier > 0 && v->adv.local_name.version > 0 && !v->scan_complete) {
+ v->scan_complete = 1;
+ printf("%s ", addr);
+
+ const char *model = NULL;
+ switch(v->adv.mfg_data.model_id) {
+ case 1:
+ model = "Kourai";
+ break;
+ case 2:
+ model = "Boson";
+ break;
+ case 3:
+ model = "Rho";
+ break;
+ case 4:
+ model = "Katal";
+ break;
+ default:
+ model = "Unknown";
+ break;
+ }
+
+ printf("%s [v%04x] (%s %04x)\n", v->adv.local_name.name, v->adv.local_name.version & 0xffff, model, v->adv.mfg_data.identifier & 0xffff);
+ }
+ }
+ }
+
+done:
+ setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of));
+
+ if (len < 0)
+ return -1;
+
+ return 0;
+}
+
+static void cmd_lescan(int dev_id, int argc, char **argv)
+{
+ int err, opt, dd;
+ uint8_t own_type = 0x00;
+ uint8_t scan_type = 0x01;
+ uint8_t filter_type = 0;
+ uint8_t filter_policy = 0x00;
+ uint16_t interval = htobs(0x0010);
+ uint16_t window = htobs(0x0010);
+ uint8_t filter_dup = 1;
+
+ for_each_opt(opt, lescan_options, NULL) {
+ switch (opt) {
+ case 'p':
+ own_type = 0x01; /* Random */
+ break;
+ case 'P':
+ scan_type = 0x00; /* Passive */
+ break;
+ case 'w':
+ filter_policy = 0x01; /* Whitelist */
+ break;
+ case 'd':
+ filter_type = optarg[0];
+ if (filter_type != 'g' && filter_type != 'l') {
+ fprintf(stderr, "Unknown discovery procedure\n");
+ exit(1);
+ }
+
+ interval = htobs(0x0012);
+ window = htobs(0x0012);
+ break;
+ case 'D':
+ filter_dup = 0x00;
+ break;
+ default:
+ printf("%s", lescan_help);
+ return;
+ }
+ }
+ helper_arg(0, 1, &argc, &argv, lescan_help);
+
+ if (dev_id < 0)
+ dev_id = hci_get_route(NULL);
+
+ dd = hci_open_dev(dev_id);
+ if (dd < 0) {
+ perror("Could not open device");
+ exit(1);
+ exit(1);
+ }
+
+ err = hci_le_set_scan_parameters(dd, scan_type, interval, window,
+ own_type, filter_policy, 2000);
+ if (err < 0) {
+ perror("Set scan parameters failed");
+ exit(1);
+ }
+
+ err = hci_le_set_scan_enable(dd, 0x01, filter_dup, 2000);
+ if (err < 0) {
+ perror("Enable scan failed");
+ exit(1);
+ }
+
+ printf("LE Scan ...\n");
+
+ err = print_advertising_devices(dd, filter_type);
+ if (err < 0) {
+ perror("Could not receive advertising events");
+ exit(1);
+ }
+
+ err = hci_le_set_scan_enable(dd, 0x00, filter_dup, 2000);
+ if (err < 0) {
+ fprintf(stderr, "Disable scan failed with error code: %d", err);
+ hci_close_dev(dd);
+ exit(1);
+ }
+
+ hci_close_dev(dd);
+}
+
+int main(int argc, char *argv[]) {
+ int opt, i, dev_id = -1;
+
+ cmd_lescan(dev_id, argc, argv);
+
+ return 0;
+}
diff --git a/drive-sdk/examples/vehicle-tool/.gitignore b/drive-sdk/examples/vehicle-tool/.gitignore
new file mode 100644
index 0000000..61ceb70
--- /dev/null
+++ b/drive-sdk/examples/vehicle-tool/.gitignore
@@ -0,0 +1,4 @@
+.deps
+.dirstamp
+vehicle-tool
+
diff --git a/drive-sdk/examples/vehicle-tool/CMakeLists.txt b/drive-sdk/examples/vehicle-tool/CMakeLists.txt
new file mode 100755
index 0000000..58a67d3
--- /dev/null
+++ b/drive-sdk/examples/vehicle-tool/CMakeLists.txt
@@ -0,0 +1,31 @@
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+
+SET (CMAKE_C_FLAGS "")
+
+include(FindGLIB2)
+include(FindReadline)
+
+include_directories(${drivekit_SOURCE_DIR}/include
+ ${bzle_SOURCE_DIR}/include
+ ${GLIB2_INCLUDE_DIRS}
+ ${READLINE_INCLUDE_DIR}
+ )
+
+
+# Add sources
+set(vehicleTool_SOURCES
+ vehicle_tool.c
+ vehicle_cmd.c
+ display.c
+)
+
+add_executable(vehicle-tool ${vehicleTool_SOURCES})
+target_link_libraries(vehicle-tool
+ ankidrive
+ bzle
+ ${GLIB2_LIBRARIES}
+ ${READLINE_LIBRARY}
+)
+install(TARGETS vehicle-tool
+ RUNTIME DESTINATION bin
+)
diff --git a/drive-sdk/examples/vehicle-tool/display.c b/drive-sdk/examples/vehicle-tool/display.c
new file mode 100644
index 0000000..bc891af
--- /dev/null
+++ b/drive-sdk/examples/vehicle-tool/display.c
@@ -0,0 +1,64 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2012 Intel Corporation. All rights reserved.
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <stdbool.h>
+#include <readline/readline.h>
+
+#include "display.h"
+
+void rl_printf(const char *fmt, ...)
+{
+ va_list args;
+ bool save_input;
+ char *saved_line;
+ int saved_point;
+
+ save_input = !RL_ISSTATE(RL_STATE_DONE);
+
+ if (save_input) {
+ saved_point = rl_point;
+ saved_line = rl_copy_text(0, rl_end);
+ rl_save_prompt();
+ rl_replace_line("", 0);
+ rl_redisplay();
+ }
+
+ va_start(args, fmt);
+ vprintf(fmt, args);
+ va_end(args);
+
+ if (save_input) {
+ rl_restore_prompt();
+ rl_replace_line(saved_line, 0);
+ rl_point = saved_point;
+ rl_redisplay();
+ free(saved_line);
+ }
+}
diff --git a/drive-sdk/examples/vehicle-tool/display.h b/drive-sdk/examples/vehicle-tool/display.h
new file mode 100644
index 0000000..91a0be9
--- /dev/null
+++ b/drive-sdk/examples/vehicle-tool/display.h
@@ -0,0 +1,32 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2012 Intel Corporation. All rights reserved.
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#define COLOR_OFF "\x1B[0m"
+#define COLOR_RED "\x1B[0;91m"
+#define COLOR_GREEN "\x1B[0;92m"
+#define COLOR_YELLOW "\x1B[0;93m"
+#define COLOR_BLUE "\x1B[0;94m"
+#define COLOR_BOLDGRAY "\x1B[1;30m"
+#define COLOR_BOLDWHITE "\x1B[1;37m"
+
+void rl_printf(const char *fmt, ...) __attribute__((format(printf, 1, 2)));
diff --git a/drive-sdk/examples/vehicle-tool/vehicle_cmd.c b/drive-sdk/examples/vehicle-tool/vehicle_cmd.c
new file mode 100644
index 0000000..172a922
--- /dev/null
+++ b/drive-sdk/examples/vehicle-tool/vehicle_cmd.c
@@ -0,0 +1,1155 @@
+/*
+ * vehicle-tool
+ *
+ * Example vehicle control tool for Anki Drive SDK
+ *
+ * Copyright (C) 2014 Anki, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * Portions of this software are derived from BlueZ, a Bluetooth protocol stack for
+ * Linux. The license for BlueZ is included below.
+ */
+
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2011 Nokia Corporation
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <errno.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <signal.h>
+#include <sys/signalfd.h>
+#include <glib.h>
+
+#include <readline/readline.h>
+#include <readline/history.h>
+
+#include <bzle/bluetooth/uuid.h>
+#include <bzle/bluetooth/btio.h>
+#include <bzle/gatt/att.h>
+#include <bzle/gatt/gattrib.h>
+#include <bzle/gatt/gatt.h>
+#include <bzle/gatt/utils.h>
+#include "display.h"
+
+#include <ankidrive.h>
+
+static GIOChannel *iochannel = NULL;
+static GAttrib *attrib = NULL;
+static GMainLoop *event_loop;
+static GString *prompt;
+
+static char *opt_src = NULL;
+static char *opt_dst = NULL;
+static char *opt_dst_type = NULL;
+static char *opt_sec_level = NULL;
+static int opt_psm = 0;
+static int opt_mtu = 0;
+static int start;
+static int end;
+
+
+typedef struct anki_vehicle {
+ struct gatt_char read_char;
+ struct gatt_char write_char;
+} anki_vehicle_t;
+
+static anki_vehicle_t vehicle;
+
+static char *effects_by_name[] = { "STEADY", "FADE", "THROB", "FLASH", "RANDOM", NULL };
+static uint8_t effect_invalid = 0xff;
+static char *channels_by_name[] = { "RED", "TAIL", "BLUE", "GREEN", "FRONTL", "FRONTR", NULL };
+static uint8_t channel_invalid = 0xff;
+static char* turn_types_by_name[] = { "NONE", "LEFT", "RIGHT", "UTURN", "UTURN_JUMP", NULL };
+
+static void discover_services(void);
+static void cmd_help(int argcp, char **argvp);
+
+static enum state {
+ STATE_DISCONNECTED,
+ STATE_CONNECTING,
+ STATE_CONNECTED
+} conn_state;
+
+#define error(fmt, arg...) \
+ rl_printf(COLOR_RED "Error: " COLOR_OFF fmt, ## arg)
+
+#define failed(fmt, arg...) \
+ rl_printf(COLOR_RED "Command Failed: " COLOR_OFF fmt, ## arg)
+
+static char *get_prompt(void)
+{
+ if (conn_state == STATE_CONNECTED)
+ g_string_assign(prompt, COLOR_BLUE);
+ else
+ g_string_assign(prompt, "");
+
+ if (opt_dst)
+ g_string_append_printf(prompt, "[%17s]", opt_dst);
+ else
+ g_string_append_printf(prompt, "[%17s]", "");
+
+ if (conn_state == STATE_CONNECTED)
+ g_string_append(prompt, COLOR_OFF);
+
+ if (opt_psm)
+ g_string_append(prompt, "[BR]");
+ else
+ g_string_append(prompt, "[LE]");
+
+ g_string_append(prompt, "> ");
+
+ return prompt->str;
+}
+
+
+static void set_state(enum state st)
+{
+ conn_state = st;
+ rl_set_prompt(get_prompt());
+}
+
+static void handle_vehicle_msg_response(const uint8_t *data, uint16_t len)
+{
+ if (len > sizeof(anki_vehicle_msg_t)) {
+ error("Invalid vehicle response\n");
+ return;
+ }
+
+ const anki_vehicle_msg_t *msg = (const anki_vehicle_msg_t *)data;
+ switch(msg->msg_id) {
+ case ANKI_VEHICLE_MSG_V2C_PING_RESPONSE:
+ {
+ rl_printf("[read] PING_RESPONSE\n");
+ break;
+ }
+ case ANKI_VEHICLE_MSG_V2C_VERSION_RESPONSE:
+ {
+ const anki_vehicle_msg_version_response_t *m = (const anki_vehicle_msg_version_response_t *)msg;
+ rl_printf("[read] VERSION_RESPONSE: 0x%04x\n", m->version);
+
+ break;
+ }
+ default:
+ // rl_printf("Received unhandled vehicle message of type 0x%02x\n", msg->msg_id);
+ break;
+ }
+}
+
+static void events_handler(const uint8_t *pdu, uint16_t len, gpointer user_data)
+{
+ uint16_t handle = att_get_u16(&pdu[1]);
+
+ if (pdu[0] == ATT_OP_HANDLE_NOTIFY) {
+ uint16_t handle = att_get_u16(&pdu[1]);
+ if (handle != vehicle.read_char.value_handle) {
+ error("Invalid vehicle read handle: 0x%04x\n", handle);
+ return;
+ }
+ const uint8_t *data = &pdu[3];
+ const uint16_t datalen = len-3;
+
+ handle_vehicle_msg_response(data, datalen);
+ return;
+ }
+}
+
+static void connect_cb(GIOChannel *io, GError *err, gpointer user_data)
+{
+ if (err) {
+ set_state(STATE_DISCONNECTED);
+ error("%s\n", err->message);
+ return;
+ }
+
+ attrib = g_attrib_new(iochannel);
+ g_attrib_register(attrib, ATT_OP_HANDLE_NOTIFY, GATTRIB_ALL_HANDLES,
+ events_handler, attrib, NULL);
+ g_attrib_register(attrib, ATT_OP_HANDLE_IND, GATTRIB_ALL_HANDLES,
+ events_handler, attrib, NULL);
+ set_state(STATE_CONNECTED);
+ rl_printf("Connection successful\n");
+
+ discover_services();
+}
+
+static void disconnect_io()
+{
+ if (conn_state == STATE_DISCONNECTED)
+ return;
+
+ g_attrib_unref(attrib);
+ attrib = NULL;
+ opt_mtu = 0;
+
+ g_io_channel_shutdown(iochannel, FALSE, NULL);
+ g_io_channel_unref(iochannel);
+ iochannel = NULL;
+
+ set_state(STATE_DISCONNECTED);
+}
+
+static void discover_char_cb(guint8 status, GSList *characteristics, gpointer user_data)
+{
+ GSList *l;
+
+ if (status) {
+ error("Discover all characteristics failed: %s\n",
+ att_ecode2str(status));
+ return;
+ }
+
+ for (l = characteristics; l; l = l->next) {
+ struct gatt_char *chars = l->data;
+
+ if (strncasecmp(chars->uuid, ANKI_STR_CHR_READ_UUID, strlen(ANKI_STR_CHR_READ_UUID)) == 0) {
+ memmove(&(vehicle.read_char), chars, sizeof(struct gatt_char));
+ rl_printf("Anki Read Characteristic: %s ", chars->uuid);
+ rl_printf("[handle: 0x%04x, char properties: 0x%02x, char value "
+ "handle: 0x%04x]\n", chars->handle,
+ chars->properties, chars->value_handle);
+ }
+
+ if (strncasecmp(chars->uuid, ANKI_STR_CHR_WRITE_UUID, strlen(ANKI_STR_CHR_WRITE_UUID)) == 0) {
+ memmove(&(vehicle.write_char), chars, sizeof(struct gatt_char));
+ rl_printf("Anki Write Characteristic: %s ", chars->uuid);
+ rl_printf("[handle: 0x%04x, char properties: 0x%02x, char value "
+ "handle: 0x%04x]\n", chars->handle,
+ chars->properties, chars->value_handle);
+ }
+ }
+
+ if (vehicle.read_char.handle > 0 && vehicle.write_char.handle > 0) {
+ // register for notifications when the vehicle sends data.
+ // We do this by setting the notification bit on the
+ // client configuration characteristic:
+ // see:
+ // https://developer.bluetooth.org/gatt/descriptors/Pages/DescriptorViewer.aspx?u=org.bluetooth.descriptor.gatt.client_characteristic_configuration.xml
+ uint8_t notify_cmd[] = { 0x01, 0x00 };
+ gatt_write_cmd(attrib, vehicle.write_char.properties, notify_cmd, 2, NULL, NULL);
+ }
+}
+
+static void char_read_cb(guint8 status, const guint8 *pdu, guint16 plen,
+ gpointer user_data)
+{
+ if (status != 0) {
+ error("Characteristic value/descriptor read failed: %s\n",
+ att_ecode2str(status));
+ return;
+ }
+
+ uint8_t value[plen];
+ ssize_t vlen = dec_read_resp(pdu, plen, value, sizeof(value));
+ if (vlen < 0) {
+ error("Protocol error\n");
+ return;
+ }
+
+ handle_vehicle_msg_response(value, vlen);
+}
+
+static void cmd_exit(int argcp, char **argvp)
+{
+ rl_callback_handler_remove();
+ g_main_loop_quit(event_loop);
+}
+
+static gboolean channel_watcher(GIOChannel *chan, GIOCondition cond,
+ gpointer user_data)
+{
+ disconnect_io();
+
+ return FALSE;
+}
+
+static void cmd_connect(int argcp, char **argvp)
+{
+ GError *gerr = NULL;
+
+ if (conn_state != STATE_DISCONNECTED)
+ return;
+
+ if (argcp > 1) {
+ g_free(opt_dst);
+ opt_dst = g_strdup(argvp[1]);
+
+ g_free(opt_dst_type);
+ if (argcp > 2)
+ opt_dst_type = g_strdup(argvp[2]);
+ else
+ opt_dst_type = g_strdup("public");
+ }
+
+ if (opt_dst == NULL) {
+ error("Remote Bluetooth address required\n");
+ return;
+ }
+
+ rl_printf("Attempting to connect to %s\n", opt_dst);
+ set_state(STATE_CONNECTING);
+ iochannel = gatt_connect(opt_src, opt_dst, opt_dst_type, opt_sec_level,
+ opt_psm, opt_mtu, connect_cb, &gerr);
+ if (iochannel == NULL) {
+ set_state(STATE_DISCONNECTED);
+ error("%s\n", gerr->message);
+ g_error_free(gerr);
+ } else
+ g_io_add_watch(iochannel, G_IO_HUP, channel_watcher, NULL);
+}
+
+static void cmd_disconnect(int argcp, char **argvp)
+{
+ disconnect_io();
+}
+
+// Discover Services
+static void discover_services_cb(guint8 status, GSList *ranges, gpointer user_data)
+{
+ GSList *l;
+
+ if (status) {
+ error("Discover primary services by UUID failed: %s\n",
+ att_ecode2str(status));
+ return;
+ }
+
+ if (ranges == NULL) {
+ error("No service UUID found\n");
+ return;
+ }
+
+ for (l = ranges; l; l = l->next) {
+ struct att_range *range = l->data;
+ rl_printf("Starting handle: 0x%04x Ending handle: 0x%04x\n",
+ range->start, range->end);
+ }
+ gatt_discover_char(attrib, 0x1, 0xffff, NULL, discover_char_cb, NULL);
+}
+
+
+static void discover_services(void)
+{
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ bt_uuid_t uuid;
+ if (bt_string_to_uuid(&uuid, ANKI_STR_SERVICE_UUID) < 0) {
+ error("Error attempting to discover service for UUID: %s\n", ANKI_STR_SERVICE_UUID);
+ return;
+ }
+
+ gatt_discover_primary(attrib, &uuid, discover_services_cb, NULL);
+}
+
+static int strtohandle(const char *src)
+{
+ char *e;
+ int dst;
+
+ errno = 0;
+ dst = strtoll(src, &e, 16);
+ if (errno != 0 || *e != '\0')
+ return -EINVAL;
+
+ return dst;
+}
+
+static void cmd_anki_vehicle_read(int argcp, char **argvp)
+{
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 1) {
+ error("Missing argument: handle\n");
+ return;
+ }
+
+ int handle = vehicle.read_char.value_handle;
+ if (handle < 0) {
+ error("Invalid handle: %s\n", argvp[1]);
+ return;
+ }
+
+ gatt_read_char(attrib, handle, char_read_cb, attrib);
+}
+
+static void char_write_req_cb(guint8 status, const guint8 *pdu, guint16 plen,
+ gpointer user_data)
+{
+ if (status != 0) {
+ error("Characteristic Write Request failed: "
+ "%s\n", att_ecode2str(status));
+ return;
+ }
+
+ if (!dec_write_resp(pdu, plen) && !dec_exec_write_resp(pdu, plen)) {
+ error("Protocol error\n");
+ return;
+ }
+
+ rl_printf("Characteristic value was written successfully\n");
+}
+
+static void cmd_anki_vehicle_write(int argcp, char **argvp)
+{
+ uint8_t *value;
+ size_t plen;
+ int handle;
+
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 2) {
+ rl_printf("Usage: %s <handle> <new value>\n", argvp[0]);
+ return;
+ }
+
+ handle = vehicle.write_char.value_handle;
+
+ plen = gatt_attr_data_from_string(argvp[1], &value);
+ if (plen == 0) {
+ error("Invalid value\n");
+ return;
+ }
+
+ if (g_strcmp0("send-data-req", argvp[0]) == 0)
+ gatt_write_char(attrib, handle, value, plen,
+ char_write_req_cb, NULL);
+ else
+ gatt_write_cmd(attrib, handle, value, plen, NULL, NULL);
+
+ g_free(value);
+}
+
+static void cmd_anki_vehicle_disconnect(int argcp, char **argvp)
+{
+ uint8_t *value;
+ size_t plen;
+ int handle;
+
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 1) {
+ rl_printf("Usage: %s\n", argvp[0]);
+ return;
+ }
+
+ handle = vehicle.write_char.value_handle;
+
+ anki_vehicle_msg_t msg;
+ plen = anki_vehicle_msg_disconnect(&msg);
+ value = (uint8_t *)&msg;
+
+ gatt_write_char(attrib, handle, value, plen,
+ NULL, NULL);
+}
+
+static void cmd_anki_vehicle_sdk_mode(int argcp, char **argvp)
+{
+ uint8_t *value;
+ size_t plen;
+ int handle;
+
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 2) {
+ rl_printf("Usage: %s <new value>\n", argvp[0]);
+ return;
+ }
+
+ handle = vehicle.write_char.value_handle;
+
+ int arg = atoi(argvp[1]);
+
+ anki_vehicle_msg_t msg;
+ plen = anki_vehicle_msg_set_sdk_mode(&msg, arg, ANKI_VEHICLE_SDK_OPTION_OVERRIDE_LOCALIZATION);
+ value = (uint8_t *)&msg;
+
+ gatt_write_char(attrib, handle, value, plen,
+ NULL, NULL);
+}
+
+static void cmd_anki_vehicle_ping(int argcp, char **argvp)
+{
+ uint8_t *value;
+ size_t plen;
+ int handle;
+
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 1) {
+ rl_printf("Usage: %s\n", argvp[0]);
+ return;
+ }
+
+ handle = vehicle.write_char.value_handle;
+
+ anki_vehicle_msg_t msg;
+ plen = anki_vehicle_msg_ping(&msg);
+ value = (uint8_t *)&msg;
+
+ gatt_write_char(attrib, handle, value, plen, NULL, NULL);
+}
+
+static void cmd_anki_vehicle_get_version(int argcp, char **argvp)
+{
+ uint8_t *value;
+ size_t plen;
+ int handle;
+
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 1) {
+ rl_printf("Usage: %s\n", argvp[0]);
+ return;
+ }
+
+ handle = vehicle.write_char.value_handle;
+
+ anki_vehicle_msg_t msg;
+ plen = anki_vehicle_msg_get_version(&msg);
+ value = (uint8_t *)&msg;
+
+ gatt_write_char(attrib, handle, value, plen, NULL, NULL);
+}
+
+static void cmd_anki_vehicle_set_speed(int argcp, char **argvp)
+{
+ uint8_t *value;
+ size_t plen;
+ int handle;
+
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 2) {
+ rl_printf("Usage: %s <new value>\n", argvp[0]);
+ return;
+ }
+
+ handle = vehicle.write_char.value_handle;
+
+ int16_t speed = (int16_t)atoi(argvp[1]);
+ int16_t accel = 25000;
+ if (argcp > 2) {
+ accel = atoi(argvp[2]);
+ }
+ rl_printf("setting speed to %d (accel = %d)\n", speed, accel);
+
+ anki_vehicle_msg_t msg;
+ plen = anki_vehicle_msg_set_speed(&msg, speed, accel);
+ value = (uint8_t *)&msg;
+
+ gatt_write_char(attrib, handle, value, plen,
+ NULL, NULL);
+}
+
+static void cmd_anki_vehicle_change_lane(int argcp, char **argvp)
+{
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 3) {
+ rl_printf("Usage: %s <horizontal speed (mm/sec)> <horizontal accel (mm/sec^2)> <offset (mm)>\n", argvp[0]);
+ return;
+ }
+
+ int handle = vehicle.write_char.value_handle;
+
+ int16_t hspeed = (int16_t)atoi(argvp[1]);
+ int16_t haccel = (int16_t)atoi(argvp[2]);
+ float offset = 1.0;
+ if (argcp > 3) {
+ offset = strtof(argvp[3], NULL);
+ }
+ rl_printf("changing lane at %d (accel = %d | offset = %1.2f)\n", hspeed, haccel, offset);
+
+ anki_vehicle_msg_t msg;
+ size_t plen = anki_vehicle_msg_set_offset_from_road_center(&msg, 0.0);
+ gatt_write_char(attrib, handle, (uint8_t*)&msg, plen, NULL, NULL);
+
+ anki_vehicle_msg_t lane_msg;
+ size_t lane_plen = anki_vehicle_msg_change_lane(&lane_msg, hspeed, haccel, offset);
+ gatt_write_char(attrib, handle, (uint8_t*)&lane_msg, lane_plen, NULL, NULL);
+}
+
+anki_vehicle_light_channel_t get_channel_by_name(const char *name)
+{
+ uint8_t i;
+ uint8_t channel = channel_invalid;
+
+ if (name == NULL)
+ return channel;
+
+ uint8_t count = sizeof(channels_by_name)/sizeof(channels_by_name[0]);
+ for (i = 0; i < count; i++) {
+ uint8_t len = MAX(strlen(name), strlen(channels_by_name[i]));
+ if (strncmp(name, channels_by_name[i], len) == 0) {
+ channel = i;
+ break;
+ }
+ }
+
+ return channel;
+}
+
+anki_vehicle_light_effect_t get_effect_by_name(const char *name)
+{
+ uint8_t i;
+ uint8_t effect = channel_invalid;
+
+ if (name == NULL)
+ return effect;
+
+ uint8_t count = sizeof(effects_by_name)/sizeof(effects_by_name[0]);
+ for (i = 0; i < count; i++) {
+ if (strncmp(name, effects_by_name[i], sizeof(effects_by_name[i])) == 0) {
+ effect = i;
+ break;
+ }
+ }
+
+ return effect;
+}
+
+static void cmd_anki_vehicle_lights_pattern(int argcp, char **argvp)
+{
+ uint8_t *value;
+ size_t plen;
+ int handle;
+
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 6) {
+ rl_printf("Usage: %s <channel> <effect> <start> <end> <cycles_per_min>\n", argvp[0]);
+ rl_printf(" channels: RED, TAIL, BLUE, GREEN, FRONTL, FRONTR\n");
+ rl_printf(" effects: STEADY, FADE, THROB, FLASH, RANDOM\n");
+ return;
+ }
+
+ handle = vehicle.write_char.value_handle;
+
+ uint8_t channel = get_channel_by_name(argvp[1]);
+ if (channel == channel_invalid) {
+ rl_printf("Unrecognized channel: %s\n", argvp[1]);
+ return;
+ }
+
+ uint8_t effect = get_effect_by_name(argvp[2]);
+ if (effect == effect_invalid) {
+ rl_printf("Unrecognized channel: %s\n", argvp[2]);
+ return;
+ }
+
+ uint8_t start = atoi(argvp[3]);
+ uint8_t end = atoi(argvp[4]);
+ uint16_t cycles_per_min = atoi(argvp[5]);
+
+ anki_vehicle_msg_t msg;
+ plen = anki_vehicle_msg_lights_pattern(&msg, channel, effect, start, end, cycles_per_min);
+ value = (uint8_t *)&msg;
+
+ gatt_write_char(attrib, handle, value, plen, NULL, NULL);
+}
+
+static void vehicle_set_rgb_lights(int handle, uint8_t effect, uint8_t start_red, uint8_t end_red, uint8_t start_green, uint8_t end_green, uint8_t start_blue, uint8_t end_blue, uint16_t cycles_per_min)
+{
+ anki_vehicle_msg_lights_pattern_t msg;
+
+ anki_vehicle_light_config_t red_config;
+ anki_vehicle_light_config(&red_config, LIGHT_RED, effect, start_red, end_red, cycles_per_min);
+ anki_vehicle_msg_lights_pattern_append(&msg, &red_config);
+
+ anki_vehicle_light_config_t green_config;
+ anki_vehicle_light_config(&green_config, LIGHT_GREEN, effect, start_green, end_green, cycles_per_min);
+ anki_vehicle_msg_lights_pattern_append(&msg, &green_config);
+
+ anki_vehicle_light_config_t blue_config;
+ anki_vehicle_light_config(&blue_config, LIGHT_BLUE, effect, start_blue, end_blue, cycles_per_min);
+ anki_vehicle_msg_lights_pattern_append(&msg, &blue_config);
+
+ uint8_t *value = (uint8_t *)&msg;
+ size_t plen = sizeof(msg);
+
+ gatt_write_char(attrib, handle, value, plen, NULL, NULL);
+}
+
+static void cmd_anki_vehicle_engine_lights(int argcp, char **argvp)
+{
+
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 5) {
+ rl_printf("Usage: %s <red> <green> <blue> <effect> <cycles per min>\n", argvp[0]);
+ rl_printf(" effects: STEADY, FADE, THROB, FLASH, RANDOM\n");
+ return;
+ }
+
+ float red = strtof(argvp[1], NULL);
+ uint8_t r = (uint8_t)(ANKI_VEHICLE_MAX_LIGHT_INTENSITY * red);
+
+ float green = strtof(argvp[2], NULL);
+ uint8_t g = (uint8_t)(ANKI_VEHICLE_MAX_LIGHT_INTENSITY * green);
+
+ float blue = strtof(argvp[3], NULL);
+ uint8_t b = (uint8_t)(ANKI_VEHICLE_MAX_LIGHT_INTENSITY * blue);
+
+ uint8_t effect = get_effect_by_name(argvp[4]);
+ uint16_t cycles_per_min = atoi(argvp[5]);
+
+ int handle = vehicle.write_char.value_handle;
+ rl_printf("%s: %u %u %u @ %u cycles/min\n", argvp[4], r, g, b, cycles_per_min);
+
+ switch(effect) {
+ case EFFECT_RANDOM:
+ case EFFECT_STEADY:
+ vehicle_set_rgb_lights(handle, effect, r, r, g, g, b, b, 0);
+ break;
+ case EFFECT_FLASH:
+ case EFFECT_THROB:
+ vehicle_set_rgb_lights(handle, effect, 0, r, 0, g, 0, b, cycles_per_min);
+ break;
+ case EFFECT_FADE:
+ vehicle_set_rgb_lights(handle, effect, r, 0, g, 0, b, 0, cycles_per_min);
+ break;
+ }
+}
+
+anki_vehicle_turn_type_t get_turn_type_by_name(const char *name)
+{
+ uint8_t i;
+ anki_vehicle_turn_type_t turn_type = VEHICLE_TURN_NONE;
+
+ if (name == NULL)
+ return turn_type;
+
+ uint8_t count = sizeof(turn_types_by_name)/sizeof(turn_types_by_name[0]);
+ for (i = 0; i < count; i++) {
+ if (strncmp(name, turn_types_by_name[i], sizeof(turn_types_by_name[i])) == 0) {
+ turn_type = (anki_vehicle_turn_type_t)i;
+ break;
+ }
+ }
+
+ return turn_type;
+}
+
+static void vehicle_turn(int handle, anki_vehicle_turn_type_t turn_type)
+{
+ anki_vehicle_msg_t msg;
+ size_t plen = anki_vehicle_msg_turn(&msg, turn_type, VEHICLE_TURN_TRIGGER_IMMEDIATE);
+
+ uint8_t *value = (uint8_t *)&msg;
+
+ gatt_write_char(attrib, handle, value, plen, NULL, NULL);
+}
+
+static void cmd_anki_vehicle_turn(int argcp, char **argvp)
+{
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (argcp < 2) {
+ rl_printf("Usage: %s <type>\n", argvp[0]);
+ rl_printf(" turn type: UTURN, LEFT, RIGHT\n");
+ return;
+ }
+
+
+ anki_vehicle_turn_type_t turn_type = get_turn_type_by_name(argvp[1]);
+ rl_printf("%s: %s (%u)\n", argvp[0], argvp[1], turn_type);
+
+ int handle = vehicle.write_char.value_handle;
+ vehicle_turn(handle, turn_type);
+}
+
+static void exchange_mtu_cb(guint8 status, const guint8 *pdu, guint16 plen,
+ gpointer user_data)
+{
+ uint16_t mtu;
+
+ if (status != 0) {
+ error("Exchange MTU Request failed: %s\n",
+ att_ecode2str(status));
+ return;
+ }
+
+ if (!dec_mtu_resp(pdu, plen, &mtu)) {
+ error("Protocol error\n");
+ return;
+ }
+
+ mtu = MIN(mtu, opt_mtu);
+ /* Set new value for MTU in client */
+ if (g_attrib_set_mtu(attrib, mtu))
+ rl_printf("MTU was exchanged successfully: %d\n", mtu);
+ else
+ error("Error exchanging MTU\n");
+}
+
+static void cmd_mtu(int argcp, char **argvp)
+{
+ if (conn_state != STATE_CONNECTED) {
+ failed("Disconnected\n");
+ return;
+ }
+
+ if (opt_psm) {
+ failed("Operation is only available for LE transport.\n");
+ return;
+ }
+
+ if (argcp < 2) {
+ rl_printf("Usage: mtu <value>\n");
+ return;
+ }
+
+ if (opt_mtu) {
+ failed("MTU exchange can only occur once per connection.\n");
+ return;
+ }
+
+ errno = 0;
+ opt_mtu = strtoll(argvp[1], NULL, 0);
+ if (errno != 0 || opt_mtu < ATT_DEFAULT_LE_MTU) {
+ error("Invalid value. Minimum MTU size is %d\n",
+ ATT_DEFAULT_LE_MTU);
+ return;
+ }
+
+ gatt_exchange_mtu(attrib, opt_mtu, exchange_mtu_cb, NULL);
+}
+
+static struct {
+ const char *cmd;
+ void (*func)(int argcp, char **argvp);
+ const char *params;
+ const char *desc;
+} commands[] = {
+ { "help", cmd_help, "",
+ "Show this help"},
+ { "exit", cmd_exit, "",
+ "Exit interactive mode" },
+ { "quit", cmd_exit, "",
+ "Exit interactive mode" },
+ { "connect", cmd_connect, "[address [address type]]",
+ "Connect to a remote device" },
+ { "disconnect", cmd_disconnect, "",
+ "Disconnect from a remote device" },
+ { "mtu", cmd_mtu, "<value>",
+ "Exchange MTU for GATT/ATT" },
+ { "sdk-mode", cmd_anki_vehicle_sdk_mode, "[on]",
+ "Set SDK Mode"},
+ { "ping", cmd_anki_vehicle_ping, "",
+ "Send ping message to vehicle."},
+ { "get-version", cmd_anki_vehicle_get_version, "",
+ "Request vehicle software version."},
+ { "set-speed", cmd_anki_vehicle_set_speed, "<speed> <accel>",
+ "Set vehicle Speed (mm/sec) with acceleration (mm/sec^2)"},
+ { "change-lane", cmd_anki_vehicle_change_lane, "<horizontal speed> <horizontal accel> <relative offset> (right(+), left(-))",
+ "Change lanes at speed (mm/sec), accel (mm/sec^2) in the specified direction (offset)"},
+ { "set-lights-pattern", cmd_anki_vehicle_lights_pattern, "<channel> <effect> <start> <end> <cycles_per_min>",
+ "Set lights pattern for vehicle LEDs."},
+ { "set-engine-lights", cmd_anki_vehicle_engine_lights, "<red> <green> <blue> <effect> <cycles_per_min>",
+ "Set the pattern for the engine lights."},
+ { "turn", cmd_anki_vehicle_turn, "<type>",
+ "Execute a turn of type UTURN, LEFT, RIGHT"},
+ { "vehicle-disconnect", cmd_anki_vehicle_disconnect, "",
+ "Request that the vehicle disconnect (often more reliable than disconnect)"},
+ { "send-data-req", cmd_anki_vehicle_write, "<new value>",
+ "Write data to vehicle (Request response)" },
+ { "send-data", cmd_anki_vehicle_write, "<new value>",
+ "Write data to vehicle (No response)" },
+ { "read-data", cmd_anki_vehicle_read, "",
+ "Read last message from vehicle" },
+ { NULL, NULL, NULL}
+};
+
+static void cmd_help(int argcp, char **argvp)
+{
+ int i;
+
+ for (i = 0; commands[i].cmd; i++)
+ rl_printf("%-15s %-30s %s\n", commands[i].cmd,
+ commands[i].params, commands[i].desc);
+}
+
+static void parse_line(char *line_read)
+{
+ char **argvp;
+ int argcp;
+ int i;
+
+ if (line_read == NULL) {
+ rl_printf("\n");
+ cmd_exit(0, NULL);
+ return;
+ }
+
+ line_read = g_strstrip(line_read);
+
+ if (*line_read == '\0')
+ goto done;
+
+ add_history(line_read);
+
+ if (g_shell_parse_argv(line_read, &argcp, &argvp, NULL) == FALSE)
+ goto done;
+
+ for (i = 0; commands[i].cmd; i++)
+ if (strcasecmp(commands[i].cmd, argvp[0]) == 0)
+ break;
+
+ if (commands[i].cmd)
+ commands[i].func(argcp, argvp);
+ else
+ error("%s: command not found\n", argvp[0]);
+
+ g_strfreev(argvp);
+
+done:
+ free(line_read);
+}
+
+static gboolean prompt_read(GIOChannel *chan, GIOCondition cond,
+ gpointer user_data)
+{
+ if (cond & (G_IO_HUP | G_IO_ERR | G_IO_NVAL)) {
+ g_io_channel_unref(chan);
+ return FALSE;
+ }
+
+ rl_callback_read_char();
+
+ return TRUE;
+}
+
+static char *completion_generator(const char *text, int state)
+{
+ static int index = 0, len = 0;
+ const char *cmd = NULL;
+
+ if (state == 0) {
+ index = 0;
+ len = strlen(text);
+ }
+
+ while ((cmd = commands[index].cmd) != NULL) {
+ index++;
+ if (strncmp(cmd, text, len) == 0)
+ return strdup(cmd);
+ }
+
+ return NULL;
+}
+
+static char **commands_completion(const char *text, int start, int end)
+{
+ if (start == 0)
+ return rl_completion_matches(text, &completion_generator);
+ else
+ return NULL;
+}
+
+static guint setup_standard_input(void)
+{
+ GIOChannel *channel;
+ guint source;
+
+ channel = g_io_channel_unix_new(fileno(stdin));
+
+ source = g_io_add_watch(channel,
+ G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+ prompt_read, NULL);
+
+ g_io_channel_unref(channel);
+
+ return source;
+}
+
+static gboolean signal_handler(GIOChannel *channel, GIOCondition condition,
+ gpointer user_data)
+{
+ static unsigned int __terminated = 0;
+ struct signalfd_siginfo si;
+ ssize_t result;
+ int fd;
+
+ if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP)) {
+ g_main_loop_quit(event_loop);
+ return FALSE;
+ }
+
+ fd = g_io_channel_unix_get_fd(channel);
+
+ result = read(fd, &si, sizeof(si));
+ if (result != sizeof(si))
+ return FALSE;
+
+ switch (si.ssi_signo) {
+ case SIGINT:
+ rl_replace_line("", 0);
+ rl_crlf();
+ rl_on_new_line();
+ rl_redisplay();
+ break;
+ case SIGTERM:
+ if (__terminated == 0) {
+ rl_replace_line("", 0);
+ rl_crlf();
+ g_main_loop_quit(event_loop);
+ }
+
+ __terminated = 1;
+ break;
+ }
+
+ return TRUE;
+}
+
+static guint setup_signalfd(void)
+{
+ GIOChannel *channel;
+ guint source;
+ sigset_t mask;
+ int fd;
+
+ sigemptyset(&mask);
+ sigaddset(&mask, SIGINT);
+ sigaddset(&mask, SIGTERM);
+
+ if (sigprocmask(SIG_BLOCK, &mask, NULL) < 0) {
+ perror("Failed to set signal mask");
+ return 0;
+ }
+
+ fd = signalfd(-1, &mask, 0);
+ if (fd < 0) {
+ perror("Failed to create signal descriptor");
+ return 0;
+ }
+
+ channel = g_io_channel_unix_new(fd);
+
+ g_io_channel_set_close_on_unref(channel, TRUE);
+ g_io_channel_set_encoding(channel, NULL, NULL);
+ g_io_channel_set_buffered(channel, FALSE);
+
+ source = g_io_add_watch(channel,
+ G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+ signal_handler, NULL);
+
+ g_io_channel_unref(channel);
+
+ return source;
+}
+
+int interactive(const char *src, const char *dst,
+ const char *dst_type, int psm)
+{
+ guint input;
+ guint signal;
+
+ opt_sec_level = g_strdup("low");
+
+ opt_src = g_strdup(src);
+ opt_dst = g_strdup(dst);
+ opt_dst_type = g_strdup(dst_type);
+ opt_psm = psm;
+
+ prompt = g_string_new(NULL);
+
+ event_loop = g_main_loop_new(NULL, FALSE);
+
+ input = setup_standard_input();
+ signal = setup_signalfd();
+
+ rl_attempted_completion_function = commands_completion;
+ rl_erase_empty_line = 1;
+ rl_callback_handler_install(get_prompt(), parse_line);
+
+ g_main_loop_run(event_loop);
+
+ rl_callback_handler_remove();
+ cmd_disconnect(0, NULL);
+ g_source_remove(input);
+ g_source_remove(signal);
+ g_main_loop_unref(event_loop);
+ g_string_free(prompt, TRUE);
+
+ g_free(opt_src);
+ g_free(opt_dst);
+ g_free(opt_sec_level);
+
+ return 0;
+}
diff --git a/drive-sdk/examples/vehicle-tool/vehicle_tool.c b/drive-sdk/examples/vehicle-tool/vehicle_tool.c
new file mode 100644
index 0000000..c5ff73e
--- /dev/null
+++ b/drive-sdk/examples/vehicle-tool/vehicle_tool.c
@@ -0,0 +1,172 @@
+/*
+ * vehicle-tool
+ *
+ * Example vehicle control tool for Anki Drive SDK
+ *
+ * Copyright (C) 2014 Anki, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * Portions of this software are derived from BlueZ, a Bluetooth protocol stack for
+ * Linux. The license for BlueZ is included below.
+ */
+
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ * Copyright (C) 2010 Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <errno.h>
+#include <glib.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <bzle/bluetooth/bluetooth.h>
+#include <bzle/bluetooth/hci.h>
+#include <bzle/bluetooth/hci_lib.h>
+
+#include <bzle/bluetooth/uuid.h>
+#include <bzle/gatt/att.h>
+#include <bzle/bluetooth/btio.h>
+#include <bzle/gatt/gattrib.h>
+#include <bzle/gatt/gatt.h>
+#include <bzle/gatt/utils.h>
+
+static char *opt_src = NULL;
+static char *opt_dst = NULL;
+static char *opt_dst_type = NULL;
+static char *opt_value = NULL;
+static char *opt_sec_level = NULL;
+static bt_uuid_t *opt_uuid = NULL;
+static int opt_handle = -1;
+static int opt_mtu = 0;
+static int opt_psm = 0;
+static GMainLoop *event_loop;
+static gboolean got_error = FALSE;
+static GSourceFunc operation;
+
+struct characteristic_data {
+ GAttrib *attrib;
+ uint16_t start;
+ uint16_t end;
+};
+
+static void events_handler(const uint8_t *pdu, uint16_t len, gpointer user_data)
+{
+ GAttrib *attrib = user_data;
+ uint8_t *opdu;
+ uint16_t handle, i, olen = 0;
+ size_t plen;
+
+ handle = att_get_u16(&pdu[1]);
+
+ switch (pdu[0]) {
+ case ATT_OP_HANDLE_NOTIFY:
+ g_print("Notification handle = 0x%04x value: ", handle);
+ break;
+ case ATT_OP_HANDLE_IND:
+ g_print("Indication handle = 0x%04x value: ", handle);
+ break;
+ default:
+ g_print("Invalid opcode\n");
+ return;
+ }
+
+ for (i = 3; i < len; i++)
+ g_print("%02x ", pdu[i]);
+
+ g_print("\n");
+
+ if (pdu[0] == ATT_OP_HANDLE_NOTIFY)
+ return;
+
+ opdu = g_attrib_get_buffer(attrib, &plen);
+ olen = enc_confirmation(opdu, plen);
+
+ if (olen > 0)
+ g_attrib_send(attrib, 0, opdu, olen, NULL, NULL, NULL);
+}
+
+static GOptionEntry options[] = {
+ { "adapter", 'i', 0, G_OPTION_ARG_STRING, &opt_src,
+ "Specify local adapter interface", "hciX" },
+ { "device", 'b', 0, G_OPTION_ARG_STRING, &opt_dst,
+ "Specify remote Bluetooth address", "MAC" },
+ { "addr-type", 't', 0, G_OPTION_ARG_STRING, &opt_dst_type,
+ "Set LE address type. Default: random", "[public | random]"},
+ { "mtu", 'm', 0, G_OPTION_ARG_INT, &opt_mtu,
+ "Specify the MTU size", "MTU" },
+ { "psm", 'p', 0, G_OPTION_ARG_INT, &opt_psm,
+ "Specify the PSM for GATT/ATT over BR/EDR", "PSM" },
+ { "sec-level", 'l', 0, G_OPTION_ARG_STRING, &opt_sec_level,
+ "Set security level. Default: low", "[low | medium | high]"},
+ { NULL },
+};
+
+int main(int argc, char *argv[])
+{
+ GOptionContext *context;
+ GOptionGroup *gatt_group, *params_group, *char_rw_group;
+ GError *gerr = NULL;
+ GIOChannel *chan;
+
+ opt_dst_type = g_strdup("random");
+ opt_sec_level = g_strdup("low");
+
+ context = g_option_context_new(NULL);
+ g_option_context_add_main_entries(context, options, NULL);
+
+ if (!g_option_context_parse(context, &argc, &argv, &gerr)) {
+ g_printerr("%s\n", gerr->message);
+ g_clear_error(&gerr);
+ }
+
+ interactive(opt_src, opt_dst, opt_dst_type, opt_psm);
+
+done:
+ g_option_context_free(context);
+ g_free(opt_src);
+ g_free(opt_dst);
+ g_free(opt_uuid);
+ g_free(opt_sec_level);
+
+ if (got_error)
+ exit(EXIT_FAILURE);
+ else
+ exit(EXIT_SUCCESS);
+}