119 lines
3.4 KiB
C++
119 lines
3.4 KiB
C++
#include "device.h"
|
|
|
|
#include "util/logging.h"
|
|
#include "util/utils.h"
|
|
|
|
using namespace acioemu;
|
|
|
|
void ACIODeviceEmu::set_header(MessageData* data, uint8_t addr, uint16_t code, uint8_t pid,
|
|
uint8_t data_size)
|
|
{
|
|
// flag as response
|
|
if (addr != 0) {
|
|
addr |= ACIO_RESPONSE_FLAG;
|
|
}
|
|
|
|
// set header data
|
|
data->addr = addr;
|
|
data->cmd.code = acio_u16(code);
|
|
data->cmd.pid = pid;
|
|
data->cmd.data_size = data_size;
|
|
}
|
|
|
|
void ACIODeviceEmu::set_version(MessageData* data, uint32_t type, uint8_t flag,
|
|
uint8_t ver_major, uint8_t ver_minor, uint8_t ver_rev, std::string code)
|
|
{
|
|
|
|
// set version data
|
|
auto data_version = &data->cmd.data_version;
|
|
data_version->type = type;
|
|
data_version->flag = flag;
|
|
data_version->ver_major = ver_major;
|
|
data_version->ver_minor = ver_minor;
|
|
data_version->ver_rev = ver_rev;
|
|
strncpy(data_version->code, code.c_str(), sizeof(data_version->code));
|
|
strncpy(data_version->date, __DATE__, sizeof(data_version->date));
|
|
strncpy(data_version->time, __TIME__, sizeof(data_version->time));
|
|
}
|
|
|
|
MessageData *ACIODeviceEmu::create_msg(uint8_t addr, uint16_t code, uint8_t pid, size_t data_size,
|
|
uint8_t *data)
|
|
{
|
|
// check data size
|
|
if (data_size > 0xFF) {
|
|
log_warning("acio", "data size > 255: {}", data_size);
|
|
data_size = 0xFF;
|
|
}
|
|
|
|
// allocate data
|
|
auto data_raw = new uint8_t[MSG_HEADER_SIZE + data_size];
|
|
|
|
// set header
|
|
auto msg = (MessageData *) &data_raw[0];
|
|
set_header(msg, addr, code, pid, (uint8_t) data_size);
|
|
|
|
// set data
|
|
if (data) {
|
|
memcpy(data_raw + MSG_HEADER_SIZE, data, data_size);
|
|
} else {
|
|
memset(data_raw + MSG_HEADER_SIZE, 0, data_size);
|
|
}
|
|
|
|
// return prepared message
|
|
return msg;
|
|
}
|
|
|
|
MessageData *ACIODeviceEmu::create_msg(MessageData *msg_in, size_t data_size, uint8_t *data) {
|
|
return create_msg(msg_in->addr, msg_in->cmd.code, msg_in->cmd.pid, data_size, data);
|
|
}
|
|
|
|
MessageData *ACIODeviceEmu::create_msg_status(uint8_t addr, uint16_t code, uint8_t pid, uint8_t status) {
|
|
return create_msg(addr, code, pid, 1, &status);
|
|
}
|
|
|
|
MessageData *ACIODeviceEmu::create_msg_status(MessageData *msg_in, uint8_t status) {
|
|
return create_msg_status(msg_in->addr, msg_in->cmd.code, msg_in->cmd.pid, status);
|
|
}
|
|
|
|
bool ACIODeviceEmu::is_applicable(uint8_t node_offset, uint8_t node) {
|
|
return node > node_offset && node <= node_offset + this->node_count;
|
|
}
|
|
|
|
void ACIODeviceEmu::write_msg(const uint8_t *data, size_t size, circular_buffer<uint8_t> *response_buffer) {
|
|
|
|
// header
|
|
for (int i = 0; i < 2; i++) {
|
|
response_buffer->put(ACIO_SOF);
|
|
}
|
|
|
|
// msg data and checksum
|
|
uint8_t b, chk = 0;
|
|
for (size_t i = 0; i <= size; i++) {
|
|
|
|
// set byte to data or checksum
|
|
if (i < size) {
|
|
b = data[i];
|
|
chk += b;
|
|
} else {
|
|
b = chk;
|
|
}
|
|
|
|
// check for escape
|
|
if (b == ACIO_SOF || b == ACIO_ESCAPE) {
|
|
response_buffer->put(ACIO_ESCAPE);
|
|
response_buffer->put(~b);
|
|
} else {
|
|
response_buffer->put(b);
|
|
}
|
|
}
|
|
|
|
#ifdef ACIOEMU_LOG
|
|
log_info("acioemu", "ACIO MSG OUT: AA{}{:02X}", bin2hex(data, size), chk);
|
|
#endif
|
|
}
|
|
|
|
void ACIODeviceEmu::write_msg(MessageData *msg, circular_buffer<uint8_t> *response_buffer) {
|
|
auto data = reinterpret_cast<const uint8_t *>(msg);
|
|
write_msg(data, MSG_HEADER_SIZE + msg->cmd.data_size, response_buffer);
|
|
}
|