lib/rpmsg/rpmsg_core.c

   1 /*
   2  * Copyright (c) 2014, Mentor Graphics Corporation
   3  * All rights reserved.
   4  * Copyright (c) 2015 Xilinx, Inc. All rights reserved.
   5  * Copyright (c) 2016 Freescale Semiconductor, Inc. All rights reserved.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions are met:
   9  *
  10  * 1. Redistributions of source code must retain the above copyright notice,
  11  *    this list of conditions and the following disclaimer.
  12  * 2. Redistributions in binary form must reproduce the above copyright notice,
  13  *    this list of conditions and the following disclaimer in the documentation
  14  *    and/or other materials provided with the distribution.
  15  * 3. Neither the name of Mentor Graphics Corporation nor the names of its
  16  *    contributors may be used to endorse or promote products derived from this
  17  *    software without specific prior written permission.
  18  *
  19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
  23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  29  * POSSIBILITY OF SUCH DAMAGE.
  30  */
  31
  32 /**************************************************************************
  33  * FILE NAME
  34  *
  35  *       rpmsg_core.c
  36  *
  37  * COMPONENT
  38  *
  39  *       OpenAMP
  40  *
  41  * DESCRIPTION
  42  *
  43  * This file provides the core functionality of RPMSG messaging part like
  44  * message parsing ,Rx/Tx callbacks handling , channel creation/deletion
  45  * and address management.
  46  *
  47  *
  48  **************************************************************************/
  49 #include <string.h>
  50 #include "openamp/rpmsg.h"
  51 #include "metal/utilities.h"
  52 #include "metal/io.h"
  53 #include "metal/cache.h"
  54 #include "metal/alloc.h"
  55 #include "metal/cpu.h"
  56
  57 /* Internal functions */
  58 static void rpmsg_rx_callback(struct virtqueue *vq);
  59 static void rpmsg_tx_callback(struct virtqueue *vq);
  60
  61 /**
  62  * rpmsg_start_ipc
  63  *
  64  * This function creates communication links(virtqueues) for remote device
  65  * and notifies it to start IPC.
  66  *
  67  * @param rdev - remote device handle
  68  *
  69  * @return - status of function execution
  70  *
  71  */
  72 int rpmsg_start_ipc(struct remote_device *rdev)
  73 {
  74         struct virtio_device *virt_dev;
  75         struct rpmsg_endpoint *ns_ept;
  76         void (*callback[2]) (struct virtqueue * vq);
  77         const char *vq_names[2];
  78         unsigned long dev_features;
  79         int status;
  80         struct virtqueue *vqs[2];
  81         int i;
  82
  83         virt_dev = &rdev->virt_dev;
  84
  85         /* Initialize names and callbacks based on the device role */
  86         if (rdev->role == RPMSG_MASTER) {
  87                 vq_names[0] = "tx_vq";
  88                 vq_names[1] = "rx_vq";
  89                 callback[0] = rpmsg_tx_callback;
  90                 callback[1] = rpmsg_rx_callback;
  91         } else {
  92                 vq_names[0] = "rx_vq";
  93                 vq_names[1] = "tx_vq";
  94                 callback[0] = rpmsg_rx_callback;
  95                 callback[1] = rpmsg_tx_callback;
  96         }
  97
  98         /* Create virtqueues for remote device */
  99         status = virt_dev->func->create_virtqueues(virt_dev, 0,
 100                                                    RPMSG_MAX_VQ_PER_RDEV,
 101                                                    vq_names, callback,
 102                                                    RPMSG_NULL);
 103         if (status != RPMSG_SUCCESS) {
 104                 return status;
 105         }
 106
 107         dev_features = virt_dev->func->get_features(virt_dev);
 108
 109         /*
 110          * Create name service announcement endpoint if device supports name
 111          * service announcement feature.
 112          */
 113         if ((dev_features & (1 << VIRTIO_RPMSG_F_NS))) {
 114                 rdev->support_ns = RPMSG_TRUE;
 115                 ns_ept = _create_endpoint(rdev, rpmsg_ns_callback, rdev,
 116                                           RPMSG_NS_EPT_ADDR);
 117                 if (!ns_ept) {
 118                         return RPMSG_ERR_NO_MEM;
 119                 }
 120         }
 121
 122         /* Initialize notifications for vring. */
 123         if (rdev->role == RPMSG_MASTER) {
 124                 vqs[0] = rdev->tvq;
 125                 vqs[1] = rdev->rvq;
 126         } else {
 127                 vqs[0] = rdev->rvq;
 128                 vqs[1] = rdev->tvq;
 129         }
 130         for (i = 0; i <= 1; i++) {
 131                 status = hil_enable_vring_notifications(i, vqs[i]);
 132                 if (status != RPMSG_SUCCESS) {
 133                         return status;
 134                 }
 135         }
 136
 137         if (rdev->role == RPMSG_MASTER) {
 138                 virt_dev->func->set_status(virt_dev,
 139                         VIRTIO_CONFIG_STATUS_DRIVER_OK);
 140                 status = rpmsg_rdev_notify(rdev);
 141         }
 142         if (status == RPMSG_SUCCESS)
 143                 rdev->state = RPMSG_DEV_STATE_ACTIVE;
 144
 145         return status;
 146 }
 147
 148 /**
 149  * _rpmsg_create_channel
 150  *
 151  * Creates new rpmsg channel with the given parameters.
 152  *
 153  * @param rdev - pointer to remote device which contains the channel
 154  * @param name - name of the device
 155  * @param src  - source address for the rpmsg channel
 156  * @param dst  - destination address for the rpmsg channel
 157  *
 158  * @return - pointer to new rpmsg channel
 159  *
 160  */
 161 struct rpmsg_channel *_rpmsg_create_channel(struct remote_device *rdev,
 162                                             char *name, unsigned long src,
 163                                             unsigned long dst)
 164 {
 165         struct rpmsg_channel *rp_chnl;
 166
 167         rp_chnl = metal_allocate_memory(sizeof(struct rpmsg_channel));
 168         if (rp_chnl) {
 169                 memset(rp_chnl, 0x00, sizeof(struct rpmsg_channel));
 170                 strncpy(rp_chnl->name, name, sizeof(rp_chnl->name));
 171                 rp_chnl->src = src;
 172                 rp_chnl->dst = dst;
 173                 rp_chnl->rdev = rdev;
 174                 /* Place channel on channels list */
 175                 metal_mutex_acquire(&rdev->lock);
 176                 metal_list_add_tail(&rdev->rp_channels, &rp_chnl->node);
 177                 metal_mutex_release(&rdev->lock);
 178         }
 179
 180         return rp_chnl;
 181 }
 182
 183 /**
 184  * _rpmsg_delete_channel
 185  *
 186  * Deletes given rpmsg channel.
 187  *
 188  * @param rp_chnl -  pointer to rpmsg channel to delete
 189  *
 190  * return - none
 191  */
 192 void _rpmsg_delete_channel(struct rpmsg_channel *rp_chnl)
 193 {
 194         if (rp_chnl) {
 195                 metal_mutex_acquire(&rp_chnl->rdev->lock);
 196                 metal_list_del(&rp_chnl->node);
 197                 metal_mutex_release(&rp_chnl->rdev->lock);
 198                 metal_free_memory(rp_chnl);
 199         }
 200 }
 201
 202 /**
 203  * _create_endpoint
 204  *
 205  * This function creates rpmsg endpoint.
 206  *
 207  * @param rdev    - pointer to remote device
 208  * @param cb      - Rx completion call back
 209  * @param priv    - private data
 210  * @param addr    - endpoint src address
 211  *
 212  * @return - pointer to endpoint control block
 213  *
 214  */
 215 struct rpmsg_endpoint *_create_endpoint(struct remote_device *rdev,
 216                                         rpmsg_rx_cb_t cb, void *priv,
 217                                         unsigned long addr)
 218 {
 219
 220         struct rpmsg_endpoint *rp_ept;
 221         int status = RPMSG_SUCCESS;
 222
 223         rp_ept = metal_allocate_memory(sizeof(struct rpmsg_endpoint));
 224         if (!rp_ept) {
 225                 return RPMSG_NULL;
 226         }
 227         memset(rp_ept, 0x00, sizeof(struct rpmsg_endpoint));
 228
 229         metal_mutex_acquire(&rdev->lock);
 230
 231         if (addr != RPMSG_ADDR_ANY) {
 232                 /*
 233                  * Application has requested a particular src address for endpoint,
 234                  * first check if address is available.
 235                  */
 236                 if (!rpmsg_is_address_set
 237                     (rdev->bitmap, RPMSG_ADDR_BMP_SIZE, addr)) {
 238                         /* Mark the address as used in the address bitmap. */
 239                         rpmsg_set_address(rdev->bitmap, RPMSG_ADDR_BMP_SIZE,
 240                                           addr);
 241
 242                 } else {
 243                         status = RPMSG_ERR_DEV_ADDR;
 244                 }
 245         } else {
 246                 addr = rpmsg_get_address(rdev->bitmap, RPMSG_ADDR_BMP_SIZE);
 247                 if ((int)addr < 0) {
 248                         status = RPMSG_ERR_DEV_ADDR;
 249                 }
 250         }
 251
 252         /* Do cleanup in case of error and return */
 253         if (RPMSG_SUCCESS != status) {
 254                 metal_free_memory(rp_ept);
 255                 metal_mutex_release(&rdev->lock);
 256                 return RPMSG_NULL;
 257         }
 258
 259         rp_ept->addr = addr;
 260         rp_ept->cb = cb;
 261         rp_ept->priv = priv;
 262
 263         metal_list_add_tail(&rdev->rp_endpoints, &rp_ept->node);
 264
 265         metal_mutex_release(&rdev->lock);
 266
 267         return rp_ept;
 268 }
 269
 270 /**
 271  * rpmsg_destroy_ept
 272  *
 273  * This function deletes rpmsg endpoint and performs cleanup.
 274  *
 275  * @param rdev   - pointer to remote device
 276  * @param rp_ept - pointer to endpoint to destroy
 277  *
 278  */
 279 void _destroy_endpoint(struct remote_device *rdev,
 280                        struct rpmsg_endpoint *rp_ept)
 281 {
 282         metal_mutex_acquire(&rdev->lock);
 283         rpmsg_release_address(rdev->bitmap, RPMSG_ADDR_BMP_SIZE,
 284                               rp_ept->addr);
 285         metal_list_del(&rp_ept->node);
 286         metal_mutex_release(&rdev->lock);
 287         /* free node and rp_ept */
 288         metal_free_memory(rp_ept);
 289 }
 290
 291 /**
 292  * rpmsg_send_ns_message
 293  *
 294  * Sends name service announcement to remote device
 295  *
 296  * @param rdev    - pointer to remote device
 297  * @param rp_chnl - pointer to rpmsg channel
 298  * @param flags   - Channel creation/deletion flags
 299  *
 300  */
 301 int rpmsg_send_ns_message(struct remote_device *rdev,
 302                            struct rpmsg_channel *rp_chnl, unsigned long flags)
 303 {
 304
 305         struct rpmsg_hdr *rp_hdr;
 306         struct rpmsg_ns_msg *ns_msg;
 307         unsigned short idx;
 308         unsigned long len;
 309
 310         metal_mutex_acquire(&rdev->lock);
 311
 312         /* Get Tx buffer. */
 313         rp_hdr = (struct rpmsg_hdr *)rpmsg_get_tx_buffer(rdev, &len, &idx);
 314         if (!rp_hdr) {
 315                 metal_mutex_release(&rdev->lock);
 316                 return -RPMSG_ERR_NO_BUFF;
 317         }
 318
 319         /* Fill out name service data. */
 320         rp_hdr->dst = RPMSG_NS_EPT_ADDR;
 321         rp_hdr->len = sizeof(struct rpmsg_ns_msg);
 322         ns_msg = (struct rpmsg_ns_msg *) RPMSG_LOCATE_DATA(rp_hdr);
 323         strncpy(ns_msg->name, rp_chnl->name, sizeof(rp_chnl->name));
 324         ns_msg->flags = flags;
 325         ns_msg->addr = rp_chnl->src;
 326
 327         /* Place the buffer on virtqueue. */
 328         rpmsg_enqueue_buffer(rdev, rp_hdr, len, idx);
 329
 330         /* Notify the other side that it has data to process. */
 331         virtqueue_kick(rdev->tvq);
 332
 333         metal_mutex_release(&rdev->lock);
 334         return RPMSG_SUCCESS;
 335 }
 336
 337 /**
 338  * rpmsg_enqueue_buffers
 339  *
 340  * Places buffer on the virtqueue for consumption by the other side.
 341  *
 342  * @param rdev   - pointer to remote core
 343  * @param buffer - buffer pointer
 344  * @param len    - buffer length
 345  * @idx          - buffer index
 346  *
 347  * @return - status of function execution
 348  *
 349  */
 350 int rpmsg_enqueue_buffer(struct remote_device *rdev, void *buffer,
 351                          unsigned long len, unsigned short idx)
 352 {
 353         int status;
 354         struct metal_sg sg;
 355         struct metal_io_region *io;
 356
 357         io = rdev->proc->sh_buff.io;
 358         if (io) {
 359                 if (! (io->mem_flags & METAL_UNCACHED))
 360                         metal_cache_flush(buffer, (unsigned int)len);
 361         }
 362         if (rdev->role == RPMSG_REMOTE) {
 363                 /* Initialize buffer node */
 364                 sg.virt = buffer;
 365                 sg.len = len;
 366                 sg.io = io;
 367                 status = virtqueue_add_buffer(rdev->tvq, &sg, 0, 1, buffer);
 368         } else {
 369                 (void)sg;
 370                 status = virtqueue_add_consumed_buffer(rdev->tvq, idx, len);
 371         }
 372
 373         return status;
 374 }
 375
 376 /**
 377  * rpmsg_return_buffer
 378  *
 379  * Places the used buffer back on the virtqueue.
 380  *
 381  * @param rdev   - pointer to remote core
 382  * @param buffer - buffer pointer
 383  * @param len    - buffer length
 384  * @param idx    - buffer index
 385  *
 386  */
 387 void rpmsg_return_buffer(struct remote_device *rdev, void *buffer,
 388                          unsigned long len, unsigned short idx)
 389 {
 390         struct metal_sg sg;
 391
 392         if (rdev->role == RPMSG_REMOTE) {
 393                 /* Initialize buffer node */
 394                 sg.virt = buffer;
 395                 sg.len = len;
 396                 sg.io = rdev->proc->sh_buff.io;
 397                 virtqueue_add_buffer(rdev->rvq, &sg, 0, 1, buffer);
 398         } else {
 399                 (void)sg;
 400                 virtqueue_add_consumed_buffer(rdev->rvq, idx, len);
 401         }
 402 }
 403
 404 /**
 405  * rpmsg_get_tx_buffer
 406  *
 407  * Provides buffer to transmit messages.
 408  *
 409  * @param rdev - pointer to remote device
 410  * @param len  - length of returned buffer
 411  * @param idx  - buffer index
 412  *
 413  * return - pointer to buffer.
 414  */
 415 void *rpmsg_get_tx_buffer(struct remote_device *rdev, unsigned long *len,
 416                           unsigned short *idx)
 417 {
 418         void *data;
 419
 420         if (rdev->role == RPMSG_REMOTE) {
 421                 data = virtqueue_get_buffer(rdev->tvq, (uint32_t *) len, idx);
 422                 if (data == RPMSG_NULL) {
 423                         data = sh_mem_get_buffer(rdev->mem_pool);
 424                         *len = RPMSG_BUFFER_SIZE;
 425                 }
 426         } else {
 427                 data =
 428                     virtqueue_get_available_buffer(rdev->tvq, idx,
 429                                                    (uint32_t *) len);
 430         }
 431         return data;
 432 }
 433
 434 /**
 435  * rpmsg_get_rx_buffer
 436  *
 437  * Retrieves the received buffer from the virtqueue.
 438  *
 439  * @param rdev - pointer to remote device
 440  * @param len  - size of received buffer
 441  * @param idx  - index of buffer
 442  *
 443  * @return - pointer to received buffer
 444  *
 445  */
 446 void *rpmsg_get_rx_buffer(struct remote_device *rdev, unsigned long *len,
 447                           unsigned short *idx)
 448 {
 449
 450         void *data;
 451         if (rdev->role == RPMSG_REMOTE) {
 452                 data = virtqueue_get_buffer(rdev->rvq, (uint32_t *) len, idx);
 453         } else {
 454                 data =
 455                     virtqueue_get_available_buffer(rdev->rvq, idx,
 456                                                    (uint32_t *) len);
 457         }
 458         if (data) {
 459                 struct metal_io_region *io;
 460                 io = rdev->proc->sh_buff.io;
 461                 if (io) {
 462                         if (! (io->mem_flags & METAL_UNCACHED))
 463                                 metal_cache_invalidate(data,
 464                                         (unsigned int)(*len));
 465                 }
 466         }
 467
 468         return data;
 469 }
 470
 471 /**
 472  * rpmsg_free_buffer
 473  *
 474  * Frees the allocated buffers.
 475  *
 476  * @param rdev   - pointer to remote device
 477  * @param buffer - pointer to buffer to free
 478  *
 479  */
 480 void rpmsg_free_buffer(struct remote_device *rdev, void *buffer)
 481 {
 482         if (rdev->role == RPMSG_REMOTE) {
 483                 sh_mem_free_buffer(buffer, rdev->mem_pool);
 484         }
 485 }
 486
 487 /**
 488  * rpmsg_tx_callback
 489  *
 490  * Tx callback function.
 491  *
 492  * @param vq - pointer to virtqueue on which Tx is has been
 493  *             completed.
 494  *
 495  */
 496 static void rpmsg_tx_callback(struct virtqueue *vq)
 497 {
 498         struct remote_device *rdev;
 499         struct virtio_device *vdev;
 500         struct rpmsg_channel *rp_chnl;
 501         struct metal_list *node;
 502
 503         vdev = (struct virtio_device *)vq->vq_dev;
 504         rdev = (struct remote_device *)vdev;
 505
 506         /* Check if the remote device is master. */
 507         if (rdev->role == RPMSG_MASTER) {
 508
 509                 /* Notification is received from the master. Now the remote(us) can
 510                  * performs one of two operations;
 511                  *
 512                  * a. If name service announcement is supported then it will send NS message.
 513                  *    else
 514                  * b. It will update the channel state to active so that further communication
 515                  *    can take place.
 516                  */
 517                 metal_list_for_each(&rdev->rp_channels, node) {
 518                         rp_chnl = metal_container_of(node,
 519                                 struct rpmsg_channel, node);
 520
 521                         if (rp_chnl->state == RPMSG_CHNL_STATE_IDLE) {
 522
 523                                 if (rdev->support_ns) {
 524                                         if (rpmsg_send_ns_message(rdev, rp_chnl,
 525                                                       RPMSG_NS_CREATE) ==
 526                                                 RPMSG_SUCCESS)
 527                                                 rp_chnl->state =
 528                                                         RPMSG_CHNL_STATE_NS;
 529                                 } else {
 530                                         rp_chnl->state =
 531                                             RPMSG_CHNL_STATE_ACTIVE;
 532                                 }
 533
 534                         }
 535
 536                 }
 537         }
 538 }
 539
 540 /**
 541  * rpmsg_rx_callback
 542  *
 543  * Rx callback function.
 544  *
 545  * @param vq - pointer to virtqueue on which messages is received
 546  *
 547  */
 548 void rpmsg_rx_callback(struct virtqueue *vq)
 549 {
 550         struct remote_device *rdev;
 551         struct virtio_device *vdev;
 552         struct rpmsg_channel *rp_chnl;
 553         struct rpmsg_endpoint *rp_ept;
 554         struct rpmsg_hdr *rp_hdr;
 555         struct rpmsg_hdr_reserved *reserved;
 556         struct metal_list *node;
 557         unsigned long len;
 558         unsigned short idx;
 559
 560         vdev = (struct virtio_device *)vq->vq_dev;
 561         rdev = (struct remote_device *)vdev;
 562
 563         if (rdev->role == RPMSG_MASTER) {
 564                 metal_list_for_each(&rdev->rp_channels, node) {
 565                         rp_chnl = metal_container_of(node,
 566                                 struct rpmsg_channel, node);
 567                         if (rp_chnl->state == RPMSG_CHNL_STATE_IDLE) {
 568                                 if (rdev->support_ns) {
 569                                         if (rpmsg_send_ns_message(rdev, rp_chnl,
 570                                                       RPMSG_NS_CREATE) ==
 571                                                 RPMSG_SUCCESS)
 572                                                 rp_chnl->state =
 573                                                         RPMSG_CHNL_STATE_NS;
 574                                 } else {
 575                                         rp_chnl->state = RPMSG_CHNL_STATE_ACTIVE;
 576                                 }
 577                                 return;
 578                         }
 579                 }
 580         }
 581
 582         metal_mutex_acquire(&rdev->lock);
 583
 584         /* Process the received data from remote node */
 585         rp_hdr = (struct rpmsg_hdr *)rpmsg_get_rx_buffer(rdev, &len, &idx);
 586
 587         metal_mutex_release(&rdev->lock);
 588
 589         while (rp_hdr) {
 590
 591                 /* Get the channel node from the remote device channels list. */
 592                 metal_mutex_acquire(&rdev->lock);
 593                 rp_ept = rpmsg_rdev_get_endpoint_from_addr(rdev, rp_hdr->dst);
 594                 metal_mutex_release(&rdev->lock);
 595
 596                 if (!rp_ept)
 597                         /* Fatal error no endpoint for the given dst addr. */
 598                         return;
 599
 600                 rp_chnl = rp_ept->rp_chnl;
 601
 602                 if ((rp_chnl) && (rp_chnl->state == RPMSG_CHNL_STATE_NS)) {
 603                         /* First message from RPMSG Master, update channel
 604                          * destination address and state */
 605                         rp_chnl->dst = rp_hdr->src;
 606                         rp_chnl->state = RPMSG_CHNL_STATE_ACTIVE;
 607
 608                         /* Notify channel creation to application */
 609                         if (rdev->channel_created) {
 610                                 rdev->channel_created(rp_chnl);
 611                         }
 612                 } else {
 613                         rp_ept->cb(rp_chnl, (void *)RPMSG_LOCATE_DATA(rp_hdr), rp_hdr->len,
 614                                    rp_ept->priv, rp_hdr->src);
 615                 }
 616
 617                 metal_mutex_acquire(&rdev->lock);
 618
 619                 /* Check whether callback wants to hold buffer */
 620                 if (rp_hdr->reserved & RPMSG_BUF_HELD)
 621                 {
 622                         /* 'rp_hdr->reserved' field is now used as storage for
 623                          * 'idx' to release buffer later */
 624                         reserved = (struct rpmsg_hdr_reserved*)&rp_hdr->reserved;
 625                         reserved->idx = (uint16_t)idx;
 626                 } else {
 627                         /* Return used buffers. */
 628                         rpmsg_return_buffer(rdev, rp_hdr, len, idx);
 629                 }
 630
 631                 rp_hdr =
 632                     (struct rpmsg_hdr *)rpmsg_get_rx_buffer(rdev, &len, &idx);
 633                 metal_mutex_release(&rdev->lock);
 634         }
 635 }
 636
 637 /**
 638  * rpmsg_ns_callback
 639  *
 640  * This callback handles name service announcement from the remote device
 641  * and creates/deletes rpmsg channels.
 642  *
 643  * @param server_chnl - pointer to server channel control block.
 644  * @param data        - pointer to received messages
 645  * @param len         - length of received data
 646  * @param priv        - any private data
 647  * @param src         - source address
 648  *
 649  * @return - none
 650  */
 651 void rpmsg_ns_callback(struct rpmsg_channel *server_chnl, void *data, int len,
 652                        void *priv, unsigned long src)
 653 {
 654         struct remote_device *rdev;
 655         struct rpmsg_channel *rp_chnl;
 656         struct rpmsg_ns_msg *ns_msg;
 657
 658         (void)server_chnl;
 659         (void)src;
 660
 661         rdev = (struct remote_device *)priv;
 662
 663         //FIXME: This assumes same name string size for channel name both on master
 664         //and remote. If this is not the case then we will have to parse the
 665         //message contents.
 666
 667         ns_msg = (struct rpmsg_ns_msg *)data;
 668         ns_msg->name[len - 1] = '\0';
 669
 670         if (ns_msg->flags & RPMSG_NS_DESTROY) {
 671                 metal_mutex_acquire(&rdev->lock);
 672                 rp_chnl = rpmsg_rdev_get_chnl_from_id(rdev, ns_msg->name);
 673                 metal_mutex_release(&rdev->lock);
 674                 if (rp_chnl) {
 675                         if (rdev->channel_destroyed) {
 676                                 rdev->channel_destroyed(rp_chnl);
 677                         }
 678                         rpmsg_destroy_ept(rp_chnl->rp_ept);
 679                         _rpmsg_delete_channel(rp_chnl);
 680                 }
 681         } else {
 682                 rp_chnl =
 683                     _rpmsg_create_channel(rdev, ns_msg->name, 0x00,
 684                                           ns_msg->addr);
 685                 if (rp_chnl) {
 686                         rp_chnl->state = RPMSG_CHNL_STATE_ACTIVE;
 687                         /* Create default endpoint for channel */
 688                         rp_chnl->rp_ept =
 689                             rpmsg_create_ept(rp_chnl, rdev->default_cb, rdev,
 690                                              RPMSG_ADDR_ANY);
 691                         if (rp_chnl->rp_ept) {
 692                                 rp_chnl->src = rp_chnl->rp_ept->addr;
 693                                 /*
 694                                  * Echo back the NS message to remote in order to
 695                                  * complete the connection stage. Remote will know the endpoint
 696                                  * address from this point onward which will enable it to send
 697                                  * message without waiting for any application level message from
 698                                  * master.
 699                                  */
 700                                 rpmsg_send(rp_chnl, data, len);
 701                                 if (rdev->channel_created) {
 702                                         rdev->channel_created(rp_chnl);
 703                                 }
 704                         }
 705                 }
 706         }
 707 }
 708
 709 /**
 710  * rpmsg_get_address
 711  *
 712  * This function provides unique 32 bit address.
 713  *
 714  * @param bitmap - bit map for addresses
 715  * @param size   - size of bitmap
 716  *
 717  * return - a unique address
 718  */
 719 int rpmsg_get_address(unsigned long *bitmap, int size)
 720 {
 721         int addr = -1;
 722         int i, tmp32;
 723
 724         /* Find first available buffer */
 725         for (i = 0; i < size; i++) {
 726                 tmp32 = get_first_zero_bit(bitmap[i]);
 727
 728                 if (tmp32 < 32) {
 729                         addr = tmp32 + (i*32);
 730                         bitmap[i] |= (1 << tmp32);
 731                         break;
 732                 }
 733         }
 734
 735         return addr;
 736 }
 737
 738 /**
 739  * rpmsg_release_address
 740  *
 741  * Frees the given address.
 742  *
 743  * @param bitmap - bit map for addresses
 744  * @param size   - size of bitmap
 745  * @param addr   - address to free
 746  *
 747  * return - none
 748  */
 749 int rpmsg_release_address(unsigned long *bitmap, int size, int addr)
 750 {
 751         unsigned int i, j;
 752         unsigned long mask = 1;
 753
 754         if (addr >= size * 32)
 755                 return -1;
 756
 757         /* Mark the addr as available */
 758         i = addr / 32;
 759         j = addr % 32;
 760
 761         mask = mask << j;
 762         bitmap[i] = bitmap[i] & (~mask);
 763
 764         return RPMSG_SUCCESS;
 765 }
 766
 767 /**
 768  * rpmsg_is_address_set
 769  *
 770  * Checks whether address is used or free.
 771  *
 772  * @param bitmap - bit map for addresses
 773  * @param size   - size of bitmap
 774  * @param addr   - address to free
 775  *
 776  * return - TRUE/FALSE
 777  */
 778 int rpmsg_is_address_set(unsigned long *bitmap, int size, int addr)
 779 {
 780         int i, j;
 781         unsigned long mask = 1;
 782
 783         if (addr >= size * 32)
 784                 return -1;
 785
 786         /* Mark the id as available */
 787         i = addr / 32;
 788         j = addr % 32;
 789         mask = mask << j;
 790
 791         return (bitmap[i] & mask);
 792 }
 793
 794 /**
 795  * rpmsg_set_address
 796  *
 797  * Marks the address as consumed.
 798  *
 799  * @param bitmap - bit map for addresses
 800  * @param size   - size of bitmap
 801  * @param addr   - address to free
 802  *
 803  * return - none
 804  */
 805 int rpmsg_set_address(unsigned long *bitmap, int size, int addr)
 806 {
 807         int i, j;
 808         unsigned long mask = 1;
 809
 810         if (addr >= size * 32)
 811                 return -1;
 812
 813         /* Mark the id as available */
 814         i = addr / 32;
 815         j = addr % 32;
 816         mask = mask << j;
 817         bitmap[i] |= mask;
 818
 819         return RPMSG_SUCCESS;
 820 }