API tools faq
paste
honey_the_codewitch

serial bridge test

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