161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan/*-------------------------------------------------------------------------- 261a2c6a19dbd06a317a419623020e02056661876Praveen ChavanCopyright (c) 2010-2013, The Linux Foundation. All rights reserved. 361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 461a2c6a19dbd06a317a419623020e02056661876Praveen ChavanRedistribution and use in source and binary forms, with or without 561a2c6a19dbd06a317a419623020e02056661876Praveen Chavanmodification, are permitted provided that the following conditions are met: 661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan * Redistributions of source code must retain the above copyright 761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan notice, this list of conditions and the following disclaimer. 861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan * Redistributions in binary form must reproduce the above copyright 961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan notice, this list of conditions and the following disclaimer in the 1061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan documentation and/or other materials provided with the distribution. 1161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan * Neither the name of The Linux Foundation nor 1261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan the names of its contributors may be used to endorse or promote 1361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan products derived from this software without specific prior written 1461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan permission. 1561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 1661a2c6a19dbd06a317a419623020e02056661876Praveen ChavanTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 1761a2c6a19dbd06a317a419623020e02056661876Praveen ChavanAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 1861a2c6a19dbd06a317a419623020e02056661876Praveen ChavanIMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 1961a2c6a19dbd06a317a419623020e02056661876Praveen ChavanNON-INFRINGEMENT ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR 2061a2c6a19dbd06a317a419623020e02056661876Praveen ChavanCONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 2161a2c6a19dbd06a317a419623020e02056661876Praveen ChavanEXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 2261a2c6a19dbd06a317a419623020e02056661876Praveen ChavanPROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 2361a2c6a19dbd06a317a419623020e02056661876Praveen ChavanOR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 2461a2c6a19dbd06a317a419623020e02056661876Praveen ChavanWHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 2561a2c6a19dbd06a317a419623020e02056661876Praveen ChavanOTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 2661a2c6a19dbd06a317a419623020e02056661876Praveen ChavanADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 2761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan--------------------------------------------------------------------------*/ 2861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan#include "message_queue.h" 2961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 3061a2c6a19dbd06a317a419623020e02056661876Praveen Chavanint check_if_queue_empty ( unsigned int queuetocheck, void* queuecontext ) 3161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan{ 3261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan struct video_queue_context *ptr_q = NULL; 3361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 3461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* 3561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan * queuetocheck - 0 command queue 3661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan * queuetocheck - 1 data queue 3761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan */ 3861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan if ( queuecontext == NULL || (queuetocheck > 1 ) ) { 3961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return 1; 4061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } 4161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 4261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ptr_q = (struct video_queue_context *)queuecontext; 4361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 4461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan if (queuetocheck == 0) { 4561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan if (ptr_q->read_comq == ptr_q->write_comq) { 4661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return 1; 4761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } 4861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } else if (queuetocheck == 1) { 4961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan if (ptr_q->write_dataq == ptr_q->read_dataq) { 5061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return 1; 5161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } 5261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } 5361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 5461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return 0; 5561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan} 5661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 5761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 5861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 5961a2c6a19dbd06a317a419623020e02056661876Praveen Chavanstruct video_msgq * queue_get_cmd (void* queuecontext ) { 6061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan struct video_queue_context *ptr_q = NULL; 6161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan struct video_msgq *pitem = NULL; 6261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 6361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan if ( NULL == queuecontext ) { 648767726b6d6b2194cbb3704d7d82f0c7ef34eeb3Praveen Chavan printf("queue_get_cmd: Invalid Input parameter"); 6561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return NULL; 6661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } 6761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 6861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ptr_q = (struct video_queue_context *)queuecontext; 6961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 7061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Wait on the semaphore till it is released */ 7161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan sem_wait(&ptr_q->sem_message); 7261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 7361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Lock the mutex to protect the critical section */ 7461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan pthread_mutex_lock(&ptr_q->mutex); 7561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 7661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan if (ptr_q->read_comq != ptr_q->write_comq) { 7761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan pitem = &ptr_q->ptr_cmdq [ptr_q->read_comq]; 7861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ptr_q->read_comq = (ptr_q->read_comq + 1) % \ 7961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ptr_q->commandq_size; 8061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } else if (ptr_q->write_dataq != ptr_q->read_dataq) { 8161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan pitem = &ptr_q->ptr_dataq [ptr_q->read_dataq]; 8261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ptr_q->read_dataq = (ptr_q->read_dataq + 1) % \ 8361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ptr_q->dataq_size; 8461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } 8561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 8661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Unlock the mutex to release the critical section */ 8761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan pthread_mutex_unlock(&ptr_q->mutex); 8861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 8961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return pitem; 9061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan} 9161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 9261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 9361a2c6a19dbd06a317a419623020e02056661876Praveen Chavanint queue_post_cmdq ( void* queuecontext, 9461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan struct video_msgq *pitem 9561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ) 9661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan{ 9761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan struct video_queue_context *ptr_q = NULL; 9861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 9961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan if (pitem == NULL || queuecontext == NULL) { 10061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return -1; 10161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } 10261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 10361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ptr_q = (struct video_queue_context *)queuecontext; 10461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 10561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Lock the mutex to protect the critical section */ 10661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan pthread_mutex_lock(&ptr_q->mutex); 10761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 10861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan if ((ptr_q->write_comq + 1) % ptr_q->commandq_size == ptr_q->read_comq) { 1098767726b6d6b2194cbb3704d7d82f0c7ef34eeb3Praveen Chavan printf("QUEUE is FULL"); 11061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Unlock the mutex to release the critical section */ 11161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan pthread_mutex_unlock(&ptr_q->mutex); 11261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return 0; 11361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } else { 11461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Store the command in the Message Queue & increment write offset */ 11561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan memcpy ( &ptr_q->ptr_cmdq [ptr_q->write_comq],pitem, \ 11661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan sizeof (struct video_msgq)); 11761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ptr_q->write_comq = (ptr_q->write_comq + 1) % ptr_q->commandq_size; 11861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } 11961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 12061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Unlock the mutex to release the critical section */ 12161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan pthread_mutex_unlock(&ptr_q->mutex); 12261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 12361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Post the semaphore */ 12461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan sem_post(&ptr_q->sem_message); 12561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return 1; 12661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan} 12761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 12861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 12961a2c6a19dbd06a317a419623020e02056661876Praveen Chavanint queue_post_dataq ( void *queuecontext, 13061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan struct video_msgq *pitem 13161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ) 13261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan{ 13361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan struct video_queue_context *ptr_q = NULL; 13461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 13561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan if (pitem == NULL || queuecontext == NULL) { 13661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return -1; 13761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } 13861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 13961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ptr_q = (struct video_queue_context *)queuecontext; 14061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 14161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Lock the mutex to protect the critical section */ 14261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan pthread_mutex_lock(&ptr_q->mutex); 14361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 14461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan if ((ptr_q->write_dataq + 1) % ptr_q->dataq_size == ptr_q->read_dataq) { 1458767726b6d6b2194cbb3704d7d82f0c7ef34eeb3Praveen Chavan printf("QUEUE is FULL"); 14661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Unlock the mutex to release the critical section */ 14761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan pthread_mutex_unlock(&ptr_q->mutex); 14861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return 0; 14961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } else { 15061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Store the command in the Message Queue & increment write offset */ 15161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan memcpy ( &ptr_q->ptr_dataq [ptr_q->write_dataq],pitem, \ 15261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan sizeof (struct video_msgq)); 15361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan ptr_q->write_dataq = (ptr_q->write_dataq + 1) % ptr_q->dataq_size; 15461a2c6a19dbd06a317a419623020e02056661876Praveen Chavan } 15561a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 15661a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Unlock the mutex to release the critical section */ 15761a2c6a19dbd06a317a419623020e02056661876Praveen Chavan pthread_mutex_unlock(&ptr_q->mutex); 15861a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 15961a2c6a19dbd06a317a419623020e02056661876Praveen Chavan /* Post the semaphore */ 16061a2c6a19dbd06a317a419623020e02056661876Praveen Chavan sem_post(&ptr_q->sem_message); 16161a2c6a19dbd06a317a419623020e02056661876Praveen Chavan return 1; 16261a2c6a19dbd06a317a419623020e02056661876Praveen Chavan 16361a2c6a19dbd06a317a419623020e02056661876Praveen Chavan} 164