drivers/hwtracing/stm/core.c

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