From cb13925433af4bf1a7cf3b53657c5db51d7a09cf Mon Sep 17 00:00:00 2001 From: Niellun Date: Mon, 2 Jun 2025 13:54:15 +0300 Subject: [PATCH] Better decoder, minor refactoring --- decoder.py | 180 +++++++++++++++++++++++++++++++++----- src/connector.cpp | 30 +++---- src/connector.h | 24 ++--- src/decoder.cpp | 6 +- src/helper/iprotocol.h | 14 --- src/pcm_audio.cpp | 3 +- src/protocol.cpp | 41 +++++---- src/protocol.h | 14 ++- src/struct/atomic_queue.h | 3 +- 9 files changed, 210 insertions(+), 105 deletions(-) delete mode 100644 src/helper/iprotocol.h diff --git a/decoder.py b/decoder.py index d60213b..c0981d2 100644 --- a/decoder.py +++ b/decoder.py @@ -2,6 +2,7 @@ from Crypto.Cipher import AES import struct import json + inEnc = False inSize = 0 inCmd = 0 @@ -14,21 +15,144 @@ outSet = False encKey = 0 -def build_iv_le(key): +###### PROTOCOL + +def printKey(bytes, len): + if len == 4: + return "Key "+printInts(bytes, len) + if len == 0: + return "" + return printInts(bytes, len) + +def printInts(bytes, len, max = 10, start=0): + max = 10 + count = min((len-start) // 4, max) + ints = [ + str(int.from_bytes(bytes[start+i*4:start+(i+1)*4], byteorder='little', signed=False)) + for i in range(count) + ] + result = ' '.join(ints) + if (len-start) // 4 > max: + result += ' ...' + return result + +def printBytes(bytes, len, max=20, start=0): + max = 20 + count = min(len-start, max) + if count<1: + return "_" + result = f"{' '.join(f'{b:02x}' for b in bytes[start:start+count])}" + if count>max: + result+= " ..." + return result + +def stringOrFunc(bytes, len, func, start=0): + result = [] + for b in bytes[start:start + len]: + if 32 <= b <= 126: + result.append(chr(b)) + else: + return func(bytes, len, start=start) + return ''.join(result) + +def printStr0(bytes, len, start=0): + return ''.join((chr(b) if (32 <= b <= 126 or b == 0) else '.') for b in bytes[start:start + len]) + +def printFile(bytes, len): + len1 = int.from_bytes(bytes[0:4], 'little') + result = printStr0(bytes, len1, 4) + len2 = int.from_bytes(bytes[4+len1:8+len1], 'little') + if len2 % 4 == 0: + result = result+":"+stringOrFunc(bytes, len, printInts, start=len1+8) + else: + result = result+":"+stringOrFunc(bytes, len, printBytes, start=len1+8) + return result + +def cmd8(bytes, len): + if len != 4: + return printBytes(bytes, len) + cmd = int.from_bytes(bytes[0:4], 'little') + res = f"[{cmd}] " + match cmd: + case 1: return res+"Start audio recording" + case 2: return res+"Stop audio recording" + case 7: return res+"Mic type car" + case 15: return res+"Mic type dongle" + case 21: return res+"Mic type phone" + case 24: return res+"Wifi 2.4Ghz" + case 25: return res+"Wifi 5Ghz" + case 22: return res+"Audio transfer bluetooth" + case 23: return res+"Audio transfer dongle" + case 500: return res+"Request video focus" + case 501: return res+"Release video focus" + case 1003: return res+"Scanning devices" + case 1004: return res+"Device found" + case 1005: return res+"Device not found" + case 1006: return res+"Connect device failed" + case 1007: return res+"Dongle bluetooth connected" + case 1008: return res+"Dongle bluetooth disconnected" + case 1009: return res+"Dongle wifi connected" + case 1010: return res+"Dongle wifi disconnected" + case 1011: return res+"Bluetooth pairing started" + + return res+"" + + + + +def getCmd(id): + match id: + case 1: return "Open", printInts + case 2: return "Plugged", printInts + case 3: return "State", None + case 4: return "Unplugged", printInts + case 5: return "Touch", None + case 6: return "Video", printInts + case 7: return "Audio", printInts + case 8: return "Control", cmd8 + case 10: return "App info", None + case 13: return "Bt info", printStr0 + case 14: return "Wifi info", printStr0 + case 15: return "Disconnect", None + case 18: return "Devices", printStr0 + case 20: return "Mfr name", None + case 22: return "Camera", None + case 25: return "Json Ctl", None + case 42: return "Media inf", None + case 136: return "App log", None + case 153: return "Send file", printFile + case 162: return "Daynight", None + case 170: return "Heartbeat", None + case 204: return "Version", printStr0 + case 206: return "Reboot", None + case 240: return "Encrypt", printInts + return "", None + +###### ENCRYPTION + +def build_iv_le(f5604m): iv = bytearray(16) - iv[1] = (key >> 0) & 0xFF - iv[4] = (key >> 8) & 0xFF - iv[9] = (key >> 16) & 0xFF - iv[12] = (key >> 24) & 0xFF + iv[1] = (f5604m >> 0) & 0xFF + iv[4] = (f5604m >> 8) & 0xFF + iv[9] = (f5604m >> 16) & 0xFF + iv[12] = (f5604m >> 24) & 0xFF return bytes(iv) -def generate_key(key): +def generate_key(f5604m): base = "SkBRDy3gmrw1ieH0" key_bytes = bytearray(16) for i in range(16): - key_bytes[i] = ord(base[(key + i) % 16]) + key_bytes[i] = ord(base[(f5604m + i) % 16]) return bytes(key_bytes) +def decrypt_hex_ciphertext(hex_ciphertext, intkey, iv_builder): + key = generate_key(intkey) + iv = iv_builder(intkey) + cipher = AES.new(key, AES.MODE_CFB, iv=iv, segment_size=128) + ciphertext = bytes.fromhex(hex_ciphertext) + plaintext = cipher.decrypt(ciphertext) + return plaintext + def decrypt_hex_cipher(ciphertext, intkey, iv_builder): key = generate_key(intkey) iv = iv_builder(intkey) @@ -36,20 +160,26 @@ def decrypt_hex_cipher(ciphertext, intkey, iv_builder): plaintext = cipher.decrypt(ciphertext) return plaintext +###### HELPERS + def bytes_to_printable_ascii(byte_data): return ''.join((chr(b) if 32 <= b <= 126 else '.') for b in byte_data) def bytes_to_int_list_le(byte_data): + # Pad if length is not a multiple of 4 length = len(byte_data) padded_length = (length + 3) // 4 * 4 padded = byte_data.ljust(padded_length, b'\x00') + # Unpack as little-endian unsigned ints return list(struct.unpack('<' + 'I' * (padded_length // 4), padded)) +###### DECODING + def check_types(capdata: bytes) -> tuple[bool, bool]: is_aa = capdata.startswith(bytes.fromhex('aa55aa55')) is_bb = capdata.startswith(bytes.fromhex('bb55bb55')) if is_aa or is_bb: - if len(capdata) >= 12: + if len(capdata) >= 12: # 4 bytes prefix + 8 bytes data size = int.from_bytes(capdata[4:8], 'little') cmd = int.from_bytes(capdata[8:12], 'little') return True, is_bb, size, cmd @@ -57,26 +187,32 @@ def check_types(capdata: bytes) -> tuple[bool, bool]: def processPacket(frame, out, capdata, cmd, size, enc): global encKey + if enc and encKey != 0 and size > 0: capdata = decrypt_hex_cipher(capdata, encKey, build_iv_le) + if out and cmd == 240 and size == 4 and len(capdata) >= 4: encKey = int.from_bytes(capdata[0:4], 'little') - if cmd == 170: - return - res = f"{frame:>5}{cmd:>5} {'<' if not out else '>'}{' ' if not enc else '*'} {size:<5} : " + + #if cmd == 170 or cmd == 7 or cmd == 5 or cmd == 6: + # return + + cmdName, cmdProc = getCmd(cmd) + cmdstr = f"{cmdName} [{cmd}]" + res = f"{frame:>5} {cmdstr:>15} {'>' if not out else '<'}{' ' if not enc else '*'} " str = "" - if len(capdata)<1000: - str = bytes_to_printable_ascii(capdata) + if cmdProc: + str = cmdProc(capdata, size) else: - bts = "" - for i in range(0, min(len(capdata), 20), 4): - chunk = capdata[i:i+4] - val = int.from_bytes(chunk, 'little') - bts = bts+f"{val} " - if size == 4: - str = int.from_bytes(capdata[0:4], 'little') + if len(capdata)<1000: + str = stringOrFunc(capdata, size, printBytes) + else: + str = f"{' '.join(f'{b:02x}' for b in capdata[:40])}" + + if size == 0: + str = "_" + print(f"{'\033[92m' if not out else '\033[93m'}{res}{str}\033[0m") - print(f" {' '.join(f'{b:02x}' for b in capdata[:20])}") def processInc(frame, capdata): @@ -111,7 +247,6 @@ def processOut(frame, capdata): processPacket(frame, True, capdata, outCmd, outSize, outEnc) outSet = header - def processFile(file_path): with open(file_path) as f: data = json.load(f) @@ -131,6 +266,7 @@ def processFile(file_path): if(direction == "1" and urb_type == "'C'"): processInc(frame_number, byte_array) + print("This tool decode carlinkit protocol") print("It requred JSON export of USB events filtered by device") file_path = input("Path to JSON export from PCAPNG: ").strip().strip("'") diff --git a/src/connector.cpp b/src/connector.cpp index 252bb1a..22234d2 100644 --- a/src/connector.cpp +++ b/src/connector.cpp @@ -51,15 +51,13 @@ Connector::~Connector() } } -void Connector::start(IProtocol *protocol) +void Connector::start() { - _protocol = protocol; - if (_active) stop(); _active = true; - _write_thread = std::thread(&Connector::write_loop, this); + _write_thread = std::thread(&Connector::writeLoop, this); } void Connector::stop() @@ -166,8 +164,8 @@ bool Connector::nextState(u_int8_t state) void Connector::state(u_int8_t state) { - if (nextState(state) && _protocol) - _protocol->onStatus(state); + if (nextState(state)) + onStatus(state); } bool Connector::linkFail(int status, const char *msg) @@ -312,7 +310,7 @@ void Connector::printMessage(uint32_t cmd, uint32_t length, uint8_t *data, bool std::cout << oss.str() << std::endl; } -void Connector::read_loop() +void Connector::readLoop() { std::mutex mtx; std::condition_variable cv; @@ -351,12 +349,6 @@ void Connector::read_loop() libusb_bulk_transfer(_device, _endpoint_in, data, header.length, &transferred, READ_TIMEOUT); } - if (!_protocol) - { - free(data); - continue; - } - if (header.magic == MAGIC_ENC) { if (!_cipher) @@ -377,11 +369,11 @@ void Connector::read_loop() if (padding > 0) std::fill(data + header.length, data + header.length + padding, 0); - _protocol->onData(header.type, header.length, data); + onData(header.type, header.length, data); } } -void Connector::write_loop() +void Connector::writeLoop() { std::mutex mtx; std::condition_variable cv; @@ -397,9 +389,8 @@ void Connector::write_loop() { std::cout << "[Connection] Device connected" << std::endl; - _read_thread = std::thread(&Connector::read_loop, this); - if (_protocol) - _protocol->onDevice(true); + _read_thread = std::thread(&Connector::readLoop, this); + onDevice(true); state(PROTOCOL_STATUS_ONLINE); while (_connected && _active) @@ -413,8 +404,7 @@ void Connector::write_loop() if (_active) { state(PROTOCOL_STATUS_NO_DEVICE); - if (_protocol) - _protocol->onDevice(false); + onDevice(false); } if (_read_thread.joinable()) diff --git a/src/connector.h b/src/connector.h index 4cf1aca..05d3af1 100644 --- a/src/connector.h +++ b/src/connector.h @@ -8,7 +8,6 @@ #include #include -#include "helper/iprotocol.h" #include "aes_cipher.h" #define READ_TIMEOUT 3000 @@ -35,24 +34,30 @@ class Connector public: Connector(uint16_t videoPadding); - ~Connector(); + virtual ~Connector(); - void start(IProtocol *protocol); + void start(); void stop(); - int send(int cmd, bool encrypt = true, uint8_t *data = nullptr, uint32_t size = 0); - void setEncryption(bool enabled); +protected: - AESCipher *Cypher() const { return _cipher; }; + int send(int cmd, bool encrypt = true, uint8_t *data = nullptr, uint32_t size = 0); + virtual void onData(uint32_t cmd, uint32_t length, uint8_t *data) = 0; + virtual void onStatus(u_int8_t status) = 0; + virtual void onDevice(bool connected) = 0; + + void setEncryption(bool enabled); static void printMessage(uint32_t cmd, uint32_t length, uint8_t *data, bool encrypted, bool out); static void printInts(uint8_t *data, uint32_t length, uint16_t max); static void printBytes(uint8_t *data, uint32_t length, uint16_t max); static const char *cmdString(int cmd); + AESCipher *_cipher = nullptr; + private: - void read_loop(); - void write_loop(); + void readLoop(); + void writeLoop(); bool connect(uint16_t vendor_id, uint16_t product_id); bool link(); @@ -80,9 +85,6 @@ private: std::atomic _active = false; u_int16_t _videoPadding; - - IProtocol *_protocol = nullptr; - AESCipher *_cipher = nullptr; }; #endif /* SRC_CONNECTOR */ diff --git a/src/decoder.cpp b/src/decoder.cpp index 71ab1bd..b846eb9 100644 --- a/src/decoder.cpp +++ b/src/decoder.cpp @@ -143,8 +143,8 @@ void Decoder::loop(AVCodecContext *context, AVCodecParserContext *parser, AVPack uint32_t counter = 0; // Main decoding loop; runs until global_quit flag is set - _data->wait(_active); - while (_active) +; + while (_data->wait(_active)) { // Get raw data segment from queue std::unique_ptr segment = _data->pop(); @@ -196,7 +196,5 @@ void Decoder::loop(AVCodecContext *context, AVCodecParserContext *parser, AVPack _vb->commit(); } } - - _data->wait(_active); } } diff --git a/src/helper/iprotocol.h b/src/helper/iprotocol.h deleted file mode 100644 index 92d00de..0000000 --- a/src/helper/iprotocol.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef SRC_HELPER_IPROTOCOL -#define SRC_HELPER_IPROTOCOL - -#include -#include - -class IProtocol -{ -public: - virtual void onData(uint32_t cmd, uint32_t length, uint8_t *data) = 0; - virtual void onStatus(u_int8_t status) = 0; - virtual void onDevice(bool connected) = 0; -}; -#endif /* SRC_HELPER_IPROTOCOL */ diff --git a/src/pcm_audio.cpp b/src/pcm_audio.cpp index 1642d38..b38f564 100644 --- a/src/pcm_audio.cpp +++ b/src/pcm_audio.cpp @@ -149,9 +149,8 @@ void PcmAudio::runner() SDL_AudioDeviceID device = 0; SDL_AudioSpec spec; - while (_active) + while (_data->wait(_active)) { - _data->wait(_active, Settings::audioDelay); const Message *segment = _data->peek(); if (!segment) continue; diff --git a/src/protocol.cpp b/src/protocol.cpp index a00242e..b07e9fe 100644 --- a/src/protocol.cpp +++ b/src/protocol.cpp @@ -7,32 +7,32 @@ #include Protocol::Protocol(uint16_t width, uint16_t height, uint16_t fps, uint16_t padding) - : connector(padding), + : Connector(padding), videoData(Settings::videoQueue), audioStreamMain(Settings::audioQueue), audioStreamAux(Settings::audioQueue), - phoneConnected(false), _width(width), _height(height), - _fps(fps) + _fps(fps), + _phoneConnected(false) + { } Protocol::~Protocol() { - stop(); } void Protocol::start(uint32_t evtStatus, uint32_t evtPhone) { _evtStatusId = evtStatus; _evtPhoneId = evtPhone; - connector.start(this); + Connector::start(); } void Protocol::stop() { - connector.stop(); + Connector::stop(); } void Protocol::sendInit(int width, int height, int fps) @@ -46,7 +46,7 @@ void Protocol::sendInit(int width, int height, int fps) write_uint32_le(&buf[20], 2); write_uint32_le(&buf[24], 2); - connector.send(1, true, buf, 28); + send(CMD_OPEN, true, buf, 28); } void Protocol::sendConfig() @@ -118,7 +118,7 @@ void Protocol::sendKey(int key) uint8_t buf[4]; write_uint32_le(&buf[0], key); - connector.send(8, false, buf, 4); + send(CMD_CONTROL, false, buf, 4); } void Protocol::sendFile(const char *filename, const char *value) @@ -150,7 +150,7 @@ void Protocol::sendClick(float x, float y, bool down) write_uint32_le(buf + 4, int(10000 * x)); write_uint32_le(buf + 8, int(10000 * y)); write_uint32_le(buf + 12, 0); - connector.send(5, false, buf, 16); + send(CMD_TOUCH, false, buf, 16); } void Protocol::sendMove(float dx, float dy) @@ -160,7 +160,7 @@ void Protocol::sendMove(float dx, float dy) write_uint32_le(buf + 4, int(10000 * dx)); write_uint32_le(buf + 8, int(10000 * dy)); write_uint32_le(buf + 12, 0); - connector.send(5, false, buf, 16); + send(CMD_TOUCH, false, buf, 16); } void Protocol::sendFile(const char *filename, const uint8_t *data, uint32_t length) @@ -188,33 +188,32 @@ void Protocol::sendFile(const char *filename, const uint8_t *data, uint32_t leng // 4) content bytes std::memcpy(buf, data, length); - connector.send(CMD_SEND_FILE, true, result.data(), total); + send(CMD_SEND_FILE, true, result.data(), total); } void Protocol::sendInt(uint32_t cmd, uint32_t value, bool encryption) { uint8_t buf[4]; write_uint32_le(buf, value); - connector.send(cmd, encryption, buf, 4); + send(cmd, encryption, buf, 4); } void Protocol::sendString(uint32_t cmd, char *str, bool encryption) { uint32_t total = strlen(str); - connector.send(cmd, true, (uint8_t *)str, total); + send(cmd, true, (uint8_t *)str, total); } void Protocol::sendEncryption() { - AESCipher *cypher = connector.Cypher(); - if (!cypher) + if (!_cipher) { std::cout << "[Protocol] Can't enable encryption: cypher is not initalised"; return; } uint8_t buf[4]; - write_uint32_le(buf, cypher->Seed()); - connector.send(CMD_ENCRYPTION, false, buf, 4); + write_uint32_le(buf, _cipher->Seed()); + send(CMD_ENCRYPTION, false, buf, 4); } void Protocol::onStatus(uint8_t status) @@ -237,15 +236,15 @@ void Protocol::onDevice(bool connected) else { onPhone(false); - connector.setEncryption(false); + setEncryption(false); } } void Protocol::onPhone(bool connected) { - if (connected == phoneConnected) + if (connected == _phoneConnected) return; - phoneConnected = connected; + _phoneConnected = connected; std::cout << (connected ? "[Protocol] Phone connected" : "[Protocol] Phone disconnected") << std::endl; @@ -303,7 +302,7 @@ void Protocol::onData(uint32_t cmd, uint32_t length, uint8_t *data) } case CMD_ENCRYPTION: if (length == 0) - connector.setEncryption(true); + setEncryption(true); break; } diff --git a/src/protocol.h b/src/protocol.h index 880fae2..1199d7d 100644 --- a/src/protocol.h +++ b/src/protocol.h @@ -3,22 +3,20 @@ #include "struct/atomic_queue.h" #include "struct/message.h" -#include "helper/iprotocol.h" + #include "settings.h" #include "connector.h" -class Protocol : public IProtocol +class Protocol : private Connector { public: Protocol(uint16_t width, uint16_t height, uint16_t fps, uint16_t padding); - ~Protocol(); + ~Protocol() override; Protocol(const Protocol &) = delete; Protocol &operator=(const Protocol &) = delete; - static const char *cmdString(int cmd); - void start(uint32_t evtStatus, uint32_t evtPhone); void stop(); @@ -29,11 +27,9 @@ public: void sendClick(float x, float y, bool down); void sendMove(float dx, float dy); - Connector connector; AtomicQueue videoData; AtomicQueue audioStreamMain; AtomicQueue audioStreamAux; - bool phoneConnected; private: void sendInt(uint32_t cmd, uint32_t value, bool encryption = true); @@ -45,14 +41,14 @@ private: void onStatus(uint8_t status) override; void onDevice(bool connected) override; void onData(uint32_t cmd, uint32_t length, uint8_t *data) override; - void onPhone(bool connected); - bool jsonFind(const char *json, uint16_t length, const char *key, char *value, uint16_t size) const; + static const char *cmdString(int cmd); uint16_t _width; uint16_t _height; uint16_t _fps; + bool _phoneConnected; uint32_t _evtStatusId = (uint32_t)-1; uint32_t _evtPhoneId = (uint32_t)-1; diff --git a/src/struct/atomic_queue.h b/src/struct/atomic_queue.h index f51b575..6ea3d8f 100644 --- a/src/struct/atomic_queue.h +++ b/src/struct/atomic_queue.h @@ -72,10 +72,9 @@ public: { unique_lock lock(_mtx); - bool exit = _count > count; _lock.wait(lock, [&] { return _count > count || !waitFlag; }); - return !exit; + return waitFlag; } void clear()