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 <stdlib.h>
  24 #include <string.h>
  25 #include <unistd.h>
  26 #include <fcntl.h>
  27 #include <sys/select.h>
  28 #include "vdagent-virtio-port.h"
  29
  30 struct vdagent_virtio_port_buf {
  31     uint8_t *buf;
  32     size_t pos;
  33     size_t size;
  34
  35     struct vdagent_virtio_port_buf *next;
  36 };
  37
  38 struct vdagent_virtio_port {
  39     int fd;
  40     FILE *errfile;
  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     FILE *errfile)
  68 {
  69     struct vdagent_virtio_port *port;
  70
  71     port = calloc(1, sizeof(*port));
  72     if (!port)
  73         return 0;
  74
  75     port->errfile = errfile;
  76     port->fd = open(portname, O_RDWR);
  77     if (port->fd == -1) {
  78         fprintf(port->errfile, "open %s: %s\n", portname, strerror(errno));
  79         free(port);
  80         return NULL;
  81     }
  82
  83     port->read_callback = read_callback;
  84     port->disconnect_callback = disconnect_callback;
  85
  86     return port;
  87 }
  88
  89 void vdagent_virtio_port_destroy(struct vdagent_virtio_port **portp)
  90 {
  91     struct vdagent_virtio_port_buf *wbuf, *next_wbuf;
  92     struct vdagent_virtio_port *port = *portp;
  93
  94     if (!port)
  95         return;
  96
  97     if (port->disconnect_callback)
  98         port->disconnect_callback(port);
  99
 100     wbuf = port->write_buf;
 101     while (wbuf) {
 102         next_wbuf = wbuf->next;
 103         free(wbuf->buf);
 104         free(wbuf);
 105         wbuf = next_wbuf;
 106     }
 107
 108     free(port->message_data);
 109
 110     close(port->fd);
 111     free(port);
 112     *portp = NULL;
 113 }
 114
 115 int vdagent_virtio_port_fill_fds(struct vdagent_virtio_port *port,
 116         fd_set *readfds, fd_set *writefds)
 117 {
 118     if (!port)
 119         return -1;
 120
 121     FD_SET(port->fd, readfds);
 122     if (port->write_buf)
 123         FD_SET(port->fd, writefds);
 124
 125     return port->fd + 1;
 126 }
 127
 128 void vdagent_virtio_port_handle_fds(struct vdagent_virtio_port **portp,
 129         fd_set *readfds, fd_set *writefds)
 130 {
 131     if (!*portp)
 132         return;
 133
 134     if (FD_ISSET((*portp)->fd, readfds))
 135         vdagent_virtio_port_do_read(portp);
 136
 137     if (*portp && FD_ISSET((*portp)->fd, writefds))
 138         vdagent_virtio_port_do_write(portp);
 139 }
 140
 141 int vdagent_virtio_port_write(
 142         struct vdagent_virtio_port *port,
 143         uint32_t port_nr,
 144         uint32_t message_type,
 145         uint32_t message_opaque,
 146         const uint8_t *data,
 147         uint32_t data_size)
 148 {
 149     struct vdagent_virtio_port_buf *wbuf, *new_wbuf;
 150     VDIChunkHeader chunk_header;
 151     VDAgentMessage message_header;
 152
 153     new_wbuf = malloc(sizeof(*new_wbuf));
 154     if (!new_wbuf)
 155         return -1;
 156
 157     new_wbuf->pos = 0;
 158     new_wbuf->size = sizeof(chunk_header) + sizeof(message_header) + data_size;
 159     new_wbuf->next = NULL;
 160     new_wbuf->buf = malloc(new_wbuf->size);
 161     if (!new_wbuf->buf) {
 162         free(new_wbuf);
 163         return -1;
 164     }
 165
 166     chunk_header.port = port_nr;
 167     chunk_header.size = sizeof(message_header) + data_size;
 168     message_header.protocol = VD_AGENT_PROTOCOL;
 169     message_header.type = message_type;
 170     message_header.opaque = message_opaque;
 171     message_header.size = data_size;
 172
 173     memcpy(new_wbuf->buf, &chunk_header, sizeof(chunk_header));
 174     memcpy(new_wbuf->buf + sizeof(chunk_header), &message_header,
 175            sizeof(message_header));
 176     memcpy(new_wbuf->buf + sizeof(chunk_header) + sizeof(message_header),
 177            data, data_size);
 178
 179     if (!port->write_buf) {
 180         port->write_buf = new_wbuf;
 181         return 0;
 182     }
 183
 184     /* maybe we should limit the write_buf stack depth ? */
 185     wbuf = port->write_buf;
 186     while (wbuf->next)
 187         wbuf = wbuf->next;
 188
 189     wbuf->next = new_wbuf;
 190
 191     return 0;
 192 }
 193
 194 void vdagent_virtio_port_flush(struct vdagent_virtio_port **portp)
 195 {
 196     while (*portp && (*portp)->write_buf)
 197         vdagent_virtio_port_do_write(portp);
 198 }
 199
 200 static void vdagent_virtio_port_do_chunk(struct vdagent_virtio_port **portp)
 201 {
 202     int avail, read, pos = 0;
 203     struct vdagent_virtio_port *port = *portp;
 204
 205     if (port->message_header_read < sizeof(port->message_header)) {
 206         read = sizeof(port->message_header) - port->message_header_read;
 207         if (read > port->chunk_header.size) {
 208             read = port->chunk_header.size;
 209         }
 210         memcpy((uint8_t *)&port->message_header + port->message_header_read,
 211                port->chunk_data, read);
 212         port->message_header_read += read;
 213         if (port->message_header_read == sizeof(port->message_header) &&
 214                 port->message_header.size) {
 215             port->message_data = malloc(port->message_header.size);
 216             if (!port->message_data) {
 217                 fprintf(port->errfile, "out of memory, disconnecting virtio\n");
 218                 vdagent_virtio_port_destroy(portp);
 219                 return;
 220             }
 221         }
 222         pos = read;
 223     }
 224
 225     if (port->message_header_read == sizeof(port->message_header)) {
 226         read  = port->message_header.size - port->message_data_pos;
 227         avail = port->chunk_header.size - pos;
 228
 229         if (avail > read) {
 230             fprintf(port->errfile, "chunk larger then message, lost sync?\n");
 231             vdagent_virtio_port_destroy(portp);
 232             return;
 233         }
 234
 235         if (avail < read)
 236             read = avail;
 237
 238         if (read) {
 239             memcpy(port->message_data + port->message_data_pos,
 240                    port->chunk_data + pos, read);
 241             port->message_data_pos += read;
 242         }
 243
 244         if (port->message_data_pos == port->message_header.size) {
 245             if (port->read_callback) {
 246                 int r = port->read_callback(port, &port->chunk_header,
 247                                     &port->message_header, port->message_data);
 248                 if (r == -1) {
 249                     vdagent_virtio_port_destroy(portp);
 250                     return;
 251                 }
 252             }
 253             port->message_header_read = 0;
 254             port->message_data_pos = 0;
 255             free(port->message_data);
 256             port->message_data = NULL;
 257         }
 258     }
 259 }
 260
 261 static void vdagent_virtio_port_do_read(struct vdagent_virtio_port **portp)
 262 {
 263     ssize_t n;
 264     size_t to_read;
 265     uint8_t *dest;
 266     struct vdagent_virtio_port *port = *portp;
 267
 268     if (port->chunk_header_read < sizeof(port->chunk_header)) {
 269         to_read = sizeof(port->chunk_header) - port->chunk_header_read;
 270         dest = (uint8_t *)&port->chunk_header + port->chunk_header_read;
 271     } else {
 272         to_read = port->chunk_header.size - port->chunk_data_pos;
 273         dest = port->chunk_data + port->chunk_data_pos;
 274     }
 275
 276     n = read(port->fd, dest, to_read);
 277     if (n < 0) {
 278         if (errno == EINTR)
 279             return;
 280         fprintf(port->errfile, "reading from vdagent virtio port: %s\n",
 281                 strerror(errno));
 282     }
 283     if (n <= 0) {
 284         vdagent_virtio_port_destroy(portp);
 285         return;
 286     }
 287
 288     if (port->chunk_header_read < sizeof(port->chunk_header)) {
 289         port->chunk_header_read += n;
 290         if (port->chunk_header_read == sizeof(port->chunk_header)) {
 291             if (port->chunk_header.size > VD_AGENT_MAX_DATA_SIZE) {
 292                 fprintf(port->errfile, "chunk size too large\n");
 293                 vdagent_virtio_port_destroy(portp);
 294                 return;
 295             }
 296         }
 297     } else {
 298         port->chunk_data_pos += n;
 299         if (port->chunk_data_pos == port->chunk_header.size) {
 300             vdagent_virtio_port_do_chunk(portp);
 301             port->chunk_header_read = 0;
 302             port->chunk_data_pos = 0;
 303         }
 304     }
 305 }
 306
 307 static void vdagent_virtio_port_do_write(struct vdagent_virtio_port **portp)
 308 {
 309     ssize_t n;
 310     size_t to_write;
 311     struct vdagent_virtio_port *port = *portp;
 312
 313     struct vdagent_virtio_port_buf* wbuf = port->write_buf;
 314     if (!wbuf) {
 315         fprintf(port->errfile,
 316                 "do_write called on a port without a write buf ?!\n");
 317         return;
 318     }
 319
 320     to_write = wbuf->size - wbuf->pos;
 321     n = write(port->fd, wbuf->buf + wbuf->pos, to_write);
 322     if (n < 0) {
 323         if (errno == EINTR)
 324             return;
 325         fprintf(port->errfile, "writing to vdagent virtio port: %s\n",
 326                 strerror(errno));
 327         vdagent_virtio_port_destroy(portp);
 328         return;
 329     }
 330
 331     wbuf->pos += n;
 332     if (wbuf->pos == wbuf->size) {
 333         port->write_buf = wbuf->next;
 334         free(wbuf->buf);
 335         free(wbuf);
 336     }
 337 }