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