cpp sdk更新

2026-07-30 19:23:47 +08:00
parent bd07331734
commit 7e4f632268
3 changed files with 303 additions and 61 deletions
--- a/fast_lowcmd_cpp/go1_fast_lowcmd.cpp
+++ b/fast_lowcmd_cpp/go1_fast_lowcmd.cpp
@@ -1,6 +1,7 @@
 #define PY_SSIZE_T_CLEAN
 #include <Python.h>
 #include <array>
+#include <cerrno>
 #include <cmath>
 #include <cstdio>
 #include <cstdint>
@@ -9,6 +10,12 @@
 #include <string>
 #include <vector>

+#include <arpa/inet.h>
+#include <fcntl.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
 namespace {

 constexpr Py_ssize_t LOWCMD_SIZE = 616;
@@ -460,6 +467,31 @@ struct FastLowCmdBuilder {
    uint32_t S[4][256];
 };

+bool load_blowfish_state(const char* state_path, uint32_t P[18], uint32_t S[4][256]) {
+    std::ifstream f(state_path, std::ios::binary);
+    if (!f) {
+        PyErr_Format(PyExc_FileNotFoundError, "cannot open Blowfish state file: %s", state_path);
+        return false;
+    }
+    std::array<uint8_t, 4168> buf{};
+    f.read(reinterpret_cast<char*>(buf.data()), static_cast<std::streamsize>(buf.size()));
+    if (f.gcount() < static_cast<std::streamsize>(buf.size())) {
+        PyErr_Format(PyExc_ValueError, "Blowfish state file must contain at least 4168 bytes: %s", state_path);
+        return false;
+    }
+    const uint8_t* p = buf.data();
+    for (int i = 0; i < 18; ++i) {
+        P[i] = get_u32_le(p + i * 4);
+    }
+    p += 18 * 4;
+    for (int s = 0; s < 4; ++s) {
+        for (int i = 0; i < 256; ++i) {
+            S[s][i] = get_u32_le(p + (s * 256 + i) * 4);
+        }
+    }
+    return true;
+}
+
 uint32_t bf_f(FastLowCmdBuilder* self, uint32_t x) {
    return (((self->S[0][(x >> 24) & 0xffu] + self->S[1][(x >> 16) & 0xffu]) & 0xffffffffu)
            ^ self->S[2][(x >> 8) & 0xffu]) + self->S[3][x & 0xffu];
@@ -623,28 +655,7 @@ int FastLowCmdBuilder_init(FastLowCmdBuilder* self, PyObject* args, PyObject* kw
        return -1;
    }

-    std::ifstream f(state_path, std::ios::binary);
-    if (!f) {
-        PyErr_Format(PyExc_FileNotFoundError, "cannot open Blowfish state file: %s", state_path);
-        return -1;
-    }
-    std::array<uint8_t, 4168> buf{};
-    f.read(reinterpret_cast<char*>(buf.data()), static_cast<std::streamsize>(buf.size()));
-    if (f.gcount() < static_cast<std::streamsize>(buf.size())) {
-        PyErr_Format(PyExc_ValueError, "Blowfish state file must contain at least 4168 bytes: %s", state_path);
-        return -1;
-    }
-    const uint8_t* p = buf.data();
-    for (int i = 0; i < 18; ++i) {
-        self->P[i] = get_u32_le(p + i * 4);
-    }
-    p += 18 * 4;
-    for (int s = 0; s < 4; ++s) {
-        for (int i = 0; i < 256; ++i) {
-            self->S[s][i] = get_u32_le(p + (s * 256 + i) * 4);
-        }
-    }
-    return 0;
+    return load_blowfish_state(state_path, self->P, self->S) ? 0 : -1;
 }

 PyObject* FastLowCmdBuilder_build_plain_damping(FastLowCmdBuilder*, PyObject*) {
@@ -865,27 +876,19 @@ PyObject* FastLowCmdBuilder_encrypt(FastLowCmdBuilder* self, PyObject* args) {
    return result;
 }

-PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject* args) {
-    Py_buffer view;
-    if (!PyArg_ParseTuple(args, "y*", &view)) {
-        return nullptr;
-    }
-    if (view.len < 807) {
-        PyBuffer_Release(&view);
+PyObject* parse_lowstate_packet(FastLowCmdBuilder* self, const uint8_t* cipher, Py_ssize_t len) {
+    if (len < 807) {
        Py_RETURN_NONE;
    }
-    Py_ssize_t aligned = (view.len / 8) * 8;
+    Py_ssize_t aligned = (len / 8) * 8;
    if (aligned < 807) {
-        PyBuffer_Release(&view);
        Py_RETURN_NONE;
    }

-    const uint8_t* cipher = reinterpret_cast<const uint8_t*>(view.buf);
    std::vector<uint8_t> data(static_cast<size_t>(aligned));
    for (Py_ssize_t i = 0; i < aligned; i += 8) {
        decrypt_block_le(self, cipher + i, data.data() + i);
    }
-    PyBuffer_Release(&view);

    if (data[0] != 0xfe || data[1] != 0xef || data[2] != 0xff || data[3] != 0x00) {
        Py_RETURN_NONE;
@@ -983,6 +986,207 @@ PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject*
    return out;
 }

+PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject* args) {
+    Py_buffer view;
+    if (!PyArg_ParseTuple(args, "y*", &view)) {
+        return nullptr;
+    }
+    PyObject* result = parse_lowstate_packet(
+        self, reinterpret_cast<const uint8_t*>(view.buf), view.len);
+    PyBuffer_Release(&view);
+    return result;
+}
+
+struct FastNativeMCUClient {
+    PyObject_HEAD
+    int sock_fd;
+    int local_port;
+    FastLowCmdBuilder codec;
+};
+
+void close_native_socket(FastNativeMCUClient* self) {
+    if (self->sock_fd >= 0) {
+        close(self->sock_fd);
+        self->sock_fd = -1;
+    }
+}
+
+PyObject* FastNativeMCUClient_new(PyTypeObject* type, PyObject*, PyObject*) {
+    auto* self = reinterpret_cast<FastNativeMCUClient*>(type->tp_alloc(type, 0));
+    if (self) {
+        self->sock_fd = -1;
+        self->local_port = 0;
+        std::memset(&self->codec, 0, sizeof(self->codec));
+    }
+    return reinterpret_cast<PyObject*>(self);
+}
+
+void FastNativeMCUClient_dealloc(FastNativeMCUClient* self) {
+    close_native_socket(self);
+    Py_TYPE(self)->tp_free(reinterpret_cast<PyObject*>(self));
+}
+
+int set_nonblocking(int fd) {
+    int flags = fcntl(fd, F_GETFL, 0);
+    if (flags < 0) {
+        return -1;
+    }
+    return fcntl(fd, F_SETFL, flags | O_NONBLOCK);
+}
+
+int FastNativeMCUClient_init(FastNativeMCUClient* self, PyObject* args, PyObject* kwargs) {
+    const char* state_path = nullptr;
+    const char* mcu_ip = "192.168.123.10";
+    const char* endian = "little";
+    int mcu_port = 8007;
+    int local_port = 0;
+    int rcvbuf_size = 4096;
+    static const char* kwlist[] = {
+        "state_path", "mcu_ip", "mcu_port", "local_port", "rcvbuf_size", "endian", nullptr};
+    if (!PyArg_ParseTupleAndKeywords(
+            args, kwargs, "s|siiis", const_cast<char**>(kwlist),
+            &state_path, &mcu_ip, &mcu_port, &local_port, &rcvbuf_size, &endian)) {
+        return -1;
+    }
+    if (std::strcmp(endian, "little") != 0) {
+        PyErr_SetString(PyExc_ValueError, "FastNativeMCUClient currently supports only little-endian Blowfish state");
+        return -1;
+    }
+    close_native_socket(self);
+    if (!load_blowfish_state(state_path, self->codec.P, self->codec.S)) {
+        return -1;
+    }
+
+    self->sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
+    if (self->sock_fd < 0) {
+        PyErr_SetFromErrno(PyExc_OSError);
+        return -1;
+    }
+    int reuse = 1;
+    setsockopt(self->sock_fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
+    if (rcvbuf_size > 0) {
+        setsockopt(self->sock_fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf_size, sizeof(rcvbuf_size));
+    }
+    if (set_nonblocking(self->sock_fd) < 0) {
+        PyErr_SetFromErrno(PyExc_OSError);
+        close_native_socket(self);
+        return -1;
+    }
+
+    sockaddr_in local{};
+    local.sin_family = AF_INET;
+    local.sin_addr.s_addr = htonl(INADDR_ANY);
+    local.sin_port = htons(static_cast<uint16_t>(local_port));
+    if (bind(self->sock_fd, reinterpret_cast<sockaddr*>(&local), sizeof(local)) < 0) {
+        PyErr_SetFromErrno(PyExc_OSError);
+        close_native_socket(self);
+        return -1;
+    }
+
+    sockaddr_in target{};
+    target.sin_family = AF_INET;
+    target.sin_port = htons(static_cast<uint16_t>(mcu_port));
+    if (inet_pton(AF_INET, mcu_ip, &target.sin_addr) != 1) {
+        PyErr_Format(PyExc_ValueError, "invalid IPv4 target: %s", mcu_ip);
+        close_native_socket(self);
+        return -1;
+    }
+    if (connect(self->sock_fd, reinterpret_cast<sockaddr*>(&target), sizeof(target)) < 0) {
+        PyErr_SetFromErrno(PyExc_OSError);
+        close_native_socket(self);
+        return -1;
+    }
+
+    sockaddr_in actual{};
+    socklen_t actual_len = sizeof(actual);
+    if (getsockname(self->sock_fd, reinterpret_cast<sockaddr*>(&actual), &actual_len) == 0) {
+        self->local_port = ntohs(actual.sin_port);
+    } else {
+        self->local_port = local_port;
+    }
+    return 0;
+}
+
+PyObject* FastNativeMCUClient_close(FastNativeMCUClient* self, PyObject*) {
+    close_native_socket(self);
+    Py_RETURN_NONE;
+}
+
+PyObject* FastNativeMCUClient_local_port(FastNativeMCUClient* self, PyObject*) {
+    return PyLong_FromLong(self->local_port);
+}
+
+PyObject* FastNativeMCUClient_send_raw(FastNativeMCUClient* self, PyObject* args) {
+    if (self->sock_fd < 0) {
+        PyErr_SetString(PyExc_RuntimeError, "FastNativeMCUClient is closed");
+        return nullptr;
+    }
+    Py_buffer view;
+    if (!PyArg_ParseTuple(args, "y*", &view)) {
+        return nullptr;
+    }
+    ssize_t sent = 0;
+    Py_BEGIN_ALLOW_THREADS
+    sent = send(self->sock_fd, view.buf, static_cast<size_t>(view.len), 0);
+    Py_END_ALLOW_THREADS
+    PyBuffer_Release(&view);
+    if (sent < 0) {
+        PyErr_SetFromErrno(PyExc_OSError);
+        return nullptr;
+    }
+    return PyLong_FromSsize_t(sent);
+}
+
+PyObject* FastNativeMCUClient_recv_latest_fields(FastNativeMCUClient* self, PyObject*) {
+    if (self->sock_fd < 0) {
+        PyErr_SetString(PyExc_RuntimeError, "FastNativeMCUClient is closed");
+        return nullptr;
+    }
+
+    std::array<uint8_t, 2048> buffer{};
+    std::array<uint8_t, 2048> latest{};
+    ssize_t latest_len = -1;
+    while (true) {
+        ssize_t received = 0;
+        Py_BEGIN_ALLOW_THREADS
+        received = recv(self->sock_fd, buffer.data(), buffer.size(), 0);
+        Py_END_ALLOW_THREADS
+        if (received > 0) {
+            latest_len = received;
+            std::memcpy(latest.data(), buffer.data(), static_cast<size_t>(received));
+            continue;
+        }
+        if (received == 0) {
+            break;
+        }
+        if (errno == EINTR) {
+            continue;
+        }
+        if (errno == EAGAIN || errno == EWOULDBLOCK) {
+            break;
+        }
+        PyErr_SetFromErrno(PyExc_OSError);
+        return nullptr;
+    }
+
+    if (latest_len < 0) {
+        Py_RETURN_NONE;
+    }
+    return parse_lowstate_packet(&self->codec, latest.data(), latest_len);
+}
+
+PyMethodDef FastNativeMCUClient_methods[] = {
+    {"close", reinterpret_cast<PyCFunction>(FastNativeMCUClient_close), METH_NOARGS,
+     "Close the native UDP socket."},
+    {"local_port", reinterpret_cast<PyCFunction>(FastNativeMCUClient_local_port), METH_NOARGS,
+     "Return the bound local UDP port."},
+    {"send_raw", reinterpret_cast<PyCFunction>(FastNativeMCUClient_send_raw), METH_VARARGS,
+     "Send an already encrypted LowCmd packet through the native UDP socket."},
+    {"recv_latest_fields", reinterpret_cast<PyCFunction>(FastNativeMCUClient_recv_latest_fields), METH_NOARGS,
+     "Drain pending UDP packets and return parsed fields for the newest LowState packet."},
+    {nullptr, nullptr, 0, nullptr},
+};
+
 PyMethodDef FastLowCmdBuilder_methods[] = {
    {"build_plain_damping", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_damping), METH_NOARGS,
     "Build a plain all-damping LowCmd packet."},
@@ -1015,6 +1219,10 @@ PyTypeObject FastLowCmdBuilderType = {
    PyVarObject_HEAD_INIT(nullptr, 0)
 };

+PyTypeObject FastNativeMCUClientType = {
+    PyVarObject_HEAD_INIT(nullptr, 0)
+};
+
 PyModuleDef module = {
    PyModuleDef_HEAD_INIT,
    "go1_fast_lowcmd",
@@ -1038,6 +1246,19 @@ PyMODINIT_FUNC PyInit_go1_fast_lowcmd(void) {
        return nullptr;
    }

+    FastNativeMCUClientType.tp_name = "go1_fast_lowcmd.FastNativeMCUClient";
+    FastNativeMCUClientType.tp_basicsize = sizeof(FastNativeMCUClient);
+    FastNativeMCUClientType.tp_flags = Py_TPFLAGS_DEFAULT;
+    FastNativeMCUClientType.tp_doc = "Native UDP MCU client with C++ LowState drain/decrypt/parse.";
+    FastNativeMCUClientType.tp_new = FastNativeMCUClient_new;
+    FastNativeMCUClientType.tp_init = reinterpret_cast<initproc>(FastNativeMCUClient_init);
+    FastNativeMCUClientType.tp_dealloc = reinterpret_cast<destructor>(FastNativeMCUClient_dealloc);
+    FastNativeMCUClientType.tp_methods = FastNativeMCUClient_methods;
+
+    if (PyType_Ready(&FastNativeMCUClientType) < 0) {
+        return nullptr;
+    }
+
    PyObject* m = PyModule_Create(&module);
    if (!m) {
        return nullptr;
@@ -1048,6 +1269,12 @@ PyMODINIT_FUNC PyInit_go1_fast_lowcmd(void) {
        Py_DECREF(m);
        return nullptr;
    }
+    Py_INCREF(&FastNativeMCUClientType);
+    if (PyModule_AddObject(m, "FastNativeMCUClient", reinterpret_cast<PyObject*>(&FastNativeMCUClientType)) < 0) {
+        Py_DECREF(&FastNativeMCUClientType);
+        Py_DECREF(m);
+        return nullptr;
+    }
    PyModule_AddIntConstant(m, "LOWCMD_SIZE", LOWCMD_SIZE);
    return m;
 }