vdagent-virtio-port.c

   1 /*  vdagent-virtio-port.c virtio port communication code
   2
   3     Copyright 2010 Red Hat, Inc.
   4
   5     Red Hat Authors:
   6     Hans de Goede <hdegoede@redhat.com>
   7
   8     This program is free software: you can redistribute it and/or modify
   9     it under the terms of the GNU General Public License as published by
  10     the Free Software Foundation, either version 3 of the License, or
  11     (at your option) any later version.
  12
  13     This program is distributed in the hope that it will be useful,
  14     but WITHOUT ANY WARRANTY; without even the implied warranty of
  15     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16     GNU General Public License for more details.
  17
  18     You should have received a copy of the GNU General Public License
  19     along with this program.  If not, see <http://www.gnu.org/licenses/>.
  20 */
  21
  22 #include <errno.h>
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26 #include <unistd.h>
  27 #include <fcntl.h>
  28 #include <sys/select.h>
  29 #include "vdagent-virtio-port.h"
  30
  31 struct vdagent_virtio_port_buf {
  32     uint8_t *buf;
  33     size_t pos;
  34     size_t size;
  35
  36     struct vdagent_virtio_port_buf *next;
  37 };
  38
  39 struct vdagent_virtio_port {
  40     int fd;
  41
  42     /* Read stuff, single buffer, separate header and data buffer */
  43     int chunk_header_read;
  44     int chunk_data_pos;
  45     int message_header_read;
  46     int message_data_pos;
  47     VDIChunkHeader chunk_header;
  48     VDAgentMessage message_header;
  49     uint8_t chunk_data[VD_AGENT_MAX_DATA_SIZE];
  50     uint8_t *message_data;
  51
  52     /* Writes are stored in a linked list of buffers, with both the header
  53        + data for a single message in 1 buffer. */
  54     struct vdagent_virtio_port_buf *write_buf;
  55
  56     /* Callbacks */
  57     vdagent_virtio_port_read_callback read_callback;
  58     vdagent_virtio_port_disconnect_callback disconnect_callback;
  59 };
  60
  61 static void vdagent_virtio_port_do_write(struct vdagent_virtio_port **portp);
  62 static void vdagent_virtio_port_do_read(struct vdagent_virtio_port **portp);
  63
  64 struct vdagent_virtio_port *vdagent_virtio_port_create(const char *portname,
  65     vdagent_virtio_port_read_callback read_callback,
  66     vdagent_virtio_port_disconnect_callback disconnect_callback)
  67 {
  68     struct vdagent_virtio_port *port;
  69
  70     port = calloc(1, sizeof(*port));
  71     if (!port)
  72         return 0;
  73
  74     port->fd = open(portname, O_RDWR);
  75     if (port->fd == -1) {
  76         fprintf(stderr, "open %s: %s\n", portname, strerror(errno));
  77         free(port);
  78         return NULL;
  79     }
  80
  81     port->read_callback = read_callback;
  82     port->disconnect_callback = disconnect_callback;
  83
  84     return port;
  85 }
  86
  87 void vdagent_virtio_port_destroy(struct vdagent_virtio_port **portp)
  88 {
  89     struct vdagent_virtio_port_buf *wbuf, *next_wbuf;
  90     struct vdagent_virtio_port *port = *portp;
  91
  92     if (!port)
  93         return;
  94
  95     if (port->disconnect_callback)
  96         port->disconnect_callback(port);
  97
  98     wbuf = port->write_buf;
  99     while (wbuf) {
 100         next_wbuf = wbuf->next;
 101         free(wbuf->buf);
 102         free(wbuf);
 103         wbuf = next_wbuf;
 104     }
 105
 106     free(port->message_data);
 107
 108     close(port->fd);
 109     free(port);
 110     *portp = NULL;
 111 }
 112
 113 int vdagent_virtio_port_fill_fds(struct vdagent_virtio_port *port,
 114         fd_set *readfds, fd_set *writefds)
 115 {
 116     if (!port)
 117         return -1;
 118
 119     FD_SET(port->fd, readfds);
 120     if (port->write_buf)
 121         FD_SET(port->fd, writefds);
 122
 123     return port->fd + 1;
 124 }
 125
 126 void vdagent_virtio_port_handle_fds(struct vdagent_virtio_port **portp,
 127         fd_set *readfds, fd_set *writefds)
 128 {
 129     if (!*portp)
 130         return;
 131
 132     if (FD_ISSET((*portp)->fd, readfds))
 133         vdagent_virtio_port_do_read(portp);
 134
 135     if (*portp && FD_ISSET((*portp)->fd, writefds))
 136         vdagent_virtio_port_do_write(portp);
 137 }
 138
 139 int vdagent_virtio_port_write(
 140         struct vdagent_virtio_port *port,
 141         uint32_t port_nr,
 142         uint32_t message_type,
 143         uint32_t message_opaque,
 144         const uint8_t *data,
 145         uint32_t data_size)
 146 {
 147     struct vdagent_virtio_port_buf *wbuf, *new_wbuf;
 148     VDIChunkHeader chunk_header;
 149     VDAgentMessage message_header;
 150
 151     new_wbuf = malloc(sizeof(*new_wbuf));
 152     if (!new_wbuf)
 153         return -1;
 154
 155     new_wbuf->pos = 0;
 156     new_wbuf->size = sizeof(chunk_header) + sizeof(message_header) + data_size;
 157     new_wbuf->next = NULL;
 158     new_wbuf->buf = malloc(new_wbuf->size);
 159     if (!new_wbuf->buf) {
 160         free(new_wbuf);
 161         return -1;
 162     }
 163
 164     chunk_header.port = port_nr;
 165     chunk_header.size = sizeof(message_header) + data_size;
 166     message_header.protocol = VD_AGENT_PROTOCOL;
 167     message_header.type = message_type;
 168     message_header.opaque = message_opaque;
 169     message_header.size = data_size;
 170
 171     memcpy(new_wbuf->buf, &chunk_header, sizeof(chunk_header));
 172     memcpy(new_wbuf->buf + sizeof(chunk_header), &message_header,
 173            sizeof(message_header));
 174     memcpy(new_wbuf->buf + sizeof(chunk_header) + sizeof(message_header),
 175            data, data_size);
 176
 177     if (!port->write_buf) {
 178         port->write_buf = new_wbuf;
 179         return 0;
 180     }
 181
 182     /* maybe we should limit the write_buf stack depth ? */
 183     wbuf = port->write_buf;
 184     while (wbuf->next)
 185         wbuf = wbuf->next;
 186
 187     wbuf->next = new_wbuf;
 188
 189     return 0;
 190 }
 191
 192 static void vdagent_virtio_port_do_chunk(struct vdagent_virtio_port **portp)
 193 {
 194     int avail, read, pos = 0;
 195     struct vdagent_virtio_port *port = *portp;
 196
 197     if (port->message_header_read < sizeof(port->message_header)) {
 198         read = sizeof(port->message_header) - port->message_header_read;
 199         memcpy((uint8_t *)&port->message_header + port->message_header_read,
 200                port->chunk_data, read);
 201         port->message_header_read += read;
 202         if (port->message_header_read == sizeof(port->message_header) &&
 203                 port->message_header.size) {
 204             port->message_data = malloc(port->message_header.size);
 205             if (!port->message_data) {
 206                 fprintf(stderr, "out of memory, disconnecting virtio\n");
 207                 vdagent_virtio_port_destroy(portp);
 208                 return;
 209             }
 210         }
 211         pos = read;
 212     }
 213
 214     if (port->message_header_read == sizeof(port->message_header)) {
 215         read  = port->message_header.size - port->message_data_pos;
 216         avail = port->chunk_header.size - pos;
 217
 218         if (avail > read) {
 219             fprintf(stderr, "chunk larger then message, lost sync?\n");
 220             vdagent_virtio_port_destroy(portp);
 221             return;
 222         }
 223
 224         if (avail < read)
 225             read = avail;
 226
 227         if (read) {
 228             memcpy(port->message_data + port->message_data_pos,
 229                    port->chunk_data + pos, read);
 230             port->message_data_pos += read;
 231         }
 232
 233         if (port->message_data_pos == port->message_header.size) {
 234             if (port->read_callback) {
 235                 int r = port->read_callback(port, &port->chunk_header,
 236                                     &port->message_header, port->message_data);
 237                 if (r == -1) {
 238                     vdagent_virtio_port_destroy(portp);
 239                     return;
 240                 }
 241             }
 242             port->message_header_read = 0;
 243             port->message_data_pos = 0;
 244             free(port->message_data);
 245             port->message_data = NULL;
 246         }
 247     }
 248 }
 249
 250 static void vdagent_virtio_port_do_read(struct vdagent_virtio_port **portp)
 251 {
 252     ssize_t n;
 253     size_t to_read;
 254     uint8_t *dest;
 255     struct vdagent_virtio_port *port = *portp;
 256
 257     if (port->chunk_header_read < sizeof(port->chunk_header)) {
 258         to_read = sizeof(port->chunk_header) - port->chunk_header_read;
 259         dest = (uint8_t *)&port->chunk_header + port->chunk_header_read;
 260     } else {
 261         to_read = port->chunk_header.size - port->chunk_data_pos;
 262         dest = port->chunk_data + port->chunk_data_pos;
 263     }
 264
 265     n = read(port->fd, dest, to_read);
 266     if (n < 0) {
 267         if (errno == EINTR)
 268             return;
 269         perror("reading from vdagent virtio port");
 270     }
 271     if (n <= 0) {
 272         vdagent_virtio_port_destroy(portp);
 273         return;
 274     }
 275
 276     if (port->chunk_header_read < sizeof(port->chunk_header)) {
 277         port->chunk_header_read += n;
 278         if (port->chunk_header_read == sizeof(port->chunk_header)) {
 279             if (port->chunk_header.size > VD_AGENT_MAX_DATA_SIZE) {
 280                 fprintf(stderr, "chunk size too large\n");
 281                 vdagent_virtio_port_destroy(portp);
 282                 return;
 283             }
 284         }
 285     } else {
 286         port->chunk_data_pos += n;
 287         if (port->chunk_data_pos == port->chunk_header.size) {
 288             vdagent_virtio_port_do_chunk(portp);
 289             port->chunk_header_read = 0;
 290             port->chunk_data_pos = 0;
 291         }
 292     }
 293 }
 294
 295 static void vdagent_virtio_port_do_write(struct vdagent_virtio_port **portp)
 296 {
 297     ssize_t n;
 298     size_t to_write;
 299     struct vdagent_virtio_port *port = *portp;
 300
 301     struct vdagent_virtio_port_buf* wbuf = port->write_buf;
 302     if (!wbuf) {
 303         fprintf(stderr,
 304                 "do_write called on a port without a write buf ?!\n");
 305         return;
 306     }
 307
 308     to_write = wbuf->size - wbuf->pos;
 309     n = write(port->fd, wbuf->buf + wbuf->pos, to_write);
 310     if (n < 0) {
 311         if (errno == EINTR)
 312             return;
 313         perror("writing to vdagent virtio port");
 314         vdagent_virtio_port_destroy(portp);
 315         return;
 316     }
 317
 318     wbuf->pos += n;
 319     if (wbuf->pos == wbuf->size) {
 320         port->write_buf = wbuf->next;
 321         free(wbuf->buf);
 322         free(wbuf);
 323     }
 324 }