drivers/staging/greybus/loopback.c

   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>
  15 #include <linux/sizes.h>
  16 #include <asm/div64.h>
  17
  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;
  37         int ms_wait;
  38
  39         struct gb_loopback_stats latency;
  40         struct gb_loopback_stats throughput;
  41         struct gb_loopback_stats frequency;
  42         struct timeval ts;
  43         struct timeval te;
  44         u64 elapsed_nsecs;
  45         u32 error;
  46 };
  47
  48 #define GB_LOOPBACK_MS_WAIT_MAX                         1000
  49 #define GB_LOOPBACK_SIZE_MAX                            SZ_4K
  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);                        \
 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 {
 114         switch (gb->type) {
 115         case GB_LOOPBACK_TYPE_PING:
 116         case GB_LOOPBACK_TYPE_TRANSFER:
 117                 break;
 118         default:
 119                 gb->type = 0;
 120                 break;
 121         }
 122         if (gb->ms_wait > GB_LOOPBACK_MS_WAIT_MAX)
 123                 gb->ms_wait = GB_LOOPBACK_MS_WAIT_MAX;
 124         if (gb->size > GB_LOOPBACK_SIZE_MAX)
 125                 gb->size = GB_LOOPBACK_SIZE_MAX;
 126         gb->error = 0;
 127         gb_loopback_reset_stats(gb);
 128 }
 129
 130 /* Time to send and receive one message */
 131 gb_loopback_stats_attrs(latency);
 132 /* Number of packet sent per second on this cport */
 133 gb_loopback_stats_attrs(frequency);
 134 /* Quantity of data sent and received on this cport */
 135 gb_loopback_stats_attrs(throughput);
 136 gb_loopback_ro_attr(error, d);
 137
 138 /*
 139  * Type of loopback message to send
 140  * 0 => Don't send message
 141  * 1 => Send ping message continuously (message without payload)
 142  * 2 => Send transer message continuously (message with payload)
 143  */
 144 gb_loopback_attr(type, d);
 145 /* Size of transfer message payload: 0-4096 bytes */
 146 gb_loopback_attr(size, u);
 147 /* Time to wait between two messages: 0-1000 ms */
 148 gb_loopback_attr(ms_wait, d);
 149
 150 #define dev_stats_attrs(name)                                           \
 151         &dev_attr_##name##_min.attr,                                    \
 152         &dev_attr_##name##_max.attr,                                    \
 153         &dev_attr_##name##_avg.attr
 154
 155 static struct attribute *loopback_attrs[] = {
 156         dev_stats_attrs(latency),
 157         dev_stats_attrs(frequency),
 158         dev_stats_attrs(throughput),
 159         &dev_attr_type.attr,
 160         &dev_attr_size.attr,
 161         &dev_attr_ms_wait.attr,
 162         &dev_attr_error.attr,
 163         NULL,
 164 };
 165 ATTRIBUTE_GROUPS(loopback);
 166
 167 static int gb_loopback_transfer(struct gb_loopback *gb,
 168                                 struct timeval *tping, u32 len)
 169 {
 170         struct timeval ts, te;
 171         u64 elapsed_nsecs;
 172         struct gb_loopback_transfer_request *request;
 173         struct gb_loopback_transfer_response *response;
 174         int retval;
 175
 176         request = kmalloc(len + sizeof(*request), GFP_KERNEL);
 177         if (!request)
 178                 return -ENOMEM;
 179         response = kmalloc(len + sizeof(*response), GFP_KERNEL);
 180         if (!response) {
 181                 kfree(request);
 182                 return -ENOMEM;
 183         }
 184
 185         request->len = cpu_to_le32(len);
 186
 187         do_gettimeofday(&ts);
 188         retval = gb_operation_sync(gb->connection, GB_LOOPBACK_TYPE_TRANSFER,
 189                                    request, len + sizeof(*request),
 190                                    response, len + sizeof(*response));
 191         do_gettimeofday(&te);
 192         elapsed_nsecs = timeval_to_ns(&te) - timeval_to_ns(&ts);
 193         *tping = ns_to_timeval(elapsed_nsecs);
 194
 195         if (retval)
 196                 goto gb_error;
 197
 198         if (memcmp(request->data, response->data, len))
 199                 retval = -EREMOTEIO;
 200
 201 gb_error:
 202         kfree(request);
 203         kfree(response);
 204
 205         return retval;
 206 }
 207
 208 static int gb_loopback_ping(struct gb_loopback *gb, struct timeval *tping)
 209 {
 210         struct timeval ts, te;
 211         u64 elapsed_nsecs;
 212         int retval;
 213
 214         do_gettimeofday(&ts);
 215         retval = gb_operation_sync(gb->connection, GB_LOOPBACK_TYPE_PING,
 216                                    NULL, 0, NULL, 0);
 217         do_gettimeofday(&te);
 218         elapsed_nsecs = timeval_to_ns(&te) - timeval_to_ns(&ts);
 219         *tping = ns_to_timeval(elapsed_nsecs);
 220
 221         return retval;
 222 }
 223
 224 static int gb_loopback_request_recv(u8 type, struct gb_operation *operation)
 225 {
 226         struct gb_connection *connection = operation->connection;
 227         struct gb_loopback_transfer_request *request;
 228         struct gb_loopback_transfer_response *response;
 229         u32 len;
 230
 231         /* By convention, the AP initiates the version operation */
 232         switch (type) {
 233         case GB_LOOPBACK_TYPE_PROTOCOL_VERSION:
 234                 dev_err(&connection->dev,
 235                         "module-initiated version operation\n");
 236                 return -EINVAL;
 237         case GB_LOOPBACK_TYPE_PING:
 238                 return 0;
 239         case GB_LOOPBACK_TYPE_TRANSFER:
 240                 if (operation->request->payload_size < sizeof(*request)) {
 241                         dev_err(&connection->dev,
 242                                 "transfer request too small (%zu < %zu)\n",
 243                                 operation->request->payload_size,
 244                                 sizeof(*request));
 245                         return -EINVAL; /* -EMSGSIZE */
 246                 }
 247                 request = operation->request->payload;
 248                 len = le32_to_cpu(request->len);
 249                 if (len) {
 250                         if (!gb_operation_response_alloc(operation, len)) {
 251                                 dev_err(&connection->dev,
 252                                         "error allocating response\n");
 253                                 return -ENOMEM;
 254                         }
 255                         response = operation->response->payload;
 256                         memcpy(response->data, request->data, len);
 257                 }
 258                 return 0;
 259         default:
 260                 dev_err(&connection->dev,
 261                         "unsupported request: %hhu\n", type);
 262                 return -EINVAL;
 263         }
 264 }
 265
 266 static void gb_loopback_reset_stats(struct gb_loopback *gb)
 267 {
 268         struct gb_loopback_stats reset = {
 269                 .min = 0xffffffff,
 270         };
 271         memcpy(&gb->latency, &reset, sizeof(struct gb_loopback_stats));
 272         memcpy(&gb->throughput, &reset, sizeof(struct gb_loopback_stats));
 273         memcpy(&gb->frequency, &reset, sizeof(struct gb_loopback_stats));
 274         memset(&gb->ts, 0, sizeof(struct timeval));
 275 }
 276
 277 static void gb_loopback_update_stats(struct gb_loopback_stats *stats,
 278                                         u64 elapsed_nsecs)
 279 {
 280         u32 avg;
 281         u64 tmp;
 282
 283         if (elapsed_nsecs >= NSEC_PER_SEC) {
 284                 if (!stats->count) {
 285                         tmp = elapsed_nsecs;
 286                         do_div(tmp, NSEC_PER_SEC);
 287                         avg = stats->sum * tmp;
 288                 } else {
 289                         avg = stats->sum / stats->count;
 290                 }
 291                 if (stats->min > avg)
 292                         stats->min = avg;
 293                 if (stats->max < avg)
 294                         stats->max = avg;
 295                 stats->avg = avg;
 296                 stats->count = 0;
 297                 stats->sum = 0;
 298         }
 299 }
 300
 301 static void gb_loopback_freq_update(struct gb_loopback *gb)
 302 {
 303         gb->frequency.sum++;
 304         gb_loopback_update_stats(&gb->frequency, gb->elapsed_nsecs);
 305 }
 306
 307 static void gb_loopback_bw_update(struct gb_loopback *gb, int error)
 308 {
 309         if (!error)
 310                 gb->throughput.sum += gb->size * 2;
 311         gb_loopback_update_stats(&gb->throughput, gb->elapsed_nsecs);
 312 }
 313
 314 static void gb_loopback_latency_update(struct gb_loopback *gb,
 315                                         struct timeval *tlat)
 316 {
 317         u32 lat;
 318         u64 tmp;
 319
 320         tmp = timeval_to_ns(tlat);
 321         do_div(tmp, NSEC_PER_MSEC);
 322         lat = tmp;
 323
 324         if (gb->latency.min > lat)
 325                 gb->latency.min = lat;
 326         if (gb->latency.max < lat)
 327                 gb->latency.max = lat;
 328         gb->latency.sum += lat;
 329         gb->latency.count++;
 330         gb_loopback_update_stats(&gb->latency, gb->elapsed_nsecs);
 331 }
 332
 333 static int gb_loopback_fn(void *data)
 334 {
 335         int error = 0;
 336         struct timeval tlat = {0, 0};
 337         struct gb_loopback *gb = (struct gb_loopback *)data;
 338
 339         while (!kthread_should_stop()) {
 340                 if (!gb->type) {
 341                         msleep(1000);
 342                         continue;
 343                 }
 344                 if (gb->type == GB_LOOPBACK_TYPE_PING)
 345                         error = gb_loopback_ping(gb, &tlat);
 346                 else if (gb->type == GB_LOOPBACK_TYPE_TRANSFER)
 347                         error = gb_loopback_transfer(gb, &tlat, gb->size);
 348                 if (error)
 349                         gb->error++;
 350                 if (gb->ts.tv_usec == 0 && gb->ts.tv_sec == 0) {
 351                         do_gettimeofday(&gb->ts);
 352                         continue;
 353                 }
 354                 do_gettimeofday(&gb->te);
 355                 gb->elapsed_nsecs = timeval_to_ns(&gb->te) -
 356                                         timeval_to_ns(&gb->ts);
 357                 gb_loopback_freq_update(gb);
 358                 if (gb->type == GB_LOOPBACK_TYPE_PING)
 359                         gb_loopback_bw_update(gb, error);
 360                 gb_loopback_latency_update(gb, &tlat);
 361                 if (gb->elapsed_nsecs >= NSEC_PER_SEC)
 362                         gb->ts = gb->te;
 363                 if (gb->ms_wait)
 364                         msleep(gb->ms_wait);
 365
 366         }
 367         return 0;
 368 }
 369
 370 static int gb_loopback_connection_init(struct gb_connection *connection)
 371 {
 372         struct gb_loopback *gb;
 373         int retval;
 374
 375         gb = kzalloc(sizeof(*gb), GFP_KERNEL);
 376         if (!gb)
 377                 return -ENOMEM;
 378
 379         gb->connection = connection;
 380         connection->private = gb;
 381         retval = sysfs_create_groups(&connection->dev.kobj, loopback_groups);
 382         if (retval)
 383                 goto out_free;
 384
 385         /* Check the version */
 386         retval = get_version(gb);
 387         if (retval)
 388                 goto out_get_ver;
 389
 390         gb_loopback_reset_stats(gb);
 391         gb->task = kthread_run(gb_loopback_fn, gb, "gb_loopback");
 392         if (IS_ERR(gb->task)) {
 393                 retval = PTR_ERR(gb->task);
 394                 goto out_get_ver;
 395         }
 396
 397         return 0;
 398
 399 out_get_ver:
 400         sysfs_remove_groups(&connection->dev.kobj, loopback_groups);
 401 out_free:
 402         kfree(gb);
 403         return retval;
 404 }
 405
 406 static void gb_loopback_connection_exit(struct gb_connection *connection)
 407 {
 408         struct gb_loopback *gb = connection->private;
 409
 410         if (!IS_ERR_OR_NULL(gb->task))
 411                 kthread_stop(gb->task);
 412         sysfs_remove_groups(&connection->dev.kobj, loopback_groups);
 413         kfree(gb);
 414 }
 415
 416 static struct gb_protocol loopback_protocol = {
 417         .name                   = "loopback",
 418         .id                     = GREYBUS_PROTOCOL_LOOPBACK,
 419         .major                  = GB_LOOPBACK_VERSION_MAJOR,
 420         .minor                  = GB_LOOPBACK_VERSION_MINOR,
 421         .connection_init        = gb_loopback_connection_init,
 422         .connection_exit        = gb_loopback_connection_exit,
 423         .request_recv           = gb_loopback_request_recv,
 424 };
 425
 426 gb_protocol_driver(&loopback_protocol);
 427
 428 MODULE_LICENSE("GPL v2");