spicetools/acioemu/acioemu.cpp

210 lines
5.5 KiB
C++
Raw Normal View History

2024-08-28 15:10:34 +00:00
#include "acioemu.h"
#include "util/logging.h"
#include "util/utils.h"
using namespace acioemu;
ACIOEmu::ACIOEmu() {
this->devices = new std::vector<ACIODeviceEmu *>();
this->response_buffer = new circular_buffer<uint8_t>(4096);
this->read_buffer = new circular_buffer<uint8_t>(1024);
}
ACIOEmu::~ACIOEmu() {
// delete devices
for (auto device : *this->devices) {
delete device;
}
delete this->devices;
// delete buffers
delete this->response_buffer;
delete this->read_buffer;
}
void ACIOEmu::add_device(ACIODeviceEmu *device) {
this->devices->push_back(device);
}
void ACIOEmu::write(uint8_t byte) {
// insert into buffer
if (!invert) {
if (byte == ACIO_ESCAPE) {
invert = true;
} else {
this->read_buffer->put(byte);
}
} else {
byte = ~byte;
invert = false;
this->read_buffer->put(byte);
}
// clean garbage
while (!this->read_buffer->empty() && this->read_buffer->peek() != 0xAA) {
this->read_buffer->get();
}
while (this->read_buffer->size() > 1 && this->read_buffer->peek(1) == 0xAA) {
this->read_buffer->get();
}
// handshake counter
static unsigned int handshake_counter = 0;
if (byte == 0xAA) {
handshake_counter++;
} else {
handshake_counter = 0;
}
// check for handshake
if (handshake_counter > 1) {
/*
* small hack - BIO2 seems to expect more bytes here - sending two bytes each time fixes it
* TODO replace this handshake code with something better
*/
this->response_buffer->put(ACIO_SOF);
this->response_buffer->put(ACIO_SOF);
handshake_counter--;
return;
}
// parse
if (!this->read_buffer->empty() && this->read_buffer->size() >= 6) {
bool is_complete = false;
// check if broadcast
if (this->read_buffer->peek(1) == ACIO_BROADCAST) {
// check msg data size
auto data_size = this->read_buffer->peek(2);
// check if msg is complete (SOF + checksum + broadcast header + data_size)
is_complete = this->read_buffer->size() >= 2u + 2u + data_size;
} else {
// check msg data size
auto data_size = this->read_buffer->peek(5);
// check if msg is complete (SOF + checksum + command header + data_size)
is_complete = this->read_buffer->size() >= 2u + MSG_HEADER_SIZE + data_size;
}
// parse message if complete
if (is_complete) {
this->msg_parse();
this->read_buffer->reset();
}
}
}
std::optional<uint8_t> ACIOEmu::read() {
if (this->response_buffer->empty()) {
return std::nullopt;
}
return this->response_buffer->get();
}
size_t ACIOEmu::bytes_available() {
return this->response_buffer->size();
}
void ACIOEmu::msg_parse() {
#ifdef ACIOEMU_LOG
log_info("acioemu", "MSG RECV: {}", bin2hex(*this->read_buffer));
#endif
// calculate checksum
uint8_t chk = 0;
size_t max = this->read_buffer->size() - 1;
for (size_t i = 1; i < max; i++) {
chk += this->read_buffer->peek(i);
}
// check checksum
uint8_t chk_receive = this->read_buffer->peek(this->read_buffer->size() - 1);
if (chk != chk_receive) {
#ifdef ACIOEMU_LOG
log_info("acioemu", "detected wrong checksum: {}/{}", chk, chk_receive);
#endif
return;
}
// get message data
auto msg_data = this->read_buffer->peek_all();
auto msg_in = (MessageData *) &msg_data[1];
// correct cmd code endianness if this is not a broadcast
if (msg_in->addr != ACIO_BROADCAST) {
msg_in->cmd.code = acio_u16(msg_in->cmd.code);
}
// pass to applicable device
uint8_t node_offset = 0;
for (auto device : *this->devices) {
if (device->is_applicable(node_offset, msg_in->addr)) {
auto cur_offset = msg_in->addr - node_offset - 1;
if (cur_offset < 0) {
break;
}
if (device->parse_msg(msg_in, this->response_buffer)) {
return;
} else {
break;
}
}
node_offset += device->node_count;
}
// ignore broadcast messages by default
if (msg_in->addr == ACIO_BROADCAST) {
return;
}
/*
* Default Behavior
* If you want to do anything different, just handle the
* commands in your own device implementation.
*/
switch (msg_in->cmd.code) {
// node count report
case ACIO_CMD_ASSIGN_ADDRS: {
if (msg_in->addr == 0x00 && node_offset > 0) {
auto msg = ACIODeviceEmu::create_msg(msg_in, 1, &node_offset);
ACIODeviceEmu::write_msg(msg, this->response_buffer);
delete msg;
return;
}
break;
}
// status 0 defaults
case ACIO_CMD_CLEAR:
case ACIO_CMD_STARTUP:
case 0x80: // KEEPALIVE
case 0xFF: // BROADCAST
{
// send status 0
auto msg = ACIODeviceEmu::create_msg_status(msg_in, 0);
ACIODeviceEmu::write_msg(msg, response_buffer);
delete msg;
return;
}
default:
break;
}
#ifdef ACIOEMU_LOG
log_info("acioemu", "UNHANDLED MSG FOR ADDR: {}, CMD: 0x{:x}), DATA: {}",
msg_in->addr,
msg_in->cmd.code,
bin2hex(*this->read_buffer));
#endif
}