libcm is a C development framework with an emphasis on audio signal processing applications.
Você não pode selecionar mais de 25 tópicos Os tópicos devem começar com uma letra ou um número, podem incluir traços ('-') e podem ter até 35 caracteres.

cmThread.c 48KB

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