seklyuts/MotorControlModuleSDFM_TMS320F28388D
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 4 Wiki Activity
71263abec9
MotorControlModuleSDFM_TMS3.../Projects/EFC_Application/UMLibrary/driver/netx/NetX.cpp

1120 lines
37 KiB
C++
Raw Normal View History

Модификация проекта EFC_Application с добавлением запуска мотора на CPU2
2024-05-22 09:42:45 +03:00
/*
* NetX.cpp
*
* Created on: 30 <EFBFBD><EFBFBD><EFBFBD><EFBFBD>. 2021 <EFBFBD>.
* Author: titov
*/
#include <algorithm>
#include "../../memories/memories.hh"
#include "lowcifx/HilscherDualPortMemoryMap.hh"
#include "NetX.hh"
namespace {
driver::netx::dpm::NETX_DEFAULT_DPM & df(peripheral::protected_char * pointer ) {
return *reinterpret_cast<driver::netx::dpm::NETX_DEFAULT_DPM *>(pointer);
};
driver::netx::dpm::HIL_DPM_DEFAULT_COMM_CHANNEL & ch(peripheral::protected_char * pointer ) {
return *reinterpret_cast<driver::netx::dpm::HIL_DPM_DEFAULT_COMM_CHANNEL *>(pointer);
};
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL & hs(peripheral::protected_char * pointer ) {
return *reinterpret_cast<driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL *>(pointer);
};
template<typename Pointer, typename Field>
void read_to(const Pointer * dpm_pointer, Field & field) {
while( not
peripheral::read_lock( reinterpret_cast<const peripheral::protected_char *>( dpm_pointer ), sizeof(Field) )
);
std::memcpy( &field, dpm_pointer, sizeof(Field) );
peripheral::read_unlock( reinterpret_cast<const peripheral::protected_char *>( dpm_pointer ), sizeof(Field) );
}
template<typename Pointer, typename Field>
void read_to(const volatile Pointer * dpm_pointer_v, Field & field) {
const Pointer * dpm_pointer = const_cast<const Pointer *>(dpm_pointer_v);
while( not
peripheral::read_lock( reinterpret_cast<const peripheral::protected_char *>( dpm_pointer ), sizeof(Field) )
);
std::memcpy( &field, dpm_pointer, sizeof(Field) );
peripheral::read_unlock( reinterpret_cast<const peripheral::protected_char *>( dpm_pointer ), sizeof(Field) );
}
template<typename Pointer, typename Field>
void write_from(Pointer * dpm_pointer, const Field & field) {
while( not
peripheral::write_lock( reinterpret_cast<peripheral::protected_char *>( dpm_pointer ), sizeof(Field) )
);
std::memcpy( dpm_pointer, &field, sizeof(Field) );
peripheral::write_unlock( reinterpret_cast<peripheral::protected_char *>( dpm_pointer ), sizeof(Field) );
}
template<typename Pointer, typename Field>
void write_from(volatile Pointer * dpm_pointer_v, const volatile Field & field_v) {
Pointer * dpm_pointer = const_cast<Pointer *>(dpm_pointer_v);
const Field & field = const_cast<const Field &>(field_v);
while( not
peripheral::write_lock( reinterpret_cast<peripheral::protected_char *>( dpm_pointer ), sizeof(Field) )
);
std::memcpy( dpm_pointer, &field, sizeof(Field) );
peripheral::write_unlock( reinterpret_cast<peripheral::protected_char *>( dpm_pointer ), sizeof(Field) );
}
}
driver::netx::NetX::NetX( std::pair<const char *, std::size_t> name,
std::pair<peripheral::protected_char *, std::size_t> dual_port_memory )
{
layout.system.channel.first = dual_port_memory.first;
layout.system.channel.second = dpm::HIL_DPM_SYSTEM_CHANNEL_SIZE;
dpm::HIL_DPM_SYSTEM_INFO_BLOCK sys_info_block;
read_to(&df(dual_port_memory.first).SystemChannel.tSystemInfo, sys_info_block);
device_info.dip_address = sys_info_block.bDevIdNumber;
device_info.device_class = sys_info_block.usDeviceClass;
device_info.device_number = sys_info_block.ulDeviceNumber;
device_info.serial_number = sys_info_block.ulSerialNumber;
dpm.base = dual_port_memory.first;
dpm.size = std::size_t(sys_info_block.ulDpmTotalSize * 8) / CHAR_BIT;
if( hasDefaultDpmLayout() ) {
layout.system.channel.first =
reinterpret_cast<peripheral::protected_char *>(&df(dpm.base).SystemChannel);
layout.system.channel.second =
sizeof(df(dpm.base).SystemChannel);
layout.system.handshake.first =
reinterpret_cast<peripheral::protected_char *>(&df(dpm.base).HandshakeChannel.tSysFlags);
layout.system.handshake.second =
sizeof(df(dpm.base).HandshakeChannel.tSysFlags);
//todo: implement mailboxes
layout.handshake.channel.first =
reinterpret_cast<peripheral::protected_char *>(&df(dpm.base).HandshakeChannel);
layout.handshake.channel.second =
sizeof(df(dpm.base).HandshakeChannel);
for( std::size_t id = 0; id < NETX_COMM_CHANNELS_NUM; ++id ) {
layout.communications[id].channel.first =
reinterpret_cast<peripheral::protected_char *>(&df(dpm.base).CommChannel[id]);
layout.communications[id].channel.second =
sizeof(df(dpm.base).CommChannel[id]);
layout.communications[id].handshake.first =
reinterpret_cast<peripheral::protected_char *>(&df(dpm.base).HandshakeChannel.tCommFlags0 + id);
layout.communications[id].handshake.second =
sizeof(df(dpm.base).HandshakeChannel.tCommFlags0);
read_to(&df(dpm.base).SystemChannel.atChannelInfo[id + 2].tCom.usCommunicationClass,
layout.communications[id].type );
read_to(&df(dpm.base).SystemChannel.atChannelInfo[id + 2].tCom.usProtocolClass,
layout.communications[id].protocol );
read_to(&df(dpm.base).SystemChannel.atChannelInfo[id + 2].tCom.usProtocolConformanceClass,
layout.communications[id].conformance );
layout.communications[id].cyclic_in.first =
reinterpret_cast<peripheral::protected_char *>(&df(dpm.base).CommChannel[id].abPd0Input);
layout.communications[id].cyclic_in.second =
sizeof(df(dpm.base).CommChannel[id].abPd0Input);
layout.communications[id].cyclic_out.first =
reinterpret_cast<peripheral::protected_char *>(&df(dpm.base).CommChannel[id].abPd0Output);
layout.communications[id].cyclic_out.second =
sizeof(df(dpm.base).CommChannel[id].abPd0Output);
layout.communications[id].mailbox_receive.first =
reinterpret_cast<peripheral::protected_char *>(&df(dpm.base).CommChannel[id].tRecvMbx.abRecvMailbox);
layout.communications[id].mailbox_receive.second =
sizeof(df(dpm.base).CommChannel[id].tRecvMbx.abRecvMailbox);
layout.communications[id].mailbox_send.first =
reinterpret_cast<peripheral::protected_char *>(&df(dpm.base).CommChannel[id].tSendMbx.abSendMailbox);
layout.communications[id].mailbox_send.second =
sizeof(df(dpm.base).CommChannel[id].tSendMbx.abSendMailbox);
}
for( std::size_t id = 0; id < NETX_APP_CHANNELS_NUM; ++id ) {
layout.applications[id].channel.first = nullptr;
layout.applications[id].channel.second = 0;
layout.applications[id].handshake.first = nullptr;
layout.applications[id].handshake.second = 0;
}
} else for( std::size_t channel_id = 0; channel_id < dpm::HIL_DPM_MAX_SUPPORTED_CHANNELS; channel_id++ ) {
dpm::HIL_DPM_CHANNEL_INFO_BLOCK info_blocks;
read_to(&df(dpm.base).SystemChannel.atChannelInfo[channel_id], info_blocks );
}
//Check for avalaible handshake block
dpm::HIL_DPM_COMMUNICATION_CHANNEL_INFO::ChannelInfo info;
read_to(&df(dpm.base).SystemChannel.atChannelInfo[0].tCom.uChannelInfo.field, info );
// info.bits.bChannelType != dpm::HIL_CHANNEL_TYPE_HANDSHAKE;
}
bool driver::netx::NetX::isReady( peripheral::protected_char * dpm_origin ) {
/* Check of DPM is initialized */
if (!checkDpmCookie(dpm_origin)) {
return false;
}
/* Check if system channel is ready */
dpm::HIL_DPM_HANDSHAKE_CELL sys_hs_cell;
std::memset(&sys_hs_cell, 0, sizeof(sys_hs_cell));
read_to(&df(dpm_origin).HandshakeChannel.tSysFlags, sys_hs_cell);
return (sys_hs_cell.t8Bit.bNetxFlags & dpm::NSF_READY) != 0;
}
bool driver::netx::NetX::isReady() const {
const bool new_ready_status =
checkDpmCookie( dpm.base ) && isChannelReady( Channel::System );
notifyChannels(new_ready_status);
ready_status = new_ready_status;
return ready_status;
}
void driver::netx::NetX::notifyChannels( bool new_ready_status ) const {
if (ready_status != new_ready_status) {
for (std::size_t i = 0; i < NETX_COMM_CHANNELS_NUM; i++) {
if (communication_channels[i] != nullptr) {
communication_channels[i]->setBoardReadyStatus( new_ready_status );
}
}
}
}
bool driver::netx::NetX::checkDpmCookie( peripheral::protected_char * dpm_origin ) {
uint32_t dpm_cookie;
read_to( dpm_origin, dpm_cookie );
return dpm_cookie == NETX_DPM_COOKIE;
}
bool driver::netx::NetX::isChannelReady(driver::netx::NetX::Channel::Id channel) const {
if (channel == Channel::System) {
/* Check system channel status */
dpm::HIL_DPM_HANDSHAKE_CELL status_flags;
memset(&status_flags, 0, sizeof(status_flags));
read_to( Channel::handshake(dpm.base, channel), status_flags);
return (status_flags.t8Bit.bNetxFlags & dpm::NSF_READY);
}
const bool is_comm_channel =
channel == Channel::CommunicationAlfa ||
channel == Channel::CommunicationBeta ||
channel == Channel::CommunicationGamma ||
channel == Channel::CommunicationDelta;
if (!is_comm_channel) {
/* ERROR: invalid channel ID */
while(true);
}
/* Check communication channel status */
uint32_t status_flags = 0;
int id = channel - Channel::CommunicationAlfa;
read_to(
&df(dpm.base).CommChannel[id].tCommonStatus.ulCommunicationCOS,
status_flags
);
return (status_flags & dpm::HIL_COMM_COS_READY);
}
bool driver::netx::NetX::isChannelRunning(driver::netx::NetX::Channel::Id channel) const {
const bool is_comm_channel =
channel == Channel::CommunicationAlfa ||
channel == Channel::CommunicationBeta ||
channel == Channel::CommunicationGamma ||
channel == Channel::CommunicationDelta;
if (!is_comm_channel) {
/* ERROR: invalid channel ID: only communication channels allowed */
while(true);
}
/* Check communication channel status */
uint32_t status_flags = 0;
int id = channel - Channel::CommunicationAlfa;
read_to(
&df(dpm.base).CommChannel[id].tCommonStatus.ulCommunicationCOS,
status_flags
);
return (status_flags & dpm::HIL_COMM_COS_RUN);
}
bool driver::netx::NetX::isChannelCommunicating(driver::netx::NetX::Channel::Id channel) const {
const bool is_comm_channel =
channel == Channel::CommunicationAlfa ||
channel == Channel::CommunicationBeta ||
channel == Channel::CommunicationGamma ||
channel == Channel::CommunicationDelta;
if (!is_comm_channel) {
/* ERROR: invalid channel ID: only communication channels allowed */
while(true);
}
/* Check communication channel status */
uint32_t status_flags = 0;
int id = channel - Channel::CommunicationAlfa;
read_to(
&df(dpm.base).CommChannel[id].tCommonStatus.ulCommunicationCOS,
status_flags
);
return (status_flags & dpm::HIL_COMM_COS_BUS_ON);
}
bool driver::netx::NetX::isError() const {
std::pair<bool, uint32_t> system_error = checkChannelError(Channel::System);
errors[Channel::System] = system_error.second;
return system_error.first;
}
std::pair<bool, uint32_t> driver::netx::NetX::checkChannelError(driver::netx::NetX::Channel::Id channel) const {
switch (channel) {
case Channel::System: {
//todo: Check handshake size.
//For system channel this 8 bit.
dpm::HIL_DPM_HANDSHAKE_CELL flags;
read_to( Channel::handshake(dpm.base, channel), flags);
if( flags.t8Bit.bNetxFlags & dpm::NSF_ERROR ) {
ErrorCode code;
read_to(&df(dpm.base).SystemChannel.tSystemState.ulSystemError, code);
return std::pair<bool, uint32_t>(true, code);
} else
return std::pair<bool, uint32_t>(false, 0);
} break;
case Channel::CommunicationAlfa:
case Channel::CommunicationBeta:
case Channel::CommunicationDelta:
case Channel::CommunicationGamma: {
dpm::HIL_DPM_HANDSHAKE_CELL flags;
read_to( Channel::handshake(dpm.base, channel), flags);
if( flags.t16Bit.usNetxFlags & dpm::NCF_ERROR ) {
ErrorCode code;
read_to( &df(dpm.base).CommChannel[channel-Channel::CommunicationAlfa].tCommonStatus.ulCommunicationError,
code);
return std::pair<bool, uint32_t>(true, code);
} else
return std::pair<bool, uint32_t>(false, 0);
} break;
default: {
//todo: Add exception.
while(true);
} break;
}
return std::pair<bool, uint32_t>();
}
void driver::netx::NetX::reset() {
resetSystem();
}
void driver::netx::NetX::resetSystem() {
if( isChannelReady(Channel::System) ) {
//Write Reset Cookie
write_from( &df(dpm.base).SystemChannel.tSystemControl.ulSystemCommandCOS, dpm::HIL_SYS_RESET_COOKIE);
//Set HSF_RESET Bit
dpm::HIL_DPM_HANDSHAKE_CELL flags;
read_to( Channel::handshake(dpm.base, Channel::System), flags);
flags.t8Bit.bHostFlags |= dpm::HSF_RESET;
write_from( Channel::handshake(dpm.base, Channel::System), flags);
}
}
driver::netx::NetX::CommunicationChannel *
driver::netx::NetX::open(driver::netx::NetX::CommunicationChannel::ChannelId id,
std::pmr::memory_resource * allocator,
std::pmr::memory_resource * description, std::pmr::memory_resource * buffer ) {
if( communication_channels[id] ) {
return nullptr;
}
communication_channels[id] = memories::instance_object<CommunicationChannel>(*allocator, description, buffer);
communication_channels[id]->channel = layout.communications[id].channel.first;
communication_channels[id]->handshake = layout.communications[id].handshake.first;
communication_channels[id]->self_info.id = id;
communication_channels[id]->self_info.type = layout.communications[id].type;
communication_channels[id]->self_info.protocol = layout.communications[id].protocol;
communication_channels[id]->self_info.conformance = layout.communications[id].conformance;
communication_channels[id]->pd0_data.input = layout.communications[id].cyclic_in.first;
communication_channels[id]->pd0_data.output = layout.communications[id].cyclic_out.first;
communication_channels[id]->pd0_data.capacity = layout.communications[id].cyclic_in.second;
communication_channels[id]->pd0_data.handshake = layout.communications[id].handshake.first;
communication_channels[id]->mb_rec_data.input = layout.communications[id].mailbox_receive.first;
communication_channels[id]->mb_rec_data.output = nullptr;
communication_channels[id]->mb_rec_data.capacity = layout.communications[id].mailbox_receive.second;
communication_channels[id]->mb_rec_data.handshake = layout.communications[id].handshake.first;
communication_channels[id]->mb_tx_data.input = nullptr;
communication_channels[id]->mb_tx_data.output = layout.communications[id].mailbox_send.first;
communication_channels[id]->mb_tx_data.capacity = layout.communications[id].mailbox_send.second;
communication_channels[id]->mb_tx_data.handshake = layout.communications[id].handshake.first;
communication_channels[id]->setBoardReadyStatus(ready_status);
return communication_channels[id];
}
bool driver::netx::NetX::hasDefaultDpmLayout() const {
uint32_t ulSystemCOS;
read_to( &df(dpm.base).SystemChannel.tSystemState.ulSystemCOS, ulSystemCOS );
return ulSystemCOS & dpm::HIL_SYS_COS_DEFAULT_MEMORY;
}
peripheral::protected_char *
driver::netx::NetX::Channel::handshake(peripheral::protected_char * base, driver::netx::NetX::Channel::Id id) {
switch (id) {
case System:
return reinterpret_cast<peripheral::protected_char *>(
&reinterpret_cast<dpm::NETX_DEFAULT_DPM *>(base)->HandshakeChannel.tSysFlags
);
case CommunicationAlfa:
case CommunicationBeta:
case CommunicationDelta:
case CommunicationGamma:
return reinterpret_cast<peripheral::protected_char *>(
&(&reinterpret_cast<dpm::NETX_DEFAULT_DPM *>(base)->HandshakeChannel.tCommFlags0)[id-CommunicationAlfa]
);
}
return nullptr;
}
bool driver::netx::NetX::CommunicationChannel::isRunning() const {
if (!board_ready_status) {
cos_cache.second = false;
return false;
}
uint32_t cos_flags = readNetXCos();
return (cos_flags & dpm::HIL_COMM_COS_READY) && (cos_flags & dpm::HIL_COMM_COS_RUN);
}
bool driver::netx::NetX::CommunicationChannel::isCommunicating() const {
if (!board_ready_status) {
cos_cache.second = false;
return false;
}
uint16_t ncf_flags = 0;
read_to(&hs(handshake).t16Bit.usNetxFlags, ncf_flags);
uint32_t cos_flags = readNetXCos();
return (cos_flags & dpm::HIL_COMM_COS_READY)
&& (cos_flags & dpm::HIL_COMM_COS_RUN)
&& (ncf_flags & dpm::NCF_COMMUNICATING);
}
bool driver::netx::NetX::CommunicationChannel::isError() const {
if (!board_ready_status) {
cos_cache.second = false;
return false;
}
uint16_t ncf_flags = 0;
read_to(&hs(handshake).t16Bit.usNetxFlags, ncf_flags);
return (ncf_flags & dpm::NCF_ERROR);
}
void driver::netx::NetX::CommunicationChannel::start() {
updateAppCos( dpm::HIL_APP_COS_BUS_ON | dpm::HIL_APP_COS_BUS_ON_ENABLE );
}
void driver::netx::NetX::CommunicationChannel::stop() {
updateAppCos( dpm::HIL_APP_COS_BUS_ON_ENABLE, dpm::HIL_APP_COS_BUS_ON );
}
uint32_t driver::netx::NetX::CommunicationChannel::readNetXCos() const {
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
read_to(&hs(handshake), cell);
const bool cos_cache_initialized = cos_cache.second;
const bool cos_cache_invalid =
(cell.t16Bit.usHostFlags ^ cell.t16Bit.usNetxFlags) & dpm::NCF_NETX_COS_CMD;
if(cos_cache_initialized || cos_cache_invalid) {
read_to(&ch(channel).tCommonStatus.ulCommunicationCOS, cos_cache);
cell.t16Bit.usHostFlags ^= dpm::HCF_NETX_COS_ACK;
write_from(&hs(handshake).t16Bit.usHostFlags, cell.t16Bit.usHostFlags);
cos_cache.second = true;
}
return cos_cache.first;
}
void driver::netx::NetX::CommunicationChannel::updateAppCos(uint32_t set, uint32_t clear) {
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
read_to(&hs(handshake), cell);
if( ( cell.t16Bit.usHostFlags ^ cell.t16Bit.usNetxFlags ) & dpm::HCF_HOST_COS_CMD ) {
uint32_t ulApplicationCOS;
read_to( &ch(channel).tControl.ulApplicationCOS, ulApplicationCOS );
ulApplicationCOS &= ~clear;
ulApplicationCOS |= set;
write_from( &ch(channel).tControl.ulApplicationCOS, ulApplicationCOS );
cell.t16Bit.usHostFlags ^= dpm::HCF_HOST_COS_CMD;
write_from( &hs(handshake).t16Bit.usHostFlags, cell.t16Bit.usHostFlags );
} else ; //todo: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
}
driver::netx::NetX::CommunicationChannel::CommunicationChannel( std::pmr::memory_resource * description, std::pmr::memory_resource * buffer ) :
pd0_data( dpm::HCF_PD0_IN_ACK, dpm::HCF_PD0_OUT_CMD ),
pd1_data( dpm::HCF_PD1_IN_ACK, dpm::HCF_PD1_OUT_CMD ),
mb_rec_data( dpm::HCF_RECV_MBX_ACK, dpm::HCF_RECV_MBX_ACK ),
mb_tx_data( dpm::HCF_SEND_MBX_CMD, dpm::HCF_SEND_MBX_CMD ),
mailbox_handler(mb_rec_data, mb_tx_data, description, buffer),
cos_cache(0, false),
board_ready_status(false) {}
driver::netx::NetX::CommunicationChannel::Info driver::netx::NetX::CommunicationChannel::info() const {
return self_info;
}
driver::netx::NetX::ErrorCode driver::netx::NetX::CommunicationChannel::getErrorCode() const {
uint32_t ulCommunicationError;
read_to( &ch(channel).tCommonStatus.ulCommunicationError, ulCommunicationError );
return ulCommunicationError;
}
float driver::netx::NetX::CommunicationChannel::getErrorCounter() const {
uint32_t ulCommunicationError;
read_to( &ch(channel).tCommonStatus.ulCommunicationError, ulCommunicationError );
return ulCommunicationError;
}
peripheral::IMemoryAccess & driver::netx::NetX::CommunicationChannel::cyclic() {
return pd0_data;
}
float driver::netx::NetX::CommunicationChannel::getWatchdogCounter() const {
uint32_t ulHostWatchdog;
read_to( &ch(channel).tCommonStatus.ulHostWatchdog, ulHostWatchdog);
return ulHostWatchdog;
}
void driver::netx::NetX::CommunicationChannel::watchdog() {
uint32_t ulHostWatchdog;
read_to( &ch(channel).tCommonStatus.ulHostWatchdog, ulHostWatchdog);
write_from( &ch(channel).tControl.ulDeviceWatchdog, ulHostWatchdog );
}
driver::netx::NetX::CommunicationChannel::Mailbox & driver::netx::NetX::CommunicationChannel::mailbox() {
return mailbox_handler;
}
bool driver::netx::NetX::CommunicationChannel::isProtocolRegistred() const {
return mailbox_handler.enable();
}
void driver::netx::NetX::CommunicationChannel::setBoardReadyStatus( bool ready_status ) {
board_ready_status = ready_status;
}
namespace {
class Lock {
peripheral::protected_char * ptr;
std::size_t sz;
bool locked;
void (* unlock)(peripheral::protected_char *,std::size_t);
public:
Lock(peripheral::protected_char * p, std::size_t s,
bool (* lock)(peripheral::protected_char *, std::size_t),
void (* unlock_function)(peripheral::protected_char *, std::size_t)) :
ptr(p), sz(s), unlock(unlock_function) {
locked = lock(ptr, sz);
}
operator bool() const { return locked; }
~Lock() {
if(locked and unlock)
unlock(ptr, sz);
}
};
struct WriteLock : public Lock {
WriteLock(peripheral::protected_char * p, std::size_t s) :
Lock(p, s, peripheral::write_lock, peripheral::write_unlock) {}
};
struct ReadLock : public Lock {
ReadLock(peripheral::protected_char * p, std::size_t s) :
Lock(p, s, peripheral::write_lock, peripheral::write_unlock) {}
};
}
bool driver::netx::NetX::CommunicationChannel::HandshakeData::read( char * data, std::size_t begin, std::size_t size ) {
if( begin + size > capacity )
return read_complete = false;
// Lock communication channel handshake memory area
WriteLock hs_lock(handshake, sizeof(driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL));
ReadLock data_lock(input + begin, size);
if( not( hs_lock and data_lock ) )
return read_complete = false;
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
std::memcpy( &cell, &hs(handshake), sizeof(cell) );
// Check if communication channel is up and running
if ( ( cell.t16Bit.usNetxFlags & dpm::NCF_COMMUNICATING) == 0 ) {
return read_complete = false;
}
// Check if host has access rights to the DPM area
if( ( ( cell.t16Bit.usHostFlags ^ cell.t16Bit.usNetxFlags ) & dpm::HCF_PD0_IN_ACK ) == 0) {
// Read data from channel cyclic input area to the buffer
std::memcpy( data, input + begin, size );
// Acknowledge device by toggling handshake flag
cell.t16Bit.usHostFlags ^= read_mask;//dpm::HCF_PD0_IN_ACK;
std::memcpy((void *) &hs(handshake).t16Bit.usHostFlags,
(const void *) &cell.t16Bit.usHostFlags,
sizeof(cell.t16Bit.usHostFlags) );
return read_complete = true;
} else {
return read_complete = false;
}
}
bool driver::netx::NetX::CommunicationChannel::HandshakeData::write(const char * data, std::size_t begin, std::size_t size) {
if( begin + size > capacity )
return write_request = false;
// Lock communication channel handshake memory area
WriteLock hs_lock(handshake, sizeof(driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL));
WriteLock data_lock(output + begin, size);
if( not( hs_lock and data_lock ) )
return write_request = false;
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
std::memcpy( &cell, &hs(handshake), sizeof(cell) );
// Check if communication channel is up and running
if ( ( cell.t16Bit.usNetxFlags & dpm::NCF_COMMUNICATING ) == 0 ) {
return write_request = false;
}
// Check if host has access rights to the DPM area
if( ( ( cell.t16Bit.usHostFlags ^ cell.t16Bit.usNetxFlags ) & write_mask ) == 0 ) {
// Write data to the channel cyclic output area
std::memcpy( output + begin, data, size );
// Acknowledge device by toggling handshake flag
cell.t16Bit.usHostFlags ^= write_mask;//dpm::HCF_PD0_OUT_CMD;
std::memcpy((void *) &hs(handshake).t16Bit.usHostFlags,
(const void *) &cell.t16Bit.usHostFlags,
sizeof(cell.t16Bit.usHostFlags) );
return write_request = true;
} else {
return write_request = false;
}
}
bool driver::netx::NetX::CommunicationChannel::HandshakeData::isReadComplete() const {
return read_complete;
}
bool driver::netx::NetX::CommunicationChannel::HandshakeData::isWriteComplete() const {
if( not write_request )
return false;
ReadLock hs_lock(handshake, sizeof(driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL));
if( not hs_lock )
return false;
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
std::memcpy( &cell, &hs(handshake), sizeof(cell) );
return not( ( cell.t16Bit.usHostFlags ^ cell.t16Bit.usNetxFlags ) & write_mask ); //dpm::HCF_PD0_OUT_CMD );
}
std::size_t driver::netx::NetX::CommunicationChannel::HandshakeData::getCapacity() const {
return capacity;
}
driver::netx::NetX::CommunicationChannel::HandshakeData::HandshakeData(uint16_t rmask, uint16_t wmask) :
read_mask(rmask), write_mask(wmask) {}
bool driver::netx::NetX::CommunicationChannel::HandshakeData::view( char * data, std::size_t begin, std::size_t size ) {
if( begin + size > capacity )
return false;
ReadLock data_lock(input + begin, size);
if( not data_lock )
return false;
std::memcpy( data, input + begin, size );
return true;
}
bool driver::netx::NetX::CommunicationChannel::HandshakeData::accept() {
WriteLock hs_lock( handshake, sizeof(driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL) );
if( not hs_lock )
return false;
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
std::memcpy( &cell, &hs(handshake), sizeof(cell) );
cell.t16Bit.usHostFlags ^= read_mask;//dpm::HCF_PD0_IN_ACK;
std::memcpy((void *) &hs(handshake).t16Bit.usHostFlags,
(const void *) &cell.t16Bit.usHostFlags,
sizeof(cell.t16Bit.usHostFlags) );
return true;
}
bool driver::netx::NetX::CommunicationChannel::HandshakeData::ready_to_be_read() {
ReadLock hs_lock(handshake, sizeof(driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL));
if( not hs_lock )
return false;
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
std::memcpy( &cell, &hs(handshake), sizeof(cell) );
//Check state handshake
return ( cell.t16Bit.usHostFlags ^ cell.t16Bit.usNetxFlags ) & read_mask;
}
bool
driver::netx::NetX::CommunicationChannel::HandshakeData::set(const char * data, std::size_t begin, std::size_t size) {
if( begin + size > capacity )
return false;
WriteLock data_lock(output + begin, size);
if( not( data_lock ) )
return false;
//Write data to buffer
std::memcpy( output + begin, data, size );
return true;
}
bool driver::netx::NetX::CommunicationChannel::HandshakeData::commit() {
//Lock memory
WriteLock hs_lock(handshake, sizeof(driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL));
if( not( hs_lock ) )
return false;
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
std::memcpy( &cell, &hs(handshake), sizeof(cell) );
//Update host flag
cell.t16Bit.usHostFlags ^= write_mask;
std::memcpy((void *) &hs(handshake).t16Bit.usHostFlags,
(const void *) &cell.t16Bit.usHostFlags,
sizeof(cell.t16Bit.usHostFlags) );
return true;
}
bool driver::netx::NetX::CommunicationChannel::HandshakeData::ready_to_be_written() {
//Lock memory
ReadLock hs_lock(handshake, sizeof(driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL));
if( not( hs_lock ) )
return false;
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
std::memcpy( &cell, &hs(handshake), sizeof(cell) );
//Check state handshake
return not( ( cell.t16Bit.usHostFlags ^ cell.t16Bit.usNetxFlags ) & write_mask );
}
void driver::netx::NetX::CommunicationChannel::MailboxHandler::process() {
switch(state) {
//Registration phase:
case RequestRegisterApplication: {
packet::HIL_PACKET_HEADER header;
header.ulDest = packet::HIL_PACKET_DEST_DEFAULT_CHANNEL;
header.ulSrc = reinterpret_cast<uintptr_t>(this) & static_cast<uint32_t>(-1);
header.ulCmd = packet::HIL_REGISTER_APP_REQ;
if( transmit.set( reinterpret_cast<char *>(&header), 0, sizeof(packet::HIL_PACKET_HEADER)) )
state = WaitRegisterApplication;
else break;
}
case WaitRegisterApplication: {
if( receive.ready_to_be_read() )
state = ConfirmationRegisterApplication;
else break;
}
case ConfirmationRegisterApplication: {
packet::HIL_PACKET_HEADER header;
if( receive.view(reinterpret_cast<char *>(&header), 0, sizeof(packet::HIL_PACKET_HEADER)) ) {
application_registred = not(header.ulSta);
state = AcceptRegisterApplication;
} else break;
}
case AcceptRegisterApplication: {
if( receive.accept() )
state = WaitReceive;
else break;
}
//Work phase:
case WaitReceive: {
if( receive.ready_to_be_read() )
state = ReadHeader;
else break;
} case ReadHeader: {
if( receive.view(reinterpret_cast<char *>(&request.header), 0, sizeof(packet::HIL_PACKET_HEADER)) ) {
//todo: handle odd size
std::size_t args_size = request.header.ulLen * 8 / CHAR_BIT;
request.data.resize( args_size );
state = ReadData;
} else break;
} case ReadData: {
if( receive.view( &request.data.front(), sizeof(packet::HIL_PACKET_HEADER), request.data.size() ) )
state = AcceptReceive;
else break;
} case AcceptReceive: {
if( receive.accept() )
state = RequestServer;
else break;
} case RequestServer: {
ServerId server_id = request.header.ulCmd;
Servers::iterator server = servers.find(server_id);
if( server != servers.end() ) {
Client client(response.data);
if( server->second->request(&client, &request.data.front(), request.data.size()) ) {
//todo: handle errors.
if( client.flag ) {
//Answer by application
response.header = request.header;
response.header.ulCmd = request.header.ulCmd + 1;
response.header.ulSta = 0;
response.header.ulLen = response.data.size() * CHAR_BIT / 8;
} else {
//Default answer by stack
response.header = request.header;
response.header.ulCmd = request.header.ulCmd + 1;
response.header.ulSta = packet::ERR_HIL_NO_APPLICATION_REGISTERED;
response.data = request.data;
}
state = WaitSend;
} else {
//Format fault
state = Stop;
} break;
} else {
//Unknown server
state = Stop;
} break;
} case WaitSend: {
if( transmit.ready_to_be_written() )
state = WriteHeader;
else break;
} case WriteHeader: {
if( transmit.set( reinterpret_cast<char *>(&response.header), 0, sizeof(packet::HIL_PACKET_HEADER)) )
state = WriteData;
else break;
} case WriteData: {
if( transmit.set( &response.data.front(), sizeof(packet::HIL_PACKET_HEADER), response.data.size() ) )
state = AcceptSend;
else break;
} case AcceptSend: {
if( transmit.commit() )
state = WaitReceive;
else break;
}
case Stop: {} break;
}
}
driver::netx::NetX::CommunicationChannel::MailboxHandler::MailboxHandler(
driver::netx::NetX::CommunicationChannel::HandshakeData & receive,
driver::netx::NetX::CommunicationChannel::HandshakeData & transmit, std::pmr::memory_resource * description,
std::pmr::memory_resource * buffer) :
receive(receive), transmit(transmit),
servers( std::greater<ServerId>(), description ), response(buffer), request(buffer),
state(Stop) {}
void driver::netx::NetX::CommunicationChannel::MailboxHandler::attach(communication::IMessageServer * server,
driver::netx::NetX::ServerId id ) {
servers[id] = server;
}
void driver::netx::NetX::CommunicationChannel::MailboxHandler::start() {
if( state == Stop )
state = WaitReceive;
}
bool driver::netx::NetX::CommunicationChannel::MailboxHandler::stop() {
if( state == WaitReceive )
state = Stop;
return state == Stop;
}
const bool &driver::netx::NetX::CommunicationChannel::MailboxHandler::enable() const {
return application_registred;
}
driver::netx::NetX::CommunicationChannel::MailboxHandler::Client::Client(
std::vector< char, std::pmr::polymorphic_allocator<char> > & buff ) : data(buff) {
data.clear();
}
bool driver::netx::NetX::CommunicationChannel::MailboxHandler::Client::response( const char * buff, std::size_t size ) {
data.insert( data.begin(), buff, buff + size );
flag = true;
return true;
}
driver::netx::NetX::CommunicationChannel::MailboxHandler::Packet::Packet( std::pmr::memory_resource * buffer ) :
data(buffer) {}
communication::IBinaryDataReadEvent & driver::netx::NetX::CommunicationChannel::cyclic_read() {
return pd0_data;
}
communication::IBinaryDataWriteEvent & driver::netx::NetX::CommunicationChannel::cyclic_write() {
return pd0_data;
}
void driver::netx::NetX::CommunicationChannel::HandshakeData::read_event(
communication::IBinaryDataSubscriber & subscriber ) {
// Lock communication channel handshake memory area
WriteLock hs_lock(handshake, sizeof(driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL));
// ReadLock data_lock(input, capacity);
if( not( hs_lock ) )
//and data_lock ) )
return;
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
std::memcpy( &cell, &hs(handshake), sizeof(cell) );
// // Check if communication channel is up and running
// if ( ( cell.t16Bit.usNetxFlags & dpm::NCF_COMMUNICATING ) == 0 )
// return;
// Check if host has access rights to the DPM area
if( ( ( cell.t16Bit.usHostFlags ^ cell.t16Bit.usNetxFlags ) & dpm::HCF_PD0_IN_ACK ) == 0 ) {
// Read data from channel cyclic input area to the buffer
subscriber.read(input, capacity);
// Acknowledge device by toggling handshake flag
cell.t16Bit.usHostFlags ^= read_mask;//dpm::HCF_PD0_IN_ACK;
std::memcpy((void *) &hs(handshake).t16Bit.usHostFlags,
(const void *) &cell.t16Bit.usHostFlags,
sizeof(cell.t16Bit.usHostFlags) );
}
}
void driver::netx::NetX::CommunicationChannel::HandshakeData::write_event(
communication::IBinaryDataIssuer & issuer ) {
// Lock communication channel handshake memory area
WriteLock hs_lock(handshake, sizeof(driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL));
// WriteLock data_lock(output, capacity);
if( not( hs_lock ) )
//and data_lock ) )
return;
driver::netx::dpm::HIL_DPM_HANDSHAKE_CELL cell;
std::memcpy( &cell, &hs(handshake), sizeof(cell) );
// // Check if communication channel is up and running
// if ( ( cell.t16Bit.usNetxFlags & dpm::NCF_COMMUNICATING ) == 0 )
// return;
// Check if host has access rights to the DPM area
if( ( ( cell.t16Bit.usHostFlags ^ cell.t16Bit.usNetxFlags ) & write_mask ) == 0 ) {
// Write data to the channel cyclic output area
issuer.write(output, capacity);
// Acknowledge device by toggling handshake flag
cell.t16Bit.usHostFlags ^= write_mask;//dpm::HCF_PD0_OUT_CMD;
std::memcpy((void *) &hs(handshake).t16Bit.usHostFlags,
(const void *) &cell.t16Bit.usHostFlags,
sizeof(cell.t16Bit.usHostFlags) );
}
}
Reference in New Issue Copy Permalink