[PATCH 3/3] input: virtio_input: add sound support

[Ahmad Fatoum]

Virtio input devices have an output channel for LEDs and a beeper.
Register a sound card for the beeper if sound support is compiled
in and the virtualized device reports sound support via the event
config bits.

Signed-off-by: Ahmad Fatoum <ahmad at a3f.at>
---
 drivers/input/virtio_input.c | 105 ++++++++++++++++++++++++++++++++++-
 1 file changed, 102 insertions(+), 3 deletions(-)

diff --git a/drivers/input/virtio_input.c b/drivers/input/virtio_input.c
index 406dc613dc37..9c2e4d923f8d 100644
--- a/drivers/input/virtio_input.c
+++ b/drivers/input/virtio_input.c
@@ -6,6 +6,7 @@
 #include <linux/virtio_config.h>
 #include <linux/virtio_ring.h>
 #include <input/input.h>
+#include <sound.h>
 #include <dt-bindings/input/linux-event-codes.h>
 
 #include <uapi/linux/virtio_ids.h>
@@ -14,9 +15,11 @@
 struct virtio_input {
 	struct input_device        idev;
 	struct virtio_device       *vdev;
-	struct virtqueue           *evt;
+	struct virtqueue           *evt, *sts;
 	struct virtio_input_event  evts[64];
 	struct bthread             *bthread;
+	struct sound_card          beeper;
+	unsigned long              sndbit[BITS_TO_LONGS(SND_CNT)];
 };
 
 static void virtinput_queue_evtbuf(struct virtio_input *vi,
@@ -52,6 +55,51 @@ static int virtinput_recv_events(struct virtio_input *vi)
 	return i;
 }
 
+/*
+ * On error we are losing the status update, which isn't critical as
+ * this is used for the bell.
+ */
+static int virtinput_send_status(struct sound_card *beeper, unsigned freq, unsigned duration)
+{
+	struct virtio_input *vi = container_of(beeper, struct virtio_input, beeper);
+	struct virtio_input_event *stsbuf;
+	struct virtio_sg sg[1];
+	u16 code;
+	int rc;
+
+	stsbuf = kzalloc(sizeof(*stsbuf), 0);
+	if (!stsbuf)
+		return -ENOMEM;
+
+	code = vi->sndbit[0] & BIT_MASK(SND_TONE) ? SND_TONE : SND_BELL;
+
+	stsbuf->type  = cpu_to_le16(EV_SND);
+	stsbuf->code  = cpu_to_le16(code);
+	stsbuf->value = cpu_to_le32(freq);
+	virtio_sg_init_one(sg, stsbuf, sizeof(*stsbuf));
+
+	rc = virtqueue_add_outbuf(vi->sts, sg, 1);
+	virtqueue_kick(vi->sts);
+
+	if (rc != 0)
+		kfree(stsbuf);
+	return rc;
+}
+
+static int virtinput_recv_status(struct virtio_input *vi)
+{
+	struct virtio_input_event *stsbuf;
+	unsigned int len;
+	int i = 0;
+
+	while ((stsbuf = virtqueue_get_buf(vi->sts, &len)) != NULL) {
+		kfree(stsbuf);
+		i++;
+	}
+
+	return i;
+}
+
 static int virtinput_poll_vqs(void *_vi)
 {
 	struct virtio_input *vi = _vi;
@@ -60,6 +108,7 @@ static int virtinput_poll_vqs(void *_vi)
 		int bufs = 0;
 
 		bufs += virtinput_recv_events(vi);
+		bufs += virtinput_recv_status(vi);
 
 		if (bufs)
 			virtqueue_kick(vi->evt);
@@ -79,6 +128,37 @@ static u8 virtinput_cfg_select(struct virtio_input *vi,
 	return size;
 }
 
+static void virtinput_cfg_bits(struct virtio_input *vi, int select, int subsel,
+			       unsigned long *bits, unsigned int bitcount)
+{
+	unsigned int bit;
+	u8 *virtio_bits;
+	u8 bytes;
+
+	bytes = virtinput_cfg_select(vi, select, subsel);
+	if (!bytes)
+		return;
+	if (bitcount > bytes * 8)
+		bitcount = bytes * 8;
+
+	/*
+	 * Bitmap in virtio config space is a simple stream of bytes,
+	 * with the first byte carrying bits 0-7, second bits 8-15 and
+	 * so on.
+	 */
+	virtio_bits = kzalloc(bytes, GFP_KERNEL);
+	if (!virtio_bits)
+		return;
+	virtio_cread_bytes(vi->vdev, offsetof(struct virtio_input_config,
+					      u.bitmap),
+			   virtio_bits, bytes);
+	for (bit = 0; bit < bitcount; bit++) {
+		if (virtio_bits[bit / 8] & (1 << (bit % 8)))
+			__set_bit(bit, bits);
+	}
+	kfree(virtio_bits);
+}
+
 static void virtinput_fill_evt(struct virtio_input *vi)
 {
 	int i, size;
@@ -93,15 +173,16 @@ static void virtinput_fill_evt(struct virtio_input *vi)
 
 static int virtinput_init_vqs(struct virtio_input *vi)
 {
-	struct virtqueue *vqs[1];
+	struct virtqueue *vqs[2];
 	int err;
 
 
-	err = virtio_find_vqs(vi->vdev, 1, vqs);
+	err = virtio_find_vqs(vi->vdev, 2, vqs);
 	if (err)
 		return err;
 
 	vi->evt = vqs[0];
+	vi->sts = vqs[1];
 
 	return 0;
 }
@@ -132,6 +213,9 @@ static int virtinput_probe(struct virtio_device *vdev)
 			   name, min(size, sizeof(name)));
 	name[size] = '\0';
 
+	virtinput_cfg_bits(vi, VIRTIO_INPUT_CFG_EV_BITS, EV_SND,
+			   vi->sndbit, SND_CNT);
+
 	virtio_device_ready(vdev);
 
 	err = input_device_register(&vi->idev);
@@ -147,6 +231,21 @@ static int virtinput_probe(struct virtio_device *vdev)
 		goto err_bthread_run;
 	}
 
+	if (IS_ENABLED(CONFIG_SOUND) &&
+	    (vi->sndbit[0] & (BIT_MASK(SND_BELL) | BIT_MASK(SND_TONE)))) {
+		struct sound_card *beeper;
+
+		beeper = &vi->beeper;
+		beeper->name = basprintf("%s/beeper0", dev_name(&vdev->dev));
+		beeper->beep = virtinput_send_status;
+
+		err = sound_card_register(&vi->beeper);
+		if (err)
+			dev_warn(&vdev->dev, "bell registration failed: %pe\n", ERR_PTR(err));
+		else
+			dev_info(&vdev->dev, "bell registered\n");
+	}
+
 	dev_info(&vdev->dev, "'%s' probed\n", name);
 
 	return 0;
-- 
2.30.0