[linux-2.6-block.git] / drivers / staging / greybus / loopback.c

/*
 * Loopback bridge driver for the Greybus loopback module.
 *
 * Copyright 2014 Google Inc.
 * Copyright 2014 Linaro Ltd.
 *
 * Released under the GPLv2 only.
 */
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/kthread.h>
#include <linux/delay.h>
#include <linux/random.h>
#include <linux/sizes.h>
#include <asm/div64.h>

#include "greybus.h"

struct gb_loopback_stats {
	u32 min;
	u32 max;
	u32 avg;
	u32 sum;
	u32 count;
};

struct gb_loopback {
	struct gb_connection *connection;
	u8 version_major;
	u8 version_minor;

	struct task_struct *task;

	int type;
	u32 size;
	size_t size_max;
	int ms_wait;

	struct gb_loopback_stats latency;
	struct gb_loopback_stats throughput;
	struct gb_loopback_stats frequency;
	struct timeval ts;
	struct timeval te;
	u64 elapsed_nsecs;
	u32 error;
};

#define GB_LOOPBACK_MS_WAIT_MAX				1000

/* Define get_version() routine */
define_get_version(gb_loopback, LOOPBACK);

/* interface sysfs attributes */
#define gb_loopback_ro_attr(field, type)				\
static ssize_t field##_show(struct device *dev,				\
			    struct device_attribute *attr,		\
			    char *buf)					\
{									\
	struct gb_connection *connection = to_gb_connection(dev);	\
	struct gb_loopback *gb =					\
		(struct gb_loopback *)connection->private;		\
	return sprintf(buf, "%"#type"\n", gb->field);			\
}									\
static DEVICE_ATTR_RO(field)

#define gb_loopback_ro_stats_attr(name, field, type)			\
static ssize_t name##_##field##_show(struct device *dev,		\
			    struct device_attribute *attr,		\
			    char *buf)					\
{									\
	struct gb_connection *connection = to_gb_connection(dev);	\
	struct gb_loopback *gb =					\
		(struct gb_loopback *)connection->private;		\
	return sprintf(buf, "%"#type"\n", gb->name.field);		\
}									\
static DEVICE_ATTR_RO(name##_##field)

#define gb_loopback_stats_attrs(field)					\
	gb_loopback_ro_stats_attr(field, min, d);			\
	gb_loopback_ro_stats_attr(field, max, d);			\
	gb_loopback_ro_stats_attr(field, avg, d);

#define gb_loopback_attr(field, type)					\
static ssize_t field##_show(struct device *dev,				\
			    struct device_attribute *attr,		\
			    char *buf)					\
{									\
	struct gb_connection *connection = to_gb_connection(dev);	\
	struct gb_loopback *gb =					\
		(struct gb_loopback *)connection->private;		\
	return sprintf(buf, "%"#type"\n", gb->field);			\
}									\
static ssize_t field##_store(struct device *dev,			\
			    struct device_attribute *attr,		\
			    const char *buf,				\
			    size_t len)					\
{									\
	int ret;							\
	struct gb_connection *connection = to_gb_connection(dev);	\
	struct gb_loopback *gb =					\
		(struct gb_loopback *)connection->private;		\
	ret = sscanf(buf, "%"#type, &gb->field);			\
	if (ret != 1)							\
		return -EINVAL;						\
	gb_loopback_check_attr(gb);					\
	return len;							\
}									\
static DEVICE_ATTR_RW(field)

static void gb_loopback_reset_stats(struct gb_loopback *gb);
static void gb_loopback_check_attr(struct gb_loopback *gb)
{
	switch (gb->type) {
	case GB_LOOPBACK_TYPE_PING:
	case GB_LOOPBACK_TYPE_TRANSFER:
	case GB_LOOPBACK_TYPE_SINK:
		break;
	default:
		gb->type = 0;
		break;
	}
	if (gb->ms_wait > GB_LOOPBACK_MS_WAIT_MAX)
		gb->ms_wait = GB_LOOPBACK_MS_WAIT_MAX;
	if (gb->size > gb->size_max)
		gb->size = gb->size_max;
	gb->error = 0;
	gb_loopback_reset_stats(gb);
}

/* Time to send and receive one message */
gb_loopback_stats_attrs(latency);
/* Number of packet sent per second on this cport */
gb_loopback_stats_attrs(frequency);
/* Quantity of data sent and received on this cport */
gb_loopback_stats_attrs(throughput);
gb_loopback_ro_attr(error, d);

/*
 * Type of loopback message to send
 * 0 => Don't send message
 * 1 => Send ping message continuously (message without payload)
 * 2 => Send transer message continuously (message with payload)
 */
gb_loopback_attr(type, d);
/* Size of transfer message payload: 0-4096 bytes */
gb_loopback_attr(size, u);
/* Time to wait between two messages: 0-1000 ms */
gb_loopback_attr(ms_wait, d);

#define dev_stats_attrs(name)						\
	&dev_attr_##name##_min.attr,					\
	&dev_attr_##name##_max.attr,					\
	&dev_attr_##name##_avg.attr

static struct attribute *loopback_attrs[] = {
	dev_stats_attrs(latency),
	dev_stats_attrs(frequency),
	dev_stats_attrs(throughput),
	&dev_attr_type.attr,
	&dev_attr_size.attr,
	&dev_attr_ms_wait.attr,
	&dev_attr_error.attr,
	NULL,
};
ATTRIBUTE_GROUPS(loopback);

static int gb_loopback_sink(struct gb_loopback *gb,
				struct timeval *tping, u32 len)
{
	struct timeval ts, te;
	u64 elapsed_nsecs;
	struct gb_loopback_transfer_request *request;
	int retval;

	request = kmalloc(len + sizeof(*request), GFP_KERNEL);
	if (!request)
		return -ENOMEM;

	request->len = cpu_to_le32(len);

	do_gettimeofday(&ts);
	retval = gb_operation_sync(gb->connection, GB_LOOPBACK_TYPE_SINK,
				   request, len + sizeof(*request), NULL, 0);

	do_gettimeofday(&te);
	elapsed_nsecs = timeval_to_ns(&te) - timeval_to_ns(&ts);
	*tping = ns_to_timeval(elapsed_nsecs);

	kfree(request);
	return retval;
}

static int gb_loopback_transfer(struct gb_loopback *gb,
				struct timeval *tping, u32 len)
{
	struct timeval ts, te;
	u64 elapsed_nsecs;
	struct gb_loopback_transfer_request *request;
	struct gb_loopback_transfer_response *response;
	int retval;

	request = kmalloc(len + sizeof(*request), GFP_KERNEL);
	if (!request)
		return -ENOMEM;
	response = kmalloc(len + sizeof(*response), GFP_KERNEL);
	if (!response) {
		kfree(request);
		return -ENOMEM;
	}

	request->len = cpu_to_le32(len);

	do_gettimeofday(&ts);
	retval = gb_operation_sync(gb->connection, GB_LOOPBACK_TYPE_TRANSFER,
				   request, len + sizeof(*request),
				   response, len + sizeof(*response));
	do_gettimeofday(&te);
	elapsed_nsecs = timeval_to_ns(&te) - timeval_to_ns(&ts);
	*tping = ns_to_timeval(elapsed_nsecs);

	if (retval)
		goto gb_error;

	if (memcmp(request->data, response->data, len))
		retval = -EREMOTEIO;

gb_error:
	kfree(request);
	kfree(response);

	return retval;
}

static int gb_loopback_ping(struct gb_loopback *gb, struct timeval *tping)
{
	struct timeval ts, te;
	u64 elapsed_nsecs;
	int retval;

	do_gettimeofday(&ts);
	retval = gb_operation_sync(gb->connection, GB_LOOPBACK_TYPE_PING,
				   NULL, 0, NULL, 0);
	do_gettimeofday(&te);
	elapsed_nsecs = timeval_to_ns(&te) - timeval_to_ns(&ts);
	*tping = ns_to_timeval(elapsed_nsecs);

	return retval;
}

static int gb_loopback_request_recv(u8 type, struct gb_operation *operation)
{
	struct gb_connection *connection = operation->connection;
	struct gb_loopback *gb = connection->private;
	struct gb_loopback_transfer_request *request;
	struct gb_loopback_transfer_response *response;
	size_t len;

	/* By convention, the AP initiates the version operation */
	switch (type) {
	case GB_LOOPBACK_TYPE_PROTOCOL_VERSION:
		dev_err(&connection->dev,
			"module-initiated version operation\n");
		return -EINVAL;
	case GB_LOOPBACK_TYPE_PING:
	case GB_LOOPBACK_TYPE_SINK:
		return 0;
	case GB_LOOPBACK_TYPE_TRANSFER:
		if (operation->request->payload_size < sizeof(*request)) {
			dev_err(&connection->dev,
				"transfer request too small (%zu < %zu)\n",
				operation->request->payload_size,
				sizeof(*request));
			return -EINVAL;	/* -EMSGSIZE */
		}
		request = operation->request->payload;
		len = le32_to_cpu(request->len);
		if (len > gb->size_max) {
			dev_err(&connection->dev,
				"transfer request too large (%zu > %zu)\n",
				len, gb->size_max);
			return -EINVAL;
		}

		if (len) {
			if (!gb_operation_response_alloc(operation, len,
							 GFP_KERNEL)) {
				dev_err(&connection->dev,
					"error allocating response\n");
				return -ENOMEM;
			}
			response = operation->response->payload;
			memcpy(response->data, request->data, len);
		}
		return 0;
	default:
		dev_err(&connection->dev,
			"unsupported request: %hhu\n", type);
		return -EINVAL;
	}
}

static void gb_loopback_reset_stats(struct gb_loopback *gb)
{
	struct gb_loopback_stats reset = {
		.min = 0xffffffff,
	};
	memcpy(&gb->latency, &reset, sizeof(struct gb_loopback_stats));
	memcpy(&gb->throughput, &reset, sizeof(struct gb_loopback_stats));
	memcpy(&gb->frequency, &reset, sizeof(struct gb_loopback_stats));
	memset(&gb->ts, 0, sizeof(struct timeval));
}

static void gb_loopback_update_stats(struct gb_loopback_stats *stats,
					u64 elapsed_nsecs)
{
	u32 avg;
	u64 tmp;

	if (elapsed_nsecs >= NSEC_PER_SEC) {
		if (!stats->count) {
			tmp = elapsed_nsecs;
			do_div(tmp, NSEC_PER_SEC);
			avg = stats->sum * tmp;
		} else {
			avg = stats->sum / stats->count;
		}
		if (stats->min > avg)
			stats->min = avg;
		if (stats->max < avg)
			stats->max = avg;
		stats->avg = avg;
		stats->count = 0;
		stats->sum = 0;
	}
}

static void gb_loopback_freq_update(struct gb_loopback *gb)
{
	gb->frequency.sum++;
	gb_loopback_update_stats(&gb->frequency, gb->elapsed_nsecs);
}

static void gb_loopback_throughput_update(struct gb_loopback *gb)
{
	u32 aggregate_size = sizeof(struct gb_operation_msg_hdr) * 2;

	switch (gb->type) {
	case GB_LOOPBACK_TYPE_PING:
		break;
	case GB_LOOPBACK_TYPE_SINK:
		aggregate_size += sizeof(struct gb_loopback_transfer_request) +
				  gb->size;
		break;
	case GB_LOOPBACK_TYPE_TRANSFER:
		aggregate_size += sizeof(struct gb_loopback_transfer_request) +
				  sizeof(struct gb_loopback_transfer_response) +
				  gb->size * 2;
		break;
	default:
		return;
	}
	gb->throughput.sum += aggregate_size;
	gb_loopback_update_stats(&gb->throughput, gb->elapsed_nsecs);
}

static void gb_loopback_latency_update(struct gb_loopback *gb,
					struct timeval *tlat)
{
	u32 lat;
	u64 tmp;

	tmp = timeval_to_ns(tlat);
	do_div(tmp, NSEC_PER_MSEC);
	lat = tmp;

	if (gb->latency.min > lat)
		gb->latency.min = lat;
	if (gb->latency.max < lat)
		gb->latency.max = lat;
	gb->latency.sum += lat;
	gb->latency.count++;
	gb_loopback_update_stats(&gb->latency, gb->elapsed_nsecs);
}

static int gb_loopback_fn(void *data)
{
	int error = 0;
	struct timeval tlat = {0, 0};
	struct gb_loopback *gb = (struct gb_loopback *)data;

	while (!kthread_should_stop()) {
		if (!gb->type) {
			msleep(1000);
			continue;
		}
		if (gb->ts.tv_usec == 0 && gb->ts.tv_sec == 0)
			do_gettimeofday(&gb->ts);
		if (gb->type == GB_LOOPBACK_TYPE_PING)
			error = gb_loopback_ping(gb, &tlat);
		else if (gb->type == GB_LOOPBACK_TYPE_TRANSFER)
			error = gb_loopback_transfer(gb, &tlat, gb->size);
		else if (gb->type == GB_LOOPBACK_TYPE_SINK)
			error = gb_loopback_sink(gb, &tlat, gb->size);
		if (error)
			gb->error++;
		do_gettimeofday(&gb->te);
		gb->elapsed_nsecs = timeval_to_ns(&gb->te) -
					timeval_to_ns(&gb->ts);
		gb_loopback_freq_update(gb);
		gb_loopback_throughput_update(gb);
		gb_loopback_latency_update(gb, &tlat);
		if (gb->elapsed_nsecs >= NSEC_PER_SEC)
			gb->ts = gb->te;
		if (gb->ms_wait)
			msleep(gb->ms_wait);

	}
	return 0;
}

static int gb_loopback_connection_init(struct gb_connection *connection)
{
	struct gb_loopback *gb;
	int retval;

	gb = kzalloc(sizeof(*gb), GFP_KERNEL);
	if (!gb)
		return -ENOMEM;

	gb->connection = connection;
	connection->private = gb;
	retval = sysfs_create_groups(&connection->dev.kobj, loopback_groups);
	if (retval)
		goto out_free;

	/* Check the version */
	retval = get_version(gb);
	if (retval)
		goto out_get_ver;

	/* Calculate maximum payload */
	gb->size_max = gb_operation_get_payload_size_max(connection);
	if (gb->size_max <= sizeof(struct gb_loopback_transfer_request)) {
		retval = -EINVAL;
		goto out_get_ver;
	}
	gb->size_max -= sizeof(struct gb_loopback_transfer_request);

	gb_loopback_reset_stats(gb);
	gb->task = kthread_run(gb_loopback_fn, gb, "gb_loopback");
	if (IS_ERR(gb->task)) {
		retval = PTR_ERR(gb->task);
		goto out_get_ver;
	}

	return 0;

out_get_ver:
	sysfs_remove_groups(&connection->dev.kobj, loopback_groups);
out_free:
	kfree(gb);
	return retval;
}

static void gb_loopback_connection_exit(struct gb_connection *connection)
{
	struct gb_loopback *gb = connection->private;

	if (!IS_ERR_OR_NULL(gb->task))
		kthread_stop(gb->task);
	sysfs_remove_groups(&connection->dev.kobj, loopback_groups);
	kfree(gb);
}

static struct gb_protocol loopback_protocol = {
	.name			= "loopback",
	.id			= GREYBUS_PROTOCOL_LOOPBACK,
	.major			= GB_LOOPBACK_VERSION_MAJOR,
	.minor			= GB_LOOPBACK_VERSION_MINOR,
	.connection_init	= gb_loopback_connection_init,
	.connection_exit	= gb_loopback_connection_exit,
	.request_recv		= gb_loopback_request_recv,
};

gb_protocol_driver(&loopback_protocol);

MODULE_LICENSE("GPL v2");
Commit	Line	Data
355a7058 AB	1	/*
	2	* Loopback bridge driver for the Greybus loopback module.
	3	*
	4	* Copyright 2014 Google Inc.
	5	* Copyright 2014 Linaro Ltd.
	6	*
	7	* Released under the GPLv2 only.
	8	*/
	9	#include <linux/kernel.h>
	10	#include <linux/module.h>
	11	#include <linux/slab.h>
	12	#include <linux/kthread.h>
	13	#include <linux/delay.h>
	14	#include <linux/random.h>
5679f783	15	#include <linux/sizes.h>
1ffc12be JH	16	#include <asm/div64.h>
1ffc12be JH	17
355a7058 AB	18	#include "greybus.h"
	19
	20	struct gb_loopback_stats {
	21	u32 min;
	22	u32 max;
	23	u32 avg;
	24	u32 sum;
	25	u32 count;
	26	};
	27
	28	struct gb_loopback {
	29	struct gb_connection *connection;
	30	u8 version_major;
	31	u8 version_minor;
	32
	33	struct task_struct *task;
	34
	35	int type;
	36	u32 size;
e51eafeb	37	size_t size_max;
355a7058 AB	38	int ms_wait;
	39
	40	struct gb_loopback_stats latency;
	41	struct gb_loopback_stats throughput;
	42	struct gb_loopback_stats frequency;
	43	struct timeval ts;
	44	struct timeval te;
	45	u64 elapsed_nsecs;
	46	u32 error;
	47	};
	48
91262c3a	49	#define GB_LOOPBACK_MS_WAIT_MAX 1000
355a7058 AB	50
	51	/* Define get_version() routine */
	52	define_get_version(gb_loopback, LOOPBACK);
	53
	54	/* interface sysfs attributes */
	55	#define gb_loopback_ro_attr(field, type) \
	56	static ssize_t field##_show(struct device *dev, \
	57	struct device_attribute *attr, \
	58	char *buf) \
	59	{ \
	60	struct gb_connection *connection = to_gb_connection(dev); \
	61	struct gb_loopback *gb = \
	62	(struct gb_loopback *)connection->private; \
	63	return sprintf(buf, "%"#type"\n", gb->field); \
	64	} \
	65	static DEVICE_ATTR_RO(field)
	66
	67	#define gb_loopback_ro_stats_attr(name, field, type) \
	68	static ssize_t name##_##field##_show(struct device *dev, \
	69	struct device_attribute *attr, \
	70	char *buf) \
	71	{ \
	72	struct gb_connection *connection = to_gb_connection(dev); \
	73	struct gb_loopback *gb = \
	74	(struct gb_loopback *)connection->private; \
	75	return sprintf(buf, "%"#type"\n", gb->name.field); \
	76	} \
	77	static DEVICE_ATTR_RO(name##_##field)
	78
	79	#define gb_loopback_stats_attrs(field) \
	80	gb_loopback_ro_stats_attr(field, min, d); \
	81	gb_loopback_ro_stats_attr(field, max, d); \
	82	gb_loopback_ro_stats_attr(field, avg, d);
	83
	84	#define gb_loopback_attr(field, type) \
	85	static ssize_t field##_show(struct device *dev, \
	86	struct device_attribute *attr, \
	87	char *buf) \
	88	{ \
	89	struct gb_connection *connection = to_gb_connection(dev); \
	90	struct gb_loopback *gb = \
	91	(struct gb_loopback *)connection->private; \
	92	return sprintf(buf, "%"#type"\n", gb->field); \
	93	} \
	94	static ssize_t field##_store(struct device *dev, \
	95	struct device_attribute *attr, \
	96	const char *buf, \
	97	size_t len) \
	98	{ \
	99	int ret; \
	100	struct gb_connection *connection = to_gb_connection(dev); \
	101	struct gb_loopback *gb = \
	102	(struct gb_loopback *)connection->private; \
	103	ret = sscanf(buf, "%"#type, &gb->field); \
355a7058 AB	104	if (ret != 1) \
	105	return -EINVAL; \
	106	gb_loopback_check_attr(gb); \
	107	return len; \
	108	} \
	109	static DEVICE_ATTR_RW(field)
	110
	111	static void gb_loopback_reset_stats(struct gb_loopback *gb);
	112	static void gb_loopback_check_attr(struct gb_loopback *gb)
	113	{
a598f438 BD	114	switch (gb->type) {
	115	case GB_LOOPBACK_TYPE_PING:
	116	case GB_LOOPBACK_TYPE_TRANSFER:
384a7a3c	117	case GB_LOOPBACK_TYPE_SINK:
a598f438 BD	118	break;
	119	default:
	120	gb->type = 0;
	121	break;
	122	}
91262c3a AE	123	if (gb->ms_wait > GB_LOOPBACK_MS_WAIT_MAX)
91262c3a AE	124	gb->ms_wait = GB_LOOPBACK_MS_WAIT_MAX;
c3bba87a BD	125	if (gb->size > gb->size_max)
c3bba87a BD	126	gb->size = gb->size_max;
355a7058 AB	127	gb->error = 0;
	128	gb_loopback_reset_stats(gb);
	129	}
	130
	131	/* Time to send and receive one message */
	132	gb_loopback_stats_attrs(latency);
	133	/* Number of packet sent per second on this cport */
	134	gb_loopback_stats_attrs(frequency);
	135	/* Quantity of data sent and received on this cport */
	136	gb_loopback_stats_attrs(throughput);
	137	gb_loopback_ro_attr(error, d);
	138
	139	/*
	140	* Type of loopback message to send
	141	* 0 => Don't send message
	142	* 1 => Send ping message continuously (message without payload)
	143	* 2 => Send transer message continuously (message with payload)
	144	*/
	145	gb_loopback_attr(type, d);
	146	/* Size of transfer message payload: 0-4096 bytes */
	147	gb_loopback_attr(size, u);
48f19ee8	148	/* Time to wait between two messages: 0-1000 ms */
355a7058 AB	149	gb_loopback_attr(ms_wait, d);
	150
	151	#define dev_stats_attrs(name) \
	152	&dev_attr_##name##_min.attr, \
	153	&dev_attr_##name##_max.attr, \
	154	&dev_attr_##name##_avg.attr
	155
	156	static struct attribute *loopback_attrs[] = {
	157	dev_stats_attrs(latency),
	158	dev_stats_attrs(frequency),
	159	dev_stats_attrs(throughput),
	160	&dev_attr_type.attr,
	161	&dev_attr_size.attr,
	162	&dev_attr_ms_wait.attr,
	163	&dev_attr_error.attr,
	164	NULL,
	165	};
	166	ATTRIBUTE_GROUPS(loopback);
	167
384a7a3c BD	168	static int gb_loopback_sink(struct gb_loopback *gb,
	169	struct timeval *tping, u32 len)
	170	{
	171	struct timeval ts, te;
	172	u64 elapsed_nsecs;
	173	struct gb_loopback_transfer_request *request;
	174	int retval;
	175
	176	request = kmalloc(len + sizeof(*request), GFP_KERNEL);
	177	if (!request)
	178	return -ENOMEM;
	179
	180	request->len = cpu_to_le32(len);
	181
	182	do_gettimeofday(&ts);
	183	retval = gb_operation_sync(gb->connection, GB_LOOPBACK_TYPE_SINK,
	184	request, len + sizeof(*request), NULL, 0);
c3bba87a	185
384a7a3c BD	186	do_gettimeofday(&te);
	187	elapsed_nsecs = timeval_to_ns(&te) - timeval_to_ns(&ts);
	188	*tping = ns_to_timeval(elapsed_nsecs);
	189
	190	kfree(request);
	191	return retval;
	192	}
	193
355a7058 AB	194	static int gb_loopback_transfer(struct gb_loopback *gb,
	195	struct timeval *tping, u32 len)
	196	{
	197	struct timeval ts, te;
	198	u64 elapsed_nsecs;
	199	struct gb_loopback_transfer_request *request;
	200	struct gb_loopback_transfer_response *response;
	201	int retval;
	202
	203	request = kmalloc(len + sizeof(*request), GFP_KERNEL);
	204	if (!request)
	205	return -ENOMEM;
	206	response = kmalloc(len + sizeof(*response), GFP_KERNEL);
	207	if (!response) {
	208	kfree(request);
	209	return -ENOMEM;
	210	}
	211
	212	request->len = cpu_to_le32(len);
	213
	214	do_gettimeofday(&ts);
	215	retval = gb_operation_sync(gb->connection, GB_LOOPBACK_TYPE_TRANSFER,
	216	request, len + sizeof(*request),
	217	response, len + sizeof(*response));
	218	do_gettimeofday(&te);
	219	elapsed_nsecs = timeval_to_ns(&te) - timeval_to_ns(&ts);
	220	*tping = ns_to_timeval(elapsed_nsecs);
	221
	222	if (retval)
	223	goto gb_error;
	224
	225	if (memcmp(request->data, response->data, len))
	226	retval = -EREMOTEIO;
	227
	228	gb_error:
	229	kfree(request);
	230	kfree(response);
	231
	232	return retval;
	233	}
	234
	235	static int gb_loopback_ping(struct gb_loopback gb, struct timeval tping)
	236	{
	237	struct timeval ts, te;
	238	u64 elapsed_nsecs;
	239	int retval;
	240
	241	do_gettimeofday(&ts);
	242	retval = gb_operation_sync(gb->connection, GB_LOOPBACK_TYPE_PING,
	243	NULL, 0, NULL, 0);
	244	do_gettimeofday(&te);
	245	elapsed_nsecs = timeval_to_ns(&te) - timeval_to_ns(&ts);
	246	*tping = ns_to_timeval(elapsed_nsecs);
	247
	248	return retval;
	249	}
	250
ac1c2840 AE	251	static int gb_loopback_request_recv(u8 type, struct gb_operation *operation)
	252	{
	253	struct gb_connection *connection = operation->connection;
c3bba87a	254	struct gb_loopback *gb = connection->private;
ac1c2840 AE	255	struct gb_loopback_transfer_request *request;
ac1c2840 AE	256	struct gb_loopback_transfer_response *response;
e51eafeb	257	size_t len;
ac1c2840 AE	258
	259	/* By convention, the AP initiates the version operation */
	260	switch (type) {
	261	case GB_LOOPBACK_TYPE_PROTOCOL_VERSION:
	262	dev_err(&connection->dev,
	263	"module-initiated version operation\n");
	264	return -EINVAL;
	265	case GB_LOOPBACK_TYPE_PING:
384a7a3c	266	case GB_LOOPBACK_TYPE_SINK:
ac1c2840 AE	267	return 0;
	268	case GB_LOOPBACK_TYPE_TRANSFER:
	269	if (operation->request->payload_size < sizeof(*request)) {
	270	dev_err(&connection->dev,
	271	"transfer request too small (%zu < %zu)\n",
	272	operation->request->payload_size,
	273	sizeof(*request));
	274	return -EINVAL; /* -EMSGSIZE */
	275	}
	276	request = operation->request->payload;
	277	len = le32_to_cpu(request->len);
c3bba87a BD	278	if (len > gb->size_max) {
	279	dev_err(&connection->dev,
	280	"transfer request too large (%zu > %zu)\n",
	281	len, gb->size_max);
	282	return -EINVAL;
	283	}
	284
ac1c2840	285	if (len) {
1c7658cf JH	286	if (!gb_operation_response_alloc(operation, len,
1c7658cf JH	287	GFP_KERNEL)) {
ac1c2840 AE	288	dev_err(&connection->dev,
	289	"error allocating response\n");
	290	return -ENOMEM;
	291	}
	292	response = operation->response->payload;
	293	memcpy(response->data, request->data, len);
	294	}
	295	return 0;
	296	default:
	297	dev_err(&connection->dev,
	298	"unsupported request: %hhu\n", type);
	299	return -EINVAL;
	300	}
	301	}
	302
355a7058 AB	303	static void gb_loopback_reset_stats(struct gb_loopback *gb)
	304	{
	305	struct gb_loopback_stats reset = {
	306	.min = 0xffffffff,
	307	};
	308	memcpy(&gb->latency, &reset, sizeof(struct gb_loopback_stats));
	309	memcpy(&gb->throughput, &reset, sizeof(struct gb_loopback_stats));
	310	memcpy(&gb->frequency, &reset, sizeof(struct gb_loopback_stats));
	311	memset(&gb->ts, 0, sizeof(struct timeval));
	312	}
	313
	314	static void gb_loopback_update_stats(struct gb_loopback_stats *stats,
	315	u64 elapsed_nsecs)
	316	{
	317	u32 avg;
1ffc12be	318	u64 tmp;
355a7058 AB	319
355a7058 AB	320	if (elapsed_nsecs >= NSEC_PER_SEC) {
1ffc12be JH	321	if (!stats->count) {
	322	tmp = elapsed_nsecs;
	323	do_div(tmp, NSEC_PER_SEC);
	324	avg = stats->sum * tmp;
	325	} else {
355a7058	326	avg = stats->sum / stats->count;
1ffc12be	327	}
355a7058 AB	328	if (stats->min > avg)
	329	stats->min = avg;
	330	if (stats->max < avg)
	331	stats->max = avg;
	332	stats->avg = avg;
	333	stats->count = 0;
	334	stats->sum = 0;
	335	}
	336	}
	337
	338	static void gb_loopback_freq_update(struct gb_loopback *gb)
	339	{
	340	gb->frequency.sum++;
	341	gb_loopback_update_stats(&gb->frequency, gb->elapsed_nsecs);
	342	}
	343
f7908e4d	344	static void gb_loopback_throughput_update(struct gb_loopback *gb)
355a7058	345	{
f7908e4d BD	346	u32 aggregate_size = sizeof(struct gb_operation_msg_hdr) * 2;
	347
	348	switch (gb->type) {
	349	case GB_LOOPBACK_TYPE_PING:
	350	break;
	351	case GB_LOOPBACK_TYPE_SINK:
	352	aggregate_size += sizeof(struct gb_loopback_transfer_request) +
	353	gb->size;
	354	break;
	355	case GB_LOOPBACK_TYPE_TRANSFER:
	356	aggregate_size += sizeof(struct gb_loopback_transfer_request) +
	357	sizeof(struct gb_loopback_transfer_response) +
	358	gb->size * 2;
	359	break;
	360	default:
	361	return;
	362	}
	363	gb->throughput.sum += aggregate_size;
355a7058 AB	364	gb_loopback_update_stats(&gb->throughput, gb->elapsed_nsecs);
	365	}
	366
	367	static void gb_loopback_latency_update(struct gb_loopback *gb,
	368	struct timeval *tlat)
	369	{
	370	u32 lat;
1ffc12be	371	u64 tmp;
355a7058	372
1ffc12be JH	373	tmp = timeval_to_ns(tlat);
	374	do_div(tmp, NSEC_PER_MSEC);
	375	lat = tmp;
355a7058 AB	376
	377	if (gb->latency.min > lat)
	378	gb->latency.min = lat;
	379	if (gb->latency.max < lat)
	380	gb->latency.max = lat;
	381	gb->latency.sum += lat;
	382	gb->latency.count++;
	383	gb_loopback_update_stats(&gb->latency, gb->elapsed_nsecs);
	384	}
	385
	386	static int gb_loopback_fn(void *data)
	387	{
	388	int error = 0;
	389	struct timeval tlat = {0, 0};
	390	struct gb_loopback gb = (struct gb_loopback )data;
	391
	392	while (!kthread_should_stop()) {
a598f438	393	if (!gb->type) {
355a7058 AB	394	msleep(1000);
	395	continue;
	396	}
3156be7d BD	397	if (gb->ts.tv_usec == 0 && gb->ts.tv_sec == 0)
3156be7d BD	398	do_gettimeofday(&gb->ts);
a598f438	399	if (gb->type == GB_LOOPBACK_TYPE_PING)
355a7058	400	error = gb_loopback_ping(gb, &tlat);
a598f438	401	else if (gb->type == GB_LOOPBACK_TYPE_TRANSFER)
355a7058	402	error = gb_loopback_transfer(gb, &tlat, gb->size);
384a7a3c BD	403	else if (gb->type == GB_LOOPBACK_TYPE_SINK)
384a7a3c BD	404	error = gb_loopback_sink(gb, &tlat, gb->size);
355a7058 AB	405	if (error)
355a7058 AB	406	gb->error++;
355a7058 AB	407	do_gettimeofday(&gb->te);
	408	gb->elapsed_nsecs = timeval_to_ns(&gb->te) -
	409	timeval_to_ns(&gb->ts);
	410	gb_loopback_freq_update(gb);
f7908e4d	411	gb_loopback_throughput_update(gb);
355a7058 AB	412	gb_loopback_latency_update(gb, &tlat);
	413	if (gb->elapsed_nsecs >= NSEC_PER_SEC)
	414	gb->ts = gb->te;
	415	if (gb->ms_wait)
	416	msleep(gb->ms_wait);
	417
	418	}
	419	return 0;
	420	}
	421
	422	static int gb_loopback_connection_init(struct gb_connection *connection)
	423	{
	424	struct gb_loopback *gb;
	425	int retval;
	426
	427	gb = kzalloc(sizeof(*gb), GFP_KERNEL);
	428	if (!gb)
	429	return -ENOMEM;
	430
	431	gb->connection = connection;
	432	connection->private = gb;
7a51b936	433	retval = sysfs_create_groups(&connection->dev.kobj, loopback_groups);
355a7058	434	if (retval)
6f8528e0	435	goto out_free;
355a7058 AB	436
	437	/* Check the version */
	438	retval = get_version(gb);
	439	if (retval)
6f8528e0	440	goto out_get_ver;
355a7058	441
c3bba87a BD	442	/* Calculate maximum payload */
	443	gb->size_max = gb_operation_get_payload_size_max(connection);
	444	if (gb->size_max <= sizeof(struct gb_loopback_transfer_request)) {
	445	retval = -EINVAL;
	446	goto out_get_ver;
	447	}
	448	gb->size_max -= sizeof(struct gb_loopback_transfer_request);
	449
355a7058 AB	450	gb_loopback_reset_stats(gb);
	451	gb->task = kthread_run(gb_loopback_fn, gb, "gb_loopback");
	452	if (IS_ERR(gb->task)) {
69f60347	453	retval = PTR_ERR(gb->task);
6f8528e0	454	goto out_get_ver;
355a7058 AB	455	}
	456
	457	return 0;
	458
6f8528e0 PT	459	out_get_ver:
	460	sysfs_remove_groups(&connection->dev.kobj, loopback_groups);
	461	out_free:
355a7058 AB	462	kfree(gb);
	463	return retval;
	464	}
	465
	466	static void gb_loopback_connection_exit(struct gb_connection *connection)
	467	{
	468	struct gb_loopback *gb = connection->private;
	469
	470	if (!IS_ERR_OR_NULL(gb->task))
	471	kthread_stop(gb->task);
7a51b936	472	sysfs_remove_groups(&connection->dev.kobj, loopback_groups);
355a7058 AB	473	kfree(gb);
	474	}
	475
	476	static struct gb_protocol loopback_protocol = {
	477	.name = "loopback",
	478	.id = GREYBUS_PROTOCOL_LOOPBACK,
	479	.major = GB_LOOPBACK_VERSION_MAJOR,
	480	.minor = GB_LOOPBACK_VERSION_MINOR,
	481	.connection_init = gb_loopback_connection_init,
	482	.connection_exit = gb_loopback_connection_exit,
ac1c2840	483	.request_recv = gb_loopback_request_recv,
355a7058 AB	484	};
	485
	486	gb_protocol_driver(&loopback_protocol);
	487
	488	MODULE_LICENSE("GPL v2");