1 /* Copyright (c) 2011-2012, 2014, 2017 The Linux Foundation. All rights reserved.
2  *
3  * Redistribution and use in source and binary forms, with or without
4  * modification, are permitted provided that the following conditions are
5  * met:
6  *     * Redistributions of source code must retain the above copyright
7  *       notice, this list of conditions and the following disclaimer.
8  *     * Redistributions in binary form must reproduce the above
9  *       copyright notice, this list of conditions and the following
10  *       disclaimer in the documentation and/or other materials provided
11  *       with the distribution.
12  *     * Neither the name of The Linux Foundation, nor the names of its
13  *       contributors may be used to endorse or promote products derived
14  *       from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
17  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
18  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
19  * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
20  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23  * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
24  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
25  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
26  * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27  */
28 
29 // Uncomment to log verbose logs
30 #define LOG_NDEBUG 1
31 #define LOG_TAG "LocSvc_utils_q"
32 #include <stdio.h>
33 #include <stdlib.h>
34 #include <pthread.h>
35 #include <loc_pla.h>
36 #include <log_util.h>
37 #include "linked_list.h"
38 #include "msg_q.h"
39 
40 typedef struct msg_q {
41    void* msg_list;                  /* Linked list to store information */
42    pthread_cond_t  list_cond;       /* Condition variable for waiting on msg queue */
43    pthread_mutex_t list_mutex;      /* Mutex for exclusive access to message queue */
44    int unblocked;                   /* Has this message queue been unblocked? */
45 } msg_q;
46 
47 /*===========================================================================
48 FUNCTION    convert_linked_list_err_type
49 
50 DESCRIPTION
51    Converts from one set of enum values to another.
52 
53    linked_list_val: Value to convert to msg_q_enum_type
54 
55 DEPENDENCIES
56    N/A
57 
58 RETURN VALUE
59    Corresponding linked_list_enum_type in msg_q_enum_type
60 
61 SIDE EFFECTS
62    N/A
63 
64 ===========================================================================*/
convert_linked_list_err_type(linked_list_err_type linked_list_val)65 static msq_q_err_type convert_linked_list_err_type(linked_list_err_type linked_list_val)
66 {
67    switch( linked_list_val )
68    {
69    case eLINKED_LIST_SUCCESS:
70       return eMSG_Q_SUCCESS;
71    case eLINKED_LIST_INVALID_PARAMETER:
72       return eMSG_Q_INVALID_PARAMETER;
73    case eLINKED_LIST_INVALID_HANDLE:
74       return eMSG_Q_INVALID_HANDLE;
75    case eLINKED_LIST_UNAVAILABLE_RESOURCE:
76       return eMSG_Q_UNAVAILABLE_RESOURCE;
77    case eLINKED_LIST_INSUFFICIENT_BUFFER:
78       return eMSG_Q_INSUFFICIENT_BUFFER;
79 
80    case eLINKED_LIST_FAILURE_GENERAL:
81    default:
82       return eMSG_Q_FAILURE_GENERAL;
83    }
84 }
85 
86 /* ----------------------- END INTERNAL FUNCTIONS ---------------------------------------- */
87 
88 /*===========================================================================
89 
90   FUNCTION:   msg_q_init
91 
92   ===========================================================================*/
msg_q_init(void ** msg_q_data)93 msq_q_err_type msg_q_init(void** msg_q_data)
94 {
95    if( msg_q_data == NULL )
96    {
97       LOC_LOGE("%s: Invalid msg_q_data parameter!\n", __FUNCTION__);
98       return eMSG_Q_INVALID_PARAMETER;
99    }
100 
101    msg_q* tmp_msg_q;
102    tmp_msg_q = (msg_q*)calloc(1, sizeof(msg_q));
103    if( tmp_msg_q == NULL )
104    {
105       LOC_LOGE("%s: Unable to allocate space for message queue!\n", __FUNCTION__);
106       return eMSG_Q_FAILURE_GENERAL;
107    }
108 
109    if( linked_list_init(&tmp_msg_q->msg_list) != 0 )
110    {
111       LOC_LOGE("%s: Unable to initialize storage list!\n", __FUNCTION__);
112       free(tmp_msg_q);
113       return eMSG_Q_FAILURE_GENERAL;
114    }
115 
116    if( pthread_mutex_init(&tmp_msg_q->list_mutex, NULL) != 0 )
117    {
118       LOC_LOGE("%s: Unable to initialize list mutex!\n", __FUNCTION__);
119       linked_list_destroy(&tmp_msg_q->msg_list);
120       free(tmp_msg_q);
121       return eMSG_Q_FAILURE_GENERAL;
122    }
123 
124    if( pthread_cond_init(&tmp_msg_q->list_cond, NULL) != 0 )
125    {
126       LOC_LOGE("%s: Unable to initialize msg q cond var!\n", __FUNCTION__);
127       linked_list_destroy(&tmp_msg_q->msg_list);
128       pthread_mutex_destroy(&tmp_msg_q->list_mutex);
129       free(tmp_msg_q);
130       return eMSG_Q_FAILURE_GENERAL;
131    }
132 
133    tmp_msg_q->unblocked = 0;
134 
135    *msg_q_data = tmp_msg_q;
136 
137    return eMSG_Q_SUCCESS;
138 }
139 
140 /*===========================================================================
141 
142   FUNCTION:   msg_q_init2
143 
144   ===========================================================================*/
msg_q_init2()145 const void* msg_q_init2()
146 {
147   void* q = NULL;
148   if (eMSG_Q_SUCCESS != msg_q_init(&q)) {
149     q = NULL;
150   }
151   return q;
152 }
153 
154 /*===========================================================================
155 
156   FUNCTION:   msg_q_destroy
157 
158   ===========================================================================*/
msg_q_destroy(void ** msg_q_data)159 msq_q_err_type msg_q_destroy(void** msg_q_data)
160 {
161    if( msg_q_data == NULL )
162    {
163       LOC_LOGE("%s: Invalid msg_q_data parameter!\n", __FUNCTION__);
164       return eMSG_Q_INVALID_HANDLE;
165    }
166 
167    msg_q* p_msg_q = (msg_q*)*msg_q_data;
168 
169    linked_list_destroy(&p_msg_q->msg_list);
170    pthread_mutex_destroy(&p_msg_q->list_mutex);
171    pthread_cond_destroy(&p_msg_q->list_cond);
172 
173    p_msg_q->unblocked = 0;
174 
175    free(*msg_q_data);
176    *msg_q_data = NULL;
177 
178    return eMSG_Q_SUCCESS;
179 }
180 
181 /*===========================================================================
182 
183   FUNCTION:   msg_q_snd
184 
185   ===========================================================================*/
msg_q_snd(void * msg_q_data,void * msg_obj,void (* dealloc)(void *))186 msq_q_err_type msg_q_snd(void* msg_q_data, void* msg_obj, void (*dealloc)(void*))
187 {
188    msq_q_err_type rv;
189    if( msg_q_data == NULL )
190    {
191       LOC_LOGE("%s: Invalid msg_q_data parameter!\n", __FUNCTION__);
192       return eMSG_Q_INVALID_HANDLE;
193    }
194    if( msg_obj == NULL )
195    {
196       LOC_LOGE("%s: Invalid msg_obj parameter!\n", __FUNCTION__);
197       return eMSG_Q_INVALID_PARAMETER;
198    }
199 
200    msg_q* p_msg_q = (msg_q*)msg_q_data;
201 
202    pthread_mutex_lock(&p_msg_q->list_mutex);
203    LOC_LOGV("%s: Sending message with handle = %p\n", __FUNCTION__, msg_obj);
204 
205    if( p_msg_q->unblocked )
206    {
207       LOC_LOGE("%s: Message queue has been unblocked.\n", __FUNCTION__);
208       pthread_mutex_unlock(&p_msg_q->list_mutex);
209       return eMSG_Q_UNAVAILABLE_RESOURCE;
210    }
211 
212    rv = convert_linked_list_err_type(linked_list_add(p_msg_q->msg_list, msg_obj, dealloc));
213 
214    /* Show data is in the message queue. */
215    pthread_cond_signal(&p_msg_q->list_cond);
216 
217    pthread_mutex_unlock(&p_msg_q->list_mutex);
218 
219    LOC_LOGV("%s: Finished Sending message with handle = %p\n", __FUNCTION__, msg_obj);
220 
221    return rv;
222 }
223 
224 /*===========================================================================
225 
226   FUNCTION:   msg_q_rcv
227 
228   ===========================================================================*/
msg_q_rcv(void * msg_q_data,void ** msg_obj)229 msq_q_err_type msg_q_rcv(void* msg_q_data, void** msg_obj)
230 {
231    msq_q_err_type rv;
232    if( msg_q_data == NULL )
233    {
234       LOC_LOGE("%s: Invalid msg_q_data parameter!\n", __FUNCTION__);
235       return eMSG_Q_INVALID_HANDLE;
236    }
237 
238    if( msg_obj == NULL )
239    {
240       LOC_LOGE("%s: Invalid msg_obj parameter!\n", __FUNCTION__);
241       return eMSG_Q_INVALID_PARAMETER;
242    }
243 
244    msg_q* p_msg_q = (msg_q*)msg_q_data;
245 
246    pthread_mutex_lock(&p_msg_q->list_mutex);
247 
248    if( p_msg_q->unblocked )
249    {
250       LOC_LOGE("%s: Message queue has been unblocked.\n", __FUNCTION__);
251       pthread_mutex_unlock(&p_msg_q->list_mutex);
252       return eMSG_Q_UNAVAILABLE_RESOURCE;
253    }
254 
255    /* Wait for data in the message queue */
256    while( linked_list_empty(p_msg_q->msg_list) && !p_msg_q->unblocked )
257    {
258       pthread_cond_wait(&p_msg_q->list_cond, &p_msg_q->list_mutex);
259    }
260 
261    rv = convert_linked_list_err_type(linked_list_remove(p_msg_q->msg_list, msg_obj));
262 
263    pthread_mutex_unlock(&p_msg_q->list_mutex);
264 
265    LOC_LOGV("%s: Received message %p rv = %d\n", __FUNCTION__, *msg_obj, rv);
266 
267    return rv;
268 }
269 
270 /*===========================================================================
271 
272   FUNCTION:   msg_q_rmv
273 
274   ===========================================================================*/
msg_q_rmv(void * msg_q_data,void ** msg_obj)275 msq_q_err_type msg_q_rmv(void* msg_q_data, void** msg_obj)
276 {
277    msq_q_err_type rv;
278    if (msg_q_data == NULL) {
279       LOC_LOGE("%s: Invalid msg_q_data parameter!\n", __FUNCTION__);
280       return eMSG_Q_INVALID_HANDLE;
281    }
282 
283    if (msg_obj == NULL) {
284       LOC_LOGE("%s: Invalid msg_obj parameter!\n", __FUNCTION__);
285       return eMSG_Q_INVALID_PARAMETER;
286    }
287 
288    msg_q* p_msg_q = (msg_q*)msg_q_data;
289 
290    pthread_mutex_lock(&p_msg_q->list_mutex);
291 
292    if (p_msg_q->unblocked) {
293       LOC_LOGE("%s: Message queue has been unblocked.\n", __FUNCTION__);
294       pthread_mutex_unlock(&p_msg_q->list_mutex);
295       return eMSG_Q_UNAVAILABLE_RESOURCE;
296    }
297 
298    if (linked_list_empty(p_msg_q->msg_list)) {
299       LOC_LOGW("%s: list is empty !!\n", __FUNCTION__);
300       pthread_mutex_unlock(&p_msg_q->list_mutex);
301       return eLINKED_LIST_EMPTY;
302    }
303 
304    rv = convert_linked_list_err_type(linked_list_remove(p_msg_q->msg_list, msg_obj));
305 
306    pthread_mutex_unlock(&p_msg_q->list_mutex);
307 
308    LOC_LOGV("%s: Removed message %p rv = %d\n", __FUNCTION__, *msg_obj, rv);
309 
310    return rv;
311 }
312 
313 
314 
315 /*===========================================================================
316 
317   FUNCTION:   msg_q_flush
318 
319   ===========================================================================*/
msg_q_flush(void * msg_q_data)320 msq_q_err_type msg_q_flush(void* msg_q_data)
321 {
322    msq_q_err_type rv;
323    if ( msg_q_data == NULL )
324    {
325       LOC_LOGE("%s: Invalid msg_q_data parameter!\n", __FUNCTION__);
326       return eMSG_Q_INVALID_HANDLE;
327    }
328 
329    msg_q* p_msg_q = (msg_q*)msg_q_data;
330 
331    LOC_LOGD("%s: Flushing Message Queue\n", __FUNCTION__);
332 
333    pthread_mutex_lock(&p_msg_q->list_mutex);
334 
335    /* Remove all elements from the list */
336    rv = convert_linked_list_err_type(linked_list_flush(p_msg_q->msg_list));
337 
338    pthread_mutex_unlock(&p_msg_q->list_mutex);
339 
340    LOC_LOGD("%s: Message Queue flushed\n", __FUNCTION__);
341 
342    return rv;
343 }
344 
345 /*===========================================================================
346 
347   FUNCTION:   msg_q_unblock
348 
349   ===========================================================================*/
msg_q_unblock(void * msg_q_data)350 msq_q_err_type msg_q_unblock(void* msg_q_data)
351 {
352    if ( msg_q_data == NULL )
353    {
354       LOC_LOGE("%s: Invalid msg_q_data parameter!\n", __FUNCTION__);
355       return eMSG_Q_INVALID_HANDLE;
356    }
357 
358    msg_q* p_msg_q = (msg_q*)msg_q_data;
359    pthread_mutex_lock(&p_msg_q->list_mutex);
360 
361    if( p_msg_q->unblocked )
362    {
363       LOC_LOGE("%s: Message queue has been unblocked.\n", __FUNCTION__);
364       pthread_mutex_unlock(&p_msg_q->list_mutex);
365       return eMSG_Q_UNAVAILABLE_RESOURCE;
366    }
367 
368    LOC_LOGD("%s: Unblocking Message Queue\n", __FUNCTION__);
369    /* Unblocking message queue */
370    p_msg_q->unblocked = 1;
371 
372    /* Allow all the waiters to wake up */
373    pthread_cond_broadcast(&p_msg_q->list_cond);
374 
375    pthread_mutex_unlock(&p_msg_q->list_mutex);
376 
377    LOC_LOGD("%s: Message Queue unblocked\n", __FUNCTION__);
378 
379    return eMSG_Q_SUCCESS;
380 }
381