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