00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <stdio.h>
00024 #include <stdlib.h>
00025 #include <platform.h>
00026 #include <earthworm.h>
00027 #include <earthworm_complex_funcs.h>
00028 #include <priority_queue.h>
00029
00030
00031
00032 #ifdef LOG_DEBUG
00033
00034 #include <stdio.h>
00035 #endif
00036
00037
00038
00039
00040 #define EW_PRI_SHIFT_DOWN -1
00041 #define EW_PRI_SHIFT_NONE 0
00042 #define EW_PRI_SHIFT_UP 1
00043
00044
00045
00046
00047
00048 int init_pri_queue( PRI_QUEUE * p_queue
00049 , unsigned long p_max_items
00050 , unsigned long p_max_item_size )
00051 {
00052 int r_status = EW_PRI_RETNORMAL;
00053
00054 if ( p_queue == NULL )
00055 {
00056 r_status = EW_PRI_RETQNULL;
00057 }
00058 else
00059 {
00060
00061
00062
00063 p_queue->queuesize = 0;
00064 p_queue->itemsused = 0;
00065 p_queue->sorted = NULL;
00066 p_queue->entries = NULL;
00067 p_queue->data = NULL;
00068 }
00069
00070 if ( p_max_items < 1 )
00071 {
00072 r_status = EW_PRI_RETPARAM;
00073 }
00074
00075 if ( r_status == 0 )
00076 {
00077 unsigned long _idx;
00078
00079 p_queue->queuesize = p_max_items;
00080 p_queue->itemmaxsize = p_max_item_size;
00081
00082
00083
00084
00085 if ( r_status == EW_PRI_RETNORMAL )
00086 {
00087 if ( ( p_queue->sorted = (PRI_QUEUE_ENTRY **) malloc( sizeof(PRI_QUEUE_ENTRY*) * p_max_items) ) == NULL )
00088 {
00089 r_status = EW_PRI_RETMALLOC;
00090 }
00091 }
00092
00093
00094
00095
00096 if ( r_status == EW_PRI_RETNORMAL )
00097 {
00098 if ( ( p_queue->entries = (PRI_QUEUE_ENTRY *) malloc( sizeof(PRI_QUEUE_ENTRY) * p_max_items) ) == NULL )
00099 {
00100 r_status = EW_PRI_RETMALLOC;
00101 }
00102 }
00103
00104
00105
00106
00107
00108 if ( r_status == EW_PRI_RETNORMAL )
00109 {
00110 if ( ( p_queue->data = (char *) malloc(sizeof(char) * p_max_item_size * p_max_items) ) == NULL )
00111 {
00112 r_status = EW_PRI_RETMALLOC;
00113 }
00114 }
00115
00116
00117
00118
00119
00120
00121 if ( r_status == EW_PRI_RETNORMAL )
00122 {
00123 PRI_QUEUE_ENTRY * _entry = p_queue->entries;
00124 PRI_QUEUE_ENTRY ** _sorted = p_queue->sorted;
00125 for ( _idx = 0 ; _idx < p_max_items ; _idx++, _entry++, _sorted++ )
00126 {
00127
00128 _entry->pri = EW_PRIORITY_NONE;
00129 _entry->length = 0;
00130
00131 _entry->data = (char *)(p_queue->data + (_idx * p_max_item_size));
00132
00133 *_sorted = _entry;
00134 }
00135 }
00136
00137
00138
00139
00140
00141
00142 if ( r_status == EW_PRI_RETNORMAL )
00143 {
00144 for ( _idx = 0 ; _idx < EW_PRIORITY_COUNT ; _idx++ ) {
00145 p_queue->insert_indices[_idx] = 0;
00146 }
00147 }
00148 CreateSpecificMutex( &(p_queue->lock) );
00149 }
00150
00151
00152 if ( r_status != EW_PRI_RETNORMAL )
00153 {
00154 release_pri_queue( p_queue );
00155 }
00156 #ifdef LOG_DEBUG
00157 else
00158 {
00159 char dbgstr[120];
00160 sprintf( dbgstr
00161 , "DEBUG init: %d %d %d\n"
00162 , p_queue->sorted, p_queue->entries, p_queue->data
00163 );
00164 logit( "t"
00165 , dbgstr
00166 , "export_pri"
00167 , "MOD_EXPORT_SCN"
00168 );
00169 }
00170 #endif
00171 return r_status;
00172 }
00173
00174
00175
00176
00177
00178 void release_pri_queue( PRI_QUEUE * p_queue )
00179 {
00180 p_queue->queuesize = 0;
00181 p_queue->itemsused = 0;
00182
00183 CloseSpecificMutex( &p_queue->lock );
00184
00185 if ( p_queue->sorted != NULL )
00186 {
00187 free( p_queue->sorted );
00188 p_queue->sorted = NULL;
00189 }
00190
00191 if ( p_queue->entries != NULL )
00192 {
00193 free( p_queue->entries );
00194 p_queue->entries = NULL;
00195 }
00196
00197 if ( p_queue->data != NULL )
00198 {
00199 free( p_queue->data );
00200 p_queue->data = NULL;
00201 }
00202 }
00203
00204
00205
00206
00207 int getNumOfElementsInQueue( PRI_QUEUE * p_queue )
00208 {
00209 if ( p_queue == NULL )
00210 {
00211 return 0;
00212 }
00213 return p_queue->itemsused;
00214 }
00215
00216
00217
00218
00219 int add_item( PRI_QUEUE * p_queue
00220 , EW_PRIORITY p_priority
00221 , MSG_LOGO p_logo
00222 , long p_size
00223 , PRI_DATA p_data
00224 )
00225 {
00226 #ifdef LOG_DEBUG
00227 char dbgstr[120];
00228 #endif
00229
00230 int r_status = EW_PRI_RETNORMAL;
00231
00232 if ( p_queue == NULL )
00233 {
00234 return( EW_PRI_RETQNULL );
00235 }
00236
00237 if ( p_queue->queuesize < 1 )
00238 {
00239 return( EW_PRI_RETNOTREADY );
00240 }
00241
00242 RequestSpecificMutex( &p_queue->lock );
00243
00244 if ( p_size < 0 || p_queue->itemmaxsize < p_size )
00245 {
00246 return ( EW_PRI_RETMSGSIZE );
00247 }
00248 else
00249 {
00250 EW_PRIORITY _usePri = p_priority;
00251
00252 long _queuesize = p_queue->queuesize
00253 , _ins_index = p_queue->queuesize - 1
00254 , _src_index
00255 ;
00256 PRI_QUEUE_ENTRY * _wrk_pointer;
00257 int _shift_direction = EW_PRI_SHIFT_NONE
00258 , _shift_stt
00259 , _shift_end
00260 , _idx
00261 ;
00262 int _doSwap = 0
00263 , _updateIdx = 0
00264 ;
00265
00266 if ( p_priority < EW_PRIORITY_MIN || EW_PRIORITY_MAX < p_priority )
00267 {
00268
00269 _usePri = EW_PRIORITY_DEF;
00270
00271 r_status = EW_PRI_RETBADPRI;
00272 }
00273
00274
00275 if ( p_queue->insert_indices[p_priority] < _queuesize )
00276 {
00277
00278
00279
00280 _ins_index = p_queue->insert_indices[p_priority];
00281
00282
00283
00284 _wrk_pointer = p_queue->sorted[_ins_index];
00285
00286 if ( _wrk_pointer->pri == 0
00287 || _ins_index == (_queuesize - 1)
00288 )
00289 {
00290
00291
00292
00293
00294
00295 _doSwap = 1;
00296 _updateIdx = 1;
00297 if ( p_queue->itemsused < _queuesize )
00298 {
00299 p_queue->itemsused++;
00300 }
00301 }
00302 else
00303 {
00304
00305
00306
00307
00308
00309
00310 if ( p_queue->itemsused < _queuesize )
00311 {
00312
00313
00314
00315
00316
00317
00318 _src_index = p_queue->itemsused++;
00319
00320
00321 _wrk_pointer = p_queue->sorted[_src_index];
00322
00323
00324 _shift_end = _ins_index;
00325 _shift_stt = _src_index;
00326 _shift_direction = EW_PRI_SHIFT_DOWN;
00327
00328 _doSwap = 1;
00329 _updateIdx = 1;
00330 }
00331 else
00332 {
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347 _src_index = p_queue->insert_indices[ p_queue->sorted[_queuesize - 1]->pri - 1 ];
00348
00349
00350 _wrk_pointer = p_queue->sorted[_src_index];
00351
00352
00353 _shift_end = _ins_index;
00354 _shift_stt = _src_index;
00355 _shift_direction = EW_PRI_SHIFT_DOWN;
00356
00357 _doSwap = 1;
00358 _updateIdx = 1;
00359
00360 r_status = EW_PRI_RETDROP;
00361 }
00362 }
00363 }
00364 else
00365 {
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382 if ( p_queue->sorted[_queuesize - 1]->pri != EW_PRIORITY_MIN
00383 && p_queue->insert_indices[p_priority - 1] < _queuesize
00384 )
00385 {
00386
00387
00388
00389
00390
00391
00392 _wrk_pointer = p_queue->sorted[ p_queue->insert_indices[p_priority - 1] ];
00393
00394
00395 _shift_stt = p_queue->insert_indices[p_priority - 1];
00396 _shift_end = _queuesize - 1;
00397 _shift_direction = EW_PRI_SHIFT_UP;
00398
00399 _doSwap = 1;
00400 _updateIdx = 1;
00401
00402 r_status = EW_PRI_RETDROP;
00403
00404 }
00405 else
00406 {
00407 r_status = EW_PRI_RETREJECT;
00408 }
00409 }
00410
00411
00412 if ( _doSwap == 1 )
00413 {
00414
00415 _wrk_pointer->pri = p_priority;
00416 (_wrk_pointer->logo).type = p_logo.type;
00417 (_wrk_pointer->logo).mod = p_logo.mod;
00418 (_wrk_pointer->logo).instid = p_logo.instid;
00419 _wrk_pointer->length = p_size;
00420
00421
00422
00423 if ( _wrk_pointer->data != NULL )
00424 {
00425 memcpy( _wrk_pointer->data, p_data, p_size );
00426 }
00427 }
00428
00429
00430 if ( _shift_direction != EW_PRI_SHIFT_NONE && p_queue->sorted != NULL )
00431 {
00432 PRI_QUEUE_ENTRY * _temp = p_queue->sorted[_shift_stt];
00433
00434 #ifdef DEBUG_DETAILS
00435 sprintf( dbgstr
00436 , "DEBUG add_item() shifting %d -> %d (%d)\n"
00437 , _shift_stt, _shift_end, _shift_direction
00438 );
00439 logit( "t"
00440 , dbgstr
00441 , "export_pri"
00442 , "MOD_EXPORT_SCN"
00443 );
00444 #endif
00445 for ( _idx = _shift_stt
00446 ; ( _shift_direction == EW_PRI_SHIFT_UP ? _idx < _shift_end : _idx > _shift_end )
00447 ; _idx += _shift_direction
00448 )
00449 {
00450 #ifdef DEBUG_DETAILS
00451 sprintf( dbgstr
00452 , "DEBUG shifting [%d] = [%d]\n"
00453 , _idx, _idx + _shift_direction
00454 );
00455 logit( "t"
00456 , dbgstr
00457 , "export_pri"
00458 , "MOD_EXPORT_SCN"
00459 );
00460 #endif
00461 p_queue->sorted[ _idx ] = p_queue->sorted[ _idx + _shift_direction ];
00462 }
00463 p_queue->sorted[_shift_end] = _temp;
00464 }
00465
00466
00467 if ( _updateIdx == 1 )
00468 {
00469
00470 for ( _idx = p_priority ; _idx < EW_PRIORITY_COUNT ; _idx++ )
00471 {
00472 if ( ( p_queue->insert_indices[_idx] + 1 ) <= _queuesize )
00473 {
00474 p_queue->insert_indices[_idx] = p_queue->insert_indices[_idx] + 1;
00475 }
00476 }
00477 }
00478 }
00479
00480 ReleaseSpecificMutex( &p_queue->lock );
00481
00482 return r_status;
00483 }
00484
00485
00486
00487
00488
00489 int peek_next_item( PRI_QUEUE * p_queue
00490 , MSG_LOGO * p_logoptr
00491 , EW_PRIORITY * p_priptr
00492 )
00493 {
00494 int r_status = EW_PRI_NOITEM;
00495
00496 if ( p_queue == NULL )
00497 {
00498 return( EW_PRI_RETQNULL );
00499 }
00500
00501 if ( p_queue->queuesize < 1 )
00502 {
00503 return( EW_PRI_RETNOTREADY );
00504 }
00505
00506 RequestSpecificMutex( &p_queue->lock );
00507
00508 if ( p_queue->sorted != NULL )
00509 {
00510 PRI_QUEUE_ENTRY * _wrk_pointer;
00511
00512
00513
00514
00515
00516
00517 _wrk_pointer = p_queue->sorted[0];
00518
00519 if ( _wrk_pointer->pri != EW_PRIORITY_NONE )
00520 {
00521
00522 p_logoptr->type = (_wrk_pointer->logo).type;
00523 p_logoptr->mod = (_wrk_pointer->logo).mod ;
00524 p_logoptr->instid = (_wrk_pointer->logo).instid;
00525
00526 *p_priptr = _wrk_pointer->pri;
00527
00528 r_status = EW_PRI_RETNORMAL;
00529 }
00530 }
00531
00532 ReleaseSpecificMutex( &p_queue->lock );
00533
00534 return r_status;
00535 }
00536
00537
00538
00539
00540
00541 int pop_next_item( PRI_QUEUE * p_queue
00542 , MSG_LOGO * p_logoptr
00543 , long * p_sizeptr
00544 , PRI_DATA p_data
00545 )
00546 {
00547 #ifdef LOG_DEBUG
00548 char dbgstr[120];
00549 #endif
00550
00551 int r_status = EW_PRI_NOITEM;
00552
00553 PRI_QUEUE_ENTRY * _wrk_pointer;
00554 unsigned long _idx
00555 , _sz
00556 ;
00557
00558 if ( p_queue == NULL )
00559 {
00560 return( EW_PRI_RETQNULL );
00561 }
00562
00563 if ( p_queue->queuesize < 1 )
00564 {
00565 return( EW_PRI_RETNOTREADY );
00566 }
00567
00568 RequestSpecificMutex( &(p_queue->lock) );
00569
00570
00571
00572
00573
00574
00575
00576
00577 _wrk_pointer = p_queue->sorted[0];
00578
00579 if ( _wrk_pointer->pri != EW_PRIORITY_NONE )
00580 {
00581
00582 p_logoptr->type = (_wrk_pointer->logo).type;
00583 p_logoptr->mod = (_wrk_pointer->logo).mod ;
00584 p_logoptr->instid = (_wrk_pointer->logo).instid;
00585
00586 *p_sizeptr = _wrk_pointer->length;
00587
00588
00589
00590
00591 memcpy( p_data, _wrk_pointer->data, (size_t)(_wrk_pointer->length) );
00592
00593
00594 for ( _idx = 0, _sz = p_queue->queuesize - 1 ; _idx < _sz ; _idx++ )
00595 {
00596 p_queue->sorted[_idx] = p_queue->sorted[_idx + 1];
00597 }
00598
00599
00600 _wrk_pointer->pri = EW_PRIORITY_NONE;
00601 _wrk_pointer->length = 0;
00602
00603
00604
00605
00606
00607 p_queue->sorted[p_queue->queuesize - 1] = _wrk_pointer;
00608
00609
00610 for ( _idx = 0 ; _idx < EW_PRIORITY_COUNT ; _idx++ )
00611 {
00612 if ( 0 <= ( p_queue->insert_indices[_idx] - 1 ) )
00613 {
00614 p_queue->insert_indices[_idx] = p_queue->insert_indices[_idx] - 1;
00615 }
00616 }
00617
00618
00619 (p_queue->itemsused)--;
00620
00621 r_status = EW_PRI_RETNORMAL;
00622 }
00623
00624 #ifdef DEBUG_DETAILS
00625 sprintf( dbgstr
00626 , "%%s(%%s): DEBUG pop_next_item() releasing mutex; q state: %d of %d\n"
00627 , p_queue->itemsused
00628 , p_queue->queuesize
00629 );
00630 logit( "t"
00631 , dbgstr
00632 , "export_pri"
00633 , "MOD_EXPORT_SCN"
00634 );
00635 #endif
00636
00637 ReleaseSpecificMutex( &p_queue->lock );
00638
00639 return r_status;
00640 }
00641
00642
00643
00644
00645
00646