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         memcpy((uint8_t *)&port->message_header + port->message_header_read,
 208                port->chunk_data, read);
 209         port->message_header_read += read;
 210         if (port->message_header_read == sizeof(port->message_header) &&
 211                 port->message_header.size) {
 212             port->message_data = malloc(port->message_header.size);
 213             if (!port->message_data) {
 214                 fprintf(port->errfile, "out of memory, disconnecting virtio\n");
 215                 vdagent_virtio_port_destroy(portp);
 216                 return;
 217             }
 218         }
 219         pos = read;
 220     }
 221
 222     if (port->message_header_read == sizeof(port->message_header)) {
 223         read  = port->message_header.size - port->message_data_pos;
 224         avail = port->chunk_header.size - pos;
 225
 226         if (avail > read) {
 227             fprintf(port->errfile, "chunk larger then message, lost sync?\n");
 228             vdagent_virtio_port_destroy(portp);
 229             return;
 230         }
 231
 232         if (avail < read)
 233             read = avail;
 234
 235         if (read) {
 236             memcpy(port->message_data + port->message_data_pos,
 237                    port->chunk_data + pos, read);
 238             port->message_data_pos += read;
 239         }
 240
 241         if (port->message_data_pos == port->message_header.size) {
 242             if (port->read_callback) {
 243                 int r = port->read_callback(port, &port->chunk_header,
 244                                     &port->message_header, port->message_data);
 245                 if (r == -1) {
 246                     vdagent_virtio_port_destroy(portp);
 247                     return;
 248                 }
 249             }
 250             port->message_header_read = 0;
 251             port->message_data_pos = 0;
 252             free(port->message_data);
 253             port->message_data = NULL;
 254         }
 255     }
 256 }
 257
 258 static void vdagent_virtio_port_do_read(struct vdagent_virtio_port **portp)
 259 {
 260     ssize_t n;
 261     size_t to_read;
 262     uint8_t *dest;
 263     struct vdagent_virtio_port *port = *portp;
 264
 265     if (port->chunk_header_read < sizeof(port->chunk_header)) {
 266         to_read = sizeof(port->chunk_header) - port->chunk_header_read;
 267         dest = (uint8_t *)&port->chunk_header + port->chunk_header_read;
 268     } else {
 269         to_read = port->chunk_header.size - port->chunk_data_pos;
 270         dest = port->chunk_data + port->chunk_data_pos;
 271     }
 272
 273     n = read(port->fd, dest, to_read);
 274     if (n < 0) {
 275         if (errno == EINTR)
 276             return;
 277         fprintf(port->errfile, "reading from vdagent virtio port: %s\n",
 278                 strerror(errno));
 279     }
 280     if (n <= 0) {
 281         vdagent_virtio_port_destroy(portp);
 282         return;
 283     }
 284
 285     if (port->chunk_header_read < sizeof(port->chunk_header)) {
 286         port->chunk_header_read += n;
 287         if (port->chunk_header_read == sizeof(port->chunk_header)) {
 288             if (port->chunk_header.size > VD_AGENT_MAX_DATA_SIZE) {
 289                 fprintf(port->errfile, "chunk size too large\n");
 290                 vdagent_virtio_port_destroy(portp);
 291                 return;
 292             }
 293         }
 294     } else {
 295         port->chunk_data_pos += n;
 296         if (port->chunk_data_pos == port->chunk_header.size) {
 297             vdagent_virtio_port_do_chunk(portp);
 298             port->chunk_header_read = 0;
 299             port->chunk_data_pos = 0;
 300         }
 301     }
 302 }
 303
 304 static void vdagent_virtio_port_do_write(struct vdagent_virtio_port **portp)
 305 {
 306     ssize_t n;
 307     size_t to_write;
 308     struct vdagent_virtio_port *port = *portp;
 309
 310     struct vdagent_virtio_port_buf* wbuf = port->write_buf;
 311     if (!wbuf) {
 312         fprintf(port->errfile,
 313                 "do_write called on a port without a write buf ?!\n");
 314         return;
 315     }
 316
 317     to_write = wbuf->size - wbuf->pos;
 318     n = write(port->fd, wbuf->buf + wbuf->pos, to_write);
 319     if (n < 0) {
 320         if (errno == EINTR)
 321             return;
 322         fprintf(port->errfile, "writing to vdagent virtio port: %s\n",
 323                 strerror(errno));
 324         vdagent_virtio_port_destroy(portp);
 325         return;
 326     }
 327
 328     wbuf->pos += n;
 329     if (wbuf->pos == wbuf->size) {
 330         port->write_buf = wbuf->next;
 331         free(wbuf->buf);
 332         free(wbuf);
 333     }
 334 }