net/rfb.c

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