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