1 #include <arch/io-mem.h>
2 #include <assert.h>
3 #include <stdbool.h>
4 #include <test-runner-arch.h>
5 #include <trusty/sysdeps.h>
6 #include <utils.h>
7 #include <virtio-console.h>
8 #include <virtio-device.h>
9 #include <virtio.h>
10 
11 #define DESIRED_FEATURES (VIRTIO_CONSOLE_F_MULTIPORT)
12 
13 static struct virtio_console console;
14 
15 static struct virtq cmd_input;
16 static struct virtq_raw cmd_input_raw;
17 static struct virtq cmd_output;
18 static struct virtq_raw cmd_output_raw;
19 
20 struct console_control {
21     uint32_t id;
22     uint16_t event;
23     uint16_t value;
24     char buf[];
25 };
26 
27 /*
28  * The biggest message we need to read for console control is a console
29  * message plus the biggest name we're willing to put on a console.
30  */
31 #define CMSG_IN_MAX (sizeof(struct console_control) + MAX_PORT_NAME_SIZE)
32 static char cmsg_buf[VQ_SIZE][CMSG_IN_MAX];
33 
send_control_msg(const struct console_control * msg)34 static void send_control_msg(const struct console_control* msg) {
35     send_vq(&cmd_output, (const void*)msg, sizeof(*msg));
36 }
37 
38 /* Check if a vq could give us a buffer now */
vq_quick_ready(struct virtq * vq)39 static bool vq_quick_ready(struct virtq* vq) {
40     if (!vq_ready(vq)) {
41         vq_kick(vq);
42     }
43     return vq_ready(vq);
44 }
45 
46 /*
47  * Gets the next control message in the channel, returning NULL if none are
48  * immediately available.
49  *
50  * To implement this, we need a slightly different approach than our other
51  * queues. We keep a buffer array in the available queue, and check whether
52  * queue is ready for retrieving message.
53  *
54  * Once done reading the message, the caller must call release_control_msg
55  * to relinquish ownership of the message before calling get_control_msg
56  * again.
57  */
get_control_msg(size_t * buf_size,size_t * msg_idx)58 static const struct console_control* get_control_msg(size_t* buf_size,
59                                                      size_t* msg_idx) {
60     uint32_t idx = cmd_input.old_used_idx % cmd_input.num_bufs;
61 
62     if (!vq_quick_ready(&cmd_input)) {
63         return NULL;
64     }
65 
66     *buf_size = vq_adv(&cmd_input);
67     *msg_idx = idx;
68 
69     return (struct console_control*)cmsg_buf[idx];
70 }
71 
72 /*
73  * Releases ownership of the control_msg from get_control_msg, enabling
74  * get_control_msg to be called again.
75  */
release_control_msg(size_t idx)76 static void release_control_msg(size_t idx) {
77     vq_set_buf_w(&cmd_input, idx, cmsg_buf[idx], CMSG_IN_MAX);
78     vq_make_avail(&cmd_input, idx);
79 }
80 
control_setup(struct virtio_config * vio)81 static void control_setup(struct virtio_config* vio) {
82     uint32_t idx = 0;
83 
84     vq_init(&cmd_input, &cmd_input_raw, vio, true);
85     vq_init(&cmd_output, &cmd_output_raw, vio, false);
86     vq_attach(&cmd_input, VIRTIO_CONSOLE_CTRL_RX);
87     vq_attach(&cmd_output, VIRTIO_CONSOLE_CTRL_TX);
88     for (idx = 0; idx < cmd_input.num_bufs; idx++) {
89         vq_set_buf_w(&cmd_input, idx, cmsg_buf[idx], CMSG_IN_MAX);
90         vq_make_avail(&cmd_input, idx);
91     }
92 }
93 
port_open(struct virtio_console * console,size_t port_id)94 static void port_open(struct virtio_console* console, size_t port_id) {
95     const struct console_control connect_msg = {
96             .event = VIRTIO_CONSOLE_PORT_OPEN,
97             .id = port_id,
98             .value = 1,
99     };
100     send_control_msg(&connect_msg);
101     console->ports[port_id].guest_connected = true;
102 }
103 
104 /*
105  * Finds the first queue for a given port.
106  * The layout is in0, out0, control_in, control_out, in1, out1, in2, out2, ...
107  */
virtio_console_q(size_t port_id)108 static uint16_t virtio_console_q(size_t port_id) {
109     return port_id * 2 + ((port_id >= 1) ? 2 : 0);
110 }
111 
virtio_console_connect_port(struct virtio_console * console,size_t port_id,struct virtq * vq_in,struct virtq * vq_out)112 void virtio_console_connect_port(struct virtio_console* console,
113                                  size_t port_id,
114                                  struct virtq* vq_in,
115                                  struct virtq* vq_out) {
116     /* Attach the virtqueues to the relevant queue IDs */
117     vq_attach(vq_in, virtio_console_q(port_id) + VIRTIO_CONSOLE_RX_OFFSET);
118     vq_attach(vq_out, virtio_console_q(port_id) + VIRTIO_CONSOLE_TX_OFFSET);
119 
120     /* Use the control queue to tell the host we are ready */
121     port_open(console, port_id);
122 }
123 
port_ready(struct virtio_console * console,size_t port_id)124 static void port_ready(struct virtio_console* console, size_t port_id) {
125     assert(console->ports[port_id].host_connected);
126     struct console_control msg_send = {
127             .id = port_id,
128             .event = VIRTIO_CONSOLE_PORT_READY,
129             .value = 1,
130     };
131     send_control_msg(&msg_send);
132 }
133 
control_scan(struct virtio_console * console)134 static void control_scan(struct virtio_console* console) {
135     size_t buf_size;
136     size_t msg_idx;
137     const struct console_control* msg;
138     for (size_t i = 0; i < MAX_PORTS; i++) {
139         console->ports[i].host_connected = false;
140         console->ports[i].guest_connected = false;
141         console->ports[i].name[0] = 0;
142     }
143 
144     const struct console_control dev_ready = {
145             .event = VIRTIO_CONSOLE_DEVICE_READY,
146             .value = 1,
147     };
148 
149     send_control_msg(&dev_ready);
150 
151     while ((msg = get_control_msg(&buf_size, &msg_idx))) {
152         switch (msg->event) {
153         case VIRTIO_CONSOLE_DEVICE_ADD:
154             console->ports[msg->id].host_connected = true;
155             console->ports[msg->id].name[0] = 0;
156             /*
157              * Must be released before port_ready is called, or QEMU will
158              * drop the response packet on the ground.
159              */
160             release_control_msg(msg_idx);
161             port_ready(console, msg->id);
162             break;
163         case VIRTIO_CONSOLE_DEVICE_REMOVE:
164             console->ports[msg->id].host_connected = false;
165             release_control_msg(msg_idx);
166             break;
167         case VIRTIO_CONSOLE_PORT_NAME:
168             buf_size = MIN(buf_size, MAX_PORT_NAME_SIZE - 1);
169             trusty_memcpy(console->ports[msg->id].name, msg->buf, buf_size);
170             console->ports[msg->id].name[buf_size] = 0;
171             release_control_msg(msg_idx);
172             break;
173         case VIRTIO_CONSOLE_DEVICE_READY:
174         case VIRTIO_CONSOLE_CONSOLE_PORT:
175         case VIRTIO_CONSOLE_RESIZE:
176         case VIRTIO_CONSOLE_PORT_OPEN:
177         default:
178             release_control_msg(msg_idx);
179             break;
180         }
181     }
182 }
183 
init_virtio_console(void)184 struct virtio_console* init_virtio_console(void) {
185     struct virtio_config* console_vio = virtio_probe_console();
186     if (!console_vio) {
187         /* We didn't find a legacy multiport console */
188         return NULL;
189     }
190 
191     /* Reset device */
192     virtio_reset_device(console_vio);
193 
194     /* Acknowledge device */
195     virtio_or_status(console_vio, VIRTIO_STATUS_ACKNOWLEDGE);
196 
197     /* Set driver bit */
198     virtio_or_status(console_vio, VIRTIO_STATUS_DRIVER);
199 
200     /* Check that our features are available */
201     uint64_t features = virtio_get_features(console_vio);
202     assert((features & DESIRED_FEATURES) == DESIRED_FEATURES);
203 
204     /* Write desired feature bits */
205     virtio_set_features(console_vio, DESIRED_FEATURES);
206 
207     /* We are done negotiating features */
208     virtio_or_status(console_vio, VIRTIO_STATUS_FEATURES_OK);
209 
210     /* The device accepted our features */
211     assert(virtio_get_status(console_vio) & VIRTIO_STATUS_FEATURES_OK);
212 
213     /* Set up control virtqueues */
214     virtio_set_guest_page_size(console_vio, PAGE_SIZE);
215 
216     control_setup(console_vio);
217     console.vio = console_vio;
218 
219     /* We are now able to drive the device */
220     virtio_or_status(console_vio, VIRTIO_STATUS_DRIVER_OK);
221 
222     /* Chat with the control queues to update our ports */
223     control_scan(&console);
224 
225     return &console;
226 }
227