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