drivers/hwtracing/stm/core.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * System Trace Module (STM) infrastructure
   4  * Copyright (c) 2014, Intel Corporation.
   5  *
   6  * STM class implements generic infrastructure for  System Trace Module devices
   7  * as defined in MIPI STPv2 specification.
   8  */
   9
  10 #include <linux/pm_runtime.h>
  11 #include <linux/uaccess.h>
  12 #include <linux/kernel.h>
  13 #include <linux/module.h>
  14 #include <linux/device.h>
  15 #include <linux/compat.h>
  16 #include <linux/kdev_t.h>
  17 #include <linux/srcu.h>
  18 #include <linux/slab.h>
  19 #include <linux/stm.h>
  20 #include <linux/fs.h>
  21 #include <linux/mm.h>
  22 #include <linux/vmalloc.h>
  23 #include "stm.h"
  24
  25 #include <uapi/linux/stm.h>
  26
  27 static unsigned int stm_core_up;
  28
  29 /*
  30  * The SRCU here makes sure that STM device doesn't disappear from under a
  31  * stm_source_write() caller, which may want to have as little overhead as
  32  * possible.
  33  */
  34 static struct srcu_struct stm_source_srcu;
  35
  36 static ssize_t masters_show(struct device *dev,
  37                             struct device_attribute *attr,
  38                             char *buf)
  39 {
  40         struct stm_device *stm = to_stm_device(dev);
  41         int ret;
  42
  43         ret = sprintf(buf, "%u %u\n", stm->data->sw_start, stm->data->sw_end);
  44
  45         return ret;
  46 }
  47
  48 static DEVICE_ATTR_RO(masters);
  49
  50 static ssize_t channels_show(struct device *dev,
  51                              struct device_attribute *attr,
  52                              char *buf)
  53 {
  54         struct stm_device *stm = to_stm_device(dev);
  55         int ret;
  56
  57         ret = sprintf(buf, "%u\n", stm->data->sw_nchannels);
  58
  59         return ret;
  60 }
  61
  62 static DEVICE_ATTR_RO(channels);
  63
  64 static ssize_t hw_override_show(struct device *dev,
  65                                 struct device_attribute *attr,
  66                                 char *buf)
  67 {
  68         struct stm_device *stm = to_stm_device(dev);
  69         int ret;
  70
  71         ret = sprintf(buf, "%u\n", stm->data->hw_override);
  72
  73         return ret;
  74 }
  75
  76 static DEVICE_ATTR_RO(hw_override);
  77
  78 static struct attribute *stm_attrs[] = {
  79         &dev_attr_masters.attr,
  80         &dev_attr_channels.attr,
  81         &dev_attr_hw_override.attr,
  82         NULL,
  83 };
  84
  85 ATTRIBUTE_GROUPS(stm);
  86
  87 static struct class stm_class = {
  88         .name           = "stm",
  89         .dev_groups     = stm_groups,
  90 };
  91
  92 static int stm_dev_match(struct device *dev, const void *data)
  93 {
  94         const char *name = data;
  95
  96         return sysfs_streq(name, dev_name(dev));
  97 }
  98
  99 /**
 100  * stm_find_device() - find stm device by name
 101  * @buf:        character buffer containing the name
 102  *
 103  * This is called when either policy gets assigned to an stm device or an
 104  * stm_source device gets linked to an stm device.
 105  *
 106  * This grabs device's reference (get_device()) and module reference, both
 107  * of which the calling path needs to make sure to drop with stm_put_device().
 108  *
 109  * Return:      stm device pointer or null if lookup failed.
 110  */
 111 struct stm_device *stm_find_device(const char *buf)
 112 {
 113         struct stm_device *stm;
 114         struct device *dev;
 115
 116         if (!stm_core_up)
 117                 return NULL;
 118
 119         dev = class_find_device(&stm_class, NULL, buf, stm_dev_match);
 120         if (!dev)
 121                 return NULL;
 122
 123         stm = to_stm_device(dev);
 124         if (!try_module_get(stm->owner)) {
 125                 /* matches class_find_device() above */
 126                 put_device(dev);
 127                 return NULL;
 128         }
 129
 130         return stm;
 131 }
 132
 133 /**
 134  * stm_put_device() - drop references on the stm device
 135  * @stm:        stm device, previously acquired by stm_find_device()
 136  *
 137  * This drops the module reference and device reference taken by
 138  * stm_find_device() or stm_char_open().
 139  */
 140 void stm_put_device(struct stm_device *stm)
 141 {
 142         module_put(stm->owner);
 143         put_device(&stm->dev);
 144 }
 145
 146 /*
 147  * Internally we only care about software-writable masters here, that is the
 148  * ones in the range [stm_data->sw_start..stm_data..sw_end], however we need
 149  * original master numbers to be visible externally, since they are the ones
 150  * that will appear in the STP stream. Thus, the internal bookkeeping uses
 151  * $master - stm_data->sw_start to reference master descriptors and such.
 152  */
 153
 154 #define __stm_master(_s, _m)                            \
 155         ((_s)->masters[(_m) - (_s)->data->sw_start])
 156
 157 static inline struct stp_master *
 158 stm_master(struct stm_device *stm, unsigned int idx)
 159 {
 160         if (idx < stm->data->sw_start || idx > stm->data->sw_end)
 161                 return NULL;
 162
 163         return __stm_master(stm, idx);
 164 }
 165
 166 static int stp_master_alloc(struct stm_device *stm, unsigned int idx)
 167 {
 168         struct stp_master *master;
 169         size_t size;
 170
 171         size = ALIGN(stm->data->sw_nchannels, 8) / 8;
 172         size += sizeof(struct stp_master);
 173         master = kzalloc(size, GFP_ATOMIC);
 174         if (!master)
 175                 return -ENOMEM;
 176
 177         master->nr_free = stm->data->sw_nchannels;
 178         __stm_master(stm, idx) = master;
 179
 180         return 0;
 181 }
 182
 183 static void stp_master_free(struct stm_device *stm, unsigned int idx)
 184 {
 185         struct stp_master *master = stm_master(stm, idx);
 186
 187         if (!master)
 188                 return;
 189
 190         __stm_master(stm, idx) = NULL;
 191         kfree(master);
 192 }
 193
 194 static void stm_output_claim(struct stm_device *stm, struct stm_output *output)
 195 {
 196         struct stp_master *master = stm_master(stm, output->master);
 197
 198         lockdep_assert_held(&stm->mc_lock);
 199         lockdep_assert_held(&output->lock);
 200
 201         if (WARN_ON_ONCE(master->nr_free < output->nr_chans))
 202                 return;
 203
 204         bitmap_allocate_region(&master->chan_map[0], output->channel,
 205                                ilog2(output->nr_chans));
 206
 207         master->nr_free -= output->nr_chans;
 208 }
 209
 210 static void
 211 stm_output_disclaim(struct stm_device *stm, struct stm_output *output)
 212 {
 213         struct stp_master *master = stm_master(stm, output->master);
 214
 215         lockdep_assert_held(&stm->mc_lock);
 216         lockdep_assert_held(&output->lock);
 217
 218         bitmap_release_region(&master->chan_map[0], output->channel,
 219                               ilog2(output->nr_chans));
 220
 221         output->nr_chans = 0;
 222         master->nr_free += output->nr_chans;
 223 }
 224
 225 /*
 226  * This is like bitmap_find_free_region(), except it can ignore @start bits
 227  * at the beginning.
 228  */
 229 static int find_free_channels(unsigned long *bitmap, unsigned int start,
 230                               unsigned int end, unsigned int width)
 231 {
 232         unsigned int pos;
 233         int i;
 234
 235         for (pos = start; pos < end + 1; pos = ALIGN(pos, width)) {
 236                 pos = find_next_zero_bit(bitmap, end + 1, pos);
 237                 if (pos + width > end + 1)
 238                         break;
 239
 240                 if (pos & (width - 1))
 241                         continue;
 242
 243                 for (i = 1; i < width && !test_bit(pos + i, bitmap); i++)
 244                         ;
 245                 if (i == width)
 246                         return pos;
 247         }
 248
 249         return -1;
 250 }
 251
 252 static int
 253 stm_find_master_chan(struct stm_device *stm, unsigned int width,
 254                      unsigned int *mstart, unsigned int mend,
 255                      unsigned int *cstart, unsigned int cend)
 256 {
 257         struct stp_master *master;
 258         unsigned int midx;
 259         int pos, err;
 260
 261         for (midx = *mstart; midx <= mend; midx++) {
 262                 if (!stm_master(stm, midx)) {
 263                         err = stp_master_alloc(stm, midx);
 264                         if (err)
 265                                 return err;
 266                 }
 267
 268                 master = stm_master(stm, midx);
 269
 270                 if (!master->nr_free)
 271                         continue;
 272
 273                 pos = find_free_channels(master->chan_map, *cstart, cend,
 274                                          width);
 275                 if (pos < 0)
 276                         continue;
 277
 278                 *mstart = midx;
 279                 *cstart = pos;
 280                 return 0;
 281         }
 282
 283         return -ENOSPC;
 284 }
 285
 286 static int stm_output_assign(struct stm_device *stm, unsigned int width,
 287                              struct stp_policy_node *policy_node,
 288                              struct stm_output *output)
 289 {
 290         unsigned int midx, cidx, mend, cend;
 291         int ret = -EINVAL;
 292
 293         if (width > stm->data->sw_nchannels)
 294                 return -EINVAL;
 295
 296         /* We no longer accept policy_node==NULL here */
 297         if (WARN_ON_ONCE(!policy_node))
 298                 return -EINVAL;
 299
 300         /*
 301          * Also, the caller holds reference to policy_node, so it won't
 302          * disappear on us.
 303          */
 304         stp_policy_node_get_ranges(policy_node, &midx, &mend, &cidx, &cend);
 305
 306         spin_lock(&stm->mc_lock);
 307         spin_lock(&output->lock);
 308         /* output is already assigned -- shouldn't happen */
 309         if (WARN_ON_ONCE(output->nr_chans))
 310                 goto unlock;
 311
 312         ret = stm_find_master_chan(stm, width, &midx, mend, &cidx, cend);
 313         if (ret < 0)
 314                 goto unlock;
 315
 316         output->master = midx;
 317         output->channel = cidx;
 318         output->nr_chans = width;
 319         if (stm->pdrv->output_open) {
 320                 void *priv = stp_policy_node_priv(policy_node);
 321
 322                 if (WARN_ON_ONCE(!priv))
 323                         goto unlock;
 324
 325                 /* configfs subsys mutex is held by the caller */
 326                 ret = stm->pdrv->output_open(priv, output);
 327                 if (ret)
 328                         goto unlock;
 329         }
 330
 331         stm_output_claim(stm, output);
 332         dev_dbg(&stm->dev, "assigned %u:%u (+%u)\n", midx, cidx, width);
 333
 334         ret = 0;
 335 unlock:
 336         if (ret)
 337                 output->nr_chans = 0;
 338
 339         spin_unlock(&output->lock);
 340         spin_unlock(&stm->mc_lock);
 341
 342         return ret;
 343 }
 344
 345 static void stm_output_free(struct stm_device *stm, struct stm_output *output)
 346 {
 347         spin_lock(&stm->mc_lock);
 348         spin_lock(&output->lock);
 349         if (output->nr_chans)
 350                 stm_output_disclaim(stm, output);
 351         if (stm->pdrv && stm->pdrv->output_close)
 352                 stm->pdrv->output_close(output);
 353         spin_unlock(&output->lock);
 354         spin_unlock(&stm->mc_lock);
 355 }
 356
 357 static void stm_output_init(struct stm_output *output)
 358 {
 359         spin_lock_init(&output->lock);
 360 }
 361
 362 static int major_match(struct device *dev, const void *data)
 363 {
 364         unsigned int major = *(unsigned int *)data;
 365
 366         return MAJOR(dev->devt) == major;
 367 }
 368
 369 /*
 370  * Framing protocol management
 371  * Modules can implement STM protocol drivers and (un-)register them
 372  * with the STM class framework.
 373  */
 374 static struct list_head stm_pdrv_head;
 375 static struct mutex stm_pdrv_mutex;
 376
 377 struct stm_pdrv_entry {
 378         struct list_head                        entry;
 379         const struct stm_protocol_driver        *pdrv;
 380         const struct config_item_type           *node_type;
 381 };
 382
 383 static const struct stm_pdrv_entry *
 384 __stm_lookup_protocol(const char *name)
 385 {
 386         struct stm_pdrv_entry *pe;
 387
 388         /*
 389          * If no name is given (NULL or ""), fall back to "p_basic".
 390          */
 391         if (!name || !*name)
 392                 name = "p_basic";
 393
 394         list_for_each_entry(pe, &stm_pdrv_head, entry) {
 395                 if (!strcmp(name, pe->pdrv->name))
 396                         return pe;
 397         }
 398
 399         return NULL;
 400 }
 401
 402 int stm_register_protocol(const struct stm_protocol_driver *pdrv)
 403 {
 404         struct stm_pdrv_entry *pe = NULL;
 405         int ret = -ENOMEM;
 406
 407         mutex_lock(&stm_pdrv_mutex);
 408
 409         if (__stm_lookup_protocol(pdrv->name)) {
 410                 ret = -EEXIST;
 411                 goto unlock;
 412         }
 413
 414         pe = kzalloc(sizeof(*pe), GFP_KERNEL);
 415         if (!pe)
 416                 goto unlock;
 417
 418         if (pdrv->policy_attr) {
 419                 pe->node_type = get_policy_node_type(pdrv->policy_attr);
 420                 if (!pe->node_type)
 421                         goto unlock;
 422         }
 423
 424         list_add_tail(&pe->entry, &stm_pdrv_head);
 425         pe->pdrv = pdrv;
 426
 427         ret = 0;
 428 unlock:
 429         mutex_unlock(&stm_pdrv_mutex);
 430
 431         if (ret)
 432                 kfree(pe);
 433
 434         return ret;
 435 }
 436 EXPORT_SYMBOL_GPL(stm_register_protocol);
 437
 438 void stm_unregister_protocol(const struct stm_protocol_driver *pdrv)
 439 {
 440         struct stm_pdrv_entry *pe, *iter;
 441
 442         mutex_lock(&stm_pdrv_mutex);
 443
 444         list_for_each_entry_safe(pe, iter, &stm_pdrv_head, entry) {
 445                 if (pe->pdrv == pdrv) {
 446                         list_del(&pe->entry);
 447
 448                         if (pe->node_type) {
 449                                 kfree(pe->node_type->ct_attrs);
 450                                 kfree(pe->node_type);
 451                         }
 452                         kfree(pe);
 453                         break;
 454                 }
 455         }
 456
 457         mutex_unlock(&stm_pdrv_mutex);
 458 }
 459 EXPORT_SYMBOL_GPL(stm_unregister_protocol);
 460
 461 static bool stm_get_protocol(const struct stm_protocol_driver *pdrv)
 462 {
 463         return try_module_get(pdrv->owner);
 464 }
 465
 466 void stm_put_protocol(const struct stm_protocol_driver *pdrv)
 467 {
 468         module_put(pdrv->owner);
 469 }
 470
 471 int stm_lookup_protocol(const char *name,
 472                         const struct stm_protocol_driver **pdrv,
 473                         const struct config_item_type **node_type)
 474 {
 475         const struct stm_pdrv_entry *pe;
 476
 477         mutex_lock(&stm_pdrv_mutex);
 478
 479         pe = __stm_lookup_protocol(name);
 480         if (pe && pe->pdrv && stm_get_protocol(pe->pdrv)) {
 481                 *pdrv = pe->pdrv;
 482                 *node_type = pe->node_type;
 483         }
 484
 485         mutex_unlock(&stm_pdrv_mutex);
 486
 487         return pe ? 0 : -ENOENT;
 488 }
 489
 490 static int stm_char_open(struct inode *inode, struct file *file)
 491 {
 492         struct stm_file *stmf;
 493         struct device *dev;
 494         unsigned int major = imajor(inode);
 495         int err = -ENOMEM;
 496
 497         dev = class_find_device(&stm_class, NULL, &major, major_match);
 498         if (!dev)
 499                 return -ENODEV;
 500
 501         stmf = kzalloc(sizeof(*stmf), GFP_KERNEL);
 502         if (!stmf)
 503                 goto err_put_device;
 504
 505         err = -ENODEV;
 506         stm_output_init(&stmf->output);
 507         stmf->stm = to_stm_device(dev);
 508
 509         if (!try_module_get(stmf->stm->owner))
 510                 goto err_free;
 511
 512         file->private_data = stmf;
 513
 514         return nonseekable_open(inode, file);
 515
 516 err_free:
 517         kfree(stmf);
 518 err_put_device:
 519         /* matches class_find_device() above */
 520         put_device(dev);
 521
 522         return err;
 523 }
 524
 525 static int stm_char_release(struct inode *inode, struct file *file)
 526 {
 527         struct stm_file *stmf = file->private_data;
 528         struct stm_device *stm = stmf->stm;
 529
 530         if (stm->data->unlink)
 531                 stm->data->unlink(stm->data, stmf->output.master,
 532                                   stmf->output.channel);
 533
 534         stm_output_free(stm, &stmf->output);
 535
 536         /*
 537          * matches the stm_char_open()'s
 538          * class_find_device() + try_module_get()
 539          */
 540         stm_put_device(stm);
 541         kfree(stmf);
 542
 543         return 0;
 544 }
 545
 546 static int
 547 stm_assign_first_policy(struct stm_device *stm, struct stm_output *output,
 548                         char **ids, unsigned int width)
 549 {
 550         struct stp_policy_node *pn;
 551         int err, n;
 552
 553         /*
 554          * On success, stp_policy_node_lookup() will return holding the
 555          * configfs subsystem mutex, which is then released in
 556          * stp_policy_node_put(). This allows the pdrv->output_open() in
 557          * stm_output_assign() to serialize against the attribute accessors.
 558          */
 559         for (n = 0, pn = NULL; ids[n] && !pn; n++)
 560                 pn = stp_policy_node_lookup(stm, ids[n]);
 561
 562         if (!pn)
 563                 return -EINVAL;
 564
 565         err = stm_output_assign(stm, width, pn, output);
 566
 567         stp_policy_node_put(pn);
 568
 569         return err;
 570 }
 571
 572 /**
 573  * stm_data_write() - send the given payload as data packets
 574  * @data:       stm driver's data
 575  * @m:          STP master
 576  * @c:          STP channel
 577  * @ts_first:   timestamp the first packet
 578  * @buf:        data payload buffer
 579  * @count:      data payload size
 580  */
 581 ssize_t notrace stm_data_write(struct stm_data *data, unsigned int m,
 582                                unsigned int c, bool ts_first, const void *buf,
 583                                size_t count)
 584 {
 585         unsigned int flags = ts_first ? STP_PACKET_TIMESTAMPED : 0;
 586         ssize_t sz;
 587         size_t pos;
 588
 589         for (pos = 0, sz = 0; pos < count; pos += sz) {
 590                 sz = min_t(unsigned int, count - pos, 8);
 591                 sz = data->packet(data, m, c, STP_PACKET_DATA, flags, sz,
 592                                   &((u8 *)buf)[pos]);
 593                 if (sz <= 0)
 594                         break;
 595
 596                 if (ts_first) {
 597                         flags = 0;
 598                         ts_first = false;
 599                 }
 600         }
 601
 602         return sz < 0 ? sz : pos;
 603 }
 604 EXPORT_SYMBOL_GPL(stm_data_write);
 605
 606 static ssize_t notrace
 607 stm_write(struct stm_device *stm, struct stm_output *output,
 608           unsigned int chan, const char *buf, size_t count)
 609 {
 610         int err;
 611
 612         /* stm->pdrv is serialized against policy_mutex */
 613         if (!stm->pdrv)
 614                 return -ENODEV;
 615
 616         err = stm->pdrv->write(stm->data, output, chan, buf, count);
 617         if (err < 0)
 618                 return err;
 619
 620         return err;
 621 }
 622
 623 static ssize_t stm_char_write(struct file *file, const char __user *buf,
 624                               size_t count, loff_t *ppos)
 625 {
 626         struct stm_file *stmf = file->private_data;
 627         struct stm_device *stm = stmf->stm;
 628         char *kbuf;
 629         int err;
 630
 631         if (count + 1 > PAGE_SIZE)
 632                 count = PAGE_SIZE - 1;
 633
 634         /*
 635          * If no m/c have been assigned to this writer up to this
 636          * point, try to use the task name and "default" policy entries.
 637          */
 638         if (!stmf->output.nr_chans) {
 639                 char comm[sizeof(current->comm)];
 640                 char *ids[] = { comm, "default", NULL };
 641
 642                 get_task_comm(comm, current);
 643
 644                 err = stm_assign_first_policy(stmf->stm, &stmf->output, ids, 1);
 645                 /*
 646                  * EBUSY means that somebody else just assigned this
 647                  * output, which is just fine for write()
 648                  */
 649                 if (err)
 650                         return err;
 651         }
 652
 653         kbuf = kmalloc(count + 1, GFP_KERNEL);
 654         if (!kbuf)
 655                 return -ENOMEM;
 656
 657         err = copy_from_user(kbuf, buf, count);
 658         if (err) {
 659                 kfree(kbuf);
 660                 return -EFAULT;
 661         }
 662
 663         pm_runtime_get_sync(&stm->dev);
 664
 665         count = stm_write(stm, &stmf->output, 0, kbuf, count);
 666
 667         pm_runtime_mark_last_busy(&stm->dev);
 668         pm_runtime_put_autosuspend(&stm->dev);
 669         kfree(kbuf);
 670
 671         return count;
 672 }
 673
 674 static void stm_mmap_open(struct vm_area_struct *vma)
 675 {
 676         struct stm_file *stmf = vma->vm_file->private_data;
 677         struct stm_device *stm = stmf->stm;
 678
 679         pm_runtime_get(&stm->dev);
 680 }
 681
 682 static void stm_mmap_close(struct vm_area_struct *vma)
 683 {
 684         struct stm_file *stmf = vma->vm_file->private_data;
 685         struct stm_device *stm = stmf->stm;
 686
 687         pm_runtime_mark_last_busy(&stm->dev);
 688         pm_runtime_put_autosuspend(&stm->dev);
 689 }
 690
 691 static const struct vm_operations_struct stm_mmap_vmops = {
 692         .open   = stm_mmap_open,
 693         .close  = stm_mmap_close,
 694 };
 695
 696 static int stm_char_mmap(struct file *file, struct vm_area_struct *vma)
 697 {
 698         struct stm_file *stmf = file->private_data;
 699         struct stm_device *stm = stmf->stm;
 700         unsigned long size, phys;
 701
 702         if (!stm->data->mmio_addr)
 703                 return -EOPNOTSUPP;
 704
 705         if (vma->vm_pgoff)
 706                 return -EINVAL;
 707
 708         size = vma->vm_end - vma->vm_start;
 709
 710         if (stmf->output.nr_chans * stm->data->sw_mmiosz != size)
 711                 return -EINVAL;
 712
 713         phys = stm->data->mmio_addr(stm->data, stmf->output.master,
 714                                     stmf->output.channel,
 715                                     stmf->output.nr_chans);
 716
 717         if (!phys)
 718                 return -EINVAL;
 719
 720         pm_runtime_get_sync(&stm->dev);
 721
 722         vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
 723         vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
 724         vma->vm_ops = &stm_mmap_vmops;
 725         vm_iomap_memory(vma, phys, size);
 726
 727         return 0;
 728 }
 729
 730 static int stm_char_policy_set_ioctl(struct stm_file *stmf, void __user *arg)
 731 {
 732         struct stm_device *stm = stmf->stm;
 733         struct stp_policy_id *id;
 734         char *ids[] = { NULL, NULL };
 735         int ret = -EINVAL;
 736         u32 size;
 737
 738         if (stmf->output.nr_chans)
 739                 return -EBUSY;
 740
 741         if (copy_from_user(&size, arg, sizeof(size)))
 742                 return -EFAULT;
 743
 744         if (size < sizeof(*id) || size >= PATH_MAX + sizeof(*id))
 745                 return -EINVAL;
 746
 747         /*
 748          * size + 1 to make sure the .id string at the bottom is terminated,
 749          * which is also why memdup_user() is not useful here
 750          */
 751         id = kzalloc(size + 1, GFP_KERNEL);
 752         if (!id)
 753                 return -ENOMEM;
 754
 755         if (copy_from_user(id, arg, size)) {
 756                 ret = -EFAULT;
 757                 goto err_free;
 758         }
 759
 760         if (id->__reserved_0 || id->__reserved_1)
 761                 goto err_free;
 762
 763         if (id->width < 1 ||
 764             id->width > PAGE_SIZE / stm->data->sw_mmiosz)
 765                 goto err_free;
 766
 767         ids[0] = id->id;
 768         ret = stm_assign_first_policy(stmf->stm, &stmf->output, ids,
 769                                       id->width);
 770         if (ret)
 771                 goto err_free;
 772
 773         if (stm->data->link)
 774                 ret = stm->data->link(stm->data, stmf->output.master,
 775                                       stmf->output.channel);
 776
 777         if (ret)
 778                 stm_output_free(stmf->stm, &stmf->output);
 779
 780 err_free:
 781         kfree(id);
 782
 783         return ret;
 784 }
 785
 786 static int stm_char_policy_get_ioctl(struct stm_file *stmf, void __user *arg)
 787 {
 788         struct stp_policy_id id = {
 789                 .size           = sizeof(id),
 790                 .master         = stmf->output.master,
 791                 .channel        = stmf->output.channel,
 792                 .width          = stmf->output.nr_chans,
 793                 .__reserved_0   = 0,
 794                 .__reserved_1   = 0,
 795         };
 796
 797         return copy_to_user(arg, &id, id.size) ? -EFAULT : 0;
 798 }
 799
 800 static long
 801 stm_char_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 802 {
 803         struct stm_file *stmf = file->private_data;
 804         struct stm_data *stm_data = stmf->stm->data;
 805         int err = -ENOTTY;
 806         u64 options;
 807
 808         switch (cmd) {
 809         case STP_POLICY_ID_SET:
 810                 err = stm_char_policy_set_ioctl(stmf, (void __user *)arg);
 811                 if (err)
 812                         return err;
 813
 814                 return stm_char_policy_get_ioctl(stmf, (void __user *)arg);
 815
 816         case STP_POLICY_ID_GET:
 817                 return stm_char_policy_get_ioctl(stmf, (void __user *)arg);
 818
 819         case STP_SET_OPTIONS:
 820                 if (copy_from_user(&options, (u64 __user *)arg, sizeof(u64)))
 821                         return -EFAULT;
 822
 823                 if (stm_data->set_options)
 824                         err = stm_data->set_options(stm_data,
 825                                                     stmf->output.master,
 826                                                     stmf->output.channel,
 827                                                     stmf->output.nr_chans,
 828                                                     options);
 829
 830                 break;
 831         default:
 832                 break;
 833         }
 834
 835         return err;
 836 }
 837
 838 #ifdef CONFIG_COMPAT
 839 static long
 840 stm_char_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 841 {
 842         return stm_char_ioctl(file, cmd, (unsigned long)compat_ptr(arg));
 843 }
 844 #else
 845 #define stm_char_compat_ioctl   NULL
 846 #endif
 847
 848 static const struct file_operations stm_fops = {
 849         .open           = stm_char_open,
 850         .release        = stm_char_release,
 851         .write          = stm_char_write,
 852         .mmap           = stm_char_mmap,
 853         .unlocked_ioctl = stm_char_ioctl,
 854         .compat_ioctl   = stm_char_compat_ioctl,
 855         .llseek         = no_llseek,
 856 };
 857
 858 static void stm_device_release(struct device *dev)
 859 {
 860         struct stm_device *stm = to_stm_device(dev);
 861
 862         vfree(stm);
 863 }
 864
 865 int stm_register_device(struct device *parent, struct stm_data *stm_data,
 866                         struct module *owner)
 867 {
 868         struct stm_device *stm;
 869         unsigned int nmasters;
 870         int err = -ENOMEM;
 871
 872         if (!stm_core_up)
 873                 return -EPROBE_DEFER;
 874
 875         if (!stm_data->packet || !stm_data->sw_nchannels)
 876                 return -EINVAL;
 877
 878         nmasters = stm_data->sw_end - stm_data->sw_start + 1;
 879         stm = vzalloc(sizeof(*stm) + nmasters * sizeof(void *));
 880         if (!stm)
 881                 return -ENOMEM;
 882
 883         stm->major = register_chrdev(0, stm_data->name, &stm_fops);
 884         if (stm->major < 0)
 885                 goto err_free;
 886
 887         device_initialize(&stm->dev);
 888         stm->dev.devt = MKDEV(stm->major, 0);
 889         stm->dev.class = &stm_class;
 890         stm->dev.parent = parent;
 891         stm->dev.release = stm_device_release;
 892
 893         mutex_init(&stm->link_mutex);
 894         spin_lock_init(&stm->link_lock);
 895         INIT_LIST_HEAD(&stm->link_list);
 896
 897         /* initialize the object before it is accessible via sysfs */
 898         spin_lock_init(&stm->mc_lock);
 899         mutex_init(&stm->policy_mutex);
 900         stm->sw_nmasters = nmasters;
 901         stm->owner = owner;
 902         stm->data = stm_data;
 903         stm_data->stm = stm;
 904
 905         err = kobject_set_name(&stm->dev.kobj, "%s", stm_data->name);
 906         if (err)
 907                 goto err_device;
 908
 909         err = device_add(&stm->dev);
 910         if (err)
 911                 goto err_device;
 912
 913         /*
 914          * Use delayed autosuspend to avoid bouncing back and forth
 915          * on recurring character device writes, with the initial
 916          * delay time of 2 seconds.
 917          */
 918         pm_runtime_no_callbacks(&stm->dev);
 919         pm_runtime_use_autosuspend(&stm->dev);
 920         pm_runtime_set_autosuspend_delay(&stm->dev, 2000);
 921         pm_runtime_set_suspended(&stm->dev);
 922         pm_runtime_enable(&stm->dev);
 923
 924         return 0;
 925
 926 err_device:
 927         unregister_chrdev(stm->major, stm_data->name);
 928
 929         /* matches device_initialize() above */
 930         put_device(&stm->dev);
 931 err_free:
 932         vfree(stm);
 933
 934         return err;
 935 }
 936 EXPORT_SYMBOL_GPL(stm_register_device);
 937
 938 static int __stm_source_link_drop(struct stm_source_device *src,
 939                                   struct stm_device *stm);
 940
 941 void stm_unregister_device(struct stm_data *stm_data)
 942 {
 943         struct stm_device *stm = stm_data->stm;
 944         struct stm_source_device *src, *iter;
 945         int i, ret;
 946
 947         pm_runtime_dont_use_autosuspend(&stm->dev);
 948         pm_runtime_disable(&stm->dev);
 949
 950         mutex_lock(&stm->link_mutex);
 951         list_for_each_entry_safe(src, iter, &stm->link_list, link_entry) {
 952                 ret = __stm_source_link_drop(src, stm);
 953                 /*
 954                  * src <-> stm link must not change under the same
 955                  * stm::link_mutex, so complain loudly if it has;
 956                  * also in this situation ret!=0 means this src is
 957                  * not connected to this stm and it should be otherwise
 958                  * safe to proceed with the tear-down of stm.
 959                  */
 960                 WARN_ON_ONCE(ret);
 961         }
 962         mutex_unlock(&stm->link_mutex);
 963
 964         synchronize_srcu(&stm_source_srcu);
 965
 966         unregister_chrdev(stm->major, stm_data->name);
 967
 968         mutex_lock(&stm->policy_mutex);
 969         if (stm->policy)
 970                 stp_policy_unbind(stm->policy);
 971         mutex_unlock(&stm->policy_mutex);
 972
 973         for (i = stm->data->sw_start; i <= stm->data->sw_end; i++)
 974                 stp_master_free(stm, i);
 975
 976         device_unregister(&stm->dev);
 977         stm_data->stm = NULL;
 978 }
 979 EXPORT_SYMBOL_GPL(stm_unregister_device);
 980
 981 /*
 982  * stm::link_list access serialization uses a spinlock and a mutex; holding
 983  * either of them guarantees that the list is stable; modification requires
 984  * holding both of them.
 985  *
 986  * Lock ordering is as follows:
 987  *   stm::link_mutex
 988  *     stm::link_lock
 989  *       src::link_lock
 990  */
 991
 992 /**
 993  * stm_source_link_add() - connect an stm_source device to an stm device
 994  * @src:        stm_source device
 995  * @stm:        stm device
 996  *
 997  * This function establishes a link from stm_source to an stm device so that
 998  * the former can send out trace data to the latter.
 999  *
1000  * Return:      0 on success, -errno otherwise.
1001  */
1002 static int stm_source_link_add(struct stm_source_device *src,
1003                                struct stm_device *stm)
1004 {
1005         char *ids[] = { NULL, "default", NULL };
1006         int err = -ENOMEM;
1007
1008         mutex_lock(&stm->link_mutex);
1009         spin_lock(&stm->link_lock);
1010         spin_lock(&src->link_lock);
1011
1012         /* src->link is dereferenced under stm_source_srcu but not the list */
1013         rcu_assign_pointer(src->link, stm);
1014         list_add_tail(&src->link_entry, &stm->link_list);
1015
1016         spin_unlock(&src->link_lock);
1017         spin_unlock(&stm->link_lock);
1018         mutex_unlock(&stm->link_mutex);
1019
1020         ids[0] = kstrdup(src->data->name, GFP_KERNEL);
1021         if (!ids[0])
1022                 goto fail_detach;
1023
1024         err = stm_assign_first_policy(stm, &src->output, ids,
1025                                       src->data->nr_chans);
1026         kfree(ids[0]);
1027
1028         if (err)
1029                 goto fail_detach;
1030
1031         /* this is to notify the STM device that a new link has been made */
1032         if (stm->data->link)
1033                 err = stm->data->link(stm->data, src->output.master,
1034                                       src->output.channel);
1035
1036         if (err)
1037                 goto fail_free_output;
1038
1039         /* this is to let the source carry out all necessary preparations */
1040         if (src->data->link)
1041                 src->data->link(src->data);
1042
1043         return 0;
1044
1045 fail_free_output:
1046         stm_output_free(stm, &src->output);
1047
1048 fail_detach:
1049         mutex_lock(&stm->link_mutex);
1050         spin_lock(&stm->link_lock);
1051         spin_lock(&src->link_lock);
1052
1053         rcu_assign_pointer(src->link, NULL);
1054         list_del_init(&src->link_entry);
1055
1056         spin_unlock(&src->link_lock);
1057         spin_unlock(&stm->link_lock);
1058         mutex_unlock(&stm->link_mutex);
1059
1060         return err;
1061 }
1062
1063 /**
1064  * __stm_source_link_drop() - detach stm_source from an stm device
1065  * @src:        stm_source device
1066  * @stm:        stm device
1067  *
1068  * If @stm is @src::link, disconnect them from one another and put the
1069  * reference on the @stm device.
1070  *
1071  * Caller must hold stm::link_mutex.
1072  */
1073 static int __stm_source_link_drop(struct stm_source_device *src,
1074                                   struct stm_device *stm)
1075 {
1076         struct stm_device *link;
1077         int ret = 0;
1078
1079         lockdep_assert_held(&stm->link_mutex);
1080
1081         /* for stm::link_list modification, we hold both mutex and spinlock */
1082         spin_lock(&stm->link_lock);
1083         spin_lock(&src->link_lock);
1084         link = srcu_dereference_check(src->link, &stm_source_srcu, 1);
1085
1086         /*
1087          * The linked device may have changed since we last looked, because
1088          * we weren't holding the src::link_lock back then; if this is the
1089          * case, tell the caller to retry.
1090          */
1091         if (link != stm) {
1092                 ret = -EAGAIN;
1093                 goto unlock;
1094         }
1095
1096         stm_output_free(link, &src->output);
1097         list_del_init(&src->link_entry);
1098         pm_runtime_mark_last_busy(&link->dev);
1099         pm_runtime_put_autosuspend(&link->dev);
1100         /* matches stm_find_device() from stm_source_link_store() */
1101         stm_put_device(link);
1102         rcu_assign_pointer(src->link, NULL);
1103
1104 unlock:
1105         spin_unlock(&src->link_lock);
1106         spin_unlock(&stm->link_lock);
1107
1108         /*
1109          * Call the unlink callbacks for both source and stm, when we know
1110          * that we have actually performed the unlinking.
1111          */
1112         if (!ret) {
1113                 if (src->data->unlink)
1114                         src->data->unlink(src->data);
1115
1116                 if (stm->data->unlink)
1117                         stm->data->unlink(stm->data, src->output.master,
1118                                           src->output.channel);
1119         }
1120
1121         return ret;
1122 }
1123
1124 /**
1125  * stm_source_link_drop() - detach stm_source from its stm device
1126  * @src:        stm_source device
1127  *
1128  * Unlinking means disconnecting from source's STM device; after this
1129  * writes will be unsuccessful until it is linked to a new STM device.
1130  *
1131  * This will happen on "stm_source_link" sysfs attribute write to undo
1132  * the existing link (if any), or on linked STM device's de-registration.
1133  */
1134 static void stm_source_link_drop(struct stm_source_device *src)
1135 {
1136         struct stm_device *stm;
1137         int idx, ret;
1138
1139 retry:
1140         idx = srcu_read_lock(&stm_source_srcu);
1141         /*
1142          * The stm device will be valid for the duration of this
1143          * read section, but the link may change before we grab
1144          * the src::link_lock in __stm_source_link_drop().
1145          */
1146         stm = srcu_dereference(src->link, &stm_source_srcu);
1147
1148         ret = 0;
1149         if (stm) {
1150                 mutex_lock(&stm->link_mutex);
1151                 ret = __stm_source_link_drop(src, stm);
1152                 mutex_unlock(&stm->link_mutex);
1153         }
1154
1155         srcu_read_unlock(&stm_source_srcu, idx);
1156
1157         /* if it did change, retry */
1158         if (ret == -EAGAIN)
1159                 goto retry;
1160 }
1161
1162 static ssize_t stm_source_link_show(struct device *dev,
1163                                     struct device_attribute *attr,
1164                                     char *buf)
1165 {
1166         struct stm_source_device *src = to_stm_source_device(dev);
1167         struct stm_device *stm;
1168         int idx, ret;
1169
1170         idx = srcu_read_lock(&stm_source_srcu);
1171         stm = srcu_dereference(src->link, &stm_source_srcu);
1172         ret = sprintf(buf, "%s\n",
1173                       stm ? dev_name(&stm->dev) : "<none>");
1174         srcu_read_unlock(&stm_source_srcu, idx);
1175
1176         return ret;
1177 }
1178
1179 static ssize_t stm_source_link_store(struct device *dev,
1180                                      struct device_attribute *attr,
1181                                      const char *buf, size_t count)
1182 {
1183         struct stm_source_device *src = to_stm_source_device(dev);
1184         struct stm_device *link;
1185         int err;
1186
1187         stm_source_link_drop(src);
1188
1189         link = stm_find_device(buf);
1190         if (!link)
1191                 return -EINVAL;
1192
1193         pm_runtime_get(&link->dev);
1194
1195         err = stm_source_link_add(src, link);
1196         if (err) {
1197                 pm_runtime_put_autosuspend(&link->dev);
1198                 /* matches the stm_find_device() above */
1199                 stm_put_device(link);
1200         }
1201
1202         return err ? : count;
1203 }
1204
1205 static DEVICE_ATTR_RW(stm_source_link);
1206
1207 static struct attribute *stm_source_attrs[] = {
1208         &dev_attr_stm_source_link.attr,
1209         NULL,
1210 };
1211
1212 ATTRIBUTE_GROUPS(stm_source);
1213
1214 static struct class stm_source_class = {
1215         .name           = "stm_source",
1216         .dev_groups     = stm_source_groups,
1217 };
1218
1219 static void stm_source_device_release(struct device *dev)
1220 {
1221         struct stm_source_device *src = to_stm_source_device(dev);
1222
1223         kfree(src);
1224 }
1225
1226 /**
1227  * stm_source_register_device() - register an stm_source device
1228  * @parent:     parent device
1229  * @data:       device description structure
1230  *
1231  * This will create a device of stm_source class that can write
1232  * data to an stm device once linked.
1233  *
1234  * Return:      0 on success, -errno otherwise.
1235  */
1236 int stm_source_register_device(struct device *parent,
1237                                struct stm_source_data *data)
1238 {
1239         struct stm_source_device *src;
1240         int err;
1241
1242         if (!stm_core_up)
1243                 return -EPROBE_DEFER;
1244
1245         src = kzalloc(sizeof(*src), GFP_KERNEL);
1246         if (!src)
1247                 return -ENOMEM;
1248
1249         device_initialize(&src->dev);
1250         src->dev.class = &stm_source_class;
1251         src->dev.parent = parent;
1252         src->dev.release = stm_source_device_release;
1253
1254         err = kobject_set_name(&src->dev.kobj, "%s", data->name);
1255         if (err)
1256                 goto err;
1257
1258         pm_runtime_no_callbacks(&src->dev);
1259         pm_runtime_forbid(&src->dev);
1260
1261         err = device_add(&src->dev);
1262         if (err)
1263                 goto err;
1264
1265         stm_output_init(&src->output);
1266         spin_lock_init(&src->link_lock);
1267         INIT_LIST_HEAD(&src->link_entry);
1268         src->data = data;
1269         data->src = src;
1270
1271         return 0;
1272
1273 err:
1274         put_device(&src->dev);
1275         kfree(src);
1276
1277         return err;
1278 }
1279 EXPORT_SYMBOL_GPL(stm_source_register_device);
1280
1281 /**
1282  * stm_source_unregister_device() - unregister an stm_source device
1283  * @data:       device description that was used to register the device
1284  *
1285  * This will remove a previously created stm_source device from the system.
1286  */
1287 void stm_source_unregister_device(struct stm_source_data *data)
1288 {
1289         struct stm_source_device *src = data->src;
1290
1291         stm_source_link_drop(src);
1292
1293         device_unregister(&src->dev);
1294 }
1295 EXPORT_SYMBOL_GPL(stm_source_unregister_device);
1296
1297 int notrace stm_source_write(struct stm_source_data *data,
1298                              unsigned int chan,
1299                              const char *buf, size_t count)
1300 {
1301         struct stm_source_device *src = data->src;
1302         struct stm_device *stm;
1303         int idx;
1304
1305         if (!src->output.nr_chans)
1306                 return -ENODEV;
1307
1308         if (chan >= src->output.nr_chans)
1309                 return -EINVAL;
1310
1311         idx = srcu_read_lock(&stm_source_srcu);
1312
1313         stm = srcu_dereference(src->link, &stm_source_srcu);
1314         if (stm)
1315                 count = stm_write(stm, &src->output, chan, buf, count);
1316         else
1317                 count = -ENODEV;
1318
1319         srcu_read_unlock(&stm_source_srcu, idx);
1320
1321         return count;
1322 }
1323 EXPORT_SYMBOL_GPL(stm_source_write);
1324
1325 static int __init stm_core_init(void)
1326 {
1327         int err;
1328
1329         err = class_register(&stm_class);
1330         if (err)
1331                 return err;
1332
1333         err = class_register(&stm_source_class);
1334         if (err)
1335                 goto err_stm;
1336
1337         err = stp_configfs_init();
1338         if (err)
1339                 goto err_src;
1340
1341         init_srcu_struct(&stm_source_srcu);
1342         INIT_LIST_HEAD(&stm_pdrv_head);
1343         mutex_init(&stm_pdrv_mutex);
1344
1345         /*
1346          * So as to not confuse existing users with a requirement
1347          * to load yet another module, do it here.
1348          */
1349         if (IS_ENABLED(CONFIG_STM_PROTO_BASIC))
1350                 (void)request_module_nowait("stm_p_basic");
1351         stm_core_up++;
1352
1353         return 0;
1354
1355 err_src:
1356         class_unregister(&stm_source_class);
1357 err_stm:
1358         class_unregister(&stm_class);
1359
1360         return err;
1361 }
1362
1363 module_init(stm_core_init);
1364
1365 static void __exit stm_core_exit(void)
1366 {
1367         cleanup_srcu_struct(&stm_source_srcu);
1368         class_unregister(&stm_source_class);
1369         class_unregister(&stm_class);
1370         stp_configfs_exit();
1371 }
1372
1373 module_exit(stm_core_exit);
1374
1375 MODULE_LICENSE("GPL v2");
1376 MODULE_DESCRIPTION("System Trace Module device class");
1377 MODULE_AUTHOR("Alexander Shishkin <[email protected]>");