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 message_header_read;
  45     VDIChunkHeader chunk_header;
  46     VDAgentMessage message_header;
  47     struct vdagent_virtio_port_buf data;
  48
  49     /* Writes are stored in a linked list of buffers, with both the header
  50        + data for a single message in 1 buffer. */
  51     struct vdagent_virtio_port_buf *write_buf;
  52
  53     /* Callbacks */
  54     vdagent_virtio_port_read_callback read_callback;
  55     vdagent_virtio_port_disconnect_callback disconnect_callback;
  56 };
  57
  58 static void vdagent_virtio_port_do_write(struct vdagent_virtio_port **portp);
  59 static void vdagent_virtio_port_do_read(struct vdagent_virtio_port **portp);
  60
  61 struct vdagent_virtio_port *vdagent_virtio_port_create(const char *portname,
  62     vdagent_virtio_port_read_callback read_callback,
  63     vdagent_virtio_port_disconnect_callback disconnect_callback)
  64 {
  65     struct vdagent_virtio_port *port;
  66
  67     port = calloc(1, sizeof(*port));
  68     if (!port)
  69         return 0;
  70
  71     port->fd = open(portname, O_RDWR);
  72     if (port->fd == -1) {
  73         fprintf(stderr, "open %s: %s\n", portname, strerror(errno));
  74         free(port);
  75         return NULL;
  76     }
  77
  78     port->read_callback = read_callback;
  79     port->disconnect_callback = disconnect_callback;
  80
  81     return port;
  82 }
  83
  84 void vdagent_virtio_port_destroy(struct vdagent_virtio_port **portp)
  85 {
  86     struct vdagent_virtio_port_buf *wbuf, *next_wbuf;
  87     struct vdagent_virtio_port *port = *portp;
  88
  89     if (!port)
  90         return;
  91
  92     if (port->disconnect_callback)
  93         port->disconnect_callback(port);
  94
  95     wbuf = port->write_buf;
  96     while (wbuf) {
  97         next_wbuf = wbuf->next;
  98         free(wbuf->buf);
  99         free(wbuf);
 100         wbuf = next_wbuf;
 101     }
 102
 103     free(port->data.buf);
 104
 105     close(port->fd);
 106     free(port);
 107     *portp = NULL;
 108 }
 109
 110 int vdagent_virtio_port_fill_fds(struct vdagent_virtio_port *port,
 111         fd_set *readfds, fd_set *writefds)
 112 {
 113     if (!port)
 114         return -1;
 115
 116     FD_SET(port->fd, readfds);
 117     if (port->write_buf)
 118         FD_SET(port->fd, writefds);
 119
 120     return port->fd + 1;
 121 }
 122
 123 void vdagent_virtio_port_handle_fds(struct vdagent_virtio_port **portp,
 124         fd_set *readfds, fd_set *writefds)
 125 {
 126     if (!*portp)
 127         return;
 128
 129     if (FD_ISSET((*portp)->fd, readfds))
 130         vdagent_virtio_port_do_read(portp);
 131
 132     if (*portp && FD_ISSET((*portp)->fd, writefds))
 133         vdagent_virtio_port_do_write(portp);
 134 }
 135
 136 int vdagent_virtio_port_write(
 137         struct vdagent_virtio_port *port,
 138         uint32_t port_nr,
 139         uint32_t message_type,
 140         uint32_t message_opaque,
 141         const uint8_t *data,
 142         uint32_t data_size)
 143 {
 144     struct vdagent_virtio_port_buf *wbuf, *new_wbuf;
 145     VDIChunkHeader chunk_header;
 146     VDAgentMessage message_header;
 147
 148     new_wbuf = malloc(sizeof(*new_wbuf));
 149     if (!new_wbuf)
 150         return -1;
 151
 152     new_wbuf->pos = 0;
 153     new_wbuf->size = sizeof(chunk_header) + sizeof(message_header) + data_size;
 154     new_wbuf->next = NULL;
 155     new_wbuf->buf = malloc(new_wbuf->size);
 156     if (!new_wbuf->buf) {
 157         free(new_wbuf);
 158         return -1;
 159     }
 160
 161     chunk_header.port = port_nr;
 162     chunk_header.size = sizeof(message_header) + data_size;
 163     message_header.protocol = VD_AGENT_PROTOCOL;
 164     message_header.type = message_type;
 165     message_header.opaque = message_opaque;
 166     message_header.size = data_size;
 167
 168     memcpy(new_wbuf->buf, &chunk_header, sizeof(chunk_header));
 169     memcpy(new_wbuf->buf + sizeof(chunk_header), &message_header,
 170            sizeof(message_header));
 171     memcpy(new_wbuf->buf + sizeof(chunk_header) + sizeof(message_header),
 172            data, data_size);
 173
 174     if (!port->write_buf) {
 175         port->write_buf = new_wbuf;
 176         return 0;
 177     }
 178
 179     /* maybe we should limit the write_buf stack depth ? */
 180     wbuf = port->write_buf;
 181     while (wbuf->next)
 182         wbuf = wbuf->next;
 183
 184     wbuf->next = new_wbuf;
 185
 186     return 0;
 187 }
 188
 189 static void vdagent_virtio_port_do_read(struct vdagent_virtio_port **portp)
 190 {
 191     ssize_t n;
 192     size_t to_read;
 193     uint8_t *dest;
 194     int r;
 195     struct vdagent_virtio_port *port = *portp;
 196
 197     if (port->chunk_header_read < sizeof(port->chunk_header)) {
 198         to_read = sizeof(port->chunk_header) - port->chunk_header_read;
 199         dest = (uint8_t *)&port->chunk_header + port->chunk_header_read;
 200     } else if (port->message_header_read < sizeof(port->message_header)) {
 201         to_read = sizeof(port->message_header) - port->message_header_read;
 202         dest = (uint8_t *)&port->message_header + port->message_header_read;
 203     } else {
 204         to_read = port->data.size - port->data.pos;
 205         dest = port->data.buf + port->data.pos;
 206     }
 207
 208     n = read(port->fd, dest, to_read);
 209     if (n < 0) {
 210         if (errno == EINTR)
 211             return;
 212         perror("reading from vdagent virtio port");
 213     }
 214     if (n <= 0) {
 215         vdagent_virtio_port_destroy(portp);
 216         return;
 217     }
 218
 219     if (port->chunk_header_read < sizeof(port->chunk_header)) {
 220         port->chunk_header_read += n;
 221         if (port->chunk_header_read == sizeof(port->chunk_header)) {
 222             if (port->chunk_header.size < sizeof(port->message_header)) {
 223                 fprintf(stderr, "chunk size < message header size\n");
 224                 vdagent_virtio_port_destroy(portp);
 225                 return;
 226             }
 227             port->message_header_read = 0;
 228         }
 229     } else if (port->message_header_read < sizeof(port->message_header)) {
 230         port->message_header_read += n;
 231         if (port->message_header_read == sizeof(port->message_header)) {
 232             if (port->message_header.size !=
 233                     (port->chunk_header.size - sizeof(port->message_header))) {
 234                 fprintf(stderr,
 235                         "read: chunk vs message header size mismatch\n");
 236                 vdagent_virtio_port_destroy(portp);
 237                 return;
 238             }
 239             if (port->message_header.size == 0) {
 240                 if (port->read_callback) {
 241                     r = port->read_callback(port, &port->chunk_header,
 242                                             &port->message_header, NULL);
 243                     if (r == -1) {
 244                         vdagent_virtio_port_destroy(portp);
 245                         return;
 246                     }
 247                 }
 248                 port->chunk_header_read = 0;
 249                 port->message_header_read = 0;
 250             } else {
 251                 port->data.pos = 0;
 252                 port->data.size = port->message_header.size;
 253                 port->data.buf = malloc(port->data.size);
 254                 if (!port->data.buf) {
 255                     fprintf(stderr, "out of memory, disportecting client\n");
 256                     vdagent_virtio_port_destroy(portp);
 257                     return;
 258                 }
 259             }
 260         }
 261     } else {
 262         port->data.pos += n;
 263         if (port->data.pos == port->data.size) {
 264             if (port->read_callback) {
 265                 r = port->read_callback(port, &port->chunk_header,
 266                                         &port->message_header, port->data.buf);
 267                 if (r == -1) {
 268                     vdagent_virtio_port_destroy(portp);
 269                     return;
 270                 }
 271             }
 272             free(port->data.buf);
 273             port->chunk_header_read = 0;
 274             port->message_header_read = 0;
 275             memset(&port->data, 0, sizeof(port->data));
 276         }
 277     }
 278 }
 279
 280 static void vdagent_virtio_port_do_write(struct vdagent_virtio_port **portp)
 281 {
 282     ssize_t n;
 283     size_t to_write;
 284     struct vdagent_virtio_port *port = *portp;
 285
 286     struct vdagent_virtio_port_buf* wbuf = port->write_buf;
 287     if (!wbuf) {
 288         fprintf(stderr,
 289                 "do_write called on a port without a write buf ?!\n");
 290         return;
 291     }
 292
 293     to_write = wbuf->size - wbuf->pos;
 294     n = write(port->fd, wbuf->buf + wbuf->pos, to_write);
 295     if (n < 0) {
 296         if (errno == EINTR)
 297             return;
 298         perror("writing to vdagent virtio port");
 299         vdagent_virtio_port_destroy(portp);
 300         return;
 301     }
 302
 303     wbuf->pos += n;
 304     if (wbuf->pos == wbuf->size) {
 305         port->write_buf = wbuf->next;
 306         free(wbuf->buf);
 307         free(wbuf);
 308     }
 309 }