建立 MAG160C 逆向工程交接仓库
This commit is contained in:
ZXCLI
@@ -0,0 +1,17 @@
|
||||
#include "core/context.hpp"
|
||||
|
||||
#ifndef MAG160C_HAS_LIBUSB
|
||||
#define MAG160C_HAS_LIBUSB 0
|
||||
#endif
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
Context::Context()
|
||||
: has_libusb_(MAG160C_HAS_LIBUSB != 0) {}
|
||||
|
||||
bool Context::has_libusb() const noexcept {
|
||||
return has_libusb_;
|
||||
}
|
||||
|
||||
} // namespace mag160c::core
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
#ifndef MAG160C_CORE_CONTEXT_HPP
|
||||
#define MAG160C_CORE_CONTEXT_HPP
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
class Context {
|
||||
public:
|
||||
Context();
|
||||
|
||||
bool has_libusb() const noexcept;
|
||||
|
||||
private:
|
||||
bool has_libusb_;
|
||||
};
|
||||
|
||||
} // namespace mag160c::core
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,185 @@
|
||||
#include "core/device.hpp"
|
||||
|
||||
#include "core/error.hpp"
|
||||
|
||||
#include <cstring>
|
||||
|
||||
#if MAG160C_HAS_LIBUSB
|
||||
#include <libusb.h>
|
||||
#endif
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
bool is_in_endpoint(uint8_t address) {
|
||||
return (address & 0x80U) != 0;
|
||||
}
|
||||
|
||||
bool find_bulk_pair(const std::vector<EndpointDescriptor>& endpoints, EndpointPair* out) {
|
||||
if (out == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
EndpointPair pair{};
|
||||
for (const EndpointDescriptor& endpoint : endpoints) {
|
||||
if (endpoint.type != EndpointType::Bulk) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (is_in_endpoint(endpoint.address) && pair.bulk_in == 0) {
|
||||
pair.bulk_in = endpoint.address;
|
||||
} else if (!is_in_endpoint(endpoint.address) && pair.bulk_out == 0) {
|
||||
pair.bulk_out = endpoint.address;
|
||||
}
|
||||
}
|
||||
|
||||
if (pair.bulk_in == 0 || pair.bulk_out == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*out = pair;
|
||||
return true;
|
||||
}
|
||||
|
||||
#if MAG160C_HAS_LIBUSB
|
||||
|
||||
namespace {
|
||||
|
||||
EndpointType endpoint_type_from_libusb(uint8_t attributes) {
|
||||
switch (attributes & LIBUSB_TRANSFER_TYPE_MASK) {
|
||||
case LIBUSB_TRANSFER_TYPE_ISOCHRONOUS:
|
||||
return EndpointType::Isochronous;
|
||||
case LIBUSB_TRANSFER_TYPE_BULK:
|
||||
return EndpointType::Bulk;
|
||||
case LIBUSB_TRANSFER_TYPE_INTERRUPT:
|
||||
return EndpointType::Interrupt;
|
||||
default:
|
||||
return EndpointType::Other;
|
||||
}
|
||||
}
|
||||
|
||||
void copy_string_descriptor(
|
||||
libusb_device_handle* handle,
|
||||
uint8_t index,
|
||||
char* out,
|
||||
size_t out_size
|
||||
) {
|
||||
if (out == nullptr || out_size == 0) {
|
||||
return;
|
||||
}
|
||||
out[0] = '\0';
|
||||
if (handle == nullptr || index == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
unsigned char buffer[128] = {};
|
||||
const int rc = libusb_get_string_descriptor_ascii(handle, index, buffer, sizeof(buffer));
|
||||
if (rc <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
const size_t n = static_cast<size_t>(rc) < out_size - 1 ? static_cast<size_t>(rc) : out_size - 1;
|
||||
std::memcpy(out, buffer, n);
|
||||
out[n] = '\0';
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
#endif
|
||||
|
||||
mag160c_error_t list_devices(std::vector<DeviceInfo>* out) {
|
||||
if (out == nullptr) {
|
||||
set_last_error("list_devices: out must not be null");
|
||||
return MAG160C_ERR_INVALID_ARGUMENT;
|
||||
}
|
||||
|
||||
out->clear();
|
||||
|
||||
#if !MAG160C_HAS_LIBUSB
|
||||
set_last_error("list_devices: libusb-1.0 was not available at build time");
|
||||
return MAG160C_ERR_UNSUPPORTED;
|
||||
#else
|
||||
libusb_context* ctx = nullptr;
|
||||
int rc = libusb_init(&ctx);
|
||||
if (rc != 0) {
|
||||
set_last_error("list_devices: libusb_init failed");
|
||||
return MAG160C_ERR_USB;
|
||||
}
|
||||
|
||||
libusb_device** list = nullptr;
|
||||
const ssize_t count = libusb_get_device_list(ctx, &list);
|
||||
if (count < 0) {
|
||||
libusb_exit(ctx);
|
||||
set_last_error("list_devices: libusb_get_device_list failed");
|
||||
return MAG160C_ERR_USB;
|
||||
}
|
||||
|
||||
for (ssize_t i = 0; i < count; ++i) {
|
||||
libusb_device* device = list[i];
|
||||
libusb_device_descriptor desc{};
|
||||
if (libusb_get_device_descriptor(device, &desc) != 0) {
|
||||
continue;
|
||||
}
|
||||
if (desc.idVendor != MAG_IR_VENDOR_ID || desc.idProduct != MAG_IR_PRODUCT_ID) {
|
||||
continue;
|
||||
}
|
||||
|
||||
libusb_config_descriptor* config = nullptr;
|
||||
if (libusb_get_active_config_descriptor(device, &config) != 0 || config == nullptr) {
|
||||
continue;
|
||||
}
|
||||
|
||||
bool added = false;
|
||||
for (uint8_t iface_index = 0; iface_index < config->bNumInterfaces && !added; ++iface_index) {
|
||||
const libusb_interface& iface = config->interface[iface_index];
|
||||
for (int alt_index = 0; alt_index < iface.num_altsetting && !added; ++alt_index) {
|
||||
const libusb_interface_descriptor& alt = iface.altsetting[alt_index];
|
||||
std::vector<EndpointDescriptor> endpoints;
|
||||
endpoints.reserve(alt.bNumEndpoints);
|
||||
for (uint8_t ep_index = 0; ep_index < alt.bNumEndpoints; ++ep_index) {
|
||||
const libusb_endpoint_descriptor& ep = alt.endpoint[ep_index];
|
||||
endpoints.push_back({ep.bEndpointAddress, endpoint_type_from_libusb(ep.bmAttributes)});
|
||||
}
|
||||
|
||||
EndpointPair pair{};
|
||||
if (!find_bulk_pair(endpoints, &pair)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
DeviceInfo info{};
|
||||
info.c_info.vendor_id = desc.idVendor;
|
||||
info.c_info.product_id = desc.idProduct;
|
||||
info.c_info.bus = static_cast<uint8_t>(libusb_get_bus_number(device));
|
||||
info.c_info.address = static_cast<uint8_t>(libusb_get_device_address(device));
|
||||
info.c_info.interface_number = alt.bInterfaceNumber;
|
||||
info.c_info.bulk_in_endpoint = pair.bulk_in;
|
||||
info.c_info.bulk_out_endpoint = pair.bulk_out;
|
||||
|
||||
libusb_device_handle* handle = nullptr;
|
||||
if (libusb_open(device, &handle) == 0) {
|
||||
copy_string_descriptor(handle, desc.iProduct, info.c_info.product, sizeof(info.c_info.product));
|
||||
copy_string_descriptor(
|
||||
handle,
|
||||
desc.iManufacturer,
|
||||
info.c_info.manufacturer,
|
||||
sizeof(info.c_info.manufacturer)
|
||||
);
|
||||
copy_string_descriptor(handle, desc.iSerialNumber, info.c_info.serial, sizeof(info.c_info.serial));
|
||||
libusb_close(handle);
|
||||
}
|
||||
|
||||
out->push_back(info);
|
||||
added = true;
|
||||
}
|
||||
}
|
||||
|
||||
libusb_free_config_descriptor(config);
|
||||
}
|
||||
|
||||
libusb_free_device_list(list, 1);
|
||||
libusb_exit(ctx);
|
||||
clear_last_error();
|
||||
return MAG160C_OK;
|
||||
#endif
|
||||
}
|
||||
|
||||
} // namespace mag160c::core
|
||||
@@ -0,0 +1,41 @@
|
||||
#ifndef MAG160C_CORE_DEVICE_HPP
|
||||
#define MAG160C_CORE_DEVICE_HPP
|
||||
|
||||
#include "mag160c/mag160c.h"
|
||||
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
constexpr uint16_t MAG_IR_VENDOR_ID = 0x833c;
|
||||
constexpr uint16_t MAG_IR_PRODUCT_ID = 0x0001;
|
||||
|
||||
enum class EndpointType {
|
||||
Other,
|
||||
Bulk,
|
||||
Interrupt,
|
||||
Isochronous
|
||||
};
|
||||
|
||||
struct EndpointDescriptor {
|
||||
uint8_t address = 0;
|
||||
EndpointType type = EndpointType::Other;
|
||||
};
|
||||
|
||||
struct EndpointPair {
|
||||
uint8_t bulk_in = 0;
|
||||
uint8_t bulk_out = 0;
|
||||
};
|
||||
|
||||
struct DeviceInfo {
|
||||
mag160c_device_info_t c_info{};
|
||||
};
|
||||
|
||||
bool is_in_endpoint(uint8_t address);
|
||||
bool find_bulk_pair(const std::vector<EndpointDescriptor>& endpoints, EndpointPair* out);
|
||||
mag160c_error_t list_devices(std::vector<DeviceInfo>* out);
|
||||
|
||||
} // namespace mag160c::core
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,51 @@
|
||||
#include "core/error.hpp"
|
||||
|
||||
#include <utility>
|
||||
|
||||
namespace mag160c::core {
|
||||
namespace {
|
||||
|
||||
thread_local std::string g_last_error;
|
||||
|
||||
} // namespace
|
||||
|
||||
const char* error_name(mag160c_error_t code) noexcept {
|
||||
switch (code) {
|
||||
case MAG160C_OK:
|
||||
return "MAG160C_OK";
|
||||
case MAG160C_ERR_INVALID_ARGUMENT:
|
||||
return "MAG160C_ERR_INVALID_ARGUMENT";
|
||||
case MAG160C_ERR_NO_DEVICE:
|
||||
return "MAG160C_ERR_NO_DEVICE";
|
||||
case MAG160C_ERR_PERMISSION:
|
||||
return "MAG160C_ERR_PERMISSION";
|
||||
case MAG160C_ERR_USB:
|
||||
return "MAG160C_ERR_USB";
|
||||
case MAG160C_ERR_TIMEOUT:
|
||||
return "MAG160C_ERR_TIMEOUT";
|
||||
case MAG160C_ERR_CHECKSUM:
|
||||
return "MAG160C_ERR_CHECKSUM";
|
||||
case MAG160C_ERR_PROTOCOL_UNKNOWN:
|
||||
return "MAG160C_ERR_PROTOCOL_UNKNOWN";
|
||||
case MAG160C_ERR_UNSUPPORTED:
|
||||
return "MAG160C_ERR_UNSUPPORTED";
|
||||
case MAG160C_ERR_INTERNAL:
|
||||
return "MAG160C_ERR_INTERNAL";
|
||||
default:
|
||||
return "MAG160C_ERR_UNKNOWN_CODE";
|
||||
}
|
||||
}
|
||||
|
||||
const char* last_error() noexcept {
|
||||
return g_last_error.c_str();
|
||||
}
|
||||
|
||||
void clear_last_error() {
|
||||
g_last_error.clear();
|
||||
}
|
||||
|
||||
void set_last_error(std::string message) {
|
||||
g_last_error = std::move(message);
|
||||
}
|
||||
|
||||
} // namespace mag160c::core
|
||||
@@ -0,0 +1,18 @@
|
||||
#ifndef MAG160C_CORE_ERROR_HPP
|
||||
#define MAG160C_CORE_ERROR_HPP
|
||||
|
||||
#include "mag160c/mag160c.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
const char* error_name(mag160c_error_t code) noexcept;
|
||||
const char* last_error() noexcept;
|
||||
void clear_last_error();
|
||||
void set_last_error(std::string message);
|
||||
|
||||
} // namespace mag160c::core
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,65 @@
|
||||
#include "core/ir_device.hpp"
|
||||
|
||||
#include "core/error.hpp"
|
||||
|
||||
#include <cstring>
|
||||
|
||||
namespace mag160c::core {
|
||||
namespace {
|
||||
|
||||
void copy_literal(char* out, size_t out_size, const char* value) {
|
||||
if (out == nullptr || out_size == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::strncpy(out, value, out_size - 1);
|
||||
out[out_size - 1] = '\0';
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
IrDevice::IrDevice() {
|
||||
info_.width = 160;
|
||||
info_.height = 120;
|
||||
info_.output_width = 160;
|
||||
info_.output_height = 120;
|
||||
info_.max_fps = 25;
|
||||
info_.current_fps = 0;
|
||||
copy_literal(info_.name, sizeof(info_.name), "MAG160C");
|
||||
copy_literal(info_.type, sizeof(info_.type), "vendor-bulk-ir");
|
||||
}
|
||||
|
||||
const mag160c_ir_info_t& IrDevice::info() const noexcept {
|
||||
return info_;
|
||||
}
|
||||
|
||||
mag160c_error_t IrDevice::trigger_ffc() const {
|
||||
set_last_error(
|
||||
"IR FFC protocol recovered from libmagcore.so.2.1.1: write {u32 0x6bb6b672, u32 param} "
|
||||
"on bulk endpoint OUT 0x03, then read response on IN 0x82 (0x1000 max, 2000 ms timeout); "
|
||||
"live USB transport is not available in this build"
|
||||
);
|
||||
return MAG160C_ERR_UNSUPPORTED;
|
||||
}
|
||||
|
||||
mag160c_error_t IrDevice::read_raw_once(
|
||||
unsigned char* /*out_bytes*/,
|
||||
size_t /*out_capacity*/,
|
||||
size_t* out_size,
|
||||
int /*timeout_ms*/
|
||||
) const {
|
||||
if (out_size == nullptr) {
|
||||
set_last_error("mag160c_ir_read_raw_once: out_size must not be null");
|
||||
return MAG160C_ERR_INVALID_ARGUMENT;
|
||||
}
|
||||
|
||||
*out_size = 0;
|
||||
set_last_error(
|
||||
"IR raw read protocol recovered: start stream with {0x6bb6b673} on OUT 0x03, then read "
|
||||
"frame stream on IN 0x81 (markers 0x1bb1b11b/0x1bb1b11c, data at +0x1c, size 0x38+len); "
|
||||
"live USB transport is not available in this build"
|
||||
);
|
||||
return MAG160C_ERR_UNSUPPORTED;
|
||||
}
|
||||
|
||||
} // namespace mag160c::core
|
||||
@@ -0,0 +1,27 @@
|
||||
#ifndef MAG160C_CORE_IR_DEVICE_HPP
|
||||
#define MAG160C_CORE_IR_DEVICE_HPP
|
||||
|
||||
#include "mag160c/mag160c.h"
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
class IrDevice {
|
||||
public:
|
||||
IrDevice();
|
||||
|
||||
const mag160c_ir_info_t& info() const noexcept;
|
||||
mag160c_error_t trigger_ffc() const;
|
||||
mag160c_error_t read_raw_once(
|
||||
unsigned char* out_bytes,
|
||||
size_t out_capacity,
|
||||
size_t* out_size,
|
||||
int timeout_ms
|
||||
) const;
|
||||
|
||||
private:
|
||||
mag160c_ir_info_t info_{};
|
||||
};
|
||||
|
||||
} // namespace mag160c::core
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,107 @@
|
||||
#include "core/ir_frame.hpp"
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
mag160c_error_t parse_ir_frame(
|
||||
const uint8_t* data,
|
||||
size_t size,
|
||||
IrFrameHeader* out_header,
|
||||
const uint16_t** out_pixels
|
||||
) {
|
||||
if (data == nullptr || out_header == nullptr || out_pixels == nullptr) {
|
||||
return MAG160C_ERR_INVALID_ARGUMENT;
|
||||
}
|
||||
if (size < MAG160C_IR_FRAME_DATA_OFFSET + 4) {
|
||||
return MAG160C_ERR_PROTOCOL_UNKNOWN;
|
||||
}
|
||||
|
||||
const uint32_t marker =
|
||||
static_cast<uint32_t>(data[0]) |
|
||||
(static_cast<uint32_t>(data[1]) << 8) |
|
||||
(static_cast<uint32_t>(data[2]) << 16) |
|
||||
(static_cast<uint32_t>(data[3]) << 24);
|
||||
if (marker != MAG160C_IR_FRAME_MARKER) {
|
||||
return MAG160C_ERR_PROTOCOL_UNKNOWN;
|
||||
}
|
||||
|
||||
auto rd32 = [&](size_t off) {
|
||||
return static_cast<uint32_t>(data[off]) |
|
||||
(static_cast<uint32_t>(data[off + 1]) << 8) |
|
||||
(static_cast<uint32_t>(data[off + 2]) << 16) |
|
||||
(static_cast<uint32_t>(data[off + 3]) << 24);
|
||||
};
|
||||
|
||||
IrFrameHeader h{};
|
||||
h.frame_counter = rd32(4);
|
||||
h.data_length = rd32(8);
|
||||
h.frame_type = rd32(12);
|
||||
h.period_shutter = rd32(16);
|
||||
|
||||
if (h.frame_type > 1) {
|
||||
return MAG160C_ERR_PROTOCOL_UNKNOWN;
|
||||
}
|
||||
if (size < MAG160C_IR_FRAME_OVERHEAD + h.data_length) {
|
||||
return MAG160C_ERR_PROTOCOL_UNKNOWN;
|
||||
}
|
||||
|
||||
const size_t trailing_offset = MAG160C_IR_FRAME_DATA_OFFSET + h.data_length;
|
||||
if (rd32(trailing_offset) != MAG160C_IR_FRAME_TRAILING_MARKER) {
|
||||
return MAG160C_ERR_PROTOCOL_UNKNOWN;
|
||||
}
|
||||
|
||||
h.marker = marker;
|
||||
*out_header = h;
|
||||
*out_pixels = reinterpret_cast<const uint16_t*>(data + MAG160C_IR_FRAME_DATA_OFFSET);
|
||||
return MAG160C_OK;
|
||||
}
|
||||
|
||||
void calibrate_frame(
|
||||
const uint16_t* frame,
|
||||
const IrCalibrationTables& tables,
|
||||
uint16_t* out
|
||||
) {
|
||||
if (frame == nullptr || tables.coeff == nullptr || tables.thresholds == nullptr ||
|
||||
out == nullptr || tables.pixel_count == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t pixels = tables.pixel_count;
|
||||
const uint32_t bands = tables.band_count == 0 ? 1 : tables.band_count;
|
||||
const uint32_t search_bands = bands - 1;
|
||||
|
||||
for (uint32_t p = 0; p < pixels; ++p) {
|
||||
int32_t diff;
|
||||
if (tables.has_baseline && tables.baseline != nullptr) {
|
||||
diff = static_cast<int32_t>(static_cast<int16_t>(
|
||||
frame[p] - tables.baseline[p]));
|
||||
} else {
|
||||
diff = static_cast<int32_t>(frame[p]);
|
||||
}
|
||||
diff >>= 1;
|
||||
|
||||
uint32_t band = 0;
|
||||
if (search_bands > 0) {
|
||||
for (uint32_t i = 0; i < search_bands; ++i) {
|
||||
if (diff <= tables.thresholds[p * search_bands + i]) {
|
||||
band = i;
|
||||
break;
|
||||
}
|
||||
band = i + 1;
|
||||
}
|
||||
}
|
||||
|
||||
const size_t entry = (static_cast<size_t>(band) * pixels + p) * 2;
|
||||
const uint32_t coeff = tables.coeff[entry];
|
||||
const uint32_t offset = tables.coeff[entry + 1];
|
||||
int32_t v = static_cast<int32_t>(offset) + ((diff * static_cast<int32_t>(coeff)) >> 12);
|
||||
if (v < 0) {
|
||||
v = 0;
|
||||
}
|
||||
if (v > 0xffff) {
|
||||
v = 0xffff;
|
||||
}
|
||||
out[p] = static_cast<uint16_t>(v);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mag160c::core
|
||||
@@ -0,0 +1,57 @@
|
||||
#ifndef MAG160C_CORE_IR_FRAME_HPP
|
||||
#define MAG160C_CORE_IR_FRAME_HPP
|
||||
|
||||
#include "mag160c/mag160c.h"
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
struct IrFrameHeader {
|
||||
uint32_t marker;
|
||||
uint32_t frame_counter;
|
||||
uint32_t data_length;
|
||||
uint32_t frame_type;
|
||||
uint32_t period_shutter;
|
||||
};
|
||||
|
||||
struct IrCalibrationTables {
|
||||
const int16_t* thresholds; /* per-pixel band thresholds, [bands] per pixel */
|
||||
const uint16_t* coeff; /* per-pixel per-band {coeff, offset} pairs, 2*bands per pixel */
|
||||
uint32_t pixel_count; /* 160*120 = 19200 */
|
||||
uint32_t band_count; /* thresholds per pixel (band_count-1 searched) */
|
||||
const uint16_t* baseline; /* optional baseline frame (may be null) */
|
||||
bool has_baseline;
|
||||
};
|
||||
|
||||
/*
|
||||
* Parse a frame stream chunk recovered from libmagcore.so.2.1.1:
|
||||
* marker 0x1bb1b11b @0, frame counter @4, data length @8, type @0xc, shutter @0x10,
|
||||
* pixel data @0x1c, trailing marker 0x1bb1b11c @0x1c+data_length, total 0x38+data_length.
|
||||
* Returns MAG160C_OK when a complete, valid frame is present.
|
||||
*/
|
||||
mag160c_error_t parse_ir_frame(
|
||||
const uint8_t* data,
|
||||
size_t size,
|
||||
IrFrameHeader* out_header,
|
||||
const uint16_t** out_pixels
|
||||
);
|
||||
|
||||
/*
|
||||
* Recovered CFunctions::Calibration piecewise-linear map (libcoresdk.so 0x6a41c):
|
||||
* diff = (int16)(frame[p] - baseline[p]) >> 1
|
||||
* band = first index i where diff <= thresholds[p*bands+i] (linear scan, max bands-1)
|
||||
* v = coeff[(band*pixels + p)*2 + 1] + ((diff * coeff[(band*pixels + p)*2]) >> 12)
|
||||
* out[p] = clamp(v, 0, 0xffff)
|
||||
* When no baseline is set, diff = frame[p] >> 1.
|
||||
*/
|
||||
void calibrate_frame(
|
||||
const uint16_t* frame,
|
||||
const IrCalibrationTables& tables,
|
||||
uint16_t* out
|
||||
);
|
||||
|
||||
} // namespace mag160c::core
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,70 @@
|
||||
#include "core/tcm_device.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace mag160c::core {
|
||||
namespace {
|
||||
|
||||
constexpr uint8_t MAIN_TCM = 0x02;
|
||||
constexpr uint8_t SUB_ROTATE = 0x77;
|
||||
constexpr uint8_t SUB_LIGHT_STEADY = 0x31;
|
||||
constexpr uint8_t SUB_LIGHT_BLINK = 0x32;
|
||||
constexpr uint8_t SUB_LIGHT_BREATH = 0x33;
|
||||
|
||||
uint8_t light_subcommand(mag160c_tcm_light_mode_t mode) {
|
||||
switch (mode) {
|
||||
case MAG160C_TCM_LIGHT_STEADY:
|
||||
return SUB_LIGHT_STEADY;
|
||||
case MAG160C_TCM_LIGHT_BLINK:
|
||||
return SUB_LIGHT_BLINK;
|
||||
case MAG160C_TCM_LIGHT_BREATH:
|
||||
return SUB_LIGHT_BREATH;
|
||||
}
|
||||
return SUB_LIGHT_STEADY;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> light_payload(mag160c_tcm_light_color_t color) {
|
||||
switch (color) {
|
||||
case MAG160C_TCM_LIGHT_OFF:
|
||||
return {0x00, 0x00, 0x00, 0x00};
|
||||
case MAG160C_TCM_LIGHT_RED:
|
||||
return {0x01, 0xff, 0x00, 0x00};
|
||||
case MAG160C_TCM_LIGHT_GREEN:
|
||||
return {0x01, 0x00, 0xff, 0x00};
|
||||
case MAG160C_TCM_LIGHT_BLUE:
|
||||
return {0x01, 0x00, 0x00, 0xff};
|
||||
case MAG160C_TCM_LIGHT_YELLOW:
|
||||
return {0x01, 0xff, 0xff, 0x00};
|
||||
}
|
||||
return {0x00, 0x00, 0x00, 0x00};
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TcmCommandBuilder::TcmCommandBuilder() : frame_id_(1) {}
|
||||
|
||||
uint16_t TcmCommandBuilder::next_frame_id() {
|
||||
const uint16_t current = frame_id_;
|
||||
++frame_id_;
|
||||
if (frame_id_ >= 0x8000) {
|
||||
frame_id_ = 1;
|
||||
}
|
||||
return current;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> TcmCommandBuilder::build_rotate_frame(int angle) {
|
||||
angle = std::clamp(angle, -128, 128);
|
||||
|
||||
const uint8_t direction = angle < 0 ? 0x01 : 0x00;
|
||||
const uint8_t magnitude = static_cast<uint8_t>(angle < 0 ? -angle : angle);
|
||||
return encode_tcm_frame(MAIN_TCM, SUB_ROTATE, next_frame_id(), {direction, magnitude});
|
||||
}
|
||||
|
||||
std::vector<uint8_t> TcmCommandBuilder::build_light_frame(
|
||||
mag160c_tcm_light_color_t color,
|
||||
mag160c_tcm_light_mode_t mode
|
||||
) {
|
||||
return encode_tcm_frame(MAIN_TCM, light_subcommand(mode), next_frame_id(), light_payload(color));
|
||||
}
|
||||
|
||||
} // namespace mag160c::core
|
||||
@@ -0,0 +1,30 @@
|
||||
#ifndef MAG160C_CORE_TCM_DEVICE_HPP
|
||||
#define MAG160C_CORE_TCM_DEVICE_HPP
|
||||
|
||||
#include "core/tcm_frame.hpp"
|
||||
#include "mag160c/mag160c.h"
|
||||
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
class TcmCommandBuilder {
|
||||
public:
|
||||
TcmCommandBuilder();
|
||||
|
||||
std::vector<uint8_t> build_rotate_frame(int angle);
|
||||
std::vector<uint8_t> build_light_frame(
|
||||
mag160c_tcm_light_color_t color,
|
||||
mag160c_tcm_light_mode_t mode
|
||||
);
|
||||
|
||||
private:
|
||||
uint16_t next_frame_id();
|
||||
|
||||
uint16_t frame_id_;
|
||||
};
|
||||
|
||||
} // namespace mag160c::core
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,101 @@
|
||||
#include "core/tcm_frame.hpp"
|
||||
|
||||
#include "core/error.hpp"
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr size_t MAX_TCM_PAYLOAD_SIZE = 0xffffU - 5U;
|
||||
|
||||
} // namespace
|
||||
|
||||
uint8_t checksum(const uint8_t* data, size_t begin, size_t end) {
|
||||
if (data == nullptr || begin > end) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t sum = 0;
|
||||
for (size_t i = begin; i < end; ++i) {
|
||||
sum = static_cast<uint8_t>(sum + data[i]);
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> encode_tcm_frame(
|
||||
uint8_t main_cmd,
|
||||
uint8_t sub_cmd,
|
||||
uint16_t frame_id,
|
||||
const std::vector<uint8_t>& payload
|
||||
) {
|
||||
if (payload.size() > MAX_TCM_PAYLOAD_SIZE) {
|
||||
throw std::length_error("TCM payload is too large");
|
||||
}
|
||||
|
||||
const uint16_t body_length = static_cast<uint16_t>(payload.size() + 5U);
|
||||
std::vector<uint8_t> out(static_cast<size_t>(body_length) + 4U, 0);
|
||||
|
||||
out[0] = 0x7e;
|
||||
out[1] = static_cast<uint8_t>((body_length >> 8) & 0xff);
|
||||
out[2] = static_cast<uint8_t>(body_length & 0xff);
|
||||
out[3] = checksum(out.data(), 0, 3);
|
||||
|
||||
out[4] = main_cmd;
|
||||
out[5] = sub_cmd;
|
||||
out[6] = static_cast<uint8_t>((frame_id >> 8) & 0xff);
|
||||
out[7] = static_cast<uint8_t>(frame_id & 0xff);
|
||||
|
||||
for (size_t i = 0; i < payload.size(); ++i) {
|
||||
out[8 + i] = payload[i];
|
||||
}
|
||||
|
||||
out[out.size() - 1] = checksum(out.data(), 4, out.size() - 1);
|
||||
return out;
|
||||
}
|
||||
|
||||
mag160c_error_t decode_tcm_frame(const uint8_t* data, size_t size, TcmFrame* out) {
|
||||
if (data == nullptr || out == nullptr) {
|
||||
set_last_error("decode_tcm_frame: data and out must not be null");
|
||||
return MAG160C_ERR_INVALID_ARGUMENT;
|
||||
}
|
||||
|
||||
if (size < 9) {
|
||||
set_last_error("decode_tcm_frame: packet shorter than minimum TCM frame");
|
||||
return MAG160C_ERR_INVALID_ARGUMENT;
|
||||
}
|
||||
|
||||
if (data[0] != 0x7e) {
|
||||
set_last_error("decode_tcm_frame: missing 0x7e header byte");
|
||||
return MAG160C_ERR_CHECKSUM;
|
||||
}
|
||||
|
||||
if (data[3] != checksum(data, 0, 3)) {
|
||||
set_last_error("decode_tcm_frame: invalid header checksum");
|
||||
return MAG160C_ERR_CHECKSUM;
|
||||
}
|
||||
|
||||
const uint16_t body_length =
|
||||
static_cast<uint16_t>((static_cast<uint16_t>(data[1]) << 8) | data[2]);
|
||||
const size_t expected_size = static_cast<size_t>(body_length) + 4U;
|
||||
if (size != expected_size) {
|
||||
set_last_error("decode_tcm_frame: packet size does not match encoded body length");
|
||||
return MAG160C_ERR_INVALID_ARGUMENT;
|
||||
}
|
||||
|
||||
if (data[size - 1] != checksum(data, 4, size - 1)) {
|
||||
set_last_error("decode_tcm_frame: invalid body checksum");
|
||||
return MAG160C_ERR_CHECKSUM;
|
||||
}
|
||||
|
||||
out->main_cmd = data[4];
|
||||
out->sub_cmd = data[5];
|
||||
out->frame_id = static_cast<uint16_t>((static_cast<uint16_t>(data[6]) << 8) | data[7]);
|
||||
out->payload.assign(data + 8, data + size - 1);
|
||||
|
||||
clear_last_error();
|
||||
return MAG160C_OK;
|
||||
}
|
||||
|
||||
} // namespace mag160c::core
|
||||
@@ -0,0 +1,30 @@
|
||||
#ifndef MAG160C_CORE_TCM_FRAME_HPP
|
||||
#define MAG160C_CORE_TCM_FRAME_HPP
|
||||
|
||||
#include "mag160c/mag160c.h"
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
struct TcmFrame {
|
||||
uint8_t main_cmd = 0;
|
||||
uint8_t sub_cmd = 0;
|
||||
uint16_t frame_id = 0;
|
||||
std::vector<uint8_t> payload;
|
||||
};
|
||||
|
||||
uint8_t checksum(const uint8_t* data, size_t begin, size_t end);
|
||||
std::vector<uint8_t> encode_tcm_frame(
|
||||
uint8_t main_cmd,
|
||||
uint8_t sub_cmd,
|
||||
uint16_t frame_id,
|
||||
const std::vector<uint8_t>& payload
|
||||
);
|
||||
mag160c_error_t decode_tcm_frame(const uint8_t* data, size_t size, TcmFrame* out);
|
||||
|
||||
} // namespace mag160c::core
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,20 @@
|
||||
#ifndef MAG160C_CORE_TRANSPORT_HPP
|
||||
#define MAG160C_CORE_TRANSPORT_HPP
|
||||
|
||||
#include "mag160c/mag160c.h"
|
||||
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
namespace mag160c::core {
|
||||
|
||||
class Transport {
|
||||
public:
|
||||
virtual ~Transport() = default;
|
||||
virtual mag160c_error_t write(const std::vector<uint8_t>& bytes) = 0;
|
||||
virtual mag160c_error_t read(std::vector<uint8_t>* out, int timeout_ms) = 0;
|
||||
};
|
||||
|
||||
} // namespace mag160c::core
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user