728645d381d9cb2754eaecc330fcae0cc46a0b52
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 */
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"
57 /* Internal functions */
58 static void rpmsg_rx_callback(struct virtqueue *vq);
59 static void rpmsg_tx_callback(struct virtqueue *vq);
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;
83 virt_dev = &rdev->virt_dev;
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 }
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 }
107 dev_features = virt_dev->func->get_features(virt_dev);
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 }
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 }
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;
145 return status;
146 }
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;
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 }
180 return rp_chnl;
181 }
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 }
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 {
220 struct rpmsg_endpoint *rp_ept;
221 int status = RPMSG_SUCCESS;
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));
229 metal_mutex_acquire(&rdev->lock);
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);
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 }
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 }
259 rp_ept->addr = addr;
260 rp_ept->cb = cb;
261 rp_ept->priv = priv;
263 metal_list_add_tail(&rdev->rp_endpoints, &rp_ept->node);
265 metal_mutex_release(&rdev->lock);
267 return rp_ept;
268 }
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 }
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 {
305 struct rpmsg_hdr *rp_hdr;
306 struct rpmsg_ns_msg *ns_msg;
307 unsigned short idx;
308 unsigned long len;
310 metal_mutex_acquire(&rdev->lock);
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 }
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;
327 /* Place the buffer on virtqueue. */
328 rpmsg_enqueue_buffer(rdev, rp_hdr, len, idx);
330 /* Notify the other side that it has data to process. */
331 virtqueue_kick(rdev->tvq);
333 metal_mutex_release(&rdev->lock);
334 return RPMSG_SUCCESS;
335 }
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;
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 }
373 return status;
374 }
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;
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 }
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;
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 }
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 {
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 }
468 return data;
469 }
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 }
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;
503 vdev = (struct virtio_device *)vq->vq_dev;
504 rdev = (struct remote_device *)vdev;
506 /* Check if the remote device is master. */
507 if (rdev->role == RPMSG_MASTER) {
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);
521 if (rp_chnl->state == RPMSG_CHNL_STATE_IDLE) {
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 }
534 }
536 }
537 }
538 }
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;
560 vdev = (struct virtio_device *)vq->vq_dev;
561 rdev = (struct remote_device *)vdev;
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 }
582 metal_mutex_acquire(&rdev->lock);
584 /* Process the received data from remote node */
585 rp_hdr = (struct rpmsg_hdr *)rpmsg_get_rx_buffer(rdev, &len, &idx);
587 metal_mutex_release(&rdev->lock);
589 while (rp_hdr) {
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);
596 if (!rp_ept)
597 /* Fatal error no endpoint for the given dst addr. */
598 return;
600 rp_chnl = rp_ept->rp_chnl;
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;
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 }
617 metal_mutex_acquire(&rdev->lock);
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 }
631 rp_hdr =
632 (struct rpmsg_hdr *)rpmsg_get_rx_buffer(rdev, &len, &idx);
633 metal_mutex_release(&rdev->lock);
634 }
635 }
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;
658 (void)server_chnl;
659 (void)src;
661 rdev = (struct remote_device *)priv;
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.
667 ns_msg = (struct rpmsg_ns_msg *)data;
668 ns_msg->name[len - 1] = '\0';
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 }
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;
724 /* Find first available buffer */
725 for (i = 0; i < size; i++) {
726 tmp32 = get_first_zero_bit(bitmap[i]);
728 if (tmp32 < 32) {
729 addr = tmp32 + (i*32);
730 bitmap[i] |= (1 << tmp32);
731 break;
732 }
733 }
735 return addr;
736 }
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;
754 if (addr >= size * 32)
755 return -1;
757 /* Mark the addr as available */
758 i = addr / 32;
759 j = addr % 32;
761 mask = mask << j;
762 bitmap[i] = bitmap[i] & (~mask);
764 return RPMSG_SUCCESS;
765 }
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;
783 if (addr >= size * 32)
784 return -1;
786 /* Mark the id as available */
787 i = addr / 32;
788 j = addr % 32;
789 mask = mask << j;
791 return (bitmap[i] & mask);
792 }
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;
810 if (addr >= size * 32)
811 return -1;
813 /* Mark the id as available */
814 i = addr / 32;
815 j = addr % 32;
816 mask = mask << j;
817 bitmap[i] |= mask;
819 return RPMSG_SUCCESS;
820 }