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