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 44KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886
  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. // If we use pointer aliasing to pun the pointer types then it will violate the
  805. // strict pointer aliasing rules used by -O2. Instead we use this union
  806. // punning scheme which in theory should always work in C99 (although no C++).
  807. typedef union
  808. {
  809. unsigned* up;
  810. float* fp;
  811. } u;
  812. u u0,u1;
  813. u0.fp = &old;
  814. u1.fp = &new;
  815. return __sync_bool_compare_and_swap((unsigned*)addr,*u0.up,*u1.up);
  816. #endif
  817. }
  818. bool cmThPtrCAS( void* addr, void* old, void* neww )
  819. {
  820. #ifdef OS_OSX
  821. // REMOVE THIS HACK AND USE OSAtomicCompareAndSwapPtrBarrier() WHEN
  822. // A 64 BIT BUILD IS POSSIBLE ON OS-X.
  823. typedef struct
  824. {
  825. union
  826. {
  827. void* addr;
  828. #ifdef OSX_VER_10_5
  829. int val;
  830. #else
  831. int64_t val;
  832. #endif
  833. } u;
  834. } s_t;
  835. s_t ov,nv;
  836. ov.u.addr = old;
  837. nv.u.addr = neww;
  838. #ifdef OSX_VER_10_5
  839. int rv = OSAtomicCompareAndSwap32Barrier(ov.u.val,nv.u.val,(int*)addr);
  840. #else
  841. int rv = OSAtomicCompareAndSwap64Barrier(ov.u.val,nv.u.val,(int64_t*)addr);
  842. #endif
  843. return rv;
  844. #endif
  845. #ifdef OS_LINUX
  846. #ifdef OSX_VER_10_5
  847. return __sync_bool_compare_and_swap((int*)addr,(int)old,(int)neww);
  848. #else
  849. return __sync_bool_compare_and_swap((long long*)addr, (long long)old, (long long)neww);
  850. #endif
  851. #endif
  852. }
  853. void cmThIntIncr( int* addr, int incr )
  854. {
  855. #ifdef OS_OSX
  856. OSAtomicAdd32Barrier(incr,addr);
  857. #endif
  858. #ifdef OS_LINUX
  859. // ... could also use __sync_add_and_fetch() ...
  860. __sync_fetch_and_add(addr,incr);
  861. #endif
  862. }
  863. void cmThUIntIncr( unsigned* addr, unsigned incr )
  864. {
  865. #ifdef OS_OSX
  866. OSAtomicAdd32Barrier((int)incr,(int*)addr);
  867. #endif
  868. #ifdef OS_LINUX
  869. __sync_fetch_and_add(addr,incr);
  870. #endif
  871. }
  872. void cmThFloatIncr(float* addr, float incr )
  873. {
  874. float old,new;
  875. do
  876. {
  877. old = *addr;
  878. new = old + incr;
  879. }while( cmThFloatCAS(addr,old,new)==0 );
  880. }
  881. void cmThIntDecr( int* addr, int decr )
  882. {
  883. #ifdef OS_OSX
  884. OSAtomicAdd32Barrier(-decr,addr);
  885. #endif
  886. #ifdef OS_LINUX
  887. __sync_fetch_and_sub(addr,decr);
  888. #endif
  889. }
  890. void cmThUIntDecr( unsigned* addr, unsigned decr )
  891. {
  892. #ifdef OS_OSX
  893. OSAtomicAdd32Barrier(-((int)decr),(int*)addr);
  894. #endif
  895. #ifdef OS_LINUX
  896. __sync_fetch_and_sub(addr,decr);
  897. #endif
  898. }
  899. void cmThFloatDecr(float* addr, float decr )
  900. {
  901. float old,new;
  902. do
  903. {
  904. old = *addr;
  905. new = old - decr;
  906. }while( cmThFloatCAS(addr,old,new)==0 );
  907. }
  908. //============================================================================================================================
  909. //
  910. //
  911. typedef pthread_t cmThreadId_t;
  912. typedef struct
  913. {
  914. cmThreadId_t id; // id of this thread as returned by pthread_self()
  915. char* buf; // buf[bn]
  916. int ii; // input index
  917. int oi; // output index (oi==ii == empty buffer)
  918. } cmTsBuf_t;
  919. // msg header - which is actually written AFTER the msg it is associated with
  920. typedef struct cmTsHdr_str
  921. {
  922. int mn; // length of the msg
  923. int ai; // buffer index
  924. struct cmTsHdr_str* link; // pointer to next msg
  925. } cmTsHdr_t;
  926. typedef struct
  927. {
  928. cmErr_t err;
  929. int bn; // bytes per thread buffer
  930. cmTsBuf_t* a; // a[an] buffer array
  931. unsigned an; // length of a[] - one buffer per thread
  932. cmTsQueueCb_t cbFunc;
  933. void* cbArg;
  934. cmTsHdr_t* ilp; // prev msg hdr record
  935. cmTsHdr_t* olp; // prev msg hdr record (wait for olp->link to be set to go to next record)
  936. } cmTsMp1c_t;
  937. cmTsMp1cH_t cmTsMp1cNullHandle = cmSTATIC_NULL_HANDLE;
  938. void _cmTsMp1cPrint( cmTsMp1c_t* p )
  939. {
  940. unsigned i;
  941. for(i=0; i<p->an; ++i)
  942. printf("%2i ii:%3i oi:%3i\n",i,p->a[i].ii,p->a[i].oi);
  943. }
  944. cmTsMp1c_t* _cmTsMp1cHandleToPtr( cmTsMp1cH_t h )
  945. {
  946. cmTsMp1c_t* p = (cmTsMp1c_t*)h.h;
  947. assert(p != NULL);
  948. return p;
  949. }
  950. unsigned _cmTsMp1cBufIndex( cmTsMp1c_t* p, cmThreadId_t id )
  951. {
  952. unsigned i;
  953. for(i=0; i<p->an; ++i)
  954. if( p->a[i].id == id )
  955. return i;
  956. p->an = i+1;
  957. p->a = cmMemResizePZ(cmTsBuf_t,p->a,p->an);
  958. p->a[i].buf = cmMemAllocZ(char,p->bn);
  959. p->a[i].id = id;
  960. return i;
  961. }
  962. cmThRC_t cmTsMp1cDestroy( cmTsMp1cH_t* hp )
  963. {
  964. if( hp == NULL || cmTsMp1cIsValid(*hp) == false )
  965. return kOkThRC;
  966. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(*hp);
  967. unsigned i;
  968. for(i=0; i<p->an; ++i)
  969. cmMemFree(p->a[i].buf);
  970. cmMemPtrFree(&p->a);
  971. cmMemFree(p);
  972. hp->h = NULL;
  973. return kOkThRC;
  974. }
  975. cmThRC_t cmTsMp1cCreate( cmTsMp1cH_t* hp, unsigned bufByteCnt, cmTsQueueCb_t cbFunc, void* cbArg, cmRpt_t* rpt )
  976. {
  977. cmThRC_t rc;
  978. if((rc = cmTsMp1cDestroy(hp)) != kOkThRC )
  979. return rc;
  980. cmTsMp1c_t* p = cmMemAllocZ(cmTsMp1c_t,1);
  981. cmErrSetup(&p->err,rpt,"TsMp1c Queue");
  982. p->a = NULL;
  983. p->an = 0;
  984. p->bn = bufByteCnt;
  985. p->cbFunc = cbFunc;
  986. p->cbArg = cbArg;
  987. p->ilp = NULL;
  988. p->olp = NULL;
  989. hp->h = p;
  990. return rc;
  991. }
  992. void cmTsMp1cSetCbFunc( cmTsMp1cH_t h, cmTsQueueCb_t cbFunc, void* cbArg )
  993. {
  994. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  995. p->cbFunc = cbFunc;
  996. p->cbArg = cbArg;
  997. }
  998. cmTsQueueCb_t cmTsMp1cCbFunc( cmTsMp1cH_t h )
  999. {
  1000. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1001. return p->cbFunc;
  1002. }
  1003. void* cmTsMp1cCbArg( cmTsMp1cH_t h )
  1004. {
  1005. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1006. return p->cbArg;
  1007. }
  1008. //#define CAS(addr,old,new) __sync_bool_compare_and_swap(addr,old,new)
  1009. //#define CAS(addr,old,neww) cmThPtrCAS(addr,old,neww)
  1010. cmThRC_t cmTsMp1cEnqueueSegMsg( cmTsMp1cH_t h, const void* msgPtrArray[], unsigned msgByteCntArray[], unsigned arrayCnt )
  1011. {
  1012. cmThRC_t rc = kOkThRC;
  1013. unsigned mn = 0;
  1014. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1015. unsigned ai = _cmTsMp1cBufIndex( p, pthread_self() );
  1016. cmTsBuf_t* b = p->a + ai;
  1017. int i,bi,ei;
  1018. cmTsHdr_t hdr;
  1019. // Use a stored oi for the duration of this function.
  1020. // b->oi may be changed by the dequeue thread but storing it here
  1021. // at least prevents it from changing during the course of the this function.
  1022. // Note: b->oi is only used to check for buffer full. Even if it changes
  1023. // it would only mean that more bytes were available than calculated based
  1024. // on the stored value. A low estimate of the actual bytes available is
  1025. // never unsafe.
  1026. volatile int oi = b->oi;
  1027. // get the total count of bytes for this msg
  1028. for(i=0; i<arrayCnt; ++i)
  1029. mn += msgByteCntArray[i];
  1030. // dn = count of msg bytes + count of header bytes
  1031. int dn = mn + sizeof(hdr);
  1032. // if oi is ahead of ii in the queue then we must write
  1033. // in the area between ii and oi
  1034. if( oi > b->ii )
  1035. {
  1036. ei = oi-1; // (never allow ii to equal oi (that's the empty condition))
  1037. bi = b->ii;
  1038. }
  1039. else // otherwise oi is same or before ii in the queue and we have the option to wrap
  1040. {
  1041. // if the new msg will not fit at the end of the queue ....
  1042. if( b->ii + dn > p->bn )
  1043. {
  1044. bi = 0; // ... then wrap to the beginning
  1045. ei = oi-1; // (never allow ii to equal oi (that's the empty condition))
  1046. }
  1047. else
  1048. {
  1049. ei = p->bn; // otherwise write at the current location
  1050. bi = b->ii;
  1051. }
  1052. }
  1053. if( bi + dn > ei )
  1054. return cmErrMsg(&p->err,kBufFullThRC,"%i consecutive bytes is not available in the queue.",dn);
  1055. char* dp = b->buf + bi;
  1056. // write the msg
  1057. for(i=0; i<arrayCnt; ++i)
  1058. {
  1059. memcpy(dp,msgPtrArray[i],msgByteCntArray[i]);
  1060. dp += msgByteCntArray[i];
  1061. }
  1062. // setup the msg header
  1063. hdr.ai = ai;
  1064. hdr.mn = mn;
  1065. hdr.link = NULL;
  1066. // write the msg header (following the msg body in memory)
  1067. cmTsHdr_t* hp = (cmTsHdr_t*)dp;
  1068. memcpy(hp,&hdr,sizeof(hdr));
  1069. // increment the buffers input index
  1070. b->ii = bi + dn;
  1071. // update the link list head to point to this msg hdr
  1072. cmTsHdr_t* old_hp, *new_hp;
  1073. do
  1074. {
  1075. old_hp = p->ilp;
  1076. new_hp = hp;
  1077. }while(!cmThPtrCAS(&p->ilp,old_hp,new_hp));
  1078. // link the prev recd to this recd
  1079. if( old_hp != NULL )
  1080. old_hp->link = hp;
  1081. // if this is the first record written by this queue then prime the output list
  1082. do
  1083. {
  1084. old_hp = p->olp;
  1085. new_hp = hp;
  1086. if( old_hp != NULL )
  1087. break;
  1088. }while(!cmThPtrCAS(&p->olp,old_hp,new_hp));
  1089. //printf("%p %p %i\n",p->ilp,p->olp,p->olp->mn);
  1090. return rc;
  1091. }
  1092. cmThRC_t cmTsMp1cEnqueueMsg( cmTsMp1cH_t h, const void* dataPtr, unsigned byteCnt )
  1093. { return cmTsMp1cEnqueueSegMsg(h,&dataPtr,&byteCnt,1); }
  1094. unsigned cmTsMp1cAllocByteCount( cmTsMp1cH_t h )
  1095. {
  1096. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1097. return p->bn;
  1098. }
  1099. unsigned cmTsMp1cAvailByteCount( cmTsMp1cH_t h )
  1100. {
  1101. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1102. unsigned ai = _cmTsMp1cBufIndex(p,pthread_self());
  1103. const cmTsBuf_t* b = p->a + ai;
  1104. if( b->oi > b->ii )
  1105. return b->oi - b->ii - 1;
  1106. return (p->bn - b->ii) + b->oi - 1;
  1107. }
  1108. unsigned _cmTsMp1cNextMsgByteCnt( cmTsMp1c_t* p )
  1109. {
  1110. if( p->olp == NULL )
  1111. return 0;
  1112. // if the current msg has not yet been read
  1113. if( p->olp->mn != 0 )
  1114. return p->olp->mn;
  1115. // if the current msg has been read but a new next msg has been linked
  1116. if( p->olp->mn == 0 && p->olp->link != NULL )
  1117. {
  1118. // advance the buffer output msg past the prev msg header
  1119. char* hp = (char*)(p->olp + 1);
  1120. p->a[p->olp->ai].oi = hp - p->a[p->olp->ai].buf;
  1121. // advance msg pointer to point to the new msg header
  1122. p->olp = p->olp->link;
  1123. // return the size of the new msg
  1124. return p->olp->mn;
  1125. }
  1126. return 0;
  1127. }
  1128. cmThRC_t cmTsMp1cDequeueMsg( cmTsMp1cH_t h, void* dataPtr, unsigned byteCnt )
  1129. {
  1130. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1131. // if there are no messages waiting
  1132. if( _cmTsMp1cNextMsgByteCnt(p) == 0 )
  1133. return kBufEmptyThRC;
  1134. char* hp = (char*)p->olp;
  1135. char* dp = hp - p->olp->mn; // the msg body is before the msg hdr
  1136. if( dataPtr == NULL )
  1137. {
  1138. p->cbFunc(p->cbArg,p->olp->mn,dp);
  1139. }
  1140. else
  1141. {
  1142. if( p->olp->mn > byteCnt )
  1143. return cmErrMsg(&p->err,kBufTooSmallThRC,"The return buffer constains too few bytes (%i) to contain %i bytes.",byteCnt,p->olp->mn);
  1144. memcpy(dataPtr,dp,p->olp->mn);
  1145. }
  1146. // advance the buffers output index past the msg body
  1147. p->a[p->olp->ai].oi = hp - p->a[p->olp->ai].buf;
  1148. // mark the msg as read
  1149. p->olp->mn = 0;
  1150. return kOkThRC;
  1151. }
  1152. bool cmTsMp1cMsgWaiting( cmTsMp1cH_t h )
  1153. {
  1154. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1155. return _cmTsMp1cNextMsgByteCnt(p) != 0;
  1156. }
  1157. unsigned cmTsMp1cDequeueMsgByteCount( cmTsMp1cH_t h )
  1158. {
  1159. cmTsMp1c_t* p = _cmTsMp1cHandleToPtr(h);
  1160. return _cmTsMp1cNextMsgByteCnt(p);
  1161. }
  1162. bool cmTsMp1cIsValid( cmTsMp1cH_t h )
  1163. { return h.h != NULL; }
  1164. //============================================================================================================================/
  1165. //
  1166. // cmTsQueueTest()
  1167. //
  1168. // param recd for use by _cmTsQueueCb0() and
  1169. // the msg record passed between the sender
  1170. // threads and the receiver thread
  1171. typedef struct
  1172. {
  1173. unsigned id;
  1174. cmTsQueueH_t qH;
  1175. int val;
  1176. } _cmTsQCbParam_t;
  1177. // Generate a random number and put it in a TS queue
  1178. bool _cmTsQueueCb0(void* param)
  1179. {
  1180. _cmTsQCbParam_t* p = (_cmTsQCbParam_t*)param;
  1181. p->val = rand(); // generate a random number
  1182. // send the msg
  1183. if( cmTsQueueEnqueueMsg( p->qH, p, sizeof(_cmTsQCbParam_t)) == kOkThRC )
  1184. printf("in:%i %i\n",p->id,p->val);
  1185. else
  1186. printf("in error %i\n",p->id);
  1187. cmSleepUs(100*1000);
  1188. return true;
  1189. }
  1190. // Monitor a TS queue for incoming messages from _cmTsQueueCb1()
  1191. bool _cmTsQueueCb1(void* param)
  1192. {
  1193. // the thread param is a ptr to the TS queue to monitor.
  1194. cmTsQueueH_t* qp = (cmTsQueueH_t*)param;
  1195. cmThRC_t rc;
  1196. _cmTsQCbParam_t msg;
  1197. // dequeue any waiting messages
  1198. if((rc = cmTsQueueDequeueMsg( *qp, &msg, sizeof(msg))) == kOkThRC )
  1199. printf("out:%i %i\n",msg.id,msg.val);
  1200. else
  1201. {
  1202. if( rc != kBufEmptyThRC )
  1203. printf("out error:%i\n", rc);
  1204. }
  1205. return true;
  1206. }
  1207. // Test the TS queue by starting sender threads (threads 0 & 1)
  1208. // and a receiver thread (thread 2) and sending messages
  1209. // from the sender to the receiver.
  1210. void cmTsQueueTest( cmRpt_t* rpt )
  1211. {
  1212. cmThreadH_t th0=cmThreadNullHandle,th1=cmThreadNullHandle,th2=cmThreadNullHandle;
  1213. cmTsQueueH_t q=cmTsQueueNullHandle;
  1214. _cmTsQCbParam_t param0, param1;
  1215. // create a TS Queue
  1216. if( cmTsQueueCreate(&q,100,NULL,NULL,rpt) != kOkThRC )
  1217. goto errLabel;
  1218. // create thread 0
  1219. param0.id = 0;
  1220. param0.qH = q;
  1221. if( cmThreadCreate(&th0,_cmTsQueueCb0,&param0,rpt) != kOkThRC )
  1222. goto errLabel;
  1223. // create thread 1
  1224. param1.id = 1;
  1225. param1.qH = q;
  1226. if( cmThreadCreate(&th1,_cmTsQueueCb0,&param1,rpt) != kOkThRC )
  1227. goto errLabel;
  1228. // create thread 2
  1229. if( cmThreadCreate(&th2,_cmTsQueueCb1,&q,rpt) != kOkThRC )
  1230. goto errLabel;
  1231. // start thread 0
  1232. if( cmThreadPause(th0,0) != kOkThRC )
  1233. goto errLabel;
  1234. // start thread 1
  1235. if( cmThreadPause(th1,0) != kOkThRC )
  1236. goto errLabel;
  1237. // start thread 2
  1238. if( cmThreadPause(th2,0) != kOkThRC )
  1239. goto errLabel;
  1240. printf("any key to quit.");
  1241. getchar();
  1242. errLabel:
  1243. if( cmThreadIsValid(th0) )
  1244. if( cmThreadDestroy(&th0) != kOkThRC )
  1245. printf("Error destroying thread 0\n");
  1246. if( cmThreadIsValid(th1) )
  1247. if( cmThreadDestroy(&th1) != kOkThRC )
  1248. printf("Error destroying thread 1\n");
  1249. if( cmThreadIsValid(th2) )
  1250. if( cmThreadDestroy(&th2) != kOkThRC )
  1251. printf("Error destroying thread 1\n");
  1252. if( cmTsQueueIsValid(q) )
  1253. if( cmTsQueueDestroy(&q) != kOkThRC )
  1254. printf("Error destroying queue\n");
  1255. }
  1256. //============================================================================================================================/
  1257. //
  1258. // cmTs1p1cTest()
  1259. //
  1260. // param recd for use by _cmTsQueueCb0() and
  1261. // the msg record passed between the sender
  1262. // threads and the receiver thread
  1263. typedef struct
  1264. {
  1265. unsigned id;
  1266. cmTs1p1cH_t qH;
  1267. int val;
  1268. } _cmTs1p1cCbParam_t;
  1269. cmTs1p1cH_t cmTs1p1cNullHandle = cmSTATIC_NULL_HANDLE;
  1270. // Generate a random number and put it in a TS queue
  1271. bool _cmTs1p1cCb0(void* param)
  1272. {
  1273. _cmTs1p1cCbParam_t* p = (_cmTs1p1cCbParam_t*)param;
  1274. p->val = rand(); // generate a random number
  1275. // send the msg
  1276. if( cmTs1p1cEnqueueMsg( p->qH, p, sizeof(_cmTs1p1cCbParam_t)) == kOkThRC )
  1277. printf("in:%i %i\n",p->id,p->val);
  1278. else
  1279. printf("in error %i\n",p->id);
  1280. ++p->id;
  1281. cmSleepUs(100*1000);
  1282. return true;
  1283. }
  1284. // Monitor a TS queue for incoming messages from _cmTs1p1cCb1()
  1285. bool _cmTs1p1cCb1(void* param)
  1286. {
  1287. // the thread param is a ptr to the TS queue to monitor.
  1288. cmTs1p1cH_t* qp = (cmTs1p1cH_t*)param;
  1289. cmThRC_t rc;
  1290. _cmTs1p1cCbParam_t msg;
  1291. // dequeue any waiting messages
  1292. if((rc = cmTs1p1cDequeueMsg( *qp, &msg, sizeof(msg))) == kOkThRC )
  1293. printf("out:%i %i\n",msg.id,msg.val);
  1294. else
  1295. {
  1296. if( rc != kBufEmptyThRC )
  1297. printf("out error:%i\n", rc);
  1298. }
  1299. return true;
  1300. }
  1301. // Test the TS queue by starting sender threads (threads 0 & 1)
  1302. // and a receiver thread (thread 2) and sending messages
  1303. // from the sender to the receiver.
  1304. void cmTs1p1cTest( cmRpt_t* rpt )
  1305. {
  1306. cmThreadH_t th0=cmThreadNullHandle,th1=cmThreadNullHandle,th2=cmThreadNullHandle;
  1307. cmTs1p1cH_t q=cmTs1p1cNullHandle;
  1308. _cmTs1p1cCbParam_t param1;
  1309. // create a TS Queue
  1310. if( cmTs1p1cCreate(&q,28*2,NULL,NULL,rpt) != kOkThRC )
  1311. goto errLabel;
  1312. // create thread 1
  1313. param1.id = 0;
  1314. param1.qH = q;
  1315. if( cmThreadCreate(&th1,_cmTs1p1cCb0,&param1,rpt) != kOkThRC )
  1316. goto errLabel;
  1317. // create thread 2
  1318. if( cmThreadCreate(&th2,_cmTs1p1cCb1,&q,rpt) != kOkThRC )
  1319. goto errLabel;
  1320. // start thread 1
  1321. if( cmThreadPause(th1,0) != kOkThRC )
  1322. goto errLabel;
  1323. // start thread 2
  1324. if( cmThreadPause(th2,0) != kOkThRC )
  1325. goto errLabel;
  1326. printf("any key to quit.");
  1327. getchar();
  1328. errLabel:
  1329. if( cmThreadIsValid(th0) )
  1330. if( cmThreadDestroy(&th0) != kOkThRC )
  1331. printf("Error destroying thread 0\n");
  1332. if( cmThreadIsValid(th1) )
  1333. if( cmThreadDestroy(&th1) != kOkThRC )
  1334. printf("Error destroying thread 1\n");
  1335. if( cmThreadIsValid(th2) )
  1336. if( cmThreadDestroy(&th2) != kOkThRC )
  1337. printf("Error destroying thread 1\n");
  1338. if( cmTs1p1cIsValid(q) )
  1339. if( cmTs1p1cDestroy(&q) != kOkThRC )
  1340. printf("Error destroying queue\n");
  1341. }
  1342. //============================================================================================================================/
  1343. //
  1344. // cmTsMp1cTest()
  1345. //
  1346. // param recd for use by _cmTsQueueCb0() and
  1347. // the msg record passed between the sender
  1348. // threads and the receiver thread
  1349. typedef struct
  1350. {
  1351. unsigned id;
  1352. cmTsMp1cH_t qH;
  1353. int val;
  1354. } _cmTsMp1cCbParam_t;
  1355. unsigned _cmTsMp1cVal = 0;
  1356. // Incr the global value _cmTsMp1cVal and put it in a TS queue
  1357. bool _cmTsMp1cCb0(void* param)
  1358. {
  1359. _cmTsMp1cCbParam_t* p = (_cmTsMp1cCbParam_t*)param;
  1360. //p->val = __sync_fetch_and_add(&_cmTsMp1cVal,1);
  1361. cmThUIntIncr(&_cmTsMp1cVal,1);
  1362. p->val = _cmTsMp1cVal;
  1363. // send the msg
  1364. if( cmTsMp1cEnqueueMsg( p->qH, p, sizeof(_cmTsMp1cCbParam_t)) == kOkThRC )
  1365. printf("in:%i %i\n",p->id,p->val);
  1366. else
  1367. printf("in error %i\n",p->id);
  1368. cmSleepUs(100*1000);
  1369. return true;
  1370. }
  1371. // Monitor a TS queue for incoming messages from _cmTsMp1cCb1()
  1372. bool _cmTsMp1cCb1(void* param)
  1373. {
  1374. // the thread param is a ptr to the TS queue to monitor.
  1375. cmTsMp1cH_t* qp = (cmTsMp1cH_t*)param;
  1376. cmThRC_t rc;
  1377. _cmTsMp1cCbParam_t msg;
  1378. // dequeue any waiting messages
  1379. if((rc = cmTsMp1cDequeueMsg( *qp, &msg, sizeof(msg))) == kOkThRC )
  1380. printf("out - cons id:%i val:%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 cmTsMp1cTest( cmRpt_t* rpt )
  1392. {
  1393. cmThreadH_t th0=cmThreadNullHandle,th1=cmThreadNullHandle,th2=cmThreadNullHandle;
  1394. cmTsMp1cH_t q=cmTsMp1cNullHandle;
  1395. _cmTsMp1cCbParam_t param0, param1;
  1396. // create a TS Queue
  1397. if( cmTsMp1cCreate(&q,1000,NULL,NULL,rpt) != kOkThRC )
  1398. goto errLabel;
  1399. // create thread 0 - producer 0
  1400. param0.id = 0;
  1401. param0.qH = q;
  1402. if( cmThreadCreate(&th0,_cmTsMp1cCb0,&param0,rpt) != kOkThRC )
  1403. goto errLabel;
  1404. // create thread 1 - producer 1
  1405. param1.id = 1;
  1406. param1.qH = q;
  1407. if( cmThreadCreate(&th1,_cmTsMp1cCb0,&param1,rpt) != kOkThRC )
  1408. goto errLabel;
  1409. // create thread 2 - consumer 0
  1410. if( cmThreadCreate(&th2,_cmTsMp1cCb1,&q,rpt) != kOkThRC )
  1411. goto errLabel;
  1412. // start thread 0
  1413. if( cmThreadPause(th0,0) != kOkThRC )
  1414. goto errLabel;
  1415. // start thread 1
  1416. if( cmThreadPause(th1,0) != kOkThRC )
  1417. goto errLabel;
  1418. // start thread 2
  1419. if( cmThreadPause(th2,0) != kOkThRC )
  1420. goto errLabel;
  1421. printf("any key to quit.");
  1422. getchar();
  1423. errLabel:
  1424. if( cmThreadIsValid(th0) )
  1425. if( cmThreadDestroy(&th0) != kOkThRC )
  1426. printf("Error destroying thread 0\n");
  1427. if( cmThreadIsValid(th1) )
  1428. if( cmThreadDestroy(&th1) != kOkThRC )
  1429. printf("Error destroying thread 1\n");
  1430. if( cmThreadIsValid(th2) )
  1431. if( cmThreadDestroy(&th2) != kOkThRC )
  1432. printf("Error destroying thread 1\n");
  1433. if( cmTsMp1cIsValid(q) )
  1434. if( cmTsMp1cDestroy(&q) != kOkThRC )
  1435. printf("Error destroying queue\n");
  1436. }
  1437. void cmSleepUs( unsigned microseconds )
  1438. { usleep(microseconds); }
  1439. void cmSleepMs( unsigned milliseconds )
  1440. { cmSleepUs(milliseconds*1000); }