spicetools/acioemu/device.cpp

119 lines
3.4 KiB
C++
Raw Normal View History

2024-08-28 15:10:34 +00:00
#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);
}