| @@ -0,0 +1,585 @@ |
| | 1 | /* |
| | 2 | * Copyright (c) 2016, Linaro Ltd. |
| | 3 | * Copyright (c) 2012, Michal Simek <monstr@monstr.eu> |
| | 4 | * Copyright (c) 2012, PetaLogix |
| | 5 | * Copyright (c) 2011, Texas Instruments, Inc. |
| | 6 | * Copyright (c) 2011, Google, Inc. |
| | 7 | * |
| | 8 | * Based on rpmsg performance statistics driver by Michal Simek, which in turn |
| | 9 | * was based on TI & Google OMX rpmsg driver. |
| | 10 | * |
| | 11 | * This program is free software; you can redistribute it and/or modify |
| | 12 | * it under the terms of the GNU General Public License version 2 and |
| | 13 | * only version 2 as published by the Free Software Foundation. |
| | 14 | * |
| | 15 | * This program is distributed in the hope that it will be useful, |
| | 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| | 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| | 18 | * GNU General Public License for more details. |
| | 19 | */ |
| | 20 | #include <linux/cdev.h> |
| | 21 | #include <linux/device.h> |
| | 22 | #include <linux/fs.h> |
| | 23 | #include <linux/idr.h> |
| | 24 | #include <linux/kernel.h> |
| | 25 | #include <linux/module.h> |
| | 26 | #include <linux/poll.h> |
| | 27 | #include <linux/rpmsg.h> |
| | 28 | #include <linux/skbuff.h> |
| | 29 | #include <linux/slab.h> |
| | 30 | #include <linux/uaccess.h> |
| | 31 | #include <uapi/linux/rpmsg.h> |
| | 32 | |
| | 33 | #include "rpmsg_internal.h" |
| | 34 | |
| | 35 | #define RPMSG_DEV_MAX (MINORMASK + 1) |
| | 36 | |
| | 37 | static dev_t rpmsg_major; |
| | 38 | static struct class *rpmsg_class; |
| | 39 | |
| | 40 | static DEFINE_IDA(rpmsg_ctrl_ida); |
| | 41 | static DEFINE_IDA(rpmsg_ept_ida); |
| | 42 | static DEFINE_IDA(rpmsg_minor_ida); |
| | 43 | |
| | 44 | #define dev_to_eptdev(dev) container_of(dev, struct rpmsg_eptdev, dev) |
| | 45 | #define cdev_to_eptdev(i_cdev) container_of(i_cdev, struct rpmsg_eptdev, cdev) |
| | 46 | |
| | 47 | #define dev_to_ctrldev(dev) container_of(dev, struct rpmsg_ctrldev, dev) |
| | 48 | #define cdev_to_ctrldev(i_cdev) container_of(i_cdev, struct rpmsg_ctrldev, cdev) |
| | 49 | |
| | 50 | /** |
| | 51 | * struct rpmsg_ctrldev - control device for instantiating endpoint devices |
| | 52 | * @rpdev: underlaying rpmsg device |
| | 53 | * @cdev: cdev for the ctrl device |
| | 54 | * @dev: device for the ctrl device |
| | 55 | */ |
| | 56 | struct rpmsg_ctrldev { |
| | 57 | struct rpmsg_device *rpdev; |
| | 58 | struct cdev cdev; |
| | 59 | struct device dev; |
| | 60 | }; |
| | 61 | |
| | 62 | /** |
| | 63 | * struct rpmsg_eptdev - endpoint device context |
| | 64 | * @dev: endpoint device |
| | 65 | * @cdev: cdev for the endpoint device |
| | 66 | * @rpdev: underlaying rpmsg device |
| | 67 | * @chinfo: info used to open the endpoint |
| | 68 | * @ept_lock: synchronization of @ept modifications |
| | 69 | * @ept: rpmsg endpoint reference, when open |
| | 70 | * @queue_lock: synchronization of @queue operations |
| | 71 | * @queue: incoming message queue |
| | 72 | * @readq: wait object for incoming queue |
| | 73 | */ |
| | 74 | struct rpmsg_eptdev { |
| | 75 | struct device dev; |
| | 76 | struct cdev cdev; |
| | 77 | |
| | 78 | struct rpmsg_device *rpdev; |
| | 79 | struct rpmsg_channel_info chinfo; |
| | 80 | |
| | 81 | struct mutex ept_lock; |
| | 82 | struct rpmsg_endpoint *ept; |
| | 83 | |
| | 84 | spinlock_t queue_lock; |
| | 85 | struct sk_buff_head queue; |
| | 86 | wait_queue_head_t readq; |
| | 87 | }; |
| | 88 | |
| | 89 | static int rpmsg_eptdev_destroy(struct device *dev, void *data) |
| | 90 | { |
| | 91 | struct rpmsg_eptdev *eptdev = dev_to_eptdev(dev); |
| | 92 | |
| | 93 | mutex_lock(&eptdev->ept_lock); |
| | 94 | if (eptdev->ept) { |
| | 95 | rpmsg_destroy_ept(eptdev->ept); |
| | 96 | eptdev->ept = NULL; |
| | 97 | } |
| | 98 | mutex_unlock(&eptdev->ept_lock); |
| | 99 | |
| | 100 | /* wake up any blocked readers */ |
| | 101 | wake_up_interruptible(&eptdev->readq); |
| | 102 | |
| | 103 | device_del(&eptdev->dev); |
| | 104 | put_device(&eptdev->dev); |
| | 105 | |
| | 106 | return 0; |
| | 107 | } |
| | 108 | |
| | 109 | static int rpmsg_ept_cb(struct rpmsg_device *rpdev, void *buf, int len, |
| | 110 | void *priv, u32 addr) |
| | 111 | { |
| | 112 | struct rpmsg_eptdev *eptdev = priv; |
| | 113 | struct sk_buff *skb; |
| | 114 | |
| | 115 | skb = alloc_skb(len, GFP_ATOMIC); |
| | 116 | if (!skb) |
| | 117 | return -ENOMEM; |
| | 118 | |
| | 119 | memcpy(skb_put(skb, len), buf, len); |
| | 120 | |
| | 121 | spin_lock(&eptdev->queue_lock); |
| | 122 | skb_queue_tail(&eptdev->queue, skb); |
| | 123 | spin_unlock(&eptdev->queue_lock); |
| | 124 | |
| | 125 | /* wake up any blocking processes, waiting for new data */ |
| | 126 | wake_up_interruptible(&eptdev->readq); |
| | 127 | |
| | 128 | return 0; |
| | 129 | } |
| | 130 | |
| | 131 | static int rpmsg_eptdev_open(struct inode *inode, struct file *filp) |
| | 132 | { |
| | 133 | struct rpmsg_eptdev *eptdev = cdev_to_eptdev(inode->i_cdev); |
| | 134 | struct rpmsg_endpoint *ept; |
| | 135 | struct rpmsg_device *rpdev = eptdev->rpdev; |
| | 136 | struct device *dev = &eptdev->dev; |
| | 137 | |
| | 138 | get_device(dev); |
| | 139 | |
| | 140 | ept = rpmsg_create_ept(rpdev, rpmsg_ept_cb, eptdev, eptdev->chinfo); |
| | 141 | if (!ept) { |
| | 142 | dev_err(dev, "failed to open %s\n", eptdev->chinfo.name); |
| | 143 | put_device(dev); |
| | 144 | return -EINVAL; |
| | 145 | } |
| | 146 | |
| | 147 | eptdev->ept = ept; |
| | 148 | filp->private_data = eptdev; |
| | 149 | |
| | 150 | return 0; |
| | 151 | } |
| | 152 | |
| | 153 | static int rpmsg_eptdev_release(struct inode *inode, struct file *filp) |
| | 154 | { |
| | 155 | struct rpmsg_eptdev *eptdev = cdev_to_eptdev(inode->i_cdev); |
| | 156 | struct device *dev = &eptdev->dev; |
| | 157 | struct sk_buff *skb; |
| | 158 | |
| | 159 | /* Close the endpoint, if it's not already destroyed by the parent */ |
| | 160 | mutex_lock(&eptdev->ept_lock); |
| | 161 | if (eptdev->ept) { |
| | 162 | rpmsg_destroy_ept(eptdev->ept); |
| | 163 | eptdev->ept = NULL; |
| | 164 | } |
| | 165 | mutex_unlock(&eptdev->ept_lock); |
| | 166 | |
| | 167 | /* Discard all SKBs */ |
| | 168 | while (!skb_queue_empty(&eptdev->queue)) { |
| | 169 | skb = skb_dequeue(&eptdev->queue); |
| | 170 | kfree_skb(skb); |
| | 171 | } |
| | 172 | |
| | 173 | put_device(dev); |
| | 174 | |
| | 175 | return 0; |
| | 176 | } |
| | 177 | |
| | 178 | static ssize_t rpmsg_eptdev_read(struct file *filp, char __user *buf, |
| | 179 | size_t len, loff_t *f_pos) |
| | 180 | { |
| | 181 | struct rpmsg_eptdev *eptdev = filp->private_data; |
| | 182 | unsigned long flags; |
| | 183 | struct sk_buff *skb; |
| | 184 | int use; |
| | 185 | |
| | 186 | if (!eptdev->ept) |
| | 187 | return -EPIPE; |
| | 188 | |
| | 189 | spin_lock_irqsave(&eptdev->queue_lock, flags); |
| | 190 | |
| | 191 | /* Wait for data in the queue */ |
| | 192 | if (skb_queue_empty(&eptdev->queue)) { |
| | 193 | spin_unlock_irqrestore(&eptdev->queue_lock, flags); |
| | 194 | |
| | 195 | if (filp->f_flags & O_NONBLOCK) |
| | 196 | return -EAGAIN; |
| | 197 | |
| | 198 | /* Wait until we get data or the endpoint goes away */ |
| | 199 | if (wait_event_interruptible(eptdev->readq, |
| | 200 | !skb_queue_empty(&eptdev->queue) || |
| | 201 | !eptdev->ept)) |
| | 202 | return -ERESTARTSYS; |
| | 203 | |
| | 204 | /* We lost the endpoint while waiting */ |
| | 205 | if (!eptdev->ept) |
| | 206 | return -EPIPE; |
| | 207 | |
| | 208 | spin_lock_irqsave(&eptdev->queue_lock, flags); |
| | 209 | } |
| | 210 | |
| | 211 | skb = skb_dequeue(&eptdev->queue); |
| | 212 | if (!skb) |
| | 213 | return -EFAULT; |
| | 214 | |
| | 215 | spin_unlock_irqrestore(&eptdev->queue_lock, flags); |
| | 216 | |
| | 217 | use = min_t(size_t, len, skb->len); |
| | 218 | if (copy_to_user(buf, skb->data, use)) |
| | 219 | use = -EFAULT; |
| | 220 | |
| | 221 | kfree_skb(skb); |
| | 222 | |
| | 223 | return use; |
| | 224 | } |
| | 225 | |
| | 226 | static ssize_t rpmsg_eptdev_write(struct file *filp, const char __user *buf, |
| | 227 | size_t len, loff_t *f_pos) |
| | 228 | { |
| | 229 | struct rpmsg_eptdev *eptdev = filp->private_data; |
| | 230 | void *kbuf; |
| | 231 | int ret; |
| | 232 | |
| | 233 | kbuf = memdup_user(buf, len); |
| | 234 | if (IS_ERR(kbuf)) |
| | 235 | return PTR_ERR(kbuf); |
| | 236 | |
| | 237 | if (mutex_lock_interruptible(&eptdev->ept_lock)) { |
| | 238 | ret = -ERESTARTSYS; |
| | 239 | goto free_kbuf; |
| | 240 | } |
| | 241 | |
| | 242 | if (!eptdev->ept) { |
| | 243 | ret = -EPIPE; |
| | 244 | goto unlock_eptdev; |
| | 245 | } |
| | 246 | |
| | 247 | if (filp->f_flags & O_NONBLOCK) |
| | 248 | ret = rpmsg_trysend(eptdev->ept, kbuf, len); |
| | 249 | else |
| | 250 | ret = rpmsg_send(eptdev->ept, kbuf, len); |
| | 251 | |
| | 252 | unlock_eptdev: |
| | 253 | mutex_unlock(&eptdev->ept_lock); |
| | 254 | |
| | 255 | free_kbuf: |
| | 256 | kfree(kbuf); |
| | 257 | return ret < 0 ? ret : len; |
| | 258 | } |
| | 259 | |
| | 260 | static unsigned int rpmsg_eptdev_poll(struct file *filp, poll_table *wait) |
| | 261 | { |
| | 262 | struct rpmsg_eptdev *eptdev = filp->private_data; |
| | 263 | unsigned int mask = 0; |
| | 264 | |
| | 265 | if (!eptdev->ept) |
| | 266 | return POLLERR; |
| | 267 | |
| | 268 | poll_wait(filp, &eptdev->readq, wait); |
| | 269 | |
| | 270 | if (!skb_queue_empty(&eptdev->queue)) |
| | 271 | mask |= POLLIN | POLLRDNORM; |
| | 272 | |
| | 273 | mask |= rpmsg_poll(eptdev->ept, filp, wait); |
| | 274 | |
| | 275 | return mask; |
| | 276 | } |
| | 277 | |
| | 278 | static long rpmsg_eptdev_ioctl(struct file *fp, unsigned int cmd, |
| | 279 | unsigned long arg) |
| | 280 | { |
| | 281 | struct rpmsg_eptdev *eptdev = fp->private_data; |
| | 282 | |
| | 283 | if (cmd != RPMSG_DESTROY_EPT_IOCTL) |
| | 284 | return -EINVAL; |
| | 285 | |
| | 286 | return rpmsg_eptdev_destroy(&eptdev->dev, NULL); |
| | 287 | } |
| | 288 | |
| | 289 | static const struct file_operations rpmsg_eptdev_fops = { |
| | 290 | .owner = THIS_MODULE, |
| | 291 | .open = rpmsg_eptdev_open, |
| | 292 | .release = rpmsg_eptdev_release, |
| | 293 | .read = rpmsg_eptdev_read, |
| | 294 | .write = rpmsg_eptdev_write, |
| | 295 | .poll = rpmsg_eptdev_poll, |
| | 296 | .unlocked_ioctl = rpmsg_eptdev_ioctl, |
| | 297 | }; |
| | 298 | |
| | 299 | static ssize_t name_show(struct device *dev, struct device_attribute *attr, |
| | 300 | char *buf) |
| | 301 | { |
| | 302 | struct rpmsg_eptdev *eptdev = dev_get_drvdata(dev); |
| | 303 | |
| | 304 | return sprintf(buf, "%s\n", eptdev->chinfo.name); |
| | 305 | } |
| | 306 | static DEVICE_ATTR_RO(name); |
| | 307 | |
| | 308 | static ssize_t src_show(struct device *dev, struct device_attribute *attr, |
| | 309 | char *buf) |
| | 310 | { |
| | 311 | struct rpmsg_eptdev *eptdev = dev_get_drvdata(dev); |
| | 312 | |
| | 313 | return sprintf(buf, "%d\n", eptdev->chinfo.src); |
| | 314 | } |
| | 315 | static DEVICE_ATTR_RO(src); |
| | 316 | |
| | 317 | static ssize_t dst_show(struct device *dev, struct device_attribute *attr, |
| | 318 | char *buf) |
| | 319 | { |
| | 320 | struct rpmsg_eptdev *eptdev = dev_get_drvdata(dev); |
| | 321 | |
| | 322 | return sprintf(buf, "%d\n", eptdev->chinfo.dst); |
| | 323 | } |
| | 324 | static DEVICE_ATTR_RO(dst); |
| | 325 | |
| | 326 | static struct attribute *rpmsg_eptdev_attrs[] = { |
| | 327 | &dev_attr_name.attr, |
| | 328 | &dev_attr_src.attr, |
| | 329 | &dev_attr_dst.attr, |
| | 330 | NULL |
| | 331 | }; |
| | 332 | ATTRIBUTE_GROUPS(rpmsg_eptdev); |
| | 333 | |
| | 334 | static void rpmsg_eptdev_release_device(struct device *dev) |
| | 335 | { |
| | 336 | struct rpmsg_eptdev *eptdev = dev_to_eptdev(dev); |
| | 337 | |
| | 338 | ida_simple_remove(&rpmsg_ept_ida, dev->id); |
| | 339 | ida_simple_remove(&rpmsg_minor_ida, MINOR(eptdev->dev.devt)); |
| | 340 | cdev_del(&eptdev->cdev); |
| | 341 | kfree(eptdev); |
| | 342 | } |
| | 343 | |
| | 344 | static int rpmsg_eptdev_create(struct rpmsg_ctrldev *ctrldev, |
| | 345 | struct rpmsg_channel_info chinfo) |
| | 346 | { |
| | 347 | struct rpmsg_device *rpdev = ctrldev->rpdev; |
| | 348 | struct rpmsg_eptdev *eptdev; |
| | 349 | struct device *dev; |
| | 350 | int ret; |
| | 351 | |
| | 352 | eptdev = kzalloc(sizeof(*eptdev), GFP_KERNEL); |
| | 353 | if (!eptdev) |
| | 354 | return -ENOMEM; |
| | 355 | |
| | 356 | dev = &eptdev->dev; |
| | 357 | eptdev->rpdev = rpdev; |
| | 358 | eptdev->chinfo = chinfo; |
| | 359 | |
| | 360 | mutex_init(&eptdev->ept_lock); |
| | 361 | spin_lock_init(&eptdev->queue_lock); |
| | 362 | skb_queue_head_init(&eptdev->queue); |
| | 363 | init_waitqueue_head(&eptdev->readq); |
| | 364 | |
| | 365 | device_initialize(dev); |
| | 366 | dev->class = rpmsg_class; |
| | 367 | dev->parent = &ctrldev->dev; |
| | 368 | dev->groups = rpmsg_eptdev_groups; |
| | 369 | dev_set_drvdata(dev, eptdev); |
| | 370 | |
| | 371 | cdev_init(&eptdev->cdev, &rpmsg_eptdev_fops); |
| | 372 | eptdev->cdev.owner = THIS_MODULE; |
| | 373 | |
| | 374 | ret = ida_simple_get(&rpmsg_minor_ida, 0, RPMSG_DEV_MAX, GFP_KERNEL); |
| | 375 | if (ret < 0) |
| | 376 | goto free_eptdev; |
| | 377 | dev->devt = MKDEV(MAJOR(rpmsg_major), ret); |
| | 378 | |
| | 379 | ret = ida_simple_get(&rpmsg_ept_ida, 0, 0, GFP_KERNEL); |
| | 380 | if (ret < 0) |
| | 381 | goto free_minor_ida; |
| | 382 | dev->id = ret; |
| | 383 | dev_set_name(dev, "rpmsg%d", ret); |
| | 384 | |
| | 385 | ret = cdev_add(&eptdev->cdev, dev->devt, 1); |
| | 386 | if (ret) |
| | 387 | goto free_ept_ida; |
| | 388 | |
| | 389 | /* We can now rely on the release function for cleanup */ |
| | 390 | dev->release = rpmsg_eptdev_release_device; |
| | 391 | |
| | 392 | ret = device_add(dev); |
| | 393 | if (ret) { |
| | 394 | dev_err(dev, "device_register failed: %d\n", ret); |
| | 395 | put_device(dev); |
| | 396 | } |
| | 397 | |
| | 398 | return ret; |
| | 399 | |
| | 400 | free_ept_ida: |
| | 401 | ida_simple_remove(&rpmsg_ept_ida, dev->id); |
| | 402 | free_minor_ida: |
| | 403 | ida_simple_remove(&rpmsg_minor_ida, MINOR(dev->devt)); |
| | 404 | free_eptdev: |
| | 405 | put_device(dev); |
| | 406 | kfree(eptdev); |
| | 407 | |
| | 408 | return ret; |
| | 409 | } |
| | 410 | |
| | 411 | static int rpmsg_ctrldev_open(struct inode *inode, struct file *filp) |
| | 412 | { |
| | 413 | struct rpmsg_ctrldev *ctrldev = cdev_to_ctrldev(inode->i_cdev); |
| | 414 | |
| | 415 | get_device(&ctrldev->dev); |
| | 416 | filp->private_data = ctrldev; |
| | 417 | |
| | 418 | return 0; |
| | 419 | } |
| | 420 | |
| | 421 | static int rpmsg_ctrldev_release(struct inode *inode, struct file *filp) |
| | 422 | { |
| | 423 | struct rpmsg_ctrldev *ctrldev = cdev_to_ctrldev(inode->i_cdev); |
| | 424 | |
| | 425 | put_device(&ctrldev->dev); |
| | 426 | |
| | 427 | return 0; |
| | 428 | } |
| | 429 | |
| | 430 | static long rpmsg_ctrldev_ioctl(struct file *fp, unsigned int cmd, |
| | 431 | unsigned long arg) |
| | 432 | { |
| | 433 | struct rpmsg_ctrldev *ctrldev = fp->private_data; |
| | 434 | void __user *argp = (void __user *)arg; |
| | 435 | struct rpmsg_endpoint_info eptinfo; |
| | 436 | struct rpmsg_channel_info chinfo; |
| | 437 | |
| | 438 | if (cmd != RPMSG_CREATE_EPT_IOCTL) |
| | 439 | return -EINVAL; |
| | 440 | |
| | 441 | if (copy_from_user(&eptinfo, argp, sizeof(eptinfo))) |
| | 442 | return -EFAULT; |
| | 443 | |
| | 444 | memcpy(chinfo.name, eptinfo.name, RPMSG_NAME_SIZE); |
| | 445 | chinfo.name[RPMSG_NAME_SIZE-1] = '\0'; |
| | 446 | chinfo.src = eptinfo.src; |
| | 447 | chinfo.dst = eptinfo.dst; |
| | 448 | |
| | 449 | return rpmsg_eptdev_create(ctrldev, chinfo); |
| | 450 | }; |
| | 451 | |
| | 452 | static const struct file_operations rpmsg_ctrldev_fops = { |
| | 453 | .owner = THIS_MODULE, |
| | 454 | .open = rpmsg_ctrldev_open, |
| | 455 | .release = rpmsg_ctrldev_release, |
| | 456 | .unlocked_ioctl = rpmsg_ctrldev_ioctl, |
| | 457 | }; |
| | 458 | |
| | 459 | static void rpmsg_ctrldev_release_device(struct device *dev) |
| | 460 | { |
| | 461 | struct rpmsg_ctrldev *ctrldev = dev_to_ctrldev(dev); |
| | 462 | |
| | 463 | ida_simple_remove(&rpmsg_ctrl_ida, dev->id); |
| | 464 | ida_simple_remove(&rpmsg_minor_ida, MINOR(dev->devt)); |
| | 465 | cdev_del(&ctrldev->cdev); |
| | 466 | kfree(ctrldev); |
| | 467 | } |
| | 468 | |
| | 469 | static int rpmsg_chrdev_probe(struct rpmsg_device *rpdev) |
| | 470 | { |
| | 471 | struct rpmsg_ctrldev *ctrldev; |
| | 472 | struct device *dev; |
| | 473 | int ret; |
| | 474 | |
| | 475 | ctrldev = kzalloc(sizeof(*ctrldev), GFP_KERNEL); |
| | 476 | if (!ctrldev) |
| | 477 | return -ENOMEM; |
| | 478 | |
| | 479 | ctrldev->rpdev = rpdev; |
| | 480 | |
| | 481 | dev = &ctrldev->dev; |
| | 482 | device_initialize(dev); |
| | 483 | dev->parent = &rpdev->dev; |
| | 484 | dev->class = rpmsg_class; |
| | 485 | |
| | 486 | cdev_init(&ctrldev->cdev, &rpmsg_ctrldev_fops); |
| | 487 | ctrldev->cdev.owner = THIS_MODULE; |
| | 488 | |
| | 489 | ret = ida_simple_get(&rpmsg_minor_ida, 0, RPMSG_DEV_MAX, GFP_KERNEL); |
| | 490 | if (ret < 0) |
| | 491 | goto free_ctrldev; |
| | 492 | dev->devt = MKDEV(MAJOR(rpmsg_major), ret); |
| | 493 | |
| | 494 | ret = ida_simple_get(&rpmsg_ctrl_ida, 0, 0, GFP_KERNEL); |
| | 495 | if (ret < 0) |
| | 496 | goto free_minor_ida; |
| | 497 | dev->id = ret; |
| | 498 | dev_set_name(&ctrldev->dev, "rpmsg_ctrl%d", ret); |
| | 499 | |
| | 500 | ret = cdev_add(&ctrldev->cdev, dev->devt, 1); |
| | 501 | if (ret) |
| | 502 | goto free_ctrl_ida; |
| | 503 | |
| | 504 | /* We can now rely on the release function for cleanup */ |
| | 505 | dev->release = rpmsg_ctrldev_release_device; |
| | 506 | |
| | 507 | ret = device_add(dev); |
| | 508 | if (ret) { |
| | 509 | dev_err(&rpdev->dev, "device_register failed: %d\n", ret); |
| | 510 | put_device(dev); |
| | 511 | } |
| | 512 | |
| | 513 | dev_set_drvdata(&rpdev->dev, ctrldev); |
| | 514 | |
| | 515 | return ret; |
| | 516 | |
| | 517 | free_ctrl_ida: |
| | 518 | ida_simple_remove(&rpmsg_ctrl_ida, dev->id); |
| | 519 | free_minor_ida: |
| | 520 | ida_simple_remove(&rpmsg_minor_ida, MINOR(dev->devt)); |
| | 521 | free_ctrldev: |
| | 522 | put_device(dev); |
| | 523 | kfree(ctrldev); |
| | 524 | |
| | 525 | return ret; |
| | 526 | } |
| | 527 | |
| | 528 | static void rpmsg_chrdev_remove(struct rpmsg_device *rpdev) |
| | 529 | { |
| | 530 | struct rpmsg_ctrldev *ctrldev = dev_get_drvdata(&rpdev->dev); |
| | 531 | int ret; |
| | 532 | |
| | 533 | /* Destroy all endpoints */ |
| | 534 | ret = device_for_each_child(&ctrldev->dev, NULL, rpmsg_eptdev_destroy); |
| | 535 | if (ret) |
| | 536 | dev_warn(&rpdev->dev, "failed to nuke endpoints: %d\n", ret); |
| | 537 | |
| | 538 | device_del(&ctrldev->dev); |
| | 539 | put_device(&ctrldev->dev); |
| | 540 | } |
| | 541 | |
| | 542 | static struct rpmsg_driver rpmsg_chrdev_driver = { |
| | 543 | .probe = rpmsg_chrdev_probe, |
| | 544 | .remove = rpmsg_chrdev_remove, |
| | 545 | .drv = { |
| | 546 | .name = "rpmsg_chrdev", |
| | 547 | }, |
| | 548 | }; |
| | 549 | |
| | 550 | static int rpmsg_char_init(void) |
| | 551 | { |
| | 552 | int ret; |
| | 553 | |
| | 554 | ret = alloc_chrdev_region(&rpmsg_major, 0, RPMSG_DEV_MAX, "rpmsg"); |
| | 555 | if (ret < 0) { |
| | 556 | pr_err("rpmsg: failed to allocate char dev region\n"); |
| | 557 | return ret; |
| | 558 | } |
| | 559 | |
| | 560 | rpmsg_class = class_create(THIS_MODULE, "rpmsg"); |
| | 561 | if (IS_ERR(rpmsg_class)) { |
| | 562 | pr_err("failed to create rpmsg class\n"); |
| | 563 | unregister_chrdev_region(rpmsg_major, RPMSG_DEV_MAX); |
| | 564 | return PTR_ERR(rpmsg_class); |
| | 565 | } |
| | 566 | |
| | 567 | ret = register_rpmsg_driver(&rpmsg_chrdev_driver); |
| | 568 | if (ret < 0) { |
| | 569 | pr_err("rpmsgchr: failed to register rpmsg driver\n"); |
| | 570 | class_destroy(rpmsg_class); |
| | 571 | unregister_chrdev_region(rpmsg_major, RPMSG_DEV_MAX); |
| | 572 | } |
| | 573 | |
| | 574 | return ret; |
| | 575 | } |
| | 576 | postcore_initcall(rpmsg_char_init); |
| | 577 | |
| | 578 | static void rpmsg_chrdev_exit(void) |
| | 579 | { |
| | 580 | unregister_rpmsg_driver(&rpmsg_chrdev_driver); |
| | 581 | class_destroy(rpmsg_class); |
| | 582 | unregister_chrdev_region(rpmsg_major, RPMSG_DEV_MAX); |
| | 583 | } |
| | 584 | module_exit(rpmsg_chrdev_exit); |
| | 585 | MODULE_LICENSE("GPL v2"); |