From 355a7058153e04b53bed3fcb792110294693d386 Mon Sep 17 00:00:00 2001 From: Alexandre Bailon Date: Tue, 31 Mar 2015 09:51:59 +0200 Subject: [PATCH] greybus: Add loopback protocol Add a simple Greybus protocol in order to stress USB and Greybus. This protocol currently support 2 requests: ping and transfer. ping request is useful to measure latency. Kernel send a ping request and firmware should respond with a ping. The transfer request request is useful to stress Greybus and USB. Kernel can send data from 0 to 4k and the firmware must send back the data to kernel. This behaviour of gb-loopback module is controlled via sysfs. Curently, connection sysfs folder is updated with new entries: - type: 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) - size: Size of transfer message payload: 0-4096 bytes - ms_wait: Time to wait between two messages: 0-1024 ms Module also export some statistics about connection: - latency: Time to send and receive one message - frequency: Number of packet sent per second on this cport - throughput: Quantity of data sent and received on this cport - error All this statistics are cleared everytime type, size or ms_wait entries are updated. Signed-off-by: Alexandre Bailon Signed-off-by: Greg Kroah-Hartman --- drivers/staging/greybus/Makefile | 2 + drivers/staging/greybus/greybus_manifest.h | 1 + drivers/staging/greybus/loopback.c | 390 +++++++++++++++++++++++++++++ 3 files changed, 393 insertions(+) create mode 100644 drivers/staging/greybus/loopback.c diff --git a/drivers/staging/greybus/Makefile b/drivers/staging/greybus/Makefile index 6cb08ae544cd..f6ad19a1329b 100644 --- a/drivers/staging/greybus/Makefile +++ b/drivers/staging/greybus/Makefile @@ -22,6 +22,7 @@ gb-phy-y := gpb.o \ # Prefix all modules with gb- gb-vibrator-y := vibrator.o gb-battery-y := battery.o +gb-loopback-y := loopback.o gb-es1-y := es1.o gb-es2-y := es2.o @@ -29,6 +30,7 @@ obj-m += greybus.o obj-m += gb-phy.o obj-m += gb-vibrator.o obj-m += gb-battery.o +obj-m += gb-loopback.o obj-m += gb-es1.o obj-m += gb-es2.o diff --git a/drivers/staging/greybus/greybus_manifest.h b/drivers/staging/greybus/greybus_manifest.h index 398630ce8ac4..4b2cf9220843 100644 --- a/drivers/staging/greybus/greybus_manifest.h +++ b/drivers/staging/greybus/greybus_manifest.h @@ -42,6 +42,7 @@ enum greybus_protocol { GREYBUS_PROTOCOL_SENSOR = 0x0e, GREYBUS_PROTOCOL_LED = 0x0f, GREYBUS_PROTOCOL_VIBRATOR = 0x10, + GREYBUS_PROTOCOL_LOOPBACK = 0x11, /* ... */ GREYBUS_PROTOCOL_VENDOR = 0xff, }; diff --git a/drivers/staging/greybus/loopback.c b/drivers/staging/greybus/loopback.c new file mode 100644 index 000000000000..ae8cc9da8347 --- /dev/null +++ b/drivers/staging/greybus/loopback.c @@ -0,0 +1,390 @@ +/* + * Loopback bridge driver for the Greybus loopback module. + * + * Copyright 2014 Google Inc. + * Copyright 2014 Linaro Ltd. + * + * Released under the GPLv2 only. + */ +#include +#include +#include +#include +#include +#include +#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; + 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; +}; + +/* Version of the Greybus loopback protocol we support */ +#define GB_LOOPBACK_VERSION_MAJOR 0x00 +#define GB_LOOPBACK_VERSION_MINOR 0x01 + +/* Greybus loopback request types */ +#define GB_LOOPBACK_TYPE_INVALID 0x00 +#define GB_LOOPBACK_TYPE_PROTOCOL_VERSION 0x01 +#define GB_LOOPBACK_TYPE_PING 0x02 +#define GB_LOOPBACK_TYPE_TRANSFER 0x03 + +#define GB_LOOPBACK_SIZE_MAX SZ_4K + +/* 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); \ + pr_err("%s = %"#type"\n", #field, 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) +{ + if (gb->ms_wait > 1000) + gb->ms_wait = 1000; + if (gb->type > 3) + gb->type = 0; + if (gb->size > GB_LOOPBACK_SIZE_MAX) + gb->size = GB_LOOPBACK_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-1024 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); + +struct gb_loopback_transfer_request { + __le32 len; + __u8 data[0]; +}; + +struct gb_loopback_transfer_response { + __u8 data[0]; +}; + + +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 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; + + if (elapsed_nsecs >= NSEC_PER_SEC) { + if (!stats->count) + avg = stats->sum * (elapsed_nsecs / NSEC_PER_SEC); + 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_bw_update(struct gb_loopback *gb, int error) +{ + if (!error) + gb->throughput.sum += gb->size * 2; + 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 nsecs; + + nsecs = timeval_to_ns(tlat); + lat = nsecs / NSEC_PER_MSEC; + + 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 == 0) { + msleep(1000); + continue; + } + if (gb->type == 1) + error = gb_loopback_ping(gb, &tlat); + if (gb->type == 2) + error = gb_loopback_transfer(gb, &tlat, gb->size); + if (error) + gb->error++; + if (gb->ts.tv_usec == 0 && gb->ts.tv_sec == 0) { + do_gettimeofday(&gb->ts); + continue; + } + do_gettimeofday(&gb->te); + gb->elapsed_nsecs = timeval_to_ns(&gb->te) - + timeval_to_ns(&gb->ts); + gb_loopback_freq_update(gb); + if (gb->type == 2) + gb_loopback_bw_update(gb, error); + 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_update_group(&connection->dev.kobj, &loopback_group); + if (retval) + goto error; + + /* Check the version */ + retval = get_version(gb); + if (retval) + goto error; + + gb_loopback_reset_stats(gb); + gb->task = kthread_run(gb_loopback_fn, gb, "gb_loopback"); + if (IS_ERR(gb->task)) { + retval = IS_ERR(gb->task); + goto error; + } + + return 0; + +error: + 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_group(&connection->dev.kobj, &loopback_group); + 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 = NULL, /* no incoming requests */ +}; + +gb_protocol_driver(&loopback_protocol); + +MODULE_LICENSE("GPL v2"); -- 2.11.0