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