From a002d6eabd2bdde9f26d67f5d825f76f07fa1b71 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 21 Jan 2016 12:07:33 -0800 Subject: [PATCH] Added rpcmem.a to posix_eagle_release build Signed-off-by: Mark Charlebois --- cmake/cmake_hexagon | 2 +- src/modules/muorb/krait/CMakeLists.txt | 5 +- .../muorb/krait/px4muorb_KraitRpcWrapper.cpp | 202 ++++++++++++++---- 3 files changed, 166 insertions(+), 43 deletions(-) diff --git a/cmake/cmake_hexagon b/cmake/cmake_hexagon index 720078ffe4..c148d92f59 160000 --- a/cmake/cmake_hexagon +++ b/cmake/cmake_hexagon @@ -1 +1 @@ -Subproject commit 720078ffe4964ee87742248f4563633d1f2b852c +Subproject commit c148d92f59c3f5e619093d16c20c4285a7f0ce11 diff --git a/src/modules/muorb/krait/CMakeLists.txt b/src/modules/muorb/krait/CMakeLists.txt index 3c003e1742..aa28b2d7b0 100644 --- a/src/modules/muorb/krait/CMakeLists.txt +++ b/src/modules/muorb/krait/CMakeLists.txt @@ -31,7 +31,10 @@ # ############################################################################ include_directories(${CMAKE_BINARY_DIR}/src/firmware/posix) -include_directories(${HEXAGON_SDK_ROOT}/inc/stddef) +include_directories( + ${HEXAGON_SDK_ROOT}/inc/stddef + ${HEXAGON_SDK_ROOT}/lib/common/rpcmem + ) px4_add_module( MODULE modules__muorb__krait diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp index 11638ab3fd..d862384398 100644 --- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp +++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. + * Copyright (C) 2016 Ramakrishna Kintada. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,77 +30,197 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ -#include #include "px4muorb_KraitRpcWrapper.hpp" +#include #include "px4muorb.h" +#include "px4_log.h" using namespace px4muorb; -/** - * Constructor - */ -KraitRpcWrapper::KraitRpcWrapper() {} +/* Flags applied to the allocation of the shared memory for RPC */ +#define MUORB_KRAIT_FASTRPC_MEM_FLAGS 0 -/** - * destructor - */ -KraitRpcWrapper::~KraitRpcWrapper() {} +/* The ID of the HEAP to be used when allocating shared memory */ +//TODO This heap id is used for test purposes. We need to find out the correct one. +#define MUORB_KRAIT_FASTRPC_HEAP_ID 22 -/** - * Initiatizes the rpc channel px4 muorb - */ -bool KraitRpcWrapper::Initialize() +static char *_TopicNameBuffer = 0; +static const int32_t _MAX_TOPIC_NAME_BUFFER = 256; + +static uint8_t *_DataBuffer = 0; +static const uint32_t _MAX_DATA_BUFFER_SIZE = 2048; + +static bool _Initialized = false; + +// These numbers are based off the fact each fastrpc call for 64K packet is 94us. +// hence we are trying to allocation 64K of byte buffers. +static const uint32_t _MAX_TOPIC_DATA_BUFFER_SIZE = 1024; +static const uint32_t _MAX_TOPICS = 64; +static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS; +static uint8_t *_BulkTransferBuffer = 0; + + +px4muorb::KraitRpcWrapper::KraitRpcWrapper() { - return (px4muorb_orb_initialize() == 0); } -/** - * Terminate to clean up the resources. This should be called at program exit - */ -bool KraitRpcWrapper::Terminate() +px4muorb::KraitRpcWrapper::~KraitRpcWrapper() { - // FIXME - TBD +} + +bool px4muorb::KraitRpcWrapper::Initialize() +{ + bool rc = true; + + PX4_DEBUG("%s Now calling rpcmem_init...", __FUNCTION__); + rpcmem_init(); + + PX4_DEBUG("%s Now calling rpcmem_alloc...", __FUNCTION__); + + _BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, MUORB_KRAIT_FASTRPC_MEM_FLAGS, + _MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)); + rc = (_BulkTransferBuffer != NULL) ? true : false; + + if (!rc) { + PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers", __FUNCTION__); + return rc; + + } else { + PX4_DEBUG("%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p", + __FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer); + } + + _TopicNameBuffer = (char *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, + MUORB_KRAIT_FASTRPC_MEM_FLAGS, _MAX_TOPIC_NAME_BUFFER * sizeof(char)); + + rc = (_TopicNameBuffer != NULL) ? true : false; + + if (!rc) { + PX4_ERR("%s rpcmem_alloc failed! for topic_name_buffer", __FUNCTION__); + rpcmem_free(_BulkTransferBuffer); + return rc; + + } else { + PX4_DEBUG("%s rpcmem_alloc passed for topic_name_buffer", __FUNCTION__); + } + + // now allocate the data buffer. + _DataBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID, + MUORB_KRAIT_FASTRPC_MEM_FLAGS, _MAX_DATA_BUFFER_SIZE * sizeof(uint8_t)); + + rc = (_DataBuffer != NULL) ? true : false; + + if (!rc) { + PX4_ERR("%s rpcmem_alloc failed! for DataBuffer", __FUNCTION__); + // free the topic name buffer; + rpcmem_free(_BulkTransferBuffer); + rpcmem_free(_TopicNameBuffer); + _TopicNameBuffer = 0; + return rc; + + } else { + PX4_DEBUG("%s rpcmem_alloc passed for data_buffer", __FUNCTION__); + } + + // call myorb intiialize rotine. + if (px4muorb_orb_initialize() != 0) { + PX4_ERR("%s Error calling the uorb fastrpc initalize method..", __FUNCTION__); + rc = false; + return rc; + } + + _Initialized = true; + return rc; +} + +bool px4muorb::KraitRpcWrapper::Terminate() +{ + if (_BulkTransferBuffer != NULL) { + rpcmem_free(_BulkTransferBuffer); + _BulkTransferBuffer = 0; + } + + if (_TopicNameBuffer != NULL) { + rpcmem_free(_TopicNameBuffer); + _TopicNameBuffer = 0; + } + + if (_DataBuffer != NULL) { + rpcmem_free(_DataBuffer); + _DataBuffer = 0; + } + + _Initialized = false; return true; } -/** - * Muorb related functions to pub/sub of orb topic from krait to adsp - */ -int32_t KraitRpcWrapper::AddSubscriber(const char *topic) +int32_t px4muorb::KraitRpcWrapper::AddSubscriber(const char *topic) { - return px4muorb_add_subscriber(topic); + return ((_Initialized) ? px4muorb_add_subscriber(topic) : -1); } -int32_t KraitRpcWrapper::RemoveSubscriber(const char *topic) +int32_t px4muorb::KraitRpcWrapper::RemoveSubscriber(const char *topic) { - return px4muorb_remove_subscriber(topic); + return (_Initialized ? px4muorb_remove_subscriber(topic) : -1); } -int32_t KraitRpcWrapper::SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data) +int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic, int32_t *status) { - return px4muorb_send_topic_data(topic, data, length_in_bytes); + return (_Initialized ? px4muorb_is_subscriber_present(topic, status) : -1); } -int32_t KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data) +int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data) { - // FIXME ?? - int topic_nameLen = 0; - int rv = px4muorb_receive_msg(msg_type, *topic, topic_nameLen, *data, *length_in_bytes, length_in_bytes); - return rv; + return (_Initialized ? px4muorb_send_topic_data(topic, data, length_in_bytes) : -1); } -int32_t KraitRpcWrapper::IsSubscriberPresent(const char *topic, int32_t *status) +int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, + uint8_t **data) { - return px4muorb_is_subscriber_present(topic, status); + int32_t rc = -1; + + if (_Initialized) { + rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, + length_in_bytes); + + if (rc == 0) { + *topic = _TopicNameBuffer; + *data = _DataBuffer; + + } else { + PX4_ERR("ERROR: Getting data from fastRPC link"); + } + + } else { + PX4_ERR("ERROR: FastRpcWrapper Not Initialized"); + } + + return rc; } -int32_t KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count) +int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count) { - return px4muorb_receive_bulk_data(*bulk_data, *length_in_bytes, length_in_bytes, topic_count); + int32_t rc = -1; + + if (_Initialized) { + //rc = px4muorb_receive_msg( msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, length_in_bytes ); + rc = px4muorb_receive_bulk_data(_BulkTransferBuffer, _MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count); + + if (rc == 0) { + *bulk_data = _BulkTransferBuffer; + + } else { + PX4_ERR("ERROR: Getting Bulk data from fastRPC link"); + } + + } else { + PX4_ERR("ERROR: FastRpcWrapper Not Initialized"); + } + + return rc; } -int32_t KraitRpcWrapper::UnblockReceiveData() +int32_t px4muorb::KraitRpcWrapper::UnblockReceiveData() { - return px4muorb_unblock_recieve_msg(); + return (_Initialized ? px4muorb_unblock_recieve_msg() : -1); } -