packages/ti/ipc/rpmsg/MessageQCopy.c

   1 /*
   2  * Copyright (c) 2011-2013, 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       MessageQCopy.c
  34  *
  35  *  @brief      A simple copy-based MessageQ, to work with Linux virtio_rp_msg.
  36  *
  37  *  Notes:
  38  *  - The logic in the functions for sending (_put()) and receiving _swiFxn()
  39  *    depend on the role (host or slave) the processor is playing in the
  40  *    asymmetric virtio I/O.
  41  *  - The host always adds *available* buffers to send/receive, while the slave
  42  *    always adds *used* buffers to send/receive.
  43  *  - The logic is summarized below:
  44  *
  45  *    Host:
  46  *    - Prime vq_host with avail bufs, and kick vq_host so slave can send.
  47  *    - To send a buffer to the slave processor:
  48  *          allocate a tx buffer, or get_used_buf(vq_slave);
  49  *               >> copy data into buf <<
  50  *          add_avail_buf(vq_slave);
  51  *          kick(vq_slave);
  52  *    - To receive buffer from slave processor:
  53  *          get_used_buf(vq_host);
  54  *              >> empty data from buf <<
  55  *          add_avail_buf(vq_host);
  56  *          kick(vq_host);
  57  *
  58  *    Slave:
  59  *    - To receive buffer from the host:
  60  *          get_avail_buf(vq_slave);
  61  *              >> empty data from buf <<
  62  *          add_used_buf(vq_slave);
  63  *          kick(vq_slave);
  64  *    - To send buffer to the host:
  65  *          get_avail_buf(vq_host);
  66  *              >> copy data into buf <<
  67  *          add_used_buf(vq_host);
  68  *          kick(vq_host);
  69  *
  70  *  ============================================================================
  71  */
  72
  73 /* this define must precede inclusion of any xdc header file */
  74 #define Registry_CURDESC ti_ipc_rpmsg_MessageQCopy__Desc
  75 #define MODULE_NAME "ti.ipc.rpmsg.MessageQCopy"
  76
  77 #include <xdc/std.h>
  78 #include <string.h>
  79
  80 #include <xdc/runtime/System.h>
  81 #include <xdc/runtime/Error.h>
  82 #include <xdc/runtime/Assert.h>
  83 #include <xdc/runtime/Memory.h>
  84 #include <xdc/runtime/Main.h>
  85 #include <xdc/runtime/Registry.h>
  86 #include <xdc/runtime/Log.h>
  87 #include <xdc/runtime/Diags.h>
  88
  89 #include <ti/sysbios/BIOS.h>
  90 #include <ti/sysbios/knl/Swi.h>
  91 #include <ti/sysbios/knl/Semaphore.h>
  92 #include <ti/sysbios/heaps/HeapBuf.h>
  93 #include <ti/sysbios/gates/GateSwi.h>
  94
  95 #include <ti/sdo/utils/List.h>
  96 #include <ti/ipc/MultiProc.h>
  97
  98 #include <ti/ipc/rpmsg/MessageQCopy.h>
  99
 100 #include "_VirtQueue.h"
 101
 102 /* TBD: VirtQueue.h needs to somehow get factored out of family directory .*/
 103 #if defined(OMAPL138)
 104 #include <ti/ipc/family/omapl138/VirtQueue.h>
 105 #elif defined(TCI6614)
 106 #include <ti/ipc/family/tci6614/VirtQueue.h>
 107 #elif defined(TCI6638)
 108 #include <ti/ipc/family/tci6638/VirtQueue.h>
 109 #elif defined(OMAP5)
 110 #include <ti/ipc/family/omap54xx/VirtQueue.h>
 111 #else
 112 #error unknown processor!
 113 #endif
 114
 115 /* =============================================================================
 116  * Structures & Enums
 117  * =============================================================================
 118  */
 119
 120 /* Various arbitrary limits: */
 121 #define MAXMESSAGEQOBJECTS     256
 122 #define MAXMESSAGEBUFFERS      512
 123 #define MSGBUFFERSIZE          512   // Max payload + sizeof(ListElem)
 124 #define MAXHEAPSIZE            (MAXMESSAGEBUFFERS * MSGBUFFERSIZE)
 125 #define HEAPALIGNMENT          8
 126
 127 /* The MessageQCopy Object */
 128 typedef struct MessageQCopy_Object {
 129     UInt32           queueId;      /* Unique id (procId | queueIndex)       */
 130     Semaphore_Handle semHandle;    /* I/O Completion                        */
 131     MessageQCopy_callback cb;      /* MessageQCopy Callback */
 132     UArg             arg;          /* Callback argument */
 133     List_Handle      queue;        /* Queue of pending messages             */
 134     Bool             unblocked;    /* Use with signal to unblock _receive() */
 135 } MessageQCopy_Object;
 136
 137 /* Module_State */
 138 typedef struct MessageQCopy_Module {
 139     /* Instance gate: */
 140     GateSwi_Handle gateSwi;
 141     /* Array of messageQObjects in the system: */
 142     struct MessageQCopy_Object  *msgqObjects[MAXMESSAGEQOBJECTS];
 143     /* Heap from which to allocate free messages for copying: */
 144     HeapBuf_Handle              heap;
 145 } MessageQCopy_Module;
 146
 147 /* Message Header: Must match mp_msg_hdr in virtio_rp_msg.h on Linux side. */
 148 typedef struct MessageQCopy_MsgHeader {
 149     Bits32 srcAddr;                 /* source endpoint addr               */
 150     Bits32 dstAddr;                 /* destination endpoint addr          */
 151     Bits32 reserved;                /* reserved                           */
 152     Bits16 dataLen;                 /* data length                        */
 153     Bits16 flags;                   /* bitmask of different flags         */
 154     UInt8  payload[];               /* Data payload                       */
 155 } MessageQCopy_MsgHeader;
 156
 157 typedef MessageQCopy_MsgHeader *MessageQCopy_Msg;
 158
 159 /* Element to hold payload copied onto receiver's queue.                  */
 160 typedef struct Queue_elem {
 161     List_Elem    elem;              /* Allow list linking.                */
 162     UInt         len;               /* Length of data                     */
 163     UInt32       src;               /* Src address/endpt of the msg       */
 164     Char         data[];            /* payload begins here                */
 165 } Queue_elem;
 166
 167 /* Combine transport related objects into a struct for future migration: */
 168 typedef struct MessageQCopy_Transport  {
 169     Swi_Handle       swiHandle;
 170     VirtQueue_Handle virtQueue_toHost;
 171     VirtQueue_Handle virtQueue_fromHost;
 172     Semaphore_Handle semHandle_toHost;
 173 } MessageQCopy_Transport;
 174
 175
 176 /* module diags mask */
 177 Registry_Desc Registry_CURDESC;
 178
 179 static MessageQCopy_Module      module;
 180 static MessageQCopy_Transport   transport;
 181
 182 /* We create a fixed size heap over this memory for copying received msgs */
 183 #pragma DATA_ALIGN (recv_buffers, HEAPALIGNMENT)
 184 static UInt8 recv_buffers[MAXHEAPSIZE];
 185
 186 /* Module ref count: */
 187 static Int curInit = 0;
 188
 189 /*
 190  *  ======== MessageQCopy_swiFxn ========
 191  */
 192 #define FXNN "MessageQCopy_swiFxn"
 193 static Void MessageQCopy_swiFxn(UArg arg0, UArg arg1)
 194 {
 195     Int16             token;
 196     MessageQCopy_Msg  msg;
 197     UInt16            dstProc = MultiProc_self();
 198     Bool              usedBufAdded = FALSE;
 199     int len;
 200
 201     Log_print0(Diags_ENTRY, "--> "FXNN);
 202
 203     /* Process all available buffers: */
 204     while ((token = VirtQueue_getAvailBuf(transport.virtQueue_fromHost,
 205                                          (Void **)&msg, &len)) >= 0) {
 206
 207         Log_print3(Diags_INFO, FXNN": \n\tReceived msg: from: 0x%x, "
 208                    "to: 0x%x, dataLen: %d",
 209                   (IArg)msg->srcAddr, (IArg)msg->dstAddr, (IArg)msg->dataLen);
 210
 211         /* Pass to destination queue (on this proc), or callback: */
 212         MessageQCopy_send(dstProc, msg->dstAddr, msg->srcAddr,
 213                          (Ptr)msg->payload, msg->dataLen);
 214
 215         VirtQueue_addUsedBuf(transport.virtQueue_fromHost, token,
 216                                                             RPMSG_BUF_SIZE);
 217         usedBufAdded = TRUE;
 218     }
 219
 220     if (usedBufAdded)  {
 221        /* Tell host we've processed the buffers: */
 222        VirtQueue_kick(transport.virtQueue_fromHost);
 223     }
 224     Log_print0(Diags_EXIT, "<-- "FXNN);
 225 }
 226 #undef FXNN
 227
 228
 229 #define FXNN "callback_availBufReady"
 230 static Void callback_availBufReady(VirtQueue_Handle vq)
 231 {
 232
 233     if (vq == transport.virtQueue_fromHost)  {
 234        /* Post a SWI to process all incoming messages */
 235         Log_print0(Diags_INFO, FXNN": virtQueue_fromHost kicked");
 236         Swi_post(transport.swiHandle);
 237     }
 238     else if (vq == transport.virtQueue_toHost) {
 239        /* Note: We normally post nothing for transport.virtQueue_toHost,
 240         * unless we were starved for buffers, and we turned on notifications.
 241         */
 242         Semaphore_post(transport.semHandle_toHost);
 243         Log_print0(Diags_INFO, FXNN": virtQueue_toHost kicked");
 244     }
 245 }
 246 #undef FXNN
 247
 248 /* =============================================================================
 249  *  MessageQCopy Functions:
 250  * =============================================================================
 251  */
 252
 253 /*
 254  *  ======== MessasgeQCopy_init ========
 255  *
 256  *
 257  */
 258 #define FXNN "MessageQCopy_init"
 259 Void MessageQCopy_init(UInt16 remoteProcId)
 260 {
 261     GateSwi_Params gatePrms;
 262     HeapBuf_Params prms;
 263     int     i;
 264     Registry_Result result;
 265     Bool    isHost;
 266     VirtQueue_Params vqParams;
 267
 268     if (curInit++) {
 269         Log_print1(Diags_ENTRY, "--> "FXNN": (remoteProcId=%d)",
 270                     (IArg)remoteProcId);
 271         goto exit;  /* module already initialized */
 272     }
 273
 274     /* register with xdc.runtime to get a diags mask */
 275     result = Registry_addModule(&Registry_CURDESC, MODULE_NAME);
 276     Assert_isTrue(result == Registry_SUCCESS, (Assert_Id)NULL);
 277
 278     /* Log should be after the Registry_CURDESC is initialized */
 279     Log_print1(Diags_ENTRY, "--> "FXNN": (remoteProcId=%d)",
 280                 (IArg)remoteProcId);
 281
 282     /* Gate to protect module object and lists: */
 283     GateSwi_Params_init(&gatePrms);
 284     module.gateSwi = GateSwi_create(&gatePrms, NULL);
 285
 286     /* Initialize Module State: */
 287     for (i = 0; i < MAXMESSAGEQOBJECTS; i++) {
 288        module.msgqObjects[i] = NULL;
 289     }
 290
 291     HeapBuf_Params_init(&prms);
 292     prms.blockSize    = MSGBUFFERSIZE;
 293     prms.numBlocks    = MAXMESSAGEBUFFERS;
 294     prms.buf          = recv_buffers;
 295     prms.bufSize      = MAXHEAPSIZE;
 296     prms.align        = HEAPALIGNMENT;
 297     module.heap       = HeapBuf_create(&prms, NULL);
 298     if (module.heap == 0) {
 299        System_abort("MessageQCopy_init: HeapBuf_create returned 0\n");
 300     }
 301     transport.semHandle_toHost = Semaphore_create(0, NULL, NULL);
 302
 303     isHost = (MultiProc_self() == MultiProc_getId("HOST"));
 304
 305     /* Initialize Transport related objects: */
 306
 307     VirtQueue_Params_init(&vqParams);
 308     if (isHost)  {
 309       /* We don't handle this case currently! Host would need to prime vq. */
 310       Assert_isTrue(FALSE, NULL);
 311     }
 312     else {
 313       vqParams.callback = callback_availBufReady;
 314     }
 315
 316     /*
 317      * Create a pair VirtQueues (one for sending, one for receiving).
 318      * Note: First one gets an even, second gets odd vq ID.
 319      */
 320     vqParams.vqId = ID_SELF_TO_A9;
 321     transport.virtQueue_toHost   = (Ptr)VirtQueue_create(remoteProcId,
 322                                                     &vqParams, NULL);
 323     vqParams.vqId = ID_A9_TO_SELF;
 324     transport.virtQueue_fromHost = (Ptr)VirtQueue_create(remoteProcId,
 325                                                     &vqParams, NULL);
 326
 327     /* Plug Vring Interrupts, and wait for host ready to recv kick: */
 328     VirtQueue_startup(remoteProcId, isHost);
 329
 330     /* construct the Swi to process incoming messages: */
 331     transport.swiHandle = Swi_create(MessageQCopy_swiFxn, NULL, NULL);
 332
 333 exit:
 334     Log_print0(Diags_EXIT, "<-- "FXNN);
 335 }
 336 #undef FXNN
 337
 338 /*
 339  *  ======== MessasgeQCopy_finalize ========
 340  */
 341 #define FXNN "MessageQCopy_finalize"
 342 Void MessageQCopy_finalize()
 343 {
 344     Log_print0(Diags_ENTRY, "--> "FXNN);
 345
 346     if (!curInit || --curInit) {
 347          goto exit; /* module still in use, or uninitialized */
 348     }
 349
 350     /* Tear down Module */
 351     HeapBuf_delete(&(module.heap));
 352
 353     Swi_delete(&(transport.swiHandle));
 354
 355     GateSwi_delete(&module.gateSwi);
 356
 357 exit:
 358     Log_print0(Diags_EXIT, "<-- "FXNN);
 359 }
 360 #undef FXNN
 361
 362 /*
 363  *  ======== MessageQCopy_create ========
 364  */
 365 #define FXNN "MessageQCopy_create"
 366 MessageQCopy_Handle MessageQCopy_create(UInt32 reserved,
 367                                         MessageQCopy_callback cb,
 368                                         UArg arg,
 369                                         UInt32 * endpoint)
 370 {
 371     MessageQCopy_Object    *obj = NULL;
 372     Bool                   found = FALSE;
 373     Int                    i;
 374     UInt16                 queueIndex = 0;
 375     IArg key;
 376
 377     Log_print4(Diags_ENTRY, "--> "FXNN": "
 378                 "(reserved=%d, cb=0x%x, arg=0x%x, endpoint=0x%x)",
 379                 (IArg)reserved, (IArg)cb, (IArg)arg, (IArg)endpoint);
 380
 381     Assert_isTrue((curInit > 0) , NULL);
 382
 383     key = GateSwi_enter(module.gateSwi);
 384
 385     if (reserved == MessageQCopy_ASSIGN_ANY)  {
 386        /* Search the array for a free slot above reserved: */
 387        for (i = MessageQCopy_MAX_RESERVED_ENDPOINT + 1;
 388            (i < MAXMESSAGEQOBJECTS) && (found == FALSE) ; i++) {
 389            if (module.msgqObjects[i] == NULL) {
 390             queueIndex = i;
 391             found = TRUE;
 392             break;
 393            }
 394        }
 395     }
 396     else if ((queueIndex = reserved) <= MessageQCopy_MAX_RESERVED_ENDPOINT) {
 397        if (module.msgqObjects[queueIndex] == NULL) {
 398            found = TRUE;
 399        }
 400     }
 401
 402     if (found)  {
 403        obj = Memory_alloc(NULL, sizeof(MessageQCopy_Object), 0, NULL);
 404        if (obj != NULL) {
 405            if (cb) {
 406                /* Store callback and it's arg instead of semaphore: */
 407                obj->cb = cb;
 408                obj->arg= arg;
 409            }
 410            else {
 411                obj->cb = NULL;
 412
 413                /* Allocate a semaphore to signal when messages received: */
 414                obj->semHandle = Semaphore_create(0, NULL, NULL);
 415
 416                /* Create our queue of to be received messages: */
 417                obj->queue = List_create(NULL, NULL);
 418            }
 419
 420            /* Store our endpoint, and object: */
 421            obj->queueId = queueIndex;
 422            module.msgqObjects[queueIndex] = obj;
 423
 424            /* See MessageQCopy_unblock() */
 425            obj->unblocked = FALSE;
 426
 427            *endpoint    = queueIndex;
 428            Log_print1(Diags_LIFECYCLE, FXNN": endPt created: %d",
 429                         (IArg)queueIndex);
 430        }
 431     }
 432
 433     GateSwi_leave(module.gateSwi, key);
 434
 435     Log_print1(Diags_EXIT, "<-- "FXNN": 0x%x", (IArg)obj);
 436     return (obj);
 437 }
 438 #undef FXNN
 439
 440 /*
 441  *  ======== MessageQCopy_delete ========
 442  */
 443 #define FXNN "MessageQCopy_delete"
 444 Int MessageQCopy_delete(MessageQCopy_Handle *handlePtr)
 445 {
 446     Int                    status = MessageQCopy_S_SUCCESS;
 447     MessageQCopy_Object    *obj;
 448     Queue_elem             *payload;
 449     IArg                   key;
 450
 451     Log_print1(Diags_ENTRY, "--> "FXNN": (handlePtr=0x%x)", (IArg)handlePtr);
 452
 453     Assert_isTrue((curInit > 0) , NULL);
 454
 455     if (handlePtr && (obj = (MessageQCopy_Object *)(*handlePtr)))  {
 456
 457        if (obj->cb) {
 458            obj->cb = NULL;
 459            obj->arg= NULL;
 460        }
 461        else {
 462            Semaphore_delete(&(obj->semHandle));
 463
 464            /* Free/discard all queued message buffers: */
 465            while ((payload = (Queue_elem *)List_get(obj->queue)) != NULL) {
 466                HeapBuf_free(module.heap, (Ptr)payload, MSGBUFFERSIZE);
 467            }
 468
 469            List_delete(&(obj->queue));
 470        }
 471
 472        /* Null out our slot: */
 473        key = GateSwi_enter(module.gateSwi);
 474        module.msgqObjects[obj->queueId] = NULL;
 475        GateSwi_leave(module.gateSwi, key);
 476
 477        Log_print1(Diags_LIFECYCLE, FXNN": endPt deleted: %d",
 478                         (IArg)obj->queueId);
 479
 480        /* Now free the obj */
 481        Memory_free(NULL, obj, sizeof(MessageQCopy_Object));
 482
 483        *handlePtr = NULL;
 484     }
 485
 486     Log_print1(Diags_EXIT, "<-- "FXNN": %d", (IArg)status);
 487     return(status);
 488 }
 489 #undef FXNN
 490
 491 /*
 492  *  ======== MessageQCopy_recv ========
 493  */
 494 #define FXNN "MessageQCopy_recv"
 495 Int MessageQCopy_recv(MessageQCopy_Handle handle, Ptr data, UInt16 *len,
 496                       UInt32 *rplyEndpt, UInt timeout)
 497 {
 498     Int                 status = MessageQCopy_S_SUCCESS;
 499     MessageQCopy_Object *obj = (MessageQCopy_Object *)handle;
 500     Bool                semStatus;
 501     Queue_elem          *payload;
 502
 503     Log_print5(Diags_ENTRY, "--> "FXNN": (handle=0x%x, data=0x%x, len=0x%x,"
 504                "rplyEndpt=0x%x, timeout=%d)", (IArg)handle, (IArg)data,
 505                (IArg)len, (IArg)rplyEndpt, (IArg)timeout);
 506
 507     Assert_isTrue((curInit > 0) , NULL);
 508     /* A callback was set: client should not be calling this fxn! */
 509     Assert_isTrue((!obj->cb), NULL);
 510
 511     /* Check vring for pending messages before we block: */
 512     Swi_post(transport.swiHandle);
 513
 514     /*  Block until notified. */
 515     semStatus = Semaphore_pend(obj->semHandle, timeout);
 516
 517     if (semStatus == FALSE)  {
 518        status = MessageQCopy_E_TIMEOUT;
 519        Log_print0(Diags_STATUS, FXNN": Sem pend timeout!");
 520     }
 521     else if (obj->unblocked) {
 522        status = MessageQCopy_E_UNBLOCKED;
 523     }
 524     else  {
 525        payload = (Queue_elem *)List_get(obj->queue);
 526        Assert_isTrue((!payload), NULL);
 527     }
 528
 529     if (status == MessageQCopy_S_SUCCESS)  {
 530        /* Now, copy payload to client and free our internal msg */
 531        memcpy(data, payload->data, payload->len);
 532        *len = payload->len;
 533        *rplyEndpt = payload->src;
 534
 535        HeapBuf_free(module.heap, (Ptr)payload,
 536                     (payload->len + sizeof(Queue_elem)));
 537     }
 538
 539     Log_print1(Diags_EXIT, "<-- "FXNN": %d", (IArg)status);
 540     return (status);
 541 }
 542 #undef FXNN
 543
 544 /*
 545  *  ======== MessageQCopy_send ========
 546  */
 547 #define FXNN "MessageQCopy_send"
 548 Int MessageQCopy_send(UInt16 dstProc,
 549                       UInt32 dstEndpt,
 550                       UInt32 srcEndpt,
 551                       Ptr    data,
 552                       UInt16 len)
 553 {
 554     Int               status = MessageQCopy_S_SUCCESS;
 555     MessageQCopy_Object   *obj;
 556     Int16             token = 0;
 557     MessageQCopy_Msg  msg;
 558     Queue_elem        *payload;
 559     UInt              size;
 560     IArg              key;
 561     int length;
 562
 563     Log_print5(Diags_ENTRY, "--> "FXNN": (dstProc=%d, dstEndpt=%d, "
 564                "srcEndpt=%d, data=0x%x, len=%d", (IArg)dstProc, (IArg)dstEndpt,
 565                (IArg)srcEndpt, (IArg)data, (IArg)len);
 566
 567     Assert_isTrue((curInit > 0) , NULL);
 568
 569     if (dstProc != MultiProc_self()) {
 570         /* Send to remote processor: */
 571         do {
 572             key = GateSwi_enter(module.gateSwi);  /* Protect vring structs */
 573             token = VirtQueue_getAvailBuf(transport.virtQueue_toHost,
 574                     (Void **)&msg, &length);
 575             GateSwi_leave(module.gateSwi, key);
 576         } while (token < 0 && Semaphore_pend(transport.semHandle_toHost,
 577                                              BIOS_WAIT_FOREVER));
 578         if (token >= 0) {
 579             /* Copy the payload and set message header: */
 580             memcpy(msg->payload, data, len);
 581             msg->dataLen = len;
 582             msg->dstAddr = dstEndpt;
 583             msg->srcAddr = srcEndpt;
 584             msg->flags = 0;
 585             msg->reserved = 0;
 586
 587             key = GateSwi_enter(module.gateSwi);  // Protect vring structs.
 588             VirtQueue_addUsedBuf(transport.virtQueue_toHost, token,
 589                                                             RPMSG_BUF_SIZE);
 590             VirtQueue_kick(transport.virtQueue_toHost);
 591             GateSwi_leave(module.gateSwi, key);
 592         }
 593         else {
 594             status = MessageQCopy_E_FAIL;
 595             Log_print0(Diags_STATUS, FXNN": getAvailBuf failed!");
 596         }
 597     }
 598     else {
 599         /* Put on a Message queue on this processor: */
 600
 601         /* Protect from MessageQCopy_delete */
 602         key = GateSwi_enter(module.gateSwi);
 603         obj = module.msgqObjects[dstEndpt];
 604         GateSwi_leave(module.gateSwi, key);
 605
 606         if (obj == NULL) {
 607             Log_print1(Diags_STATUS, FXNN": no object for endpoint: %d",
 608                    (IArg)dstEndpt);
 609             status = MessageQCopy_E_NOENDPT;
 610             return status;
 611         }
 612
 613         /* If callback registered, call it: */
 614         if (obj->cb) {
 615             Log_print2(Diags_INFO, FXNN": calling callback with data len: "
 616                             "%d, from: %d\n", len, srcEndpt);
 617             obj->cb(obj, obj->arg, data, len, srcEndpt);
 618         }
 619         else {
 620             /* else, put on a Message queue on this processor: */
 621             /* Allocate a buffer to copy the payload: */
 622             size = len + sizeof(Queue_elem);
 623
 624             /* HeapBuf_alloc() is non-blocking, so needs protection: */
 625             key = GateSwi_enter(module.gateSwi);
 626             payload = (Queue_elem *)HeapBuf_alloc(module.heap, size, 0, NULL);
 627             GateSwi_leave(module.gateSwi, key);
 628
 629             if (payload != NULL)  {
 630                 memcpy(payload->data, data, len);
 631                 payload->len = len;
 632                 payload->src = srcEndpt;
 633
 634                 /* Put on the endpoint's queue and signal: */
 635                 List_put(obj->queue, (List_Elem *)payload);
 636                 Semaphore_post(obj->semHandle);
 637             }
 638             else {
 639                 status = MessageQCopy_E_MEMORY;
 640                 Log_print0(Diags_STATUS, FXNN": HeapBuf_alloc failed!");
 641             }
 642         }
 643     }
 644
 645     Log_print1(Diags_EXIT, "<-- "FXNN": %d", (IArg)status);
 646     return (status);
 647 }
 648 #undef FXNN
 649
 650 /*
 651  *  ======== MessageQCopy_unblock ========
 652  */
 653 #define FXNN "MessageQCopy_unblock"
 654 Void MessageQCopy_unblock(MessageQCopy_Handle handle)
 655 {
 656     MessageQCopy_Object *obj = (MessageQCopy_Object *)handle;
 657
 658     Log_print1(Diags_ENTRY, "--> "FXNN": (handle=0x%x)", (IArg)handle);
 659
 660     Assert_isTrue((!obj->cb), NULL);
 661     /* Set instance to 'unblocked' state, and post */
 662     obj->unblocked = TRUE;
 663     Semaphore_post(obj->semHandle);
 664     Log_print0(Diags_EXIT, "<-- "FXNN);
 665 }
 666 #undef FXNN