libcm is a C development framework with an emphasis on audio signal processing applications.
Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.

cmThread.c 44KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872
  1. #include "cmPrefix.h"
  2. #include "cmGlobal.h"
  3. #include "cmRpt.h"
  4. #include "cmErr.h"
  5. #include "cmMem.h"
  6. #include "cmMallocDebug.h"
  7. #include "cmThread.h"
  8. #include <pthread.h>
  9. #include <unistd.h> // usleep
  10. #ifdef OS_OSX
  11. #include <libkern/OSAtomic.h>
  12. #endif
  13. cmThreadH_t cmThreadNullHandle = {NULL};
  14. enum
  15. {
  16. kDoExitThFl = 0x01,
  17. kDoPauseThFl = 0x02,
  18. kDoRunThFl = 0x04
  19. };
  20. typedef struct
  21. {
  22. cmErr_t err;
  23. cmThreadFunc_t funcPtr;
  24. pthread_t pthreadH;
  25. cmThStateId_t state;
  26. void* funcParam;
  27. unsigned doFlags;
  28. unsigned pauseMicroSecs;
  29. unsigned waitMicroSecs;
  30. } cmThThread_t;
  31. cmThRC_t _cmThError( cmErr_t* err, cmThRC_t rc, int sysErr, const char* fmt, ... )
  32. {
  33. va_list vl;
  34. va_start(vl,fmt);
  35. cmErrVSysMsg(err,rc,sysErr,fmt,vl);
  36. va_end(vl);
  37. return rc;
  38. }
  39. void _cmThThreadCleanUpCallback(void* t)
  40. {
  41. ((cmThThread_t*)t)->state = kExitedThId;
  42. }
  43. void* _cmThThreadCallback(void* param)
  44. {
  45. cmThThread_t* t = (cmThThread_t*)param;
  46. // set a clean up handler - this will be called when the
  47. // thread terminates unexpectedly or pthread_cleanup_pop() is called.
  48. pthread_cleanup_push(_cmThThreadCleanUpCallback,t);
  49. while( cmIsFlag(t->doFlags,kDoExitThFl) == false )
  50. {
  51. if( t->state == kPausedThId )
  52. {
  53. cmSleepUs( t->pauseMicroSecs );
  54. if( cmIsFlag(t->doFlags,kDoRunThFl) )
  55. {
  56. t->doFlags = cmClrFlag(t->doFlags,kDoRunThFl);
  57. t->state = kRunningThId;
  58. }
  59. }
  60. else
  61. {
  62. if( t->funcPtr(t->funcParam)==false )
  63. break;
  64. if( cmIsFlag(t->doFlags,kDoPauseThFl) )
  65. {
  66. t->doFlags = cmClrFlag(t->doFlags,kDoPauseThFl);
  67. t->state = kPausedThId;
  68. }
  69. }
  70. }
  71. pthread_cleanup_pop(1);
  72. pthread_exit(NULL);
  73. return t;
  74. }
  75. cmThThread_t* _cmThThreadFromHandle( cmThreadH_t h )
  76. {
  77. cmThThread_t* tp = (cmThThread_t*)h.h;
  78. assert(tp != NULL);
  79. return tp->state==kNotInitThId ? NULL : tp;
  80. }
  81. cmThRC_t _cmThWaitForState( cmThThread_t* t, unsigned stateId )
  82. {
  83. unsigned waitTimeMicroSecs = 0;
  84. while( t->state != stateId && waitTimeMicroSecs < t->waitMicroSecs )
  85. {
  86. //cmSleepUs( t->waitMicroSecs );
  87. cmSleepUs( 15000 );
  88. waitTimeMicroSecs += 15000; //t->waitMicroSecs;
  89. }
  90. return t->state==stateId ? kOkThRC : kTimeOutThRC;
  91. }
  92. cmThRC_t cmThreadCreate( cmThreadH_t* hPtr, cmThreadFunc_t funcPtr, void* funcParam, cmRpt_t* rpt )
  93. {
  94. //pthread_attr_t attr;
  95. cmThRC_t rc = kOkThRC;
  96. cmThThread_t* tp = cmMemAllocZ( cmThThread_t, 1 );
  97. int sysErr;
  98. cmErrSetup(&tp->err,rpt,"Thread");
  99. tp->funcPtr = funcPtr;
  100. tp->funcParam = funcParam;
  101. tp->state = kPausedThId;
  102. tp->doFlags = 0;
  103. tp->pauseMicroSecs = 50000;
  104. tp->waitMicroSecs = 1000000;
  105. if((sysErr = pthread_create(&tp->pthreadH,NULL,_cmThThreadCallback, (void*)tp )) != 0 )
  106. {
  107. tp->state = kNotInitThId;
  108. rc = _cmThError(&tp->err,kCreateFailThRC,sysErr,"Thread create failed.");
  109. }
  110. hPtr->h = tp;
  111. return rc;
  112. }
  113. cmThRC_t cmThreadDestroy( cmThreadH_t* hPtr )
  114. {
  115. cmThRC_t rc = kOkThRC;
  116. if( hPtr==NULL || cmThreadIsValid(*hPtr)==false )
  117. return rc;
  118. cmThThread_t* t = _cmThThreadFromHandle(*hPtr );
  119. if( t == NULL )
  120. return kInvalidHandleThRC;
  121. // tell the thread to exit
  122. t->doFlags = cmSetFlag(t->doFlags,kDoExitThFl);
  123. // wait for the thread to exit and then deallocate the thread object
  124. if((rc = _cmThWaitForState(t,kExitedThId)) == kOkThRC )
  125. {
  126. cmMemFree(t);
  127. hPtr->h = NULL;
  128. }
  129. else
  130. {
  131. rc = _cmThError(&t->err,rc,0,"Thread timed out waiting for destroy.");
  132. }
  133. return rc;
  134. }
  135. cmThRC_t cmThreadPause( cmThreadH_t h, unsigned cmdFlags )
  136. {
  137. cmThRC_t rc = kOkThRC;
  138. bool pauseFl = cmIsFlag(cmdFlags,kPauseThFl);
  139. bool waitFl = cmIsFlag(cmdFlags,kWaitThFl);
  140. cmThThread_t* t = _cmThThreadFromHandle(h);
  141. unsigned waitId;
  142. if( t == NULL )
  143. return kInvalidHandleThRC;
  144. bool isPausedFl = t->state == kPausedThId;
  145. if( isPausedFl == pauseFl )
  146. return kOkThRC;
  147. if( pauseFl )
  148. {
  149. t->doFlags = cmSetFlag(t->doFlags,kDoPauseThFl);
  150. waitId = kPausedThId;
  151. }
  152. else
  153. {
  154. t->doFlags = cmSetFlag(t->doFlags,kDoRunThFl);
  155. waitId = kRunningThId;
  156. }
  157. if( waitFl )
  158. rc = _cmThWaitForState(t,waitId);
  159. if( rc != kOkThRC )
  160. _cmThError(&t->err,rc,0,"Thread timed out waiting for '%s'.", pauseFl ? "pause" : "un-pause");
  161. return rc;
  162. }
  163. cmThStateId_t cmThreadState( cmThreadH_t h )
  164. {
  165. cmThThread_t* tp = _cmThThreadFromHandle(h);
  166. if( tp == NULL )
  167. return kNotInitThId;
  168. return tp->state;
  169. }
  170. bool cmThreadIsValid( cmThreadH_t h )
  171. { return h.h != NULL; }
  172. unsigned cmThreadPauseTimeOutMicros( cmThreadH_t h )
  173. {
  174. cmThThread_t* tp = _cmThThreadFromHandle(h);
  175. return tp->pauseMicroSecs;
  176. }
  177. void cmThreadSetPauseTimeOutMicros( cmThreadH_t h, unsigned usecs )
  178. {
  179. cmThThread_t* tp = _cmThThreadFromHandle(h);
  180. tp->pauseMicroSecs = usecs;
  181. }
  182. unsigned cmThreadWaitTimeOutMicros( cmThreadH_t h )
  183. {
  184. cmThThread_t* tp = _cmThThreadFromHandle(h);
  185. return tp->waitMicroSecs;
  186. }
  187. void cmThreadSetWaitTimeOutMicros( cmThreadH_t h, unsigned usecs )
  188. {
  189. cmThThread_t* tp = _cmThThreadFromHandle(h);
  190. tp->waitMicroSecs = usecs;
  191. }
  192. bool _cmThreadTestCb( void* p )
  193. {
  194. unsigned* ip = (unsigned*)p;
  195. ip[0]++;
  196. return true;
  197. }
  198. void cmThreadTest(cmRpt_t* rpt)
  199. {
  200. cmThreadH_t th0;
  201. unsigned val = 0;
  202. if( cmThreadCreate(&th0,_cmThreadTestCb,&val,rpt) == kOkThRC )
  203. {
  204. if( cmThreadPause(th0,0) != kOkThRC )
  205. {
  206. cmRptPrintf(rpt,"Thread start failed.\n");
  207. return;
  208. }
  209. char c = 0;
  210. cmRptPrintf(rpt,"o=print p=pause s=state q=quit\n");
  211. while( c != 'q' )
  212. {
  213. c = (char)fgetc(stdin);
  214. fflush(stdin);
  215. switch(c)
  216. {
  217. case 'o':
  218. cmRptPrintf(rpt,"val: 0x%x\n",val);
  219. break;
  220. case 's':
  221. cmRptPrintf(rpt,"state=%i\n",cmThreadState(th0));
  222. break;
  223. case 'p':
  224. {
  225. cmRC_t rc;
  226. if( cmThreadState(th0) == kPausedThId )
  227. rc = cmThreadPause(th0,kWaitThFl);
  228. else
  229. rc = cmThreadPause(th0,kPauseThFl|kWaitThFl);
  230. if( rc == kOkThRC )
  231. cmRptPrintf(rpt,"new state:%i\n", cmThreadState(th0));
  232. else
  233. cmRptPrintf(rpt,"cmThreadPause() failed.");
  234. }
  235. break;
  236. case 'q':
  237. break;
  238. //default:
  239. //cmRptPrintf(rpt,"Unknown:%c\n",c);
  240. }
  241. }
  242. if( cmThreadDestroy(&th0) != kOkThRC )
  243. cmRptPrintf(rpt,"Thread destroy failed.\n");
  244. }
  245. }
  246. //-----------------------------------------------------------------------------
  247. //-----------------------------------------------------------------------------
  248. //-----------------------------------------------------------------------------
  249. typedef struct
  250. {
  251. cmErr_t err;
  252. pthread_mutex_t mutex;
  253. pthread_cond_t cvar;
  254. } cmThreadMutex_t;
  255. cmThreadMutexH_t kThreadMutexNULL = {NULL};
  256. cmThreadMutex_t* _cmThreadMutexFromHandle( cmThreadMutexH_t h )
  257. {
  258. cmThreadMutex_t* p = (cmThreadMutex_t*)h.h;
  259. assert(p != NULL);
  260. return p;
  261. }
  262. cmThRC_t cmThreadMutexCreate( cmThreadMutexH_t* hPtr, cmRpt_t* rpt )
  263. {
  264. int sysErr;
  265. cmThreadMutex_t* p = cmMemAllocZ( cmThreadMutex_t, 1 );
  266. cmErrSetup(&p->err,rpt,"Thread Mutex");
  267. if((sysErr = pthread_mutex_init(&p->mutex,NULL)) != 0 )
  268. return _cmThError(&p->err,kCreateFailThRC,sysErr,"Thread mutex create failed.");
  269. if((sysErr = pthread_cond_init(&p->cvar,NULL)) != 0 )
  270. return _cmThError(&p->err,kCreateFailThRC,sysErr,"Thread Condition var. create failed.");
  271. hPtr->h = p;
  272. return kOkThRC;
  273. }
  274. cmThRC_t cmThreadMutexDestroy( cmThreadMutexH_t* hPtr )
  275. {
  276. int sysErr;
  277. cmThreadMutex_t* p = _cmThreadMutexFromHandle(*hPtr);
  278. if( p == NULL )
  279. return kInvalidHandleThRC;
  280. if((sysErr = pthread_cond_destroy(&p->cvar)) != 0)
  281. return _cmThError(&p->err,kDestroyFailThRC,sysErr,"Thread condition var. destroy failed.");
  282. if((sysErr = pthread_mutex_destroy(&p->mutex)) != 0)
  283. return _cmThError(&p->err,kDestroyFailThRC,sysErr,"Thread mutex destroy failed.");
  284. cmMemFree(p);
  285. hPtr->h = NULL;
  286. return kOkThRC;
  287. }
  288. cmThRC_t cmThreadMutexTryLock( cmThreadMutexH_t h, bool* lockFlPtr )
  289. {
  290. cmThreadMutex_t* p = _cmThreadMutexFromHandle(h);
  291. if( p == NULL )
  292. return kInvalidHandleThRC;
  293. int sysErr = pthread_mutex_trylock(&p->mutex);
  294. switch(sysErr)
  295. {
  296. case EBUSY:
  297. *lockFlPtr = false;
  298. break;
  299. case 0:
  300. *lockFlPtr = true;
  301. break;
  302. default:
  303. return _cmThError(&p->err,kLockFailThRC,sysErr,"Thread mutex try-lock failed.");;
  304. }
  305. return kOkThRC;
  306. }
  307. cmThRC_t cmThreadMutexLock( cmThreadMutexH_t h )
  308. {
  309. cmThreadMutex_t* p = _cmThreadMutexFromHandle(h);
  310. if( p == NULL )
  311. return kInvalidHandleThRC;
  312. int sysErr = pthread_mutex_lock(&p->mutex);
  313. if( sysErr == 0 )
  314. return kOkThRC;
  315. return _cmThError(&p->err,kLockFailThRC,sysErr,"Thread mutex lock failed.");
  316. }
  317. cmThRC_t cmThreadMutexUnlock( cmThreadMutexH_t h )
  318. {
  319. cmThreadMutex_t* p = _cmThreadMutexFromHandle(h);
  320. if( p == NULL )
  321. return kInvalidHandleThRC;
  322. int sysErr = pthread_mutex_unlock(&p->mutex);
  323. if( sysErr == 0 )
  324. return kOkThRC;
  325. return _cmThError(&p->err,kUnlockFailThRC,sysErr,"Thread mutex unlock failed.");
  326. }
  327. bool cmThreadMutexIsValid( cmThreadMutexH_t h )
  328. { return h.h != NULL; }
  329. cmThRC_t cmThreadMutexWaitOnCondVar( cmThreadMutexH_t h, bool lockFl )
  330. {
  331. cmThreadMutex_t* p = _cmThreadMutexFromHandle(h);
  332. if( p == NULL )
  333. return kInvalidHandleThRC;
  334. int sysErr;
  335. if( lockFl )
  336. if( (sysErr=pthread_mutex_lock(&p->mutex)) != 0 )
  337. _cmThError(&p->err,kLockFailThRC,sysErr,"Thread lock failed on cond. var. wait.");
  338. if((sysErr = pthread_cond_wait(&p->cvar,&p->mutex)) != 0 )
  339. _cmThError(&p->err,kCVarWaitFailThRC,sysErr,"Thread cond. var. wait failed.");
  340. return kOkThRC;
  341. }
  342. cmThRC_t cmThreadMutexSignalCondVar( cmThreadMutexH_t h )
  343. {
  344. int sysErr;
  345. cmThreadMutex_t* p = _cmThreadMutexFromHandle(h);
  346. if( p == NULL )
  347. return kInvalidHandleThRC;
  348. if((sysErr = pthread_cond_signal(&p->cvar)) != 0 )
  349. return _cmThError(&p->err,kCVarSignalFailThRC,sysErr,"Thread cond. var. signal failed.");
  350. return kOkThRC;
  351. }
  352. //-----------------------------------------------------------------------------
  353. //-----------------------------------------------------------------------------
  354. //-----------------------------------------------------------------------------
  355. cmTsQueueH_t cmTsQueueNullHandle = { NULL };
  356. enum { cmTsQueueBufCnt = 2 };
  357. typedef struct
  358. {
  359. unsigned allocCnt; // count of bytes allocated for the buffer
  360. unsigned fullCnt; // count of bytes used in the buffer
  361. char* basePtr; // base of buffer memory
  362. unsigned* msgPtr; // pointer to first msg
  363. unsigned msgCnt;
  364. } cmTsQueueBuf;
  365. typedef struct
  366. {
  367. cmThreadMutexH_t mutexH;
  368. cmTsQueueBuf bufArray[cmTsQueueBufCnt];
  369. unsigned inBufIdx;
  370. unsigned outBufIdx;
  371. char* memPtr;
  372. cmTsQueueCb_t cbFunc;
  373. void* userCbPtr;
  374. } cmTsQueue_t;
  375. cmTsQueue_t* _cmTsQueueFromHandle( cmTsQueueH_t h )
  376. {
  377. cmTsQueue_t* p = h.h;
  378. assert(p != NULL);
  379. return p;
  380. }
  381. cmThRC_t _cmTsQueueDestroy( cmTsQueue_t* p )
  382. {
  383. cmThRC_t rc;
  384. if( p == NULL )
  385. return kInvalidHandleThRC;
  386. if( p->mutexH.h != NULL )
  387. if((rc = cmThreadMutexDestroy(&p->mutexH)) != kOkThRC )
  388. return rc;
  389. if( p->memPtr != NULL )
  390. cmMemPtrFree(&p->memPtr);
  391. cmMemPtrFree(&p);
  392. return kOkThRC;
  393. }
  394. cmThRC_t cmTsQueueCreate( cmTsQueueH_t* hPtr, unsigned bufByteCnt, cmTsQueueCb_t cbFunc, void* userCbPtr, cmRpt_t* rpt )
  395. {
  396. cmTsQueue_t* p = cmMemAllocZ( cmTsQueue_t, 1 );
  397. unsigned i;
  398. if( cmThreadMutexCreate(&p->mutexH,rpt) != kOkThRC )
  399. goto errLabel;
  400. p->memPtr = cmMemAllocZ( char, bufByteCnt*cmTsQueueBufCnt );
  401. p->outBufIdx = 0;
  402. p->inBufIdx = 1;
  403. p->cbFunc = cbFunc;
  404. p->userCbPtr = userCbPtr;
  405. for(i=0; i<cmTsQueueBufCnt; ++i)
  406. {
  407. p->bufArray[i].allocCnt = bufByteCnt;
  408. p->bufArray[i].fullCnt = 0;
  409. p->bufArray[i].basePtr = p->memPtr + (i*bufByteCnt);
  410. p->bufArray[i].msgPtr = NULL;
  411. p->bufArray[i].msgCnt = 0;
  412. }
  413. hPtr->h = p;
  414. return kOkThRC;
  415. errLabel:
  416. _cmTsQueueDestroy(p);
  417. return kCreateFailThRC;
  418. }
  419. cmThRC_t cmTsQueueDestroy( cmTsQueueH_t* hPtr )
  420. {
  421. cmThRC_t rc = kOkThRC;
  422. if( (hPtr != NULL) && cmTsQueueIsValid(*hPtr))
  423. if((rc = _cmTsQueueDestroy(_cmTsQueueFromHandle(*hPtr))) == kOkThRC )
  424. hPtr->h = NULL;
  425. return rc;
  426. }
  427. cmThRC_t cmTsQueueSetCallback( cmTsQueueH_t h, cmTsQueueCb_t cbFunc, void* cbArg )
  428. {
  429. cmTsQueue_t* p = _cmTsQueueFromHandle(h);
  430. p->cbFunc = cbFunc;
  431. p->userCbPtr = cbArg;
  432. return kOkThRC;
  433. }
  434. unsigned cmTsQueueAllocByteCount( cmTsQueueH_t h )
  435. {
  436. cmTsQueue_t* p = _cmTsQueueFromHandle(h);
  437. unsigned n = 0;
  438. if( cmThreadMutexLock(p->mutexH) == kOkThRC )
  439. {
  440. n = p->bufArray[ p->inBufIdx ].allocCnt;
  441. cmThreadMutexUnlock(p->mutexH);
  442. }
  443. return n;
  444. }
  445. unsigned cmTsQueueAvailByteCount( cmTsQueueH_t h )
  446. {
  447. cmTsQueue_t* p = _cmTsQueueFromHandle(h);
  448. unsigned n = 0;
  449. if(cmThreadMutexLock(p->mutexH) == kOkThRC )
  450. {
  451. n = p->bufArray[ p->inBufIdx ].allocCnt - p->bufArray[ p->inBufIdx].fullCnt;
  452. cmThreadMutexUnlock(p->mutexH);
  453. }
  454. return n;
  455. }
  456. cmThRC_t _cmTsQueueEnqueueMsg( cmTsQueueH_t h, const void* msgPtrArray[], unsigned msgByteCntArray[], unsigned arrayCnt )
  457. {
  458. cmThRC_t rc;
  459. cmTsQueue_t* p = _cmTsQueueFromHandle(h);
  460. if( p == NULL )
  461. return kInvalidHandleThRC;
  462. // lock the mutex
  463. if((rc = cmThreadMutexLock(p->mutexH)) == kOkThRC )
  464. {
  465. cmTsQueueBuf* b = p->bufArray + p->inBufIdx; // ptr to buf recd
  466. const char* ep = b->basePtr + b->allocCnt; // end of buf data space
  467. unsigned *mp = (unsigned*)(b->basePtr + b->fullCnt); // ptr to size of new msg space
  468. char* dp = (char*)(mp+1); // ptr to data area of new msg space
  469. unsigned ttlByteCnt = 0; // track size of msg data
  470. unsigned i = 0;
  471. // get the total size of the msg
  472. for(i=0; i<arrayCnt; ++i)
  473. ttlByteCnt += msgByteCntArray[i];
  474. // if the msg is too big for the queue buf
  475. if( dp + ttlByteCnt > ep )
  476. rc = kBufFullThRC;
  477. else
  478. {
  479. // for each segment of the incoming msg
  480. for(i=0; i<arrayCnt; ++i)
  481. {
  482. // get the size of the segment
  483. unsigned n = msgByteCntArray[i];
  484. // copy in the segment
  485. memcpy(dp,msgPtrArray[i],n);
  486. dp += n; //
  487. }
  488. assert(dp <= ep );
  489. // write the size ofthe msg into the buffer
  490. *mp = ttlByteCnt;
  491. // update the pointer to the first msg
  492. if( b->msgPtr == NULL )
  493. b->msgPtr = mp;
  494. // track the count of msgs in this buffer
  495. ++b->msgCnt;
  496. // update fullCnt last since dequeue uses fullCnt to
  497. // notice that a msg may be waiting
  498. b->fullCnt += sizeof(unsigned) + ttlByteCnt;
  499. }
  500. cmThreadMutexUnlock(p->mutexH);
  501. }
  502. return rc;
  503. }
  504. cmThRC_t cmTsQueueEnqueueSegMsg( cmTsQueueH_t h, const void* msgPtrArray[], unsigned msgByteCntArray[], unsigned arrayCnt )
  505. { return _cmTsQueueEnqueueMsg(h,msgPtrArray,msgByteCntArray,arrayCnt); }
  506. cmThRC_t cmTsQueueEnqueueMsg( cmTsQueueH_t h, const void* dataPtr, unsigned byteCnt )
  507. {
  508. const void* msgPtrArray[] = { dataPtr };
  509. unsigned msgByteCntArray[] = { byteCnt };
  510. return _cmTsQueueEnqueueMsg(h,msgPtrArray,msgByteCntArray,1);
  511. }
  512. cmThRC_t cmTsQueueEnqueueIdMsg( cmTsQueueH_t h, unsigned id, const void* dataPtr, unsigned byteCnt )
  513. {
  514. const void* msgPtrArray[] = { &id, dataPtr };
  515. unsigned msgByteCntArray[] = { sizeof(id), byteCnt };
  516. return _cmTsQueueEnqueueMsg(h,msgPtrArray,msgByteCntArray,2);
  517. }
  518. cmThRC_t _cmTsQueueDequeueMsg( cmTsQueue_t* p, void* retBuf, unsigned refBufByteCnt )
  519. {
  520. cmTsQueueBuf* b = p->bufArray + p->outBufIdx;
  521. // if the output buffer is empty - there is nothing to do
  522. if( b->fullCnt == 0 )
  523. return kBufEmptyThRC;
  524. assert( b->msgPtr != NULL );
  525. // get the output msg size and data
  526. unsigned msgByteCnt = *b->msgPtr;
  527. char* msgDataPtr = (char*)(b->msgPtr + 1);
  528. // transmit the msg via a callback
  529. if( retBuf == NULL && p->cbFunc != NULL )
  530. p->cbFunc(p->userCbPtr,msgByteCnt,msgDataPtr);
  531. else
  532. {
  533. // retBuf may be NULL if the func is being used by cmTsQueueDequeueByteCount()
  534. if( retBuf == NULL || msgByteCnt > refBufByteCnt )
  535. return kBufTooSmallThRC;
  536. // copy the msg to a buffer
  537. if( retBuf != NULL )
  538. memcpy(retBuf,msgDataPtr,msgByteCnt);
  539. }
  540. // update the buffer
  541. b->fullCnt -= sizeof(unsigned) + msgByteCnt;
  542. b->msgPtr = (unsigned*)(msgDataPtr + msgByteCnt);
  543. --(b->msgCnt);
  544. if( b->fullCnt == 0 )
  545. {
  546. assert(b->msgCnt == 0);
  547. b->msgPtr = NULL;
  548. }
  549. return kOkThRC;
  550. }
  551. cmThRC_t cmTsQueueDequeueMsg( cmTsQueueH_t h, void* retBuf, unsigned refBufByteCnt )
  552. {
  553. cmThRC_t rc;
  554. cmTsQueue_t* p = _cmTsQueueFromHandle(h);
  555. if( p == NULL )
  556. return kInvalidHandleThRC;
  557. // dequeue the next msg from the current output buffer
  558. if((rc =_cmTsQueueDequeueMsg( p, retBuf, refBufByteCnt )) != kBufEmptyThRC )
  559. return rc;
  560. // the current output buffer was empty
  561. cmTsQueueBuf* b = p->bufArray + p->inBufIdx;
  562. // if the input buffer has msg's ...
  563. if( b->fullCnt > 0 )
  564. {
  565. bool lockFl = false;
  566. // ...attempt to lock the mutex ...
  567. if( (cmThreadMutexTryLock(p->mutexH,&lockFl) == kOkThRC) && lockFl )
  568. {
  569. // ... swap the input and the output buffers ...
  570. unsigned tmp = p->inBufIdx;
  571. p->inBufIdx = p->outBufIdx;
  572. p->outBufIdx = tmp;
  573. // .. unlock the mutex
  574. cmThreadMutexUnlock(p->mutexH);
  575. // ... and dequeue the first msg from the new output buffer
  576. rc = _cmTsQueueDequeueMsg( p, retBuf, refBufByteCnt );
  577. }
  578. }
  579. return rc;
  580. }
  581. bool cmTsQueueMsgWaiting( cmTsQueueH_t h )
  582. {
  583. cmTsQueue_t* p = _cmTsQueueFromHandle(h);
  584. if( p == NULL )
  585. return false;
  586. if( p->bufArray[p->outBufIdx].fullCnt )
  587. return true;
  588. return p->bufArray[p->inBufIdx].fullCnt > 0;
  589. }
  590. unsigned cmTsQueueDequeueMsgByteCount( cmTsQueueH_t h )
  591. {
  592. cmTsQueue_t* p = _cmTsQueueFromHandle(h);
  593. if( p == NULL )
  594. return 0;
  595. // if output msgs are available then the msgPtr points to the size of the msg
  596. if( p->bufArray[p->outBufIdx].fullCnt )
  597. return *(p->bufArray[p->outBufIdx].msgPtr);
  598. // no msgs are waiting in the output buffer
  599. // force the buffers to swap - returns kBufEmptyThRC if there are
  600. // still no msgs waiting after the swap (the input buf was also empty)
  601. if( cmTsQueueDequeueMsg(h,NULL,0) == kBufTooSmallThRC )
  602. {
  603. // the buffers swapped so there must be msg waiting
  604. assert( p->bufArray[p->outBufIdx].fullCnt );
  605. return *(p->bufArray[p->outBufIdx].msgPtr);
  606. }
  607. return 0;
  608. }
  609. bool cmTsQueueIsValid( cmTsQueueH_t h )
  610. { return h.h != NULL; }
  611. //--------------------------------------------------------------------------------------------------
  612. //--------------------------------------------------------------------------------------------------
  613. //--------------------------------------------------------------------------------------------------
  614. //--------------------------------------------------------------------------------------------------
  615. //--------------------------------------------------------------------------------------------------
  616. //--------------------------------------------------------------------------------------------------
  617. typedef struct
  618. {
  619. volatile unsigned ii;
  620. cmErr_t err;
  621. char* buf;
  622. unsigned bn;
  623. cmTsQueueCb_t cbFunc;
  624. void* cbArg;
  625. volatile unsigned oi;
  626. } cmTs1p1c_t;
  627. cmTs1p1c_t* _cmTs1p1cHandleToPtr( cmTs1p1cH_t h )
  628. {
  629. cmTs1p1c_t* p = (cmTs1p1c_t*)h.h;
  630. assert( p != NULL );
  631. return p;
  632. }
  633. cmThRC_t cmTs1p1cCreate( cmTs1p1cH_t* hPtr, unsigned bufByteCnt, cmTsQueueCb_t cbFunc, void* cbArg, cmRpt_t* rpt )
  634. {
  635. cmThRC_t rc;
  636. if((rc = cmTs1p1cDestroy(hPtr)) != kOkThRC )
  637. return rc;
  638. cmTs1p1c_t* p = cmMemAllocZ(cmTs1p1c_t,1);
  639. cmErrSetup(&p->err,rpt,"1p1c Queue");
  640. p->buf = cmMemAllocZ(char,bufByteCnt+sizeof(unsigned));
  641. p->ii = 0;
  642. p->oi = 0;
  643. p->bn = bufByteCnt;
  644. p->cbFunc = cbFunc;
  645. p->cbArg = cbArg;
  646. hPtr->h = p;
  647. return rc;
  648. }
  649. cmThRC_t cmTs1p1cDestroy( cmTs1p1cH_t* hp )
  650. {
  651. cmThRC_t rc = kOkThRC;
  652. if( hp == NULL || cmTs1p1cIsValid(*hp)==false )
  653. return kOkThRC;
  654. cmTs1p1c_t* p = _cmTs1p1cHandleToPtr(*hp);
  655. cmMemFree(p->buf);
  656. cmMemFree(p);
  657. hp->h = NULL;
  658. return rc;
  659. }
  660. cmThRC_t cmTs1p1cSetCallback( cmTs1p1cH_t h, cmTsQueueCb_t cbFunc, void* cbArg )
  661. {
  662. cmTs1p1c_t* p = _cmTs1p1cHandleToPtr(h);
  663. p->cbFunc = cbFunc;
  664. p->cbArg = cbArg;
  665. return kOkThRC;
  666. }
  667. cmThRC_t cmTs1p1cEnqueueSegMsg( cmTs1p1cH_t h, const void* msgPtrArray[], unsigned msgByteCntArray[], unsigned arrayCnt )
  668. {
  669. cmThRC_t rc = kOkThRC;
  670. unsigned mn = 0;
  671. unsigned i;
  672. cmTs1p1c_t* p = _cmTs1p1cHandleToPtr(h);
  673. // get the total count of bytes for this msg
  674. for(i=0; i<arrayCnt; ++i)
  675. mn += msgByteCntArray[i];
  676. int dn = mn + sizeof(unsigned);
  677. int oi = p->oi;
  678. int bi = p->ii; // 'bi' is the idx of the leftmost cell which can be written
  679. int en = p->bn; // 'en' is the idx of the cell just to the right of the rightmost cell that can be written
  680. // note: If 'oi' marks the rightmost location then 'en' must be set
  681. // one cell to the left of 'oi', because 'ii' can never be allowed to
  682. // advance onto 'oi' - because 'oi'=='ii' marks an empty (NOT a full)
  683. // queue.
  684. //
  685. // If 'bn' marks the rightmost location then 'ii' can advance onto 'bn'
  686. // beause the true queue length is bn+1.
  687. // if we need to wrap
  688. if( en-bi < dn && oi<=bi )
  689. {
  690. bi = 0;
  691. en = oi - 1; // note if oi==0 then en is negative - see note above re: oi==ii
  692. assert( p->ii>=0 && p->ii <= p->bn );
  693. *(unsigned*)(p->buf + p->ii) = cmInvalidIdx; // mark the wrap location
  694. }
  695. // if oi is between ii and bn
  696. if( oi > bi )
  697. en = oi - 1; // never allow ii to advance onto oi - see note above
  698. // if the msg won't fit
  699. if( en - bi < dn )
  700. return cmErrMsg(&p->err,kBufFullThRC,"%i consecutive bytes is not available in the queue.",dn);
  701. // set the msg byte count - the msg byte cnt precedes the msg body
  702. char* dp = p->buf + bi;
  703. *(unsigned*)dp = dn - sizeof(unsigned);
  704. dp += sizeof(unsigned);
  705. // copy the msg into the buffer
  706. for(i=0,dn=0; i<arrayCnt; ++i)
  707. {
  708. memcpy(dp,msgPtrArray[i],msgByteCntArray[i]);
  709. dp += msgByteCntArray[i];
  710. dn += msgByteCntArray[i];
  711. }
  712. // incrementing p->ii must occur last - the unnecessary accumulation
  713. // of dn in the above loop is intended to prevent this line from
  714. // begin moved before the copy loop.
  715. p->ii = bi + dn + sizeof(unsigned);
  716. assert( p->ii >= 0 && p->ii <= p->bn);
  717. return rc;
  718. }
  719. cmThRC_t cmTs1p1cEnqueueMsg( cmTs1p1cH_t h, const void* dataPtr, unsigned byteCnt )
  720. { return cmTs1p1cEnqueueSegMsg(h,&dataPtr,&byteCnt,1); }
  721. unsigned cmTs1p1cAllocByteCount( cmTs1p1cH_t h )
  722. {
  723. cmTs1p1c_t* p = _cmTs1p1cHandleToPtr(h);
  724. return p->bn;
  725. }
  726. unsigned cmTs1p1cAvailByteCount( cmTs1p1cH_t h )
  727. {
  728. cmTs1p1c_t* p = _cmTs1p1cHandleToPtr(h);
  729. unsigned oi = p->oi;
  730. unsigned ii = p->ii;
  731. return oi < ii ? p->bn - ii + oi : oi - ii;
  732. }
  733. unsigned _cmTs1p1cDequeueMsgByteCount( cmTs1p1c_t* p )
  734. {
  735. // if the buffer is empty
  736. if( p->ii == p->oi )
  737. return 0;
  738. // get the length of the next msg
  739. unsigned mn = *(unsigned*)(p->buf + p->oi);
  740. // if the msg length is cmInvalidIdx ...
  741. if( mn == cmInvalidIdx )
  742. {
  743. p->oi = 0; // ... wrap to buf begin and try again
  744. return _cmTs1p1cDequeueMsgByteCount(p);
  745. }
  746. return mn;
  747. }
  748. cmThRC_t cmTs1p1cDequeueMsg( cmTs1p1cH_t h, void* dataPtr, unsigned byteCnt )
  749. {
  750. cmThRC_t rc = kOkThRC;
  751. cmTs1p1c_t* p = _cmTs1p1cHandleToPtr(h);
  752. unsigned mn;
  753. if((mn = _cmTs1p1cDequeueMsgByteCount(p)) == 0 )
  754. return kBufEmptyThRC;
  755. void* mp = p->buf + p->oi + sizeof(unsigned);
  756. if( dataPtr != NULL )
  757. {
  758. if( byteCnt < mn )
  759. return cmErrMsg(&p->err,kBufTooSmallThRC,"The return buffer constains too few bytes (%i) to contain %i bytes.",byteCnt,mn);
  760. memcpy(dataPtr,mp,mn);
  761. }
  762. else
  763. {
  764. p->cbFunc(p->cbArg,mn,mp);
  765. }
  766. p->oi += mn + sizeof(unsigned);
  767. return rc;
  768. }
  769. unsigned cmTs1p1cDequeueMsgByteCount( cmTs1p1cH_t h )
  770. {
  771. cmTs1p1c_t* p = _cmTs1p1cHandleToPtr(h);
  772. return _cmTs1p1cDequeueMsgByteCount(p);
  773. }
  774. bool cmTs1p1cMsgWaiting( cmTs1p1cH_t h )
  775. { return cmTs1p1cDequeueMsgByteCount(h) > 0; }
  776. bool cmTs1p1cIsValid( cmTs1p1cH_t h )
  777. { return h.h != NULL; }
  778. //============================================================================================================================
  779. bool cmThIntCAS( int* addr, int old, int new )
  780. {
  781. #ifdef OS_OSX
  782. int rv = OSAtomicCompareAndSwap32Barrier(old,new,addr);
  783. return rv;
  784. #endif
  785. #ifdef OS_LINUX
  786. return __sync_bool_compare_and_swap(addr,old,new);
  787. #endif
  788. }
  789. bool cmThUIntCAS( unsigned* addr, unsigned old, unsigned new )
  790. {
  791. #ifdef OS_OSX
  792. return OSAtomicCompareAndSwap32Barrier((int)old,(int)new,(int*)addr);
  793. #endif
  794. #ifdef OS_LINUX
  795. return __sync_bool_compare_and_swap(addr,old,new);
  796. #endif
  797. }
  798. bool cmThFloatCAS( float* addr, float old, float new )
  799. {
  800. #ifdef OS_OSX
  801. return OSAtomicCompareAndSwap32Barrier(*(int*)(&old),*(int*)(&new),(int*)addr );
  802. #endif
  803. #ifdef OS_LINUX
  804. return __sync_bool_compare_and_swap((unsigned*)addr, *(unsigned*)(&old),*(unsigned*)(&new));
  805. #endif
  806. }
  807. bool cmThPtrCAS( void* addr, void* old, void* neww )
  808. {
  809. #ifdef OS_OSX
  810. // REMOVE THIS HACK AND USE OSAtomicCompareAndSwapPtrBarrier() WHEN
  811. // A 64 BIT BUILD IS POSSIBLE ON OS-X.
  812. typedef struct
  813. {
  814. union
  815. {
  816. void* addr;
  817. #ifdef OSX_VER_10_5
  818. int val;
  819. #else
  820. int64_t val;
  821. #endif
  822. } u;
  823. } s_t;
  824. s_t ov,nv;
  825. ov.u.addr = old;
  826. nv.u.addr = neww;
  827. #ifdef OSX_VER_10_5
  828. int rv = OSAtomicCompareAndSwap32Barrier(ov.u.val,nv.u.val,(int*)addr);
  829. #else
  830. int rv = OSAtomicCompareAndSwap64Barrier(ov.u.val,nv.u.val,(int64_t*)addr);
  831. #endif
  832. return rv;
  833. #endif
  834. #ifdef OS_LINUX
  835. #ifdef OSX_VER_10_5
  836. return __sync_bool_compare_and_swap((int*)addr,(int)old,(int)neww);
  837. #else
  838. return __sync_bool_compare_and_swap((long long*)addr, (long long)old, (long long)neww);
  839. #endif
  840. #endif
  841. }
  842. void cmThIntIncr( int* addr, int incr )
  843. {
  844. #ifdef OS_OSX
  845. OSAtomicAdd32Barrier(incr,addr);
  846. #endif
  847. #ifdef OS_LINUX
  848. // ... could also use __sync_add_and_fetch() ...
  849. __sync_fetch_and_add(addr,incr);
  850. #endif
  851. }
  852. void cmThUIntIncr( unsigned* addr, unsigned incr )
  853. {
  854. #ifdef OS_OSX
  855. OSAtomicAdd32Barrier((int)incr,(int*)addr);
  856. #endif
  857. #ifdef OS_LINUX
  858. __sync_fetch_and_add(addr,incr);
  859. #endif
  860. }
  861. void cmThFloatIncr(float* addr, float incr )
  862. {
  863. float old,new;
  864. do
  865. {
  866. old = *addr;
  867. new = old + incr;
  868. }while( cmThFloatCAS(addr,old,new)==0 );
  869. }
  870. void cmThIntDecr( int* addr, int decr )
  871. {
  872. #ifdef OS_OSX
  873. OSAtomicAdd32Barrier(-decr,addr);
  874. #endif
  875. #ifdef OS_LINUX
  876. __sync_fetch_and_sub(addr,decr);
  877. #endif
  878. }
  879. void cmThUIntDecr( unsigned* addr, unsigned decr )
  880. {
  881. #ifdef OS_OSX
  882. OSAtomicAdd32Barrier(-((int)decr),(int*)addr);
  883. #endif
  884. #ifdef OS_LINUX
  885. __sync_fetch_and_sub(addr,decr);
  886. #endif
  887. }
  888. void cmThFloatDecr(float* addr, float decr )
  889. {
  890. float old,new;
  891. do
  892. {
  893. old = *addr;
  894. new = old - decr;
  895. }while( cmThFloatCAS(addr,old,new)==0 );
  896. }
  897. //============================================================================================================================
  898. //
  899. //
  900. typedef pthread_t cmThreadId_t;
  901. typedef struct
  902. {
  903. cmThreadId_t id; // id of this thread as returned by pthread_self()
  904. char* buf; // buf[bn]
  905. int ii; // input index
  906. int oi; // output index (oi==ii == empty buffer)
  907. } cmTsBuf_t;
  908. // msg header - which is actually written AFTER the msg it is associated with
  909. typedef struct cmTsHdr_str
  910. {
  911. int mn; // length of the msg
  912. int ai; // buffer index
  913. struct cmTsHdr_str* link; // pointer to next msg
  914. } cmTsHdr_t;
  915. typedef struct
  916. {
  917. cmErr_t err;
  918. int bn; // bytes per thread buffer
  919. cmTsBuf_t* a; // a[an] buffer array
  920. unsigned an; // length of a[] - one buffer per thread
  921. cmTsQueueCb_t cbFunc;
  922. void* cbArg;
  923. cmTsHdr_t* ilp; // prev msg hdr record
  924. cmTsHdr_t* olp; // prev msg hdr record (wait for olp->link to be set to go to next record)
  925. } cmTsMp1c_t;
  926. cmTsMp1cH_t cmTsMp1cNullHandle = cmSTATIC_NULL_HANDLE;
  927. void _cmTsMp1cPrint( cmTsMp1c_t* p )
  928. {
  929. unsigned i;
  930. for(i=0; i<p->an; ++i)
  931. printf("%2i ii:%3i oi:%3i\n",i,p->a[i].ii,p->a[i].oi);
  932. }
  933. cmTsMp1c_t* _cmTsMp1cHandleToPtr( cmTsMp1cH_t h )
  934. {
  935. cmTsMp1c_t* p = (cmTsMp1c_t*)h.h;
  936. assert(p != NULL);
  937. return p;
  938. }
  939. unsigned _cmTsMp1cBufIndex( cmTsMp1c_t* p, cmThreadId_t id )
  940. {
  941. unsigned i;
  942. for(i=0; i<p->an; ++i)
  943. if( p->a[i].id == id )
  944. return i;
  945. p->an = i+1;
  946. p->a = cmMemResizePZ(cmTsBuf_t,p->a,p->an);
  947. p->a[i].buf = cmMemAllocZ(char,p->bn);
  948. p->a[i].id = id;
  949. return i;
  950. }
  951. cmThRC_t cmTsMp1cDestroy( cmTsMp1cH_t* hp )
  952. {
  953. if( hp == NULL || cmTsMp1cIsValid(*hp) == false )
  954. return kOkThRC;
  955. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(*hp);
  956. unsigned i;
  957. for(i=0; i<p->an; ++i)
  958. cmMemFree(p->a[i].buf);
  959. cmMemPtrFree(&p->a);
  960. cmMemFree(p);
  961. hp->h = NULL;
  962. return kOkThRC;
  963. }
  964. cmThRC_t cmTsMp1cCreate( cmTsMp1cH_t* hp, unsigned bufByteCnt, cmTsQueueCb_t cbFunc, void* cbArg, cmRpt_t* rpt )
  965. {
  966. cmThRC_t rc;
  967. if((rc = cmTsMp1cDestroy(hp)) != kOkThRC )
  968. return rc;
  969. cmTsMp1c_t* p = cmMemAllocZ(cmTsMp1c_t,1);
  970. cmErrSetup(&p->err,rpt,"TsMp1c Queue");
  971. p->a = NULL;
  972. p->an = 0;
  973. p->bn = bufByteCnt;
  974. p->cbFunc = cbFunc;
  975. p->cbArg = cbArg;
  976. p->ilp = NULL;
  977. p->olp = NULL;
  978. hp->h = p;
  979. return rc;
  980. }
  981. void cmTsMp1cSetCbFunc( cmTsMp1cH_t h, cmTsQueueCb_t cbFunc, void* cbArg )
  982. {
  983. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  984. p->cbFunc = cbFunc;
  985. p->cbArg = cbArg;
  986. }
  987. cmTsQueueCb_t cmTsMp1cCbFunc( cmTsMp1cH_t h )
  988. {
  989. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  990. return p->cbFunc;
  991. }
  992. void* cmTsMp1cCbArg( cmTsMp1cH_t h )
  993. {
  994. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  995. return p->cbArg;
  996. }
  997. //#define CAS(addr,old,new) __sync_bool_compare_and_swap(addr,old,new)
  998. //#define CAS(addr,old,neww) cmThPtrCAS(addr,old,neww)
  999. cmThRC_t cmTsMp1cEnqueueSegMsg( cmTsMp1cH_t h, const void* msgPtrArray[], unsigned msgByteCntArray[], unsigned arrayCnt )
  1000. {
  1001. cmThRC_t rc = kOkThRC;
  1002. unsigned mn = 0;
  1003. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1004. unsigned ai = _cmTsMp1cBufIndex( p, pthread_self() );
  1005. cmTsBuf_t* b = p->a + ai;
  1006. int i,bi,ei;
  1007. cmTsHdr_t hdr;
  1008. // Use a stored oi for the duration of this function.
  1009. // b->oi may be changed by the dequeue thread but storing it here
  1010. // at least prevents it from changing during the course of the this function.
  1011. // Note: b->oi is only used to check for buffer full. Even if it changes
  1012. // it would only mean that more bytes were available than calculated based
  1013. // on the stored value. A low estimate of the actual bytes available is
  1014. // never unsafe.
  1015. volatile int oi = b->oi;
  1016. // get the total count of bytes for this msg
  1017. for(i=0; i<arrayCnt; ++i)
  1018. mn += msgByteCntArray[i];
  1019. // dn = count of msg bytes + count of header bytes
  1020. int dn = mn + sizeof(hdr);
  1021. // if oi is ahead of ii in the queue then we must write
  1022. // in the area between ii and oi
  1023. if( oi > b->ii )
  1024. {
  1025. ei = oi-1; // (never allow ii to equal oi (that's the empty condition))
  1026. bi = b->ii;
  1027. }
  1028. else // otherwise oi is same or before ii in the queue and we have the option to wrap
  1029. {
  1030. // if the new msg will not fit at the end of the queue ....
  1031. if( b->ii + dn > p->bn )
  1032. {
  1033. bi = 0; // ... then wrap to the beginning
  1034. ei = oi-1; // (never allow ii to equal oi (that's the empty condition))
  1035. }
  1036. else
  1037. {
  1038. ei = p->bn; // otherwise write at the current location
  1039. bi = b->ii;
  1040. }
  1041. }
  1042. if( bi + dn > ei )
  1043. return cmErrMsg(&p->err,kBufFullThRC,"%i consecutive bytes is not available in the queue.",dn);
  1044. char* dp = b->buf + bi;
  1045. // write the msg
  1046. for(i=0; i<arrayCnt; ++i)
  1047. {
  1048. memcpy(dp,msgPtrArray[i],msgByteCntArray[i]);
  1049. dp += msgByteCntArray[i];
  1050. }
  1051. // setup the msg header
  1052. hdr.ai = ai;
  1053. hdr.mn = mn;
  1054. hdr.link = NULL;
  1055. // write the msg header (following the msg body in memory)
  1056. cmTsHdr_t* hp = (cmTsHdr_t*)dp;
  1057. memcpy(hp,&hdr,sizeof(hdr));
  1058. // increment the buffers input index
  1059. b->ii = bi + dn;
  1060. // update the link list head to point to this msg hdr
  1061. cmTsHdr_t* old_hp, *new_hp;
  1062. do
  1063. {
  1064. old_hp = p->ilp;
  1065. new_hp = hp;
  1066. }while(!cmThPtrCAS(&p->ilp,old_hp,new_hp));
  1067. // link the prev recd to this recd
  1068. if( old_hp != NULL )
  1069. old_hp->link = hp;
  1070. // if this is the first record written by this queue then prime the output list
  1071. do
  1072. {
  1073. old_hp = p->olp;
  1074. new_hp = hp;
  1075. if( old_hp != NULL )
  1076. break;
  1077. }while(!cmThPtrCAS(&p->olp,old_hp,new_hp));
  1078. //printf("%p %p %i\n",p->ilp,p->olp,p->olp->mn);
  1079. return rc;
  1080. }
  1081. cmThRC_t cmTsMp1cEnqueueMsg( cmTsMp1cH_t h, const void* dataPtr, unsigned byteCnt )
  1082. { return cmTsMp1cEnqueueSegMsg(h,&dataPtr,&byteCnt,1); }
  1083. unsigned cmTsMp1cAllocByteCount( cmTsMp1cH_t h )
  1084. {
  1085. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1086. return p->bn;
  1087. }
  1088. unsigned cmTsMp1cAvailByteCount( cmTsMp1cH_t h )
  1089. {
  1090. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1091. unsigned ai = _cmTsMp1cBufIndex(p,pthread_self());
  1092. const cmTsBuf_t* b = p->a + ai;
  1093. if( b->oi > b->ii )
  1094. return b->oi - b->ii - 1;
  1095. return (p->bn - b->ii) + b->oi - 1;
  1096. }
  1097. unsigned _cmTsMp1cNextMsgByteCnt( cmTsMp1c_t* p )
  1098. {
  1099. if( p->olp == NULL )
  1100. return 0;
  1101. // if the current msg has not yet been read
  1102. if( p->olp->mn != 0 )
  1103. return p->olp->mn;
  1104. // if the current msg has been read but a new next msg has been linked
  1105. if( p->olp->mn == 0 && p->olp->link != NULL )
  1106. {
  1107. // advance the buffer output msg past the prev msg header
  1108. char* hp = (char*)(p->olp + 1);
  1109. p->a[p->olp->ai].oi = hp - p->a[p->olp->ai].buf;
  1110. // advance msg pointer to point to the new msg header
  1111. p->olp = p->olp->link;
  1112. // return the size of the new msg
  1113. return p->olp->mn;
  1114. }
  1115. return 0;
  1116. }
  1117. cmThRC_t cmTsMp1cDequeueMsg( cmTsMp1cH_t h, void* dataPtr, unsigned byteCnt )
  1118. {
  1119. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1120. // if there are no messages waiting
  1121. if( _cmTsMp1cNextMsgByteCnt(p) == 0 )
  1122. return kBufEmptyThRC;
  1123. char* hp = (char*)p->olp;
  1124. char* dp = hp - p->olp->mn; // the msg body is before the msg hdr
  1125. if( dataPtr == NULL )
  1126. {
  1127. p->cbFunc(p->cbArg,p->olp->mn,dp);
  1128. }
  1129. else
  1130. {
  1131. if( p->olp->mn > byteCnt )
  1132. return cmErrMsg(&p->err,kBufTooSmallThRC,"The return buffer constains too few bytes (%i) to contain %i bytes.",byteCnt,p->olp->mn);
  1133. memcpy(dataPtr,dp,p->olp->mn);
  1134. }
  1135. // advance the buffers output index past the msg body
  1136. p->a[p->olp->ai].oi = hp - p->a[p->olp->ai].buf;
  1137. // mark the msg as read
  1138. p->olp->mn = 0;
  1139. return kOkThRC;
  1140. }
  1141. bool cmTsMp1cMsgWaiting( cmTsMp1cH_t h )
  1142. {
  1143. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1144. return _cmTsMp1cNextMsgByteCnt(p) != 0;
  1145. }
  1146. unsigned cmTsMp1cDequeueMsgByteCount( cmTsMp1cH_t h )
  1147. {
  1148. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1149. return _cmTsMp1cNextMsgByteCnt(p);
  1150. }
  1151. bool cmTsMp1cIsValid( cmTsMp1cH_t h )
  1152. { return h.h != NULL; }
  1153. //============================================================================================================================/
  1154. //
  1155. // cmTsQueueTest()
  1156. //
  1157. // param recd for use by _cmTsQueueCb0() and
  1158. // the msg record passed between the sender
  1159. // threads and the receiver thread
  1160. typedef struct
  1161. {
  1162. unsigned id;
  1163. cmTsQueueH_t qH;
  1164. int val;
  1165. } _cmTsQCbParam_t;
  1166. // Generate a random number and put it in a TS queue
  1167. bool _cmTsQueueCb0(void* param)
  1168. {
  1169. _cmTsQCbParam_t* p = (_cmTsQCbParam_t*)param;
  1170. p->val = rand(); // generate a random number
  1171. // send the msg
  1172. if( cmTsQueueEnqueueMsg( p->qH, p, sizeof(_cmTsQCbParam_t)) == kOkThRC )
  1173. printf("in:%i %i\n",p->id,p->val);
  1174. else
  1175. printf("in error %i\n",p->id);
  1176. cmSleepUs(100*1000);
  1177. return true;
  1178. }
  1179. // Monitor a TS queue for incoming messages from _cmTsQueueCb1()
  1180. bool _cmTsQueueCb1(void* param)
  1181. {
  1182. // the thread param is a ptr to the TS queue to monitor.
  1183. cmTsQueueH_t* qp = (cmTsQueueH_t*)param;
  1184. cmThRC_t rc;
  1185. _cmTsQCbParam_t msg;
  1186. // dequeue any waiting messages
  1187. if((rc = cmTsQueueDequeueMsg( *qp, &msg, sizeof(msg))) == kOkThRC )
  1188. printf("out:%i %i\n",msg.id,msg.val);
  1189. else
  1190. {
  1191. if( rc != kBufEmptyThRC )
  1192. printf("out error:%i\n", rc);
  1193. }
  1194. return true;
  1195. }
  1196. // Test the TS queue by starting sender threads (threads 0 & 1)
  1197. // and a receiver thread (thread 2) and sending messages
  1198. // from the sender to the receiver.
  1199. void cmTsQueueTest( cmRpt_t* rpt )
  1200. {
  1201. cmThreadH_t th0=cmThreadNullHandle,th1=cmThreadNullHandle,th2=cmThreadNullHandle;
  1202. cmTsQueueH_t q=cmTsQueueNullHandle;
  1203. _cmTsQCbParam_t param0, param1;
  1204. // create a TS Queue
  1205. if( cmTsQueueCreate(&q,100,NULL,NULL,rpt) != kOkThRC )
  1206. goto errLabel;
  1207. // create thread 0
  1208. param0.id = 0;
  1209. param0.qH = q;
  1210. if( cmThreadCreate(&th0,_cmTsQueueCb0,&param0,rpt) != kOkThRC )
  1211. goto errLabel;
  1212. // create thread 1
  1213. param1.id = 1;
  1214. param1.qH = q;
  1215. if( cmThreadCreate(&th1,_cmTsQueueCb0,&param1,rpt) != kOkThRC )
  1216. goto errLabel;
  1217. // create thread 2
  1218. if( cmThreadCreate(&th2,_cmTsQueueCb1,&q,rpt) != kOkThRC )
  1219. goto errLabel;
  1220. // start thread 0
  1221. if( cmThreadPause(th0,0) != kOkThRC )
  1222. goto errLabel;
  1223. // start thread 1
  1224. if( cmThreadPause(th1,0) != kOkThRC )
  1225. goto errLabel;
  1226. // start thread 2
  1227. if( cmThreadPause(th2,0) != kOkThRC )
  1228. goto errLabel;
  1229. printf("any key to quit.");
  1230. getchar();
  1231. errLabel:
  1232. if( cmThreadIsValid(th0) )
  1233. if( cmThreadDestroy(&th0) != kOkThRC )
  1234. printf("Error destroying thread 0\n");
  1235. if( cmThreadIsValid(th1) )
  1236. if( cmThreadDestroy(&th1) != kOkThRC )
  1237. printf("Error destroying thread 1\n");
  1238. if( cmThreadIsValid(th2) )
  1239. if( cmThreadDestroy(&th2) != kOkThRC )
  1240. printf("Error destroying thread 1\n");
  1241. if( cmTsQueueIsValid(q) )
  1242. if( cmTsQueueDestroy(&q) != kOkThRC )
  1243. printf("Error destroying queue\n");
  1244. }
  1245. //============================================================================================================================/
  1246. //
  1247. // cmTs1p1cTest()
  1248. //
  1249. // param recd for use by _cmTsQueueCb0() and
  1250. // the msg record passed between the sender
  1251. // threads and the receiver thread
  1252. typedef struct
  1253. {
  1254. unsigned id;
  1255. cmTs1p1cH_t qH;
  1256. int val;
  1257. } _cmTs1p1cCbParam_t;
  1258. cmTs1p1cH_t cmTs1p1cNullHandle = cmSTATIC_NULL_HANDLE;
  1259. // Generate a random number and put it in a TS queue
  1260. bool _cmTs1p1cCb0(void* param)
  1261. {
  1262. _cmTs1p1cCbParam_t* p = (_cmTs1p1cCbParam_t*)param;
  1263. p->val = rand(); // generate a random number
  1264. // send the msg
  1265. if( cmTs1p1cEnqueueMsg( p->qH, p, sizeof(_cmTs1p1cCbParam_t)) == kOkThRC )
  1266. printf("in:%i %i\n",p->id,p->val);
  1267. else
  1268. printf("in error %i\n",p->id);
  1269. ++p->id;
  1270. cmSleepUs(100*1000);
  1271. return true;
  1272. }
  1273. // Monitor a TS queue for incoming messages from _cmTs1p1cCb1()
  1274. bool _cmTs1p1cCb1(void* param)
  1275. {
  1276. // the thread param is a ptr to the TS queue to monitor.
  1277. cmTs1p1cH_t* qp = (cmTs1p1cH_t*)param;
  1278. cmThRC_t rc;
  1279. _cmTs1p1cCbParam_t msg;
  1280. // dequeue any waiting messages
  1281. if((rc = cmTs1p1cDequeueMsg( *qp, &msg, sizeof(msg))) == kOkThRC )
  1282. printf("out:%i %i\n",msg.id,msg.val);
  1283. else
  1284. {
  1285. if( rc != kBufEmptyThRC )
  1286. printf("out error:%i\n", rc);
  1287. }
  1288. return true;
  1289. }
  1290. // Test the TS queue by starting sender threads (threads 0 & 1)
  1291. // and a receiver thread (thread 2) and sending messages
  1292. // from the sender to the receiver.
  1293. void cmTs1p1cTest( cmRpt_t* rpt )
  1294. {
  1295. cmThreadH_t th0=cmThreadNullHandle,th1=cmThreadNullHandle,th2=cmThreadNullHandle;
  1296. cmTs1p1cH_t q=cmTs1p1cNullHandle;
  1297. _cmTs1p1cCbParam_t param1;
  1298. // create a TS Queue
  1299. if( cmTs1p1cCreate(&q,28*2,NULL,NULL,rpt) != kOkThRC )
  1300. goto errLabel;
  1301. // create thread 1
  1302. param1.id = 0;
  1303. param1.qH = q;
  1304. if( cmThreadCreate(&th1,_cmTs1p1cCb0,&param1,rpt) != kOkThRC )
  1305. goto errLabel;
  1306. // create thread 2
  1307. if( cmThreadCreate(&th2,_cmTs1p1cCb1,&q,rpt) != kOkThRC )
  1308. goto errLabel;
  1309. // start thread 1
  1310. if( cmThreadPause(th1,0) != kOkThRC )
  1311. goto errLabel;
  1312. // start thread 2
  1313. if( cmThreadPause(th2,0) != kOkThRC )
  1314. goto errLabel;
  1315. printf("any key to quit.");
  1316. getchar();
  1317. errLabel:
  1318. if( cmThreadIsValid(th0) )
  1319. if( cmThreadDestroy(&th0) != kOkThRC )
  1320. printf("Error destroying thread 0\n");
  1321. if( cmThreadIsValid(th1) )
  1322. if( cmThreadDestroy(&th1) != kOkThRC )
  1323. printf("Error destroying thread 1\n");
  1324. if( cmThreadIsValid(th2) )
  1325. if( cmThreadDestroy(&th2) != kOkThRC )
  1326. printf("Error destroying thread 1\n");
  1327. if( cmTs1p1cIsValid(q) )
  1328. if( cmTs1p1cDestroy(&q) != kOkThRC )
  1329. printf("Error destroying queue\n");
  1330. }
  1331. //============================================================================================================================/
  1332. //
  1333. // cmTsMp1cTest()
  1334. //
  1335. // param recd for use by _cmTsQueueCb0() and
  1336. // the msg record passed between the sender
  1337. // threads and the receiver thread
  1338. typedef struct
  1339. {
  1340. unsigned id;
  1341. cmTsMp1cH_t qH;
  1342. int val;
  1343. } _cmTsMp1cCbParam_t;
  1344. unsigned _cmTsMp1cVal = 0;
  1345. // Incr the global value _cmTsMp1cVal and put it in a TS queue
  1346. bool _cmTsMp1cCb0(void* param)
  1347. {
  1348. _cmTsMp1cCbParam_t* p = (_cmTsMp1cCbParam_t*)param;
  1349. //p->val = __sync_fetch_and_add(&_cmTsMp1cVal,1);
  1350. cmThUIntIncr(&_cmTsMp1cVal,1);
  1351. p->val = _cmTsMp1cVal;
  1352. // send the msg
  1353. if( cmTsMp1cEnqueueMsg( p->qH, p, sizeof(_cmTsMp1cCbParam_t)) == kOkThRC )
  1354. printf("in:%i %i\n",p->id,p->val);
  1355. else
  1356. printf("in error %i\n",p->id);
  1357. cmSleepUs(100*1000);
  1358. return true;
  1359. }
  1360. // Monitor a TS queue for incoming messages from _cmTsMp1cCb1()
  1361. bool _cmTsMp1cCb1(void* param)
  1362. {
  1363. // the thread param is a ptr to the TS queue to monitor.
  1364. cmTsMp1cH_t* qp = (cmTsMp1cH_t*)param;
  1365. cmThRC_t rc;
  1366. _cmTsMp1cCbParam_t msg;
  1367. // dequeue any waiting messages
  1368. if((rc = cmTsMp1cDequeueMsg( *qp, &msg, sizeof(msg))) == kOkThRC )
  1369. printf("out - cons id:%i val:%i\n",msg.id,msg.val);
  1370. else
  1371. {
  1372. if( rc != kBufEmptyThRC )
  1373. printf("out error:%i\n", rc);
  1374. }
  1375. return true;
  1376. }
  1377. // Test the TS queue by starting sender threads (threads 0 & 1)
  1378. // and a receiver thread (thread 2) and sending messages
  1379. // from the sender to the receiver.
  1380. void cmTsMp1cTest( cmRpt_t* rpt )
  1381. {
  1382. cmThreadH_t th0=cmThreadNullHandle,th1=cmThreadNullHandle,th2=cmThreadNullHandle;
  1383. cmTsMp1cH_t q=cmTsMp1cNullHandle;
  1384. _cmTsMp1cCbParam_t param0, param1;
  1385. // create a TS Queue
  1386. if( cmTsMp1cCreate(&q,1000,NULL,NULL,rpt) != kOkThRC )
  1387. goto errLabel;
  1388. // create thread 0 - producer 0
  1389. param0.id = 0;
  1390. param0.qH = q;
  1391. if( cmThreadCreate(&th0,_cmTsMp1cCb0,&param0,rpt) != kOkThRC )
  1392. goto errLabel;
  1393. // create thread 1 - producer 1
  1394. param1.id = 1;
  1395. param1.qH = q;
  1396. if( cmThreadCreate(&th1,_cmTsMp1cCb0,&param1,rpt) != kOkThRC )
  1397. goto errLabel;
  1398. // create thread 2 - consumer 0
  1399. if( cmThreadCreate(&th2,_cmTsMp1cCb1,&q,rpt) != kOkThRC )
  1400. goto errLabel;
  1401. // start thread 0
  1402. if( cmThreadPause(th0,0) != kOkThRC )
  1403. goto errLabel;
  1404. // start thread 1
  1405. if( cmThreadPause(th1,0) != kOkThRC )
  1406. goto errLabel;
  1407. // start thread 2
  1408. if( cmThreadPause(th2,0) != kOkThRC )
  1409. goto errLabel;
  1410. printf("any key to quit.");
  1411. getchar();
  1412. errLabel:
  1413. if( cmThreadIsValid(th0) )
  1414. if( cmThreadDestroy(&th0) != kOkThRC )
  1415. printf("Error destroying thread 0\n");
  1416. if( cmThreadIsValid(th1) )
  1417. if( cmThreadDestroy(&th1) != kOkThRC )
  1418. printf("Error destroying thread 1\n");
  1419. if( cmThreadIsValid(th2) )
  1420. if( cmThreadDestroy(&th2) != kOkThRC )
  1421. printf("Error destroying thread 1\n");
  1422. if( cmTsMp1cIsValid(q) )
  1423. if( cmTsMp1cDestroy(&q) != kOkThRC )
  1424. printf("Error destroying queue\n");
  1425. }
  1426. void cmSleepUs( unsigned microseconds )
  1427. { usleep(microseconds); }
  1428. void cmSleepMs( unsigned milliseconds )
  1429. { cmSleepUs(milliseconds*1000); }