QNX: Modification for QNX 6.5 compilation
[ipc/ipcdev.git] / qnx / src / ipc3x_dev / ti / syslink / ipc / hlos / knl / transports / virtio / VirtQueue.c
1 /*
2  * Copyright (c) 2011-2015, Texas Instruments Incorporated
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * *  Redistributions of source code must retain the above copyright
10  *    notice, this list of conditions and the following disclaimer.
11  *
12  * *  Redistributions in binary form must reproduce the above copyright
13  *    notice, this list of conditions and the following disclaimer in the
14  *    documentation and/or other materials provided with the distribution.
15  *
16  * *  Neither the name of Texas Instruments Incorporated nor the names of
17  *    its contributors may be used to endorse or promote products derived
18  *    from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  */
32 /** ============================================================================
33  *  @file       VirtQueue.c
34  *
35  *  @brief      Virtio Queue implementation for BIOS
36  *
37  *  Differences between BIOS version and Linux kernel (include/linux/virtio.h):
38  *  - Renamed module from virtio.h to VirtQueue_Object.h to match the API prefixes;
39  *  - BIOS (XDC) types and CamelCasing used;
40  *  - virtio_device concept removed (i.e, assumes no containing device);
41  *  - simplified scatterlist from Linux version;
42  *  - VirtQueue_Objects are created statically here, so just added a VirtQueue_Object_init()
43  *    fxn to take the place of the Virtio vring_new_virtqueue() API;
44  *  - The notify function is implicit in the implementation, and not provided
45  *    by the client, as it is in Linux virtio.
46  *
47  *  All VirtQueue operations can be called in any context.
48  *
49  *  The virtio header should be included in an application as follows:
50  *  @code
51  *  #include <ti/ipc/rpmsg/VirtQueue.h>
52  *  @endcode
53  *
54  */
56 #include <ti/syslink/Std.h>
57 #include <ti/syslink/utils/Memory.h>
58 #include <ti/syslink/utils/Trace.h>
60 #include <_ArchIpcInt.h>
61 #include <ArchIpcInt.h>
62 #include "VirtQueue.h"
64 #include <ti/ipc/MultiProc.h>
65 #include <ti/syslink/ProcMgr.h>
67 #include <ti/syslink/utils/String.h>
69 #include "virtio_ring.h"
70 #include "_rpmsg.h"
71 #include <ipu_pm.h>
72 #include <pthread.h>
74 /* Used for defining the size of the virtqueue registry */
75 #define NUM_QUEUES                      2
77 typedef struct VirtQueue_Object {
78     /* Id for this VirtQueue_Object */
79     UInt16                  id;
81     /* The function to call when buffers are consumed (can be NULL) */
82     VirtQueue_callback      callback;
84     /* Shared state */
85     struct vring            vring;
87     /* Number of free buffers */
88     UInt16                  num_free;
90     /* Last available index; updated by VirtQueue_getUsedBuf */
91     UInt16                  last_used_idx;
93     /* Will eventually be used to kick remote processor */
94     UInt16                  procId;
96     /* Interrupt Id for kicking remote processor */
97     UInt16                  intId;
99     /* Local virtual address for vring struct */
100     UInt32                  vaddr;
102     /* Physical address for vring struct */
103     UInt32                  paddr;
105     /* Private arg from user */
106     void *                  arg;
108     /* Mutex to protect vrings from multi-thread access */
109     pthread_mutex_t         mutex;
110 } VirtQueue_Object;
112 static UInt numQueues = 0;
113 static struct VirtQueue_Object *queueRegistry[MultiProc_MAXPROCESSORS][NUM_QUEUES];
115 static UInt32 coreIntId[MultiProc_MAXPROCESSORS];
117 static inline Void * mapPAtoVA(VirtQueue_Handle vq, UInt pa)
119     UInt offset = vq->paddr - pa;
120     return (Void *)(vq->vaddr - offset);
123 static inline UInt mapVAtoPA(VirtQueue_Handle vq, Void * va)
125     UInt offset = vq->vaddr - (UInt)va;
126     return (UInt)(vq->paddr - offset);
129 /*!
130  * ======== VirtQueue_cb ========
131  */
132 Void VirtQueue_cb(Void *buf, VirtQueue_Handle vq)
134     if (vq/* && vq->cb_enabled*/) {
135         /* Call the registered vq callback */
136         vq->callback(vq, vq->arg);
137     }
140 /*!
141  * ======== VirtQueue_kick ========
142  */
143 Void VirtQueue_kick(VirtQueue_Handle vq)
145     /*
146      * We need to expose available array entries before sending an
147      * interrupt.
148      */
149 #if _NTO_VERSION >= 660
150     asm("   DSB ST");
151 #else
152     asm("   DSB  ");
153 #endif
155     GT_2trace(curTrace, GT_2CLASS,
156             "VirtQueue_kick: Sending interrupt to proc %d with payload 0x%x",
157             vq->procId, vq->id);
159 #if defined (IPC_USE_IPU_PM)
160     ipu_pm_restore_ctx(vq->procId);
161 #endif
162     ArchIpcInt_sendInterrupt(vq->procId, vq->intId, vq->id);
165 /*!
166  * ======== VirtQueue_addUsedBufAddr ========
167  */
168 Int VirtQueue_addUsedBufAddr(VirtQueue_Handle vq, Void *buf, UInt32 len)
170     struct vring_used_elem *used = NULL;
171     UInt16 head = 0;
172     Int status = 0;
174     if ((head > vq->vring.num) || (head < 0)) {
175         status = -1;
176         GT_setFailureReason (curTrace,
177                              GT_4CLASS,
178                              "VirtQueue_addUsedBuf",
179                              status,
180                              "head is invalid!");
181     }
182     else {
183         /*
184          * The virtqueue contains a ring of used buffers.  Get a pointer to the
185          * next entry in that used ring.
186          */
187         head = vq->vring.used->idx % vq->vring.num;
188         vq->vring.desc[head].addr = mapVAtoPA(vq, buf);
189         vq->vring.desc[head].len = len;
190         vq->vring.desc[head].flags = 0;
191         used = &vq->vring.used->ring[head];
192         used->id = head;
193         used->len = len;
195         vq->vring.used->idx++;
196     }
198     return status;
201 /*!
202  * ======== VirtQueue_addAvailBuf ========
203  */
204 Int VirtQueue_addAvailBuf(VirtQueue_Handle vq, Void *buf, UInt32 len, Int16 head)
206     UInt16 avail;
208     pthread_mutex_lock(&vq->mutex);
210     if (vq->num_free == 0) {
211         /* There's no more space */
212         GT_setFailureReason (curTrace,
213                              GT_4CLASS,
214                              "VirtQueue_addAvailBuf",
215                              (-1), // TODO: Make this a valid error code
216                              "no more space!");
218     }
219     else {
220         vq->num_free--;
222         avail = vq->vring.avail->idx % vq->vring.num;
223         vq->vring.avail->ring[avail] = head;
225         vq->vring.desc[head].addr = mapVAtoPA(vq, buf);
226         vq->vring.desc[head].len = len;
227         vq->vring.desc[head].flags = 0;
229         /*
230          * Descriptors and available array need to be set before we expose the
231          * new available array entries.
232          */
233 #if _NTO_VERSION >= 660
234         asm("   DMB ST");
235 #else
236         asm("   DMB  ");
237 #endif
239         vq->vring.avail->idx++;
240     }
242     pthread_mutex_unlock(&vq->mutex);
244     return (vq->num_free);
247 /*!
248  * ======== VirtQueue_getUsedBuf ========
249  */
250 Int16 VirtQueue_getUsedBuf(VirtQueue_Object *vq, Void **buf)
252     UInt16 head;
254     pthread_mutex_lock(&vq->mutex);
256     /* There's nothing available? */
257     if (vq->last_used_idx == vq->vring.used->idx) {
258         /* We need to know about added buffers */
259         vq->vring.avail->flags &= ~VRING_AVAIL_F_NO_INTERRUPT;
261         pthread_mutex_unlock(&vq->mutex);
263         return (-1);
264     }
266     /* Only get used array entries after they have been exposed. */
267     asm("   DMB");
269     /* No need to know be kicked about added buffers anymore */
270     //vq->vring.avail->flags |= VRING_AVAIL_F_NO_INTERRUPT; // disabling for now, since there seems to be a race condition where an M3->A9 message is not detected because the interrupt isn't sent.
272     head = vq->vring.used->ring[vq->last_used_idx % vq->vring.num].id;
273     vq->last_used_idx++;
274     vq->num_free++;
276     pthread_mutex_unlock(&vq->mutex);
278     *buf = mapPAtoVA(vq, vq->vring.desc[head].addr);
280     return (head);
283 /*!
284  * ======== VirtQueue_disableCallback ========
285  */
286 Void VirtQueue_disableCallback(VirtQueue_Handle vq)
288     //TODO
289     GT_0trace(curTrace, GT_3CLASS, "VirtQueue_disableCallback not supported.");
292 /*!
293  * ======== VirtQueue_enableCallback ========
294  */
295 Bool VirtQueue_enableCallback(VirtQueue_Handle vq)
297     GT_0trace(curTrace, GT_3CLASS, "VirtQueue_enableCallback not supported.");
299     //TODO
300     return (FALSE);
303 /*!
304  *  @brief      This function implements the interrupt service routine for the
305  *              interrupt received from the remote cores.
306  *
307  *  @param      refData       object to be handled in ISR
308  *
309  *  @sa         VirtQueue_cb
310  */
311 static
312 Bool
313 VirtQueue_ISR (UInt32 msg, Void * arg)
315     UInt32 procId = (UInt32)arg;
316     ProcMgr_Handle handle = NULL;
317     Int status = 0;
319     GT_2trace (curTrace, GT_ENTER, "_VirtQueue_ISR", msg, arg);
321         /* Interrupt clear is done by ArchIpcInt. */
323         switch(msg) {
324             case (UInt)RP_MBOX_ECHO_REPLY:
325                 Osal_printf ("Echo reply from %s",
326                              MultiProc_getName(procId));
327                 break;
329             case (UInt)RP_MBOX_CRASH:
330                 Osal_printf ("Crash notification for %s",
331                              MultiProc_getName(procId));
332                 status = ProcMgr_open(&handle, procId);
333                 if (status >= 0) {
334                     ProcMgr_setState(handle, ProcMgr_State_Error);
335                     ProcMgr_close(&handle);
336                 }
337                 else {
338                     Osal_printf("Failed to open ProcMgr handle");
339                 }
340                 break;
342             case (UInt)RP_MBOX_BOOTINIT_DONE:
343                 Osal_printf ("Got BootInit done from %s",
344                              MultiProc_getName(procId));
345                 // TODO: What to do with this message?
346                 break;
348             case (UInt)RP_MBOX_HIBERNATION_ACK:
349                 Osal_printf ("Got Hibernation ACK from %s",
350                              MultiProc_getName(procId));
351                 break;
353             case (UInt)RP_MBOX_HIBERNATION_CANCEL:
354                 Osal_printf ("Got Hibernation CANCEL from %s",
355                              MultiProc_getName(procId));
356                 break;
358             default:
359                 /*
360                  *  If the message isn't one of the above, it's a virtqueue
361                  *  message
362                  */
363                 if (msg%2 < NUM_QUEUES) {
364                     /* This message is for us! */
365                     VirtQueue_cb((void *)msg, queueRegistry[procId][msg%2]);
366                 }
367             break;
368         }
370     return TRUE;
373 /*!
374  * ======== VirtQueue_create ========
375  */
376 VirtQueue_Handle VirtQueue_create (VirtQueue_callback callback, UInt16 procId,
377                                    UInt16 id, UInt32 vaddr, UInt32 paddr,
378                                    UInt32 num, UInt32 align, Void *arg)
380     VirtQueue_Object *vq = NULL;
382     vq = Memory_alloc(NULL, sizeof(VirtQueue_Object), 0, NULL);
383     if (!vq) {
384         return (NULL);
385     }
387     vq->callback = callback;
388     vq->id = id;
389     numQueues++;
390     vq->procId = procId;
391     vq->intId = coreIntId[procId];
392     vq->arg = arg;
394     /* init the vring */
395     vring_init(&(vq->vring), num, (void *)vaddr, align);
397     vq->num_free = num;
398     vq->last_used_idx = 0;
399     vq->vaddr = vaddr;
400     vq->paddr = paddr;
402     vq->vring.avail->idx = 0;
403     vq->vring.used->idx = 0;
405     /* Initialize the flags */
406     vq->vring.avail->flags = 0;
407     vq->vring.used->flags = 0;
409     /* Store the VirtQueue locally */
410     if (queueRegistry[procId][vq->id%2] == NULL)
411         queueRegistry[procId][vq->id%2] = vq;
412     else {
413         Osal_printf ("VirtQueue ID %d already created", id);
414         Memory_free(NULL, vq, sizeof(VirtQueue_Object));
415         vq = NULL;
416     }
418     /* Initialize mutex */
419     pthread_mutex_init(&vq->mutex, NULL);
421     return (vq);
424 /*!
425  * ======== VirtQueue_delete ========
426 */
427 Int VirtQueue_delete (VirtQueue_Handle * vq)
429     VirtQueue_Object * obj = (VirtQueue_Object *)(*vq);
430     /* Store the VirtQueue locally */
431     queueRegistry[obj->procId][obj->id%2] = NULL;
433     /* Destroy mutex */
434     pthread_mutex_destroy(&obj->mutex);
436     Memory_free(NULL, obj, sizeof(VirtQueue_Object));
437     *vq = NULL;
438     numQueues--;
440     return 0;
443 /*!
444  * ======== VirtQueue_startup ========
445  */
446 Void VirtQueue_startup(UInt16 procId, UInt32 intId, UInt32 paddr)
448     Int32 status = 0;
449     UInt32 arg = procId;
451     coreIntId[procId] = intId;
453     /* Register for interrupts with CORE0, CORE1 messages come through CORE0 */
454     status = ArchIpcInt_interruptRegister (procId,
455                                            intId,
456                                            VirtQueue_ISR, (Ptr)arg);
457     if (status >= 0) {
458         /* Notify the remote proc that the mbox is ready */
459         status = ArchIpcInt_sendInterrupt (procId,
460                                            intId,
461                                            RP_MBOX_READY);
462     }
465 /*!
466  * ======== VirtQueue_startup ========
467  */
468 Void VirtQueue_destroy(UInt16 procId)
470     Int32 status = 0;
471     Int i = 0;
473     for (i = 0; i < NUM_QUEUES; i++) {
474         if (queueRegistry[procId][i]) {
475             VirtQueue_delete(&queueRegistry[procId][i]);
476             queueRegistry[procId][i] = NULL;
477         }
478     }
480     /* Un-register for interrupts with CORE0 */
481     status = ArchIpcInt_interruptUnregister (procId);
482     if (status < 0) {
483         GT_setFailureReason (curTrace,
484                              GT_4CLASS,
485                              "VirtQueue_destroy",
486                              status,
487                              "ArchIpcInt_interruptUnregister failed");
488     }