send keys over WebSocket

CMakeLists.txt

@@ -33,6 +33,7 @@ target_sources(${PROJECT} PRIVATE
 	usb_descriptors.c
 	parse_keys.c
 	dhcpserver.c
+	websocket.c
 )
 
 pico_enable_stdio_usb(${PROJECT} 1)
@@ -47,6 +48,7 @@ target_link_libraries(${PROJECT}
 	pico_lwip_http
 	pico_stdlib
 	pico_multicore
+	pico_mbedtls
 	tinyusb_device
 )
 

html/index.html

@@ -48,12 +48,6 @@ div.blank {
 	opacity: 0;
 }
 
-div.info div {
-	font-size: 1.5em;
-	display: inline-block;
-	margin: 1em;
-}
-
 div#macros div {
 	width: 9em;
 	display: inline-block;
@@ -89,9 +83,8 @@ div.menu a:hover {
 
 <script>
 var keys = [];
-var fetchqueue = [];
-var curLed = 0;
-var sendKeyInterval = null;
+var mouse = [];
+var sendMouseInterval = null;
 const indicator_list = [
 	{id: "num", label: "NumLock"},
 	{id: "caps", label: "CapsLock"},
@@ -241,23 +234,25 @@ const mouse_list = [
 		{id: "", label: ""}
 	],
 	[
-		{id: "", label: "", width: 3}
-	],
-	[
-		{id: "MouseClickLeftt", label: "Left Click", width: 1.5, height: 0.75},
-		{id: "MouseClickRight", label: "Right Click", width: 1.5, height: 0.75}
+		{id: "MouseClickLeftt", label: "Left Click", width: 1.5},
+		{id: "MouseClickRight", label: "Right Click", width: 1.5}
 	],
 ];
 
-window.setInterval("refreshIndicators();",2000);
-
-window.addEventListener("keydown", onKeyDown, true);
-window.addEventListener("keyup", onKeyUp, true);
 
 window.onload = (event) => {
 	createMacros("macros");
 	createKeys("keyboard", keyboard_list);
 	createKeys("mouse", mouse_list);
+	createIndicators("indicators");
+
+	window.addEventListener("keydown", onKeyDown, true);
+	window.addEventListener("keyup", onKeyUp, true);
+
+	socket = new WebSocket("ws://" + window.location.hostname + ":8080/");
+	//socket.onopen = function (event) { document.getElementById("websocket").innerHTML="connected"; };
+	//socket.onclose = function (event) { document.getElementById("websocket").innerHTML="closed"; };
+	socket.onmessage = function (event) { updateLEDs(event.data); };
 }
 
 function onKeyDown(event) {
@@ -272,13 +267,18 @@ function onKeyDown(event) {
 }
 
 function pressKey(code, repeat=false) {
+	if (code.indexOf("Mouse") >= 0) {
+		if (mouse.includes(code)) { return; }
+		mouse.push(code);
+		sendKeys(mouse, true);
+	} else{
 		if (keys.includes(code)) { return; }
-
 		keys.push(code);
-
 		sendKeys(keys);
-	if ( sendKeyInterval == null && repeat) {
-		sendKeyInterval = setInterval( function() { sendKeys(keys); }, 200 );
+	}
+
+	if ( sendMouseInterval == null && repeat) {
+		sendMouseInterval = setInterval( function() { sendKeys(mouse, true); }, 50 );
 	}
 }
 
@@ -294,25 +294,26 @@ function onKeyUp(event) {
 }
 
 function releaseKey(code, repeat=false) {
+	if (code.indexOf("Mouse") >= 0) {
+		if (mouse.includes(code)) {
+			mouse.splice(mouse.indexOf(code),1);
+		}
+		sendKeys(mouse, true);
+	} else{ 
 		if (keys.includes(code)) {
 			keys.splice(keys.indexOf(code),1);
 		}
-
 		sendKeys(keys);
+	}
 
-	if (sendKeyInterval != null && repeat) {
-		clearInterval(sendKeyInterval);
-		sendKeyInterval = null;
+	if (sendMouseInterval != null && repeat) {
+		clearInterval(sendMouseInterval);
+		sendMouseInterval = null;
 	}
 }
 
-
-function sendKeys(curKeys) {
-	const url="sendkeys.cgi?keys=".concat(curKeys);
-	fetchqueue.push(url);
-	//console.log(fetchqueue);
-	document.getElementById("downKeys")
-		.innerHTML = curKeys;
+function sendKeys(curKeys, is_mouse) {
+	// update highlighting of pressed keys
 	prev_keys = document.getElementsByClassName("pressed");
 	for (let key of curKeys) {
 		keyDiv = document.getElementById(key);
@@ -323,29 +324,15 @@ function sendKeys(curKeys) {
 			key.classList.remove("pressed");
 		}
 	}
-	if (fetchqueue.length == 1) {
-		runQueue();
-	}
-}
 
-async function runQueue() {
-	const url = fetchqueue[0];
-	await fetch(url);
-	fetchqueue.shift();
-	if (fetchqueue.length > 0) {
-		runQueue();
+	// send updated key or mouse status
+	if (socket.readyState == WebSocket.OPEN) {
+		if (is_mouse == true) {
+			socket.send("M: " + curKeys);
+		} else {
+			socket.send("K: " + curKeys);
 		}
 	}
-
-function updateIndicators() {
-	let indicatorFrame = document.getElementById("indicatorFrame");
-	let indicatorDiv = document.getElementById("indicators");
-	indicatorDiv.innerHTML = indicatorFrame.contentDocument.body.innerHTML;
-}
-
-function refreshIndicators() {
-	let indicatorFrame = document.getElementById("indicatorFrame");
-	indicatorFrame.contentWindow.location.reload(true);
 }
 
 function createKeys(div_id, key_list) {
@@ -410,6 +397,29 @@ function createMacros(macro_id) {
 	}
 }
 
+function createIndicators(indicator_id) {
+	let indicatorDiv = document.getElementById(indicator_id);
+	for (let indicator of indicator_list) {
+		let newDiv = document.createElement("div");
+		newDiv.id = indicator.id;
+		newDiv.textContent = indicator.label;
+		newDiv.className = "off";
+		indicatorDiv.appendChild(newDiv);
+	}
+}
+
+function updateLEDs(cur_state) {
+	let on_leds = cur_state.split(",");
+	for (let indicator of indicator_list) {
+		let indicator_div = document.getElementById(indicator.id);
+		if (on_leds.includes(indicator.id)) {
+			indicator_div.className = "on";
+		} else {
+			indicator_div.className = "off";
+		}
+	}
+}
+
 </script>
 
 </head>
@@ -419,17 +429,13 @@ function createMacros(macro_id) {
 	<div id="keyboard"></div>
 	<div>
 		<div id="indicators"></div>
+		<div>Mouse Controls:</div>
 		<div id="mouse"></div>
 	</div>
 </div>
 
 <div id="macros"></div>
 
-<div class="info">
-	<div>Currently pressed keys:</div>
-	<div id="downKeys"></div>
-</div>
-
 <div>
 	<div class="menu">
 		<a href="wifi.shtml">Configure Wi-Fi</a>
@@ -439,11 +445,6 @@ function createMacros(macro_id) {
 	</div>
 </div>
 
-<div id="status_frames">
-	<iframe id="indicatorFrame" src="indicators.shtml"
-		onload="updateIndicators();"></iframe>
-</div>
-
 </body>
 
 </html>

mbedtls_config.h

@@ -0,0 +1,74 @@
+#ifndef MBEDTLS_CONFIG_EXAMPLES_COMMON_H
+#define MBEDTLS_CONFIG_EXAMPLES_COMMON_H
+
+/* Workaround for some mbedtls source files using INT_MAX without including limits.h */
+#include <limits.h>
+
+#define MBEDTLS_NO_PLATFORM_ENTROPY
+#define MBEDTLS_ENTROPY_HARDWARE_ALT
+
+#define MBEDTLS_SSL_OUT_CONTENT_LEN    2048
+
+#define MBEDTLS_ALLOW_PRIVATE_ACCESS
+#define MBEDTLS_HAVE_TIME
+
+#define MBEDTLS_CIPHER_MODE_CBC
+#define MBEDTLS_ECP_DP_SECP192R1_ENABLED
+#define MBEDTLS_ECP_DP_SECP224R1_ENABLED
+#define MBEDTLS_ECP_DP_SECP256R1_ENABLED
+#define MBEDTLS_ECP_DP_SECP384R1_ENABLED
+#define MBEDTLS_ECP_DP_SECP521R1_ENABLED
+#define MBEDTLS_ECP_DP_SECP192K1_ENABLED
+#define MBEDTLS_ECP_DP_SECP224K1_ENABLED
+#define MBEDTLS_ECP_DP_SECP256K1_ENABLED
+#define MBEDTLS_ECP_DP_BP256R1_ENABLED
+#define MBEDTLS_ECP_DP_BP384R1_ENABLED
+#define MBEDTLS_ECP_DP_BP512R1_ENABLED
+#define MBEDTLS_ECP_DP_CURVE25519_ENABLED
+#define MBEDTLS_KEY_EXCHANGE_RSA_ENABLED
+#define MBEDTLS_PKCS1_V15
+#define MBEDTLS_SHA256_SMALLER
+#define MBEDTLS_SSL_SERVER_NAME_INDICATION
+#define MBEDTLS_AES_C
+#define MBEDTLS_ASN1_PARSE_C
+#define MBEDTLS_BIGNUM_C
+#define MBEDTLS_CIPHER_C
+#define MBEDTLS_CTR_DRBG_C
+#define MBEDTLS_ENTROPY_C
+#define MBEDTLS_ERROR_C
+#define MBEDTLS_MD_C
+#define MBEDTLS_MD5_C
+#define MBEDTLS_OID_C
+#define MBEDTLS_PKCS5_C
+#define MBEDTLS_PK_C
+#define MBEDTLS_PK_PARSE_C
+#define MBEDTLS_PLATFORM_C
+#define MBEDTLS_RSA_C
+#define MBEDTLS_SHA1_C
+#define MBEDTLS_SHA224_C
+#define MBEDTLS_SHA256_C
+#define MBEDTLS_SHA512_C
+#define MBEDTLS_SSL_CLI_C
+#define MBEDTLS_SSL_SRV_C
+#define MBEDTLS_SSL_TLS_C
+#define MBEDTLS_X509_CRT_PARSE_C
+#define MBEDTLS_X509_USE_C
+#define MBEDTLS_AES_FEWER_TABLES
+
+/* TLS 1.2 */
+#define MBEDTLS_SSL_PROTO_TLS1_2
+#define MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA_ENABLED
+#define MBEDTLS_GCM_C
+#define MBEDTLS_ECDH_C
+#define MBEDTLS_ECP_C
+#define MBEDTLS_ECDSA_C
+#define MBEDTLS_ASN1_WRITE_C
+
+// The following is needed to parse a certificate
+#define MBEDTLS_PEM_PARSE_C
+#define MBEDTLS_BASE64_C
+
+// The following significantly speeds up mbedtls due to NIST optimizations.
+#define MBEDTLS_ECP_NIST_OPTIM
+
+#endif

parse_keys.c

@@ -13,9 +13,7 @@ void parse_key_list(char * keys) {
 	uint8_t keypos = 2; // bytes 2-7 are used for normal keys
 	uint8_t code = 0x00;
 	static unsigned char boot_report[8];
-	static unsigned char mouse_report[3];
 	memset(boot_report, 0x00, 8);
-	memset(mouse_report, 0x00, 3);
 
 	// Javascript sends the list as comma delimited, so split into individual
 	// keys by splitting at commas
@@ -29,41 +27,18 @@ void parse_key_list(char * keys) {
 			keypos++;
 		}
 
-		// if a scan code was not returned, it might be a modifier or mouse
+		// if a scan code was not returned, it might be a modifier
 		if (code == 0x00) {
 			// search for the correct modifier bit and add it to byte 0 of the
 			// USB boot keyboard report
 			code = parse_mod(token);
 			if (code) {
 				boot_report[0] = boot_report[0] | code;
-			} else if ( strncmp(&token[0],"Mouse", 5) == 0 ){
-				if ( strncmp(&token[5], "Left", 4) == 0 ) {
-					mouse_report[1] = -1*MOUSE_SPEED;
-				} else if ( strncmp(&token[5], "Right", 5) == 0 ) {
-					mouse_report[1] = 1*MOUSE_SPEED;
-				} else if ( strncmp(&token[5], "Up", 2) == 0 ) {
-					mouse_report[2] = -1*MOUSE_SPEED;
-				} else if ( strncmp(&token[5], "Down", 4) == 0 ) {
-					mouse_report[2] = 1*MOUSE_SPEED;
-				} else if ( strncmp(&token[5], "ClickLeft", 9) == 0 ) {
-					mouse_report[0] = mouse_report[0] | 1;
-				} else if ( strncmp(&token[5], "ClickRight", 10) == 0) {
-					mouse_report[0] = mouse_report[0] | 2;
-				}
 			}
 		}
 		token = strtok(NULL, ",");
 	}
 
-
-	// print resulting HID boot mouse report to CDC for debugging
-	printf("Mouse report: ");
-	for(int i=0; i<3; i++){
-		printf("%02X ",mouse_report[i]);
-	}
-	printf("\n");
-	tud_hid_report(REPORT_ID_MOUSE, mouse_report, 3);
-		
 	// print resulting HID boot keyboard report to CDC for debugging
 	printf("Keyboard report: ");
 	for(int i=0; i<8; i++){
@@ -74,6 +49,38 @@ void parse_key_list(char * keys) {
 	tud_hid_report(REPORT_ID_KEYBOARD, boot_report, 8);
 }
 
+void parse_mouse_list (char * keys) {
+	static unsigned char mouse_report[3];
+	memset(mouse_report, 0x00, 3);
+
+	// Javascript sends the list as comma delimited, so split into individual
+	// keys by splitting at commas
+	char * token = strtok(keys, ",");
+	while (token != NULL) {
+		if ( strncmp(&token[0],"MouseLeft", 9) == 0 ){
+			mouse_report[1] = -1*MOUSE_SPEED;
+		} else if ( strncmp(&token[0], "MouseRight", 10) == 0 ) {
+			mouse_report[1] = 1*MOUSE_SPEED;
+		} else if ( strncmp(&token[0], "MouseUp", 7) == 0 ) {
+			mouse_report[2] = -1*MOUSE_SPEED;
+		} else if ( strncmp(&token[0], "MouseDown", 9) == 0 ) {
+			mouse_report[2] = 1*MOUSE_SPEED;
+		} else if ( strncmp(&token[0], "MouseClickLeft", 14) == 0 ) {
+			mouse_report[0] = mouse_report[0] | 1;
+		} else if ( strncmp(&token[0], "MouseClickRight", 15) == 0) {
+			mouse_report[0] = mouse_report[0] | 2;
+		}
+		token = strtok(NULL, ",");
+	}
+
+	// print resulting HID boot mouse report to CDC for debugging
+	printf("Mouse report: ");
+	for(int i=0; i<3; i++){
+		printf("%02X ",mouse_report[i]);
+	}
+	printf("\n");
+	tud_hid_report(REPORT_ID_MOUSE, mouse_report, 3);
+}
 
 // take a single Javascript key code and convert it to USB HID scancode
 // if it is a regular key, i.e. not a modifier key

parse_keys.h

@@ -6,6 +6,7 @@
 static unsigned char boot_buf[8];
 
 void parse_key_list(char * keys);
+void parse_mouse_list(char * keys);
 uint8_t parse_key_single(char * key);
 uint8_t parse_mod(char * key);
 

server.c

@@ -10,9 +10,11 @@
 #include "hardware/sync.h"
 #include "pico/multicore.h"
 
-#include "server.h"
 #include "parse_keys.h"
 #include "dhcpserver.h"
+#include "websocket.h"
+
+#include "server.h"
 
 uint8_t led_code = 0;
 static uint8_t ip[4];
@@ -29,9 +31,6 @@ static void *current_connection;
 
 // list of SSI variable names for lwIP SSI handler
 const char * __not_in_flash("httpd") ssi_tags[] = {
-	"num",
-	"caps",
-	"scroll",
 	"ssid",
 	"pass",
 	"host",
@@ -142,6 +141,12 @@ void run_http_server() {
 	http_set_ssi_handler(ssi_handler, ssi_tags, LWIP_ARRAYSIZE(ssi_tags));
 	printf("HTTP server initialized\n");
 
+	// start the websocket server
+	ws_server_init();
+	ws_set_open_handler(ws_open_handler);
+	ws_set_receive_handler(ws_receive_handler);
+	printf("Websocket server initialized\n");
+
 	// start a watchdog timer with 8 seconds so it can reboot if it disconnects
 	watchdog_enable(8000,1);
 
@@ -242,79 +247,58 @@ bool net_config_load(net_config *config) {
 uint16_t __time_critical_func(ssi_handler)(int iIndex, char *pcInsert, int iInsertLen) {
 	size_t printed;
 	switch (iIndex) {
-		case 0: //num - numlock status
-			if (led_code & 1) {
-				printed = snprintf(pcInsert, iInsertLen, "on");
-			} else {
-				printed = snprintf(pcInsert, iInsertLen, "off");
-			}
-			break;
-		case 1: //caps - capslock status
-			if (led_code & 2) {
-				printed = snprintf(pcInsert, iInsertLen, "on");
-			} else {
-				printed = snprintf(pcInsert, iInsertLen, "off");
-			}
-			break;
-		case 2: //scroll - scrollock status
-			if (led_code & 4) {
-				printed = snprintf(pcInsert, iInsertLen, "on");
-			} else {
-				printed = snprintf(pcInsert, iInsertLen, "off");
-			}
-			break;
-		case 3: //ssid - network config SSID
+		case 0: //ssid - network config SSID
 			if (config_loaded && wifi.ssid) {
 				printed = snprintf(pcInsert, iInsertLen, "value=\"%s\"", wifi.ssid);
 			} else {
 				printed = 0;
 			}
 			break;
-		case 4: //pass - network config password
+		case 1: //pass - network config password
 			if (config_loaded && wifi.pass) {
 				printed = snprintf(pcInsert, iInsertLen, "value=\"%s\"", wifi.pass);
 			} else {
 				printed = 0;
 			}
 			break;
-		case 5: //host - network config hostname
+		case 2: //host - network config hostname
 			if (config_loaded && wifi.host) {
 				printed = snprintf(pcInsert, iInsertLen, "value=\"%s\"", wifi.host);
 			} else {
 				printed = 0;
 			}
 			break;
-		case 6:
+		case 3:
+		case 4:
+		case 5:
+		case 6: //ip0-3 - network config manual IP address parts
+			if (config_loaded && wifi.ip.addr) {
+				printed = snprintf(pcInsert, iInsertLen, "value=\"%d\"", ip4_addr_get_byte(&(wifi.ip), iIndex-3));
+			} else {
+				printed = 0;
+			}
+			break;
 		case 7:
 		case 8:
-		case 9: //ip0-3 - network config manual IP address parts
-			if (config_loaded && wifi.ip.addr) {
-				printed = snprintf(pcInsert, iInsertLen, "value=\"%d\"", ip4_addr_get_byte(&(wifi.ip), iIndex-6));
+		case 9:
+		case 10: //mask0-3 - network config manual netmask parts
+			if (config_loaded && wifi.mask.addr) {
+				printed = snprintf(pcInsert, iInsertLen, "value=\"%d\"", ip4_addr_get_byte(&(wifi.mask), iIndex-7));
 			} else {
 				printed = 0;
 			}
 			break;
-		case 10:
 		case 11:
 		case 12:
-		case 13: //mask0-3 - network config manual netmask parts
-			if (config_loaded && wifi.mask.addr) {
-				printed = snprintf(pcInsert, iInsertLen, "value=\"%d\"", ip4_addr_get_byte(&(wifi.mask), iIndex-10));
-			} else {
-				printed = 0;
-			}
-			break;
-		case 14:
-		case 15:
-		case 16:
-		case 17: //gw0-3 - network config manual gateway parts
+		case 13:
+		case 14: //gw0-3 - network config manual gateway parts
 			if (config_loaded && wifi.gw.addr) {
-				printed = snprintf(pcInsert, iInsertLen, "value=\"%d\"", ip4_addr_get_byte(&(wifi.gw), iIndex-14));
+				printed = snprintf(pcInsert, iInsertLen, "value=\"%d\"", ip4_addr_get_byte(&(wifi.gw), iIndex-11));
 			} else {
 				printed = 0;
 			}
 			break;
-		case 18: // dhcp - network config DHCP client enabled/disabled
+		case 15: // dhcp - network config DHCP client enabled/disabled
 			if ((!config_loaded) || !(wifi.manual) ){
 				printed = snprintf(pcInsert, iInsertLen, "checked");
 			} else {
@@ -448,7 +432,7 @@ void httpd_post_finished(void *connection, char *response_uri, u16_t response_ur
 	current_connection = NULL;
 }
 
-// Return a value for a parameter from POST
+// return a value for a parameter from POST
 char *find_post_param(struct pbuf *p, const char *param, char *buf, size_t len) {
 	size_t param_len = strlen(param);
 	uint16_t param_pos = pbuf_memfind(p, param, param_len, 0);
@@ -474,9 +458,40 @@ char *find_post_param(struct pbuf *p, const char *param, char *buf, size_t len)
 	return NULL;
 }
 
+const void ws_open_handler(struct ws_state * wss) {
+	send_indicators();
+}
+
+// handler for data received on websocket connection
+const void ws_receive_handler(uint8_t *data, uint16_t len) {
+	if (strncmp(data, "K: ", 3)==0) {
+		parse_key_list(&data[3]);
+	} else if (strncmp(data, "M: ", 3) == 0) {
+		parse_mouse_list(&data[3]);
+	}
+}
+
 // save keyboard's LED indicator status to memory
 void set_indicator(uint8_t const* buffer) {
 	led_code = *buffer;
+
+	send_indicators();
+}
+
+// read individual bits for indicator LED status and send over websocket to client
+void send_indicators(void){
+	char buf[16]="";
+	if (led_code & 1) {
+		strcat(buf, "num,");
+	}
+	if (led_code & 2) {
+		strcat(buf, "caps,");
+	}
+	if (led_code & 4) {
+		strcat(buf, "scroll");
+	}
+
+	ws_send_all(buf, strlen(buf));
 }
 
 // take URL-formatted string from GET request and turn into regular string

server.h

@@ -2,6 +2,7 @@
 #define SERVER_H_
 
 #include "lwip/ip4_addr.h"
+#include "lwip/pbuf.h"
 
 #define STARTFILE	0x7fc6
 #define ENDFILE		0x0464
@@ -26,10 +27,13 @@ const char * sendkeys_cgi(int iIndex, int iNumParams, char *pcParam[], char *pcV
 const char * reboot_cgi(int iIndex, int iNumParams, char *pcParam[], char *pcValue[]);
 uint16_t __time_critical_func(ssi_handler)(int iIndex, char *pcInsert, int iInsertLen);
 void set_indicator(uint8_t const* buffer);
+void send_indicators(void);
 void urldecode(char *urlstring, char *dest);
 bool net_config_load(net_config *wifi);
 void net_config_write(net_config *wifi);
 char *find_post_param(struct pbuf *p, const char *param, char *buf, size_t len);
+const void ws_receive_handler(uint8_t *data, uint16_t len);
+const void ws_open_handler(struct ws_state * wss);
 
 #define FLASH_TARGET_OFFSET ((PICO_FLASH_SIZE_BYTES) - FLASH_SECTOR_SIZE)
 

usb_descriptors.c

@@ -128,7 +128,7 @@ char const* string_desc_arr [] =
   "Pico Web Keyboard",           // 2: Product
   "1234567890123456789",         // 3: Serials, should use chip ID
   "Pico Web Keyboard CDC",       // 4: CDC Interface
-  "TinyUSB Keyboard",            // 5: HID Keyboard Interface
+  "Pico Web Keyboard HID",       // 5: HID Keyboard Interface
 };
 
 static uint16_t _desc_str[32];

websocket.c

@@ -0,0 +1,374 @@
+#include <string.h>
+
+#include "lwip/altcp.h"
+#include "lwip/debug.h"
+#include "mbedtls/base64.h"
+#include "mbedtls/sha1.h"
+
+#include "websocket.h"
+
+static const char WS_GUID[] = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11";
+static const char WS_RESPONSE[] = "HTTP/1.1 101 Switching Protocols\r\n" \
+	"Upgrade: websocket\r\n" \
+	"Connection: Upgrade\r\n" \
+	"Sec-WebSocket-Accept: ";
+
+static tWSHandler ws_receive_cb = NULL;
+static tWSOpenHandler ws_open_cb = NULL;
+
+static struct ws_state * ws_connections;
+static uint8_t ws_num_conns = 0;
+
+// allocate memory for ws_state instance
+static struct ws_state * ws_state_alloc(void) {
+	struct ws_state *ret =  WS_ALLOC_WS_STATE();
+	
+	if ( ret != NULL) {
+		ws_state_init(ret);
+		if (ws_connections == NULL) {
+			ws_connections = ret;
+		} else {
+			struct ws_state *last;
+			for (last=ws_connections; last->next != NULL; last=last->next);
+			LWIP_ASSERT("last != NULL", last != NULL);
+			last->next = ret;
+		}
+	}
+
+	return ret;
+}
+
+// initiate ws_state instance
+static void ws_state_init(struct ws_state *wss) {
+	memset(wss, 0, sizeof(struct ws_state));
+	wss->active = false;
+}
+
+// free memory from ws_state instance
+static void ws_state_free(struct ws_state *wss) {
+	if (wss != NULL) {
+		if (ws_connections == wss) {
+			ws_connections = wss->next;
+		} else {
+			struct ws_state * last;
+			for (last = ws_connections; last->next != NULL; last = last->next) {
+				if (last->next = wss) {
+					last->next = wss->next;
+					break;
+				}
+			}
+		}
+		mem_free(wss);
+	}
+}
+
+// initiate websocket server on specified pcb
+static void ws_server_init_pcb( struct altcp_pcb *pcb, uint16_t port) {
+	err_t err;
+
+	if (pcb) {
+		altcp_setprio(pcb, TCP_PRIO_MIN);
+		err = altcp_bind(pcb, IP_ANY_TYPE, port);
+		LWIP_UNUSED_ARG(err);
+		LWIP_ASSERT("ws_server_init: tcp_bind failed", err == ERR_OK);
+		pcb = altcp_listen(pcb);
+		LWIP_ASSERT("ws_server_init: tcp_listen failed", pcb != NULL);
+		altcp_accept(pcb, ws_accept);
+	}
+}
+
+// initiate a websocket server
+void ws_server_init(void) {
+	struct altcp_pcb *pcb = altcp_tcp_new_ip_type(IPADDR_TYPE_ANY);
+	LWIP_ASSERT("ws_server_init: tcp_new failed", pcb != NULL);
+	ws_server_init_pcb(pcb, WS_PORT);
+}
+
+// set ws_receive_handler
+void ws_set_receive_handler( tWSHandler ws_handler)
+{
+	ws_receive_cb = ws_handler;
+}
+
+// set ws_open_handler
+void ws_set_open_handler( tWSOpenHandler ws_handler)
+{
+	ws_open_cb = ws_handler;
+}
+
+// callback for accepted websocket connection
+static err_t ws_accept(void *arg, struct altcp_pcb *pcb, err_t err) {
+	struct ws_state *wss;
+	LWIP_UNUSED_ARG(err);
+	LWIP_UNUSED_ARG(arg);
+	LWIP_DEBUGF(WS_DEBUG, ("ws_accept %p / %p\n", (void *)pcb, arg));
+
+	if ((err != ERR_OK) || (pcb == NULL)) {
+		return ERR_VAL;
+	}
+
+	// create new ws_state object
+	wss = ws_state_alloc();
+	if (wss == NULL) {
+		LWIP_DEBUGF(WS_DEBUG, ("ws_accept: Out of memory, RST\n"));
+		return ERR_MEM;
+	}
+	wss->pcb = pcb;
+
+	// make ws_state object the argument of callbacks
+	altcp_arg(pcb, wss);
+	
+	// register callbacks for tcp events
+	altcp_recv(pcb, ws_recv);
+	altcp_poll(pcb, ws_poll, WS_POLL_INTERVAL);
+	altcp_err(pcb, ws_err);
+
+	return ERR_OK;
+}
+
+// call when data is received
+static err_t ws_recv(void *arg, struct altcp_pcb *pcb, struct pbuf *p, err_t err) {
+	struct ws_state *wss = (struct ws_state *) arg;
+	
+	if ((err != ERR_OK) || (p == NULL) || (wss == NULL)) {
+		// error  or closed by client
+		if (p != NULL) {
+			// inform TCP that we have taken the data
+			altcp_recved(pcb, p->tot_len);
+			pbuf_free(p);
+		}
+		if (wss == NULL) {
+			// should not occur
+			LWIP_DEBUGF(WS_DEBUG, ("Error, ws_recv: wss is NULL, close\n"));
+		}
+		ws_close_conn(pcb, wss);
+		return ERR_OK;
+	}
+
+	if (wss->active) {
+		// process websocket message
+		err = ws_read(pcb, wss, p);
+	} else {
+		// init websocket connection
+		LWIP_DEBUGF(WS_DEBUG, ("ws_recv: websocket inactive, checking for handshake\n"));
+		
+		err = ws_handshake(pcb, wss, p);
+	}
+
+	// inform TCP that we have taken the data.
+	altcp_recved(pcb, p->tot_len);
+	pbuf_free(p);
+
+	if (err == ERR_CLSD) {
+		ws_close_conn(pcb, wss);
+	}
+
+	return ERR_OK;
+}
+
+static void ws_err (void *arg, err_t err) {
+	struct ws_state *wss = (struct ws_state *) arg;
+	LWIP_UNUSED_ARG(err);
+
+	LWIP_DEBUGF(WS_DEBUG, ("ws_err: %s", lwip_strerr(err)));
+
+	if (wss != NULL) {
+		ws_state_free(wss);
+	}
+}
+
+// initiate close of connection
+static err_t ws_close_conn(struct altcp_pcb *pcb, struct ws_state *wss) {
+	return ws_close_or_abort_conn(pcb, wss, 0);
+}
+
+// call when closing connection or connection was aborted
+static err_t ws_close_or_abort_conn(struct altcp_pcb *pcb, struct ws_state *wss,
+	uint8_t abort_conn) {
+
+	err_t err;
+	LWIP_DEBUGF(WS_DEBUG, ("Closing connection %p\n", (void *)pcb));
+
+	// clear callbacks
+	altcp_arg(pcb, NULL);
+	altcp_recv(pcb, NULL);
+	altcp_poll(pcb, NULL, 0);
+	altcp_err(pcb, NULL);
+
+	// remove and free memory from ws_state object
+	if (wss != NULL) {
+		ws_state_free(wss);
+	}
+
+	if (abort_conn) {
+		altcp_abort(pcb);
+		return ERR_OK;
+	}
+	err = altcp_close(pcb);
+	if (err != ERR_OK) {
+		LWIP_DEBUGF(WS_DEBUG, ("Error %d closing %p\n", err, (void *)pcb));
+		// error closing, try again later in poll
+		altcp_poll(pcb, ws_poll, WS_POLL_INTERVAL);
+	}
+	return err;
+}
+
+// callback for polling process
+static err_t ws_poll(void *arg, struct altcp_pcb *pcb) {
+	struct ws_state *wss = (struct ws_state *) arg;
+	if (wss == NULL) {
+		err_t closed;
+		LWIP_DEBUGF(WS_DEBUG, ("ws_poll: arg is NULL, close\n"));
+		closed = ws_close_conn(pcb, NULL);
+		LWIP_UNUSED_ARG(closed);
+		if (closed == ERR_MEM) {
+			altcp_abort(pcb);
+			return ERR_ABRT;
+		}
+		return ERR_OK;
+	} else {
+		wss->retries++;
+		if (wss->retries == WS_MAX_RETRIES) {
+			LWIP_DEBUGF(WS_DEBUG, ("ws_poll: too may retries, close\n"));
+			ws_close_conn(pcb, wss);
+			return ERR_OK;
+		}
+	}
+
+	return ERR_OK;
+}
+
+// check for and complete handshake with client
+static err_t ws_handshake(struct altcp_pcb *pcb, struct ws_state *wss, struct pbuf *p){
+	uint8_t *data = (uint8_t *) p->payload;
+	uint16_t len = p->len;
+
+	// check if client is initiating a websocket connecttion
+	if (strstr(data, "Upgrade: websocket")) {
+		LWIP_DEBUGF(WS_DEBUG, ("ws_handshake: received websocket upgrade request\n"));
+
+		// search for websocket security key
+		char *key_start = strstr(data, "Sec-WebSocket-Key: ");
+
+		if (key_start) {
+			key_start += 19;
+			const char *key_end = strstr(key_start, "\r\n");
+			if (key_end) {
+				char key[64];
+				uint16_t key_len = key_end-key_start;
+				if ( (key_len>0) && (key_len + sizeof(WS_GUID) < sizeof(key)) ) {
+					// create response key by concatenating with websocket GUID,
+					// taking SHA1 hash, then encoding in base 64
+					strncpy(key, key_start, key_len);
+					strlcpy(&key[key_len], WS_GUID, sizeof(key)-key_len);
+
+					key_len += sizeof(WS_GUID)-1;
+					unsigned char key_sha1[20];
+					unsigned char key_base64[29];
+					size_t encoded_len;
+					mbedtls_sha1( (unsigned char *) key, key_len, key_sha1);
+					mbedtls_base64_encode( key_base64, 29, &encoded_len, key_sha1, 20);
+
+					// create response packet with encoded response key
+					unsigned char response[sizeof(WS_RESPONSE) + sizeof(key_base64)+3];
+					//strncpy(response, WS_RESPONSE, sizeof(WS_RESPONSE));
+					//strlcpy(&response[sizeof(WS_RESPONSE)-1], key_base64, strlen(key_base64));
+					size_t count = sprintf(response, "%s%s\r\n\r\n", WS_RESPONSE, key_base64);
+
+					// send completed data packet
+					LWIP_DEBUGF(WS_DEBUG, ("ws_handshake: sending response\n"));
+					if(altcp_write(pcb, response, strlen(response), TCP_WRITE_FLAG_COPY) == ERR_OK) {
+						wss->active = true;
+					}
+
+					if (ws_open_cb != NULL) {
+						ws_open_cb(wss);
+					}
+
+					return ERR_OK;
+				}
+			}
+
+			LWIP_DEBUGF(WS_DEBUG, ("ws_handshake: key overflow\n"));
+			return ERR_MEM;
+		} else {
+			LWIP_DEBUGF(WS_DEBUG, ("ws_handshake: key not received\n"));
+			return ERR_ARG;
+		}
+	}
+
+	LWIP_DEBUGF(WS_DEBUG, ("ws_handshake: not a websocket request\n"));
+	return ERR_ARG;
+}
+
+// handle reading of websocket data and pass to ws_receive_cb
+static err_t ws_read(struct altcp_pcb *pcb, struct ws_state *wss, struct pbuf *p) {
+	uint8_t *data = (uint8_t *) p->payload;
+	uint16_t len = p->len;
+
+	if (data != NULL && len > 1) {
+		// successful read, reset timeout
+		wss->retries = 0;
+
+		uint8_t mode = data[0] & 0x0F;
+		uint16_t msg_len = data[1] & 0x7F;
+		switch (mode) {
+			case 0x01: //text
+				LWIP_DEBUGF(WS_DEBUG, ("ws_read: received text data\n"));
+			case 0x02: //binary
+				LWIP_DEBUGF(WS_DEBUG, ("ws_read: decoding data\n"));
+				if (len >= 6 && ws_receive_cb != NULL) {
+					uint8_t *mask = &data[2];
+					uint8_t *msg = &data[6];
+
+					for (int i=0; i<msg_len; i++) {
+						msg[i] ^= mask[i % 4];
+					}
+					msg[msg_len]=0;
+
+					ws_receive_cb(msg, msg_len);
+				}
+				break;
+			case 0x08: //close
+				LWIP_DEBUGF(WS_DEBUG, ("ws_read: close request"));
+				return ERR_CLSD;
+			default:
+				LWIP_DEBUGF(WS_DEBUG, ("ws_read: invalid data mode %02X\n", mode));
+				return ERR_ARG;
+		}
+
+		return ERR_OK;
+	}
+	LWIP_DEBUGF(WS_DEBUG, ("ws_read: received empty payload\n"));
+	return ERR_VAL;
+}
+
+static err_t ws_send(struct ws_state *wss, uint8_t *data, uint16_t len) {
+	uint8_t buf[128];
+	buf[0] = 0x81;
+	buf[1] = len & 0x7F;
+	memcpy(&buf[2], data, len);
+	
+	err_t err;
+	err = altcp_write(wss->pcb, buf, len+2, TCP_WRITE_FLAG_COPY);
+	if (err == ERR_OK) {
+		altcp_output(wss->pcb);
+		wss->retries = 0;
+	}
+
+	return err;
+}
+
+void ws_send_all(uint8_t *data, uint16_t len) {
+	// send message to all connections
+	if (ws_connections != NULL) {
+		struct ws_state *wss;
+		err_t err;
+		for (wss=ws_connections; wss != NULL; wss=wss->next) {
+			err = ws_send(wss, data, len);
+			if (err != ERR_OK ) {
+				LWIP_DEBUGF(WS_DEBUG, ("ws_send_all: error sending to %p\n", wss));
+			}
+		}
+	}
+}

websocket.h

@@ -0,0 +1,41 @@
+#ifndef WEBSOCKET_H_
+#define WEBSOCKET_H_
+
+#define WS_PORT				8080
+#define WS_TIMEOUT			10
+#define WS_DEBUG			LWIP_DBG_ON
+#define WS_MAX_RETRIES		10
+#define WS_POLL_INTERVAL	60 // WS_POLL_INTERVAL/2 seconds
+#define WS_MAX_CONN			4
+
+struct ws_state {
+	bool active;
+	uint8_t retries;
+	struct altcp_pcb *pcb;
+	struct ws_state *next;
+};
+
+#define WS_ALLOC_WS_STATE() (struct ws_state *)mem_malloc(sizeof(struct ws_state))
+
+typedef void (* tWSHandler ) (uint8_t *data, uint16_t len);
+typedef void (* tWSOpenHandler ) (struct ws_state * wss);
+
+static struct ws_state* ws_state_alloc(void);
+static void ws_state_init(struct ws_state *wss);
+static void ws_state_free(struct ws_state *wss);
+void ws_server_init(void);
+static void ws_server_init_pcb( struct altcp_pcb *pcb, uint16_t port);
+static err_t ws_accept(void *arg, struct altcp_pcb *pcb, err_t err);
+static err_t ws_recv(void *arg, struct altcp_pcb *pcb, struct pbuf *p, err_t err);
+static void ws_err (void *arg, err_t err);
+static err_t ws_close_conn(struct altcp_pcb *pcb, struct ws_state *wss);
+static err_t ws_close_or_abort_conn(struct altcp_pcb *pcb, struct ws_state *wss, uint8_t abort_conn);
+static err_t ws_poll(void *arg, struct altcp_pcb *pcb);
+static err_t ws_handshake(struct altcp_pcb *pcb, struct ws_state *wss, struct pbuf *p);
+static err_t ws_read(struct altcp_pcb *pcb, struct ws_state *wss, struct pbuf *p);
+static err_t ws_send(struct ws_state *wss, uint8_t *data, uint16_t len);
+void ws_send_all(uint8_t *data, uint16_t len);
+void ws_set_receive_handler( tWSHandler ws_handler);
+void ws_set_open_handler( tWSOpenHandler ws_handler);
+
+#endif