diff --git a/libraries/RPC/examples/Basic_AddSub/basic_add_sub.ino b/libraries/RPC/examples/Basic_AddSub/basic_add_sub.ino new file mode 100644 index 000000000..e2dc9d514 --- /dev/null +++ b/libraries/RPC/examples/Basic_AddSub/basic_add_sub.ino @@ -0,0 +1,25 @@ +#include "RPC.h" + +int add(int a, int b) { + return a + b; +} + +void setup() { + RPC.begin(); + RPC.bind("add", add); + pinMode(LEDG, OUTPUT); +} + +void loop() { + static size_t loop_count = 0; + // Blink every 512 iterations + if ((loop_count++ % 512) == 0) { + digitalWrite(LEDG, LOW); + delay(10); + digitalWrite(LEDG, HIGH); + delay(10); + } + int res = RPC.call("add", 1, 2).as(); + RPC.call("sub", res, 1).as(); + delay(250); +} diff --git a/libraries/RPC/examples/MD5_Checksum/MD5.cpp b/libraries/RPC/examples/MD5_Checksum/MD5.cpp new file mode 100644 index 000000000..d8d31fbf1 --- /dev/null +++ b/libraries/RPC/examples/MD5_Checksum/MD5.cpp @@ -0,0 +1,302 @@ +#include "MD5.h" + +MD5::MD5() +{ + //nothing + return; +} + +char* MD5::make_digest(const unsigned char *digest, int len) /* {{{ */ +{ + char * md5str = (char*) malloc(sizeof(char)*(len*2+1)); + static const char hexits[17] = "0123456789abcdef"; + int i; + + for (i = 0; i < len; i++) { + md5str[i * 2] = hexits[digest[i] >> 4]; + md5str[(i * 2) + 1] = hexits[digest[i] & 0x0F]; + } + md5str[len * 2] = '\0'; + return md5str; +} + +/* + * The basic MD5 functions. + * + * E and G are optimized compared to their RFC 1321 definitions for + * architectures that lack an AND-NOT instruction, just like in Colin Plumb's + * implementation. + * E() has been used instead of F() because F() is already defined in the Arduino core + */ +#define E(x, y, z) ((z) ^ ((x) & ((y) ^ (z)))) +#define G(x, y, z) ((y) ^ ((z) & ((x) ^ (y)))) +#define H(x, y, z) ((x) ^ (y) ^ (z)) +#define I(x, y, z) ((y) ^ ((x) | ~(z))) + +/* + * The MD5 transformation for all four rounds. + */ +#define STEP(f, a, b, c, d, x, t, s) \ + (a) += f((b), (c), (d)) + (x) + (t); \ + (a) = (((a) << (s)) | (((a) & 0xffffffff) >> (32 - (s)))); \ + (a) += (b); + +/* + * SET reads 4 input bytes in little-endian byte order and stores them + * in a properly aligned word in host byte order. + * + * The check for little-endian architectures that tolerate unaligned + * memory accesses is just an optimization. Nothing will break if it + * doesn't work. + */ +#if defined(__i386__) || defined(__x86_64__) || defined(__vax__) +# define SET(n) \ + (*(MD5_u32plus *)&ptr[(n) * 4]) +# define GET(n) \ + SET(n) +#else +# define SET(n) \ + (ctx->block[(n)] = \ + (MD5_u32plus)ptr[(n) * 4] | \ + ((MD5_u32plus)ptr[(n) * 4 + 1] << 8) | \ + ((MD5_u32plus)ptr[(n) * 4 + 2] << 16) | \ + ((MD5_u32plus)ptr[(n) * 4 + 3] << 24)) +# define GET(n) \ + (ctx->block[(n)]) +#endif + +/* + * This processes one or more 64-byte data blocks, but does NOT update + * the bit counters. There are no alignment requirements. + */ +const void *MD5::body(void *ctxBuf, const void *data, size_t size) +{ + MD5_CTX *ctx = (MD5_CTX*)ctxBuf; + const unsigned char *ptr; + MD5_u32plus a, b, c, d; + MD5_u32plus saved_a, saved_b, saved_c, saved_d; + + ptr = (unsigned char*)data; + + a = ctx->a; + b = ctx->b; + c = ctx->c; + d = ctx->d; + + do { + saved_a = a; + saved_b = b; + saved_c = c; + saved_d = d; + +/* Round 1 + * E() has been used instead of F() because F() is already defined in the Arduino core + */ + STEP(E, a, b, c, d, SET(0), 0xd76aa478, 7) + STEP(E, d, a, b, c, SET(1), 0xe8c7b756, 12) + STEP(E, c, d, a, b, SET(2), 0x242070db, 17) + STEP(E, b, c, d, a, SET(3), 0xc1bdceee, 22) + STEP(E, a, b, c, d, SET(4), 0xf57c0faf, 7) + STEP(E, d, a, b, c, SET(5), 0x4787c62a, 12) + STEP(E, c, d, a, b, SET(6), 0xa8304613, 17) + STEP(E, b, c, d, a, SET(7), 0xfd469501, 22) + STEP(E, a, b, c, d, SET(8), 0x698098d8, 7) + STEP(E, d, a, b, c, SET(9), 0x8b44f7af, 12) + STEP(E, c, d, a, b, SET(10), 0xffff5bb1, 17) + STEP(E, b, c, d, a, SET(11), 0x895cd7be, 22) + STEP(E, a, b, c, d, SET(12), 0x6b901122, 7) + STEP(E, d, a, b, c, SET(13), 0xfd987193, 12) + STEP(E, c, d, a, b, SET(14), 0xa679438e, 17) + STEP(E, b, c, d, a, SET(15), 0x49b40821, 22) + +/* Round 2 */ + STEP(G, a, b, c, d, GET(1), 0xf61e2562, 5) + STEP(G, d, a, b, c, GET(6), 0xc040b340, 9) + STEP(G, c, d, a, b, GET(11), 0x265e5a51, 14) + STEP(G, b, c, d, a, GET(0), 0xe9b6c7aa, 20) + STEP(G, a, b, c, d, GET(5), 0xd62f105d, 5) + STEP(G, d, a, b, c, GET(10), 0x02441453, 9) + STEP(G, c, d, a, b, GET(15), 0xd8a1e681, 14) + STEP(G, b, c, d, a, GET(4), 0xe7d3fbc8, 20) + STEP(G, a, b, c, d, GET(9), 0x21e1cde6, 5) + STEP(G, d, a, b, c, GET(14), 0xc33707d6, 9) + STEP(G, c, d, a, b, GET(3), 0xf4d50d87, 14) + STEP(G, b, c, d, a, GET(8), 0x455a14ed, 20) + STEP(G, a, b, c, d, GET(13), 0xa9e3e905, 5) + STEP(G, d, a, b, c, GET(2), 0xfcefa3f8, 9) + STEP(G, c, d, a, b, GET(7), 0x676f02d9, 14) + STEP(G, b, c, d, a, GET(12), 0x8d2a4c8a, 20) + +/* Round 3 */ + STEP(H, a, b, c, d, GET(5), 0xfffa3942, 4) + STEP(H, d, a, b, c, GET(8), 0x8771f681, 11) + STEP(H, c, d, a, b, GET(11), 0x6d9d6122, 16) + STEP(H, b, c, d, a, GET(14), 0xfde5380c, 23) + STEP(H, a, b, c, d, GET(1), 0xa4beea44, 4) + STEP(H, d, a, b, c, GET(4), 0x4bdecfa9, 11) + STEP(H, c, d, a, b, GET(7), 0xf6bb4b60, 16) + STEP(H, b, c, d, a, GET(10), 0xbebfbc70, 23) + STEP(H, a, b, c, d, GET(13), 0x289b7ec6, 4) + STEP(H, d, a, b, c, GET(0), 0xeaa127fa, 11) + STEP(H, c, d, a, b, GET(3), 0xd4ef3085, 16) + STEP(H, b, c, d, a, GET(6), 0x04881d05, 23) + STEP(H, a, b, c, d, GET(9), 0xd9d4d039, 4) + STEP(H, d, a, b, c, GET(12), 0xe6db99e5, 11) + STEP(H, c, d, a, b, GET(15), 0x1fa27cf8, 16) + STEP(H, b, c, d, a, GET(2), 0xc4ac5665, 23) + +/* Round 4 */ + STEP(I, a, b, c, d, GET(0), 0xf4292244, 6) + STEP(I, d, a, b, c, GET(7), 0x432aff97, 10) + STEP(I, c, d, a, b, GET(14), 0xab9423a7, 15) + STEP(I, b, c, d, a, GET(5), 0xfc93a039, 21) + STEP(I, a, b, c, d, GET(12), 0x655b59c3, 6) + STEP(I, d, a, b, c, GET(3), 0x8f0ccc92, 10) + STEP(I, c, d, a, b, GET(10), 0xffeff47d, 15) + STEP(I, b, c, d, a, GET(1), 0x85845dd1, 21) + STEP(I, a, b, c, d, GET(8), 0x6fa87e4f, 6) + STEP(I, d, a, b, c, GET(15), 0xfe2ce6e0, 10) + STEP(I, c, d, a, b, GET(6), 0xa3014314, 15) + STEP(I, b, c, d, a, GET(13), 0x4e0811a1, 21) + STEP(I, a, b, c, d, GET(4), 0xf7537e82, 6) + STEP(I, d, a, b, c, GET(11), 0xbd3af235, 10) + STEP(I, c, d, a, b, GET(2), 0x2ad7d2bb, 15) + STEP(I, b, c, d, a, GET(9), 0xeb86d391, 21) + + a += saved_a; + b += saved_b; + c += saved_c; + d += saved_d; + + ptr += 64; + } while (size -= 64); + + ctx->a = a; + ctx->b = b; + ctx->c = c; + ctx->d = d; + + return ptr; +} + +void MD5::MD5Init(void *ctxBuf) +{ + MD5_CTX *ctx = (MD5_CTX*)ctxBuf; + ctx->a = 0x67452301; + ctx->b = 0xefcdab89; + ctx->c = 0x98badcfe; + ctx->d = 0x10325476; + + ctx->lo = 0; + ctx->hi = 0; + + memset(ctx->block, 0, sizeof(ctx->block)); + memset(ctx->buffer, 0, sizeof(ctx->buffer)); +} + +void MD5::MD5Update(void *ctxBuf, const void *data, size_t size) +{ + MD5_CTX *ctx = (MD5_CTX*)ctxBuf; + MD5_u32plus saved_lo; + MD5_u32plus used, free; + + saved_lo = ctx->lo; + if ((ctx->lo = (saved_lo + size) & 0x1fffffff) < saved_lo) { + ctx->hi++; + } + ctx->hi += size >> 29; + + used = saved_lo & 0x3f; + + if (used) { + free = 64 - used; + + if (size < free) { + memcpy(&ctx->buffer[used], data, size); + return; + } + + memcpy(&ctx->buffer[used], data, free); + data = (unsigned char *)data + free; + size -= free; + body(ctx, ctx->buffer, 64); + } + + if (size >= 64) { + data = body(ctx, data, size & ~(size_t)0x3f); + size &= 0x3f; + } + + memcpy(ctx->buffer, data, size); +} + +void MD5::MD5Final(unsigned char *result, void *ctxBuf) +{ + MD5_CTX *ctx = (MD5_CTX*)ctxBuf; + MD5_u32plus used, free; + + used = ctx->lo & 0x3f; + + ctx->buffer[used++] = 0x80; + + free = 64 - used; + + if (free < 8) { + memset(&ctx->buffer[used], 0, free); + body(ctx, ctx->buffer, 64); + used = 0; + free = 64; + } + + memset(&ctx->buffer[used], 0, free - 8); + + ctx->lo <<= 3; + ctx->buffer[56] = ctx->lo; + ctx->buffer[57] = ctx->lo >> 8; + ctx->buffer[58] = ctx->lo >> 16; + ctx->buffer[59] = ctx->lo >> 24; + ctx->buffer[60] = ctx->hi; + ctx->buffer[61] = ctx->hi >> 8; + ctx->buffer[62] = ctx->hi >> 16; + ctx->buffer[63] = ctx->hi >> 24; + + body(ctx, ctx->buffer, 64); + + result[0] = ctx->a; + result[1] = ctx->a >> 8; + result[2] = ctx->a >> 16; + result[3] = ctx->a >> 24; + result[4] = ctx->b; + result[5] = ctx->b >> 8; + result[6] = ctx->b >> 16; + result[7] = ctx->b >> 24; + result[8] = ctx->c; + result[9] = ctx->c >> 8; + result[10] = ctx->c >> 16; + result[11] = ctx->c >> 24; + result[12] = ctx->d; + result[13] = ctx->d >> 8; + result[14] = ctx->d >> 16; + result[15] = ctx->d >> 24; + + memset(ctx, 0, sizeof(*ctx)); +} +unsigned char* MD5::make_hash(char *arg) +{ + MD5_CTX context; + unsigned char * hash = (unsigned char *) malloc(16); + MD5Init(&context); + MD5Update(&context, arg, strlen(arg)); + MD5Final(hash, &context); + return hash; +} +unsigned char* MD5::make_hash(char *arg,size_t size) +{ + MD5_CTX context; + unsigned char * hash = (unsigned char *) malloc(16); + MD5Init(&context); + MD5Update(&context, arg, size); + MD5Final(hash, &context); + return hash; +} + diff --git a/libraries/RPC/examples/MD5_Checksum/MD5.h b/libraries/RPC/examples/MD5_Checksum/MD5.h new file mode 100644 index 000000000..3ec8d8122 --- /dev/null +++ b/libraries/RPC/examples/MD5_Checksum/MD5.h @@ -0,0 +1,52 @@ +#ifndef MD5_h +#define MD5_h + +#include "Arduino.h" + +/* + * This is an OpenSSL-compatible implementation of the RSA Data Security, + * Inc. MD5 Message-Digest Algorithm (RFC 1321). + * + * Written by Solar Designer in 2001, and placed + * in the public domain. There's absolutely no warranty. + * + * This differs from Colin Plumb's older public domain implementation in + * that no 32-bit integer data type is required, there's no compile-time + * endianness configuration, and the function prototypes match OpenSSL's. + * The primary goals are portability and ease of use. + * + * This implementation is meant to be fast, but not as fast as possible. + * Some known optimizations are not included to reduce source code size + * and avoid compile-time configuration. + */ + +/* + * Updated by Scott MacVicar for arduino + * + */ + +#include + +typedef unsigned long MD5_u32plus; + +typedef struct { + MD5_u32plus lo, hi; + MD5_u32plus a, b, c, d; + unsigned char buffer[64]; + MD5_u32plus block[16]; +} MD5_CTX; + +class MD5 +{ +public: + MD5(); + static unsigned char* make_hash(char *arg); + static unsigned char* make_hash(char *arg,size_t size); + static char* make_digest(const unsigned char *digest, int len); + static const void *body(void *ctxBuf, const void *data, size_t size); + static void MD5Init(void *ctxBuf); + static void MD5Final(unsigned char *result, void *ctxBuf); + static void MD5Update(void *ctxBuf, const void *data, size_t size); +}; + +#endif \ No newline at end of file diff --git a/libraries/RPC/examples/MD5_Checksum/md5_checksum.ino b/libraries/RPC/examples/MD5_Checksum/md5_checksum.ino new file mode 100644 index 000000000..7784548e4 --- /dev/null +++ b/libraries/RPC/examples/MD5_Checksum/md5_checksum.ino @@ -0,0 +1,113 @@ +#include "RPC.h" +#include "MD5.h" + +size_t hash_in_count = 0; +size_t hash_out_count = 0; + +#ifdef CORE_CM4 +size_t data_buf_size = 0; +#else +size_t data_buf_size = 512; +#endif + +typedef std::vector vec_t; + +void fatal_error() { + while (true) { + digitalWrite(LEDR, LOW); + delay(500); + digitalWrite(LEDR, HIGH); + delay(500); + } +} + +vec_t hash_block(vec_t &buf) { + MD5_CTX context; + MD5::MD5Init(&context); + MD5::MD5Update(&context, buf.data(), buf.size()); + + vec_t hash(16); + MD5::MD5Final(&hash[0], &context); + return hash; +} + +vec_t md5hash(vec_t &buf) { + hash_out_count++; + return hash_block(buf); +} + +#ifdef CORE_CM4 +// Called by the host to set the data buffer size. +size_t set_buffer_size(size_t size) { + data_buf_size = size; + return 0; +} +#endif + +void setup() { + #ifdef CORE_CM7 + Serial.begin(115200); + while (!Serial) { + + } + #endif + + if (!RPC.begin()) { + fatal_error(); + } + RPC.bind("md5hash", md5hash); + #ifdef CORE_CM4 + RPC.bind("set_buffer_size", set_buffer_size); + #else + delay(100); + auto ret = RPC.call("set_buffer_size", data_buf_size).as(); + #endif + + pinMode(LEDR, OUTPUT); + pinMode(LEDG, OUTPUT); +} + +void loop() { + static vec_t data; + static uint32_t ticks_start = millis(); + + // Wait for the host processor to set the data buffer size. + if (data_buf_size == 0) { + return; + } else if (data.size() == 0) { + data.resize(data_buf_size, 0); + } + + // Fill the buffer with random data. + for (int i=0; i(); + + // Calculate checksum and compare with the received checksum. + vec_t hash = hash_block(data); + if (memcmp(&hash[0], &ret[0], 16) != 0) { + fatal_error(); + } + + #ifdef CORE_CM4 + if ((hash_in_count % 512) == 0) { + digitalWrite(LEDG, LOW); + delay(10); + digitalWrite(LEDG, HIGH); + delay(10); + } + #endif + + #ifdef CORE_CM7 + if ((hash_in_count % 16) == 0) { + float khs = (hash_in_count + hash_out_count) / (float) (millis() - ticks_start); + Serial.println("Generated: " + String(hash_out_count) + " Received: " + String(hash_in_count) + " " + String(khs) +" KH/S"); + } + //delay(1); + #endif + + hash_in_count++; +} diff --git a/libraries/RPC/examples/PingPong_RAW/ping_pong_raw.ino b/libraries/RPC/examples/PingPong_RAW/ping_pong_raw.ino new file mode 100644 index 000000000..6c79226ce --- /dev/null +++ b/libraries/RPC/examples/PingPong_RAW/ping_pong_raw.ino @@ -0,0 +1,49 @@ +#include "RPC.h" + +void fatal_error() { + while (true) { + digitalWrite(LEDR, LOW); + delay(500); + digitalWrite(LEDR, HIGH); + delay(500); + } +} + +void recv_callback(const uint8_t *buf, size_t len) { + #ifdef CORE_CM7 + Serial.print("<= "); + Serial.write(buf, len); + Serial.println(); + #else + const uint8_t msg[] = "Pong!"; + RPC.write(&msg[0], sizeof(msg), false, true); + #endif +} + +void setup() { + #ifdef CORE_CM7 + Serial.begin(115200); + while (!Serial) { + + } + #endif + + if (!RPC.begin()) { + fatal_error(); + } + RPC.attach(recv_callback); + + pinMode(LEDR, OUTPUT); + pinMode(LEDG, OUTPUT); +} + +void loop() { + #ifdef CORE_CM7 + const uint8_t buf[] = "Ping!"; + Serial.print("=> "); + Serial.write(buf, sizeof(buf)); + Serial.println(); + RPC.write(&buf[0], sizeof(buf)); + delay(100); + #endif +} diff --git a/libraries/RPC/examples/SerialPassthrough_RPC/SerialPassthrough_RPC.ino b/libraries/RPC/examples/SerialPassthrough_RPC/SerialPassthrough_RPC.ino index 38cf7d331..98451da3b 100644 --- a/libraries/RPC/examples/SerialPassthrough_RPC/SerialPassthrough_RPC.ino +++ b/libraries/RPC/examples/SerialPassthrough_RPC/SerialPassthrough_RPC.ino @@ -3,22 +3,18 @@ void setup() { Serial.begin(115200); + while (!Serial) { + } RPC.begin(); } void loop() { - String data = ""; - while (RPC.available()) { - data += (char)RPC.read(); - } - if (data != "") { - Serial.write(data.c_str(), data.length()); - } - data = ""; - while (Serial.available()) { - data += (char)Serial.read(); - } - if (data != "") { - RPC.write(data.c_str(), data.length()); + if (HAL_GetCurrentCPUID() == CM4_CPUID) { + RPC.println("Printed from M4 core"); + delay(1000); + } else { + while (RPC.available()) { + Serial.print((char) RPC.read()); + } } } diff --git a/libraries/RPC/src/RPC.cpp b/libraries/RPC/src/RPC.cpp index 0c327ff82..e2109ee61 100644 --- a/libraries/RPC/src/RPC.cpp +++ b/libraries/RPC/src/RPC.cpp @@ -1,373 +1,258 @@ #include "RPC.h" -static struct rpmsg_endpoint rp_endpoints[4]; +#define ENDPOINT_ID_RAW 0 +#define ENDPOINT_ID_RPC 1 -enum endpoints_t { - ENDPOINT_RAW = 0, - ENDPOINT_RESPONSE = 1 -}; +#define MSGPACK_TYPE_REQUEST 0 +#define MSGPACK_TYPE_RESPONSE 1 +#define MSGPACK_TYPE_NOTIFY 2 -void rpc::client::send_msgpack(RPCLIB_MSGPACK::sbuffer *buffer) { - OPENAMP_send(&rp_endpoints[ENDPOINT_RAW], (const uint8_t*)buffer->data(), buffer->size()); -} - -static RingBufferN intermediate_buffer; -static RingBufferN intermediate_buffer_resp; -//static uint8_t intermediate_buffer_resp[256]; -static rtos::Mutex rx_mtx; - -static bool _init_recv_message = true; - -int RPCClass::rpmsg_recv_callback(struct rpmsg_endpoint *ept, void *data, - size_t len, uint32_t src, void *priv) -{ - RPCClass* rpc = (RPCClass*)priv; +arduino::RPCClass RPC; +osThreadId eventHandlerThreadId; +static rtos::Mutex mutex; +static struct rpmsg_endpoint endpoints[2]; #ifdef CORE_CM4 - if (_init_recv_message) { - _init_recv_message = false; - return 0; - } +static bool endpoints_init[2] = { 0 }; #endif - rx_mtx.lock(); - for (size_t i = 0; i < len; i++) { - intermediate_buffer.store_char(((uint8_t*)data)[i]); - } - rx_mtx.unlock(); +void RPCClass::new_service_cb(struct rpmsg_device *rdev, const char *name, uint32_t dest) { + uint8_t buffer[1] = {0}; + struct rpmsg_endpoint *ept = NULL; - //memcpy(intermediate_buffer, data, len); - - osSignalSet(rpc->dispatcherThreadId, len); + if (strcmp(name, "rpc") == 0) { + ept = &endpoints[ENDPOINT_ID_RPC]; + } else if (strcmp(name, "raw") == 0) { + ept = &endpoints[ENDPOINT_ID_RAW]; + } - return 0; + if (ept) { + OPENAMP_create_endpoint(ept, name, dest, rpmsg_recv_callback, NULL); + OPENAMP_send(ept, buffer, sizeof(buffer)); + } } -static bool _init_resp_message = true; - -int RPCClass::rpmsg_recv_response_callback(struct rpmsg_endpoint *ept, void *data, - size_t len, uint32_t src, void *priv) -{ - RPCClass* rpc = (RPCClass*)priv; - -#ifdef CORE_CM4 - if (_init_resp_message) { - _init_resp_message = false; +int RPCClass::rpmsg_recv_callback(struct rpmsg_endpoint *ept, void *data, size_t len, uint32_t src, void *priv) { + #ifdef CORE_CM4 + if (!endpoints_init[ENDPOINT_ID_RPC] && ept == &endpoints[ENDPOINT_ID_RPC]) { + endpoints_init[ENDPOINT_ID_RPC] = true; return 0; - } -#endif - - rx_mtx.lock(); - for (size_t i = 0; i < len; i++) { - intermediate_buffer_resp.store_char(((uint8_t*)data)[i]); - } - //memcpy(intermediate_buffer_resp, data, len); - rx_mtx.unlock(); + } else if (!endpoints_init[ENDPOINT_ID_RAW] && ept == &endpoints[ENDPOINT_ID_RAW]) { + endpoints_init[ENDPOINT_ID_RAW] = true; + return 0; + } + #endif - osSignalSet(rpc->responseThreadId, len); + if (ept == &endpoints[ENDPOINT_ID_RAW]) { + // data on raw endpoint + if (RPC.raw_callback) { + RPC.raw_callback.call((uint8_t *) data, len); + } else { + for (size_t i=0; istart(&eventHandler); + // Allow the event thread to run once to set the thread ID, and get into a known state. + osDelay(1); - eventThread = new rtos::Thread(osPriorityHigh, 4096, nullptr, "rpc_evt"); - eventThread->start(&eventHandler); - - dispatcherThread = new rtos::Thread(osPriorityNormal, 4096, nullptr, "rpc_dispatch"); - dispatcherThread->start(mbed::callback(this, &RPCClass::dispatch)); - - responseThread = new rtos::Thread(osPriorityNormal, 4096, nullptr, "rpc_response"); - responseThread->start(mbed::callback(this, &RPCClass::response)); - - /* Initialize OpenAmp and libmetal libraries */ - if (MX_OPENAMP_Init(RPMSG_MASTER, new_service_cb) != HAL_OK) { - return 0; - } - - //metal_set_log_handler(metal_default_log_handler); - - /* Initialize the rpmsg endpoint to set default addresses to RPMSG_ADDR_ANY */ - rpmsg_init_ept(&rp_endpoints[ENDPOINT_RAW], "raw", RPMSG_ADDR_ANY, RPMSG_ADDR_ANY, NULL, NULL); - rpmsg_init_ept(&rp_endpoints[ENDPOINT_RESPONSE], "response", RPMSG_ADDR_ANY, RPMSG_ADDR_ANY, NULL, NULL); - - rp_endpoints[ENDPOINT_RAW].priv = this; - rp_endpoints[ENDPOINT_RESPONSE].priv = this; - - bootM4(); - - /* - * The rpmsg service is initiate by the remote processor, on H7 new_service_cb - * callback is received on service creation. Wait for the callback - */ - auto err = OPENAMP_Wait_EndPointready(&rp_endpoints[ENDPOINT_RAW], millis() + 500); - err |= OPENAMP_Wait_EndPointready(&rp_endpoints[ENDPOINT_RESPONSE], millis() + 500); + // Initialize OpenAmp and libmetal libraries + if (MX_OPENAMP_Init(RPMSG_HOST, new_service_cb) != HAL_OK) { + return 0; + } - if (err == 0) { - initialized = true; - } else { - return 0; - } + // Initialize rpmsg endpoints. + memset(endpoints, 0, sizeof(endpoints)); - // Send first dummy message to enable the channel - uint8_t message = 0x00; - write(ENDPOINT_RAW, &message, sizeof(message)); - write(ENDPOINT_RESPONSE, &message, sizeof(message)); + // Boot the CM4. + cm4_kick(); - return 1; + // Wait for the remote to announce the services with a timeout. + uint32_t millis_start = millis(); + while (endpoints[ENDPOINT_ID_RPC].rdev == NULL || endpoints[ENDPOINT_ID_RAW].rdev == NULL) { + if ((millis() - millis_start) >= 5000) { + return 0; + } + osDelay(10); + } + return 1; } - #endif - #ifdef CORE_CM4 +int RPCClass::begin() { + eventThread = new rtos::Thread(osPriorityHigh, 16*1024, nullptr, "rpc_evt"); + eventThread->start(&eventHandler); + // Allow the event thread to run once to set the thread ID, and get into a known state. + osDelay(1); + + // Initialize OpenAmp and libmetal libraries + if (MX_OPENAMP_Init(RPMSG_REMOTE, NULL) != 0) { + return 0; + } -int RPCClass::begin(long unsigned int np, uint16_t nd) { - - eventThread = new rtos::Thread(osPriorityHigh, 4096, nullptr, "rpc_evt"); - eventThread->start(&eventHandler); - - dispatcherThread = new rtos::Thread(osPriorityNormal, 4096, nullptr, "rpc_dispatch"); - dispatcherThread->start(mbed::callback(this, &RPCClass::dispatch)); - - responseThread = new rtos::Thread(osPriorityNormal, 4096, nullptr, "rpc_response"); - responseThread->start(mbed::callback(this, &RPCClass::response)); - - /* Initialize OpenAmp and libmetal libraries */ - if (MX_OPENAMP_Init(RPMSG_REMOTE, NULL) != 0) { - return 0; - } - - rp_endpoints[ENDPOINT_RAW].priv = this; - rp_endpoints[ENDPOINT_RESPONSE].priv = this; - - /* create a endpoint for raw rmpsg communication */ - int status = OPENAMP_create_endpoint(&rp_endpoints[ENDPOINT_RAW], "raw", RPMSG_ADDR_ANY, - rpmsg_recv_callback, NULL); - if (status < 0) { - return 0; - } - - status = OPENAMP_create_endpoint(&rp_endpoints[ENDPOINT_RESPONSE], "response", RPMSG_ADDR_ANY, - rpmsg_recv_response_callback, NULL); - if (status < 0) { - return 0; - } + // Create RAW endpoint. + if (OPENAMP_create_endpoint(&endpoints[ENDPOINT_ID_RAW], "raw", RPMSG_ADDR_ANY, rpmsg_recv_callback, NULL) < 0) { + return 0; + } + + // Create RPC endpoint. + if (OPENAMP_create_endpoint(&endpoints[ENDPOINT_ID_RPC], "rpc", RPMSG_ADDR_ANY, rpmsg_recv_callback, NULL) < 0) { + return 0; + } - initialized = true; + // Wait for endpoints to be initialized first by the host before allowing + // the remote to use the endpoints. + uint32_t millis_start = millis(); + while (!endpoints_init[ENDPOINT_ID_RPC] || !endpoints_init[ENDPOINT_ID_RAW]) { + if ((millis() - millis_start) >= 5000) { + return 0; + } + osDelay(10); + } - return 1; + return 1; } - #endif -using raw_call_t = std::tuple; +void RPCClass::response(uint8_t *buf, size_t len) { + unpacker.reset(); + unpacker.reserve_buffer(len); + memcpy(unpacker.buffer(), buf, len); + unpacker.buffer_consumed(len); -void RPCClass::response() { - responseThreadId = osThreadGetId(); - - for (int i = 0; i< 10; i++) { - clients[i] = NULL; - } - - while (true) { - osSignalWait(0, osWaitForever); - -{ - - RPCLIB_MSGPACK::unpacker pac; - - rx_mtx.lock(); - int len = intermediate_buffer_resp.available(); - for (int i = 0; i < len; i++) { - pac.buffer()[i] = intermediate_buffer_resp.read_char(); - } - pac.buffer_consumed(len); - rx_mtx.unlock(); - - //memcpy(pac.buffer(), intermediate_buffer_resp, v.value.signals); - //pac.buffer_consumed(v.value.signals); - - RPCLIB_MSGPACK::unpacked result; - while (pac.next(result)) { + RPCLIB_MSGPACK::unpacked result; + while (unpacker.next(result)) { auto r = rpc::detail::response(std::move(result)); auto id = r.get_id(); // fill the correct client stuff rpc::client* client = NULL; for (int i = 0; i < 10; i++) { - if (clients[i] != NULL) { - if ((uint)clients[i]->callThreadId == id) { - client = clients[i]; - break; + if (clients[i] != NULL) { + if ((uint)clients[i]->callThreadId == id) { + client = clients[i]; + break; + } } - } } if (client != NULL) { - client->result = std::move(*r.get_result()); - // Unlock callThreadId thread - osSignalSet(client->callThreadId, 0x1); + client->result = std::move(*r.get_result()); + // Unlock callThreadId thread + osSignalSet(client->callThreadId, 0x1); } - } } - } } -void RPCClass::dispatch() { - - dispatcherThreadId = osThreadGetId(); - - while (true) { - osSignalWait(0, osWaitForever); - -{ - RPCLIB_MSGPACK::unpacker pac; - rx_mtx.lock(); - int len = intermediate_buffer.available(); - for (int i = 0; i< len; i++) { - pac.buffer()[i] = intermediate_buffer.read_char(); - } - pac.buffer_consumed(len); - rx_mtx.unlock(); - - //memcpy(pac.buffer(), intermediate_buffer, v.value.signals); - //pac.buffer_consumed(v.value.signals); +void RPCClass::request(uint8_t *buf, size_t len) { + unpacker.reset(); + unpacker.reserve_buffer(len); + memcpy(unpacker.buffer(), buf, len); + unpacker.buffer_consumed(len); RPCLIB_MSGPACK::unpacked result; - while (pac.next(result)) { - auto msg = result.get(); - if (msg.via.array.size == 1) { - // raw array - raw_call_t arr; - msg.convert(arr); - - std::vector buf; - std::get<0>(arr).convert(buf); - - for (size_t i = 0; i < buf.size(); i++) { - rx_buffer.store_char(buf[i]); - } - // call attached function - if (_rx) { - _rx.call(); - } - } - - if (msg.via.array.size > 2) { - auto resp = rpc::detail::dispatcher::dispatch(msg, true); + while (unpacker.next(result)) { + auto msg = result.get(); + auto resp = rpc::detail::dispatcher::dispatch(msg, false); auto data = resp.get_data(); - if (resp.is_empty()) { - //printf("no response\n"); - } else { - OPENAMP_send(&rp_endpoints[ENDPOINT_RESPONSE], (const uint8_t*)data.data(), data.size()); + if (!resp.is_empty()) { + OPENAMP_send(&endpoints[ENDPOINT_ID_RPC], data.data(), data.size()); } - } } - } -} } - size_t RPCClass::write(uint8_t c) { - write(&c, 1); - return 1; + return write(&c, 1, true); } -size_t RPCClass::write(const uint8_t* buf, size_t len) { - return write(ENDPOINT_RAW, buf, len); +void rpc::client::write(RPCLIB_MSGPACK::sbuffer *buffer) { + RPC.write((const uint8_t *) buffer->data(), buffer->size(), false); } -size_t RPCClass::write(uint8_t ep, const uint8_t* buf, size_t len) { - - std::vector tx_buffer; - for (size_t i = 0; i < len; i++) { - tx_buffer.push_back(buf[i]); - } - auto call_obj = std::make_tuple(tx_buffer); - - auto buffer = new RPCLIB_MSGPACK::sbuffer; - RPCLIB_MSGPACK::pack(*buffer, call_obj); - - OPENAMP_send(&rp_endpoints[ep], (const uint8_t*)buffer->data(), buffer->size()); - delete buffer; - return len; +size_t RPCClass::write(const uint8_t *buf, size_t len, bool raw) { + mutex.lock(); + OPENAMP_send(&endpoints[raw ? ENDPOINT_ID_RAW : ENDPOINT_ID_RPC], buf, len); + mutex.unlock(); + return len; } - -arduino::RPCClass RPC; \ No newline at end of file diff --git a/libraries/RPC/src/RPC.h b/libraries/RPC/src/RPC.h index 8aa8fa7e7..533988323 100644 --- a/libraries/RPC/src/RPC.h +++ b/libraries/RPC/src/RPC.h @@ -35,127 +35,100 @@ extern "C" { #include "mbed.h" -typedef struct _service_request { - uint8_t* data; -} service_request; - namespace arduino { - class RPCClass : public Stream, public rpc::detail::dispatcher { - public: - RPCClass() {}; - int begin(long unsigned int = 0, uint16_t = 0); - void end() {}; - int available(void) { - return rx_buffer.available(); - }; - int peek(void) { - return rx_buffer.peek(); - } - int read(void) { - return rx_buffer.read_char(); - } - void flush(void) {}; - size_t write(uint8_t c); - size_t write(const uint8_t*, size_t); - size_t write(uint8_t ep, const uint8_t* buf, size_t len); - - using Print::write; // pull in write(str) and write(buf, size) from Print - operator bool() { - return initialized; - } - - void attach(void (*fptr)(void)) - { - if (fptr != NULL) { - _rx = mbed::Callback(fptr); - } - } - - template - void send(std::string const &func_name, - Args... args) { - - auto client = new rpc::client(); - client->send(func_name, args...); - delete client; - } - - void setTimeout(uint32_t milliseconds) { - _timeout = milliseconds; - } - - template - RPCLIB_MSGPACK::object_handle call(std::string const &func_name, - Args... args) { - // find a free spot in clients[] - // create new object - // protect this with mutex - - mtx.lock(); - int i = 0; - for (i=0; i<10; i++) { - if (clients[i] == NULL) { - clients[i] = new rpc::client(); - break; - } - } - mtx.unlock(); - - clients[i]->setTimeout(_timeout); - has_timed_out = false; - - // thread start and client .call - clients[i]->call(func_name, args...); - - if (clients[i]->timedOut()) { - has_timed_out = true; - } - RPCLIB_MSGPACK::object_handle ret = std::move(clients[i]->result); - - mtx.lock(); - delete clients[i]; - clients[i] = NULL; - mtx.unlock(); - return ret; - } - - rpc::client* clients[10]; - - bool timedOut() { - return has_timed_out; - } - - private: - RingBufferN<256> rx_buffer; - bool initialized = false; - - static int rpmsg_recv_callback(struct rpmsg_endpoint *ept, void *data, - size_t len, uint32_t src, void *priv); - static int rpmsg_recv_response_callback(struct rpmsg_endpoint *ept, void *data, - size_t len, uint32_t src, void *priv); - - static void new_service_cb(struct rpmsg_device *rdev, const char *name, uint32_t dest); - - void dispatch(); - void response(); - events::EventQueue eventQueue; - mbed::Ticker ticker; - rtos::Thread* eventThread; - rtos::Thread* dispatcherThread; - rtos::Thread* responseThread; - rtos::Mutex mtx; - - mbed::Callback _rx; - - uint32_t _timeout = osWaitForever; - bool has_timed_out = false; - - //rpc::detail::response response; - RPCLIB_MSGPACK::object_handle call_result; - - osThreadId dispatcherThreadId; - osThreadId responseThreadId; + public: + RPCClass() { + for (int i = 0; i< 10; i++) { + clients[i] = NULL; + } + }; + int begin(); + void end() {}; + int available(void) { + return rx_buffer.available(); + }; + int peek(void) { + return rx_buffer.peek(); + } + int read(void) { + return rx_buffer.read_char(); + } + void flush(void) {}; + size_t write(uint8_t c); + size_t write(const uint8_t *buf, size_t len, bool raw = true); + + using Print::write; // pull in write(str) and write(buf, size) from Print + operator bool() { + return initialized; + } + + void attach(void (*fptr)(const uint8_t *buf, size_t len)) { + if (fptr != NULL) { + raw_callback = mbed::Callback(fptr); + } + } + + template + void send(std::string const &func_name, Args... args) { + auto client = new rpc::client(); + client->send(func_name, args...); + delete client; + } + + void setTimeout(uint32_t milliseconds) { + _timeout = milliseconds; + } + + template + RPCLIB_MSGPACK::object_handle call(std::string const &func_name, Args... args) { + // find a free spot in clients[] + // create new object + // protect this with mutex + + int i = 0; + for (i=0; i<10; i++) { + if (clients[i] == NULL) { + clients[i] = new rpc::client(); + break; + } + } + + clients[i]->setTimeout(_timeout); + has_timed_out = false; + + // thread start and client .call + clients[i]->call(func_name, args...); + + if (clients[i]->timedOut()) { + has_timed_out = true; + } + RPCLIB_MSGPACK::object_handle ret = std::move(clients[i]->result); + + delete clients[i]; + clients[i] = NULL; + return ret; + } + + bool timedOut() { + return has_timed_out; + } + + rpc::client* clients[10]; + RingBufferN<512> rx_buffer; + mbed::Callback raw_callback; + + private: + bool initialized = false; + uint32_t _timeout = osWaitForever; + bool has_timed_out = false; + rtos::Thread* eventThread; + RPCLIB_MSGPACK::unpacker unpacker; + + static void new_service_cb(struct rpmsg_device *rdev, const char *name, uint32_t dest); + static int rpmsg_recv_callback(struct rpmsg_endpoint *ept, void *data, size_t len, uint32_t src, void *priv); + void request(uint8_t *buf, size_t len); + void response(uint8_t *buf, size_t len); }; } diff --git a/libraries/RPC/src/RPC_client.h b/libraries/RPC/src/RPC_client.h index b1e0ed34c..860c0be32 100644 --- a/libraries/RPC/src/RPC_client.h +++ b/libraries/RPC/src/RPC_client.h @@ -26,7 +26,7 @@ class client { auto buffer = new RPCLIB_MSGPACK::sbuffer; RPCLIB_MSGPACK::pack(*buffer, call_obj); - send_msgpack(buffer); + write(buffer); auto e = osSignalWait(0, timeout); delete buffer; @@ -58,7 +58,7 @@ class client { auto buffer = new RPCLIB_MSGPACK::sbuffer; RPCLIB_MSGPACK::pack(*buffer, call_obj); - send_msgpack(buffer); + write(buffer); delete buffer; } @@ -81,7 +81,7 @@ class client { private: enum class request_type { call = 0, notification = 2 };; - void send_msgpack(RPCLIB_MSGPACK::sbuffer *buffer); + void write(RPCLIB_MSGPACK::sbuffer *buffer); void getResult(RPCLIB_MSGPACK::object_handle& res); }; }