usr.sbin/bhyve/rfb.c

   1 /*-
   2  * Copyright (c) 2015 Tycho Nightingale <tycho.nightingale@pluribusnetworks.com>
   3  * Copyright (c) 2015 Leon Dang
   4  * All rights reserved.
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions
   8  * are met:
   9  * 1. Redistributions of source code must retain the above copyright
  10  *    notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  *    notice, this list of conditions and the following disclaimer in the
  13  *    documentation and/or other materials provided with the distribution.
  14  *
  15  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND
  16  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  18  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  19  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  20  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  21  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  22  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  23  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  24  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  25  * SUCH DAMAGE.
  26  */
  27
  28 #include <sys/cdefs.h>
  29 __FBSDID("$FreeBSD$");
  30
  31 #include <sys/param.h>
  32 #ifndef WITHOUT_CAPSICUM
  33 #include <sys/capsicum.h>
  34 #endif
  35 #include <sys/socket.h>
  36 #include <sys/select.h>
  37 #include <sys/time.h>
  38 #include <arpa/inet.h>
  39 #include <machine/cpufunc.h>
  40 #include <machine/specialreg.h>
  41 #include <netinet/in.h>
  42
  43 #include <assert.h>
  44 #include <err.h>
  45 #include <errno.h>
  46 #include <pthread.h>
  47 #include <pthread_np.h>
  48 #include <signal.h>
  49 #include <stdbool.h>
  50 #include <stdlib.h>
  51 #include <stdio.h>
  52 #include <string.h>
  53 #include <sysexits.h>
  54 #include <unistd.h>
  55
  56 #include <zlib.h>
  57
  58 #include "bhyvegc.h"
  59 #include "console.h"
  60 #include "rfb.h"
  61 #include "sockstream.h"
  62
  63 static int rfb_debug = 0;
  64 #define DPRINTF(params) if (rfb_debug) printf params
  65 #define WPRINTF(params) printf params
  66
  67 struct rfb_softc {
  68         int             sfd;
  69         pthread_t       tid;
  70
  71         int             cfd;
  72
  73         int             width, height;
  74
  75         bool            enc_raw_ok;
  76         bool            enc_zlib_ok;
  77         bool            enc_resize_ok;
  78
  79         z_stream        zstream;
  80         uint8_t         *zbuf;
  81         int             zbuflen;
  82
  83         int             conn_wait;
  84         int             sending;
  85         pthread_mutex_t mtx;
  86         pthread_cond_t  cond;
  87
  88         int             hw_crc;
  89         uint32_t        *crc;           /* WxH crc cells */
  90         uint32_t        *crc_tmp;       /* buffer to store single crc row */
  91         int             crc_width, crc_height;
  92 };
  93
  94 struct rfb_pixfmt {
  95         uint8_t         bpp;
  96         uint8_t         depth;
  97         uint8_t         bigendian;
  98         uint8_t         truecolor;
  99         uint16_t        red_max;
 100         uint16_t        green_max;
 101         uint16_t        blue_max;
 102         uint8_t         red_shift;
 103         uint8_t         green_shift;
 104         uint8_t         blue_shift;
 105         uint8_t         pad[3];
 106 };
 107
 108 struct rfb_srvr_info {
 109         uint16_t                width;
 110         uint16_t                height;
 111         struct rfb_pixfmt       pixfmt;
 112         uint32_t                namelen;
 113 };
 114
 115 struct rfb_pixfmt_msg {
 116         uint8_t                 type;
 117         uint8_t                 pad[3];
 118         struct rfb_pixfmt       pixfmt;
 119 };
 120
 121 #define RFB_ENCODING_RAW                0
 122 #define RFB_ENCODING_ZLIB               6
 123 #define RFB_ENCODING_RESIZE             -223
 124
 125 #define RFB_MAX_WIDTH                   2000
 126 #define RFB_MAX_HEIGHT                  1200
 127 #define RFB_ZLIB_BUFSZ                  RFB_MAX_WIDTH*RFB_MAX_HEIGHT*4
 128
 129 /* percentage changes to screen before sending the entire screen */
 130 #define RFB_SEND_ALL_THRESH             25
 131
 132 struct rfb_enc_msg {
 133         uint8_t         type;
 134         uint8_t         pad;
 135         uint16_t        numencs;
 136 };
 137
 138 struct rfb_updt_msg {
 139         uint8_t         type;
 140         uint8_t         incremental;
 141         uint16_t        x;
 142         uint16_t        y;
 143         uint16_t        width;
 144         uint16_t        height;
 145 };
 146
 147 struct rfb_key_msg {
 148         uint8_t         type;
 149         uint8_t         down;
 150         uint16_t        pad;
 151         uint32_t        code;
 152 };
 153
 154 struct rfb_ptr_msg {
 155         uint8_t         type;
 156         uint8_t         button;
 157         uint16_t        x;
 158         uint16_t        y;
 159 };
 160
 161 struct rfb_srvr_updt_msg {
 162         uint8_t         type;
 163         uint8_t         pad;
 164         uint16_t        numrects;
 165 };
 166
 167 struct rfb_srvr_rect_hdr {
 168         uint16_t        x;
 169         uint16_t        y;
 170         uint16_t        width;
 171         uint16_t        height;
 172         uint32_t        encoding;
 173 };
 174
 175 struct rfb_cuttext_msg {
 176         uint8_t         type;
 177         uint8_t         padding[3];
 178         uint32_t        length;
 179 };
 180
 181
 182 static void
 183 rfb_send_server_init_msg(int cfd)
 184 {
 185         struct bhyvegc_image *gc_image;
 186         struct rfb_srvr_info sinfo;
 187
 188         gc_image = console_get_image();
 189
 190         sinfo.width = htons(gc_image->width);
 191         sinfo.height = htons(gc_image->height);
 192         sinfo.pixfmt.bpp = 32;
 193         sinfo.pixfmt.depth = 32;
 194         sinfo.pixfmt.bigendian = 0;
 195         sinfo.pixfmt.truecolor = 1;
 196         sinfo.pixfmt.red_max = htons(255);
 197         sinfo.pixfmt.green_max = htons(255);
 198         sinfo.pixfmt.blue_max = htons(255);
 199         sinfo.pixfmt.red_shift = 16;
 200         sinfo.pixfmt.green_shift = 8;
 201         sinfo.pixfmt.blue_shift = 0;
 202         sinfo.namelen = htonl(strlen("bhyve"));
 203         (void)stream_write(cfd, &sinfo, sizeof(sinfo));
 204         (void)stream_write(cfd, "bhyve", strlen("bhyve"));
 205 }
 206
 207 static void
 208 rfb_send_resize_update_msg(struct rfb_softc *rc, int cfd)
 209 {
 210         struct rfb_srvr_updt_msg supdt_msg;
 211         struct rfb_srvr_rect_hdr srect_hdr;
 212
 213         /* Number of rectangles: 1 */
 214         supdt_msg.type = 0;
 215         supdt_msg.pad = 0;
 216         supdt_msg.numrects = htons(1);
 217         stream_write(cfd, &supdt_msg, sizeof(struct rfb_srvr_updt_msg));
 218
 219         /* Rectangle header */
 220         srect_hdr.x = htons(0);
 221         srect_hdr.y = htons(0);
 222         srect_hdr.width = htons(rc->width);
 223         srect_hdr.height = htons(rc->height);
 224         srect_hdr.encoding = htonl(RFB_ENCODING_RESIZE);
 225         stream_write(cfd, &srect_hdr, sizeof(struct rfb_srvr_rect_hdr));
 226 }
 227
 228 static void
 229 rfb_recv_set_pixfmt_msg(struct rfb_softc *rc, int cfd)
 230 {
 231         struct rfb_pixfmt_msg pixfmt_msg;
 232
 233         (void)stream_read(cfd, ((void *)&pixfmt_msg)+1, sizeof(pixfmt_msg)-1);
 234 }
 235
 236
 237 static void
 238 rfb_recv_set_encodings_msg(struct rfb_softc *rc, int cfd)
 239 {
 240         struct rfb_enc_msg enc_msg;
 241         int i;
 242         uint32_t encoding;
 243
 244         assert((sizeof(enc_msg) - 1) == 3);
 245         (void)stream_read(cfd, ((void *)&enc_msg)+1, sizeof(enc_msg)-1);
 246
 247         for (i = 0; i < htons(enc_msg.numencs); i++) {
 248                 (void)stream_read(cfd, &encoding, sizeof(encoding));
 249                 switch (htonl(encoding)) {
 250                 case RFB_ENCODING_RAW:
 251                         rc->enc_raw_ok = true;
 252                         break;
 253                 case RFB_ENCODING_ZLIB:
 254                         rc->enc_zlib_ok = true;
 255                         deflateInit(&rc->zstream, Z_BEST_SPEED);
 256                         break;
 257                 case RFB_ENCODING_RESIZE:
 258                         rc->enc_resize_ok = true;
 259                         break;
 260                 }
 261         }
 262 }
 263
 264 /*
 265  * Calculate CRC32 using SSE4.2; Intel or AMD Bulldozer+ CPUs only
 266  */
 267 static __inline uint32_t
 268 fast_crc32(void *buf, int len, uint32_t crcval)
 269 {
 270         uint32_t q = len / sizeof(uint32_t);
 271         uint32_t *p = (uint32_t *)buf;
 272
 273         while (q--) {
 274                 asm volatile (
 275                         ".byte 0xf2, 0xf, 0x38, 0xf1, 0xf1;"
 276                         :"=S" (crcval)
 277                         :"0" (crcval), "c" (*p)
 278                 );
 279                 p++;
 280         }
 281
 282         return (crcval);
 283 }
 284
 285
 286 static int
 287 rfb_send_rect(struct rfb_softc *rc, int cfd, struct bhyvegc_image *gc,
 288               int x, int y, int w, int h)
 289 {
 290         struct rfb_srvr_updt_msg supdt_msg;
 291         struct rfb_srvr_rect_hdr srect_hdr;
 292         unsigned long zlen;
 293         ssize_t nwrite, total;
 294         int err;
 295         uint32_t *p;
 296         uint8_t *zbufp;
 297
 298         /*
 299          * Send a single rectangle of the given x, y, w h dimensions.
 300          */
 301
 302         /* Number of rectangles: 1 */
 303         supdt_msg.type = 0;
 304         supdt_msg.pad = 0;
 305         supdt_msg.numrects = htons(1);
 306         nwrite = stream_write(cfd, &supdt_msg,
 307                               sizeof(struct rfb_srvr_updt_msg));
 308         if (nwrite <= 0)
 309                 return (nwrite);
 310
 311
 312         /* Rectangle header */
 313         srect_hdr.x = htons(x);
 314         srect_hdr.y = htons(y);
 315         srect_hdr.width = htons(w);
 316         srect_hdr.height = htons(h);
 317
 318         h = y + h;
 319         w *= sizeof(uint32_t);
 320         if (rc->enc_zlib_ok) {
 321                 zbufp = rc->zbuf;
 322                 rc->zstream.total_in = 0;
 323                 rc->zstream.total_out = 0;
 324                 for (p = &gc->data[y * gc->width + x]; y < h; y++) {
 325                         rc->zstream.next_in = (Bytef *)p;
 326                         rc->zstream.avail_in = w;
 327                         rc->zstream.next_out = (Bytef *)zbufp;
 328                         rc->zstream.avail_out = RFB_ZLIB_BUFSZ + 16 -
 329                                                 rc->zstream.total_out;
 330                         rc->zstream.data_type = Z_BINARY;
 331
 332                         /* Compress with zlib */
 333                         err = deflate(&rc->zstream, Z_SYNC_FLUSH);
 334                         if (err != Z_OK) {
 335                                 WPRINTF(("zlib[rect] deflate err: %d\n", err));
 336                                 rc->enc_zlib_ok = false;
 337                                 deflateEnd(&rc->zstream);
 338                                 goto doraw;
 339                         }
 340                         zbufp = rc->zbuf + rc->zstream.total_out;
 341                         p += gc->width;
 342                 }
 343                 srect_hdr.encoding = htonl(RFB_ENCODING_ZLIB);
 344                 nwrite = stream_write(cfd, &srect_hdr,
 345                                       sizeof(struct rfb_srvr_rect_hdr));
 346                 if (nwrite <= 0)
 347                         return (nwrite);
 348
 349                 zlen = htonl(rc->zstream.total_out);
 350                 nwrite = stream_write(cfd, &zlen, sizeof(uint32_t));
 351                 if (nwrite <= 0)
 352                         return (nwrite);
 353                 return (stream_write(cfd, rc->zbuf, rc->zstream.total_out));
 354         }
 355
 356 doraw:
 357
 358         total = 0;
 359         zbufp = rc->zbuf;
 360         for (p = &gc->data[y * gc->width + x]; y < h; y++) {
 361                 memcpy(zbufp, p, w);
 362                 zbufp += w;
 363                 total += w;
 364                 p += gc->width;
 365         }
 366
 367         srect_hdr.encoding = htonl(RFB_ENCODING_RAW);
 368         nwrite = stream_write(cfd, &srect_hdr,
 369                               sizeof(struct rfb_srvr_rect_hdr));
 370         if (nwrite <= 0)
 371                 return (nwrite);
 372
 373         total = stream_write(cfd, rc->zbuf, total);
 374
 375         return (total);
 376 }
 377
 378 static int
 379 rfb_send_all(struct rfb_softc *rc, int cfd, struct bhyvegc_image *gc)
 380 {
 381         struct rfb_srvr_updt_msg supdt_msg;
 382         struct rfb_srvr_rect_hdr srect_hdr;
 383         ssize_t nwrite;
 384         unsigned long zlen;
 385         int err;
 386
 387         /*
 388          * Send the whole thing
 389          */
 390
 391         /* Number of rectangles: 1 */
 392         supdt_msg.type = 0;
 393         supdt_msg.pad = 0;
 394         supdt_msg.numrects = htons(1);
 395         nwrite = stream_write(cfd, &supdt_msg,
 396                               sizeof(struct rfb_srvr_updt_msg));
 397         if (nwrite <= 0)
 398                 return (nwrite);
 399
 400         /* Rectangle header */
 401         srect_hdr.x = 0;
 402         srect_hdr.y = 0;
 403         srect_hdr.width = htons(gc->width);
 404         srect_hdr.height = htons(gc->height);
 405         if (rc->enc_zlib_ok) {
 406                 rc->zstream.next_in = (Bytef *)gc->data;
 407                 rc->zstream.avail_in = gc->width * gc->height *
 408                                    sizeof(uint32_t);
 409                 rc->zstream.next_out = (Bytef *)rc->zbuf;
 410                 rc->zstream.avail_out = RFB_ZLIB_BUFSZ + 16;
 411                 rc->zstream.data_type = Z_BINARY;
 412
 413                 rc->zstream.total_in = 0;
 414                 rc->zstream.total_out = 0;
 415
 416                 /* Compress with zlib */
 417                 err = deflate(&rc->zstream, Z_SYNC_FLUSH);
 418                 if (err != Z_OK) {
 419                         WPRINTF(("zlib deflate err: %d\n", err));
 420                         rc->enc_zlib_ok = false;
 421                         deflateEnd(&rc->zstream);
 422                         goto doraw;
 423                 }
 424
 425                 srect_hdr.encoding = htonl(RFB_ENCODING_ZLIB);
 426                 nwrite = stream_write(cfd, &srect_hdr,
 427                                       sizeof(struct rfb_srvr_rect_hdr));
 428                 if (nwrite <= 0)
 429                         return (nwrite);
 430
 431                 zlen = htonl(rc->zstream.total_out);
 432                 nwrite = stream_write(cfd, &zlen, sizeof(uint32_t));
 433                 if (nwrite <= 0)
 434                         return (nwrite);
 435                 return (stream_write(cfd, rc->zbuf, rc->zstream.total_out));
 436         }
 437
 438 doraw:
 439         srect_hdr.encoding = htonl(RFB_ENCODING_RAW);
 440         nwrite = stream_write(cfd, &srect_hdr,
 441                               sizeof(struct rfb_srvr_rect_hdr));
 442         if (nwrite <= 0)
 443                 return (nwrite);
 444
 445         nwrite = stream_write(cfd, gc->data,
 446                        gc->width * gc->height * sizeof(uint32_t));
 447
 448         return (nwrite);
 449 }
 450
 451 #define PIX_PER_CELL    32
 452 #define PIXCELL_SHIFT   5
 453 #define PIXCELL_MASK    0x1F
 454
 455 static int
 456 rfb_send_screen(struct rfb_softc *rc, int cfd, int all)
 457 {
 458         struct bhyvegc_image *gc_image;
 459         ssize_t nwrite;
 460         int x, y;
 461         int celly, cellwidth;
 462         int xcells, ycells;
 463         int w, h;
 464         uint32_t *p;
 465         int rem_x, rem_y;   /* remainder for resolutions not x32 pixels ratio */
 466         int retval;
 467         uint32_t *crc_p, *orig_crc;
 468         int changes;
 469
 470         console_refresh();
 471         gc_image = console_get_image();
 472
 473         pthread_mutex_lock(&rc->mtx);
 474         if (rc->sending) {
 475                 pthread_mutex_unlock(&rc->mtx);
 476                 return (1);
 477         }
 478         rc->sending = 1;
 479         pthread_mutex_unlock(&rc->mtx);
 480
 481         retval = 0;
 482
 483         if (all) {
 484                 retval = rfb_send_all(rc, cfd, gc_image);
 485                 goto done;
 486         }
 487
 488         /*
 489          * Calculate the checksum for each 32x32 cell. Send each that
 490          * has changed since the last scan.
 491          */
 492
 493         /* Resolution changed */
 494
 495         rc->crc_width = gc_image->width;
 496         rc->crc_height = gc_image->height;
 497
 498         w = rc->crc_width;
 499         h = rc->crc_height;
 500         xcells = howmany(rc->crc_width, PIX_PER_CELL);
 501         ycells = howmany(rc->crc_height, PIX_PER_CELL);
 502
 503         rem_x = w & PIXCELL_MASK;
 504
 505         rem_y = h & PIXCELL_MASK;
 506         if (!rem_y)
 507                 rem_y = PIX_PER_CELL;
 508
 509         p = gc_image->data;
 510
 511         /*
 512          * Go through all cells and calculate crc. If significant number
 513          * of changes, then send entire screen.
 514          * crc_tmp is dual purpose: to store the new crc and to flag as
 515          * a cell that has changed.
 516          */
 517         crc_p = rc->crc_tmp - xcells;
 518         orig_crc = rc->crc - xcells;
 519         changes = 0;
 520         memset(rc->crc_tmp, 0, sizeof(uint32_t) * xcells * ycells);
 521         for (y = 0; y < h; y++) {
 522                 if ((y & PIXCELL_MASK) == 0) {
 523                         crc_p += xcells;
 524                         orig_crc += xcells;
 525                 }
 526
 527                 for (x = 0; x < xcells; x++) {
 528                         if (rc->hw_crc)
 529                                 crc_p[x] = fast_crc32(p,
 530                                              PIX_PER_CELL * sizeof(uint32_t),
 531                                              crc_p[x]);
 532                         else
 533                                 crc_p[x] = (uint32_t)crc32(crc_p[x],
 534                                              (Bytef *)p,
 535                                              PIX_PER_CELL * sizeof(uint32_t));
 536
 537                         p += PIX_PER_CELL;
 538
 539                         /* check for crc delta if last row in cell */
 540                         if ((y & PIXCELL_MASK) == PIXCELL_MASK || y == (h-1)) {
 541                                 if (orig_crc[x] != crc_p[x]) {
 542                                         orig_crc[x] = crc_p[x];
 543                                         crc_p[x] = 1;
 544                                         changes++;
 545                                 } else {
 546                                         crc_p[x] = 0;
 547                                 }
 548                         }
 549                 }
 550
 551                 if (rem_x) {
 552                         if (rc->hw_crc)
 553                                 crc_p[x] = fast_crc32(p,
 554                                                     rem_x * sizeof(uint32_t),
 555                                                     crc_p[x]);
 556                         else
 557                                 crc_p[x] = (uint32_t)crc32(crc_p[x],
 558                                                     (Bytef *)p,
 559                                                     rem_x * sizeof(uint32_t));
 560                         p += rem_x;
 561
 562                         if ((y & PIXCELL_MASK) == PIXCELL_MASK || y == (h-1)) {
 563                                 if (orig_crc[x] != crc_p[x]) {
 564                                         orig_crc[x] = crc_p[x];
 565                                         crc_p[x] = 1;
 566                                         changes++;
 567                                 } else {
 568                                         crc_p[x] = 0;
 569                                 }
 570                         }
 571                 }
 572         }
 573
 574         /* If number of changes is > THRESH percent, send the whole screen */
 575         if (((changes * 100) / (xcells * ycells)) >= RFB_SEND_ALL_THRESH) {
 576                 retval = rfb_send_all(rc, cfd, gc_image);
 577                 goto done;
 578         }
 579
 580         /* Go through all cells, and send only changed ones */
 581         crc_p = rc->crc_tmp;
 582         for (y = 0; y < h; y += PIX_PER_CELL) {
 583                 /* previous cell's row */
 584                 celly = (y >> PIXCELL_SHIFT);
 585
 586                 /* Delta check crc to previous set */
 587                 for (x = 0; x < xcells; x++) {
 588                         if (*crc_p++ == 0)
 589                                 continue;
 590
 591                         if (x == (xcells - 1) && rem_x > 0)
 592                                 cellwidth = rem_x;
 593                         else
 594                                 cellwidth = PIX_PER_CELL;
 595                         nwrite = rfb_send_rect(rc, cfd,
 596                                 gc_image,
 597                                 x * PIX_PER_CELL,
 598                                 celly * PIX_PER_CELL,
 599                                 cellwidth,
 600                                 y + PIX_PER_CELL >= h ? rem_y : PIX_PER_CELL);
 601                         if (nwrite <= 0) {
 602                                 retval = nwrite;
 603                                 goto done;
 604                         }
 605                 }
 606         }
 607         retval = 1;
 608
 609 done:
 610         pthread_mutex_lock(&rc->mtx);
 611         rc->sending = 0;
 612         pthread_mutex_unlock(&rc->mtx);
 613
 614         return (retval);
 615 }
 616
 617
 618 static void
 619 rfb_recv_update_msg(struct rfb_softc *rc, int cfd, int discardonly)
 620 {
 621         struct rfb_updt_msg updt_msg;
 622         struct bhyvegc_image *gc_image;
 623
 624         (void)stream_read(cfd, ((void *)&updt_msg) + 1 , sizeof(updt_msg) - 1);
 625
 626         console_refresh();
 627         gc_image = console_get_image();
 628
 629         updt_msg.x = htons(updt_msg.x);
 630         updt_msg.y = htons(updt_msg.y);
 631         updt_msg.width = htons(updt_msg.width);
 632         updt_msg.height = htons(updt_msg.height);
 633
 634         if (updt_msg.width != gc_image->width ||
 635             updt_msg.height != gc_image->height) {
 636                 rc->width = gc_image->width;
 637                 rc->height = gc_image->height;
 638                 if (rc->enc_resize_ok)
 639                         rfb_send_resize_update_msg(rc, cfd);
 640         }
 641
 642         if (discardonly)
 643                 return;
 644
 645         rfb_send_screen(rc, cfd, 1);
 646 }
 647
 648 static void
 649 rfb_recv_key_msg(struct rfb_softc *rc, int cfd)
 650 {
 651         struct rfb_key_msg key_msg;
 652
 653         (void)stream_read(cfd, ((void *)&key_msg) + 1, sizeof(key_msg) - 1);
 654
 655         console_key_event(key_msg.down, htonl(key_msg.code));
 656 }
 657
 658 static void
 659 rfb_recv_ptr_msg(struct rfb_softc *rc, int cfd)
 660 {
 661         struct rfb_ptr_msg ptr_msg;
 662
 663         (void)stream_read(cfd, ((void *)&ptr_msg) + 1, sizeof(ptr_msg) - 1);
 664
 665         console_ptr_event(ptr_msg.button, htons(ptr_msg.x), htons(ptr_msg.y));
 666 }
 667
 668 static void
 669 rfb_recv_cuttext_msg(struct rfb_softc *rc, int cfd)
 670 {
 671         struct rfb_cuttext_msg ct_msg;
 672         unsigned char buf[32];
 673         int len;
 674
 675         len = stream_read(cfd, ((void *)&ct_msg) + 1, sizeof(ct_msg) - 1);
 676         ct_msg.length = htonl(ct_msg.length);
 677         while (ct_msg.length > 0) {
 678                 len = stream_read(cfd, buf, ct_msg.length > sizeof(buf) ?
 679                         sizeof(buf) : ct_msg.length);
 680                 ct_msg.length -= len;
 681         }
 682 }
 683
 684 static int64_t
 685 timeval_delta(struct timeval *prev, struct timeval *now)
 686 {
 687         int64_t n1, n2;
 688         n1 = now->tv_sec * 1000000 + now->tv_usec;
 689         n2 = prev->tv_sec * 1000000 + prev->tv_usec;
 690         return (n1 - n2);
 691 }
 692
 693 static void *
 694 rfb_wr_thr(void *arg)
 695 {
 696         struct rfb_softc *rc;
 697         fd_set rfds;
 698         struct timeval tv;
 699         struct timeval prev_tv;
 700         int64_t tdiff;
 701         int cfd;
 702         int err;
 703
 704         rc = arg;
 705         cfd = rc->cfd;
 706
 707         prev_tv.tv_sec = 0;
 708         prev_tv.tv_usec = 0;
 709         while (rc->cfd >= 0) {
 710                 FD_ZERO(&rfds);
 711                 FD_SET(cfd, &rfds);
 712                 tv.tv_sec = 0;
 713                 tv.tv_usec = 10000;
 714
 715                 err = select(cfd+1, &rfds, NULL, NULL, &tv);
 716                 if (err < 0)
 717                         return (NULL);
 718
 719                 /* Determine if its time to push screen; ~24hz */
 720                 gettimeofday(&tv, NULL);
 721                 tdiff = timeval_delta(&prev_tv, &tv);
 722                 if (tdiff > 40000) {
 723                         prev_tv.tv_sec = tv.tv_sec;
 724                         prev_tv.tv_usec = tv.tv_usec;
 725                         if (rfb_send_screen(rc, cfd, 0) <= 0) {
 726                                 return (NULL);
 727                         }
 728                 } else {
 729                         /* sleep */
 730                         usleep(40000 - tdiff);
 731                 }
 732         }
 733
 734         return (NULL);
 735 }
 736
 737 void
 738 rfb_handle(struct rfb_softc *rc, int cfd)
 739 {
 740         const char *vbuf = "RFB 003.008\n";
 741         unsigned char buf[80];
 742         pthread_t tid;
 743         uint32_t sres;
 744         int len;
 745
 746         rc->cfd = cfd;
 747
 748         /* 1a. Send server version */
 749         stream_write(cfd, vbuf, strlen(vbuf));
 750
 751         /* 1b. Read client version */
 752         len = read(cfd, buf, sizeof(buf));
 753
 754         /* 2a. Send security type 'none' */
 755         buf[0] = 1;
 756         buf[1] = 1; /* none */
 757         stream_write(cfd, buf, 2);
 758
 759
 760         /* 2b. Read agreed security type */
 761         len = stream_read(cfd, buf, 1);
 762
 763         /* 2c. Write back a status of 0 */
 764         sres = 0;
 765         stream_write(cfd, &sres, 4);
 766
 767         /* 3a. Read client shared-flag byte */
 768         len = stream_read(cfd, buf, 1);
 769
 770         /* 4a. Write server-init info */
 771         rfb_send_server_init_msg(cfd);
 772
 773         if (!rc->zbuf) {
 774                 rc->zbuf = malloc(RFB_ZLIB_BUFSZ + 16);
 775                 assert(rc->zbuf != NULL);
 776         }
 777
 778         rfb_send_screen(rc, cfd, 1);
 779
 780         pthread_create(&tid, NULL, rfb_wr_thr, rc);
 781         pthread_set_name_np(tid, "rfbout");
 782
 783         /* Now read in client requests. 1st byte identifies type */
 784         for (;;) {
 785                 len = read(cfd, buf, 1);
 786                 if (len <= 0) {
 787                         DPRINTF(("rfb client exiting\r\n"));
 788                         break;
 789                 }
 790
 791                 switch (buf[0]) {
 792                 case 0:
 793                         rfb_recv_set_pixfmt_msg(rc, cfd);
 794                         break;
 795                 case 2:
 796                         rfb_recv_set_encodings_msg(rc, cfd);
 797                         break;
 798                 case 3:
 799                         rfb_recv_update_msg(rc, cfd, 1);
 800                         break;
 801                 case 4:
 802                         rfb_recv_key_msg(rc, cfd);
 803                         break;
 804                 case 5:
 805                         rfb_recv_ptr_msg(rc, cfd);
 806                         break;
 807                 case 6:
 808                         rfb_recv_cuttext_msg(rc, cfd);
 809                         break;
 810                 default:
 811                         WPRINTF(("rfb unknown cli-code %d!\n", buf[0] & 0xff));
 812                         goto done;
 813                 }
 814         }
 815 done:
 816         rc->cfd = -1;
 817         pthread_join(tid, NULL);
 818         if (rc->enc_zlib_ok)
 819                 deflateEnd(&rc->zstream);
 820 }
 821
 822 static void *
 823 rfb_thr(void *arg)
 824 {
 825         struct rfb_softc *rc;
 826         sigset_t set;
 827
 828         int cfd;
 829
 830         rc = arg;
 831
 832         sigemptyset(&set);
 833         sigaddset(&set, SIGPIPE);
 834         if (pthread_sigmask(SIG_BLOCK, &set, NULL) != 0) {
 835                 perror("pthread_sigmask");
 836                 return (NULL);
 837         }
 838
 839         for (;;) {
 840                 rc->enc_raw_ok = false;
 841                 rc->enc_zlib_ok = false;
 842                 rc->enc_resize_ok = false;
 843
 844                 cfd = accept(rc->sfd, NULL, NULL);
 845                 if (rc->conn_wait) {
 846                         pthread_mutex_lock(&rc->mtx);
 847                         pthread_cond_signal(&rc->cond);
 848                         pthread_mutex_unlock(&rc->mtx);
 849                         rc->conn_wait = 0;
 850                 }
 851                 rfb_handle(rc, cfd);
 852                 close(cfd);
 853         }
 854
 855         /* NOTREACHED */
 856         return (NULL);
 857 }
 858
 859 static int
 860 sse42_supported(void)
 861 {
 862         u_int cpu_registers[4], ecx;
 863
 864         do_cpuid(1, cpu_registers);
 865
 866         ecx = cpu_registers[2];
 867
 868         return ((ecx & CPUID2_SSE42) != 0);
 869 }
 870
 871 int
 872 rfb_init(char *hostname, int port, int wait)
 873 {
 874         struct rfb_softc *rc;
 875         struct sockaddr_in sin;
 876         int on = 1;
 877 #ifndef WITHOUT_CAPSICUM
 878         cap_rights_t rights;
 879 #endif
 880
 881         rc = calloc(1, sizeof(struct rfb_softc));
 882
 883         rc->crc = calloc(howmany(RFB_MAX_WIDTH * RFB_MAX_HEIGHT, 32),
 884                          sizeof(uint32_t));
 885         rc->crc_tmp = calloc(howmany(RFB_MAX_WIDTH * RFB_MAX_HEIGHT, 32),
 886                              sizeof(uint32_t));
 887         rc->crc_width = RFB_MAX_WIDTH;
 888         rc->crc_height = RFB_MAX_HEIGHT;
 889
 890         rc->sfd = socket(AF_INET, SOCK_STREAM, 0);
 891         if (rc->sfd < 0) {
 892                 perror("socket");
 893                 return (-1);
 894         }
 895
 896         setsockopt(rc->sfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
 897
 898         sin.sin_len = sizeof(sin);
 899         sin.sin_family = AF_INET;
 900         sin.sin_port = port ? htons(port) : htons(5900);
 901         if (hostname && strlen(hostname) > 0)
 902                 inet_pton(AF_INET, hostname, &(sin.sin_addr));
 903         else
 904                 sin.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
 905
 906         if (bind(rc->sfd, (struct sockaddr *)&sin, sizeof(sin)) < 0) {
 907                 perror("bind");
 908                 return (-1);
 909         }
 910
 911         if (listen(rc->sfd, 1) < 0) {
 912                 perror("listen");
 913                 return (-1);
 914         }
 915
 916 #ifndef WITHOUT_CAPSICUM
 917         cap_rights_init(&rights, CAP_ACCEPT, CAP_EVENT, CAP_READ, CAP_WRITE);
 918         if (cap_rights_limit(rc->sfd, &rights) == -1 && errno != ENOSYS)
 919                 errx(EX_OSERR, "Unable to apply rights for sandbox");
 920 #endif
 921
 922         rc->hw_crc = sse42_supported();
 923
 924         rc->conn_wait = wait;
 925         if (wait) {
 926                 pthread_mutex_init(&rc->mtx, NULL);
 927                 pthread_cond_init(&rc->cond, NULL);
 928         }
 929
 930         pthread_create(&rc->tid, NULL, rfb_thr, rc);
 931         pthread_set_name_np(rc->tid, "rfb");
 932
 933         if (wait) {
 934                 DPRINTF(("Waiting for rfb client...\n"));
 935                 pthread_mutex_lock(&rc->mtx);
 936                 pthread_cond_wait(&rc->cond, &rc->mtx);
 937                 pthread_mutex_unlock(&rc->mtx);
 938         }
 939
 940         return (0);
 941 }