sl_async_transceiver.cpp

Go to the documentation of this file.

/*
 *  Slamtec LIDAR SDK
 *
 *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
 *  http://www.slamtec.com
 *
 */
 /*
  * 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.
  *
  * 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 HOLDER 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 "sdkcommon.h"
#include "hal/abs_rxtx.h"
#include "hal/thread.h"
#include "hal/types.h"
#include "hal/assert.h"
#include "hal/locker.h"
#include "hal/socket.h"
#include "hal/event.h"
 
#include "sl_async_transceiver.h"
 
 
 
namespace sl { namespace internal {
 
 
 
 
ProtocolMessage::ProtocolMessage()
        : len(0)
        , cmd(0)
        , data(NULL)
        , _databufsize(0)
                , _usingOutterData(false)
{
    _changeBufSize();
}
 
ProtocolMessage::ProtocolMessage(_u8 cmd, const void* buffer, size_t size)
        : len(size)
        , cmd(cmd)
        , data(NULL)
    , _databufsize(0)
        , _usingOutterData(false)
{
    _changeBufSize();
        if (buffer)
        {
                memcpy(data, buffer, size);
        }
}
 
ProtocolMessage::ProtocolMessage(const ProtocolMessage& srcMsg)
        : len(srcMsg.len)
        , cmd(srcMsg.cmd)
        , data(NULL)
    , _databufsize(0)
        , _usingOutterData(false)
{
    _changeBufSize( true );
        if (srcMsg.data && len)
        {
                memcpy(data, srcMsg.data, len);
        }
}
 
ProtocolMessage::~ProtocolMessage()
{
        this->cleanData();
}
 
ProtocolMessage& ProtocolMessage::operator =(const ProtocolMessage& srcMessage)
{
        this->cleanData();
 
 
        this->len = srcMessage.len;
        this->cmd = srcMessage.cmd;
 
    _changeBufSize( true );
        if (srcMessage.data && len)
        {
                memcpy(data, srcMessage.data, len);
        }
 
        return *this;
}
 
void ProtocolMessage::setDataBuf(_u8 *buffer, size_t size)
{
        this->cleanData();
 
        len = size;
        data = buffer;
    _databufsize = size;
        _usingOutterData = true;
}
 
void ProtocolMessage::fillData(const void * buffer, size_t size)
{
        len = size;
    _changeBufSize();
    if (buffer)
            memcpy(data, buffer, size);
}
 
void ProtocolMessage::cleanData()
{
        if (data) 
        {
                if (!_usingOutterData)
                {
                        delete [] data;
                }
                data = NULL;
                len = 1;
        _databufsize = 0;
        }
}
 
void ProtocolMessage::_changeBufSize( bool force_compact)
{
    size_t actual_size  = getPayloadSize();
 
    size_t new_buf_size = actual_size;
 
 
    if (!_usingOutterData)
    {
        // nothing to do
        if ( new_buf_size == _databufsize ) return;
 
        if ( new_buf_size < _databufsize){
 
            if ( (_databufsize >> 1) < new_buf_size)
            {
                // reuse the current buffer
                if (!force_compact) return;
            }else
            {
                // the current buffer size is much bigger, we need to release it to save memory
            }
        }
    }
 
    // we need to change the buffer
    cleanData();
    // the cleanData() will reset the length info, so we need to restore it
    len = actual_size;
    data = new _u8[new_buf_size];
    _databufsize = new_buf_size;
}
 
 
AsyncTransceiver::AsyncTransceiver(IAsyncProtocolCodec& codec)
        : _bindedChannel(NULL)
        , _codec(codec)
        , _isWorking(false)
    , _workingFlag(0)
{
 
}
 
AsyncTransceiver::~AsyncTransceiver()
{
    unbindAndClose();
}
 
u_result AsyncTransceiver::openChannelAndBind(IChannel* channel)
{
    if (!channel) return RESULT_INVALID_DATA;
 
    unbindAndClose();
        u_result ans = RESULT_OK;
        do 
        {
                rp::hal::AutoLocker l(_opLocker);
 
        // try to open the channel ...
        Result<nullptr_t> ans = SL_RESULT_OK;
 
        if (!channel->open()) {
            ans= RESULT_OPERATION_FAIL;
            break;
        }
 
 
        // force a flush to clear any pending data
        channel->flush();
 
                _dataEvt.set(false);
 
                _isWorking = true;
        _workingFlag = 0;
        _bindedChannel = channel;
 
 
                _decoderThread = CLASS_THREAD(AsyncTransceiver, _proc_decoderThread);
                _rxThread = CLASS_THREAD(AsyncTransceiver, _proc_rxThread);
 
        
 
 
        } while (0);
 
        return ans;
}
 
void AsyncTransceiver::unbindAndClose()
{
        rp::hal::AutoLocker l(_opLocker);
        if (!_isWorking) return;
 
    assert(_bindedChannel);
 
    
        _isWorking = false;
        _dataEvt.set(); // set signal to wake up threads
 
        _decoderThread.join();
        _rxThread.join();
 
 
    _bindedChannel->close();
 
    _bindedChannel = NULL;
 
 
    for (std::list< Buffer* >::iterator itr = _rxQueue.begin(); itr != _rxQueue.end(); ++itr)
    {
        delete [] *itr;
    }
    _rxQueue.clear();
 
}
 
u_result AsyncTransceiver::sendMessage(message_autoptr_t& msg)
{
    assert(msg);
 
    if (!_isWorking) return RESULT_OPERATION_NOT_SUPPORT;
 
    rp::hal::AutoLocker l(_opLocker);
 
    size_t requiredBufferSize = _codec.estimateLength(msg);
 
    if (requiredBufferSize == 0) {
        // nothing to send
        return RESULT_OK;
    }
 
    u_result ans = RESULT_OK;
 
    _u8* txBuffer = new _u8[requiredBufferSize];
 
    do {
  
        if (!txBuffer) {
            return RESULT_INSUFFICIENT_MEMORY;
        }
 
        _codec.onEncodeData(msg, txBuffer, &requiredBufferSize);
 
        int txSize = _bindedChannel->write(txBuffer, requiredBufferSize);
 
        if (txSize < 0) ans = RESULT_OPERATION_FAIL;
 
    } while (0);
 
 
    delete[] txBuffer;
    return ans;
}
 
sl_result AsyncTransceiver::_proc_rxThread()
{
    assert(_bindedChannel);
 
    rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH);
 
    u_result result;
    size_t hintedSize = 0;
    while (_isWorking)
    {
        result = _bindedChannel->waitForDataExt(hintedSize, 1000);
 
        if (IS_FAIL(result))
        {
            // timeout is allowed
            if (result == RESULT_OPERATION_TIMEOUT) {
                continue;
            }
            if (_isWorking) {
                _workingFlag |= WORKING_FLAG_ERROR;
                _codec.onChannelError(result);
                break;
            }
        }
 
        // no data in buffer, sleep and wait for the next round
        if (!hintedSize)
        {
            continue;
        }
 
 
        Buffer* decodeBuffer = new Buffer();
        
        decodeBuffer->data = new _u8[hintedSize];
 
        decodeBuffer->size = _bindedChannel->read(decodeBuffer->data, hintedSize);
#ifdef _DEBUG_DUMP_PACKET
        printf("Revc: %d\n", decodeBuffer->size);
#endif
         
        if  (!decodeBuffer->size) {
            delete decodeBuffer;
 
            
            _workingFlag |= WORKING_FLAG_ERROR;
            _codec.onChannelError(RESULT_OPERATION_ABORTED);
            break;
        }
 
        assert(hintedSize >= decodeBuffer->size);
 
 
#ifdef _DEBUG_DUMP_PACKET
        printf("=== Dump RX Packet, size = %d ===\n", decodeBuffer->size);
        for (int pos = 0; pos < decodeBuffer->size; pos++)
        {
            printf("%02x ", decodeBuffer->data[pos]);
        }
        printf("\n=== END ===\n");
#endif
 
        _rxLocker.lock();
        _rxQueue.push_back(decodeBuffer);
        _dataEvt.set();
        _rxLocker.unlock();
 
 
    }
    _workingFlag |= WORKING_FLAG_RX_DISABLED;
    return RESULT_OK;
}
 
sl_result AsyncTransceiver::_proc_decoderThread()
{
 
    assert(_bindedChannel);
    rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH);
    _codec.onDecodeReset();
    
 
    while (_isWorking)
    {
        _rxLocker.lock();
 
        if (_rxQueue.empty())
        {
            _rxLocker.unlock();
 
            if (_dataEvt.wait(1000))
                continue;
 
            _rxLocker.lock();
        }
        assert(!_rxQueue.empty());
 
        Buffer * bufferToDecode = _rxQueue.front();
        _rxQueue.pop_front();
 
        _rxLocker.unlock();
 
        //cout<<"decoding "<< bufferToDecode->size <<" bytes of data"<<endl;
        _codec.onDecodeData(bufferToDecode->data, bufferToDecode->size);
 
 
        delete bufferToDecode;
    }
 
    return RESULT_OK;
 
}
 
 
}}