519 lines
13 KiB
C++
519 lines
13 KiB
C++
/*
|
||
* ParamStaticStorage.cpp
|
||
*
|
||
* Created on: 24 нояб. 2018 г.
|
||
* Author: titov
|
||
*/
|
||
|
||
#include "ParamStaticStorage.hh"
|
||
#include "../common/Crc.hh"
|
||
|
||
#include <cstring>
|
||
|
||
driver::detail::ParamStaticStorage::ParamStaticStorage(
|
||
peripheral::IMemoryAccess & memory, char * buff, size_t buff_size, std::pmr::memory_resource * task_buffer ) :
|
||
memory(memory), tasks(task_buffer),
|
||
buff(buff), buff_size(buff_size), //TODO: Взять минимум между тем что есть в памяти и тем какой буфер передали.
|
||
// proc_ctrl( &systemic::getProcDummy() ),
|
||
image(), state(State::Idle),
|
||
data_ready(false), crc_error(false) {}
|
||
|
||
systemic::SharedData driver::detail::ParamStaticStorage::getParameterBuff( systemic::IParameterProvider::Id parameter_id ) const {
|
||
|
||
if( isMemoryProcessing() )
|
||
return { nullptr, 0 };
|
||
|
||
std::size_t parameter_size = image.show_setting_size( parameter_id );
|
||
|
||
char * temporary_buff = image.extract_setting_data_buff( parameter_id, parameter_size );
|
||
|
||
return { temporary_buff, parameter_size };
|
||
|
||
}
|
||
|
||
bool driver::detail::ParamStaticStorage::flushParameterBuff( systemic::IParameterProvider::Id parameter_id ) {
|
||
|
||
const char * setting = image.show_setting_data_buff(parameter_id);
|
||
|
||
if( setting ) {
|
||
|
||
tasks.push_back( parameter_id );
|
||
|
||
data_ready = false;
|
||
|
||
}
|
||
|
||
return setting;
|
||
|
||
}
|
||
|
||
void driver::detail::ParamStaticStorage::flush() {
|
||
|
||
if( state == State::Idle ) {
|
||
|
||
state = State::StartWrite;
|
||
|
||
enableProcess();
|
||
|
||
}
|
||
|
||
}
|
||
|
||
void driver::detail::ParamStaticStorage::load() {
|
||
|
||
if( state == State::Idle ) {
|
||
|
||
data_ready = false;
|
||
|
||
state = State::StartReadHead;
|
||
|
||
enableProcess();
|
||
|
||
}
|
||
|
||
}
|
||
|
||
systemic::SharedData driver::detail::ParamStaticStorage::createParameterBuff( Id parameter_id, std::size_t parameter_size ) {
|
||
|
||
if( isMemoryProcessing() )
|
||
return { nullptr, 0 };
|
||
|
||
char * temporary_buff = image.extract_setting_data_buff( parameter_id, parameter_size );
|
||
|
||
if( not temporary_buff ) {
|
||
|
||
bool setting_info_update = image.change_setting_info( parameter_id, parameter_size );
|
||
|
||
if( setting_info_update )
|
||
|
||
temporary_buff = image.extract_setting_data_buff( parameter_id, parameter_size );
|
||
|
||
}
|
||
|
||
if( temporary_buff )
|
||
data_ready = false;
|
||
|
||
return { temporary_buff, parameter_size };
|
||
|
||
}
|
||
|
||
bool driver::detail::ParamStaticStorage::isDataSync() const {
|
||
|
||
return data_ready;
|
||
|
||
}
|
||
|
||
bool driver::detail::ParamStaticStorage::isCrcError() const {
|
||
|
||
return crc_error;
|
||
|
||
}
|
||
|
||
void driver::detail::ParamStaticStorage::init( float ts ) {}
|
||
|
||
std::pair<const char *, std::size_t> driver::detail::ParamStaticStorage::get4read( Id parameter_id ) const {
|
||
|
||
if( isMemoryProcessing() )
|
||
return { nullptr, 0 };
|
||
|
||
const char * data_buff = image.show_setting_data_buff( parameter_id );
|
||
unsigned short size = image.show_setting_size( parameter_id );
|
||
|
||
return std::make_pair( data_buff, size );
|
||
|
||
}
|
||
|
||
bool driver::detail::ParamStaticStorage::isMemoryProcessing() const {
|
||
|
||
return state != State::Idle;
|
||
|
||
}
|
||
|
||
bool driver::detail::ParamStaticStorage::check_crc() {
|
||
|
||
const size_t image_size = image.get_image_size();
|
||
|
||
Crc<16, 0x1021, 0xFFFF, false, false, 0> crc_checker;
|
||
char next_byte;
|
||
|
||
for( size_t i = sizeof(DataImage::Head); i < image_size; i++ ) {
|
||
#ifdef __TMS320C2000__
|
||
next_byte = __byte( reinterpret_cast<int *>( buff ), i*2 );
|
||
crc_checker.calcPartial( &next_byte, 1, false );
|
||
|
||
next_byte = __byte( reinterpret_cast<int *>( buff ), i*2 + 1 );
|
||
crc_checker.calcPartial( &next_byte, 1, false );
|
||
#else
|
||
next_byte = buff[i];
|
||
crc_checker.calcPartial( &next_byte, 1, false );
|
||
#endif
|
||
}
|
||
|
||
return image.head->setting_crc == crc_checker.get();
|
||
}
|
||
|
||
void driver::detail::ParamStaticStorage::update_crc() {
|
||
|
||
const size_t image_size = image.get_image_size();
|
||
|
||
Crc<16, 0x1021, 0xFFFF, false, false, 0> crc_checker;
|
||
char next_byte;
|
||
|
||
for( size_t i = sizeof(DataImage::Head); i < image_size; i++ ) {
|
||
#ifdef __TMS320C2000__
|
||
next_byte = __byte( reinterpret_cast<int *>( buff ), i*2 );
|
||
crc_checker.calcPartial( &next_byte, 1, false );
|
||
|
||
next_byte = __byte( reinterpret_cast<int *>( buff ), i*2 + 1 );
|
||
crc_checker.calcPartial( &next_byte, 1, false );
|
||
#else
|
||
next_byte = buff[i];
|
||
crc_checker.calcPartial( &next_byte, 1, false );
|
||
#endif
|
||
}
|
||
|
||
image.head->setting_crc = crc_checker.get();
|
||
|
||
}
|
||
|
||
void driver::detail::ParamStaticStorage::temp_construct() {
|
||
|
||
temp.buff.first = reinterpret_cast<char *>( tasks.get_allocator().resource()->allocate(temp.buff.second, 1) );
|
||
temp.buff.second = image.show_setting_size(tasks.front());
|
||
temp.target = image.extract_setting_data_buff(tasks.front(), temp.buff.second);
|
||
temp.offset = temp.target - buff;
|
||
|
||
}
|
||
|
||
void driver::detail::ParamStaticStorage::temp_destruct() {
|
||
|
||
tasks.get_allocator().resource()->deallocate(temp.buff.first, temp.buff.second, 1);
|
||
|
||
temp.buff = std::pair<char *, std::size_t>(nullptr, 0);
|
||
temp.target = nullptr;
|
||
temp.offset = 0;
|
||
|
||
}
|
||
|
||
void driver::detail::ParamStaticStorage::process() {
|
||
|
||
switch(state) {
|
||
case State::StartWriteOnce: {
|
||
|
||
if( memory.write( temp.target, temp.offset, temp.buff.second ) )
|
||
state = State::WaitWriteOnce;
|
||
|
||
} break;
|
||
case State::WaitWriteOnce: {
|
||
|
||
if( memory.isWriteComplete() )
|
||
state = State::StartReadOnce;
|
||
|
||
} break;
|
||
case State::StartReadOnce: {
|
||
|
||
if( memory.read( temp.buff.first, temp.offset, temp.buff.second ) )
|
||
state = State::WaitReadOnce;
|
||
|
||
} break;
|
||
case State::WaitReadOnce: {
|
||
|
||
if( memory.isReadComplete() ) {
|
||
|
||
if( std::memcmp( temp.buff.first, temp.target, temp.buff.second ) == 0 ) {
|
||
|
||
tasks.pop_front();
|
||
temp_destruct();
|
||
|
||
if( tasks.empty() ) {
|
||
|
||
update_crc();
|
||
|
||
state = State::StartUpdateCrc;
|
||
|
||
} else {
|
||
|
||
temp_construct();
|
||
|
||
state = State::StartWriteOnce;
|
||
|
||
}
|
||
|
||
} else {
|
||
|
||
state = State::StartWriteOnce;
|
||
|
||
}
|
||
}
|
||
|
||
} break;
|
||
case State::StartUpdateCrc: {
|
||
|
||
if( memory.write( buff, 0, sizeof(DataImage::Head) ) )
|
||
state = State::WaitUpdateCrc;
|
||
|
||
} break;
|
||
case State::WaitUpdateCrc: {
|
||
|
||
if( memory.isWriteComplete() )
|
||
state = State::StartReadCrc;
|
||
|
||
} break;
|
||
case State::StartReadCrc: {
|
||
|
||
if( memory.read( buff, 0, sizeof(DataImage::Head) ) )
|
||
state = State::WaitReadEnd;
|
||
|
||
} break;
|
||
|
||
case State::Idle: {
|
||
|
||
if( tasks.empty() )
|
||
|
||
disableProcess();
|
||
|
||
else {
|
||
|
||
temp_construct();
|
||
|
||
state = State::StartWriteOnce;
|
||
}
|
||
|
||
} break;
|
||
case State::StartReadHead: {
|
||
|
||
if( memory.read( buff, 0, sizeof(DataImage::Head) ) )
|
||
state = State::WaitReadHead;
|
||
|
||
} break;
|
||
case State::WaitReadHead: {
|
||
|
||
if( memory.isReadComplete() ) {
|
||
|
||
if( image.build_from_buff(buff, buff_size, 4) ) {
|
||
|
||
state = State::StartReadEnd;
|
||
|
||
} else {
|
||
|
||
image.reset();
|
||
|
||
crc_error = true;
|
||
|
||
state = State::Idle;
|
||
|
||
}
|
||
}
|
||
|
||
} break;
|
||
case State::StartReadEnd: {
|
||
|
||
if( memory.read( buff, 0, image.get_image_size() ) )
|
||
state = State::WaitReadEnd;
|
||
|
||
} break;
|
||
case State::WaitReadEnd: {
|
||
|
||
if( memory.isReadComplete() ) {
|
||
|
||
bool crc_equal = check_crc();
|
||
|
||
crc_error = not crc_equal;
|
||
data_ready = crc_equal;
|
||
|
||
state = State::Idle;
|
||
}
|
||
|
||
} break;
|
||
case State::StartWrite: {
|
||
|
||
update_crc();
|
||
|
||
if( memory.write( buff, 0, image.get_image_size() ) )
|
||
state = State::WaitWrite;
|
||
|
||
} break;
|
||
case State::WaitWrite: {
|
||
|
||
if( memory.isWriteComplete() )
|
||
state = State::StartReadHead;
|
||
|
||
} break;
|
||
}
|
||
}
|
||
|
||
bool driver::detail::ParamStaticStorage::DataImage::insert_new_setting(
|
||
unsigned int id, unsigned int size ) {
|
||
|
||
if( get_image_size() + sizeof( SettingInfo ) + aligned_size( size ) > max_size )
|
||
return false;
|
||
|
||
//Смещяем данные в буфере.
|
||
move_buff( data_buff, data_buff + head->buff_size, sizeof( SettingInfo ) );
|
||
|
||
unsigned int next_index = head->setting_count;
|
||
|
||
//Обнавляем инофрмацию о настройке в образе данных.
|
||
info[next_index].id = id;
|
||
info[next_index].setting_size = size;
|
||
|
||
//Обновляем информацию о количестве настроек в образе данных.
|
||
head->setting_count = next_index + 1;
|
||
|
||
//Смещяем указатель на данные в DataImage.
|
||
data_buff = data_buff + sizeof( SettingInfo );
|
||
|
||
//Обновляем информацию о размере данных в буфере.
|
||
head->buff_size = head->buff_size + aligned_size( size );
|
||
|
||
return true;
|
||
}
|
||
|
||
|
||
bool driver::detail::ParamStaticStorage::DataImage::build_from_buff( char * buff, std::size_t buff_size, std::size_t buff_align ) {
|
||
|
||
max_size = buff_size;
|
||
data_align = buff_align;
|
||
|
||
head = reinterpret_cast<DataImage::Head *>( buff );
|
||
info = reinterpret_cast<DataImage::SettingInfo *>( buff + sizeof(Head) );
|
||
data_buff = buff + sizeof(Head) + sizeof(SettingInfo) * head->setting_count;
|
||
|
||
return get_image_size() < buff_size;
|
||
|
||
}
|
||
|
||
bool driver::detail::ParamStaticStorage::DataImage::change_size(
|
||
unsigned int size, unsigned int index, char * data_pointer ) {
|
||
|
||
int delta = aligned_size( size ) - aligned_size( info[index].setting_size );
|
||
|
||
//Ошибка: нужно больше золота! (Буфер памяти переполнен).
|
||
if( delta + get_image_size() > max_size )
|
||
return false;
|
||
|
||
move_buff( data_pointer, data_buff + head->buff_size, delta );
|
||
|
||
//Обнавляем инофрмацию о настройке в образе данных.
|
||
info[index].setting_size = size;
|
||
|
||
//Обновляем информацию о размере данных в буфере.
|
||
head->buff_size = head->buff_size + delta;
|
||
|
||
return true;
|
||
|
||
}
|
||
|
||
bool driver::detail::ParamStaticStorage::DataImage::change_setting_info(
|
||
unsigned int id, unsigned int size ) {
|
||
|
||
char * data_pointer = data_buff;
|
||
unsigned int index;
|
||
|
||
for( index = 0; index < head->setting_count; index++ ) {
|
||
|
||
data_pointer = data_pointer + aligned_size( info[index].setting_size );
|
||
|
||
if( info[index].id == id )
|
||
break;
|
||
|
||
}
|
||
|
||
if( index == head->setting_count )
|
||
return insert_new_setting( id, size );
|
||
else
|
||
return change_size( size, index, data_pointer );
|
||
|
||
}
|
||
|
||
size_t driver::detail::ParamStaticStorage::DataImage::get_image_size() const {
|
||
|
||
return sizeof(Head) + sizeof(SettingInfo) * head->setting_count + head->buff_size;
|
||
|
||
}
|
||
|
||
unsigned int driver::detail::ParamStaticStorage::DataImage::show_setting_size(
|
||
unsigned int id ) const {
|
||
|
||
for( unsigned int i = 0; i < head->setting_count; i++ ) {
|
||
if( info[i].id == id )
|
||
return info[i].setting_size;
|
||
}
|
||
|
||
return 0;
|
||
|
||
}
|
||
|
||
char * driver::detail::ParamStaticStorage::DataImage::extract_setting_data_buff(
|
||
unsigned int id, unsigned int size ) const {
|
||
|
||
char * current_ptr = data_buff;
|
||
|
||
for( unsigned int i = 0; i < head->setting_count; i++ ) {
|
||
|
||
if( info[i].id == id
|
||
and info[i].setting_size == size )
|
||
return current_ptr;
|
||
|
||
current_ptr = current_ptr + aligned_size( info[i].setting_size );
|
||
|
||
}
|
||
|
||
return nullptr;
|
||
|
||
}
|
||
|
||
const char * driver::detail::ParamStaticStorage::DataImage::show_setting_data_buff(
|
||
unsigned int id ) const {
|
||
|
||
char * current_ptr = data_buff;
|
||
|
||
for( unsigned int i = 0; i < head->setting_count; i++ ) {
|
||
|
||
if( info[i].id == id )
|
||
return current_ptr;
|
||
|
||
current_ptr = current_ptr + aligned_size( info[i].setting_size );
|
||
|
||
}
|
||
|
||
return nullptr;
|
||
}
|
||
|
||
void driver::detail::ParamStaticStorage::DataImage::move_buff(
|
||
char * from, char * to, int delta ) {
|
||
|
||
if( delta > 0 ) do {
|
||
*( to + delta ) = *to;
|
||
} while( to-- != from );
|
||
else if( delta < 0 ) do {
|
||
*( from + delta ) = *from;
|
||
} while( from++ != to );
|
||
|
||
}
|
||
|
||
std::size_t driver::detail::ParamStaticStorage::DataImage::aligned_size(
|
||
std::size_t size ) const {
|
||
|
||
return ( size + data_align - 1 ) & ~( data_align - 1 );
|
||
|
||
}
|
||
|
||
void driver::detail::ParamStaticStorage::DataImage::reset() {
|
||
|
||
if(head) {
|
||
head->buff_size = 0;
|
||
head->setting_count = 0;
|
||
head->setting_crc = 0;
|
||
|
||
data_buff = reinterpret_cast<char *>( head ) + sizeof(Head) + sizeof(SettingInfo) * head->setting_count;
|
||
}
|
||
|
||
}
|
||
|
||
void driver::detail::ParamStaticStorage::clear() {
|
||
|
||
image.reset();
|
||
|
||
}
|