Gerd Hoffmann | 271c865 | 2015-03-27 12:46:12 +1030 | [diff] [blame] | 1 | #include <linux/module.h> |
| 2 | #include <linux/virtio.h> |
| 3 | #include <linux/virtio_config.h> |
| 4 | #include <linux/input.h> |
| 5 | |
| 6 | #include <uapi/linux/virtio_ids.h> |
| 7 | #include <uapi/linux/virtio_input.h> |
| 8 | |
| 9 | struct virtio_input { |
| 10 | struct virtio_device *vdev; |
| 11 | struct input_dev *idev; |
| 12 | char name[64]; |
| 13 | char serial[64]; |
| 14 | char phys[64]; |
| 15 | struct virtqueue *evt, *sts; |
| 16 | struct virtio_input_event evts[64]; |
| 17 | spinlock_t lock; |
| 18 | bool ready; |
| 19 | }; |
| 20 | |
| 21 | static void virtinput_queue_evtbuf(struct virtio_input *vi, |
| 22 | struct virtio_input_event *evtbuf) |
| 23 | { |
| 24 | struct scatterlist sg[1]; |
| 25 | |
| 26 | sg_init_one(sg, evtbuf, sizeof(*evtbuf)); |
| 27 | virtqueue_add_inbuf(vi->evt, sg, 1, evtbuf, GFP_ATOMIC); |
| 28 | } |
| 29 | |
| 30 | static void virtinput_recv_events(struct virtqueue *vq) |
| 31 | { |
| 32 | struct virtio_input *vi = vq->vdev->priv; |
| 33 | struct virtio_input_event *event; |
| 34 | unsigned long flags; |
| 35 | unsigned int len; |
| 36 | |
| 37 | spin_lock_irqsave(&vi->lock, flags); |
| 38 | if (vi->ready) { |
| 39 | while ((event = virtqueue_get_buf(vi->evt, &len)) != NULL) { |
| 40 | spin_unlock_irqrestore(&vi->lock, flags); |
| 41 | input_event(vi->idev, |
| 42 | le16_to_cpu(event->type), |
| 43 | le16_to_cpu(event->code), |
| 44 | le32_to_cpu(event->value)); |
| 45 | spin_lock_irqsave(&vi->lock, flags); |
| 46 | virtinput_queue_evtbuf(vi, event); |
| 47 | } |
| 48 | virtqueue_kick(vq); |
| 49 | } |
| 50 | spin_unlock_irqrestore(&vi->lock, flags); |
| 51 | } |
| 52 | |
| 53 | /* |
| 54 | * On error we are losing the status update, which isn't critical as |
| 55 | * this is typically used for stuff like keyboard leds. |
| 56 | */ |
| 57 | static int virtinput_send_status(struct virtio_input *vi, |
| 58 | u16 type, u16 code, s32 value) |
| 59 | { |
| 60 | struct virtio_input_event *stsbuf; |
| 61 | struct scatterlist sg[1]; |
| 62 | unsigned long flags; |
| 63 | int rc; |
| 64 | |
| 65 | stsbuf = kzalloc(sizeof(*stsbuf), GFP_ATOMIC); |
| 66 | if (!stsbuf) |
| 67 | return -ENOMEM; |
| 68 | |
| 69 | stsbuf->type = cpu_to_le16(type); |
| 70 | stsbuf->code = cpu_to_le16(code); |
| 71 | stsbuf->value = cpu_to_le32(value); |
| 72 | sg_init_one(sg, stsbuf, sizeof(*stsbuf)); |
| 73 | |
| 74 | spin_lock_irqsave(&vi->lock, flags); |
| 75 | if (vi->ready) { |
| 76 | rc = virtqueue_add_outbuf(vi->sts, sg, 1, stsbuf, GFP_ATOMIC); |
| 77 | virtqueue_kick(vi->sts); |
| 78 | } else { |
| 79 | rc = -ENODEV; |
| 80 | } |
| 81 | spin_unlock_irqrestore(&vi->lock, flags); |
| 82 | |
| 83 | if (rc != 0) |
| 84 | kfree(stsbuf); |
| 85 | return rc; |
| 86 | } |
| 87 | |
| 88 | static void virtinput_recv_status(struct virtqueue *vq) |
| 89 | { |
| 90 | struct virtio_input *vi = vq->vdev->priv; |
| 91 | struct virtio_input_event *stsbuf; |
| 92 | unsigned long flags; |
| 93 | unsigned int len; |
| 94 | |
| 95 | spin_lock_irqsave(&vi->lock, flags); |
| 96 | while ((stsbuf = virtqueue_get_buf(vi->sts, &len)) != NULL) |
| 97 | kfree(stsbuf); |
| 98 | spin_unlock_irqrestore(&vi->lock, flags); |
| 99 | } |
| 100 | |
| 101 | static int virtinput_status(struct input_dev *idev, unsigned int type, |
| 102 | unsigned int code, int value) |
| 103 | { |
| 104 | struct virtio_input *vi = input_get_drvdata(idev); |
| 105 | |
| 106 | return virtinput_send_status(vi, type, code, value); |
| 107 | } |
| 108 | |
| 109 | static u8 virtinput_cfg_select(struct virtio_input *vi, |
| 110 | u8 select, u8 subsel) |
| 111 | { |
| 112 | u8 size; |
| 113 | |
| 114 | virtio_cwrite(vi->vdev, struct virtio_input_config, select, &select); |
| 115 | virtio_cwrite(vi->vdev, struct virtio_input_config, subsel, &subsel); |
| 116 | virtio_cread(vi->vdev, struct virtio_input_config, size, &size); |
| 117 | return size; |
| 118 | } |
| 119 | |
| 120 | static void virtinput_cfg_bits(struct virtio_input *vi, int select, int subsel, |
| 121 | unsigned long *bits, unsigned int bitcount) |
| 122 | { |
| 123 | unsigned int bit; |
| 124 | u8 *virtio_bits; |
| 125 | u8 bytes; |
| 126 | |
| 127 | bytes = virtinput_cfg_select(vi, select, subsel); |
| 128 | if (!bytes) |
| 129 | return; |
| 130 | if (bitcount > bytes * 8) |
| 131 | bitcount = bytes * 8; |
| 132 | |
| 133 | /* |
| 134 | * Bitmap in virtio config space is a simple stream of bytes, |
| 135 | * with the first byte carrying bits 0-7, second bits 8-15 and |
| 136 | * so on. |
| 137 | */ |
| 138 | virtio_bits = kzalloc(bytes, GFP_KERNEL); |
| 139 | if (!virtio_bits) |
| 140 | return; |
| 141 | virtio_cread_bytes(vi->vdev, offsetof(struct virtio_input_config, |
| 142 | u.bitmap), |
| 143 | virtio_bits, bytes); |
| 144 | for (bit = 0; bit < bitcount; bit++) { |
| 145 | if (virtio_bits[bit / 8] & (1 << (bit % 8))) |
| 146 | __set_bit(bit, bits); |
| 147 | } |
| 148 | kfree(virtio_bits); |
| 149 | |
| 150 | if (select == VIRTIO_INPUT_CFG_EV_BITS) |
| 151 | __set_bit(subsel, vi->idev->evbit); |
| 152 | } |
| 153 | |
| 154 | static void virtinput_cfg_abs(struct virtio_input *vi, int abs) |
| 155 | { |
| 156 | u32 mi, ma, re, fu, fl; |
| 157 | |
| 158 | virtinput_cfg_select(vi, VIRTIO_INPUT_CFG_ABS_INFO, abs); |
| 159 | virtio_cread(vi->vdev, struct virtio_input_config, u.abs.min, &mi); |
| 160 | virtio_cread(vi->vdev, struct virtio_input_config, u.abs.max, &ma); |
| 161 | virtio_cread(vi->vdev, struct virtio_input_config, u.abs.res, &re); |
| 162 | virtio_cread(vi->vdev, struct virtio_input_config, u.abs.fuzz, &fu); |
| 163 | virtio_cread(vi->vdev, struct virtio_input_config, u.abs.flat, &fl); |
| 164 | input_set_abs_params(vi->idev, abs, mi, ma, fu, fl); |
| 165 | input_abs_set_res(vi->idev, abs, re); |
| 166 | } |
| 167 | |
| 168 | static int virtinput_init_vqs(struct virtio_input *vi) |
| 169 | { |
| 170 | struct virtqueue *vqs[2]; |
| 171 | vq_callback_t *cbs[] = { virtinput_recv_events, |
| 172 | virtinput_recv_status }; |
| 173 | static const char *names[] = { "events", "status" }; |
| 174 | int err; |
| 175 | |
| 176 | err = vi->vdev->config->find_vqs(vi->vdev, 2, vqs, cbs, names); |
| 177 | if (err) |
| 178 | return err; |
| 179 | vi->evt = vqs[0]; |
| 180 | vi->sts = vqs[1]; |
| 181 | |
| 182 | return 0; |
| 183 | } |
| 184 | |
| 185 | static void virtinput_fill_evt(struct virtio_input *vi) |
| 186 | { |
| 187 | unsigned long flags; |
| 188 | int i, size; |
| 189 | |
| 190 | spin_lock_irqsave(&vi->lock, flags); |
| 191 | size = virtqueue_get_vring_size(vi->evt); |
| 192 | if (size > ARRAY_SIZE(vi->evts)) |
| 193 | size = ARRAY_SIZE(vi->evts); |
| 194 | for (i = 0; i < size; i++) |
| 195 | virtinput_queue_evtbuf(vi, &vi->evts[i]); |
| 196 | virtqueue_kick(vi->evt); |
| 197 | spin_unlock_irqrestore(&vi->lock, flags); |
| 198 | } |
| 199 | |
| 200 | static int virtinput_probe(struct virtio_device *vdev) |
| 201 | { |
| 202 | struct virtio_input *vi; |
| 203 | unsigned long flags; |
| 204 | size_t size; |
| 205 | int abs, err; |
| 206 | |
| 207 | if (!virtio_has_feature(vdev, VIRTIO_F_VERSION_1)) |
| 208 | return -ENODEV; |
| 209 | |
| 210 | vi = kzalloc(sizeof(*vi), GFP_KERNEL); |
| 211 | if (!vi) |
| 212 | return -ENOMEM; |
| 213 | |
| 214 | vdev->priv = vi; |
| 215 | vi->vdev = vdev; |
| 216 | spin_lock_init(&vi->lock); |
| 217 | |
| 218 | err = virtinput_init_vqs(vi); |
| 219 | if (err) |
| 220 | goto err_init_vq; |
| 221 | |
| 222 | vi->idev = input_allocate_device(); |
| 223 | if (!vi->idev) { |
| 224 | err = -ENOMEM; |
| 225 | goto err_input_alloc; |
| 226 | } |
| 227 | input_set_drvdata(vi->idev, vi); |
| 228 | |
| 229 | size = virtinput_cfg_select(vi, VIRTIO_INPUT_CFG_ID_NAME, 0); |
| 230 | virtio_cread_bytes(vi->vdev, offsetof(struct virtio_input_config, |
| 231 | u.string), |
| 232 | vi->name, min(size, sizeof(vi->name))); |
| 233 | size = virtinput_cfg_select(vi, VIRTIO_INPUT_CFG_ID_SERIAL, 0); |
| 234 | virtio_cread_bytes(vi->vdev, offsetof(struct virtio_input_config, |
| 235 | u.string), |
| 236 | vi->serial, min(size, sizeof(vi->serial))); |
| 237 | snprintf(vi->phys, sizeof(vi->phys), |
| 238 | "virtio%d/input0", vdev->index); |
| 239 | vi->idev->name = vi->name; |
| 240 | vi->idev->phys = vi->phys; |
| 241 | vi->idev->uniq = vi->serial; |
| 242 | |
| 243 | size = virtinput_cfg_select(vi, VIRTIO_INPUT_CFG_ID_DEVIDS, 0); |
| 244 | if (size >= sizeof(struct virtio_input_devids)) { |
| 245 | virtio_cread(vi->vdev, struct virtio_input_config, |
| 246 | u.ids.bustype, &vi->idev->id.bustype); |
| 247 | virtio_cread(vi->vdev, struct virtio_input_config, |
| 248 | u.ids.vendor, &vi->idev->id.vendor); |
| 249 | virtio_cread(vi->vdev, struct virtio_input_config, |
| 250 | u.ids.product, &vi->idev->id.product); |
| 251 | virtio_cread(vi->vdev, struct virtio_input_config, |
| 252 | u.ids.version, &vi->idev->id.version); |
| 253 | } else { |
| 254 | vi->idev->id.bustype = BUS_VIRTUAL; |
| 255 | } |
| 256 | |
| 257 | virtinput_cfg_bits(vi, VIRTIO_INPUT_CFG_PROP_BITS, 0, |
| 258 | vi->idev->propbit, INPUT_PROP_CNT); |
| 259 | size = virtinput_cfg_select(vi, VIRTIO_INPUT_CFG_EV_BITS, EV_REP); |
| 260 | if (size) |
| 261 | __set_bit(EV_REP, vi->idev->evbit); |
| 262 | |
| 263 | vi->idev->dev.parent = &vdev->dev; |
| 264 | vi->idev->event = virtinput_status; |
| 265 | |
| 266 | /* device -> kernel */ |
| 267 | virtinput_cfg_bits(vi, VIRTIO_INPUT_CFG_EV_BITS, EV_KEY, |
| 268 | vi->idev->keybit, KEY_CNT); |
| 269 | virtinput_cfg_bits(vi, VIRTIO_INPUT_CFG_EV_BITS, EV_REL, |
| 270 | vi->idev->relbit, REL_CNT); |
| 271 | virtinput_cfg_bits(vi, VIRTIO_INPUT_CFG_EV_BITS, EV_ABS, |
| 272 | vi->idev->absbit, ABS_CNT); |
| 273 | virtinput_cfg_bits(vi, VIRTIO_INPUT_CFG_EV_BITS, EV_MSC, |
| 274 | vi->idev->mscbit, MSC_CNT); |
| 275 | virtinput_cfg_bits(vi, VIRTIO_INPUT_CFG_EV_BITS, EV_SW, |
| 276 | vi->idev->swbit, SW_CNT); |
| 277 | |
| 278 | /* kernel -> device */ |
| 279 | virtinput_cfg_bits(vi, VIRTIO_INPUT_CFG_EV_BITS, EV_LED, |
| 280 | vi->idev->ledbit, LED_CNT); |
| 281 | virtinput_cfg_bits(vi, VIRTIO_INPUT_CFG_EV_BITS, EV_SND, |
| 282 | vi->idev->sndbit, SND_CNT); |
| 283 | |
| 284 | if (test_bit(EV_ABS, vi->idev->evbit)) { |
| 285 | for (abs = 0; abs < ABS_CNT; abs++) { |
| 286 | if (!test_bit(abs, vi->idev->absbit)) |
| 287 | continue; |
| 288 | virtinput_cfg_abs(vi, abs); |
| 289 | } |
| 290 | } |
| 291 | |
| 292 | virtio_device_ready(vdev); |
| 293 | vi->ready = true; |
| 294 | err = input_register_device(vi->idev); |
| 295 | if (err) |
| 296 | goto err_input_register; |
| 297 | |
| 298 | virtinput_fill_evt(vi); |
| 299 | return 0; |
| 300 | |
| 301 | err_input_register: |
| 302 | spin_lock_irqsave(&vi->lock, flags); |
| 303 | vi->ready = false; |
| 304 | spin_unlock_irqrestore(&vi->lock, flags); |
| 305 | input_free_device(vi->idev); |
| 306 | err_input_alloc: |
| 307 | vdev->config->del_vqs(vdev); |
| 308 | err_init_vq: |
| 309 | kfree(vi); |
| 310 | return err; |
| 311 | } |
| 312 | |
| 313 | static void virtinput_remove(struct virtio_device *vdev) |
| 314 | { |
| 315 | struct virtio_input *vi = vdev->priv; |
Jason Wang | df4198b | 2015-08-06 13:54:21 +0800 | [diff] [blame] | 316 | void *buf; |
Gerd Hoffmann | 271c865 | 2015-03-27 12:46:12 +1030 | [diff] [blame] | 317 | unsigned long flags; |
| 318 | |
| 319 | spin_lock_irqsave(&vi->lock, flags); |
| 320 | vi->ready = false; |
| 321 | spin_unlock_irqrestore(&vi->lock, flags); |
| 322 | |
| 323 | input_unregister_device(vi->idev); |
Jason Wang | df4198b | 2015-08-06 13:54:21 +0800 | [diff] [blame] | 324 | vdev->config->reset(vdev); |
| 325 | while ((buf = virtqueue_detach_unused_buf(vi->sts)) != NULL) |
| 326 | kfree(buf); |
Gerd Hoffmann | 271c865 | 2015-03-27 12:46:12 +1030 | [diff] [blame] | 327 | vdev->config->del_vqs(vdev); |
| 328 | kfree(vi); |
| 329 | } |
| 330 | |
| 331 | #ifdef CONFIG_PM_SLEEP |
| 332 | static int virtinput_freeze(struct virtio_device *vdev) |
| 333 | { |
| 334 | struct virtio_input *vi = vdev->priv; |
| 335 | unsigned long flags; |
| 336 | |
| 337 | spin_lock_irqsave(&vi->lock, flags); |
| 338 | vi->ready = false; |
| 339 | spin_unlock_irqrestore(&vi->lock, flags); |
| 340 | |
| 341 | vdev->config->del_vqs(vdev); |
| 342 | return 0; |
| 343 | } |
| 344 | |
| 345 | static int virtinput_restore(struct virtio_device *vdev) |
| 346 | { |
| 347 | struct virtio_input *vi = vdev->priv; |
| 348 | int err; |
| 349 | |
| 350 | err = virtinput_init_vqs(vi); |
| 351 | if (err) |
| 352 | return err; |
| 353 | |
| 354 | virtio_device_ready(vdev); |
| 355 | vi->ready = true; |
| 356 | virtinput_fill_evt(vi); |
| 357 | return 0; |
| 358 | } |
| 359 | #endif |
| 360 | |
| 361 | static unsigned int features[] = { |
| 362 | /* none */ |
| 363 | }; |
| 364 | static struct virtio_device_id id_table[] = { |
| 365 | { VIRTIO_ID_INPUT, VIRTIO_DEV_ANY_ID }, |
| 366 | { 0 }, |
| 367 | }; |
| 368 | |
| 369 | static struct virtio_driver virtio_input_driver = { |
| 370 | .driver.name = KBUILD_MODNAME, |
| 371 | .driver.owner = THIS_MODULE, |
| 372 | .feature_table = features, |
| 373 | .feature_table_size = ARRAY_SIZE(features), |
| 374 | .id_table = id_table, |
| 375 | .probe = virtinput_probe, |
| 376 | .remove = virtinput_remove, |
| 377 | #ifdef CONFIG_PM_SLEEP |
| 378 | .freeze = virtinput_freeze, |
| 379 | .restore = virtinput_restore, |
| 380 | #endif |
| 381 | }; |
| 382 | |
| 383 | module_virtio_driver(virtio_input_driver); |
| 384 | MODULE_DEVICE_TABLE(virtio, id_table); |
| 385 | |
| 386 | MODULE_LICENSE("GPL"); |
| 387 | MODULE_DESCRIPTION("Virtio input device driver"); |
| 388 | MODULE_AUTHOR("Gerd Hoffmann <kraxel@redhat.com>"); |