commit 999ac89fc735bd3f51c5853680828e18ca315b1b
parent a876b51bb7c802939bda8f7d0bf146413b9b7c01
Author: Remy Noulin <loader2x@gmail.com>
Date: Sun, 16 Sep 2018 15:11:42 +0200
websocket frame builder and decoder
package.yml | 32 ++++
ws.c | 485 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
ws.h | 37 +++++
3 files changed, 554 insertions(+)
Diffstat:
| A | package.yml | | | 32 | ++++++++++++++++++++++++++++++++ |
| A | ws.c | | | 485 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| A | ws.h | | | 37 | +++++++++++++++++++++++++++++++++++++ |
3 files changed, 554 insertions(+), 0 deletions(-)
diff --git a/package.yml b/package.yml
@@ -0,0 +1,32 @@
+---
+ name: ws
+ version: 0.0.1
+ description: "Websocket frame builder and decoder (no transport)"
+ bin: ./ws.c
+ #cflags: -DA -ggdb -std=gnu11 -fPIC -pipe
+ #lflags: -lpcre
+ repository:
+ type: git
+ url: git+https://github.com/RemyNoun/ws.git
+ keywords:
+ - library
+ - websocket
+ author: Remy
+ license: MIT
+ bugs:
+ url: https://github.com/RemyNoulin/ws/issues
+ homepage: https://github.com/RemyNoulin/ws#readme
+ #compileHelp: # text displayed when there is a compilation error
+ #dependencies:
+ # md4c:
+ # Test configuration:
+ #testBin: ./testWs.c
+ #testCflags: -ggdb -std=gnu11 -fPIC -pipe -fprofile-arcs -ftest-coverage -Wall -Wextra
+ #testLflags: -lcheck_pic -lrt -lm -lsubunit -fprofile-arcs -ftest-coverage -rdynamic
+ # Memcheck configuration:
+ #memcheckBin: ./memcheckWs.c
+ #memcheckCmd: valgrind --leak-check=full --show-leak-kinds=all
+ #memcheckCflags: -ggdb -std=gnu11 -fPIC -pipe
+ #memcheckLflags: -rdynamic
+ #documentationCmd: # command for generating the documentation with spm doc
+ private: false # true for private package
diff --git a/ws.c b/ws.c
@@ -0,0 +1,485 @@
+#include "ws.h"
+#include "libsheepyObject.h"
+// from libsheepy.h:
+// internal
+// u8, u16, u32, u64
+// randomUrandomOpen, randomWord, randomUrandomClose
+// bCatS, eqS
+// logE
+// range
+// MIN, MIN3
+//
+// from libsheepyObject.h:
+// findG, indexOfG
+// setG
+
+#include <arpa/inet.h>
+
+/* enable/disable logging */
+#undef pLog
+#define pLog(...)
+
+#define BASE64_ENCODED_SIZE(len) (len+2)/3*4+1
+
+const char *BASE64_CHARS = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
+
+void _base64_encode_triple(u8 triple[3], char result[4]) {
+ u32 tripleValue;
+
+ tripleValue = triple[0];
+ tripleValue *= 256;
+ tripleValue += triple[1];
+ tripleValue *= 256;
+ tripleValue += triple[2];
+
+ range(i,4) {
+ result[3-i] = BASE64_CHARS[tripleValue%64];
+ tripleValue /= 64;
+ }
+}
+
+bool base64_encode(u8 *source, size_t sourcelen, char *target, size_t targetlen) {
+
+ if ((sourcelen+2)/3*4 > targetlen-1)
+ return false;
+
+ while (sourcelen >= 3) {
+ _base64_encode_triple(source, target);
+ sourcelen -= 3;
+ source += 3;
+ target += 4;
+ }
+
+ if (sourcelen > 0) {
+ u8 temp[3];
+ memset(temp, 0, sizeof(temp));
+ memcpy(temp, source, sourcelen);
+ _base64_encode_triple(temp, target);
+ target[3] = '=';
+ if (sourcelen == 1)
+ target[2] = '=';
+
+ target += 4;
+ }
+
+ target[0] = 0;
+
+ return true;
+}
+
+struct sha1 {
+ u64 len; /* processed message length */
+ u32 h[5]; /* hash state */
+ u8 buf[64]; /* message block buffer */
+};
+
+#define SHA1_DIGEST_LEN 20
+
+/* reset state */
+internal void sha1Init(struct sha1 *ctx);
+
+/* process message */
+internal void sha1Update(struct sha1 *ctx, const void *m, u64 len);
+
+/* get message digest
+ * state is ruined after sum, keep a copy if multiple sum is needed
+ * part of the message might be left in s, zero it if secrecy is needed
+ */
+internal void sha1Final(struct sha1 *ctx, u8 md[SHA1_DIGEST_LEN]);
+
+internal u32 rol(u32 n, u8 k) { return (n << k) | (n >> (32-k)); }
+
+#define F0(b,c,d) (d ^ (b & (c ^ d)))
+#define F1(b,c,d) (b ^ c ^ d)
+#define F2(b,c,d) ((b & c) | (d & (b | c)))
+#define F3(b,c,d) (b ^ c ^ d)
+#define G0(a,b,c,d,e,i) e += rol(a,5)+F0(b,c,d)+W[i]+0x5A827999; b = rol(b,30)
+#define G1(a,b,c,d,e,i) e += rol(a,5)+F1(b,c,d)+W[i]+0x6ED9EBA1; b = rol(b,30)
+#define G2(a,b,c,d,e,i) e += rol(a,5)+F2(b,c,d)+W[i]+0x8F1BBCDC; b = rol(b,30)
+#define G3(a,b,c,d,e,i) e += rol(a,5)+F3(b,c,d)+W[i]+0xCA62C1D6; b = rol(b,30)
+
+internal void processblock(struct sha1 *s, const u8 *buf) {
+ u32 W[80], a, b, c, d, e;
+ u8 i;
+
+ for (i = 0; i < 16; i++) {
+ W[i] = (u32)buf[4*i]<<24;
+ W[i] |= (u32)buf[4*i+1]<<16;
+ W[i] |= (u32)buf[4*i+2]<<8;
+ W[i] |= buf[4*i+3];
+ }
+ for (; i < 80; i++)
+ W[i] = rol(W[i-3] ^ W[i-8] ^ W[i-14] ^ W[i-16], 1);
+ a = s->h[0];
+ b = s->h[1];
+ c = s->h[2];
+ d = s->h[3];
+ e = s->h[4];
+ for (i = 0; i < 20; ) {
+ G0(a,b,c,d,e,i++);
+ G0(e,a,b,c,d,i++);
+ G0(d,e,a,b,c,i++);
+ G0(c,d,e,a,b,i++);
+ G0(b,c,d,e,a,i++);
+ }
+ while (i < 40) {
+ G1(a,b,c,d,e,i++);
+ G1(e,a,b,c,d,i++);
+ G1(d,e,a,b,c,i++);
+ G1(c,d,e,a,b,i++);
+ G1(b,c,d,e,a,i++);
+ }
+ while (i < 60) {
+ G2(a,b,c,d,e,i++);
+ G2(e,a,b,c,d,i++);
+ G2(d,e,a,b,c,i++);
+ G2(c,d,e,a,b,i++);
+ G2(b,c,d,e,a,i++);
+ }
+ while (i < 80) {
+ G3(a,b,c,d,e,i++);
+ G3(e,a,b,c,d,i++);
+ G3(d,e,a,b,c,i++);
+ G3(c,d,e,a,b,i++);
+ G3(b,c,d,e,a,i++);
+ }
+ s->h[0] += a;
+ s->h[1] += b;
+ s->h[2] += c;
+ s->h[3] += d;
+ s->h[4] += e;
+}
+
+internal void pad(struct sha1 *s) {
+ u8 r = s->len % 64;
+
+ s->buf[r++] = 0x80;
+ if (r > 56) {
+ memset(s->buf + r, 0, 64 - r);
+ r = 0;
+ processblock(s, s->buf);
+ }
+ memset(s->buf + r, 0, 56 - r);
+ s->len *= 8;
+ s->buf[56] = s->len >> 56;
+ s->buf[57] = s->len >> 48;
+ s->buf[58] = s->len >> 40;
+ s->buf[59] = s->len >> 32;
+ s->buf[60] = s->len >> 24;
+ s->buf[61] = s->len >> 16;
+ s->buf[62] = s->len >> 8;
+ s->buf[63] = s->len;
+ processblock(s, s->buf);
+}
+
+internal void sha1Init(struct sha1 *s) {
+ s->len = 0;
+ s->h[0] = 0x67452301;
+ s->h[1] = 0xEFCDAB89;
+ s->h[2] = 0x98BADCFE;
+ s->h[3] = 0x10325476;
+ s->h[4] = 0xC3D2E1F0;
+}
+
+internal void sha1Final(struct sha1 *s, u8 md[SHA1_DIGEST_LEN]) {
+
+ pad(s);
+ range(i,5) {
+ md[4*i] = s->h[i] >> 24;
+ md[4*i+1] = s->h[i] >> 16;
+ md[4*i+2] = s->h[i] >> 8;
+ md[4*i+3] = s->h[i];
+ }
+}
+
+internal void sha1Update(struct sha1 *s, const void *m, u64 len) {
+ const u8 *p = m;
+ u8 r = s->len % 64;
+
+ s->len += len;
+ if (r) {
+ if (len < 64 - r) {
+ memcpy(s->buf + r, p, len);
+ return;
+ }
+ memcpy(s->buf + r, p, 64 - r);
+ len -= 64 - r;
+ p += 64 - r;
+ processblock(s, s->buf);
+ }
+ for (; len >= 64; len -= 64, p += 64)
+ processblock(s, p);
+ memcpy(s->buf, p, len);
+}
+
+internal u64 keyRnd[2];
+internal char clientKey[BASE64_ENCODED_SIZE(sizeof(keyRnd))];
+
+internal const char handshake[] =
+ "GET /connect HTTP/1.1\r\n"
+ "Host: %s\r\n"
+ "Upgrade: websocket\r\n"
+ "Connection: Upgrade\r\n"
+ "Sec-WebSocket-Key: %s\r\n"
+ "Sec-WebSocket-Version: 13\r\n"
+ "\r\n";
+
+internal const char serverResponse[] = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11";
+
+size_t wsHandshakeSize(char *hostname) {
+ return (sizeof(handshake) -1 -4) + BASE64_ENCODED_SIZE(sizeof(keyRnd))-1 + strlen(hostname) +1;
+}
+
+int wsHandshake(char *frame, size_t size, char *hostname) {
+
+ randomUrandomOpen();
+ keyRnd[0] = randomWord();
+ keyRnd[1] = randomWord();
+ randomUrandomClose();
+
+ base64_encode((u8*)keyRnd, sizeof(keyRnd), clientKey, sizeof(clientKey));
+
+ snprintf(frame, size, "GET /connect HTTP/1.1\r\n"
+ "Host: %s\r\n"
+ "Upgrade: websocket\r\n"
+ "Connection: Upgrade\r\n"
+ "Sec-WebSocket-Key: %s\r\n"
+ "Sec-WebSocket-Version: 13\r\n"
+ "\r\n", hostname, clientKey);
+ return 0;
+}
+
+bool wsHanskakeCheck(char *frame, size_t size) {
+ char *srvRes;
+
+ if (srvRes = findG(frame, "sec-websocket-accept: ")) {
+ srvRes += 22;
+ int idx = indexOfG(srvRes, "\r\n");
+
+ if ((srvRes - frame + idx) > size)
+ return false;
+
+ setG(srvRes, idx, 0);
+
+ // createAcceptKey
+ char s[128];
+ bCatS(s, clientKey, serverResponse);
+
+ struct sha1 shc;
+ sha1Init(&shc);
+ sha1Update(&shc, s, BASE64_ENCODED_SIZE(sizeof(keyRnd)) -1 + sizeof(serverResponse) -1);
+ u8 digest[SHA1_DIGEST_LEN];
+ sha1Final(&shc, digest);
+
+ char base64Digest[800];
+ base64_encode(digest, sizeof(digest), base64Digest, sizeof(base64Digest));
+ if (!eqS(srvRes, base64Digest)) {
+ logE("Bad server response!");
+ return false;
+ }
+ }
+ else {
+ return false;
+ }
+ return true;
+}
+
+#define WS_FINAL_FRAME 1 << 7
+#define WS_OP_MASK 0xF
+#define WS_MASK 1 << 7
+#define WS_HEADER_SIZE 2
+#define WS_MASK_SIZE 4
+#define WS_LEN_MASK 0x7F
+
+#define WS_PAYLOAD_EXTEND_1 126
+#define WS_PAYLOAD_EXTEND_2 127
+
+#define WS_LEN_SIZE_1 2
+#define WS_LEN_SIZE_2 8
+
+// 1 if opcode is control frame opcode, otherwise 0
+#define isCtlFrame(opcode) ((opcode >> 3) & 1)
+
+ssize_t wsMaskSize(wsOpt op, size_t pSize) {
+ if (isCtlFrame(op) && pSize >= WS_PAYLOAD_EXTEND_1) return -1;
+ if (pSize < WS_PAYLOAD_EXTEND_1) return pSize + WS_HEADER_SIZE + WS_MASK_SIZE;
+ if (pSize < 1 << 16) return pSize + WS_HEADER_SIZE + WS_MASK_SIZE + WS_LEN_SIZE_1;
+ return pSize + WS_HEADER_SIZE + WS_MASK_SIZE + WS_LEN_SIZE_2;
+}
+
+ssize_t wsNoMaskSize(wsOpt op, size_t pSize) {
+ if (isCtlFrame(op) && pSize >= WS_PAYLOAD_EXTEND_1) return -1;
+ if (pSize < WS_PAYLOAD_EXTEND_1) return pSize + WS_HEADER_SIZE;
+ if (pSize < 1 << 16) return pSize + WS_HEADER_SIZE + WS_LEN_SIZE_1;
+ return pSize + WS_HEADER_SIZE + WS_LEN_SIZE_2;
+}
+
+ssize_t wsControlMaskSize(void) {
+ return WS_HEADER_SIZE + WS_MASK_SIZE;
+}
+
+ssize_t wsControlNoMaskSize(void) {
+ return WS_HEADER_SIZE;
+}
+
+bool wsMask(char *frame, size_t size, wsOpt op, char *payload, size_t pSize, bool final) {
+ #define WS_STEP1\
+ ssize_t frameSize = wsMaskSize(op, pSize);\
+ \
+ if (frameSize == -1 || frameSize > size) return false;\
+ \
+ zeroBuf(frame, frameSize);\
+ \
+ if (final) frame[0] = WS_FINAL_FRAME | op;\
+ else frame[0] = op;
+ WS_STEP1;
+
+ u8 wsPLen, maskOffset;
+ if (pSize < WS_PAYLOAD_EXTEND_1) {
+ wsPLen = pSize;
+ maskOffset = WS_HEADER_SIZE;
+ }
+ else if (pSize < 1 << 16) {
+ wsPLen = WS_PAYLOAD_EXTEND_1;
+ maskOffset = WS_HEADER_SIZE + WS_LEN_SIZE_1;
+ u16 *len = (u16*)&frame[2];
+ *len = htons((u16)pSize);
+ }
+ else {
+ wsPLen = WS_PAYLOAD_EXTEND_2;
+ maskOffset = WS_HEADER_SIZE + WS_LEN_SIZE_2;
+ u64 *len = (u64*)&frame[2];
+ *len = htobe64(pSize);
+
+ }
+ frame[1] = WS_MASK | wsPLen;
+ // mask key 4 bytes, random = always 0
+
+ u8 payloadOffset = maskOffset + WS_MASK_SIZE;
+
+ strncpy(frame+payloadOffset, payload, pSize);
+
+ range(i, pSize) {
+ frame[i + payloadOffset] ^= frame[maskOffset + (i % 4)];
+ }
+
+ return true;
+}
+
+bool wsNoMask(char *frame, size_t size, wsOpt op, char *payload, size_t pSize, bool final) {
+ WS_STEP1;
+
+ u8 wsPLen, payloadOffset;
+ if (pSize < WS_PAYLOAD_EXTEND_1) {
+ wsPLen = pSize;
+ payloadOffset = WS_HEADER_SIZE;
+ }
+ else if (pSize < 1 << 16) {
+ wsPLen = WS_PAYLOAD_EXTEND_1;
+ payloadOffset = WS_HEADER_SIZE + WS_LEN_SIZE_1;
+ u16 *len = (u16*)&frame[2];
+ *len = htons((u16)pSize);
+ }
+ else {
+ wsPLen = WS_PAYLOAD_EXTEND_2;
+ payloadOffset = WS_HEADER_SIZE + WS_LEN_SIZE_2;
+ u64 *len = (u64*)&frame[2];
+ *len = htobe64(pSize);
+
+ }
+ frame[1] = wsPLen;
+
+ strncpy(frame+payloadOffset, payload, pSize);
+
+ return true;
+}
+
+#define genControlFrame(SIZE_FUNC) \
+ ssize_t frameSize = SIZE_FUNC(op, 0);\
+ zeroBuf(frame, frameSize);\
+ frame[0] = WS_FINAL_FRAME | op
+
+bool wsControlMask(char *frame, wsOpt op) {
+
+ if (!isCtlFrame(op)) return false;
+ genControlFrame(wsMaskSize);
+ frame[1] = WS_MASK;
+
+ return true;
+}
+
+bool wsControlNoMask(char *frame, wsOpt op) {
+
+ if (!isCtlFrame(op)) return false;
+ genControlFrame(wsNoMaskSize);
+
+ return true;
+}
+
+
+size_t wsDecodeSize(char *frame) {
+ u8 len = frame[1] & WS_LEN_MASK;
+ switch(len) {
+ case WS_PAYLOAD_EXTEND_1:;
+ u16 len1 = *(u16*)&frame[2];
+ return ntohs(len1);
+ case WS_PAYLOAD_EXTEND_2:;
+ u64 len2 = *(u64*)&frame[2];
+ return be64toh(len2);
+ default:
+ return len;
+ }
+}
+
+void wsDecode(char *data, size_t size, char *frame, size_t fSize) {
+
+ size_t sz = MIN3(wsDecodeSize(frame), size, fSize);
+ char *payload = wsDecodePayOffset(frame);
+ strncpy(data, payload, sz);
+
+ if (wsIsMasked(frame)) {
+ char *mask = payload -4;
+ range(i, sz) {
+ data[i] ^= *(mask + (i % 4));
+ }
+ }
+}
+
+char *wsDecodePayOffset(char *frame) {
+ char *r;
+ r = frame + WS_HEADER_SIZE;
+ u8 len = frame[1] & WS_LEN_MASK;
+ if (len == WS_PAYLOAD_EXTEND_1) {
+ r += WS_LEN_SIZE_1;
+ }
+ else if (len == WS_PAYLOAD_EXTEND_2) {
+ r += WS_LEN_SIZE_2;
+ }
+ if (wsIsMasked(frame)) r += WS_MASK_SIZE;
+ return r;
+}
+
+char *wsDecodeInPlace(char *frame, size_t size) {
+
+ char *r = wsDecodePayOffset(frame);
+ if (wsIsMasked(frame)) {
+ char *mask = r -4;
+ size_t sz = MIN(wsDecodeSize(frame), size);
+ range(i, sz) {
+ *(r+i) ^= *(mask + (i % 4));
+ }
+ }
+ return r;
+}
+
+bool wsIsFinal(char *frame) {
+ return (frame[0] & WS_FINAL_FRAME) == WS_FINAL_FRAME;
+}
+
+bool wsIsMasked(char *frame) {
+ return (frame[1] & WS_MASK) == WS_MASK;
+}
+
+wsOpt wsDecodeOp(char *frame) {
+ return (wsOpt) frame[0] & WS_OP_MASK;
+}
diff --git a/ws.h b/ws.h
@@ -0,0 +1,37 @@
+#pragma once
+#include <stdbool.h> // bool
+#include <sys/types.h> // size_t and ssize_t
+
+typedef enum {
+ WS_OP_CONT = 0x00,
+ WS_OP_TEXT = 0x01,
+ WS_OP_BINARY = 0x02,
+ WS_OP_CLOSE = 0x08,
+ WS_OP_PING = 0x09,
+ WS_OP_PONG = 0x0a
+} wsOpt;
+
+#define WS_FINAL true
+#define WS_NOT_FINAL false
+
+size_t wsHandshakeSize (char *hostname);
+int wsHandshake (char *frame, size_t size, char *hostname);
+bool wsHanskakeCheck (char *frame, size_t size);
+
+ssize_t wsMaskSize (wsOpt op, size_t pSize);
+ssize_t wsNoMaskSize (wsOpt op, size_t pSize);
+ssize_t wsControlMaskSize (void);
+ssize_t wsControlNoMaskSize(void);
+
+bool wsMask (char *frame, size_t size, wsOpt op, char *payload, size_t pSize, bool final);
+bool wsNoMask (char *frame, size_t size, wsOpt op, char *payload, size_t pSize, bool final);
+bool wsControlMask (char *frame, wsOpt op);
+bool wsControlNoMask (char *frame, wsOpt op);
+
+size_t wsDecodeSize (char *frame);
+void wsDecode (char *data, size_t size, char *frame, size_t fSize);
+char *wsDecodePayOffset (char *frame);
+char *wsDecodeInPlace (char *frame, size_t size);
+bool wsIsFinal (char *frame);
+bool wsIsMasked (char *frame);
+wsOpt wsDecodeOp (char *frame);
