diff --git a/websocket.c b/websocket.c
index 9df358a..54deee0 100644
--- a/websocket.c
+++ b/websocket.c
@@ -354,12 +354,14 @@ static err_t ws_read(struct altcp_pcb *pcb, struct ws_state *wss, struct pbuf *p
 		uint8_t masked = data[1] & 0x80;
 		uint16_t msg_len = data[1] & 0x7F;
 		uint8_t *msg;
-		
+		uint8_t *mask;
+
 		switch (msg_len) {
 			case 126: // next two bytes are length
-				memcpy(&msg_len, &data[2], 2);
+				msg_len = ( (uint16_t)data[2] << 8) | data[3];
 				if (len >= 8) {
 					msg = &data[8];
+					mask = &data[4];
 				}
 				break;
 			case 127: // next four bytes are length
@@ -369,6 +371,7 @@ static err_t ws_read(struct altcp_pcb *pcb, struct ws_state *wss, struct pbuf *p
 			default:
 				if (len >= 6) {
 					msg = &data[6];
+					mask = &data[2];
 				}
 				break;
 		}
@@ -382,8 +385,6 @@ static err_t ws_read(struct altcp_pcb *pcb, struct ws_state *wss, struct pbuf *p
 				if (msg && ws_receive_cb != NULL) {
 					// unmask the data if mask bit is received
 					if (masked) {
-						uint8_t *mask = &data[2];
-	
 						for (int i=0; i<msg_len; i++) {
 							msg[i] ^= mask[i % 4];
 						}
diff --git a/websocket.h b/websocket.h
index adeb8cc..3412223 100644
--- a/websocket.h
+++ b/websocket.h
@@ -7,7 +7,7 @@
 #define WS_MAX_RETRIES		10
 #define WS_POLL_INTERVAL	60 // WS_POLL_INTERVAL/2 seconds
 #define WS_MAX_CONN			4
-#define WS_BUFFER_SIZE		512
+#define WS_BUFFER_SIZE		1024
 
 #define OP_CONT		0x00
 #define OP_TEXT		0x01