519 lines
12 KiB
C++
519 lines
12 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();
|
|
|
|
}
|