core.c 30.6 KB
Newer Older
1
// SPDX-License-Identifier: GPL-2.0
2 3 4 5 6 7 8 9
/*
 * System Trace Module (STM) infrastructure
 * Copyright (c) 2014, Intel Corporation.
 *
 * STM class implements generic infrastructure for  System Trace Module devices
 * as defined in MIPI STPv2 specification.
 */

10
#include <linux/pm_runtime.h>
11 12 13 14 15 16 17 18 19 20 21
#include <linux/uaccess.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/device.h>
#include <linux/compat.h>
#include <linux/kdev_t.h>
#include <linux/srcu.h>
#include <linux/slab.h>
#include <linux/stm.h>
#include <linux/fs.h>
#include <linux/mm.h>
22
#include <linux/vmalloc.h>
23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63
#include "stm.h"

#include <uapi/linux/stm.h>

static unsigned int stm_core_up;

/*
 * The SRCU here makes sure that STM device doesn't disappear from under a
 * stm_source_write() caller, which may want to have as little overhead as
 * possible.
 */
static struct srcu_struct stm_source_srcu;

static ssize_t masters_show(struct device *dev,
			    struct device_attribute *attr,
			    char *buf)
{
	struct stm_device *stm = to_stm_device(dev);
	int ret;

	ret = sprintf(buf, "%u %u\n", stm->data->sw_start, stm->data->sw_end);

	return ret;
}

static DEVICE_ATTR_RO(masters);

static ssize_t channels_show(struct device *dev,
			     struct device_attribute *attr,
			     char *buf)
{
	struct stm_device *stm = to_stm_device(dev);
	int ret;

	ret = sprintf(buf, "%u\n", stm->data->sw_nchannels);

	return ret;
}

static DEVICE_ATTR_RO(channels);

64 65 66 67 68 69 70 71 72 73 74 75 76 77
static ssize_t hw_override_show(struct device *dev,
				struct device_attribute *attr,
				char *buf)
{
	struct stm_device *stm = to_stm_device(dev);
	int ret;

	ret = sprintf(buf, "%u\n", stm->data->hw_override);

	return ret;
}

static DEVICE_ATTR_RO(hw_override);

78 79 80
static struct attribute *stm_attrs[] = {
	&dev_attr_masters.attr,
	&dev_attr_channels.attr,
81
	&dev_attr_hw_override.attr,
82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124
	NULL,
};

ATTRIBUTE_GROUPS(stm);

static struct class stm_class = {
	.name		= "stm",
	.dev_groups	= stm_groups,
};

static int stm_dev_match(struct device *dev, const void *data)
{
	const char *name = data;

	return sysfs_streq(name, dev_name(dev));
}

/**
 * stm_find_device() - find stm device by name
 * @buf:	character buffer containing the name
 *
 * This is called when either policy gets assigned to an stm device or an
 * stm_source device gets linked to an stm device.
 *
 * This grabs device's reference (get_device()) and module reference, both
 * of which the calling path needs to make sure to drop with stm_put_device().
 *
 * Return:	stm device pointer or null if lookup failed.
 */
struct stm_device *stm_find_device(const char *buf)
{
	struct stm_device *stm;
	struct device *dev;

	if (!stm_core_up)
		return NULL;

	dev = class_find_device(&stm_class, NULL, buf, stm_dev_match);
	if (!dev)
		return NULL;

	stm = to_stm_device(dev);
	if (!try_module_get(stm->owner)) {
125
		/* matches class_find_device() above */
126 127 128 129 130 131 132 133 134 135 136 137
		put_device(dev);
		return NULL;
	}

	return stm;
}

/**
 * stm_put_device() - drop references on the stm device
 * @stm:	stm device, previously acquired by stm_find_device()
 *
 * This drops the module reference and device reference taken by
138
 * stm_find_device() or stm_char_open().
139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197
 */
void stm_put_device(struct stm_device *stm)
{
	module_put(stm->owner);
	put_device(&stm->dev);
}

/*
 * Internally we only care about software-writable masters here, that is the
 * ones in the range [stm_data->sw_start..stm_data..sw_end], however we need
 * original master numbers to be visible externally, since they are the ones
 * that will appear in the STP stream. Thus, the internal bookkeeping uses
 * $master - stm_data->sw_start to reference master descriptors and such.
 */

#define __stm_master(_s, _m)				\
	((_s)->masters[(_m) - (_s)->data->sw_start])

static inline struct stp_master *
stm_master(struct stm_device *stm, unsigned int idx)
{
	if (idx < stm->data->sw_start || idx > stm->data->sw_end)
		return NULL;

	return __stm_master(stm, idx);
}

static int stp_master_alloc(struct stm_device *stm, unsigned int idx)
{
	struct stp_master *master;
	size_t size;

	size = ALIGN(stm->data->sw_nchannels, 8) / 8;
	size += sizeof(struct stp_master);
	master = kzalloc(size, GFP_ATOMIC);
	if (!master)
		return -ENOMEM;

	master->nr_free = stm->data->sw_nchannels;
	__stm_master(stm, idx) = master;

	return 0;
}

static void stp_master_free(struct stm_device *stm, unsigned int idx)
{
	struct stp_master *master = stm_master(stm, idx);

	if (!master)
		return;

	__stm_master(stm, idx) = NULL;
	kfree(master);
}

static void stm_output_claim(struct stm_device *stm, struct stm_output *output)
{
	struct stp_master *master = stm_master(stm, output->master);

198 199 200
	lockdep_assert_held(&stm->mc_lock);
	lockdep_assert_held(&output->lock);

201 202 203 204 205 206 207 208 209 210 211 212 213 214
	if (WARN_ON_ONCE(master->nr_free < output->nr_chans))
		return;

	bitmap_allocate_region(&master->chan_map[0], output->channel,
			       ilog2(output->nr_chans));

	master->nr_free -= output->nr_chans;
}

static void
stm_output_disclaim(struct stm_device *stm, struct stm_output *output)
{
	struct stp_master *master = stm_master(stm, output->master);

215 216 217
	lockdep_assert_held(&stm->mc_lock);
	lockdep_assert_held(&output->lock);

218 219 220 221
	bitmap_release_region(&master->chan_map[0], output->channel,
			      ilog2(output->nr_chans));

	master->nr_free += output->nr_chans;
222
	output->nr_chans = 0;
223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246
}

/*
 * This is like bitmap_find_free_region(), except it can ignore @start bits
 * at the beginning.
 */
static int find_free_channels(unsigned long *bitmap, unsigned int start,
			      unsigned int end, unsigned int width)
{
	unsigned int pos;
	int i;

	for (pos = start; pos < end + 1; pos = ALIGN(pos, width)) {
		pos = find_next_zero_bit(bitmap, end + 1, pos);
		if (pos + width > end + 1)
			break;

		if (pos & (width - 1))
			continue;

		for (i = 1; i < width && !test_bit(pos + i, bitmap); i++)
			;
		if (i == width)
			return pos;
247 248 249

		/* step over [pos..pos+i) to continue search */
		pos += i;
250 251 252 253 254
	}

	return -1;
}

255
static int
256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298
stm_find_master_chan(struct stm_device *stm, unsigned int width,
		     unsigned int *mstart, unsigned int mend,
		     unsigned int *cstart, unsigned int cend)
{
	struct stp_master *master;
	unsigned int midx;
	int pos, err;

	for (midx = *mstart; midx <= mend; midx++) {
		if (!stm_master(stm, midx)) {
			err = stp_master_alloc(stm, midx);
			if (err)
				return err;
		}

		master = stm_master(stm, midx);

		if (!master->nr_free)
			continue;

		pos = find_free_channels(master->chan_map, *cstart, cend,
					 width);
		if (pos < 0)
			continue;

		*mstart = midx;
		*cstart = pos;
		return 0;
	}

	return -ENOSPC;
}

static int stm_output_assign(struct stm_device *stm, unsigned int width,
			     struct stp_policy_node *policy_node,
			     struct stm_output *output)
{
	unsigned int midx, cidx, mend, cend;
	int ret = -EINVAL;

	if (width > stm->data->sw_nchannels)
		return -EINVAL;

299 300 301 302 303 304 305 306 307
	/* We no longer accept policy_node==NULL here */
	if (WARN_ON_ONCE(!policy_node))
		return -EINVAL;

	/*
	 * Also, the caller holds reference to policy_node, so it won't
	 * disappear on us.
	 */
	stp_policy_node_get_ranges(policy_node, &midx, &mend, &cidx, &cend);
308 309

	spin_lock(&stm->mc_lock);
310
	spin_lock(&output->lock);
311 312 313 314 315
	/* output is already assigned -- shouldn't happen */
	if (WARN_ON_ONCE(output->nr_chans))
		goto unlock;

	ret = stm_find_master_chan(stm, width, &midx, mend, &cidx, cend);
316
	if (ret < 0)
317 318 319 320 321
		goto unlock;

	output->master = midx;
	output->channel = cidx;
	output->nr_chans = width;
322 323 324 325 326 327 328 329 330 331 332 333
	if (stm->pdrv->output_open) {
		void *priv = stp_policy_node_priv(policy_node);

		if (WARN_ON_ONCE(!priv))
			goto unlock;

		/* configfs subsys mutex is held by the caller */
		ret = stm->pdrv->output_open(priv, output);
		if (ret)
			goto unlock;
	}

334 335 336 337 338
	stm_output_claim(stm, output);
	dev_dbg(&stm->dev, "assigned %u:%u (+%u)\n", midx, cidx, width);

	ret = 0;
unlock:
339 340 341
	if (ret)
		output->nr_chans = 0;

342
	spin_unlock(&output->lock);
343 344 345 346 347 348 349 350
	spin_unlock(&stm->mc_lock);

	return ret;
}

static void stm_output_free(struct stm_device *stm, struct stm_output *output)
{
	spin_lock(&stm->mc_lock);
351
	spin_lock(&output->lock);
352 353
	if (output->nr_chans)
		stm_output_disclaim(stm, output);
354 355
	if (stm->pdrv && stm->pdrv->output_close)
		stm->pdrv->output_close(output);
356
	spin_unlock(&output->lock);
357 358 359
	spin_unlock(&stm->mc_lock);
}

360 361 362 363 364
static void stm_output_init(struct stm_output *output)
{
	spin_lock_init(&output->lock);
}

365 366 367 368 369 370 371
static int major_match(struct device *dev, const void *data)
{
	unsigned int major = *(unsigned int *)data;

	return MAJOR(dev->devt) == major;
}

372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492
/*
 * Framing protocol management
 * Modules can implement STM protocol drivers and (un-)register them
 * with the STM class framework.
 */
static struct list_head stm_pdrv_head;
static struct mutex stm_pdrv_mutex;

struct stm_pdrv_entry {
	struct list_head			entry;
	const struct stm_protocol_driver	*pdrv;
	const struct config_item_type		*node_type;
};

static const struct stm_pdrv_entry *
__stm_lookup_protocol(const char *name)
{
	struct stm_pdrv_entry *pe;

	/*
	 * If no name is given (NULL or ""), fall back to "p_basic".
	 */
	if (!name || !*name)
		name = "p_basic";

	list_for_each_entry(pe, &stm_pdrv_head, entry) {
		if (!strcmp(name, pe->pdrv->name))
			return pe;
	}

	return NULL;
}

int stm_register_protocol(const struct stm_protocol_driver *pdrv)
{
	struct stm_pdrv_entry *pe = NULL;
	int ret = -ENOMEM;

	mutex_lock(&stm_pdrv_mutex);

	if (__stm_lookup_protocol(pdrv->name)) {
		ret = -EEXIST;
		goto unlock;
	}

	pe = kzalloc(sizeof(*pe), GFP_KERNEL);
	if (!pe)
		goto unlock;

	if (pdrv->policy_attr) {
		pe->node_type = get_policy_node_type(pdrv->policy_attr);
		if (!pe->node_type)
			goto unlock;
	}

	list_add_tail(&pe->entry, &stm_pdrv_head);
	pe->pdrv = pdrv;

	ret = 0;
unlock:
	mutex_unlock(&stm_pdrv_mutex);

	if (ret)
		kfree(pe);

	return ret;
}
EXPORT_SYMBOL_GPL(stm_register_protocol);

void stm_unregister_protocol(const struct stm_protocol_driver *pdrv)
{
	struct stm_pdrv_entry *pe, *iter;

	mutex_lock(&stm_pdrv_mutex);

	list_for_each_entry_safe(pe, iter, &stm_pdrv_head, entry) {
		if (pe->pdrv == pdrv) {
			list_del(&pe->entry);

			if (pe->node_type) {
				kfree(pe->node_type->ct_attrs);
				kfree(pe->node_type);
			}
			kfree(pe);
			break;
		}
	}

	mutex_unlock(&stm_pdrv_mutex);
}
EXPORT_SYMBOL_GPL(stm_unregister_protocol);

static bool stm_get_protocol(const struct stm_protocol_driver *pdrv)
{
	return try_module_get(pdrv->owner);
}

void stm_put_protocol(const struct stm_protocol_driver *pdrv)
{
	module_put(pdrv->owner);
}

int stm_lookup_protocol(const char *name,
			const struct stm_protocol_driver **pdrv,
			const struct config_item_type **node_type)
{
	const struct stm_pdrv_entry *pe;

	mutex_lock(&stm_pdrv_mutex);

	pe = __stm_lookup_protocol(name);
	if (pe && pe->pdrv && stm_get_protocol(pe->pdrv)) {
		*pdrv = pe->pdrv;
		*node_type = pe->node_type;
	}

	mutex_unlock(&stm_pdrv_mutex);

	return pe ? 0 : -ENOENT;
}

493 494 495 496 497
static int stm_char_open(struct inode *inode, struct file *file)
{
	struct stm_file *stmf;
	struct device *dev;
	unsigned int major = imajor(inode);
498
	int err = -ENOMEM;
499 500 501 502 503 504 505

	dev = class_find_device(&stm_class, NULL, &major, major_match);
	if (!dev)
		return -ENODEV;

	stmf = kzalloc(sizeof(*stmf), GFP_KERNEL);
	if (!stmf)
506
		goto err_put_device;
507

508
	err = -ENODEV;
509
	stm_output_init(&stmf->output);
510 511 512 513 514 515 516 517 518 519
	stmf->stm = to_stm_device(dev);

	if (!try_module_get(stmf->stm->owner))
		goto err_free;

	file->private_data = stmf;

	return nonseekable_open(inode, file);

err_free:
520 521
	kfree(stmf);
err_put_device:
522 523
	/* matches class_find_device() above */
	put_device(dev);
524 525 526 527 528 529 530

	return err;
}

static int stm_char_release(struct inode *inode, struct file *file)
{
	struct stm_file *stmf = file->private_data;
531 532 533 534 535
	struct stm_device *stm = stmf->stm;

	if (stm->data->unlink)
		stm->data->unlink(stm->data, stmf->output.master,
				  stmf->output.channel);
536

537
	stm_output_free(stm, &stmf->output);
538 539 540 541 542

	/*
	 * matches the stm_char_open()'s
	 * class_find_device() + try_module_get()
	 */
543
	stm_put_device(stm);
544 545 546 547 548
	kfree(stmf);

	return 0;
}

549 550 551
static int
stm_assign_first_policy(struct stm_device *stm, struct stm_output *output,
			char **ids, unsigned int width)
552
{
553 554
	struct stp_policy_node *pn;
	int err, n;
555

556 557 558 559 560 561 562 563
	/*
	 * On success, stp_policy_node_lookup() will return holding the
	 * configfs subsystem mutex, which is then released in
	 * stp_policy_node_put(). This allows the pdrv->output_open() in
	 * stm_output_assign() to serialize against the attribute accessors.
	 */
	for (n = 0, pn = NULL; ids[n] && !pn; n++)
		pn = stp_policy_node_lookup(stm, ids[n]);
564

565 566
	if (!pn)
		return -EINVAL;
567

568
	err = stm_output_assign(stm, width, pn, output);
569

570 571 572
	stp_policy_node_put(pn);

	return err;
573 574
}

575 576 577 578 579 580 581 582 583 584 585 586
/**
 * stm_data_write() - send the given payload as data packets
 * @data:	stm driver's data
 * @m:		STP master
 * @c:		STP channel
 * @ts_first:	timestamp the first packet
 * @buf:	data payload buffer
 * @count:	data payload size
 */
ssize_t notrace stm_data_write(struct stm_data *data, unsigned int m,
			       unsigned int c, bool ts_first, const void *buf,
			       size_t count)
587
{
588
	unsigned int flags = ts_first ? STP_PACKET_TIMESTAMPED : 0;
589
	ssize_t sz;
590
	size_t pos;
591

592
	for (pos = 0, sz = 0; pos < count; pos += sz) {
593
		sz = min_t(unsigned int, count - pos, 8);
594 595 596
		sz = data->packet(data, m, c, STP_PACKET_DATA, flags, sz,
				  &((u8 *)buf)[pos]);
		if (sz <= 0)
597
			break;
598 599 600 601 602

		if (ts_first) {
			flags = 0;
			ts_first = false;
		}
603 604
	}

605 606 607 608
	return sz < 0 ? sz : pos;
}
EXPORT_SYMBOL_GPL(stm_data_write);

609 610 611
static ssize_t notrace
stm_write(struct stm_device *stm, struct stm_output *output,
	  unsigned int chan, const char *buf, size_t count)
612
{
613 614 615 616 617
	int err;

	/* stm->pdrv is serialized against policy_mutex */
	if (!stm->pdrv)
		return -ENODEV;
618

619 620 621
	err = stm->pdrv->write(stm->data, output, chan, buf, count);
	if (err < 0)
		return err;
622

623
	return err;
624 625 626 627 628 629 630 631 632 633
}

static ssize_t stm_char_write(struct file *file, const char __user *buf,
			      size_t count, loff_t *ppos)
{
	struct stm_file *stmf = file->private_data;
	struct stm_device *stm = stmf->stm;
	char *kbuf;
	int err;

634 635 636
	if (count + 1 > PAGE_SIZE)
		count = PAGE_SIZE - 1;

637
	/*
638 639
	 * If no m/c have been assigned to this writer up to this
	 * point, try to use the task name and "default" policy entries.
640 641
	 */
	if (!stmf->output.nr_chans) {
642 643 644 645 646 647
		char comm[sizeof(current->comm)];
		char *ids[] = { comm, "default", NULL };

		get_task_comm(comm, current);

		err = stm_assign_first_policy(stmf->stm, &stmf->output, ids, 1);
648 649 650 651
		/*
		 * EBUSY means that somebody else just assigned this
		 * output, which is just fine for write()
		 */
652
		if (err)
653 654 655 656 657 658 659 660 661 662 663 664 665
			return err;
	}

	kbuf = kmalloc(count + 1, GFP_KERNEL);
	if (!kbuf)
		return -ENOMEM;

	err = copy_from_user(kbuf, buf, count);
	if (err) {
		kfree(kbuf);
		return -EFAULT;
	}

666 667
	pm_runtime_get_sync(&stm->dev);

668
	count = stm_write(stm, &stmf->output, 0, kbuf, count);
669

670 671
	pm_runtime_mark_last_busy(&stm->dev);
	pm_runtime_put_autosuspend(&stm->dev);
672 673 674 675 676
	kfree(kbuf);

	return count;
}

677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698
static void stm_mmap_open(struct vm_area_struct *vma)
{
	struct stm_file *stmf = vma->vm_file->private_data;
	struct stm_device *stm = stmf->stm;

	pm_runtime_get(&stm->dev);
}

static void stm_mmap_close(struct vm_area_struct *vma)
{
	struct stm_file *stmf = vma->vm_file->private_data;
	struct stm_device *stm = stmf->stm;

	pm_runtime_mark_last_busy(&stm->dev);
	pm_runtime_put_autosuspend(&stm->dev);
}

static const struct vm_operations_struct stm_mmap_vmops = {
	.open	= stm_mmap_open,
	.close	= stm_mmap_close,
};

699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722
static int stm_char_mmap(struct file *file, struct vm_area_struct *vma)
{
	struct stm_file *stmf = file->private_data;
	struct stm_device *stm = stmf->stm;
	unsigned long size, phys;

	if (!stm->data->mmio_addr)
		return -EOPNOTSUPP;

	if (vma->vm_pgoff)
		return -EINVAL;

	size = vma->vm_end - vma->vm_start;

	if (stmf->output.nr_chans * stm->data->sw_mmiosz != size)
		return -EINVAL;

	phys = stm->data->mmio_addr(stm->data, stmf->output.master,
				    stmf->output.channel,
				    stmf->output.nr_chans);

	if (!phys)
		return -EINVAL;

723 724
	pm_runtime_get_sync(&stm->dev);

725 726
	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
727
	vma->vm_ops = &stm_mmap_vmops;
728 729 730 731 732 733 734 735 736
	vm_iomap_memory(vma, phys, size);

	return 0;
}

static int stm_char_policy_set_ioctl(struct stm_file *stmf, void __user *arg)
{
	struct stm_device *stm = stmf->stm;
	struct stp_policy_id *id;
737
	char *ids[] = { NULL, NULL };
738
	int ret = -EINVAL, wlimit = 1;
739 740 741 742 743 744 745 746
	u32 size;

	if (stmf->output.nr_chans)
		return -EBUSY;

	if (copy_from_user(&size, arg, sizeof(size)))
		return -EFAULT;

747
	if (size < sizeof(*id) || size >= PATH_MAX + sizeof(*id))
748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765
		return -EINVAL;

	/*
	 * size + 1 to make sure the .id string at the bottom is terminated,
	 * which is also why memdup_user() is not useful here
	 */
	id = kzalloc(size + 1, GFP_KERNEL);
	if (!id)
		return -ENOMEM;

	if (copy_from_user(id, arg, size)) {
		ret = -EFAULT;
		goto err_free;
	}

	if (id->__reserved_0 || id->__reserved_1)
		goto err_free;

766 767 768 769
	if (stm->data->sw_mmiosz)
		wlimit = PAGE_SIZE / stm->data->sw_mmiosz;

	if (id->width < 1 || id->width > wlimit)
770 771
		goto err_free;

772 773 774
	ids[0] = id->id;
	ret = stm_assign_first_policy(stmf->stm, &stmf->output, ids,
				      id->width);
775 776 777 778 779 780 781
	if (ret)
		goto err_free;

	if (stm->data->link)
		ret = stm->data->link(stm->data, stmf->output.master,
				      stmf->output.channel);

782
	if (ret)
783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866
		stm_output_free(stmf->stm, &stmf->output);

err_free:
	kfree(id);

	return ret;
}

static int stm_char_policy_get_ioctl(struct stm_file *stmf, void __user *arg)
{
	struct stp_policy_id id = {
		.size		= sizeof(id),
		.master		= stmf->output.master,
		.channel	= stmf->output.channel,
		.width		= stmf->output.nr_chans,
		.__reserved_0	= 0,
		.__reserved_1	= 0,
	};

	return copy_to_user(arg, &id, id.size) ? -EFAULT : 0;
}

static long
stm_char_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
	struct stm_file *stmf = file->private_data;
	struct stm_data *stm_data = stmf->stm->data;
	int err = -ENOTTY;
	u64 options;

	switch (cmd) {
	case STP_POLICY_ID_SET:
		err = stm_char_policy_set_ioctl(stmf, (void __user *)arg);
		if (err)
			return err;

		return stm_char_policy_get_ioctl(stmf, (void __user *)arg);

	case STP_POLICY_ID_GET:
		return stm_char_policy_get_ioctl(stmf, (void __user *)arg);

	case STP_SET_OPTIONS:
		if (copy_from_user(&options, (u64 __user *)arg, sizeof(u64)))
			return -EFAULT;

		if (stm_data->set_options)
			err = stm_data->set_options(stm_data,
						    stmf->output.master,
						    stmf->output.channel,
						    stmf->output.nr_chans,
						    options);

		break;
	default:
		break;
	}

	return err;
}

#ifdef CONFIG_COMPAT
static long
stm_char_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
	return stm_char_ioctl(file, cmd, (unsigned long)compat_ptr(arg));
}
#else
#define stm_char_compat_ioctl	NULL
#endif

static const struct file_operations stm_fops = {
	.open		= stm_char_open,
	.release	= stm_char_release,
	.write		= stm_char_write,
	.mmap		= stm_char_mmap,
	.unlocked_ioctl	= stm_char_ioctl,
	.compat_ioctl	= stm_char_compat_ioctl,
	.llseek		= no_llseek,
};

static void stm_device_release(struct device *dev)
{
	struct stm_device *stm = to_stm_device(dev);

867
	vfree(stm);
868 869 870 871 872 873 874 875 876 877 878 879 880 881 882
}

int stm_register_device(struct device *parent, struct stm_data *stm_data,
			struct module *owner)
{
	struct stm_device *stm;
	unsigned int nmasters;
	int err = -ENOMEM;

	if (!stm_core_up)
		return -EPROBE_DEFER;

	if (!stm_data->packet || !stm_data->sw_nchannels)
		return -EINVAL;

883
	nmasters = stm_data->sw_end - stm_data->sw_start + 1;
884
	stm = vzalloc(sizeof(*stm) + nmasters * sizeof(void *));
885 886 887 888 889 890 891 892 893 894 895 896 897
	if (!stm)
		return -ENOMEM;

	stm->major = register_chrdev(0, stm_data->name, &stm_fops);
	if (stm->major < 0)
		goto err_free;

	device_initialize(&stm->dev);
	stm->dev.devt = MKDEV(stm->major, 0);
	stm->dev.class = &stm_class;
	stm->dev.parent = parent;
	stm->dev.release = stm_device_release;

898
	mutex_init(&stm->link_mutex);
899 900 901
	spin_lock_init(&stm->link_lock);
	INIT_LIST_HEAD(&stm->link_list);

902
	/* initialize the object before it is accessible via sysfs */
903 904 905 906 907 908 909
	spin_lock_init(&stm->mc_lock);
	mutex_init(&stm->policy_mutex);
	stm->sw_nmasters = nmasters;
	stm->owner = owner;
	stm->data = stm_data;
	stm_data->stm = stm;

910 911 912 913 914 915 916 917
	err = kobject_set_name(&stm->dev.kobj, "%s", stm_data->name);
	if (err)
		goto err_device;

	err = device_add(&stm->dev);
	if (err)
		goto err_device;

918 919 920 921 922 923 924 925 926 927 928
	/*
	 * Use delayed autosuspend to avoid bouncing back and forth
	 * on recurring character device writes, with the initial
	 * delay time of 2 seconds.
	 */
	pm_runtime_no_callbacks(&stm->dev);
	pm_runtime_use_autosuspend(&stm->dev);
	pm_runtime_set_autosuspend_delay(&stm->dev, 2000);
	pm_runtime_set_suspended(&stm->dev);
	pm_runtime_enable(&stm->dev);

929 930 931
	return 0;

err_device:
932 933
	unregister_chrdev(stm->major, stm_data->name);

934
	/* matches device_initialize() above */
935 936
	put_device(&stm->dev);
err_free:
937
	vfree(stm);
938 939 940 941 942

	return err;
}
EXPORT_SYMBOL_GPL(stm_register_device);

943 944
static int __stm_source_link_drop(struct stm_source_device *src,
				  struct stm_device *stm);
945 946 947 948 949

void stm_unregister_device(struct stm_data *stm_data)
{
	struct stm_device *stm = stm_data->stm;
	struct stm_source_device *src, *iter;
950
	int i, ret;
951

952 953 954
	pm_runtime_dont_use_autosuspend(&stm->dev);
	pm_runtime_disable(&stm->dev);

955
	mutex_lock(&stm->link_mutex);
956
	list_for_each_entry_safe(src, iter, &stm->link_list, link_entry) {
957 958 959 960 961 962 963 964 965
		ret = __stm_source_link_drop(src, stm);
		/*
		 * src <-> stm link must not change under the same
		 * stm::link_mutex, so complain loudly if it has;
		 * also in this situation ret!=0 means this src is
		 * not connected to this stm and it should be otherwise
		 * safe to proceed with the tear-down of stm.
		 */
		WARN_ON_ONCE(ret);
966
	}
967
	mutex_unlock(&stm->link_mutex);
968 969 970 971 972 973 974 975 976 977

	synchronize_srcu(&stm_source_srcu);

	unregister_chrdev(stm->major, stm_data->name);

	mutex_lock(&stm->policy_mutex);
	if (stm->policy)
		stp_policy_unbind(stm->policy);
	mutex_unlock(&stm->policy_mutex);

978
	for (i = stm->data->sw_start; i <= stm->data->sw_end; i++)
979 980 981 982 983 984 985
		stp_master_free(stm, i);

	device_unregister(&stm->dev);
	stm_data->stm = NULL;
}
EXPORT_SYMBOL_GPL(stm_unregister_device);

986 987 988 989 990 991 992 993 994 995 996
/*
 * stm::link_list access serialization uses a spinlock and a mutex; holding
 * either of them guarantees that the list is stable; modification requires
 * holding both of them.
 *
 * Lock ordering is as follows:
 *   stm::link_mutex
 *     stm::link_lock
 *       src::link_lock
 */

997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009
/**
 * stm_source_link_add() - connect an stm_source device to an stm device
 * @src:	stm_source device
 * @stm:	stm device
 *
 * This function establishes a link from stm_source to an stm device so that
 * the former can send out trace data to the latter.
 *
 * Return:	0 on success, -errno otherwise.
 */
static int stm_source_link_add(struct stm_source_device *src,
			       struct stm_device *stm)
{
1010 1011
	char *ids[] = { NULL, "default", NULL };
	int err = -ENOMEM;
1012

1013
	mutex_lock(&stm->link_mutex);
1014 1015 1016 1017 1018 1019 1020 1021 1022
	spin_lock(&stm->link_lock);
	spin_lock(&src->link_lock);

	/* src->link is dereferenced under stm_source_srcu but not the list */
	rcu_assign_pointer(src->link, stm);
	list_add_tail(&src->link_entry, &stm->link_list);

	spin_unlock(&src->link_lock);
	spin_unlock(&stm->link_lock);
1023
	mutex_unlock(&stm->link_mutex);
1024

1025 1026 1027
	ids[0] = kstrdup(src->data->name, GFP_KERNEL);
	if (!ids[0])
		goto fail_detach;
1028

1029 1030 1031
	err = stm_assign_first_policy(stm, &src->output, ids,
				      src->data->nr_chans);
	kfree(ids[0]);
1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053

	if (err)
		goto fail_detach;

	/* this is to notify the STM device that a new link has been made */
	if (stm->data->link)
		err = stm->data->link(stm->data, src->output.master,
				      src->output.channel);

	if (err)
		goto fail_free_output;

	/* this is to let the source carry out all necessary preparations */
	if (src->data->link)
		src->data->link(src->data);

	return 0;

fail_free_output:
	stm_output_free(stm, &src->output);

fail_detach:
1054
	mutex_lock(&stm->link_mutex);
1055 1056 1057 1058 1059 1060 1061 1062
	spin_lock(&stm->link_lock);
	spin_lock(&src->link_lock);

	rcu_assign_pointer(src->link, NULL);
	list_del_init(&src->link_entry);

	spin_unlock(&src->link_lock);
	spin_unlock(&stm->link_lock);
1063
	mutex_unlock(&stm->link_mutex);
1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075

	return err;
}

/**
 * __stm_source_link_drop() - detach stm_source from an stm device
 * @src:	stm_source device
 * @stm:	stm device
 *
 * If @stm is @src::link, disconnect them from one another and put the
 * reference on the @stm device.
 *
1076
 * Caller must hold stm::link_mutex.
1077
 */
1078 1079
static int __stm_source_link_drop(struct stm_source_device *src,
				  struct stm_device *stm)
1080
{
1081
	struct stm_device *link;
1082
	int ret = 0;
1083

1084 1085 1086 1087
	lockdep_assert_held(&stm->link_mutex);

	/* for stm::link_list modification, we hold both mutex and spinlock */
	spin_lock(&stm->link_lock);
1088
	spin_lock(&src->link_lock);
1089
	link = srcu_dereference_check(src->link, &stm_source_srcu, 1);
1090 1091 1092 1093 1094 1095 1096 1097

	/*
	 * The linked device may have changed since we last looked, because
	 * we weren't holding the src::link_lock back then; if this is the
	 * case, tell the caller to retry.
	 */
	if (link != stm) {
		ret = -EAGAIN;
1098
		goto unlock;
1099
	}
1100

1101
	stm_output_free(link, &src->output);
1102
	list_del_init(&src->link_entry);
1103 1104
	pm_runtime_mark_last_busy(&link->dev);
	pm_runtime_put_autosuspend(&link->dev);
1105
	/* matches stm_find_device() from stm_source_link_store() */
1106
	stm_put_device(link);
1107 1108
	rcu_assign_pointer(src->link, NULL);

1109
unlock:
1110
	spin_unlock(&src->link_lock);
1111
	spin_unlock(&stm->link_lock);
1112

1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124
	/*
	 * Call the unlink callbacks for both source and stm, when we know
	 * that we have actually performed the unlinking.
	 */
	if (!ret) {
		if (src->data->unlink)
			src->data->unlink(src->data);

		if (stm->data->unlink)
			stm->data->unlink(stm->data, src->output.master,
					  src->output.channel);
	}
1125 1126

	return ret;
1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141
}

/**
 * stm_source_link_drop() - detach stm_source from its stm device
 * @src:	stm_source device
 *
 * Unlinking means disconnecting from source's STM device; after this
 * writes will be unsuccessful until it is linked to a new STM device.
 *
 * This will happen on "stm_source_link" sysfs attribute write to undo
 * the existing link (if any), or on linked STM device's de-registration.
 */
static void stm_source_link_drop(struct stm_source_device *src)
{
	struct stm_device *stm;
1142
	int idx, ret;
1143

1144
retry:
1145
	idx = srcu_read_lock(&stm_source_srcu);
1146 1147 1148 1149 1150
	/*
	 * The stm device will be valid for the duration of this
	 * read section, but the link may change before we grab
	 * the src::link_lock in __stm_source_link_drop().
	 */
1151 1152
	stm = srcu_dereference(src->link, &stm_source_srcu);

1153
	ret = 0;
1154
	if (stm) {
1155
		mutex_lock(&stm->link_mutex);
1156
		ret = __stm_source_link_drop(src, stm);
1157
		mutex_unlock(&stm->link_mutex);
1158 1159 1160
	}

	srcu_read_unlock(&stm_source_srcu, idx);
1161 1162 1163 1164

	/* if it did change, retry */
	if (ret == -EAGAIN)
		goto retry;
1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197
}

static ssize_t stm_source_link_show(struct device *dev,
				    struct device_attribute *attr,
				    char *buf)
{
	struct stm_source_device *src = to_stm_source_device(dev);
	struct stm_device *stm;
	int idx, ret;

	idx = srcu_read_lock(&stm_source_srcu);
	stm = srcu_dereference(src->link, &stm_source_srcu);
	ret = sprintf(buf, "%s\n",
		      stm ? dev_name(&stm->dev) : "<none>");
	srcu_read_unlock(&stm_source_srcu, idx);

	return ret;
}

static ssize_t stm_source_link_store(struct device *dev,
				     struct device_attribute *attr,
				     const char *buf, size_t count)
{
	struct stm_source_device *src = to_stm_source_device(dev);
	struct stm_device *link;
	int err;

	stm_source_link_drop(src);

	link = stm_find_device(buf);
	if (!link)
		return -EINVAL;

1198 1199
	pm_runtime_get(&link->dev);

1200
	err = stm_source_link_add(src, link);
1201
	if (err) {
1202
		pm_runtime_put_autosuspend(&link->dev);
1203
		/* matches the stm_find_device() above */
1204
		stm_put_device(link);
1205
	}
1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262

	return err ? : count;
}

static DEVICE_ATTR_RW(stm_source_link);

static struct attribute *stm_source_attrs[] = {
	&dev_attr_stm_source_link.attr,
	NULL,
};

ATTRIBUTE_GROUPS(stm_source);

static struct class stm_source_class = {
	.name		= "stm_source",
	.dev_groups	= stm_source_groups,
};

static void stm_source_device_release(struct device *dev)
{
	struct stm_source_device *src = to_stm_source_device(dev);

	kfree(src);
}

/**
 * stm_source_register_device() - register an stm_source device
 * @parent:	parent device
 * @data:	device description structure
 *
 * This will create a device of stm_source class that can write
 * data to an stm device once linked.
 *
 * Return:	0 on success, -errno otherwise.
 */
int stm_source_register_device(struct device *parent,
			       struct stm_source_data *data)
{
	struct stm_source_device *src;
	int err;

	if (!stm_core_up)
		return -EPROBE_DEFER;

	src = kzalloc(sizeof(*src), GFP_KERNEL);
	if (!src)
		return -ENOMEM;

	device_initialize(&src->dev);
	src->dev.class = &stm_source_class;
	src->dev.parent = parent;
	src->dev.release = stm_source_device_release;

	err = kobject_set_name(&src->dev.kobj, "%s", data->name);
	if (err)
		goto err;

1263 1264 1265
	pm_runtime_no_callbacks(&src->dev);
	pm_runtime_forbid(&src->dev);

1266 1267 1268 1269
	err = device_add(&src->dev);
	if (err)
		goto err;

1270
	stm_output_init(&src->output);
1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290 1291 1292 1293 1294 1295 1296 1297
	spin_lock_init(&src->link_lock);
	INIT_LIST_HEAD(&src->link_entry);
	src->data = data;
	data->src = src;

	return 0;

err:
	put_device(&src->dev);
	kfree(src);

	return err;
}
EXPORT_SYMBOL_GPL(stm_source_register_device);

/**
 * stm_source_unregister_device() - unregister an stm_source device
 * @data:	device description that was used to register the device
 *
 * This will remove a previously created stm_source device from the system.
 */
void stm_source_unregister_device(struct stm_source_data *data)
{
	struct stm_source_device *src = data->src;

	stm_source_link_drop(src);

1298
	device_unregister(&src->dev);
1299 1300 1301
}
EXPORT_SYMBOL_GPL(stm_source_unregister_device);

1302 1303 1304
int notrace stm_source_write(struct stm_source_data *data,
			     unsigned int chan,
			     const char *buf, size_t count)
1305 1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319
{
	struct stm_source_device *src = data->src;
	struct stm_device *stm;
	int idx;

	if (!src->output.nr_chans)
		return -ENODEV;

	if (chan >= src->output.nr_chans)
		return -EINVAL;

	idx = srcu_read_lock(&stm_source_srcu);

	stm = srcu_dereference(src->link, &stm_source_srcu);
	if (stm)
1320
		count = stm_write(stm, &src->output, chan, buf, count);
1321 1322 1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334 1335 1336 1337 1338 1339 1340 1341 1342 1343 1344 1345 1346
	else
		count = -ENODEV;

	srcu_read_unlock(&stm_source_srcu, idx);

	return count;
}
EXPORT_SYMBOL_GPL(stm_source_write);

static int __init stm_core_init(void)
{
	int err;

	err = class_register(&stm_class);
	if (err)
		return err;

	err = class_register(&stm_source_class);
	if (err)
		goto err_stm;

	err = stp_configfs_init();
	if (err)
		goto err_src;

	init_srcu_struct(&stm_source_srcu);
1347 1348
	INIT_LIST_HEAD(&stm_pdrv_head);
	mutex_init(&stm_pdrv_mutex);
1349

1350 1351 1352 1353 1354 1355
	/*
	 * So as to not confuse existing users with a requirement
	 * to load yet another module, do it here.
	 */
	if (IS_ENABLED(CONFIG_STM_PROTO_BASIC))
		(void)request_module_nowait("stm_p_basic");
1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382
	stm_core_up++;

	return 0;

err_src:
	class_unregister(&stm_source_class);
err_stm:
	class_unregister(&stm_class);

	return err;
}

module_init(stm_core_init);

static void __exit stm_core_exit(void)
{
	cleanup_srcu_struct(&stm_source_srcu);
	class_unregister(&stm_source_class);
	class_unregister(&stm_class);
	stp_configfs_exit();
}

module_exit(stm_core_exit);

MODULE_LICENSE("GPL v2");
MODULE_DESCRIPTION("System Trace Module device class");
MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");