PX4-Autopilot/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp

/****************************************************************************
 *
 * 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
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include <time.h>
#include <unistd.h>
#include <fcntl.h>
#include "px4muorb_KraitRpcWrapper.hpp"
#include <rpcmem.h>
#include <px4muorb.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/shmem.h>
#include <drivers/drv_hrt.h>
#include <string.h>

using namespace px4muorb;

/* Flags applied to the allocation of the shared memory for RPC */
#define MUORB_KRAIT_FASTRPC_MEM_FLAGS         0

/* 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

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;

unsigned char *adsp_changed_index = 0;

// The DSP timer can be read from this file.
#define DSP_TIMER_FILE "/sys/kernel/boot_adsp/qdsp_qtimer"

/**
 * Helper function to get timer difference between time on DSP and appsproc side.
 * Usually the DSP gets started around 2s before the appsproc (Linux) side and
 * therefore the clocks are not in sync. We change the clock on the DSP side but
 * for this we need to find the offset first and then tell code on the DSP side.
 *
 * @param time_diff_us: pointer to time offset to set.
 * @return: 0 on success, < 0 on error.
 */
int calc_timer_diff_to_dsp_us(int32_t *time_diff_us);

int calc_timer_diff_to_dsp_us(int32_t *time_diff_us)
{
#if defined(__PX4_POSIX_EXCELSIOR)
	*time_diff_us = 0;
	return 0;
#endif
	int fd = open(DSP_TIMER_FILE, O_RDONLY);

	if (fd < 0) {
		PX4_ERR("Could not open DSP timer file %s.", DSP_TIMER_FILE);
		return -1;
	}

	char buffer[21] {};
	int bytes_read = read(fd, buffer, sizeof(buffer));

	if (bytes_read < 0) {
		PX4_ERR("Could not read DSP timer file %s.", DSP_TIMER_FILE);
		close(fd);
		return -2;
	}

	// Do this call right after reading to avoid latency here.
	timespec ts;
	clock_gettime(CLOCK_MONOTONIC, &ts);
	uint64_t time_appsproc = ((uint64_t) ts.tv_sec) * 1000000llu
				 + (ts.tv_nsec / 1000);

	close(fd);

	uint64_t time_dsp;
	int ret = sscanf(buffer, "%llx", &time_dsp);

	if (ret < 0) {
		PX4_ERR("Could not parse DSP timer.");
		return -3;
	}

	// The clock count needs to get converted to us.
	// The magic value of 19.2 was provided by Qualcomm.
	time_dsp /= 19.2;

	// Before casting to in32_t, check if it fits.
	uint64_t abs_diff =
		(time_appsproc > time_dsp) ?
		(time_appsproc - time_dsp) : (time_dsp - time_appsproc);

	if (abs_diff > INT32_MAX) {
		PX4_ERR("Timer difference too big");
		return -4;
	}

	*time_diff_us = time_appsproc - time_dsp;

	PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us",
		  time_dsp, time_appsproc);
	PX4_DEBUG("found time_diff: %li us, %.6f s",
		  *time_diff_us, ((double)*time_diff_us) / 1e6);

	return 0;
}

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__);
	}

	adsp_changed_index = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
			     MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t));

	rc = (adsp_changed_index != NULL) ? true : false;

	if (!rc) {
		PX4_ERR("%s rpcmem_alloc failed! for adsp_changed_index", __FUNCTION__);

	} else {
		memset(adsp_changed_index, 0, PARAM_BUFFER_SIZE * sizeof(uint8_t));
	}

	int32_t time_diff_us;

	if (calc_timer_diff_to_dsp_us(&time_diff_us) != 0) {
		rc = false;
		return rc;
	}

	// call muorb initialize routine.
	if (px4muorb_orb_initialize() != 0) {
		PX4_ERR("%s Error calling the uorb fastrpc initalize method..",
			__FUNCTION__);
		rc = false;
		return rc;
	}

	// TODO FIXME: remove this check or make it less verbose later
	px4muorb_set_absolute_time_offset(time_diff_us);

	uint64_t time_dsp;
	px4muorb_get_absolute_time(&time_dsp);

	uint64_t time_appsproc = hrt_absolute_time();

	int diff = (time_dsp - time_appsproc);

	PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us",
		  time_dsp, time_appsproc, diff);

	_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;
	}

	if (adsp_changed_index != NULL) {
		rpcmem_free(adsp_changed_index);
		adsp_changed_index = 0;
	}

	_Initialized = false;
	return true;
}

int32_t px4muorb::KraitRpcWrapper::TopicAdvertised(const char *topic)
{
	return ((_Initialized) ? px4muorb_topic_advertised(topic) : -1);
}

int32_t px4muorb::KraitRpcWrapper::TopicUnadvertised(const char *topic)
{
	return ((_Initialized) ? px4muorb_topic_unadvertised(topic) : -1);
}

int32_t px4muorb::KraitRpcWrapper::AddSubscriber(const char *topic)
{
	return ((_Initialized) ? px4muorb_add_subscriber(topic) : -1);
}

int32_t px4muorb::KraitRpcWrapper::RemoveSubscriber(const char *topic)
{
	return (_Initialized ? px4muorb_remove_subscriber(topic) : -1);
}

int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic,
		int32_t *status)
{
	return (_Initialized ? px4muorb_is_subscriber_present(topic, status) : -1);
}

int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic,
		int32_t length_in_bytes, const uint8_t *data)
{
	return (_Initialized ?
		px4muorb_send_topic_data(topic, data, length_in_bytes) : -1);
}

int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic,
		int32_t *length_in_bytes, uint8_t **data)
{
	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 px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data,
		int32_t *length_in_bytes, int32_t *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 px4muorb::KraitRpcWrapper::UnblockReceiveData()
{
	return (_Initialized ? px4muorb_unblock_recieve_msg() : -1);
}