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                         outputf("actually sent");
 259                         state->chunk_actually_sent = 1;
 260
 261                         /* Send a header */
 262                         hdr.msgtype = 0;
 263                         state->chunk_bytes_sent = 0;
 264                         hdr.nrects = htons(1);
 265                         hdr.xpos = htons(state->chunk_xpos);
 266                         hdr.ypos = htons(state->chunk_ypos);
 267                         hdr.width = htons(state->chunk_width);
 268                         hdr.height= htons(state->chunk_height);
 269                         hdr.enctype = htonl(0);
 270
 271                         err = tcp_write(pcb, &hdr, sizeof(hdr), TCP_WRITE_FLAG_COPY);
 272
 273                         if (err != ERR_OK) {
 274                                 if (err != ERR_MEM)
 275                                         outputf("RFB: header send error %d", err);
 276
 277                                 /* Try again later. */
 278                                 return;
 279                         }
 280
 281                         state->send_state = SST_DATA;
 282
 283                         /* Snag the data. */
 284                         fb->copy_pixels(state->blockbuf,
 285                                 state->chunk_xpos, state->chunk_ypos,
 286                                 state->chunk_width, state->chunk_height);
 287
 288                         /* FALL THROUGH to SST_DATA */
 289
 290                 case SST_DATA:
 291
 292                         bytes_left = 4 * state->chunk_width * state->chunk_height - state->chunk_bytes_sent;
 293
 294                         if (bytes_left == 0) {
 295                                 state->send_state = SST_HEADER;
 296                                 state->checksums[state->chunk_xnum][state->chunk_ynum] = state->chunk_checksum;
 297                                 if (advance_chunk(state))
 298                                         return;
 299                                 break;
 300                         }
 301
 302                         /* That's enough. */
 303                         if (bytes_left > 1400) {
 304                                 bytes_left = 1400;
 305                         }
 306
 307                         err = tcp_write(pcb, state->blockbuf + state->chunk_bytes_sent,
 308                                 bytes_left, TCP_WRITE_FLAG_COPY);
 309
 310                         if (err == ERR_OK) {
 311                                 state->chunk_bytes_sent += bytes_left;
 312                         } else {
 313                                 if (err != ERR_MEM)
 314                                         outputf("RFB: send error %d", err);
 315
 316                                 return;
 317                         }
 318
 319                         if (tcp_sndbuf(pcb) == 0) {
 320                                 return;
 321                         }
 322                 }
 323         }
 324
 325         if (tcp_output(pcb) != ERR_OK)
 326                 outputf("RFB: tcp_output bailed in send_fsm?");
 327 }
 328
 329 static err_t rfb_sent(void *arg, struct tcp_pcb *pcb, uint16_t len) {
 330         struct rfb_state *state = arg;
 331         send_fsm(pcb, state);
 332         return ERR_OK;
 333 }
 334
 335 static err_t rfb_poll(void *arg, struct tcp_pcb *pcb) {
 336         struct rfb_state *state = arg;
 337         send_fsm(pcb, state);
 338         if (state->try_in_a_bit) {
 339                 state->try_in_a_bit--;
 340                 if (!(state->try_in_a_bit)) {
 341                         state->update_requested = 1;
 342                 }
 343         }
 344 /*
 345         stats_display();
 346 */
 347         return ERR_OK;
 348 }
 349
 350 static void close_conn(struct tcp_pcb *pcb, struct rfb_state *state) {
 351         outputf("close_conn: bailing");
 352         tcp_arg(pcb, NULL);
 353         tcp_sent(pcb, NULL);
 354         tcp_recv(pcb, NULL);
 355         mem_free(state);
 356         mem_free(state->blockbuf);
 357         tcp_close(pcb);
 358         outputf("close_conn: done");
 359 }
 360
 361 enum fsm_result {
 362         NEEDMORE,
 363         OK,
 364         FAIL
 365 };
 366
 367 static enum fsm_result recv_fsm(struct tcp_pcb *pcb, struct rfb_state *state) {
 368         int i;
 369         int pktsize;
 370
 371         outputf("RFB FSM: st %d rp %d wp %d", state->state, state->readpos,
 372                 state->writepos);
 373
 374         switch(state->state) {
 375         case ST_BEGIN:
 376                 if (state->writepos < 12) return NEEDMORE;
 377
 378                 if (!strncmp(state->data, "RFB 003.003\n", 12)) {
 379                         state->version = 3;
 380                 } else if (!strncmp(state->data, "RFB 003.005\n", 12)) {
 381                         /* Spec states that "RFB 003.005", an incorrect value,
 382                          * should be treated by the server as 3.3. */
 383                         state->version = 3;
 384                 } else if (!strncmp(state->data, "RFB 003.007\n", 12)) {
 385                         state->version = 7;
 386                 } else if (!strncmp(state->data, "RFB 003.008\n", 12)) {
 387                         state->version = 8;
 388                 } else {
 389                         outputf("RFB: Negotiation fail");
 390                         return FAIL;
 391                 }
 392
 393                 outputf("RFB: Negotiated v3.%d", state->version);
 394
 395                 state->readpos += 12;
 396                 state->state = ST_CLIENTINIT;
 397
 398                 /* We support one security type, currently "none".
 399                  * Send that and SecurityResult. */
 400                 if (state->version >= 7) {
 401                         tcp_write(pcb, "\x01\x01\x00\x00\x00\x00", 6, 0);
 402                 } else {
 403                         tcp_write(pcb, "\x01\x00\x00\x00\x00", 5, 0);
 404                 }
 405
 406                 tcp_output(pcb);
 407
 408                 return OK;
 409
 410         case ST_CLIENTINIT:
 411                 if (state->version >= 7) {
 412                         /* Ignore the security type and ClientInit */
 413                         if (state->writepos < 2) return NEEDMORE;
 414                         state->readpos += 2;
 415                 } else {
 416                         /* Just ClientInit */
 417                         if (state->writepos < 1) return NEEDMORE;
 418                         state->readpos += 1;
 419                 }
 420
 421                 state->state = ST_MAIN;
 422
 423                 outputf("RFB: Sending server info", state->version);
 424                 tcp_write(pcb, &server_info, sizeof(server_info), TCP_WRITE_FLAG_COPY);
 425                 tcp_output(pcb);
 426
 427                 return OK;
 428
 429         case ST_MAIN:
 430                 if (state->writepos < 1) return NEEDMORE;
 431
 432                 outputf("RFB: cmd %d", state->data[0]);
 433                 switch (state->data[0]) {
 434
 435                 case SET_PIXEL_FORMAT:
 436                         /* SetPixelFormat */
 437                         if (state->writepos < (sizeof(struct pixel_format) + 4))
 438                                 return NEEDMORE;
 439                         outputf("RFB: SetPixelFormat");
 440 /*
 441                         struct pixel_format * new_fmt =
 442                                 (struct pixel_format *)(&state->data[4]);
 443 */
 444                         /* XXX ... */
 445
 446                         state->readpos += sizeof(struct pixel_format) + 4;
 447                         return OK;
 448
 449                 case SET_ENCODINGS:
 450                         if (state->writepos < 4) return NEEDMORE;
 451
 452                         struct set_encs_req * req = (struct set_encs_req *)state->data;
 453
 454                         pktsize = sizeof(struct set_encs_req) + (4 * ntohs(req->num));
 455
 456                         outputf("RFB: SetEncodings [%d]", ntohs(req->num));
 457                         if (state->writepos < pktsize) return NEEDMORE;
 458
 459                         for (i = 0; i < ntohs(req->num); i++) {
 460                                 outputf("RFB: Encoding: %d", ntohl(req->encodings[i]));
 461                                 /* XXX ... */
 462                         }
 463
 464                         state->readpos += pktsize;
 465                         return OK;
 466
 467                 case FB_UPDATE_REQUEST:
 468                         if (state->writepos < sizeof(struct fb_update_req))
 469                                 return NEEDMORE;
 470                         outputf("RFB: UpdateRequest");
 471
 472                         state->update_requested = 1;
 473                         memcpy(&state->client_interest_area, state->data,
 474                                sizeof(struct fb_update_req));
 475
 476                         state->readpos += sizeof(struct fb_update_req);
 477                         return OK;
 478
 479                 case KEY_EVENT:
 480                         if (state->writepos < sizeof(struct key_event_pkt))
 481                                 return NEEDMORE;
 482
 483                         struct key_event_pkt * p = (struct key_event_pkt *)state->data;
 484
 485                         outputf("RFB: Key: %d (%c)", htonl(p->keysym), (htonl(p->keysym) & 0xFF));
 486                         kbd_inject_keysym(htonl(p->keysym), p->downflag);
 487
 488                         state->readpos += sizeof(struct key_event_pkt);
 489                         return OK;
 490
 491                 case POINTER_EVENT:
 492                         if (state->writepos < sizeof(struct pointer_event_pkt))
 493                                 return NEEDMORE;
 494                         outputf("RFB: Pointer");
 495
 496                         /* XXX stub */
 497
 498                         state->readpos += sizeof(struct pointer_event_pkt);
 499                         return OK;
 500
 501                 case CLIENT_CUT_TEXT:
 502                         if (state->writepos < sizeof(struct text_event_pkt))
 503                                 return NEEDMORE;
 504                         outputf("RFB: Cut Text");
 505
 506                         struct text_event_pkt * pkt =
 507                                 (struct text_event_pkt *)state->data;
 508
 509                         if (state->writepos < sizeof(struct text_event_pkt)
 510                                               + pkt->length)
 511                                 return NEEDMORE;
 512
 513                         /* XXX stub */
 514
 515                         state->readpos += sizeof(struct text_event_pkt)
 516                                           + pkt->length;
 517                         return OK;
 518
 519                 default:
 520                         outputf("RFB: Bad command: %d", state->data[0]);
 521                         return FAIL;
 522                 }
 523         default:
 524                 outputf("RFB: Bad state");
 525                 return FAIL;
 526         }
 527 }
 528
 529 static err_t rfb_recv(void *arg, struct tcp_pcb *pcb,
 530                       struct pbuf *p, err_t err) {
 531         struct rfb_state *state = arg;
 532         uint16_t copylen;
 533
 534         if (state == NULL)
 535
 536         if (err != ERR_OK) {
 537                 outputf("RFB: recv err %d", err);
 538                 /* FIXME do something better here? */
 539                 return ERR_OK;
 540         }
 541
 542         if (p == NULL) {
 543                 outputf("RFB: Connection closed");
 544                 close_conn(pcb, state);
 545                 return ERR_OK;
 546         }
 547
 548         if (p->tot_len > (RFB_BUF_SIZE - state->writepos)) {
 549                 /* Overflow! */
 550                 outputf("RFB: Overflow!");
 551                 close_conn(pcb, state);
 552                 return ERR_OK;
 553         }
 554
 555         copylen = pbuf_copy_partial(p, state->data + state->writepos, p->tot_len, 0);
 556         outputf("RFB: Processing %d, wp %d, cp %d", p->tot_len, state->writepos, copylen);
 557         state->writepos += p->tot_len;
 558
 559         tcp_recved(pcb, p->tot_len);
 560         pbuf_free(p);
 561
 562         while (1) {
 563                 switch (recv_fsm(pcb, state)) {
 564                 case NEEDMORE:
 565                         outputf("RFB FSM: blocking");
 566                         goto doneprocessing;
 567
 568                 case OK:
 569                         outputf("RFB FSM: ok");
 570
 571                         if (state->readpos == state->writepos) {
 572                                 state->readpos = 0;
 573                                 state->writepos = 0;
 574                                 goto doneprocessing;
 575                         } else {
 576                                 memmove(state->data,
 577                                         state->data + state->readpos,
 578                                         state->writepos - state->readpos);
 579                                 state->writepos -= state->readpos;
 580                                 state->readpos = 0;
 581                         }
 582                         break;
 583                 case FAIL:
 584                         /* Shit */
 585                         outputf("RFB: Protocol error");
 586                         close_conn(pcb, state);
 587                         return ERR_OK;
 588                 }
 589         }
 590
 591 doneprocessing:
 592
 593         /* Kick off a send. */
 594         if (state->send_state == SST_IDLE && state->update_requested) {
 595                 send_fsm(pcb, state);
 596         }
 597
 598         return ERR_OK;
 599 }
 600
 601 static err_t rfb_accept(void *arg, struct tcp_pcb *pcb, err_t err) {
 602         struct rfb_state *state;
 603         char * blockbuf;
 604
 605         LWIP_UNUSED_ARG(arg);
 606         LWIP_UNUSED_ARG(err);
 607
 608         state = (struct rfb_state *)mem_malloc(sizeof(struct rfb_state));
 609
 610         if (!state)
 611         {
 612                 outputf("rfb_accept: out of memory\n");
 613                 return ERR_MEM;
 614         }
 615
 616         memset(state, 0, sizeof(struct rfb_state));
 617
 618         blockbuf = mem_malloc(ceildiv(fb->curmode.xres, SCREEN_CHUNKS_X)
 619                             * ceildiv(fb->curmode.yres, SCREEN_CHUNKS_Y) * 4);
 620
 621         if (!blockbuf)
 622         {
 623                 outputf("rfb_accept: out of memory allocating blockbuf\n");
 624                 mem_free(state);
 625                 return ERR_MEM;
 626         }
 627
 628         state->blockbuf = blockbuf;
 629         state->state = ST_BEGIN;
 630         state->send_state = SST_IDLE;
 631
 632         /* XXX: update_server_info() should be called from the 64ms timer, and deal
 633          * with screen resizes appropriately. */
 634         update_server_info();
 635
 636         tcp_arg(pcb, state);
 637         tcp_recv(pcb, rfb_recv);
 638         tcp_sent(pcb, rfb_sent);
 639         tcp_poll(pcb, rfb_poll, 1);
 640 /*
 641         tcp_err(pcb, rfb_err);
 642 */
 643         tcp_write(pcb, "RFB 003.008\n", 12, 0);
 644         tcp_output(pcb);
 645
 646         return ERR_OK;
 647 }
 648
 649 void rfb_init() {
 650         struct tcp_pcb *pcb;
 651
 652         init_server_info();
 653
 654         pcb = tcp_new();
 655         tcp_bind(pcb, IP_ADDR_ANY, RFB_PORT);
 656         pcb = tcp_listen(pcb);
 657         tcp_accept(pcb, rfb_accept);
 658 }