commit 842fb957a657064be42188911bcadb1431a3483d Author: Peter Johnson Date: Sun Jun 21 21:02:10 2015 -0700 ntcore: Initial commit. Change-Id: Id8df275a34aaa628581b18e39014b880c29bf9b2 diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000000..d3e33360aa --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 2.8) +project(NetworkTables) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++1y -Wformat=2 -Wall -Wextra -Werror -pedantic -Wno-unused-parameter") + +file(GLOB_RECURSE SRC_TARGET_FILES src/*.cpp) +#file(GLOB_RECURSE SRC_SHARE_FILES lib/share/*.cpp) +include_directories(include) +add_library(ntcore STATIC ${SRC_SHARE_FILES} ${SRC_TARGET_FILES}) +target_link_libraries(ntcore) +INSTALL(TARGETS ntcore ARCHIVE DESTINATION lib COMPONENT lib) +INSTALL(DIRECTORY include DESTINATION ${CMAKE_INSTALL_PREFIX} COMPONENT headers) +# lib/ c gcc_s ld-linux +# usr/lib stdc++ +# NiRioSrv diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000000..fc91d7622a --- /dev/null +++ b/build.gradle @@ -0,0 +1,21 @@ +apply plugin: 'cpp' + +model { + components { + NetworkTables(NativeLibrarySpec) { + targetPlatform 'arm' + sources { + cpp { + source { + srcDirs = ["src"] + includes = ["**/*.c"] + } + exportedHeaders { + srcDirs = ["include"] + } + } + } + } + } +} + diff --git a/include/ntcore.h b/include/ntcore.h new file mode 100644 index 0000000000..28acc68354 --- /dev/null +++ b/include/ntcore.h @@ -0,0 +1,333 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#ifndef NTCORE_H_ +#define NTCORE_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** Default network tables port number */ +#define NT_DEFAULT_PORT 1735 + +/** NetworkTables data types. */ +enum NT_Type { + NT_UNASSIGNED = 0, + NT_BOOLEAN = 0x01, + NT_DOUBLE = 0x02, + NT_STRING = 0x04, + NT_RAW = 0x08, + NT_BOOLEAN_ARRAY = 0x10, + NT_DOUBLE_ARRAY = 0x20, + NT_STRING_ARRAY = 0x40, + NT_RPC = 0x80 +}; + +/** NetworkTables entry flags. */ +enum NT_EntryFlags { + NT_PERSISTENT = 0x01 +}; + +/* + * Structures + */ + +/** A NetworkTables string. */ +struct NT_String { + /** String contents (UTF-8). + * The string is NOT required to be zero-terminated. + * When returned by the library, this is zero-terminated and allocated with + * malloc(). + */ + char *str; + + /** Length of the string in bytes. If the string happens to be zero + * terminated, this does not include the zero-termination. + */ + size_t len; +}; + +/** NetworkTables Entry Value. Note this is a typed union. */ +struct NT_Value { + enum NT_Type type; + unsigned long last_change; + union { + int v_boolean; + double v_double; + struct NT_String v_string; + struct NT_String v_raw; + struct { + int *arr; + size_t size; + } arr_boolean; + struct { + double *arr; + size_t size; + } arr_double; + struct { + struct NT_String *arr; + size_t size; + } arr_string; + } data; +}; + +/** A NetworkTables Entry */ +struct NT_Entry { + /** Entry name */ + struct NT_String name; + + /** Entry value */ + struct NT_Value value; + + /** Entry flags */ + unsigned int flags; +}; + +/** NetworkTables Entry Information */ +struct NT_EntryInfo { + /** Entry name */ + struct NT_String name; + + /** Entry type */ + enum NT_Type type; + + /** Entry flags */ + unsigned int flags; + + /** Timestamp of last change to entry (type or value). */ + unsigned long last_change; +}; + +/** NetworkTables Connection Information */ +struct NT_ConnectionInfo { + struct NT_String remote_id; + const char *remote_name; + unsigned int remote_port; + long last_update; + unsigned int protocol_version; +}; + +/** NetworkTables RPC Parameter Definition */ +struct NT_RpcParamDef { + struct NT_String name; + struct NT_Value def_value; +}; + +/** NetworkTables RPC Result Definition */ +struct NT_RpcResultDef { + struct NT_String name; + enum NT_Type type; +}; + +/** NetworkTables RPC Definition */ +struct NT_RpcDefinition { + unsigned int version; + struct NT_String name; + size_t num_params; + NT_RpcParamDef *params; + size_t num_results; + NT_RpcResultDef *results; +}; + +/* + * Table Functions + */ + +/** Get Entry Value. + * Returns copy of current entry value. + * Note that one of the type options is "unassigned". + * + * @param name entry name (UTF-8 string) + * @param name_len length of name in bytes + * @param value storage for returned entry value + * + * It is the caller's responsibility to free value once it's no longer + * needed (the utility function NT_DisposeValue() is useful for this + * purpose). + */ +void NT_GetEntryValue(const char *name, size_t name_len, + struct NT_Value *value); + +/** Set Entry Value. + * Sets new entry value. If type of new value differs from the type of the + * currently stored entry, returns error and does not update value. + * + * @param name entry name (UTF-8 string) + * @param name_len length of name in bytes + * @param value new entry value + * @return 0 on error (type mismatch), 1 on success + */ +int NT_SetEntryValue(const char *name, size_t name_len, + const struct NT_Value *value); + +/** Set Entry Type and Value. + * Sets new entry value. If type of new value differs from the type of the + * currently stored entry, the currently stored entry type is overridden + * (generally this will generate an Entry Assignment message). + * + * This is NOT the preferred method to update a value; generally + * NT_SetEntryValue() should be used instead, with appropriate error handling. + * + * @param name entry name (UTF-8 string) + * @param name_len length of name in bytes + * @param value new entry value + */ +void NT_SetEntryTypeValue(const char *name, size_t name_len, + const struct NT_Value *value); + +/** Set Entry Flags. + */ +void NT_SetEntryFlags(const char *name, size_t name_len, unsigned int flags); + +/** Get Entry Flags. + */ +unsigned int NT_GetEntryFlags(const char *name, size_t name_len); + +/** Delete Entry. + * Deletes an entry. This is a new feature in version 3.0 of the protocol, + * so this may not have an effect if any other node in the network is not + * version 3.0 or newer. + * + * Note: NT_GetConnections() can be used to determine the protocol version + * of direct remote connection(s), but this is not sufficient to determine + * if all nodes in the network are version 3.0 or newer. + * + * @param name entry name (UTF-8 string) + * @param name_len length of name in bytes + */ +void NT_DeleteEntry(const char *name, size_t name_len); + +/** Delete All Entries. + * Deletes ALL table entries. This is a new feature in version 3.0 of the + * so this may not have an effect if any other node in the network is not + * version 3.0 or newer. + * + * Note: NT_GetConnections() can be used to determine the protocol version + * of direct remote connection(s), but this is not sufficient to determine + * if all nodes in the network are version 3.0 or newer. + */ +void NT_DeleteAllEntries(void); + +/** Get Entry Information. + * Returns an array of entry information (name, entry type, + * and timestamp of last change to type/value). The results are optionally + * filtered by string prefix and entry type to only return a subset of all + * entries. + * + * @param prefix entry name required prefix; only entries whose name + * starts with this string are returned + * @param prefix_len length of prefix in bytes + * @param types bitmask of NT_Type values; 0 is treated specially + * as a "don't care" + * @param count output parameter; set to length of returned array + * @return Array of entry information. + */ +struct NT_EntryInfo *NT_GetEntryInfo(const char *prefix, size_t prefix_len, + unsigned int types, size_t *count); + +/** Flush Entries. + * Forces an immediate flush of all local entry changes to network. + * Normally this is done on a regularly scheduled interval (see + * NT_SetUpdateRate()). + * + * Note: flushes are rate limited to avoid excessive network traffic. If + * the time between calls is too short, the flush will occur after the minimum + * time elapses (rather than immediately). + */ +void NT_Flush(void); + +/* + * Callback Creation Functions + */ + +typedef void (*NT_EntryListenerCallback) (unsigned int uid, void *data, + const char *name, size_t name_len, struct NT_Value *value); + +typedef void (*NT_ConnectionListenerCallback) (unsigned int uid, void *data, + int connected, const struct NT_ConnectionInfo *conn); + +unsigned int NT_AddEntryListener(const char *prefix, size_t prefix_len, + void *data, NT_EntryListenerCallback callback); +void NT_RemoveEntryListener(unsigned int entry_listener_uid); +unsigned int NT_AddConnectionListener(void *data, + NT_ConnectionListenerCallback callback); +void NT_RemoveConnectionListener(unsigned int conn_listener_uid); + +/* + * Remote Procedure Call Functions + */ + +typedef NT_Value * (*NT_RpcCallback) (unsigned int uid, void *data, + const char *name, size_t name_len, + NT_Value *params, size_t params_len, + size_t *results_len); + +unsigned int NT_CreateRpc(const char *name, size_t name_len, + const NT_RpcDefinition *def, void *data, NT_RpcCallback callback); +void NT_DeleteRpc(unsigned int rpc_uid); +unsigned int NT_CallRpc(const char *name, size_t name_len, + const NT_Value *params, size_t params_len); +NT_Value *NT_GetRpcResult(unsigned int result_uid, size_t *results_len); + +/* + * Client/Server Functions + */ +void NT_SetNetworkIdentity(const char *name, size_t name_len); +void NT_StartServer(const char *persist_filename, const char *listen_address, + unsigned int port); +void NT_StopServer(void); +void NT_StartClient(const char *server_name, unsigned int port); +void NT_StopClient(void); +void NT_SetUpdateRate(double interval); +struct NT_ConnectionInfo *NT_GetConnections(size_t *count); + +/* + * Persistent Functions + */ +void NT_SavePersistent(const char *filename); +void NT_LoadPersistent(const char *filename); + +/* + * Utility Functions + */ + +/* frees value memory */ +void NT_DisposeValue(struct NT_Value *value); + +/* sets type to unassigned and clears rest of struct */ +void NT_InitValue(struct NT_Value *value); + +/* frees string memory */ +void NT_DisposeString(struct NT_String *str); + +/* sets length to zero and pointer to null */ +void NT_InitString(struct NT_String *str); + +void NT_DisposeConnectionInfoArray(struct NT_ConnectionInfo *arr, size_t count); + +/* + * Message Encoding Functions + */ +size_t NT_WriteType(char *buf, enum NT_Type type, unsigned int proto_rev); +size_t NT_ReadType(const char *buf, enum NT_Type *type, unsigned int proto_rev); + +size_t NT_GetStringSize(const struct NT_String *value, unsigned int proto_rev); +size_t NT_WriteString(char *buf, const struct NT_String *str, + unsigned int proto_rev); + +size_t NT_GetValueSize(const struct NT_Value *value, unsigned int proto_rev); +size_t NT_WriteValue(char *buf, const struct NT_Value *value, + unsigned int proto_rev); + +#ifdef __cplusplus +} +#endif + +#endif /* NTCORE_H_ */ diff --git a/src/nt_base64.cpp b/src/nt_base64.cpp new file mode 100644 index 0000000000..9aecdfe897 --- /dev/null +++ b/src/nt_base64.cpp @@ -0,0 +1,193 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +/* ==================================================================== + * Copyright (c) 1995-1999 The Apache Group. 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. All advertising materials mentioning features or use of this + * software must display the following acknowledgment: + * "This product includes software developed by the Apache Group + * for use in the Apache HTTP server project (http://www.apache.org/)." + * + * 4. The names "Apache Server" and "Apache Group" must not be used to + * endorse or promote products derived from this software without + * prior written permission. For written permission, please contact + * apache@apache.org. + * + * 5. Products derived from this software may not be called "Apache" + * nor may "Apache" appear in their names without prior written + * permission of the Apache Group. + * + * 6. Redistributions of any form whatsoever must retain the following + * acknowledgment: + * "This product includes software developed by the Apache Group + * for use in the Apache HTTP server project (http://www.apache.org/)." + * + * THIS SOFTWARE IS PROVIDED BY THE APACHE GROUP ``AS IS'' AND ANY + * EXPRESSED 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 APACHE GROUP OR + * ITS 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. + * ==================================================================== + * + * This software consists of voluntary contributions made by many + * individuals on behalf of the Apache Group and was originally based + * on public domain software written at the National Center for + * Supercomputing Applications, University of Illinois, Urbana-Champaign. + * For more information on the Apache Group and the Apache HTTP server + * project, please see . + * + */ + +#include "nt_base64.h" + +namespace NtImpl { + +/* aaaack but it's fast and const should make it shared text page. */ +static const unsigned char pr2six[256] = +{ + /* ASCII table */ + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 62, 64, 64, 64, 63, + 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 64, 64, 64, 64, 64, 64, + 64, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, + 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 64, 64, 64, 64, 64, + 64, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, + 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 64, 64, 64, 64, 64, + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64 +}; + +std::size_t +base64decode_len(const char *bufcoded) +{ + const unsigned char *bufin = (const unsigned char *) bufcoded; + while (pr2six[*(bufin++)] <= 63) {} + + std::size_t nprbytes = (bufin - (const unsigned char *) bufcoded) - 1; + std::size_t nbytesdecoded = ((nprbytes + 3) / 4) * 3; + + return nbytesdecoded + 1; +} + +std::size_t +base64decode(char *bufplain, const char *bufcoded) +{ + const unsigned char *bufin = (const unsigned char *) bufcoded; + while (pr2six[*(bufin++)] <= 63) {} + std::size_t nprbytes = (bufin - (const unsigned char *) bufcoded) - 1; + std::size_t nbytesdecoded = ((nprbytes + 3) / 4) * 3; + + unsigned char *bufout = (unsigned char *) bufplain; + bufin = (const unsigned char *) bufcoded; + + while (nprbytes > 4) + { + *(bufout++) = + (unsigned char) (pr2six[*bufin] << 2 | pr2six[bufin[1]] >> 4); + *(bufout++) = + (unsigned char) (pr2six[bufin[1]] << 4 | pr2six[bufin[2]] >> 2); + *(bufout++) = + (unsigned char) (pr2six[bufin[2]] << 6 | pr2six[bufin[3]]); + bufin += 4; + nprbytes -= 4; + } + + /* Note: (nprbytes == 1) would be an error, so just ingore that case */ + if (nprbytes > 1) + { + *(bufout++) = + (unsigned char) (pr2six[*bufin] << 2 | pr2six[bufin[1]] >> 4); + } + if (nprbytes > 2) + { + *(bufout++) = + (unsigned char) (pr2six[bufin[1]] << 4 | pr2six[bufin[2]] >> 2); + } + if (nprbytes > 3) + { + *(bufout++) = + (unsigned char) (pr2six[bufin[2]] << 6 | pr2six[bufin[3]]); + } + + *(bufout++) = '\0'; + nbytesdecoded -= (4 - nprbytes) & 3; + return nbytesdecoded; +} + +static const char basis_64[] = + "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + +std::size_t +base64encode_len(std::size_t len) +{ + return ((len + 2) / 3 * 4) + 1; +} + +std::size_t +base64encode(char *encoded, const unsigned char *string, std::size_t len) +{ + std::size_t i; + char *p = encoded; + + for (i = 0; i < len - 2; i += 3) + { + *p++ = basis_64[(string[i] >> 2) & 0x3F]; + *p++ = basis_64[((string[i] & 0x3) << 4) | + ((int) (string[i + 1] & 0xF0) >> 4)]; + *p++ = basis_64[((string[i + 1] & 0xF) << 2) | + ((int) (string[i + 2] & 0xC0) >> 6)]; + *p++ = basis_64[string[i + 2] & 0x3F]; + } + if (i < len) + { + *p++ = basis_64[(string[i] >> 2) & 0x3F]; + if (i == (len - 1)) + { + *p++ = basis_64[((string[i] & 0x3) << 4)]; + *p++ = '='; + } + else + { + *p++ = basis_64[((string[i] & 0x3) << 4) | + ((int) (string[i + 1] & 0xF0) >> 4)]; + *p++ = basis_64[((string[i + 1] & 0xF) << 2)]; + } + *p++ = '='; + } + + *p++ = '\0'; + return p - encoded; +} + +} // namespace NtImpl diff --git a/src/nt_base64.h b/src/nt_base64.h new file mode 100644 index 0000000000..46ad21e0e8 --- /dev/null +++ b/src/nt_base64.h @@ -0,0 +1,23 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#ifndef NT_BASE64_H_ +#define NT_BASE64_H_ + +#include + +namespace NtImpl { + +std::size_t base64decode_len(const char *bufcoded); +std::size_t base64decode(char *bufplain, const char *bufcoded); +std::size_t base64encode_len(std::size_t len); +std::size_t base64encode(char *encoded, const unsigned char *string, + std::size_t len); + +} // namespace NtImpl + +#endif /* NT_BASE64_H_ */ diff --git a/src/nt_encoding.cpp b/src/nt_encoding.cpp new file mode 100644 index 0000000000..8a6ac9ab0c --- /dev/null +++ b/src/nt_encoding.cpp @@ -0,0 +1,186 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "nt_encoding.h" +#include "ntcore.h" + +#include +#include + +#include "nt_leb128.h" + +using namespace NtImpl; + +void +NtImpl::WriteDouble(char* &buf, double val) +{ + std::uint64_t v = *reinterpret_cast(&val); + *buf++ = (char)((v >> 56) & 0xff); + *buf++ = (char)((v >> 48) & 0xff); + *buf++ = (char)((v >> 40) & 0xff); + *buf++ = (char)((v >> 32) & 0xff); + *buf++ = (char)((v >> 24) & 0xff); + *buf++ = (char)((v >> 16) & 0xff); + *buf++ = (char)((v >> 8) & 0xff); + *buf++ = (char)(v & 0xff); +} + +double +NtImpl::ReadDouble(char* &buf) +{ + std::uint64_t val = (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; + return *reinterpret_cast(&val); +} + +size_t +NT_WriteType(char *buf, NT_Type type, unsigned int proto_rev) +{ + switch (type) + { + case NT_BOOLEAN: *buf = 0x00; break; + case NT_DOUBLE: *buf = 0x01; break; + case NT_STRING: *buf = 0x02; break; + case NT_RAW: + if (proto_rev < 0x0300u) + return 0; + *buf = 0x03; + break; + case NT_BOOLEAN_ARRAY: *buf = 0x10; break; + case NT_DOUBLE_ARRAY: *buf = 0x11; break; + case NT_STRING_ARRAY: *buf = 0x12; break; + case NT_RPC: + if (proto_rev < 0x0300u) + return 0; + *buf = 0x20; + break; + default: + return 0; + } + return 1; +} + +size_t +NT_ReadType(const char *buf, NT_Type *type, unsigned int proto_rev) +{ + switch (buf[0]) + { + case 0x00: *type = NT_BOOLEAN; break; + case 0x01: *type = NT_DOUBLE; break; + case 0x02: *type = NT_STRING; break; + case 0x03: *type = NT_RAW; break; + case 0x10: *type = NT_BOOLEAN_ARRAY; break; + case 0x11: *type = NT_DOUBLE_ARRAY; break; + case 0x12: *type = NT_STRING_ARRAY; break; + case 0x20: *type = NT_RPC; break; + default: return 0; + } + return 1; +} + +size_t +NT_GetStringSize(const NT_String *str, unsigned int proto_rev) +{ + if (proto_rev < 0x0300u) + return 2 + str->len; + return size_uleb128(str->len) + str->len; +} + +size_t +NT_WriteString(char *buf, const NT_String *str, unsigned int proto_rev) +{ + char *start = buf; + if (proto_rev < 0x0300u) + Write16(buf, str->len); + else + buf += write_uleb128(buf, str->len); + std::memcpy(buf, str->str, str->len); + return (buf - start) + str->len; +} + +size_t +NT_GetValueSize(const NT_Value *value, unsigned int proto_rev) +{ + switch (value->type) + { + case NT_BOOLEAN: + return 1; + case NT_DOUBLE: + return 8; + case NT_STRING: + return NT_GetStringSize(&value->data.v_string, proto_rev); + case NT_RAW: + case NT_RPC: + if (proto_rev < 0x0300u) + return 0; + return NT_GetStringSize(&value->data.v_raw, proto_rev); + case NT_BOOLEAN_ARRAY: + return 1 + value->data.arr_boolean.size; + case NT_DOUBLE_ARRAY: + return 1 + value->data.arr_double.size * 8; + case NT_STRING_ARRAY: + { + size_t len = 1; + for (size_t i=0; idata.arr_string.size; ++i) + len += NT_GetStringSize(&value->data.arr_string.arr[i], proto_rev); + return len; + } + default: + return 0; + } + return 1; +} + +size_t +NT_WriteValue(char *buf, const NT_Value *value, unsigned int proto_rev) +{ + char *start = buf; + switch (value->type) + { + case NT_BOOLEAN: + Write8(buf, value->data.v_boolean ? 1 : 0); + break; + case NT_DOUBLE: + WriteDouble(buf, value->data.v_double); + break; + case NT_STRING: + buf += NT_WriteString(buf, &value->data.v_string, proto_rev); + break; + case NT_RAW: + case NT_RPC: + if (proto_rev < 0x0300u) + return 0; + buf += NT_WriteString(buf, &value->data.v_raw, proto_rev); + break; + case NT_BOOLEAN_ARRAY: + Write8(buf, value->data.arr_boolean.size); + for (size_t i=0; idata.arr_boolean.size; ++i) + Write8(buf, value->data.arr_boolean.arr[i] ? 1 : 0); + break; + case NT_DOUBLE_ARRAY: + Write8(buf, value->data.arr_double.size); + for (size_t i=0; idata.arr_double.size; ++i) + WriteDouble(buf, value->data.arr_double.arr[i] ? 1 : 0); + break; + case NT_STRING_ARRAY: + Write8(buf, value->data.arr_string.size); + for (size_t i=0; idata.arr_string.size; ++i) + buf += NT_WriteString(buf, &value->data.arr_string.arr[i], + proto_rev); + break; + default: + return 0; + } + return buf - start; +} diff --git a/src/nt_encoding.h b/src/nt_encoding.h new file mode 100644 index 0000000000..f7ca8c569f --- /dev/null +++ b/src/nt_encoding.h @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#ifndef NT_UTIL_H_ +#define NT_UTIL_H_ + +namespace NtImpl { + +inline void +Write8(char* &buf, unsigned int val) +{ + *buf++ = (char)(val & 0xff); +} + +inline void +Write16(char* &buf, unsigned int val) +{ + *buf++ = (char)((val >> 8) & 0xff); + *buf++ = (char)(val & 0xff); +} + +inline void +Write32(char* &buf, unsigned long val) +{ + *buf++ = (char)((val >> 24) & 0xff); + *buf++ = (char)((val >> 16) & 0xff); + *buf++ = (char)((val >> 8) & 0xff); + *buf++ = (char)(val & 0xff); +} + +void WriteDouble(char* &buf, double val); + +inline unsigned int +Read8(char* &buf) +{ + unsigned int val = (*((unsigned char *)buf)) & 0xff; + ++buf; + return val; +} + +inline unsigned int +Read16(char* &buf) +{ + unsigned int val = (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; + return val; +} + +inline unsigned long +Read32(char* &buf) +{ + unsigned int val = (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; val <<= 8; val |= (*((unsigned char *)buf)) & 0xff; + ++buf; + return val; +} + +double ReadDouble(char* &buf); + +} // namespace NtImpl + +#endif /* NT_UTIL_H */ diff --git a/src/nt_internal.h b/src/nt_internal.h new file mode 100644 index 0000000000..81cd7d6341 --- /dev/null +++ b/src/nt_internal.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#ifndef NT_INTERNAL_H_ +#define NT_INTERNAL_H_ + +namespace NtImpl { + +enum MsgType { + NT_MSG_KEEP_ALIVE = 0x00, + NT_MSG_CLIENT_HELLO = 0x01, + NT_MSG_PROTO_UNSUP = 0x02, + NT_MSG_SERVER_HELLO_DONE = 0x03, + NT_MSG_SERVER_HELLO = 0x04, + NT_MSG_CLIENT_HELLO_DONE = 0x05, + NT_MSG_ENTRY_ASSIGN = 0x10, + NT_MSG_ENTRY_UPDATE = 0x11, + NT_MSG_FLAGS_UPDATE = 0x12, + NT_MSG_ENTRY_DELETE = 0x13, + NT_MSG_CLEAR_ENTRIES = 0x14, + NT_MSG_EXECUTE_RPC = 0x20, + NT_MSG_RPC_RESPONSE = 0x21 +}; + +#define NT_CLEAR_ALL_MAGIC 0xD06CB27Aul + +} // namespace NtImpl + +#endif /* NT_INTERNAL_H_ */ diff --git a/src/nt_leb128.cpp b/src/nt_leb128.cpp new file mode 100644 index 0000000000..d1b8056f93 --- /dev/null +++ b/src/nt_leb128.cpp @@ -0,0 +1,133 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "nt_leb128.h" + +#include "nt_raw_istream.h" + +namespace NtImpl { + +/** + * size_uleb128 - get size of unsigned LEB128 data + * @val: value + * + * Determine the number of bytes required to encode an unsigned LEB128 datum. + * The algorithm is taken from Appendix C of the DWARF 3 spec. For information + * on the encodings refer to section "7.6 - Variable Length Data". Return + * the number of bytes required. + */ +std::size_t +size_uleb128(unsigned long val) +{ + std::size_t count = 0; + do { + val >>= 7; + ++count; + } while (val != 0); + return count; +} + +/** + * write_uleb128 - write unsigned LEB128 data + * @addr: the address where the ULEB128 data is to be stored + * @val: value to be stored + * + * Encode an unsigned LEB128 encoded datum. The algorithm is taken + * from Appendix C of the DWARF 3 spec. For information on the + * encodings refer to section "7.6 - Variable Length Data". Return + * the number of bytes written. + */ +std::size_t +write_uleb128(char *addr, unsigned long val) +{ + std::size_t count = 0; + + do { + unsigned char byte = val & 0x7f; + val >>= 7; + + if (val != 0) + byte |= 0x80; // mark this byte to show that more bytes will follow + + *((unsigned char *)addr) = byte; + addr++; + count++; + } while (val != 0); + + return count; +} + +/** + * read_uleb128 - read unsigned LEB128 data + * @addr: the address where the ULEB128 data is stored + * @ret: address to store the result + * + * Decode an unsigned LEB128 encoded datum. The algorithm is taken + * from Appendix C of the DWARF 3 spec. For information on the + * encodings refer to section "7.6 - Variable Length Data". Return + * the number of bytes read. + */ +std::size_t +read_uleb128(char *addr, unsigned long *ret) +{ + unsigned long result = 0; + int shift = 0; + std::size_t count = 0; + + while (1) + { + unsigned char byte = *((unsigned char *)addr); + addr++; + count++; + + result |= (byte & 0x7f) << shift; + shift += 7; + + if (!(byte & 0x80)) + break; + } + + *ret = result; + + return count; +} + +/** + * read_uleb128 - read unsigned LEB128 data from a stream + * @is: the input stream where the ULEB128 data is to be read from + * @ret: address to store the result + * + * Decode an unsigned LEB128 encoded datum. The algorithm is taken + * from Appendix C of the DWARF 3 spec. For information on the + * encodings refer to section "7.6 - Variable Length Data". Return + * false on stream error, true on success. + */ +bool +read_uleb128(raw_istream &is, unsigned long *ret) +{ + unsigned long result = 0; + int shift = 0; + + while (1) + { + unsigned char byte; + if (!is.read((char*)&byte, 1)) + return false; + + result |= (byte & 0x7f) << shift; + shift += 7; + + if (!(byte & 0x80)) + break; + } + + *ret = result; + + return true; +} + +} // namespace NtImpl diff --git a/src/nt_leb128.h b/src/nt_leb128.h new file mode 100644 index 0000000000..cf255994fd --- /dev/null +++ b/src/nt_leb128.h @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#ifndef NT_LEB128_H_ +#define NT_LEB128_H_ + +#include + +namespace NtImpl { + +class raw_istream; + +std::size_t size_uleb128(unsigned long val); +std::size_t write_uleb128(char *addr, unsigned long val); +std::size_t read_uleb128(char *addr, unsigned long *ret); +bool read_uleb128(raw_istream &is, unsigned long *ret); + +} // namespace NtImpl + +#endif /* NT_LEB128_H_ */ diff --git a/src/nt_messagereader.cpp b/src/nt_messagereader.cpp new file mode 100644 index 0000000000..c210b7d7d5 --- /dev/null +++ b/src/nt_messagereader.cpp @@ -0,0 +1,337 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "nt_messagereader.h" + +#include +#include +#include + +#include "nt_internal.h" +#include "nt_leb128.h" + +using namespace NtImpl; + +void +MessageHandler::anchor() +{ +} + +MessageReader::MessageReader(MessageHandler &handler, + raw_istream& is, + unsigned int proto_rev) + : m_handler(handler) + , m_is(is) +{ + m_allocated = 1024; + m_buf = (char *)std::malloc(m_allocated); + m_proto_rev = proto_rev; + m_error = 0; +} + +MessageReader::~MessageReader() +{ + std::free(m_buf); +} + +void +MessageReader::Realloc(std::size_t len) +{ + if (m_allocated >= len) + return; + std::size_t newlen = m_allocated * 2; + while (newlen < len) + newlen *= 2; + m_buf = (char *)std::realloc(m_buf, newlen); + m_allocated = newlen; +} + +bool +MessageReader::Run() +{ + unsigned int msg_type; + if (!Read8(&msg_type)) return false; + switch (msg_type) + { + case NT_MSG_KEEP_ALIVE: + m_handler.GotKeepAlive(); + break; + case NT_MSG_CLIENT_HELLO: + { + unsigned int proto_rev; + if (!Read16(&proto_rev)) return false; + NT_String self_id; + NT_InitString(&self_id); + // This intentionally uses the provided proto_rev instead of + // m_proto_rev. + if (proto_rev >= 0x0300u) + { + if (!ReadString(&self_id)) return false; + } + m_handler.GotClientHello(proto_rev, self_id); + break; + } + case NT_MSG_PROTO_UNSUP: + { + unsigned int proto_rev; + if (!Read16(&proto_rev)) return false; + m_handler.GotProtoUnsup(proto_rev); + break; + } + case NT_MSG_SERVER_HELLO_DONE: + if (m_proto_rev < 0x0300u) + { + m_error = "received SERVER_HELLO_DONE in protocol < 3.0"; + return false; + } + m_handler.GotServerHelloDone(); + break; + case NT_MSG_CLIENT_HELLO_DONE: + if (m_proto_rev < 0x0300u) + { + m_error = "received CLIENT_HELLO_DONE in protocol < 3.0"; + return false; + } + m_handler.GotClientHelloDone(); + break; + case NT_MSG_ENTRY_ASSIGN: + { + NT_String name; + if (!ReadString(&name)) return false; + NT_Type type; + unsigned int id, seq_num; + unsigned int flags = 0; + NT_Value value; + if (!ReadType(&type) || + !Read16(&id) || + !Read16(&seq_num) || + (m_proto_rev >= 0x0300u && !Read8(&flags)) || + !ReadValue(type, &value)) + { + NT_DisposeString(&name); + return false; + } + m_handler.GotEntryAssign(name, id, seq_num, value, flags); + break; + } + case NT_MSG_ENTRY_UPDATE: + { + unsigned int id, seq_num; + NT_Value value; + if (!Read16(&id)) return false; + if (!Read16(&seq_num)) return false; + NT_Type type; + if (m_proto_rev >= 0x0300u) + { + unsigned int itype; + if (!Read8(&itype)) return false; + type = static_cast(itype); + } + else + type = m_handler.GetEntryType(id); + if (!ReadValue(type, &value)) return false; + m_handler.GotEntryUpdate(id, seq_num, value); + break; + } + case NT_MSG_FLAGS_UPDATE: + { + if (m_proto_rev < 0x0300u) + { + m_error = "received FLAGS_UPDATE in protocol < 3.0"; + return false; + } + unsigned int id, flags; + if (!Read16(&id)) return false; + if (!Read8(&flags)) return false; + m_handler.GotFlagsUpdate(id, flags); + break; + } + case NT_MSG_ENTRY_DELETE: + { + if (m_proto_rev < 0x0300u) + { + m_error = "received ENTRY_DELETE in protocol < 3.0"; + return false; + } + unsigned int id; + if (!Read16(&id)) return false; + m_handler.GotEntryDelete(id); + break; + } + case NT_MSG_CLEAR_ENTRIES: + { + if (m_proto_rev < 0x0300u) + { + m_error = "received CLEAR_ENTRIES in protocol < 3.0"; + return false; + } + unsigned long magic; + if (!Read32(&magic)) return false; + if (magic != NT_CLEAR_ALL_MAGIC) + { + m_error = "received incorrect CLEAR_ENTRIES magic value, ignoring"; + return true; + } + m_handler.GotClearEntries(); + break; + } + case NT_MSG_EXECUTE_RPC: + { + if (m_proto_rev < 0x0300u) + { + m_error = "received EXECUTE_RPC in protocol < 3.0"; + return false; + } + unsigned int id, uid; + if (!Read16(&id)) return false; + if (!Read16(&uid)) return false; + unsigned long size; + if (!ReadULEB128(&size)) return false; + if (!Read(size)) return false; + } + case NT_MSG_RPC_RESPONSE: + default: + m_error = "unrecognized message type"; + return false; + } + return true; +} + +bool +MessageReader::ReadType(NT_Type *type) +{ + if (!Read(1)) return false; + size_t len = NT_ReadType(m_buf, type, m_proto_rev); + if (len == 0) + { + m_error = "unrecognized value type"; + return false; + } + return true; +} + +bool +MessageReader::ReadValue(NT_Type type, NT_Value *value) +{ + value->type = type; + value->last_change = 0; + switch (type) + { + case NT_BOOLEAN: + { + unsigned int v; + if (!Read8(&v)) return false; + value->data.v_boolean = v ? 1 : 0; + break; + } + case NT_DOUBLE: + { + if (!Read(8)) return false; + char *buf = m_buf; + value->data.v_double = ReadDouble(buf); + break; + } + case NT_STRING: + if (!ReadString(&value->data.v_string)) return false; + break; + case NT_RAW: + case NT_RPC: + if (m_proto_rev < 0x0300u) + { + m_error = "received raw or RPC value in protocol < 3.0"; + return false; + } + if (!ReadString(&value->data.v_raw)) return false; + break; + case NT_BOOLEAN_ARRAY: + { + // size + unsigned int size; + if (!Read8(&size)) return false; + value->data.arr_boolean.size = size; + + // array values + if (!Read(size)) return false; + value->data.arr_boolean.arr = (int *)std::malloc(size * sizeof(int)); + for (unsigned int i=0; idata.arr_boolean.arr[i] = m_buf[i] ? 1 : 0; + break; + } + case NT_DOUBLE_ARRAY: + { + // size + unsigned int size; + if (!Read8(&size)) return false; + value->data.arr_double.size = size; + + // array values + if (!Read(size*8)) return false; + value->data.arr_double.arr = + (double *)std::malloc(size * sizeof(double)); + char *buf = m_buf; + for (unsigned int i=0; idata.arr_double.arr[i] = ReadDouble(buf); + break; + } + case NT_STRING_ARRAY: + { + // size + unsigned int size; + if (!Read8(&size)) return false; + value->data.arr_string.size = size; + + // array values + if (!Read(size*8)) return false; + value->data.arr_string.arr = + (NT_String *)std::malloc(size * sizeof(NT_String)); + for (unsigned int i=0; idata.arr_string.arr[i])) + { + // cleanup to avoid memory leaks + for (unsigned int j=0; jdata.arr_string.arr[j].str); + } + std::free(value->data.arr_string.arr); + return false; + } + } + break; + } + default: + m_error = "invalid type when trying to read value"; + return false; + } + return true; +} + +bool +MessageReader::ReadString(NT_String *str) +{ + if (m_proto_rev < 0x0300u) + { + unsigned int v; + if (!Read16(&v)) return false; + str->len = v; + } + else + { + unsigned long v; + if (!ReadULEB128(&v)) return false; + str->len = v; + } + str->str = (char *)std::malloc(str->len + 1); + if (!m_is.read(str->str, str->len)) + { + std::free(str->str); + str->str = 0; + return false; + } + str->str[str->len] = '\0'; + return true; +} diff --git a/src/nt_messagereader.h b/src/nt_messagereader.h new file mode 100644 index 0000000000..98ddf8ec06 --- /dev/null +++ b/src/nt_messagereader.h @@ -0,0 +1,150 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#ifndef NT_MESSAGEREADER_H_ +#define NT_MESSAGEREADER_H_ + +#include + +#include "ntcore.h" +#include "nt_leb128.h" +#include "nt_raw_istream.h" +#include "nt_encoding.h" + +namespace NtImpl { + +class MessageHandler +{ + void anchor(); + +public: + virtual NT_Type GetEntryType(unsigned int id) = 0; + virtual size_t GetRpcParamTypes(NT_Type *types, unsigned int id) = 0; + virtual size_t GetRpcResultTypes(NT_Type *types, unsigned int id) = 0; + + // All of these functions are expected to take ownership of passed + // strings/values. + virtual ~MessageHandler() {} + virtual void GotKeepAlive() = 0; + virtual void GotClientHello(unsigned int proto_rev, + NT_String &self_id) = 0; + virtual void GotProtoUnsup(unsigned int proto_rev) = 0; + virtual void GotServerHelloDone() = 0; + virtual void GotServerHello(unsigned int flags, + NT_String &self_id) = 0; + virtual void GotClientHelloDone() = 0; + virtual void GotEntryAssign(NT_String &name, + unsigned int id, + unsigned int seq_num, + NT_Value &value, + unsigned int flags) = 0; + virtual void GotEntryUpdate(unsigned int id, + unsigned int seq_num, + NT_Value &value) = 0; + virtual void GotFlagsUpdate(unsigned int id, unsigned int flags) = 0; + virtual void GotEntryDelete(unsigned int id) = 0; + virtual void GotClearEntries() = 0; + virtual void GotExecuteRpc(unsigned int id, + unsigned int uid, + NT_Value *params_start, + NT_Value *params_end) = 0; + virtual void GotRpcResponse(unsigned int id, + unsigned int uid, + NT_Value *results_start, + NT_Value *results_end) = 0; + +private: + MessageHandler(const MessageHandler&); + MessageHandler& operator= (const MessageHandler&); +}; + +class MessageReader +{ +public: + explicit MessageReader(MessageHandler &handler, + raw_istream& is, + unsigned int proto_rev); + ~MessageReader(); + + void SetProtocolRev(unsigned int proto_rev) + { + m_proto_rev = proto_rev; + } + + bool Run(); + + void Reset() + { + m_error = 0; + } + + const char *GetError() const + { + return m_error; + } + +protected: + bool Read(std::size_t len) + { + if (len > m_allocated) + Realloc(len); + return m_is.read(m_buf, len); + } + + bool Read8(unsigned int *val) + { + if (!Read(1)) return false; + char *buf = m_buf; + *val = NtImpl::Read8(buf); + return true; + } + + bool Read16(unsigned int *val) + { + if (!Read(2)) return false; + char *buf = m_buf; + *val = NtImpl::Read16(buf); + return true; + } + + bool Read32(unsigned long *val) + { + if (!Read(4)) return false; + char *buf = m_buf; + *val = NtImpl::Read32(buf); + return true; + } + + bool ReadULEB128(unsigned long *val) + { + return read_uleb128(m_is, val); + } + + bool ReadType(NT_Type *type); + bool ReadValue(NT_Type type, NT_Value *value); + bool ReadString(NT_String *str); + +private: + MessageReader(const MessageReader&); + MessageReader& operator= (const MessageReader&); + + void Realloc(std::size_t len); + + MessageHandler &m_handler; + raw_istream &m_is; + + char *m_buf; + std::size_t m_allocated; + + unsigned int m_proto_rev; + const char *m_error; + NT_Type m_rpc_types[256]; +}; + +} // namespace NtImpl + +#endif /* NT_MESSAGEREADER_H_ */ diff --git a/src/nt_messagewriter.cpp b/src/nt_messagewriter.cpp new file mode 100644 index 0000000000..0be8908be4 --- /dev/null +++ b/src/nt_messagewriter.cpp @@ -0,0 +1,247 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "nt_messagewriter.h" + +#include +#include + +#include "nt_internal.h" +#include "nt_leb128.h" + +using namespace NtImpl; + +MessageWriter::MessageWriter(unsigned int proto_rev) +{ + m_start = m_cur = (char *)std::malloc(1024); + m_end = m_start + 1024; + m_proto_rev = proto_rev; + m_error = 0; +} + +MessageWriter::~MessageWriter() +{ + std::free(m_start); +} + +void +MessageWriter::EnsureSlow(std::size_t len) +{ + assert(m_end > m_cur); + if (static_cast(m_end - m_cur) >= len) + return; + std::size_t pos = m_cur - m_start; + std::size_t newlen = (m_end - m_start) * 2; + while (newlen < (pos + len)) + newlen *= 2; + m_start = (char *)std::realloc(m_start, newlen); + m_cur = m_start + pos; + m_end = m_start + newlen; +} + +void +MessageWriter::WriteKeepAlive() +{ + Ensure(1); + Write8(NT_MSG_KEEP_ALIVE); +} + +void +MessageWriter::WriteClientHello(const NT_String &self_id) +{ + Ensure(3); + Write8(NT_MSG_CLIENT_HELLO); + Write16(m_proto_rev); + if (m_proto_rev < 0x0300u) + return; + WriteString(self_id); +} + +void +MessageWriter::WriteProtoUnsup() +{ + Ensure(3); + Write8(NT_MSG_PROTO_UNSUP); + Write16(m_proto_rev); +} + +void +MessageWriter::WriteServerHelloDone() +{ + Ensure(1); + Write8(NT_MSG_SERVER_HELLO_DONE); +} + +void +MessageWriter::WriteServerHello(unsigned int flags, const NT_String &self_id) +{ + if (m_proto_rev < 0x0300u) + return; // new message in version 3.0 + Ensure(2); + Write8(NT_MSG_SERVER_HELLO); + Write8(flags); + WriteString(self_id); +} + +void +MessageWriter::WriteClientHelloDone() +{ + if (m_proto_rev < 0x0300u) + return; // new message in version 3.0 + Ensure(1); + Write8(NT_MSG_CLIENT_HELLO_DONE); +} + +void +MessageWriter::WriteEntryAssign(const NT_String &name, + unsigned int id, + unsigned int seq_num, + const NT_Value &value, + unsigned int flags) +{ + Ensure(1); + Write8(NT_MSG_ENTRY_ASSIGN); + WriteString(name); + Ensure(6); + WriteType(value.type); + Write16(id); + Write16(seq_num); + if (m_proto_rev >= 0x0300u) + Write8(flags); + WriteValue(value); +} + +void +MessageWriter::WriteEntryUpdate(unsigned int id, + unsigned int seq_num, + const NT_Value &value) +{ + Ensure(6); + Write8(NT_MSG_ENTRY_UPDATE); + Write16(id); + Write16(seq_num); + if (m_proto_rev >= 0x0300u) + Write8(value.type); + WriteValue(value); +} + +void +MessageWriter::WriteFlagsUpdate(unsigned int id, unsigned int flags) +{ + if (m_proto_rev < 0x0300u) + return; // new message in version 3.0 + Ensure(4); + Write8(NT_MSG_FLAGS_UPDATE); + Write16(id); + Write8(flags); +} + +void +MessageWriter::WriteEntryDelete(unsigned int id) +{ + if (m_proto_rev < 0x0300u) + return; // new message in version 3.0 + Ensure(3); + Write8(NT_MSG_ENTRY_DELETE); + Write16(id); +} + +void +MessageWriter::WriteClearEntries() +{ + if (m_proto_rev < 0x0300u) + return; // new message in version 3.0 + + Ensure(5); + Write8(NT_MSG_CLEAR_ENTRIES); + Write32(NT_CLEAR_ALL_MAGIC); +} + +void +MessageWriter::WriteExecuteRpc(unsigned int id, + unsigned int uid, + const NT_Value *params_start, + const NT_Value *params_end) +{ + WriteRpc(NT_MSG_EXECUTE_RPC, id, uid, params_start, params_end); +} + +void +MessageWriter::WriteRpcResponse(unsigned int id, + unsigned int uid, + const NT_Value *results_start, + const NT_Value *results_end) +{ + WriteRpc(NT_MSG_RPC_RESPONSE, id, uid, results_start, results_end); +} + +void +MessageWriter::WriteRpc(unsigned int msg_type, + unsigned int id, + unsigned int uid, + const NT_Value *values_start, + const NT_Value *values_end) +{ + if (m_proto_rev < 0x0300u) + return; // new message in version 3.0 + + Ensure(5); + Write8(msg_type); + Write16(id); + Write16(uid); + + unsigned long len = 0; + for (const NT_Value *value = values_start; value != values_end; ++value) + len += NT_GetValueSize(value, m_proto_rev); + WriteULEB128(len); + + for (const NT_Value *value = values_start; value != values_end; ++value) + WriteValue(*value); +} + +void +MessageWriter::WriteULEB128(unsigned long val) +{ + Ensure(size_uleb128(val)); + m_cur += write_uleb128(m_cur, val); +} + +void +MessageWriter::WriteType(NT_Type type) +{ + Ensure(1); + size_t len = NT_WriteType(m_cur, type, m_proto_rev); + if (len == 0) + m_error = "unrecognized type"; + m_cur += len; +} + +void +MessageWriter::WriteValue(const NT_Value &value) +{ + std::size_t len = NT_GetValueSize(&value, m_proto_rev); + if (len == 0) + { + m_error = "unrecognized type when writing value"; + return; + } + Ensure(len); + m_cur += NT_WriteValue(m_cur, &value, m_proto_rev); +} + +void +MessageWriter::WriteString(const NT_String &str) +{ + std::size_t len = NT_GetStringSize(&str, m_proto_rev); + if (len == 0) + { + m_error = "invalid string"; + return; + } + Ensure(len); + m_cur += NT_WriteString(m_cur, &str, m_proto_rev); +} diff --git a/src/nt_messagewriter.h b/src/nt_messagewriter.h new file mode 100644 index 0000000000..64b22cc7f9 --- /dev/null +++ b/src/nt_messagewriter.h @@ -0,0 +1,125 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#ifndef NT_MESSAGEWRITER_H_ +#define NT_MESSAGEWRITER_H_ + +#include +#include + +#include "ntcore.h" +#include "nt_encoding.h" + +namespace NtImpl { + +class MessageWriter +{ +public: + explicit MessageWriter(unsigned int proto_rev); + ~MessageWriter(); + + void SetProtocolRev(unsigned int proto_rev) + { + m_proto_rev = proto_rev; + } + + void Reset() + { + m_cur = m_start; + m_error = 0; + } + + const char *GetError() const + { + return m_error; + } + + const char *GetData() const + { + return m_start; + } + + std::size_t GetSize() const + { + return m_cur - m_start; + } + + void WriteKeepAlive(); + void WriteClientHello(const NT_String &self_id); + void WriteProtoUnsup(); + void WriteServerHelloDone(); + void WriteServerHello(unsigned int flags, const NT_String &self_id); + void WriteClientHelloDone(); + void WriteEntryAssign(const NT_String &name, + unsigned int id, + unsigned int seq_num, + const NT_Value &value, + unsigned int flags); + void WriteEntryUpdate(unsigned int id, + unsigned int seq_num, + const NT_Value &value); + void WriteFlagsUpdate(unsigned int id, unsigned int flags); + void WriteEntryDelete(unsigned int id); + void WriteClearEntries(); + void WriteExecuteRpc(unsigned int id, + unsigned int uid, + const NT_Value *params_start, + const NT_Value *params_end); + void WriteRpcResponse(unsigned int id, + unsigned int uid, + const NT_Value *results_start, + const NT_Value *results_end); + +protected: + void Ensure(std::size_t len) + { + assert(m_end > m_cur); + if (static_cast(m_end - m_cur) < len) + EnsureSlow(len); + } + + void Write8(unsigned int val) + { + NtImpl::Write8(m_cur, val); + } + + void Write16(unsigned int val) + { + NtImpl::Write16(m_cur, val); + } + + void Write32(unsigned long val) + { + NtImpl::Write32(m_cur, val); + } + + void WriteULEB128(unsigned long val); + void WriteType(NT_Type type); + void WriteValue(const NT_Value &value); + void WriteString(const NT_String &str); + +private: + MessageWriter(const MessageWriter&); + MessageWriter& operator= (const MessageWriter&); + + void EnsureSlow(std::size_t len); + void WriteRpc(unsigned int msg_type, + unsigned int id, + unsigned int uid, + const NT_Value *values_start, + const NT_Value *values_end); + + char *m_start; + char *m_cur; + char *m_end; + unsigned int m_proto_rev; + const char *m_error; +}; + +} // namespace NtImpl + +#endif /* NT_MESSAGEWRITER_H_ */ diff --git a/src/nt_raw_istream.cpp b/src/nt_raw_istream.cpp new file mode 100644 index 0000000000..59a2237899 --- /dev/null +++ b/src/nt_raw_istream.cpp @@ -0,0 +1,15 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "nt_raw_istream.h" + +using namespace NtImpl; + +void +raw_istream::anchor() +{ +} diff --git a/src/nt_raw_istream.h b/src/nt_raw_istream.h new file mode 100644 index 0000000000..af0a715183 --- /dev/null +++ b/src/nt_raw_istream.h @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#ifndef NT_RAW_ISTREAM_H_ +#define NT_RAW_ISTREAM_H_ + +#include + +namespace NtImpl { + +class raw_istream +{ + void anchor(); +public: + virtual ~raw_istream() {} + virtual bool read(void *data, std::size_t len) = 0; + virtual void close() = 0; +private: + raw_istream(const raw_istream&); + raw_istream& operator= (const raw_istream&); +}; + +} // namespace NtImpl + +#endif /* NT_RAW_ISTREAM_H_ */ diff --git a/src/nt_seqnum.cpp b/src/nt_seqnum.cpp new file mode 100644 index 0000000000..098d133079 --- /dev/null +++ b/src/nt_seqnum.cpp @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "nt_seqnum.h" + +namespace NtImpl { + +bool operator< (const SequenceNumber &lhs, const SequenceNumber &rhs) +{ + if (lhs.m_val < rhs.m_val) + return (rhs.m_val - lhs.m_val) < (1u<<15); + else if (lhs.m_val > rhs.m_val) + return (lhs.m_val - rhs.m_val) > (1u<<15); + else + return false; +} + +bool operator> (const SequenceNumber &lhs, const SequenceNumber &rhs) +{ + if (lhs.m_val < rhs.m_val) + return (rhs.m_val - lhs.m_val) > (1u<<15); + else if (lhs.m_val > rhs.m_val) + return (lhs.m_val - rhs.m_val) < (1u<<15); + else + return false; +} + +} // namespace NtImpl diff --git a/src/nt_seqnum.h b/src/nt_seqnum.h new file mode 100644 index 0000000000..3e2357be03 --- /dev/null +++ b/src/nt_seqnum.h @@ -0,0 +1,74 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#ifndef NT_SEQNUM_H_ +#define NT_SEQNUM_H_ + +namespace NtImpl { + +class SequenceNumber +{ +public: + explicit SequenceNumber(unsigned int val) : m_val(val) {} + unsigned int GetVal() const { return m_val; } + + SequenceNumber& operator++() + { + ++m_val; + if (m_val > 0xffff) + m_val = 0; + return *this; + } + SequenceNumber operator++(int) + { + SequenceNumber tmp(*this); + operator++(); + return tmp; + } + + friend bool operator< (const SequenceNumber &lhs, + const SequenceNumber &rhs); + friend bool operator> (const SequenceNumber &lhs, + const SequenceNumber &rhs); + friend bool operator<= (const SequenceNumber &lhs, + const SequenceNumber &rhs); + friend bool operator>= (const SequenceNumber &lhs, + const SequenceNumber &rhs); + friend bool operator== (const SequenceNumber &lhs, + const SequenceNumber &rhs); + friend bool operator!= (const SequenceNumber &lhs, + const SequenceNumber &rhs); +private: + unsigned int m_val; +}; + +bool operator< (const SequenceNumber &lhs, const SequenceNumber &rhs); +bool operator> (const SequenceNumber &lhs, const SequenceNumber &rhs); + +inline bool operator<= (const SequenceNumber &lhs, const SequenceNumber &rhs) +{ + return lhs == rhs || lhs < rhs; +} + +inline bool operator>= (const SequenceNumber &lhs, const SequenceNumber &rhs) +{ + return lhs == rhs || lhs > rhs; +} + +inline bool operator== (const SequenceNumber &lhs, const SequenceNumber &rhs) +{ + return lhs.m_val == rhs.m_val; +} + +inline bool operator!= (const SequenceNumber &lhs, const SequenceNumber &rhs) +{ + return lhs.m_val != rhs.m_val; +} + +} // namespace NtImpl + +#endif /* NT_SEQNUM_H_ */ diff --git a/src/nt_utility.cpp b/src/nt_utility.cpp new file mode 100644 index 0000000000..3f92a37b7f --- /dev/null +++ b/src/nt_utility.cpp @@ -0,0 +1,76 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include + +#include "ntcore.h" + +void +NT_DisposeValue(NT_Value *value) +{ + switch (value->type) + { + case NT_UNASSIGNED: + case NT_BOOLEAN: + case NT_DOUBLE: + break; + case NT_STRING: + case NT_RAW: + case NT_RPC: + std::free(value->data.v_string.str); + break; + case NT_BOOLEAN_ARRAY: + std::free(value->data.arr_boolean.arr); + break; + case NT_DOUBLE_ARRAY: + std::free(value->data.arr_double.arr); + break; + case NT_STRING_ARRAY: + { + for (size_t i=0; idata.arr_string.size; i++) + std::free(value->data.arr_string.arr[i].str); + std::free(value->data.arr_string.arr); + break; + } + default: + assert(0 && "unknown value type"); + } + value->type = NT_UNASSIGNED; + value->last_change = 0; +} + +void +NT_InitValue(NT_Value *value) +{ + value->type = NT_UNASSIGNED; + value->last_change = 0; +} + +void +NT_DisposeString(NT_String *str) +{ + std::free(str->str); + str->str = 0; + str->len = 0; +} + +void +NT_InitString(NT_String *str) +{ + str->str = 0; + str->len = 0; +} + +void +NT_DisposeConnectionInfoArray(NT_ConnectionInfo *arr, size_t count) +{ + unsigned int i; + for (i=0; i +#include + +#include "ntcore.h" + +bool +operator== (const NT_Value &lhs, const NT_Value &rhs) +{ + if (lhs.type != rhs.type) + return 0; + switch (lhs.type) + { + case NT_UNASSIGNED: + return 1; // XXX: is this better being false instead? + case NT_BOOLEAN: + return lhs.data.v_boolean == rhs.data.v_boolean; + case NT_DOUBLE: + return lhs.data.v_double == rhs.data.v_double; + case NT_STRING: + case NT_RAW: + case NT_RPC: + if (lhs.data.v_string.len != rhs.data.v_string.len) + return 0; + return std::memcmp(lhs.data.v_string.str, rhs.data.v_string.str, + lhs.data.v_string.len) == 0; + case NT_BOOLEAN_ARRAY: + if (lhs.data.arr_boolean.size != rhs.data.arr_boolean.size) + return 0; + return std::memcmp(lhs.data.arr_boolean.arr, rhs.data.arr_boolean.arr, + lhs.data.arr_boolean.size * + sizeof(lhs.data.arr_boolean.arr[0])) == 0; + case NT_DOUBLE_ARRAY: + if (lhs.data.arr_double.size != rhs.data.arr_double.size) + return 0; + return std::memcmp(lhs.data.arr_double.arr, rhs.data.arr_double.arr, + lhs.data.arr_double.size * + sizeof(lhs.data.arr_double.arr[0])) == 0; + case NT_STRING_ARRAY: + { + if (lhs.data.arr_string.size != rhs.data.arr_string.size) + return 0; + for (size_t i=0; i