rt_benchs/communication_techniques/src/communication/batch_queue.c

/*
 * Copyright (C) 2009-2012 Thomas Preud'homme <thomas.preud-homme@lip6.fr>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#define _BATCH_QUEUE_C_ 1

#include <stdio.h>
#include <stdlib.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdint.h>
#include <limits.h>

/* Non standard include */
#include <commtech.h>
#include <specific_comm.h>

#define BASE_SHM_NAME "/channel"
#define ROUNDUP(size) ((size + page_size) & ~(page_size - 1))


static uintptr_t buf_mask;
static uintptr_t page_mask;
static int nb_buf_entries;
static unsigned int page_size;


void *create_comm_channel(void)
{
	int ret, shm_fd;
	static int chan_idx = 0;
	char shm_name[NAME_MAX];
	struct channel *channel1, *channel2;

	channel1 = NULL;
	channel2 = NULL;
	ret = sysconf(_SC_PAGESIZE);
	if (ret == -1)
	{
		perror("BatchQueue init sysconf");
		return NULL;
	}
	page_size = ret;
	page_mask = ~((uintptr_t) (page_size - 1));
	ret = snprintf(shm_name, NAME_MAX, BASE_SHM_NAME"%d\n", chan_idx);
	if (ret < 0)
	{
		fprintf(stderr, "BatchQueue init snprintf failed\n");
		return NULL;
	} else if (ret >= NAME_MAX)
	{
		fprintf(stderr, "Too many channels created: impossible to ");
		fprintf(stderr, "create a channel named "BASE_SHM_NAME);
		fprintf(stderr, "%d\n", chan_idx);
		return NULL;
	}
	shm_fd = shm_open(shm_name, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR);
	if (shm_fd == -1)
	{
		perror("BatchQueue init shm_open failed");
		return NULL;
	}
	ret = ftruncate(shm_fd, sizeof(*channel1));
	if (ret == -1)
	{
		perror("BatchQueue init ftruncate failed");
		goto close_file;
	}
	channel1 = mmap(NULL, ROUNDUP(sizeof(*channel1)),
		PROT_READ | PROT_WRITE, MAP_PRIVATE, shm_fd, 0);
	channel2 = mmap(NULL, ROUNDUP(sizeof(*channel1)),
		PROT_READ | PROT_WRITE, MAP_PRIVATE, shm_fd, 0);
	if ((channel1 == MAP_FAILED) || (channel2 == MAP_FAILED))
	{
		perror("Batchqueue init mmap failed");
		channel1 = NULL;
		goto close_file;
	}

	channel1->mapping1 = channel1;
	channel1->mapping2 = channel2;

	channel1->state = 0;

	channel1->sender_ptr = channel1->buf[0];
	channel1->receiver_ptr = channel1->buf[0];

	channel1->buf_end1 = channel1->buf[0] +
		(sizeof channel1->buf[0] / sizeof channel1->buf[0][0]);
	channel1->buf_end2 = channel1->mapping2->buf[1] +
		(sizeof channel1->buf[1] / sizeof channel1->buf[1][0]);
	channel1->sender_ptr_end = channel1->buf_end1;

	buf_mask = sizeof channel1->buf[0] - 1;
	nb_buf_entries = sizeof channel1->buf[0] / sizeof channel1->buf[0][0];
close_file:
	shm_unlink(shm_name);
	return channel1;
}

int end_producer(void *unused __attribute__ ((unused)))
{
	return 0;
}

int destroy_comm_channel(void *channel)
{
	size_t mapping_size;
	struct channel *mapping1, *mapping2, *channel_ptr;

	channel_ptr = (struct channel *) channel;
	mapping1 = channel_ptr->mapping1;
	mapping2 = channel_ptr->mapping2;
	mapping_size = ROUNDUP(sizeof(*mapping1));
	if (munmap(mapping1, mapping_size) || munmap(mapping2, mapping_size))
		return -1;
	else
		return 0;
}

/*
 * Copy at max count received data into buf
 * @param buf The buffer in which received data must be copied into
 * @return Number of data received and copied into buf
 *
 * @warning recv_one_data should not be used in conjonction of
 * recv_some_data
 */
void *recv_one_data(struct channel *channel)
{
	void *result;

	if (unlikely(!((uintptr_t) channel->receiver_ptr & buf_mask)))
		while (!channel->state);
	result = *channel->receiver_ptr++;
	if (unlikely(!((uintptr_t) channel->receiver_ptr & buf_mask)))
	{
		uintptr_t rcvr_ptr_high;

		rcvr_ptr_high = (uintptr_t) channel->receiver_ptr & page_mask;
		channel->state = 0;
		if ((void *) rcvr_ptr_high == channel->mapping1)
			channel->receiver_ptr = channel->mapping2->buf[1];
		else
			channel->receiver_ptr = channel->mapping1->buf[0];
	}
	return result;
}

/*
 * Copy at max count received data into buf
 * @param buf The buffer in which received data must be copied into
 * @return Number of data received and copied into buf
 *
 * @warning recv_some_data should not be used in conjonction of
 * recv_one_data
 * @warning count must be a multiple of BUF_SIZE
 */
ssize_t recv_some_data(struct channel *channel, void **buf, size_t count)
{
	size_t nb_read;

	nb_read = 0;
	while (channel->state && nb_read < count)
	{
		void * volatile *chanbuf_ptr;
		uintptr_t rcvr_ptr_high;

		chanbuf_ptr = channel->receiver_ptr;
		channel->receiver_ptr += nb_buf_entries;
		rcvr_ptr_high = (uintptr_t) channel->receiver_ptr & page_mask;
		for(; chanbuf_ptr != channel->receiver_ptr; chanbuf_ptr++)
			*buf++ = *chanbuf_ptr;
		nb_read += nb_buf_entries;
		channel->state = 0;
		if ((void *) rcvr_ptr_high == channel->mapping1)
			channel->receiver_ptr = channel->mapping2->buf[1];
		else
			channel->receiver_ptr = channel->mapping1->buf[0];
	}
	return nb_read;
}