net/rfb.c

   1 #include <stdint.h>
   2 #include <minilib.h>
   3 #include <output.h>
   4 #include <fb.h>
   5 #include <keyboard.h>
   6
   7 #include "lwip/tcp.h"
   8 #include "lwip/stats.h"
   9
  10 #include "rfb.h"
  11
  12 #define SET_PIXEL_FORMAT        0
  13 #define SET_ENCODINGS           2
  14 #define FB_UPDATE_REQUEST       3
  15 #define KEY_EVENT               4
  16 #define POINTER_EVENT           5
  17 #define CLIENT_CUT_TEXT         6
  18
  19 #define RFB_BUF_SIZE    1536
  20
  21 #define SCREEN_CHUNKS_X 8
  22 #define SCREEN_CHUNKS_Y 8
  23
  24 struct pixel_format {
  25         uint8_t bpp;
  26         uint8_t depth;
  27         uint8_t big_endian;
  28         uint8_t true_color;
  29         uint16_t red_max;
  30         uint16_t green_max;
  31         uint16_t blue_max;
  32         uint8_t red_shift;
  33         uint8_t green_shift;
  34         uint8_t blue_shift;
  35         uint8_t padding[3];
  36 };
  37
  38 struct server_init_message {
  39         uint16_t fb_width;
  40         uint16_t fb_height;
  41         struct pixel_format fmt;
  42         uint32_t name_length;
  43         char name_string[8];
  44 };
  45
  46 struct fb_update_req {
  47         uint8_t msgtype;
  48         uint8_t incremental;
  49         uint16_t xpos;
  50         uint16_t ypos;
  51         uint16_t width;
  52         uint16_t height;
  53 };
  54
  55 struct set_encs_req {
  56         uint8_t msgtype;
  57         uint8_t padding;
  58         uint16_t num;
  59         int32_t encodings[];
  60 };
  61
  62 struct key_event_pkt {
  63         uint8_t msgtype;
  64         uint8_t downflag;
  65         uint8_t pad[2];
  66         uint32_t keysym;
  67 };
  68
  69 struct pointer_event_pkt {
  70         uint8_t msgtype;
  71         uint8_t button_mask;
  72         uint16_t x;
  73         uint16_t y;
  74 };
  75
  76 struct text_event_pkt {
  77         uint8_t msgtype;
  78         uint8_t padding[3];
  79         uint32_t length;
  80         char text[];
  81 };
  82
  83 struct update_header {
  84         uint8_t msgtype;
  85         uint8_t padding;
  86         uint16_t nrects;
  87         uint16_t xpos;
  88         uint16_t ypos;
  89         uint16_t width;
  90         uint16_t height;
  91         int32_t enctype;
  92 };
  93
  94 struct rfb_state {
  95         enum {
  96                 ST_BEGIN = 0,
  97                 ST_CLIENTINIT,
  98                 ST_MAIN
  99         } state;
 100         int version;
 101         int encs_remaining;
 102
 103         char data[RFB_BUF_SIZE];
 104         int readpos;
 105         int writepos;
 106
 107         char next_update_incremental;
 108         char update_requested;
 109
 110         struct fb_update_req client_interest_area;
 111
 112         enum {
 113                 SST_IDLE = 0,
 114                 SST_HEADER,
 115                 SST_DATA
 116         } send_state;
 117
 118         uint32_t checksums[SCREEN_CHUNKS_X][SCREEN_CHUNKS_Y];
 119
 120         uint32_t chunk_xnum;
 121         uint32_t chunk_ynum;
 122         uint32_t chunk_xpos;
 123         uint32_t chunk_ypos;
 124         uint32_t chunk_width;
 125         uint32_t chunk_height;
 126
 127         uint32_t chunk_bytes_sent;
 128
 129         uint32_t chunk_checksum;
 130
 131         int chunk_actually_sent;
 132         int try_in_a_bit;
 133
 134         char * blockbuf;
 135 };
 136
 137 static struct server_init_message server_info;
 138
 139 static void init_server_info() {
 140         server_info.name_length = htonl(8);
 141         memcpy(server_info.name_string, "NetWatch", 8);
 142 }
 143
 144 static void update_server_info() {
 145         if (fb != NULL) {
 146                 outputf("RFB: setting fmt %d", fb->curmode.format);
 147                 server_info.fb_width = htons(fb->curmode.xres);
 148                 server_info.fb_height = htons(fb->curmode.yres);
 149                 switch (fb->curmode.format) {
 150                 case FB_RGB888:
 151                         server_info.fmt.bpp = 32;
 152                         server_info.fmt.depth = 24;
 153                         server_info.fmt.big_endian = 0;
 154                         server_info.fmt.true_color = 1;
 155                         server_info.fmt.red_max = htons(255);
 156                         server_info.fmt.green_max = htons(255);
 157                         server_info.fmt.blue_max = htons(255);
 158                         server_info.fmt.red_shift = 0;
 159                         server_info.fmt.green_shift = 8;
 160                         server_info.fmt.blue_shift = 16;
 161                         break;
 162                 default:
 163                         outputf("RFB: unknown fb fmt %d", fb->curmode.format);
 164                         break;
 165                 }
 166         } else {
 167                 outputf("RFB: fb null");
 168         }
 169 }
 170
 171 static int advance_chunk(struct rfb_state *state) {
 172
 173         state->chunk_xnum += 1;
 174
 175         if (state->chunk_xnum == SCREEN_CHUNKS_X) {
 176                 state->chunk_ynum += 1;
 177                 state->chunk_xnum = 0;
 178         }
 179
 180         if (state->chunk_ynum == SCREEN_CHUNKS_Y) {
 181                 state->chunk_ynum = 0;
 182                 state->send_state = SST_IDLE;
 183                 if (!(state->chunk_actually_sent))
 184                         state->try_in_a_bit = 2;
 185                         return 1;
 186         }
 187
 188         return 0;
 189 }
 190
 191 static int ceildiv(int a, int b) {
 192         int res = a / b;
 193         if (a % b != 0) {
 194                 res++;
 195         }
 196         return res;
 197 }
 198
 199 static void send_fsm(struct tcp_pcb *pcb, struct rfb_state *state) {
 200         struct update_header hdr;
 201         int bytes_left;
 202         int totaldim;
 203         err_t err;
 204
 205         while(1) {
 206
 207                 switch (state->send_state) {
 208
 209                 case SST_IDLE:
 210                         /* Nothing to do */
 211
 212                         if (state->update_requested) {
 213                                 outputf("RFB send: update requested");
 214                                 state->update_requested = 0;
 215                                 state->chunk_actually_sent = 0;
 216                                 state->send_state = SST_HEADER;
 217                         } else {
 218                                 return;
 219                         }
 220
 221                         /* FALL THROUGH to SST_HEADER */
 222
 223                 case SST_HEADER:
 224
 225                         /* Calculate the width and height for this chunk, remembering
 226                          * that if SCREEN_CHUNKS_[XY] do not evenly divide the width and
 227                          * height, we may need to have shorter chunks at the edge of
 228                          * the screen. */
 229
 230                         state->chunk_width = ceildiv(fb->curmode.xres, SCREEN_CHUNKS_X);
 231                         state->chunk_xpos = state->chunk_width * state->chunk_xnum;
 232                         totaldim = state->chunk_width * (state->chunk_xnum + 1);
 233                         if (totaldim > fb->curmode.xres) {
 234                                 state->chunk_width -= (totaldim - fb->curmode.xres);
 235                         }
 236
 237                         state->chunk_height = ceildiv(fb->curmode.yres, SCREEN_CHUNKS_Y);
 238                         state->chunk_ypos = state->chunk_height
 239                                                  * state->chunk_ynum;
 240                         totaldim = state->chunk_height * (state->chunk_ynum + 1);
 241                         if (totaldim > fb->curmode.yres) {
 242                                 state->chunk_height -= (totaldim - fb->curmode.yres);
 243                         }
 244
 245                         /* Do we _actually_ need to send this chunk? */
 246                         if (fb->checksum_rect) {
 247                                 state->chunk_checksum = fb->checksum_rect(state->chunk_xpos, state->chunk_ypos,
 248                                                                 state->chunk_width, state->chunk_height);
 249
 250                                 if (state->chunk_checksum == state->checksums[state->chunk_xnum][state->chunk_ynum]) {
 251                                         if (advance_chunk(state))
 252                                                 return;
 253                                         continue;
 254                                 }
 255                                 /* Checksum gets set in data block, AFTER the data has been sent. */
 256                         }
 257
 258                         state->chunk_actually_sent = 1;
 259
 260                         /* Send a header */
 261                         hdr.msgtype = 0;
 262                         state->chunk_bytes_sent = 0;
 263                         hdr.nrects = htons(1);
 264                         hdr.xpos = htons(state->chunk_xpos);
 265                         hdr.ypos = htons(state->chunk_ypos);
 266                         hdr.width = htons(state->chunk_width);
 267                         hdr.height= htons(state->chunk_height);
 268                         hdr.enctype = htonl(0);
 269
 270                         err = tcp_write(pcb, &hdr, sizeof(hdr), TCP_WRITE_FLAG_COPY);
 271
 272                         if (err != ERR_OK) {
 273                                 if (err != ERR_MEM)
 274                                         outputf("RFB: header send error %d", err);
 275
 276                                 /* Try again later. */
 277                                 return;
 278                         }
 279
 280                         state->send_state = SST_DATA;
 281
 282                         /* Snag the data. */
 283                         fb->copy_pixels(state->blockbuf,
 284                                 state->chunk_xpos, state->chunk_ypos,
 285                                 state->chunk_width, state->chunk_height);
 286
 287                         /* FALL THROUGH to SST_DATA */
 288
 289                 case SST_DATA:
 290
 291                         bytes_left = 4 * state->chunk_width * state->chunk_height - state->chunk_bytes_sent;
 292
 293                         if (bytes_left == 0) {
 294                                 state->send_state = SST_HEADER;
 295                                 state->checksums[state->chunk_xnum][state->chunk_ynum] = state->chunk_checksum;
 296                                 if (advance_chunk(state))
 297                                         return;
 298                                 break;
 299                         }
 300
 301                         /* That's enough. */
 302                         if (bytes_left > 1400) {
 303                                 bytes_left = 1400;
 304                         }
 305
 306                         err = tcp_write(pcb, state->blockbuf + state->chunk_bytes_sent,
 307                                 bytes_left, TCP_WRITE_FLAG_COPY);
 308
 309                         if (err == ERR_OK) {
 310                                 state->chunk_bytes_sent += bytes_left;
 311                         } else {
 312                                 if (err != ERR_MEM)
 313                                         outputf("RFB: send error %d", err);
 314
 315                                 return;
 316                         }
 317
 318                         if (tcp_sndbuf(pcb) == 0) {
 319                                 return;
 320                         }
 321                 }
 322         }
 323
 324         if (tcp_output(pcb) != ERR_OK)
 325                 outputf("RFB: tcp_output bailed in send_fsm?");
 326 }
 327
 328 static err_t rfb_sent(void *arg, struct tcp_pcb *pcb, uint16_t len) {
 329         struct rfb_state *state = arg;
 330         send_fsm(pcb, state);
 331         return ERR_OK;
 332 }
 333
 334 static err_t rfb_poll(void *arg, struct tcp_pcb *pcb) {
 335         struct rfb_state *state = arg;
 336         send_fsm(pcb, state);
 337         if (state->try_in_a_bit) {
 338                 state->try_in_a_bit--;
 339                 if (!(state->try_in_a_bit)) {
 340                         state->update_requested = 1;
 341                 }
 342         }
 343 /*
 344         stats_display();
 345 */
 346         return ERR_OK;
 347 }
 348
 349 static void close_conn(struct tcp_pcb *pcb, struct rfb_state *state) {
 350         outputf("close_conn: bailing");
 351         tcp_arg(pcb, NULL);
 352         tcp_sent(pcb, NULL);
 353         tcp_recv(pcb, NULL);
 354         mem_free(state);
 355         mem_free(state->blockbuf);
 356         tcp_close(pcb);
 357         outputf("close_conn: done");
 358 }
 359
 360 enum fsm_result {
 361         NEEDMORE,
 362         OK,
 363         FAIL
 364 };
 365
 366 static enum fsm_result recv_fsm(struct tcp_pcb *pcb, struct rfb_state *state) {
 367         int i;
 368         int pktsize;
 369
 370         outputf("RFB FSM: st %d rp %d wp %d", state->state, state->readpos,
 371                 state->writepos);
 372
 373         switch(state->state) {
 374         case ST_BEGIN:
 375                 if (state->writepos < 12) return NEEDMORE;
 376
 377                 if (!strncmp(state->data, "RFB 003.003\n", 12)) {
 378                         state->version = 3;
 379                 } else if (!strncmp(state->data, "RFB 003.005\n", 12)) {
 380                         /* Spec states that "RFB 003.005", an incorrect value,
 381                          * should be treated by the server as 3.3. */
 382                         state->version = 3;
 383                 } else if (!strncmp(state->data, "RFB 003.007\n", 12)) {
 384                         state->version = 7;
 385                 } else if (!strncmp(state->data, "RFB 003.008\n", 12)) {
 386                         state->version = 8;
 387                 } else {
 388                         outputf("RFB: Negotiation fail");
 389                         return FAIL;
 390                 }
 391
 392                 outputf("RFB: Negotiated v3.%d", state->version);
 393
 394                 state->readpos += 12;
 395                 state->state = ST_CLIENTINIT;
 396
 397                 /* We support one security type, currently "none".
 398                  * Send that and SecurityResult. */
 399                 if (state->version >= 7) {
 400                         tcp_write(pcb, "\x01\x01\x00\x00\x00\x00", 6, 0);
 401                 } else {
 402                         tcp_write(pcb, "\x01\x00\x00\x00\x00", 5, 0);
 403                 }
 404
 405                 tcp_output(pcb);
 406
 407                 return OK;
 408
 409         case ST_CLIENTINIT:
 410                 if (state->version >= 7) {
 411                         /* Ignore the security type and ClientInit */
 412                         if (state->writepos < 2) return NEEDMORE;
 413                         state->readpos += 2;
 414                 } else {
 415                         /* Just ClientInit */
 416                         if (state->writepos < 1) return NEEDMORE;
 417                         state->readpos += 1;
 418                 }
 419
 420                 state->state = ST_MAIN;
 421
 422                 outputf("RFB: Sending server info", state->version);
 423                 tcp_write(pcb, &server_info, sizeof(server_info), TCP_WRITE_FLAG_COPY);
 424                 tcp_output(pcb);
 425
 426                 return OK;
 427
 428         case ST_MAIN:
 429                 if (state->writepos < 1) return NEEDMORE;
 430
 431                 outputf("RFB: cmd %d", state->data[0]);
 432                 switch (state->data[0]) {
 433
 434                 case SET_PIXEL_FORMAT:
 435                         /* SetPixelFormat */
 436                         if (state->writepos < (sizeof(struct pixel_format) + 4))
 437                                 return NEEDMORE;
 438                         outputf("RFB: SetPixelFormat");
 439 /*
 440                         struct pixel_format * new_fmt =
 441                                 (struct pixel_format *)(&state->data[4]);
 442 */
 443                         /* XXX ... */
 444
 445                         state->readpos += sizeof(struct pixel_format) + 4;
 446                         return OK;
 447
 448                 case SET_ENCODINGS:
 449                         if (state->writepos < 4) return NEEDMORE;
 450
 451                         struct set_encs_req * req = (struct set_encs_req *)state->data;
 452
 453                         pktsize = sizeof(struct set_encs_req) + (4 * ntohs(req->num));
 454
 455                         outputf("RFB: SetEncodings [%d]", ntohs(req->num));
 456                         if (state->writepos < pktsize) return NEEDMORE;
 457
 458                         for (i = 0; i < ntohs(req->num); i++) {
 459                                 outputf("RFB: Encoding: %d", ntohl(req->encodings[i]));
 460                                 /* XXX ... */
 461                         }
 462
 463                         state->readpos += pktsize;
 464                         return OK;
 465
 466                 case FB_UPDATE_REQUEST:
 467                         if (state->writepos < sizeof(struct fb_update_req))
 468                                 return NEEDMORE;
 469                         outputf("RFB: UpdateRequest");
 470
 471                         state->update_requested = 1;
 472                         memcpy(&state->client_interest_area, state->data,
 473                                sizeof(struct fb_update_req));
 474
 475                         state->readpos += sizeof(struct fb_update_req);
 476                         return OK;
 477
 478                 case KEY_EVENT:
 479                         if (state->writepos < sizeof(struct key_event_pkt))
 480                                 return NEEDMORE;
 481
 482                         struct key_event_pkt * p = (struct key_event_pkt *)state->data;
 483
 484                         outputf("RFB: Key: %d (%c)", htonl(p->keysym), (htonl(p->keysym) & 0xFF));
 485                         kbd_inject_keysym(htonl(p->keysym), p->downflag);
 486
 487                         state->readpos += sizeof(struct key_event_pkt);
 488                         return OK;
 489
 490                 case POINTER_EVENT:
 491                         if (state->writepos < sizeof(struct pointer_event_pkt))
 492                                 return NEEDMORE;
 493                         outputf("RFB: Pointer");
 494
 495                         /* XXX stub */
 496
 497                         state->readpos += sizeof(struct pointer_event_pkt);
 498                         return OK;
 499
 500                 case CLIENT_CUT_TEXT:
 501                         if (state->writepos < sizeof(struct text_event_pkt))
 502                                 return NEEDMORE;
 503                         outputf("RFB: Cut Text");
 504
 505                         struct text_event_pkt * pkt =
 506                                 (struct text_event_pkt *)state->data;
 507
 508                         if (state->writepos < sizeof(struct text_event_pkt)
 509                                               + pkt->length)
 510                                 return NEEDMORE;
 511
 512                         /* XXX stub */
 513
 514                         state->readpos += sizeof(struct text_event_pkt)
 515                                           + pkt->length;
 516                         return OK;
 517
 518                 default:
 519                         outputf("RFB: Bad command: %d", state->data[0]);
 520                         return FAIL;
 521                 }
 522         default:
 523                 outputf("RFB: Bad state");
 524                 return FAIL;
 525         }
 526 }
 527
 528 static err_t rfb_recv(void *arg, struct tcp_pcb *pcb,
 529                       struct pbuf *p, err_t err) {
 530         struct rfb_state *state = arg;
 531         uint16_t copylen;
 532
 533         if (state == NULL)
 534
 535         if (err != ERR_OK) {
 536                 outputf("RFB: recv err %d", err);
 537                 /* FIXME do something better here? */
 538                 return ERR_OK;
 539         }
 540
 541         if (p == NULL) {
 542                 outputf("RFB: Connection closed");
 543                 close_conn(pcb, state);
 544                 return ERR_OK;
 545         }
 546
 547         if (p->tot_len > (RFB_BUF_SIZE - state->writepos)) {
 548                 /* Overflow! */
 549                 outputf("RFB: Overflow!");
 550                 close_conn(pcb, state);
 551                 return ERR_OK;
 552         }
 553
 554         copylen = pbuf_copy_partial(p, state->data + state->writepos, p->tot_len, 0);
 555         outputf("RFB: Processing %d, wp %d, cp %d", p->tot_len, state->writepos, copylen);
 556         state->writepos += p->tot_len;
 557
 558         tcp_recved(pcb, p->tot_len);
 559         pbuf_free(p);
 560
 561         while (1) {
 562                 switch (recv_fsm(pcb, state)) {
 563                 case NEEDMORE:
 564                         outputf("RFB FSM: blocking");
 565                         goto doneprocessing;
 566
 567                 case OK:
 568                         outputf("RFB FSM: ok");
 569
 570                         if (state->readpos == state->writepos) {
 571                                 state->readpos = 0;
 572                                 state->writepos = 0;
 573                                 goto doneprocessing;
 574                         } else {
 575                                 memmove(state->data,
 576                                         state->data + state->readpos,
 577                                         state->writepos - state->readpos);
 578                                 state->writepos -= state->readpos;
 579                                 state->readpos = 0;
 580                         }
 581                         break;
 582                 case FAIL:
 583                         /* Shit */
 584                         outputf("RFB: Protocol error");
 585                         close_conn(pcb, state);
 586                         return ERR_OK;
 587                 }
 588         }
 589
 590 doneprocessing:
 591
 592         /* Kick off a send. */
 593         if (state->send_state == SST_IDLE && state->update_requested) {
 594                 send_fsm(pcb, state);
 595         }
 596
 597         return ERR_OK;
 598 }
 599
 600 static err_t rfb_accept(void *arg, struct tcp_pcb *pcb, err_t err) {
 601         struct rfb_state *state;
 602         char * blockbuf;
 603
 604         LWIP_UNUSED_ARG(arg);
 605         LWIP_UNUSED_ARG(err);
 606
 607         state = (struct rfb_state *)mem_malloc(sizeof(struct rfb_state));
 608
 609         if (!state)
 610         {
 611                 outputf("rfb_accept: out of memory\n");
 612                 return ERR_MEM;
 613         }
 614
 615         memset(state, 0, sizeof(struct rfb_state));
 616
 617         blockbuf = mem_malloc(ceildiv(fb->curmode.xres, SCREEN_CHUNKS_X)
 618                             * ceildiv(fb->curmode.yres, SCREEN_CHUNKS_Y) * 4);
 619
 620         if (!blockbuf)
 621         {
 622                 outputf("rfb_accept: out of memory allocating blockbuf\n");
 623                 mem_free(state);
 624                 return ERR_MEM;
 625         }
 626
 627         state->blockbuf = blockbuf;
 628         state->state = ST_BEGIN;
 629         state->send_state = SST_IDLE;
 630
 631         /* XXX: update_server_info() should be called from the 64ms timer, and deal
 632          * with screen resizes appropriately. */
 633         update_server_info();
 634
 635         tcp_arg(pcb, state);
 636         tcp_recv(pcb, rfb_recv);
 637         tcp_sent(pcb, rfb_sent);
 638         tcp_poll(pcb, rfb_poll, 1);
 639 /*
 640         tcp_err(pcb, rfb_err);
 641 */
 642         tcp_write(pcb, "RFB 003.008\n", 12, 0);
 643         tcp_output(pcb);
 644
 645         return ERR_OK;
 646 }
 647
 648 void rfb_init() {
 649         struct tcp_pcb *pcb;
 650
 651         init_server_info();
 652
 653         pcb = tcp_new();
 654         tcp_bind(pcb, IP_ADDR_ANY, RFB_PORT);
 655         pcb = tcp_listen(pcb);
 656         tcp_accept(pcb, rfb_accept);
 657 }