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
  53 /* Internal functions */
  54 static void rpmsg_rx_callback(struct virtqueue *vq);
  55 static void rpmsg_tx_callback(struct virtqueue *vq);
  56
  57 /**
  58  * rpmsg_start_ipc
  59  *
  60  * This function creates communication links(virtqueues) for remote device
  61  * and notifies it to start IPC.
  62  *
  63  * @param rdev - remote device handle
  64  *
  65  * @return - status of function execution
  66  *
  67  */
  68 int rpmsg_start_ipc(struct remote_device *rdev)
  69 {
  70         struct virtio_device *virt_dev;
  71         struct rpmsg_endpoint *ns_ept;
  72         void (*callback[2]) (struct virtqueue * vq);
  73         const char *vq_names[2];
  74         unsigned long dev_features;
  75         int status;
  76         struct virtqueue *vqs[2];
  77         int i;
  78
  79         virt_dev = &rdev->virt_dev;
  80
  81         /* Initialize names and callbacks based on the device role */
  82         if (rdev->role == RPMSG_MASTER) {
  83                 vq_names[0] = "tx_vq";
  84                 vq_names[1] = "rx_vq";
  85                 callback[0] = rpmsg_tx_callback;
  86                 callback[1] = rpmsg_rx_callback;
  87         } else {
  88                 vq_names[0] = "rx_vq";
  89                 vq_names[1] = "tx_vq";
  90                 callback[0] = rpmsg_rx_callback;
  91                 callback[1] = rpmsg_tx_callback;
  92         }
  93
  94         /* Create virtqueues for remote device */
  95         status = virt_dev->func->create_virtqueues(virt_dev, 0,
  96                                                    RPMSG_MAX_VQ_PER_RDEV,
  97                                                    vq_names, callback,
  98                                                    RPMSG_NULL);
  99         if (status != RPMSG_SUCCESS) {
 100                 return status;
 101         }
 102
 103         dev_features = virt_dev->func->get_features(virt_dev);
 104
 105         /*
 106          * Create name service announcement endpoint if device supports name
 107          * service announcement feature.
 108          */
 109         if ((dev_features & (1 << VIRTIO_RPMSG_F_NS))) {
 110                 rdev->support_ns = RPMSG_TRUE;
 111                 ns_ept = _create_endpoint(rdev, rpmsg_ns_callback, rdev,
 112                                           RPMSG_NS_EPT_ADDR);
 113                 if (!ns_ept) {
 114                         return RPMSG_ERR_NO_MEM;
 115                 }
 116         }
 117
 118         /* Initialize notifications for vring. */
 119         if (rdev->role == RPMSG_MASTER) {
 120                 vqs[0] = rdev->tvq;
 121                 vqs[1] = rdev->rvq;
 122         } else {
 123                 vqs[0] = rdev->rvq;
 124                 vqs[1] = rdev->tvq;
 125         }
 126         for (i = 0; i <= 1; i++) {
 127                 status = hil_enable_vring_notifications(i, vqs[i]);
 128                 if (status != RPMSG_SUCCESS) {
 129                         return status;
 130                 }
 131         }
 132
 133         status = rpmsg_rdev_notify(rdev);
 134
 135         return status;
 136 }
 137
 138 /**
 139  * _rpmsg_create_channel
 140  *
 141  * Creates new rpmsg channel with the given parameters.
 142  *
 143  * @param rdev - pointer to remote device which contains the channel
 144  * @param name - name of the device
 145  * @param src  - source address for the rpmsg channel
 146  * @param dst  - destination address for the rpmsg channel
 147  *
 148  * @return - pointer to new rpmsg channel
 149  *
 150  */
 151 struct rpmsg_channel *_rpmsg_create_channel(struct remote_device *rdev,
 152                                             char *name, unsigned long src,
 153                                             unsigned long dst)
 154 {
 155         struct rpmsg_channel *rp_chnl;
 156
 157         rp_chnl = env_allocate_memory(sizeof(struct rpmsg_channel));
 158         if (rp_chnl) {
 159                 memset(rp_chnl, 0x00, sizeof(struct rpmsg_channel));
 160                 strncpy(rp_chnl->name, name, sizeof(rp_chnl->name));
 161                 rp_chnl->src = src;
 162                 rp_chnl->dst = dst;
 163                 rp_chnl->rdev = rdev;
 164                 /* Place channel on channels list */
 165                 metal_mutex_acquire(&rdev->lock);
 166                 metal_list_add_tail(&rdev->rp_channels, &rp_chnl->node);
 167                 metal_mutex_release(&rdev->lock);
 168         }
 169
 170         return rp_chnl;
 171 }
 172
 173 /**
 174  * _rpmsg_delete_channel
 175  *
 176  * Deletes given rpmsg channel.
 177  *
 178  * @param rp_chnl -  pointer to rpmsg channel to delete
 179  *
 180  * return - none
 181  */
 182 void _rpmsg_delete_channel(struct rpmsg_channel *rp_chnl)
 183 {
 184         if (rp_chnl) {
 185                 metal_mutex_acquire(&rp_chnl->rdev->lock);
 186                 metal_list_del(&rp_chnl->node);
 187                 metal_mutex_release(&rp_chnl->rdev->lock);
 188                 env_free_memory(rp_chnl);
 189         }
 190 }
 191
 192 /**
 193  * _create_endpoint
 194  *
 195  * This function creates rpmsg endpoint.
 196  *
 197  * @param rdev    - pointer to remote device
 198  * @param cb      - Rx completion call back
 199  * @param priv    - private data
 200  * @param addr    - endpoint src address
 201  *
 202  * @return - pointer to endpoint control block
 203  *
 204  */
 205 struct rpmsg_endpoint *_create_endpoint(struct remote_device *rdev,
 206                                         rpmsg_rx_cb_t cb, void *priv,
 207                                         unsigned long addr)
 208 {
 209
 210         struct rpmsg_endpoint *rp_ept;
 211         struct llist *node;
 212         int status = RPMSG_SUCCESS;
 213
 214         rp_ept = env_allocate_memory(sizeof(struct rpmsg_endpoint));
 215         if (!rp_ept) {
 216                 return RPMSG_NULL;
 217         }
 218         memset(rp_ept, 0x00, sizeof(struct rpmsg_endpoint));
 219
 220         node = env_allocate_memory(sizeof(struct llist));
 221         if (!node) {
 222                 env_free_memory(rp_ept);
 223                 return RPMSG_NULL;
 224         }
 225
 226         metal_mutex_acquire(&rdev->lock);
 227
 228         if (addr != RPMSG_ADDR_ANY) {
 229                 /*
 230                  * Application has requested a particular src address for endpoint,
 231                  * first check if address is available.
 232                  */
 233                 if (!rpmsg_is_address_set
 234                     (rdev->bitmap, RPMSG_ADDR_BMP_SIZE, addr)) {
 235                         /* Mark the address as used in the address bitmap. */
 236                         rpmsg_set_address(rdev->bitmap, RPMSG_ADDR_BMP_SIZE,
 237                                           addr);
 238
 239                 } else {
 240                         status = RPMSG_ERR_DEV_ADDR;
 241                 }
 242         } else {
 243                 addr = rpmsg_get_address(rdev->bitmap, RPMSG_ADDR_BMP_SIZE);
 244                 if ((int)addr < 0) {
 245                         status = RPMSG_ERR_DEV_ADDR;
 246                 }
 247         }
 248
 249         /* Do cleanup in case of error and return */
 250         if (RPMSG_SUCCESS != status) {
 251                 env_free_memory(node);
 252                 env_free_memory(rp_ept);
 253                 metal_mutex_release(&rdev->lock);
 254                 return RPMSG_NULL;
 255         }
 256
 257         rp_ept->addr = addr;
 258         rp_ept->cb = cb;
 259         rp_ept->priv = priv;
 260
 261         node->data = rp_ept;
 262         add_to_list(&rdev->rp_endpoints, node);
 263
 264         metal_mutex_release(&rdev->lock);
 265
 266         return rp_ept;
 267 }
 268
 269 /**
 270  * rpmsg_destroy_ept
 271  *
 272  * This function deletes rpmsg endpoint and performs cleanup.
 273  *
 274  * @param rdev   - pointer to remote device
 275  * @param rp_ept - pointer to endpoint to destroy
 276  *
 277  */
 278 void _destroy_endpoint(struct remote_device *rdev,
 279                        struct rpmsg_endpoint *rp_ept)
 280 {
 281         struct llist *node;
 282         metal_mutex_acquire(&rdev->lock);
 283         node = rpmsg_rdev_get_endpoint_from_addr(rdev, rp_ept->addr);
 284         if (node) {
 285                 rpmsg_release_address(rdev->bitmap, RPMSG_ADDR_BMP_SIZE,
 286                                       rp_ept->addr);
 287                 remove_from_list(&rdev->rp_endpoints, node);
 288                 metal_mutex_release(&rdev->lock);
 289                 /* free node and rp_ept */
 290                 env_free_memory(node);
 291                 env_free_memory(rp_ept);
 292         } else {
 293                 metal_mutex_release(&rdev->lock);
 294         }
 295 }
 296
 297 /**
 298  * rpmsg_send_ns_message
 299  *
 300  * Sends name service announcement to remote device
 301  *
 302  * @param rdev    - pointer to remote device
 303  * @param rp_chnl - pointer to rpmsg channel
 304  * @param flags   - Channel creation/deletion flags
 305  *
 306  */
 307 void rpmsg_send_ns_message(struct remote_device *rdev,
 308                            struct rpmsg_channel *rp_chnl, unsigned long flags)
 309 {
 310
 311         struct rpmsg_hdr *rp_hdr;
 312         struct rpmsg_ns_msg *ns_msg;
 313         unsigned short idx;
 314         unsigned long len;
 315
 316         metal_mutex_acquire(&rdev->lock);
 317
 318         /* Get Tx buffer. */
 319         rp_hdr = (struct rpmsg_hdr *)rpmsg_get_tx_buffer(rdev, &len, &idx);
 320         if (!rp_hdr) {
 321                 metal_mutex_release(&rdev->lock);
 322                 return;
 323         }
 324
 325         /* Fill out name service data. */
 326         rp_hdr->dst = RPMSG_NS_EPT_ADDR;
 327         rp_hdr->len = sizeof(struct rpmsg_ns_msg);
 328         ns_msg = (struct rpmsg_ns_msg *) RPMSG_LOCATE_DATA(rp_hdr);
 329         strncpy(ns_msg->name, rp_chnl->name, sizeof(rp_chnl->name));
 330         ns_msg->flags = flags;
 331         ns_msg->addr = rp_chnl->src;
 332
 333         /* Place the buffer on virtqueue. */
 334         rpmsg_enqueue_buffer(rdev, rp_hdr, len, idx);
 335
 336         /* Notify the other side that it has data to process. */
 337         virtqueue_kick(rdev->tvq);
 338
 339         metal_mutex_release(&rdev->lock);
 340 }
 341
 342 /**
 343  * rpmsg_enqueue_buffers
 344  *
 345  * Places buffer on the virtqueue for consumption by the other side.
 346  *
 347  * @param rdev   - pointer to remote core
 348  * @param buffer - buffer pointer
 349  * @param len    - buffer length
 350  * @idx          - buffer index
 351  *
 352  * @return - status of function execution
 353  *
 354  */
 355 int rpmsg_enqueue_buffer(struct remote_device *rdev, void *buffer,
 356                          unsigned long len, unsigned short idx)
 357 {
 358         struct llist node;
 359         int status;
 360
 361         /* Initialize buffer node */
 362         node.data = buffer;
 363         node.attr = len;
 364         node.next = RPMSG_NULL;
 365         node.prev = RPMSG_NULL;
 366
 367         if (rdev->role == RPMSG_REMOTE) {
 368                 status = virtqueue_add_buffer(rdev->tvq, &node, 0, 1, buffer);
 369         } else {
 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 llist node;
 391
 392         /* Initialize buffer node */
 393         node.data = buffer;
 394         node.attr = len;
 395         node.next = RPMSG_NULL;
 396         node.prev = RPMSG_NULL;
 397
 398         if (rdev->role == RPMSG_REMOTE) {
 399                 virtqueue_add_buffer(rdev->rvq, &node, 0, 1, buffer);
 400         } else {
 401                 virtqueue_add_consumed_buffer(rdev->rvq, idx, len);
 402         }
 403 }
 404
 405 /**
 406  * rpmsg_get_tx_buffer
 407  *
 408  * Provides buffer to transmit messages.
 409  *
 410  * @param rdev - pointer to remote device
 411  * @param len  - length of returned buffer
 412  * @param idx  - buffer index
 413  *
 414  * return - pointer to buffer.
 415  */
 416 void *rpmsg_get_tx_buffer(struct remote_device *rdev, unsigned long *len,
 417                           unsigned short *idx)
 418 {
 419         void *data;
 420
 421         if (rdev->role == RPMSG_REMOTE) {
 422                 data = virtqueue_get_buffer(rdev->tvq, (uint32_t *) len);
 423                 if (data == RPMSG_NULL) {
 424                         data = sh_mem_get_buffer(rdev->mem_pool);
 425                         *len = RPMSG_BUFFER_SIZE;
 426                 }
 427         } else {
 428                 data =
 429                     virtqueue_get_available_buffer(rdev->tvq, idx,
 430                                                    (uint32_t *) len);
 431         }
 432         return ((void *)env_map_vatopa(data));
 433 }
 434
 435 /**
 436  * rpmsg_get_rx_buffer
 437  *
 438  * Retrieves the received buffer from the virtqueue.
 439  *
 440  * @param rdev - pointer to remote device
 441  * @param len  - size of received buffer
 442  * @param idx  - index of buffer
 443  *
 444  * @return - pointer to received buffer
 445  *
 446  */
 447 void *rpmsg_get_rx_buffer(struct remote_device *rdev, unsigned long *len,
 448                           unsigned short *idx)
 449 {
 450
 451         void *data;
 452         if (rdev->role == RPMSG_REMOTE) {
 453                 data = virtqueue_get_buffer(rdev->rvq, (uint32_t *) len);
 454         } else {
 455                 data =
 456                     virtqueue_get_available_buffer(rdev->rvq, idx,
 457                                                    (uint32_t *) len);
 458         }
 459         return ((void *)env_map_vatopa(data));
 460 }
 461
 462 /**
 463  * rpmsg_free_buffer
 464  *
 465  * Frees the allocated buffers.
 466  *
 467  * @param rdev   - pointer to remote device
 468  * @param buffer - pointer to buffer to free
 469  *
 470  */
 471 void rpmsg_free_buffer(struct remote_device *rdev, void *buffer)
 472 {
 473         if (rdev->role == RPMSG_REMOTE) {
 474                 sh_mem_free_buffer(buffer, rdev->mem_pool);
 475         }
 476 }
 477
 478 /**
 479  * rpmsg_tx_callback
 480  *
 481  * Tx callback function.
 482  *
 483  * @param vq - pointer to virtqueue on which Tx is has been
 484  *             completed.
 485  *
 486  */
 487 static void rpmsg_tx_callback(struct virtqueue *vq)
 488 {
 489         struct remote_device *rdev;
 490         struct virtio_device *vdev;
 491         struct rpmsg_channel *rp_chnl;
 492         struct metal_list *node;
 493
 494         vdev = (struct virtio_device *)vq->vq_dev;
 495         rdev = (struct remote_device *)vdev;
 496
 497         /* Check if the remote device is master. */
 498         if (rdev->role == RPMSG_MASTER) {
 499
 500                 /* Notification is received from the master. Now the remote(us) can
 501                  * performs one of two operations;
 502                  *
 503                  * a. If name service announcement is supported then it will send NS message.
 504                  *    else
 505                  * b. It will update the channel state to active so that further communication
 506                  *    can take place.
 507                  */
 508                 metal_list_for_each(&rdev->rp_channels, node) {
 509                         rp_chnl = metal_container_of(node,
 510                                 struct rpmsg_channel, node);
 511
 512                         if (rp_chnl->state == RPMSG_CHNL_STATE_IDLE) {
 513
 514                                 if (rdev->support_ns) {
 515                                         rp_chnl->state = RPMSG_CHNL_STATE_NS;
 516                                 } else {
 517                                         rp_chnl->state =
 518                                             RPMSG_CHNL_STATE_ACTIVE;
 519                                 }
 520
 521                                 if (rp_chnl->state == RPMSG_CHNL_STATE_NS) {
 522                                         rpmsg_send_ns_message(rdev, rp_chnl,
 523                                                               RPMSG_NS_CREATE);
 524                                 }
 525                         }
 526
 527                 }
 528         }
 529 }
 530
 531 /**
 532  * rpmsg_rx_callback
 533  *
 534  * Rx callback function.
 535  *
 536  * @param vq - pointer to virtqueue on which messages is received
 537  *
 538  */
 539 void rpmsg_rx_callback(struct virtqueue *vq)
 540 {
 541         struct remote_device *rdev;
 542         struct virtio_device *vdev;
 543         struct rpmsg_channel *rp_chnl;
 544         struct rpmsg_endpoint *rp_ept;
 545         struct rpmsg_hdr *rp_hdr;
 546         struct llist *node;
 547         struct metal_list *chnode;
 548         unsigned long len;
 549         unsigned short idx;
 550
 551         vdev = (struct virtio_device *)vq->vq_dev;
 552         rdev = (struct remote_device *)vdev;
 553
 554         if (rdev->role == RPMSG_MASTER) {
 555                 metal_list_for_each(&rdev->rp_channels, chnode) {
 556                         rp_chnl = metal_container_of(chnode,
 557                                 struct rpmsg_channel, node);
 558                         if (rp_chnl->state == RPMSG_CHNL_STATE_IDLE) {
 559                                 if (rdev->support_ns) {
 560                                         rp_chnl->state = RPMSG_CHNL_STATE_NS;
 561                                         rpmsg_send_ns_message(rdev, rp_chnl,
 562                                                       RPMSG_NS_CREATE);
 563                                 } else {
 564                                         rp_chnl->state = RPMSG_CHNL_STATE_ACTIVE;
 565                                 }
 566                                 return;
 567                         }
 568                 }
 569         }
 570
 571         metal_mutex_acquire(&rdev->lock);
 572
 573         /* Process the received data from remote node */
 574         rp_hdr = (struct rpmsg_hdr *)rpmsg_get_rx_buffer(rdev, &len, &idx);
 575
 576         metal_mutex_release(&rdev->lock);
 577
 578         while (rp_hdr) {
 579
 580                 /* Get the channel node from the remote device channels list. */
 581                 metal_mutex_acquire(&rdev->lock);
 582                 node = rpmsg_rdev_get_endpoint_from_addr(rdev, rp_hdr->dst);
 583                 metal_mutex_release(&rdev->lock);
 584
 585                 if (!node)
 586                         /* Fatal error no endpoint for the given dst addr. */
 587                         return;
 588
 589                 rp_ept = (struct rpmsg_endpoint *)node->data;
 590
 591                 rp_chnl = rp_ept->rp_chnl;
 592
 593                 if ((rp_chnl) && (rp_chnl->state == RPMSG_CHNL_STATE_NS)) {
 594                         /* First message from RPMSG Master, update channel
 595                          * destination address and state */
 596                         rp_chnl->dst = rp_hdr->src;
 597                         rp_chnl->state = RPMSG_CHNL_STATE_ACTIVE;
 598
 599                         /* Notify channel creation to application */
 600                         if (rdev->channel_created) {
 601                                 rdev->channel_created(rp_chnl);
 602                         }
 603                 } else {
 604                         rp_ept->cb(rp_chnl, (void *)RPMSG_LOCATE_DATA(rp_hdr), rp_hdr->len,
 605                                    rp_ept->priv, rp_hdr->src);
 606                 }
 607
 608                 metal_mutex_acquire(&rdev->lock);
 609
 610                 /* Return used buffers. */
 611                 rpmsg_return_buffer(rdev, rp_hdr, len, idx);
 612
 613                 rp_hdr =
 614                     (struct rpmsg_hdr *)rpmsg_get_rx_buffer(rdev, &len, &idx);
 615                 metal_mutex_release(&rdev->lock);
 616         }
 617 }
 618
 619 /**
 620  * rpmsg_ns_callback
 621  *
 622  * This callback handles name service announcement from the remote device
 623  * and creates/deletes rpmsg channels.
 624  *
 625  * @param server_chnl - pointer to server channel control block.
 626  * @param data        - pointer to received messages
 627  * @param len         - length of received data
 628  * @param priv        - any private data
 629  * @param src         - source address
 630  *
 631  * @return - none
 632  */
 633 void rpmsg_ns_callback(struct rpmsg_channel *server_chnl, void *data, int len,
 634                        void *priv, unsigned long src)
 635 {
 636         struct remote_device *rdev;
 637         struct rpmsg_channel *rp_chnl;
 638         struct rpmsg_ns_msg *ns_msg;
 639
 640         (void)server_chnl;
 641         (void)src;
 642
 643         rdev = (struct remote_device *)priv;
 644
 645         //FIXME: This assumes same name string size for channel name both on master
 646         //and remote. If this is not the case then we will have to parse the
 647         //message contents.
 648
 649         ns_msg = (struct rpmsg_ns_msg *)data;
 650         ns_msg->name[len - 1] = '\0';
 651
 652         if (ns_msg->flags & RPMSG_NS_DESTROY) {
 653                 metal_mutex_acquire(&rdev->lock);
 654                 rp_chnl = rpmsg_rdev_get_chnl_from_id(rdev, ns_msg->name);
 655                 metal_mutex_release(&rdev->lock);
 656                 if (rp_chnl) {
 657                         if (rdev->channel_destroyed) {
 658                                 rdev->channel_destroyed(rp_chnl);
 659                         }
 660                         rpmsg_destroy_ept(rp_chnl->rp_ept);
 661                         _rpmsg_delete_channel(rp_chnl);
 662                 }
 663         } else {
 664                 rp_chnl =
 665                     _rpmsg_create_channel(rdev, ns_msg->name, 0x00,
 666                                           ns_msg->addr);
 667                 if (rp_chnl) {
 668                         rp_chnl->state = RPMSG_CHNL_STATE_ACTIVE;
 669                         /* Create default endpoint for channel */
 670                         rp_chnl->rp_ept =
 671                             rpmsg_create_ept(rp_chnl, rdev->default_cb, rdev,
 672                                              RPMSG_ADDR_ANY);
 673                         if (rp_chnl->rp_ept) {
 674                                 rp_chnl->src = rp_chnl->rp_ept->addr;
 675                                 /*
 676                                  * Echo back the NS message to remote in order to
 677                                  * complete the connection stage. Remote will know the endpoint
 678                                  * address from this point onward which will enable it to send
 679                                  * message without waiting for any application level message from
 680                                  * master.
 681                                  */
 682                                 rpmsg_send(rp_chnl, data, len);
 683                                 if (rdev->channel_created) {
 684                                         rdev->channel_created(rp_chnl);
 685                                 }
 686                         }
 687                 }
 688         }
 689 }
 690
 691 /**
 692  * rpmsg_get_address
 693  *
 694  * This function provides unique 32 bit address.
 695  *
 696  * @param bitmap - bit map for addresses
 697  * @param size   - size of bitmap
 698  *
 699  * return - a unique address
 700  */
 701 int rpmsg_get_address(unsigned long *bitmap, int size)
 702 {
 703         int addr = -1;
 704         int i, tmp32;
 705
 706         /* Find first available buffer */
 707         for (i = 0; i < size; i++) {
 708                 tmp32 = get_first_zero_bit(bitmap[i]);
 709
 710                 if (tmp32 < 32) {
 711                         addr = tmp32 + (i*32);
 712                         bitmap[i] |= (1 << tmp32);
 713                         break;
 714                 }
 715         }
 716
 717         return addr;
 718 }
 719
 720 /**
 721  * rpmsg_release_address
 722  *
 723  * Frees the given address.
 724  *
 725  * @param bitmap - bit map for addresses
 726  * @param size   - size of bitmap
 727  * @param addr   - address to free
 728  *
 729  * return - none
 730  */
 731 int rpmsg_release_address(unsigned long *bitmap, int size, int addr)
 732 {
 733         unsigned int i, j;
 734         unsigned long mask = 1;
 735
 736         if (addr >= size * 32)
 737                 return -1;
 738
 739         /* Mark the addr as available */
 740         i = addr / 32;
 741         j = addr % 32;
 742
 743         mask = mask << j;
 744         bitmap[i] = bitmap[i] & (~mask);
 745
 746         return RPMSG_SUCCESS;
 747 }
 748
 749 /**
 750  * rpmsg_is_address_set
 751  *
 752  * Checks whether address is used or free.
 753  *
 754  * @param bitmap - bit map for addresses
 755  * @param size   - size of bitmap
 756  * @param addr   - address to free
 757  *
 758  * return - TRUE/FALSE
 759  */
 760 int rpmsg_is_address_set(unsigned long *bitmap, int size, int addr)
 761 {
 762         int i, j;
 763         unsigned long mask = 1;
 764
 765         if (addr >= size * 32)
 766                 return -1;
 767
 768         /* Mark the id as available */
 769         i = addr / 32;
 770         j = addr % 32;
 771         mask = mask << j;
 772
 773         return (bitmap[i] & mask);
 774 }
 775
 776 /**
 777  * rpmsg_set_address
 778  *
 779  * Marks the address as consumed.
 780  *
 781  * @param bitmap - bit map for addresses
 782  * @param size   - size of bitmap
 783  * @param addr   - address to free
 784  *
 785  * return - none
 786  */
 787 int rpmsg_set_address(unsigned long *bitmap, int size, int addr)
 788 {
 789         int i, j;
 790         unsigned long mask = 1;
 791
 792         if (addr >= size * 32)
 793                 return -1;
 794
 795         /* Mark the id as available */
 796         i = addr / 32;
 797         j = addr % 32;
 798         mask = mask << j;
 799         bitmap[i] |= mask;
 800
 801         return RPMSG_SUCCESS;
 802 }