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