Threaded version 0.7

2026-10-06 23:50:07 -06:00
parent 490c9bdc5b
commit c39c226885
15 changed files with 13126 additions and 0 deletions
@@ -0,0 +1,115 @@
+#ifndef _GNU_SOURCE
+#define _GNU_SOURCE
+#endif
+#include "atom_bridge.h"
+#include "atom_mb.h"
+#include <pthread.h>
+#include <sched.h>
+#include <errno.h>
+#include <time.h>
+#include <string.h>
+
+/* All engine access is serialized. The worker never holds an IEC pointer.
+ * Network I/O is nonblocking; no mutex is held across sleep or thread join.
+ * Serial open/ioctl latency is driver dependent: this is not a hard RT API. */
+uint32_t ab_dispatch(uint32_t,uint32_t,uint32_t,uint16_t,uint8_t,uint8_t,uint16_t,uint16_t,
+ uint16_t *,uint32_t,uint32_t *,uint32_t,uint64_t,uint32_t,uint32_t,uint32_t);
+void ab_pump(uint64_t,uint32_t);
+void ab_close_all(void);
+static pthread_mutex_t aw_lifecycle=PTHREAD_MUTEX_INITIALIZER;
+static pthread_mutex_t aw_engine;
+static pthread_once_t aw_once=PTHREAD_ONCE_INIT;
+static int aw_init_error;
+static void aw_init_mutex(void){
+ pthread_mutexattr_t a;
+ int e=pthread_mutexattr_init(&a);
+ if(e){aw_init_error=e;return;}
+ /* Priority inheritance limits inversion if the PLC task has RT priority. */
+ e=pthread_mutexattr_setprotocol(&a,PTHREAD_PRIO_INHERIT);
+ if(e){pthread_mutexattr_destroy(&a);aw_init_error=e;return;}
+ aw_init_error=pthread_mutex_init(&aw_engine,&a);
+ pthread_mutexattr_destroy(&a);
+}
+static pthread_t aw_thread;
+static int aw_running,aw_stop,aw_error,aw_cpu=-1;
+static uint32_t aw_interval=1,aw_watchdog=2000;
+static uint64_t aw_cycles,aw_heartbeat;
+static cpu_set_t aw_original_mask;
+static uint64_t aw_now(void){
+ struct timespec t;clock_gettime(CLOCK_MONOTONIC,&t);
+ return (uint64_t)t.tv_sec*1000u+(uint64_t)t.tv_nsec/1000000u;
+}
+static void *aw_main(void *unused){
+ (void)unused;
+ for(;;){
+  pthread_mutex_lock(&aw_engine);
+  if(aw_stop){pthread_mutex_unlock(&aw_engine);break;}
+  aw_heartbeat=aw_now();ab_pump(aw_heartbeat,aw_watchdog);aw_cycles++;
+  uint32_t delay=aw_interval;
+  pthread_mutex_unlock(&aw_engine);
+  struct timespec t={delay/1000u,(long)(delay%1000u)*1000000L};
+  while(nanosleep(&t,&t)<0 && errno==EINTR){}
+ }
+ return NULL;
+}
+static int aw_start(void){
+ if(aw_running)return 0;
+ int e=pthread_getaffinity_np(pthread_self(),sizeof(aw_original_mask),&aw_original_mask);
+ if(e)return e;
+ pthread_attr_t attr;struct sched_param priority={0};e=pthread_attr_init(&attr);
+ if(e)return e;
+ /* Do not inherit a high-priority real-time PLC task's scheduling policy. */
+ e=pthread_attr_setinheritsched(&attr,PTHREAD_EXPLICIT_SCHED);
+ if(!e)e=pthread_attr_setschedpolicy(&attr,SCHED_OTHER);
+ if(!e)e=pthread_attr_setschedparam(&attr,&priority);
+ aw_stop=0;
+ if(!e)e=pthread_create(&aw_thread,&attr,aw_main,NULL);
+ pthread_attr_destroy(&attr);
+ if(!e){aw_running=1;aw_cpu=-1;}
+ return e;
+}
+static void aw_snapshot(uint32_t *o){
+ memset(o,0,24*sizeof(*o));o[0]=0x00040000u;o[1]=(uint32_t)aw_running;
+ o[2]=(uint32_t)aw_error;o[3]=(uint32_t)(aw_cpu+1);o[4]=aw_interval;o[5]=aw_watchdog;
+ o[6]=(uint32_t)aw_cycles;o[7]=(uint32_t)(aw_cycles>>32);
+ o[8]=(uint32_t)aw_heartbeat;o[9]=(uint32_t)(aw_heartbeat>>32);
+}
+uint32_t ab_call(uint32_t cmd,uint32_t h,uint32_t ip,uint16_t port,uint8_t unit,uint8_t f,
+ uint16_t addr,uint16_t q,uint16_t *data,uint32_t cap,uint32_t *status,uint32_t sc,
+ uint64_t ignored_now,uint32_t ct,uint32_t rt,uint32_t retry){
+ (void)ignored_now;
+ if(!status || sc<AM_STATUS_WORDS)return AB_BAD_CAPACITY;
+ pthread_once(&aw_once,aw_init_mutex);
+ if(aw_init_error){memset(status,0,24*sizeof(*status));status[2]=status[7]=(uint32_t)aw_init_error;return AB_WORKER_ERROR;}
+ pthread_mutex_lock(&aw_lifecycle);pthread_mutex_lock(&aw_engine);
+ uint32_t result=0;
+ if(cmd==1 || cmd==20 || cmd==30 || cmd==40){
+  aw_error=aw_start();
+  if(aw_error){memset(status,0,24*sizeof(*status));status[2]=status[7]=(uint32_t)aw_error;result=AB_WORKER_ERROR;goto done;}
+ }
+ if(cmd==40){
+  /* addr=CPU+1 (0 restores inherited mask), ct=pump interval, rt=scan watchdog. */
+  if(addr>CPU_SETSIZE || ct<1 || ct>20 || rt<100 || rt>60000){result=AM_ARGUMENT;goto worker_status;}
+  cpu_set_t mask=aw_original_mask;
+  if(addr){CPU_ZERO(&mask);CPU_SET(addr-1,&mask);}
+  aw_error=pthread_setaffinity_np(aw_thread,sizeof(mask),&mask);
+  if(aw_error){result=AB_WORKER_ERROR;goto worker_status;}
+  aw_cpu=(int)addr-1;aw_interval=ct;aw_watchdog=rt;
+worker_status:
+  aw_snapshot(status);
+ }else if(cmd==41){aw_snapshot(status);}
+ else result=ab_dispatch(cmd,h,ip,port,unit,f,addr,q,data,cap,status,sc,aw_now(),ct,rt,retry);
+done:
+ pthread_mutex_unlock(&aw_engine);pthread_mutex_unlock(&aw_lifecycle);
+ return result;
+}
+void ab_shutdown(void){
+ pthread_once(&aw_once,aw_init_mutex);
+ if(aw_init_error)return;
+ pthread_mutex_lock(&aw_lifecycle);pthread_mutex_lock(&aw_engine);
+ int join=aw_running;aw_stop=1;pthread_mutex_unlock(&aw_engine);
+ if(join)pthread_join(aw_thread,NULL);
+ pthread_mutex_lock(&aw_engine);ab_close_all();aw_running=0;aw_cpu=-1;
+ aw_error=0;aw_interval=1;aw_watchdog=2000;aw_cycles=0;aw_heartbeat=0;
+ pthread_mutex_unlock(&aw_engine);pthread_mutex_unlock(&aw_lifecycle);
+}