API tools faq
paste
honey_the_codewitch

esplink wip

honey_the_codewitch
Feb 26th, 2026
83
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 18.55 KB | None | 0 0
raw download clone embed print report
  1. #include <stdio.h>
  2. #include "abi.h"
  3. #include <string.h>
  4. #define WIN32_LEAN_AND_MEAN
  5. #include <Windows.h>
  6. #define FRAME_ESCAPE (0xDB)
  7. #define FRAME_DELIMITER (0xC0)
  8. typedef struct {
  9.     uint8_t* data;
  10.     size_t size;
  11.     size_t capacity;
  12. } arr_t;
  13. typedef struct {
  14.     int32_t timeout_ts;
  15.     arr_t read_frame;
  16.     bool found_start;
  17. } read_frame_state_t;
  18. typedef struct {
  19.     arr_t write_frame;
  20.     int state;
  21.     wchar_t portName[64];
  22.     esplink_disconnected_callback_t disconnected_callback;
  23.     void* disconnected_callback_state;
  24.     esplink_progress_callback_t progress_callback;
  25.     void* progress_callback_state;
  26.     uint8_t* write_buf;          // malloc'd full frame; thread frees after write
  27.     size_t write_buf_length;
  28.     HANDLE port;
  29.     HANDLE exit_event;
  30.     HANDLE exited_event;
  31.     HANDLE write_pending_event;  // auto-reset: WriteFrame signals, thread waits
  32.     HANDLE write_slot_event;     // manual-reset: starts signaled, WriteFrame waits for slot free
  33.     HANDLE thread;
  34. } session_t;
  35. typedef struct {
  36.     int state;
  37.     uint8_t cmd;
  38.     uint32_t len;
  39.     uint32_t crc;
  40. } state_machine_t;
  41. static bool array_ensure(arr_t* arr,size_t slots) {
  42.     if(arr->data==nullptr) {
  43.         if(arr->capacity==0) {
  44.             arr->capacity=slots<16?16:slots;
  45.             arr->size = 0;
  46.         }
  47.         arr->data = (uint8_t*)malloc(arr->capacity);
  48.         if(arr->data==nullptr) {arr->capacity=0; return false;}
  49.         return true;
  50.     }
  51.     size_t cap = arr->capacity;
  52.     while(arr->size+slots>cap) { cap*=2; }    
  53.     uint8_t* p = (uint8_t*)realloc(arr->data,cap);
  54.     if(p==nullptr) {return false;}
  55.     arr->data = p;
  56.     arr->capacity=cap;
  57.     return true;
  58. }
  59. static bool array_append(arr_t* arr, uint8_t data) {
  60.     if(!array_ensure(arr,1)) { return false; }
  61.     arr->data[arr->size++]=data;
  62.     return true;
  63. }
  64. static void array_destroy(arr_t* arr) {
  65.     if(arr->capacity>0 & arr->data!=nullptr) {
  66.         free(arr->data);
  67.     }
  68.     arr->size = 0;
  69.     arr->capacity = 0;
  70.     arr->data = nullptr;
  71. }
  72. static uint32_t crc32(const uint8_t* data, size_t length, uint32_t seed = UINT32_MAX / 3) {
  73.     uint32_t result = seed;
  74.     while (length--) {
  75.         result ^= *data++;
  76.     }
  77.     return result;
  78. }
  79. static bool build_frame(session_t* sess, const uint8_t* payload, size_t payload_length) {
  80.     int count = 0;
  81.     for(int i = 0;i<payload_length;++i)
  82.     {
  83.         uint8_t b = payload[i];
  84.         switch(b)
  85.         {
  86.             case FRAME_ESCAPE:
  87.             case FRAME_DELIMITER:
  88.                 count += 2;
  89.                 break;
  90.             default:
  91.                 ++count;
  92.                 break;
  93.         }
  94.     }
  95.     if(!array_ensure(&sess->write_frame,count)) {
  96.         return false;
  97.     }
  98.     uint8_t* p = sess->write_frame.data;
  99.     *p=FRAME_DELIMITER;
  100.     int j = 1;
  101.     for(int i = 0;i<payload_length;++i)
  102.     {
  103.         uint8_t src = payload[i];
  104.         switch(src)
  105.         {
  106.             case FRAME_ESCAPE:
  107.                 p[j++] = FRAME_ESCAPE;
  108.                 p[j++] = 0xDD;
  109.                 break;
  110.             case FRAME_DELIMITER:
  111.                 p[j++] = FRAME_ESCAPE;
  112.                 p[j++] = 0xDC;
  113.                 break;
  114.             default:
  115.                 p[j++] = src;
  116.                 break;
  117.         }
  118.     }
  119.     p[j] = FRAME_DELIMITER;
  120. }
  121. static double qpc_freq = 0;
  122. static LARGE_INTEGER qpc_start;
  123. static void millis_init() {
  124.    
  125.     LARGE_INTEGER li;
  126.     if(!QueryPerformanceFrequency(&li))
  127.     qpc_freq = double(li.QuadPart)/1000.0;
  128.     QueryPerformanceCounter(&qpc_start);
  129.    
  130. }
  131. static uint32_t millis_get() {
  132.     if(qpc_freq==0) {
  133.         millis_init();
  134.     }
  135.     LARGE_INTEGER li;
  136.     QueryPerformanceCounter(&li);
  137.     return double(li.QuadPart-qpc_start.QuadPart)/qpc_freq;
  138. }
  139.  
  140. static void read_frame_start(read_frame_state_t* state, int32_t timeout) {
  141.     state->read_frame.size = 0;
  142.     uint32_t start_ms = millis_get();
  143.     state->timeout_ts = timeout>-1?start_ms + timeout:-1;
  144.     state->found_start = false;
  145. }
  146. static char read_frame_next(read_frame_state_t* state,uint8_t data) {
  147.     if(state->timeout_ts>0 && millis_get()>=state->timeout_ts) {
  148.         return -1;
  149.     }
  150.     if(!state->found_start)
  151.     {
  152.         if(data == FRAME_DELIMITER)
  153.         {
  154.             state->found_start = true;
  155.         }
  156.         return 1;
  157.     } else
  158.     {
  159.         if(data==FRAME_DELIMITER) {
  160.             return 0;
  161.         }
  162.         if (state->read_frame.size>0 && state->read_frame.data[state->read_frame.size-1]==FRAME_ESCAPE)
  163.         {
  164.             switch (data)
  165. {
  166.                 case 0xDD:
  167.                     state->read_frame.data[state->read_frame.size-1]=FRAME_ESCAPE;
  168.                     break;
  169.                 case 0xDC:
  170.                     state->read_frame.data[state->read_frame.size-1]=FRAME_DELIMITER;
  171.                     break;
  172.                 default:
  173.                     // Invalid escape content in frame
  174.                     return -1;
  175.             }
  176.             return 1;
  177.         }
  178.         if(!array_append(&state->read_frame,data)) {
  179.             return -1;
  180.         }
  181.     }
  182. }
  183.  
  184. static bool do_write(HANDLE port, OVERLAPPED* ov, HANDLE exit_event, const uint8_t* data, size_t length) {
  185.     if(length==0) return true;
  186.     HANDLE handles[] { exit_event, ov->hEvent };
  187.     DWORD written;
  188.     if(WriteFile(port,data,(DWORD)length,&written,ov)) {
  189.         ResetEvent(ov->hEvent);
  190.         return true;
  191.     }
  192.     DWORD err = GetLastError();
  193.     if(err!=ERROR_IO_PENDING) {
  194.         puts("ERROR: write error");
  195.         return false;
  196.     }
  197.     DWORD wait = WaitForMultipleObjects(2,handles,FALSE,INFINITE);
  198.     ResetEvent(ov->hEvent);
  199.     if(wait==WAIT_OBJECT_0) return false; // exit signalled
  200.     if(wait!=WAIT_OBJECT_0+1) return false;
  201.     return GetOverlappedResult(port,ov,&written,FALSE)!=0;
  202. }
  203. static void deinit_session(session_t* sess) {
  204.     CloseHandle(sess->write_pending_event);
  205.     CloseHandle(sess->write_slot_event);
  206.     CloseHandle(sess->exited_event);
  207.     CloseHandle(sess->exit_event);
  208.     CloseHandle(sess->port);
  209.     delete sess;
  210. }
  211. static ULONG STDMETHODCALLTYPE SessionThread(LPVOID threadArg) {
  212.     session_t* sess = (session_t*)threadArg;
  213.     state_machine_t mach = {0};
  214.     OVERLAPPED o_reader = {0};
  215.     o_reader.hEvent = CreateEvent(nullptr,TRUE,FALSE,nullptr);
  216.     OVERLAPPED o_writer = {0};
  217.     o_writer.hEvent = CreateEvent(nullptr,TRUE,FALSE,nullptr);
  218.     OVERLAPPED o_status = {0};
  219.     o_status.hEvent = CreateEvent(nullptr,TRUE,FALSE,nullptr);
  220.     // handles[0]=exit, handles[1]=status handles[2]=write_pending, handles[3]=read completion
  221.     // Index 2 is only meaningful when read_pending is true; we always wait on
  222.     // all foue so writes are never delayed by a slow/absent read.
  223.     HANDLE handles[] {
  224.         sess->exit_event,
  225.         o_status.hEvent,
  226.         sess->write_pending_event,
  227.         o_reader.hEvent
  228.     };
  229.     read_frame_state_t read_mach = {0};
  230.     bool in_read_frame = false;
  231.     uint8_t peek;
  232.     DWORD bytes_read;
  233.     DWORD event_mask = 0;
  234.     bool need_frame = false;
  235.     bool read_pending = false;
  236.     bool status_pending = false;
  237.     while(sess->state==0) {
  238.         if(!status_pending) {
  239.             if(WaitCommEvent(sess->port,&event_mask,&o_status)) {
  240.                 ResetEvent(o_status.hEvent);
  241.                 if(0!=(event_mask&EV_RLSD)) {
  242.                     sess->state=2;
  243.                     if(sess->disconnected_callback!=nullptr) {
  244.                         sess->disconnected_callback(sess->disconnected_callback_state);
  245.                     }
  246.                     continue;
  247.                 }
  248.             } else {
  249.                 DWORD err = GetLastError();
  250.                 if(err==ERROR_IO_PENDING) {
  251.                     status_pending=true;
  252.                 } else {
  253.                     sess->state = 2;
  254.                     if(sess->disconnected_callback!=nullptr) {
  255.                         sess->disconnected_callback(sess->disconnected_callback_state);
  256.                     }
  257.                     continue;
  258.                 }
  259.             }
  260.         }
  261.         if(need_frame) {
  262.             // Issue a new read if one isn't already in flight
  263.             if(!read_pending) {
  264.                 uint8_t* buf;
  265.                 DWORD bytes_to_read;
  266.                
  267.                 buf = &peek;
  268.                 bytes_to_read = 1;
  269.            
  270.                 DWORD bytes_read_sync;
  271.                 if(ReadFile(sess->port,buf,bytes_to_read,&bytes_read_sync,&o_reader)) {
  272.                     // completed synchronously - process immediately, no wait needed
  273.                     ResetEvent(o_reader.hEvent);
  274.                     if(!in_read_frame) {
  275.                         read_frame_start(&read_mach,1000);
  276.                         in_read_frame = true;
  277.                     }
  278.                     if(0==read_frame_next(&read_mach,peek)) {
  279.                         // TODO: call frame handler
  280.                         in_read_frame = false;
  281.                         need_frame = false;
  282.                     }
  283.                     continue;
  284.                 } else {
  285.                     DWORD err = GetLastError();
  286.                     if(err==ERROR_IO_PENDING) {
  287.                         read_pending = true;
  288.                     } else {
  289.                         puts("ERROR: read error");
  290.                     }
  291.                 }
  292.             }
  293.         }
  294.  
  295.         // Wait: always watch for exit and write requests;
  296.         // only include o_reader.hEvent when a read is actually in flight.
  297.         DWORD nhandles = read_pending ? 4 : 3;
  298.         DWORD wait = WaitForMultipleObjects(nhandles,handles,FALSE,INFINITE);
  299.         if(wait==WAIT_OBJECT_0) {
  300.             sess->state=1;
  301.         } else if(wait==WAIT_OBJECT_0+1) {
  302.             status_pending = false;
  303.             if(GetOverlappedResult(sess->port,&o_reader,&bytes_read,FALSE)) {
  304.                 ResetEvent(o_status.hEvent);
  305.                 if(0!=(event_mask&EV_RLSD)) {
  306.                     sess->state=2;
  307.                     if(sess->disconnected_callback!=nullptr) {
  308.                         sess->disconnected_callback(sess->disconnected_callback_state);
  309.                     }
  310.                     continue;
  311.                 }
  312.             }
  313.         } else if(wait==WAIT_OBJECT_0+2) {
  314.             // write request - do it while the read remains in flight in the kernel
  315.             do_write(sess->port,&o_writer,sess->exit_event,sess->write_buf,sess->write_buf_length);
  316.             free(sess->write_buf);
  317.             sess->write_buf = nullptr;
  318.             SetEvent(sess->write_slot_event);
  319.         } else if(wait==WAIT_OBJECT_0+3) {
  320.             read_pending = false;
  321.             if(GetOverlappedResult(sess->port,&o_reader,&bytes_read,FALSE)) {
  322.                 ResetEvent(o_reader.hEvent);
  323.                 if(!in_read_frame) {
  324.                     read_frame_start(&read_mach,1000);
  325.                     in_read_frame = true;
  326.                 }
  327.                 if(0==read_frame_next(&read_mach,peek)) {
  328.                     // TODO: call frame handler
  329.                     in_read_frame = false;
  330.                     need_frame = false;
  331.                 }
  332.             }
  333.         }
  334.     }
  335.     CloseHandle(o_reader.hEvent);
  336.     CloseHandle(o_writer.hEvent);
  337.     CloseHandle(o_status.hEvent);
  338.     if(sess->state==2) {
  339.         SetEvent(sess->exited_event);
  340.         deinit_session(sess);
  341.     } else {
  342.         sess->state = 2;
  343.         SetEvent(sess->exited_event);
  344.     }
  345.     return 0;
  346. }
  347. void ReadLogData(espmon_session_t handle, uint8_t* buffer,uint32_t* in_out_length) {
  348.     session_t* sess = (session_t*)handle;
  349.     size_t size = sess->log.size<*in_out_length?sess->log.size:*in_out_length;
  350.     *in_out_length = size;
  351.     if(!size) {
  352.         return;
  353.     }
  354.     memcpy(buffer,sess->log.data,size);
  355.     if(size<sess->log.size) {
  356.         memmove(sess->log.data,sess->log.data+size,sess->log.size-size);
  357.     }
  358.     sess->log.size-=size;
  359. }
  360. void DestroySession(espmon_session_t handle) {
  361.     session_t* sess = (session_t*)handle;
  362.     SetEvent(sess->exit_event);
  363.     WaitForSingleObject(sess->exited_event,INFINITE);
  364.     deinit_session(sess);
  365. }
  366. espmon_session_t CreateSession(
  367.     const wchar_t* portName,
  368.     espmon_frame_callback_t frame_callback,
  369.     void* frame_callback_state,
  370.     espmon_disconnected_callback_t disconnected_callback,
  371.     void* disconnected_callback_state
  372. ) {
  373.     session_t* result = new session_t();
  374.     result->payload.capacity = 0;
  375.     result->payload.size = 0;
  376.     result->payload.data = nullptr;
  377.     result->log.capacity = 0;
  378.     result->log.size = 0;
  379.     result->log.data = nullptr;
  380.     result->write_buf = nullptr;
  381.     result->write_buf_length = 0;
  382.     wcsncpy(result->portName,portName,63);
  383.     result->frame_callback = frame_callback;
  384.     result->frame_callback_state = frame_callback_state;
  385.     result->disconnected_callback = disconnected_callback;
  386.     result->disconnected_callback_state = disconnected_callback_state;
  387.     wchar_t portFile[96];
  388.     wcscpy(portFile,L"\\\\.\\");
  389.     wcscat(portFile,portName);
  390.     result->port = CreateFile(portFile,FILE_SHARE_READ|FILE_SHARE_WRITE,0,nullptr,OPEN_ALWAYS,FILE_FLAG_OVERLAPPED,nullptr);
  391.     if(result->port==NULL) {
  392.         DWORD err = GetLastError();
  393.         delete result;
  394.         SetLastError(err);
  395.         return nullptr;
  396.     }
  397.     DCB dcb = {0};
  398.     dcb.DCBlength = sizeof(DCB);
  399.     //BuildCommDCB(L"115200,8,n,1",&dcb);
  400.     if (!::GetCommState (result->port,&dcb))
  401.     {
  402.         DWORD err = GetLastError();
  403.         CloseHandle(result->port);
  404.         delete result;
  405.         SetLastError(err);
  406.         return nullptr;
  407.     }
  408.  
  409.     dcb.BaudRate    = 115200;
  410.     dcb.ByteSize    = 8;
  411.     dcb.Parity        = 0;
  412.     dcb.StopBits    = ONESTOPBIT;
  413.  
  414.     if (!::SetCommState (result->port,&dcb))
  415.     {
  416.         DWORD err = GetLastError();
  417.         CloseHandle(result->port);
  418.         delete result;
  419.         SetLastError(err);
  420.         return nullptr;
  421.     }
  422.     if(!SetCommMask(result->port,EV_RLSD)) {
  423.         DWORD err = GetLastError();
  424.         CloseHandle(result->port);
  425.         delete result;
  426.         SetLastError(err);
  427.         return nullptr;
  428.     }
  429.     result->exit_event = CreateEvent(nullptr,TRUE,FALSE,nullptr);
  430.     if(result->exit_event==nullptr) {
  431.         DWORD err = GetLastError();
  432.         CloseHandle(result->port);
  433.         delete result;
  434.         SetLastError(err);
  435.         return nullptr;
  436.     }
  437.  
  438.     result->exited_event = CreateEvent(nullptr,TRUE,FALSE,nullptr);
  439.     if(result->exited_event==nullptr) {
  440.         DWORD err = GetLastError();
  441.         CloseHandle(result->exit_event);
  442.         CloseHandle(result->port);
  443.         delete result;
  444.         SetLastError(err);
  445.         return nullptr;
  446.     }
  447.  
  448.     // auto-reset: WriteFrame signals this to kick the thread
  449.     result->write_pending_event = CreateEvent(nullptr,FALSE,FALSE,nullptr);
  450.     if(result->write_pending_event==nullptr) {
  451.         DWORD err = GetLastError();
  452.         CloseHandle(result->exit_event);
  453.         CloseHandle(result->exited_event);
  454.         CloseHandle(result->port);
  455.         delete result;
  456.         SetLastError(err);
  457.         return nullptr;
  458.     }
  459.     // manual-reset, starts signaled: WriteFrame waits here for the slot to be free
  460.     result->write_slot_event = CreateEvent(nullptr,TRUE,TRUE,nullptr);
  461.     if(result->write_slot_event==nullptr) {
  462.         DWORD err = GetLastError();
  463.         CloseHandle(result->write_pending_event);
  464.         CloseHandle(result->exit_event);
  465.         CloseHandle(result->exited_event);
  466.         CloseHandle(result->port);
  467.         delete result;
  468.         SetLastError(err);
  469.         return nullptr;
  470.     }
  471.     DWORD threadId;
  472.     result->thread = CreateThread(NULL,8192,SessionThread,result,0,&threadId);
  473.     if(result->thread==nullptr) {
  474.         DWORD err = GetLastError();
  475.         CloseHandle(result->write_pending_event);
  476.         CloseHandle(result->write_slot_event);
  477.         CloseHandle(result->exit_event);
  478.         CloseHandle(result->exited_event);
  479.         CloseHandle(result->port);
  480.         delete result;
  481.         SetLastError(err);
  482.         return nullptr;
  483.  
  484.     }
  485.     return result;
  486. }
  487. void WriteFrame(espmon_session_t handle, uint8_t cmd, const uint8_t* payload, size_t length) {
  488.     session_t* sess = (session_t*)handle;
  489.     // Allocate one contiguous frame: 8 cmd bytes + 4 length + 4 crc + payload
  490.     size_t frame_size = 16 + length;
  491.     uint8_t* frame = (uint8_t*)malloc(frame_size);
  492.     if(frame==nullptr) { puts("ERROR: out of memory in WriteFrame"); return; }
  493.     for(int i = 0;i<8;++i) frame[i] = cmd;
  494.     uint32_t len32 = (uint32_t)length;
  495.     memcpy(&frame[8],&len32,4);
  496.     uint32_t crc = crc32(payload,length);
  497.     memcpy(&frame[12],&crc,4);
  498.     if(length>0) memcpy(&frame[16],payload,length);
  499.     // Block only if a previous write is still in flight, then hand off and return
  500.     WaitForSingleObject(sess->write_slot_event,INFINITE);
  501.     ResetEvent(sess->write_slot_event);
  502.     sess->write_buf = frame;
  503.     sess->write_buf_length = frame_size;
  504.     SetEvent(sess->write_pending_event);
  505. }
  506. static void on_frame_callback(uint8_t cmd, const uint8_t* data, size_t length, void* state) {
  507.     printf("Frame received. CMD = %d, length = %d\r\n",(int)cmd,(int)length);
  508. }
  509. static bool connected = false;
  510. static void on_disconnected_callback(void* state) {
  511.     puts("Forcibly disconnected");
  512.     connected = false;
  513. }
  514.  
  515. int main(int argc, char**argv) {
  516.     LARGE_INTEGER freq;
  517.     QueryPerformanceFrequency(&freq);
  518.     double tick_freq = double(freq.QuadPart)/1000.0;
  519.  
  520.     LARGE_INTEGER start;
  521.     QueryPerformanceCounter(&start);
  522.     char tmp[1025];
  523.     espmon_session_t handle= CreateSession(L"COM14",on_frame_callback,nullptr,on_disconnected_callback,nullptr);
  524.     connected=true;
  525.     HANDLE stdin_handle = GetStdHandle(STD_INPUT_HANDLE);
  526.     DWORD event_count = 0;
  527.     if(handle!=nullptr && stdin_handle!=nullptr) {
  528.         if(!GetNumberOfConsoleInputEvents(stdin_handle,&event_count)) {
  529.             event_count = 0;
  530.         }
  531.         while(connected && event_count==0) {
  532.             memset(tmp,0,sizeof(tmp));
  533.             uint32_t len = sizeof(tmp)-1;
  534.             ReadLogData(handle,(uint8_t*)tmp,&len);
  535.             if(len>0) {
  536.                 fwrite(tmp,1,len,stdout);
  537.             }
  538.             if(!GetNumberOfConsoleInputEvents(stdin_handle,&event_count)) {
  539.                 event_count = 0;
  540.             }
  541.             LARGE_INTEGER current;
  542.             QueryPerformanceCounter(&current);
  543.             if(((current.QuadPart - start.QuadPart)/tick_freq)>=1000) {
  544.                 QueryPerformanceCounter(&start);
  545.                 puts("Writing packet");
  546.                 uint8_t tmp[4] = {0};
  547.                 WriteFrame(handle,5,tmp,1);
  548.             }
  549.         };
  550.         if(connected) {
  551.             DestroySession(handle);
  552.         }
  553.     }
  554.     return 0;
  555. }
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!