src/websock.c

   1 /*
   2  * Copyright (C) 2016, 2017 "IoT.bzh"
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  *     http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17 /*
  18  * This work is a far adaptation of apache-websocket:
  19  *   origin:  https://github.com/disconnect/apache-websocket
  20  *   commit:  cfaef071223f11ba016bff7e1e4b7c9e5df45b50
  21  *   Copyright 2010-2012 self.disconnect (APACHE-2)
  22  */
  23
  24 #include <stdlib.h>
  25 #include <stdint.h>
  26 #include <errno.h>
  27 #include <string.h>
  28 #include <sys/uio.h>
  29
  30 #include "websock.h"
  31
  32 #define BLOCK_DATA_SIZE              4096
  33
  34 #define FRAME_GET_FIN(BYTE)         (((BYTE) >> 7) & 0x01)
  35 #define FRAME_GET_RSV1(BYTE)        (((BYTE) >> 6) & 0x01)
  36 #define FRAME_GET_RSV2(BYTE)        (((BYTE) >> 5) & 0x01)
  37 #define FRAME_GET_RSV3(BYTE)        (((BYTE) >> 4) & 0x01)
  38 #define FRAME_GET_OPCODE(BYTE)      ( (BYTE)       & 0x0F)
  39 #define FRAME_GET_MASK(BYTE)        (((BYTE) >> 7) & 0x01)
  40 #define FRAME_GET_PAYLOAD_LEN(BYTE) ( (BYTE)       & 0x7F)
  41
  42 #define FRAME_SET_FIN(BYTE)         (((BYTE) & 0x01) << 7)
  43 #define FRAME_SET_RSV1(BYTE)        (((BYTE) & 0x01) << 6)
  44 #define FRAME_SET_RSV2(BYTE)        (((BYTE) & 0x01) << 5)
  45 #define FRAME_SET_RSV3(BYTE)        (((BYTE) & 0x01) << 4)
  46 #define FRAME_SET_OPCODE(BYTE)      ((BYTE) & 0x0F)
  47 #define FRAME_SET_MASK(BYTE)        (((BYTE) & 0x01) << 7)
  48 #define FRAME_SET_LENGTH(X64, IDX)  (unsigned char)((sizeof(X64)) <= (IDX) ? 0 : (((X64) >> ((IDX)*8)) & 0xFF))
  49
  50 #define OPCODE_CONTINUATION 0x0
  51 #define OPCODE_TEXT         0x1
  52 #define OPCODE_BINARY       0x2
  53 #define OPCODE_CLOSE        0x8
  54 #define OPCODE_PING         0x9
  55 #define OPCODE_PONG         0xA
  56
  57 #define STATE_INIT    0
  58 #define STATE_START   1
  59 #define STATE_LENGTH  2
  60 #define STATE_DATA    3
  61
  62 struct websock {
  63         int state;
  64         uint64_t maxlength;
  65         int lenhead, szhead;
  66         uint64_t length;
  67         uint32_t mask;
  68         unsigned char header[14];       /* 2 + 8 + 4 */
  69         const struct websock_itf *itf;
  70         void *closure;
  71 };
  72
  73 static ssize_t ws_writev(struct websock *ws, const struct iovec *iov, int iovcnt)
  74 {
  75         return ws->itf->writev(ws->closure, iov, iovcnt);
  76 }
  77
  78 static ssize_t ws_readv(struct websock *ws, const struct iovec *iov, int iovcnt)
  79 {
  80         return ws->itf->readv(ws->closure, iov, iovcnt);
  81 }
  82
  83 #if 0
  84 static ssize_t ws_write(struct websock *ws, const void *buffer, size_t buffer_size)
  85 {
  86         struct iovec iov;
  87         iov.iov_base = (void *)buffer;  /* const cast */
  88         iov.iov_len = buffer_size;
  89         return ws_writev(ws, &iov, 1);
  90 }
  91 #endif
  92
  93 static ssize_t ws_read(struct websock *ws, void *buffer, size_t buffer_size)
  94 {
  95         struct iovec iov;
  96         iov.iov_base = buffer;
  97         iov.iov_len = buffer_size;
  98         return ws_readv(ws, &iov, 1);
  99 }
 100
 101 static int websock_send_internal_v(struct websock *ws, unsigned char first, const struct iovec *iovec, int count)
 102 {
 103         struct iovec iov[32];
 104         int i, j;
 105         size_t pos, size, len;
 106         ssize_t rc;
 107         unsigned char header[32];
 108
 109         /* checks count */
 110         if (count < 0 || (count + 1) > (int)(sizeof iov / sizeof * iov)) {
 111                 errno = EINVAL;
 112                 return -1;
 113         }
 114
 115         /* computes the size */
 116         size = 0;
 117         i = 1;
 118         for (j = 0 ; j < count ; j++) {
 119                 iov[i].iov_base = iovec[j].iov_base;
 120                 len = iovec[j].iov_len;
 121                 if (len != 0) {
 122                         iov[i].iov_len = len;
 123                         size += len;
 124                         i++;
 125                 }
 126         }
 127
 128         /* makes the header */
 129         pos = 0;
 130         header[pos++] = first;
 131         size = (uint64_t) size;
 132         if (size < 126) {
 133                 header[pos++] = FRAME_SET_MASK(0) | FRAME_SET_LENGTH(size, 0);
 134         } else {
 135                 if (size < 65536) {
 136                         header[pos++] = FRAME_SET_MASK(0) | 126;
 137                 } else {
 138                         header[pos++] = FRAME_SET_MASK(0) | 127;
 139                         header[pos++] = FRAME_SET_LENGTH(size, 7);
 140                         header[pos++] = FRAME_SET_LENGTH(size, 6);
 141                         header[pos++] = FRAME_SET_LENGTH(size, 5);
 142                         header[pos++] = FRAME_SET_LENGTH(size, 4);
 143                         header[pos++] = FRAME_SET_LENGTH(size, 3);
 144                         header[pos++] = FRAME_SET_LENGTH(size, 2);
 145                 }
 146                 header[pos++] = FRAME_SET_LENGTH(size, 1);
 147                 header[pos++] = FRAME_SET_LENGTH(size, 0);
 148         }
 149
 150         /* allocates the vec */
 151         iov[0].iov_base = header;
 152         iov[0].iov_len = pos;
 153         rc = ws_writev(ws, iov, i);
 154
 155         return rc < 0 ? -1 : 0;
 156 }
 157
 158 static int websock_send_internal(struct websock *ws, unsigned char first, const void *buffer, size_t size)
 159 {
 160         struct iovec iov;
 161
 162         iov.iov_base = (void *)buffer;
 163         iov.iov_len = size;
 164         return websock_send_internal_v(ws, first, &iov, 1);
 165 }
 166
 167 static inline int websock_send_v(struct websock *ws, int last, int rsv1, int rsv2, int rsv3, int opcode, const struct iovec *iovec, int count)
 168 {
 169         unsigned char first = (unsigned char)(FRAME_SET_FIN(last)
 170                                 | FRAME_SET_RSV1(rsv1)
 171                                 | FRAME_SET_RSV1(rsv2)
 172                                 | FRAME_SET_RSV1(rsv3)
 173                                 | FRAME_SET_OPCODE(opcode));
 174         return websock_send_internal_v(ws, first, iovec, count);
 175 }
 176
 177 static inline int websock_send(struct websock *ws, int last, int rsv1, int rsv2, int rsv3, int opcode, const void *buffer, size_t size)
 178 {
 179         unsigned char first = (unsigned char)(FRAME_SET_FIN(last)
 180                                 | FRAME_SET_RSV1(rsv1)
 181                                 | FRAME_SET_RSV1(rsv2)
 182                                 | FRAME_SET_RSV1(rsv3)
 183                                 | FRAME_SET_OPCODE(opcode));
 184         return websock_send_internal(ws, first, buffer, size);
 185 }
 186
 187 int websock_close_empty(struct websock *ws)
 188 {
 189         return websock_close(ws, WEBSOCKET_CODE_NOT_SET, NULL, 0);
 190 }
 191
 192 int websock_close(struct websock *ws, uint16_t code, const void *data, size_t length)
 193 {
 194         unsigned char buffer[2];
 195         struct iovec iov[2];
 196
 197         if (code == WEBSOCKET_CODE_NOT_SET && length == 0)
 198                 return websock_send(ws, 1, 0, 0, 0, OPCODE_CLOSE, NULL, 0);
 199
 200         /* checks the length */
 201         if (length > 123) {
 202                 errno = EINVAL;
 203                 return -1;
 204         }
 205
 206         /* prepare the buffer */
 207         buffer[0] = (unsigned char)((code >> 8) & 0xFF);
 208         buffer[1] = (unsigned char)(code & 0xFF);
 209
 210         /* Send server-side closing handshake */
 211         iov[0].iov_base = (void *)buffer;
 212         iov[0].iov_len = 2;
 213         iov[1].iov_base = (void *)data;
 214         iov[1].iov_len = length;
 215         return websock_send_v(ws, 1, 0, 0, 0, OPCODE_CLOSE, iov, 2);
 216 }
 217
 218 int websock_ping(struct websock *ws, const void *data, size_t length)
 219 {
 220         /* checks the length */
 221         if (length > 125) {
 222                 errno = EINVAL;
 223                 return -1;
 224         }
 225
 226         return websock_send(ws, 1, 0, 0, 0, OPCODE_PING, data, length);
 227 }
 228
 229 int websock_pong(struct websock *ws, const void *data, size_t length)
 230 {
 231         /* checks the length */
 232         if (length > 125) {
 233                 errno = EINVAL;
 234                 return -1;
 235         }
 236
 237         return websock_send(ws, 1, 0, 0, 0, OPCODE_PONG, data, length);
 238 }
 239
 240 int websock_text(struct websock *ws, int last, const void *text, size_t length)
 241 {
 242         return websock_send(ws, last, 0, 0, 0, OPCODE_TEXT, text, length);
 243 }
 244
 245 int websock_text_v(struct websock *ws, int last, const struct iovec *iovec, int count)
 246 {
 247         return websock_send_v(ws, last, 0, 0, 0, OPCODE_TEXT, iovec, count);
 248 }
 249
 250 int websock_binary(struct websock *ws, int last, const void *data, size_t length)
 251 {
 252         return websock_send(ws, last, 0, 0, 0, OPCODE_BINARY, data, length);
 253 }
 254
 255 int websock_binary_v(struct websock *ws, int last, const struct iovec *iovec, int count)
 256 {
 257         return websock_send_v(ws, last, 0, 0, 0, OPCODE_BINARY, iovec, count);
 258 }
 259
 260 int websock_continue(struct websock *ws, int last, const void *data, size_t length)
 261 {
 262         return websock_send(ws, last, 0, 0, 0, OPCODE_CONTINUATION, data, length);
 263 }
 264
 265 int websock_continue_v(struct websock *ws, int last, const struct iovec *iovec, int count)
 266 {
 267         return websock_send_v(ws, last, 0, 0, 0, OPCODE_CONTINUATION, iovec, count);
 268 }
 269
 270 int websock_error(struct websock *ws, uint16_t code, const void *data, size_t size)
 271 {
 272         int rc = websock_close(ws, code, data, size);
 273         if (ws->itf->on_error != NULL)
 274                 ws->itf->on_error(ws->closure, code, data, size);
 275         return rc;
 276 }
 277
 278 static int read_header(struct websock *ws)
 279 {
 280         if (ws->lenhead < ws->szhead) {
 281                 ssize_t rbc =
 282                     ws_read(ws, &ws->header[ws->lenhead], (size_t)(ws->szhead - ws->lenhead));
 283                 if (rbc < 0)
 284                         return -1;
 285                 ws->lenhead += (int)rbc;
 286         }
 287         return 0;
 288 }
 289
 290 static int check_control_header(struct websock *ws)
 291 {
 292         /* sanity checks */
 293         if (FRAME_GET_RSV1(ws->header[0]) != 0)
 294                 return 0;
 295         if (FRAME_GET_RSV2(ws->header[0]) != 0)
 296                 return 0;
 297         if (FRAME_GET_RSV3(ws->header[0]) != 0)
 298                 return 0;
 299         if (FRAME_GET_PAYLOAD_LEN(ws->header[1]) > 125)
 300                 return 0;
 301         if (FRAME_GET_OPCODE(ws->header[0]) == OPCODE_CLOSE)
 302                 return FRAME_GET_PAYLOAD_LEN(ws->header[1]) != 1;
 303         return 1;
 304 }
 305
 306 int websock_dispatch(struct websock *ws, int loop)
 307 {
 308         uint16_t code;
 309 loop:
 310         switch (ws->state) {
 311         case STATE_INIT:
 312                 ws->lenhead = 0;
 313                 ws->szhead = 2;
 314                 ws->state = STATE_START;
 315                 /*@fallthrough@*/
 316
 317         case STATE_START:
 318                 /* read the header */
 319                 if (read_header(ws))
 320                         return -1;
 321                 else if (ws->lenhead < ws->szhead)
 322                         return 0;
 323                 /* fast track */
 324                 switch (FRAME_GET_OPCODE(ws->header[0])) {
 325                 case OPCODE_CONTINUATION:
 326                 case OPCODE_TEXT:
 327                 case OPCODE_BINARY:
 328                         break;
 329                 case OPCODE_CLOSE:
 330                         if (!check_control_header(ws))
 331                                 goto protocol_error;
 332                         if (FRAME_GET_PAYLOAD_LEN(ws->header[1]))
 333                                 ws->szhead += 2;
 334                         break;
 335                 case OPCODE_PING:
 336                 case OPCODE_PONG:
 337                         if (!check_control_header(ws))
 338                                 goto protocol_error;
 339                 default:
 340                         break;
 341                 }
 342                 /* update heading size */
 343                 switch (FRAME_GET_PAYLOAD_LEN(ws->header[1])) {
 344                 case 127:
 345                         ws->szhead += 6;
 346                         /*@fallthrough@*/
 347                 case 126:
 348                         ws->szhead += 2;
 349                         /*@fallthrough@*/
 350                 default:
 351                         ws->szhead += 4 * FRAME_GET_MASK(ws->header[1]);
 352                 }
 353                 ws->state = STATE_LENGTH;
 354                 /*@fallthrough@*/
 355
 356         case STATE_LENGTH:
 357                 /* continue to read the header */
 358                 if (read_header(ws))
 359                         return -1;
 360                 else if (ws->lenhead < ws->szhead)
 361                         return 0;
 362
 363                 /* compute length */
 364                 switch (FRAME_GET_PAYLOAD_LEN(ws->header[1])) {
 365                 case 127:
 366                         ws->length = (((uint64_t) ws->header[2]) << 56)
 367                             | (((uint64_t) ws->header[3]) << 48)
 368                             | (((uint64_t) ws->header[4]) << 40)
 369                             | (((uint64_t) ws->header[5]) << 32)
 370                             | (((uint64_t) ws->header[6]) << 24)
 371                             | (((uint64_t) ws->header[7]) << 16)
 372                             | (((uint64_t) ws->header[8]) << 8)
 373                             | (uint64_t) ws->header[9];
 374                         break;
 375                 case 126:
 376                         ws->length = (((uint64_t) ws->header[2]) << 8)
 377                             | (uint64_t) ws->header[3];
 378                         break;
 379                 default:
 380                         ws->length = FRAME_GET_PAYLOAD_LEN(ws->header[1]);
 381                         break;
 382                 }
 383                 if (FRAME_GET_OPCODE(ws->header[0]) == OPCODE_CLOSE && ws->length != 0)
 384                         ws->length -= 2;
 385                 if (ws->length > ws->maxlength)
 386                         goto too_long_error;
 387
 388                 /* compute mask */
 389                 if (FRAME_GET_MASK(ws->header[1])) {
 390                         ((unsigned char *)&ws->mask)[0] = ws->header[ws->szhead - 4];
 391                         ((unsigned char *)&ws->mask)[1] = ws->header[ws->szhead - 3];
 392                         ((unsigned char *)&ws->mask)[2] = ws->header[ws->szhead - 2];
 393                         ((unsigned char *)&ws->mask)[3] = ws->header[ws->szhead - 1];
 394                 } else
 395                         ws->mask = 0;
 396
 397                 /* all heading fields are known, process */
 398                 ws->state = STATE_DATA;
 399                 if (ws->itf->on_extension != NULL) {
 400                         if (ws->itf->on_extension(ws->closure,
 401                                         FRAME_GET_FIN(ws->header[0]),
 402                                         FRAME_GET_RSV1(ws->header[0]),
 403                                         FRAME_GET_RSV2(ws->header[0]),
 404                                         FRAME_GET_RSV3(ws->header[0]),
 405                                         FRAME_GET_OPCODE(ws->header[0]),
 406                                         (size_t) ws->length)) {
 407                                 return 0;
 408                         }
 409                 }
 410
 411                 /* not an extension case */
 412                 if (FRAME_GET_RSV1(ws->header[0]) != 0)
 413                         goto protocol_error;
 414                 if (FRAME_GET_RSV2(ws->header[0]) != 0)
 415                         goto protocol_error;
 416                 if (FRAME_GET_RSV3(ws->header[0]) != 0)
 417                         goto protocol_error;
 418
 419                 /* handle */
 420                 switch (FRAME_GET_OPCODE(ws->header[0])) {
 421                 case OPCODE_CONTINUATION:
 422                         ws->itf->on_continue(ws->closure,
 423                                              FRAME_GET_FIN(ws->header[0]),
 424                                              (size_t) ws->length);
 425                         if (!loop)
 426                                 return 0;
 427                         break;
 428                 case OPCODE_TEXT:
 429                         ws->itf->on_text(ws->closure,
 430                                          FRAME_GET_FIN(ws->header[0]),
 431                                          (size_t) ws->length);
 432                         if (!loop)
 433                                 return 0;
 434                         break;
 435                 case OPCODE_BINARY:
 436                         ws->itf->on_binary(ws->closure,
 437                                            FRAME_GET_FIN(ws->header[0]),
 438                                            (size_t) ws->length);
 439                         if (!loop)
 440                                 return 0;
 441                         break;
 442                 case OPCODE_CLOSE:
 443                         if (ws->length == 0)
 444                                 code = WEBSOCKET_CODE_NOT_SET;
 445                         else {
 446                                 code = (uint16_t)(ws->header[ws->szhead - 2] & 0xff);
 447                                 code = (uint16_t)(code << 8);
 448                                 code = (uint16_t)(code | (uint16_t)(ws->header[ws->szhead - 1] & 0xff));
 449                         }
 450                         ws->itf->on_close(ws->closure, code, (size_t) ws->length);
 451                         return 0;
 452                 case OPCODE_PING:
 453                         if (ws->itf->on_ping)
 454                                 ws->itf->on_ping(ws->closure, ws->length);
 455                         else {
 456                                 websock_drop(ws);
 457                                 websock_pong(ws, NULL, 0);
 458                         }
 459                         ws->state = STATE_INIT;
 460                         if (!loop)
 461                                 return 0;
 462                         break;
 463                 case OPCODE_PONG:
 464                         if (ws->itf->on_pong)
 465                                 ws->itf->on_pong(ws->closure, ws->length);
 466                         else
 467                                 websock_drop(ws);
 468                         ws->state = STATE_INIT;
 469                         if (!loop)
 470                                 return 0;
 471                         break;
 472                 default:
 473                         goto protocol_error;
 474                 }
 475                 break;
 476
 477         case STATE_DATA:
 478                 if (ws->length)
 479                         return 0;
 480                 ws->state = STATE_INIT;
 481                 break;
 482         }
 483         goto loop;
 484
 485  too_long_error:
 486         websock_error(ws, WEBSOCKET_CODE_MESSAGE_TOO_LARGE, NULL, 0);
 487         return 0;
 488
 489  protocol_error:
 490         websock_error(ws, WEBSOCKET_CODE_PROTOCOL_ERROR, NULL, 0);
 491         return 0;
 492 }
 493
 494 static void unmask(struct websock * ws, void *buffer, size_t size)
 495 {
 496         uint32_t mask, *b32;
 497         uint8_t m, *b8;
 498
 499         mask = ws->mask;
 500         b8 = buffer;
 501         while (size && ((sizeof(uint32_t) - 1) & (uintptr_t) b8)) {
 502                 m = ((uint8_t *) & mask)[0];
 503                 ((uint8_t *) & mask)[0] = ((uint8_t *) & mask)[1];
 504                 ((uint8_t *) & mask)[1] = ((uint8_t *) & mask)[2];
 505                 ((uint8_t *) & mask)[2] = ((uint8_t *) & mask)[3];
 506                 ((uint8_t *) & mask)[3] = m;
 507                 *b8++ ^= m;
 508                 size--;
 509         }
 510         b32 = (uint32_t *) b8;
 511         while (size >= sizeof(uint32_t)) {
 512                 *b32++ ^= mask;
 513                 size -= sizeof(uint32_t);
 514         }
 515         b8 = (uint8_t *) b32;
 516         while (size) {
 517                 m = ((uint8_t *) & mask)[0];
 518                 ((uint8_t *) & mask)[0] = ((uint8_t *) & mask)[1];
 519                 ((uint8_t *) & mask)[1] = ((uint8_t *) & mask)[2];
 520                 ((uint8_t *) & mask)[2] = ((uint8_t *) & mask)[3];
 521                 ((uint8_t *) & mask)[3] = m;
 522                 *b8++ ^= m;
 523                 size--;
 524         }
 525         ws->mask = mask;
 526 }
 527
 528 ssize_t websock_read(struct websock * ws, void *buffer, size_t size)
 529 {
 530         ssize_t rc;
 531
 532         if (ws->state != STATE_DATA)
 533                 return 0;
 534
 535         if (size > ws->length)
 536                 size = (size_t) ws->length;
 537
 538         rc = ws_read(ws, buffer, size);
 539         if (rc > 0) {
 540                 size = (size_t) rc;
 541                 ws->length -= size;
 542
 543                 if (ws->mask != 0)
 544                         unmask(ws, buffer, size);
 545         }
 546         return rc;
 547 }
 548
 549 int websock_drop(struct websock *ws)
 550 {
 551         char buffer[8000];
 552
 553         while (ws->length)
 554                 if (websock_read(ws, buffer, sizeof buffer) < 0)
 555                         return -1;
 556         return 0;
 557 }
 558
 559 struct websock *websock_create_v13(const struct websock_itf *itf, void *closure)
 560 {
 561         struct websock *result = calloc(1, sizeof *result);
 562         if (result) {
 563                 result->itf = itf;
 564                 result->closure = closure;
 565                 result->maxlength = 65000;
 566         }
 567         return result;
 568 }
 569
 570 void websock_destroy(struct websock *ws)
 571 {
 572         free(ws);
 573 }