commit b23181fe9ed44b4cd3304332732d592ac74c7849
parent f37069ba3613aee8014716f3adb490e041a94477
Author: Brian Swetland <swetland@frotz.net>
Date: Thu, 6 Aug 2015 16:39:41 -0700
debugger: make SWO data available via raw (2332) and web (5557) socket
Diffstat:
11 files changed, 878 insertions(+), 8 deletions(-)
diff --git a/Makefile b/Makefile
@@ -20,6 +20,9 @@ SRCS := tools/debugger.c \
tools/rswdp.c \
tools/socket.c \
tools/swo.c \
+ tools/websocket.c \
+ tools/base64.c \
+ tools/sha1.c \
tools/usb.c
ifneq ($(TOOLCHAIN),)
diff --git a/tools/base64.c b/tools/base64.c
@@ -0,0 +1,126 @@
+/* base64.h
+ *
+ * Copyright 2011 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <string.h>
+
+typedef uint8_t u8;
+typedef uint32_t u32;
+
+#define OP_MASK 0xC0
+#define OP_BITS 0x00
+#define OP_END 0x40
+#define OP_SKIP 0x80
+#define OP_BAD 0xC0
+
+static u8 DTABLE[256] =
+ "\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\x80\x80\xc0\xc0\x80\xc0\xc0"
+ "\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0"
+ "\x80\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\x3e\xc0\xc0\xc0\x3f"
+ "\x34\x35\x36\x37\x38\x39\x3a\x3b\x3c\x3d\xc0\xc0\xc0\x40\xc0\xc0"
+ "\xc0\x00\x01\x02\x03\x04\x05\x06\x07\x08\x09\x0a\x0b\x0c\x0d\x0e"
+ "\x0f\x10\x11\x12\x13\x14\x15\x16\x17\x18\x19\xc0\xc0\xc0\xc0\xc0"
+ "\xc0\x1a\x1b\x1c\x1d\x1e\x1f\x20\x21\x22\x23\x24\x25\x26\x27\x28"
+ "\x29\x2a\x2b\x2c\x2d\x2e\x2f\x30\x31\x32\x33\xc0\xc0\xc0\xc0\xc0"
+ "\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0"
+ "\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0"
+ "\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0"
+ "\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0"
+ "\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0"
+ "\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0"
+ "\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0"
+ "\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0\xc0";
+
+static u8 ETABLE[64] =
+ "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
+ "abcdefghijklmnopqrstuvwxyz"
+ "0123456789+/";
+
+int base64_decode(u8 *data, u32 len, u8 *out)
+{
+ u8 *start = out;
+ u32 tmp = 0, pos = 0;
+
+ while (len-- > 0) {
+ u8 x = DTABLE[*data++];
+ switch (x & OP_MASK) {
+ case OP_BITS:
+ tmp = (tmp << 6) | x;
+ if (++pos == 4) {
+ *out++ = tmp >> 16;
+ *out++ = tmp >> 8;
+ *out++ = tmp;
+ tmp = 0;
+ pos = 0;
+ }
+ break;
+ case OP_SKIP:
+ break;
+ case OP_END:
+ if (pos == 2) {
+ if (*data != '=')
+ return -1;
+ *out++ = (tmp >> 4);
+ } else if (pos == 3) {
+ *out++ = (tmp >> 10);
+ *out++ = (tmp >> 2);
+ } else {
+ return -1;
+ }
+ return out - start;
+ case OP_BAD:
+ return -1;
+ }
+ }
+
+ if (pos != 0)
+ return -1;
+
+ return out - start;
+}
+
+int base64_encode(u8 *data, u32 len, u8 *out)
+{
+ u8 *start = out;
+ u32 n;
+
+ while (len >= 3) {
+ n = (data[0] << 16) | (data[1] << 8) | data[2];
+ data += 3;
+ len -= 3;
+ *out++ = ETABLE[(n >> 18) & 63];
+ *out++ = ETABLE[(n >> 12) & 63];
+ *out++ = ETABLE[(n >> 6) & 63];
+ *out++ = ETABLE[n & 63];
+ }
+ if (len == 2) {
+ *out++ = ETABLE[(data[0] >> 2) & 63];
+ *out++ = ETABLE[((data[0] << 4) | (data[1] >> 4)) & 63];
+ *out++ = ETABLE[(data[1] << 2) & 63];
+ *out++ = '=';
+ } else if (len == 1) {
+ *out++ = ETABLE[(data[0] >> 2) & 63];
+ *out++ = ETABLE[(data[0] << 4) & 63];
+ *out++ = '=';
+ *out++ = '=';
+ }
+
+ return out - start;
+}
diff --git a/tools/debugger-core.c b/tools/debugger-core.c
@@ -22,6 +22,7 @@
#include <string.h>
#include <ctype.h>
#include <stdarg.h>
+#include <errno.h>
#include <pthread.h>
#include <sys/socket.h>
@@ -31,6 +32,8 @@
#include "debugger.h"
#include "rswdp.h"
+#include "websocket.h"
+
#define DHCSR_C_DEBUGEN (1 << 0)
#define DHCSR_C_HALT (1 << 1)
#define DHCSR_C_STEP (1 << 2)
@@ -53,6 +56,10 @@
extern int swd_verbose;
+#define GDB_SOCKET 5555
+#define SWO_SOCKET 2332
+#define WEB_SOCKET 5557
+
static void m_event(const char *evt) {
xprintf(XCORE, "DEBUG EVENT: %s\n", evt);
}
@@ -74,6 +81,13 @@ static void monitor(void) {
static pthread_mutex_t _dbg_lock = PTHREAD_MUTEX_INITIALIZER;
static pthread_t _dbg_thread;
+static pthread_t _gdb_thread;
+
+static pthread_mutex_t _swo_lock = PTHREAD_MUTEX_INITIALIZER;
+static pthread_t _swo_thread;
+static pthread_t _ws_thread;
+static int swo_fd = -1;
+static ws_server_t *_ws = NULL;
void debugger_lock() {
pthread_mutex_lock(&_dbg_lock);
@@ -109,11 +123,10 @@ void *debugger_monitor(void *arg) {
void gdb_server(int fd);
int socket_listen_tcp(unsigned port);
-static pthread_t _listen_master;
void *gdb_listener(void *arg) {
int fd;
- if ((fd = socket_listen_tcp(5555)) < 0) {
- xprintf(XGDB, "gdb_listener() cannot bind to 5555\n");
+ if ((fd = socket_listen_tcp(GDB_SOCKET)) < 0) {
+ xprintf(XGDB, "gdb_listener() cannot bind to %d\n", GDB_SOCKET);
return NULL;
}
for (;;) {
@@ -126,9 +139,90 @@ void *gdb_listener(void *arg) {
return NULL;
}
+void transmit_swo_data(void *data, unsigned len) {
+ pthread_mutex_lock(&_swo_lock);
+ if (swo_fd >= 0) {
+ if (write(swo_fd, data, len)) ;
+ }
+ if (_ws != NULL) {
+ unsigned char *x = data;
+ x--;
+ *x = 3; // SWO DATA MARKER
+ ws_send_binary(_ws, x, len + 1);
+ }
+ pthread_mutex_unlock(&_swo_lock);
+}
+
+void *swo_listener(void *arg) {
+ char buf[1024];
+ int fd;
+ if ((fd = socket_listen_tcp(SWO_SOCKET)) < 0) {
+ xprintf(XCORE, "swo_listener() cannot bind to %d\n", SWO_SOCKET);
+ return NULL;
+ }
+ for (;;) {
+ int s = accept(fd, NULL, NULL);
+ if (s >= 0) {
+ xprintf(XCORE, "[ swo listener connected ]\n");
+ pthread_mutex_lock(&_swo_lock);
+ swo_fd = s;
+ pthread_mutex_unlock(&_swo_lock);
+ for (;;) {
+ int r = read(s, buf, 1024);
+ if (r < 0) {
+ if (errno != EINTR) break;
+ }
+ if (r == 0) break;
+ }
+ pthread_mutex_lock(&_swo_lock);
+ swo_fd = -1;
+ pthread_mutex_unlock(&_swo_lock);
+ close(s);
+ xprintf(XCORE, "[ swo listener disconnected ]\n");
+ }
+ }
+ return NULL;
+}
+
+static void ws_message(unsigned op, void *msg, size_t len, void *cookie) {
+}
+
+void *ws_listener(void *arg) {
+ ws_server_t *ws;
+ int fd;
+ if ((fd = socket_listen_tcp(WEB_SOCKET)) < 0) {
+ xprintf(XCORE, "websocket cannot bind to %d\n", WEB_SOCKET);
+ return NULL;
+ }
+ for (;;) {
+ int s = accept(fd, NULL, NULL);
+ if (s >= 0) {
+ ws = ws_handshake(s, ws_message, NULL);
+ if (ws) {
+ xprintf(XCORE, "[ websocket connected ]\n");
+ } else {
+ xprintf(XCORE, "[ websocket handshake failed ]\n");
+ continue;
+ }
+ pthread_mutex_lock(&_swo_lock);
+ _ws = ws;
+ pthread_mutex_unlock(&_swo_lock);
+ ws_process_messages(ws);
+ pthread_mutex_lock(&_swo_lock);
+ _ws = NULL;
+ pthread_mutex_unlock(&_swo_lock);
+ ws_close(ws);
+ xprintf(XCORE, "[ websocket disconnected ]\n");
+ }
+ }
+ return NULL;
+}
+
void debugger_init() {
pthread_create(&_dbg_thread, NULL, debugger_monitor, NULL);
- pthread_create(&_listen_master, NULL, gdb_listener, NULL);
+ pthread_create(&_gdb_thread, NULL, gdb_listener, NULL);
+ pthread_create(&_swo_thread, NULL, swo_listener, NULL);
+ pthread_create(&_ws_thread, NULL, ws_listener, NULL);
}
diff --git a/tools/debugger.h b/tools/debugger.h
@@ -24,6 +24,7 @@ typedef enum {
XCORE, // debugger core
XDATA, // debugger command response
XGDB, // messages from GDB bridge
+ XREMOTE, // remote console messages
} xpchan;
#define printf __use_xprintf_in_debugger__
diff --git a/tools/module.mk b/tools/module.mk
@@ -11,12 +11,15 @@ M_OBJS += tools/socket.o
M_OBJS += tools/gdb-bridge.o
M_OBJS += tools/lkdebug.o
M_OBJS += tools/swo.o
+M_OBJS += tools/websocket.o
+M_OBJS += tools/sha1.o
+M_OBJS += tools/base64.o
M_OBJS += out/debugger-builtins.o
$(call build-host-executable)
out/debugger-builtins.c: $(AGENTS) bin/mkbuiltins
@mkdir -p out
- ./bin/mkbuiltins $(AGENTS) > $@
+ ./bin/mkbuiltins $(AGENTS) > $@
M_NAME := stm32boot
M_OBJS := tools/stm32boot.o
@@ -40,7 +43,7 @@ M_NAME := lpcboot
M_OBJS := tools/lpcboot.o tools/usb.o
$(call build-host-executable)
-M_NAME := uconsole
+M_NAME := uconsole
M_OBJS := tools/uconsole.o tools/usb.o
$(call build-host-executable)
diff --git a/tools/rswdp.c b/tools/rswdp.c
@@ -92,6 +92,7 @@ struct txn {
};
void process_swo_data(void *data, unsigned count);
+void transmit_swo_data(void *data, unsigned count);
static void process_async(u32 *data, unsigned count) {
unsigned msg, n;
@@ -117,6 +118,7 @@ static void process_async(u32 *data, unsigned count) {
if (n > count)
return;
process_swo_data(data, n * 4);
+ transmit_swo_data(data, n * 4);
data += n;
count -= n;
default:
diff --git a/tools/sha1.c b/tools/sha1.c
@@ -0,0 +1,307 @@
+/* sha.c
+**
+** Copyright 2008, The Android Open Source Project
+**
+** Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are met:
+** * Redistributions of source code must retain the above copyright
+** notice, this list of conditions and the following disclaimer.
+** * Redistributions in binary form must reproduce the above copyright
+** notice, this list of conditions and the following disclaimer in the
+** documentation and/or other materials provided with the distribution.
+** * Neither the name of Google Inc. nor the names of its contributors may
+** be used to endorse or promote products derived from this software
+** without specific prior written permission.
+**
+** THIS SOFTWARE IS PROVIDED BY Google Inc. ``AS IS'' AND ANY EXPRESS OR
+** IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+** MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
+** EVENT SHALL Google Inc. BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+** PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+** OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+** WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+** OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+** ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "sha1.h"
+
+// Some machines lack byteswap.h and endian.h. These have to use the
+// slower code, even if they're little-endian.
+
+#if defined(HAVE_ENDIAN_H) && defined(HAVE_LITTLE_ENDIAN)
+
+#include <byteswap.h>
+#include <memory.h>
+
+// This version is about 28% faster than the generic version below,
+// but assumes little-endianness.
+
+static inline uint32_t ror27(uint32_t val) {
+ return (val >> 27) | (val << 5);
+}
+static inline uint32_t ror2(uint32_t val) {
+ return (val >> 2) | (val << 30);
+}
+static inline uint32_t ror31(uint32_t val) {
+ return (val >> 31) | (val << 1);
+}
+
+static void SHA1_Transform(SHA_CTX* ctx) {
+ uint32_t W[80];
+ register uint32_t A, B, C, D, E;
+ int t;
+
+ A = ctx->state[0];
+ B = ctx->state[1];
+ C = ctx->state[2];
+ D = ctx->state[3];
+ E = ctx->state[4];
+
+#define SHA_F1(A,B,C,D,E,t) \
+ E += ror27(A) + \
+ (W[t] = bswap_32(ctx->buf.w[t])) + \
+ (D^(B&(C^D))) + 0x5A827999; \
+ B = ror2(B);
+
+ for (t = 0; t < 15; t += 5) {
+ SHA_F1(A,B,C,D,E,t + 0);
+ SHA_F1(E,A,B,C,D,t + 1);
+ SHA_F1(D,E,A,B,C,t + 2);
+ SHA_F1(C,D,E,A,B,t + 3);
+ SHA_F1(B,C,D,E,A,t + 4);
+ }
+ SHA_F1(A,B,C,D,E,t + 0); // 16th one, t == 15
+
+#undef SHA_F1
+
+#define SHA_F1(A,B,C,D,E,t) \
+ E += ror27(A) + \
+ (W[t] = ror31(W[t-3] ^ W[t-8] ^ W[t-14] ^ W[t-16])) + \
+ (D^(B&(C^D))) + 0x5A827999; \
+ B = ror2(B);
+
+ SHA_F1(E,A,B,C,D,t + 1);
+ SHA_F1(D,E,A,B,C,t + 2);
+ SHA_F1(C,D,E,A,B,t + 3);
+ SHA_F1(B,C,D,E,A,t + 4);
+
+#undef SHA_F1
+
+#define SHA_F2(A,B,C,D,E,t) \
+ E += ror27(A) + \
+ (W[t] = ror31(W[t-3] ^ W[t-8] ^ W[t-14] ^ W[t-16])) + \
+ (B^C^D) + 0x6ED9EBA1; \
+ B = ror2(B);
+
+ for (t = 20; t < 40; t += 5) {
+ SHA_F2(A,B,C,D,E,t + 0);
+ SHA_F2(E,A,B,C,D,t + 1);
+ SHA_F2(D,E,A,B,C,t + 2);
+ SHA_F2(C,D,E,A,B,t + 3);
+ SHA_F2(B,C,D,E,A,t + 4);
+ }
+
+#undef SHA_F2
+
+#define SHA_F3(A,B,C,D,E,t) \
+ E += ror27(A) + \
+ (W[t] = ror31(W[t-3] ^ W[t-8] ^ W[t-14] ^ W[t-16])) + \
+ ((B&C)|(D&(B|C))) + 0x8F1BBCDC; \
+ B = ror2(B);
+
+ for (; t < 60; t += 5) {
+ SHA_F3(A,B,C,D,E,t + 0);
+ SHA_F3(E,A,B,C,D,t + 1);
+ SHA_F3(D,E,A,B,C,t + 2);
+ SHA_F3(C,D,E,A,B,t + 3);
+ SHA_F3(B,C,D,E,A,t + 4);
+ }
+
+#undef SHA_F3
+
+#define SHA_F4(A,B,C,D,E,t) \
+ E += ror27(A) + \
+ (W[t] = ror31(W[t-3] ^ W[t-8] ^ W[t-14] ^ W[t-16])) + \
+ (B^C^D) + 0xCA62C1D6; \
+ B = ror2(B);
+
+ for (; t < 80; t += 5) {
+ SHA_F4(A,B,C,D,E,t + 0);
+ SHA_F4(E,A,B,C,D,t + 1);
+ SHA_F4(D,E,A,B,C,t + 2);
+ SHA_F4(C,D,E,A,B,t + 3);
+ SHA_F4(B,C,D,E,A,t + 4);
+ }
+
+#undef SHA_F4
+
+ ctx->state[0] += A;
+ ctx->state[1] += B;
+ ctx->state[2] += C;
+ ctx->state[3] += D;
+ ctx->state[4] += E;
+}
+
+void SHA_update(SHA_CTX* ctx, const void* data, int len) {
+ int i = ctx->count % sizeof(ctx->buf);
+ const uint8_t* p = (const uint8_t*)data;
+
+ ctx->count += len;
+
+ while (len > sizeof(ctx->buf) - i) {
+ memcpy(&ctx->buf.b[i], p, sizeof(ctx->buf) - i);
+ len -= sizeof(ctx->buf) - i;
+ p += sizeof(ctx->buf) - i;
+ SHA1_Transform(ctx);
+ i = 0;
+ }
+
+ while (len--) {
+ ctx->buf.b[i++] = *p++;
+ if (i == sizeof(ctx->buf)) {
+ SHA1_Transform(ctx);
+ i = 0;
+ }
+ }
+}
+
+
+const uint8_t* SHA_final(SHA_CTX* ctx) {
+ uint64_t cnt = ctx->count * 8;
+ int i;
+
+ SHA_update(ctx, (uint8_t*)"\x80", 1);
+ while ((ctx->count % sizeof(ctx->buf)) != (sizeof(ctx->buf) - 8)) {
+ SHA_update(ctx, (uint8_t*)"\0", 1);
+ }
+ for (i = 0; i < 8; ++i) {
+ uint8_t tmp = cnt >> ((7 - i) * 8);
+ SHA_update(ctx, &tmp, 1);
+ }
+
+ for (i = 0; i < 5; i++) {
+ ctx->buf.w[i] = bswap_32(ctx->state[i]);
+ }
+
+ return ctx->buf.b;
+}
+
+#else // #if defined(HAVE_ENDIAN_H) && defined(HAVE_LITTLE_ENDIAN)
+
+#define rol(bits, value) (((value) << (bits)) | ((value) >> (32 - (bits))))
+
+static void SHA1_transform(SHA_CTX *ctx) {
+ uint32_t W[80];
+ uint32_t A, B, C, D, E;
+ uint8_t *p = ctx->buf;
+ int t;
+
+ for(t = 0; t < 16; ++t) {
+ uint32_t tmp = *p++ << 24;
+ tmp |= *p++ << 16;
+ tmp |= *p++ << 8;
+ tmp |= *p++;
+ W[t] = tmp;
+ }
+
+ for(; t < 80; t++) {
+ W[t] = rol(1,W[t-3] ^ W[t-8] ^ W[t-14] ^ W[t-16]);
+ }
+
+ A = ctx->state[0];
+ B = ctx->state[1];
+ C = ctx->state[2];
+ D = ctx->state[3];
+ E = ctx->state[4];
+
+ for(t = 0; t < 80; t++) {
+ uint32_t tmp = rol(5,A) + E + W[t];
+
+ if (t < 20)
+ tmp += (D^(B&(C^D))) + 0x5A827999;
+ else if ( t < 40)
+ tmp += (B^C^D) + 0x6ED9EBA1;
+ else if ( t < 60)
+ tmp += ((B&C)|(D&(B|C))) + 0x8F1BBCDC;
+ else
+ tmp += (B^C^D) + 0xCA62C1D6;
+
+ E = D;
+ D = C;
+ C = rol(30,B);
+ B = A;
+ A = tmp;
+ }
+
+ ctx->state[0] += A;
+ ctx->state[1] += B;
+ ctx->state[2] += C;
+ ctx->state[3] += D;
+ ctx->state[4] += E;
+}
+
+void SHA_update(SHA_CTX *ctx, const void *data, int len) {
+ int i = ctx->count % sizeof(ctx->buf);
+ const uint8_t* p = (const uint8_t*)data;
+
+ ctx->count += len;
+
+ while (len--) {
+ ctx->buf[i++] = *p++;
+ if (i == sizeof(ctx->buf)) {
+ SHA1_transform(ctx);
+ i = 0;
+ }
+ }
+}
+const uint8_t *SHA_final(SHA_CTX *ctx) {
+ uint8_t *p = ctx->buf;
+ uint64_t cnt = ctx->count * 8;
+ int i;
+
+ SHA_update(ctx, (uint8_t*)"\x80", 1);
+ while ((ctx->count % sizeof(ctx->buf)) != (sizeof(ctx->buf) - 8)) {
+ SHA_update(ctx, (uint8_t*)"\0", 1);
+ }
+ for (i = 0; i < 8; ++i) {
+ uint8_t tmp = cnt >> ((7 - i) * 8);
+ SHA_update(ctx, &tmp, 1);
+ }
+
+ for (i = 0; i < 5; i++) {
+ uint32_t tmp = ctx->state[i];
+ *p++ = tmp >> 24;
+ *p++ = tmp >> 16;
+ *p++ = tmp >> 8;
+ *p++ = tmp >> 0;
+ }
+
+ return ctx->buf;
+}
+
+#endif // endianness
+
+void SHA_init(SHA_CTX* ctx) {
+ ctx->state[0] = 0x67452301;
+ ctx->state[1] = 0xEFCDAB89;
+ ctx->state[2] = 0x98BADCFE;
+ ctx->state[3] = 0x10325476;
+ ctx->state[4] = 0xC3D2E1F0;
+ ctx->count = 0;
+}
+
+/* Convenience function */
+const uint8_t* SHA(const void *data, int len, uint8_t *digest) {
+ const uint8_t *p;
+ int i;
+ SHA_CTX ctx;
+ SHA_init(&ctx);
+ SHA_update(&ctx, data, len);
+ p = SHA_final(&ctx);
+ for (i = 0; i < SHA_DIGEST_SIZE; ++i) {
+ digest[i] = *p++;
+ }
+ return digest;
+}
diff --git a/tools/sha1.h b/tools/sha1.h
@@ -0,0 +1,63 @@
+/* sha.h
+**
+** Copyright 2008, The Android Open Source Project
+**
+** Redistribution and use in source and binary forms, with or without
+** modification, are permitted provided that the following conditions are met:
+** * Redistributions of source code must retain the above copyright
+** notice, this list of conditions and the following disclaimer.
+** * Redistributions in binary form must reproduce the above copyright
+** notice, this list of conditions and the following disclaimer in the
+** documentation and/or other materials provided with the distribution.
+** * Neither the name of Google Inc. nor the names of its contributors may
+** be used to endorse or promote products derived from this software
+** without specific prior written permission.
+**
+** THIS SOFTWARE IS PROVIDED BY Google Inc. ``AS IS'' AND ANY EXPRESS OR
+** IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+** MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
+** EVENT SHALL Google Inc. BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+** PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+** OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+** WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+** OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+** ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef _EMBEDDED_SHA_H_
+#define _EMBEDDED_SHA_H_
+
+#include <inttypes.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct SHA_CTX {
+ uint64_t count;
+ uint32_t state[5];
+#if defined(HAVE_ENDIAN_H) && defined(HAVE_LITTLE_ENDIAN)
+ union {
+ uint8_t b[64];
+ uint32_t w[16];
+ } buf;
+#else
+ uint8_t buf[64];
+#endif
+} SHA_CTX;
+
+void SHA_init(SHA_CTX* ctx);
+void SHA_update(SHA_CTX* ctx, const void* data, int len);
+const uint8_t* SHA_final(SHA_CTX* ctx);
+
+/* Convenience method. Returns digest parameter value. */
+const uint8_t* SHA(const void* data, int len, uint8_t* digest);
+
+#define SHA_DIGEST_SIZE 20
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/tools/swo.c b/tools/swo.c
@@ -37,11 +37,11 @@ static void handle_swv_src(unsigned id, unsigned val, unsigned n) {
console_data[console_ptr++] = val;
if (val == '\n') {
console_data[console_ptr] = 0;
- xprintf(XDATA, "[remote] %s", console_data);
+ xprintf(XREMOTE, "[remote] %s", console_data);
console_ptr = 0;
} else if (console_ptr == 254) {
console_data[console_ptr] = 0;
- xprintf(XDATA, "[remote] %s\n", console_data);
+ xprintf(XREMOTE, "[remote] %s\n", console_data);
}
}
}
diff --git a/tools/websocket.c b/tools/websocket.c
@@ -0,0 +1,238 @@
+/* websocket.c
+ *
+ * Copyright 2015 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/uio.h>
+
+#include "sha1.h"
+#include "websocket.h"
+
+struct ws_server {
+ FILE *fp;
+ int fd;
+ ws_callback_t func;
+ void *cookie;
+};
+
+int base64_decode(uint8_t *data, uint32_t len, uint8_t *out);
+int base64_encode(uint8_t *data, uint32_t len, uint8_t *out);
+
+static const char *wsmagic = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11";
+
+#define F_FIN 0x80
+#define F_INVAL 0x70
+#define OP_NONE 0
+#define OP_TEXT 1
+#define OP_BINARY 2
+#define OP_CLOSE 8
+#define OP_PING 9
+#define OP_PONG 10
+
+#define F_MASKED 0x80
+#define LEN_16BIT 0x7E
+#define LEN_64BIT 0x7F
+
+static int ws_send(int fd, unsigned op, const void *data, size_t len);
+
+int ws_send_text(ws_server_t *ws, const void *data, size_t len) {
+ return ws_send(ws->fd, OP_TEXT, data, len);
+}
+
+int ws_send_binary(ws_server_t *ws, const void *data, size_t len) {
+ return ws_send(ws->fd, OP_BINARY, data, len);
+}
+
+static int _ws_handshake(ws_server_t *ws) {
+ FILE *fp = ws->fp;
+ unsigned check = 0;
+ char line[8192 + 128];
+ uint8_t digest[SHA_DIGEST_SIZE];
+ int n;
+
+ for (;;) {
+ if (fgets(line, sizeof(line), fp) == NULL) {
+ return -1;
+ }
+ if ((n = strlen(line)) == 0) {
+ return -1;
+ }
+ if (line[n-1] == '\n') {
+ line[n-1] = 0;
+ if ((n > 1) && (line[n-2] == '\r')) {
+ line[n-2] = 0;
+ }
+ }
+ if (line[0] == 0) {
+ //printf(">BREAK<\n");
+ break;
+ }
+ //printf(">HEADER: %s<\n", line);
+ if (!strcasecmp(line, "upgrade: websocket")) {
+ check |= 1;
+ continue;
+ }
+ if (!strcasecmp(line, "connection: upgrade")) {
+ // may be keep-alive, etc
+ check |= 2;
+ continue;
+ }
+ if (!strncasecmp(line, "sec-websocket-key: ", 19)) {
+ check |= 4;
+ strcat(line, wsmagic);
+ SHA(line + 19, strlen(line + 19), digest);
+ }
+ if (!strcasecmp(line, "sec-websocket-version: 13")) {
+ check |= 8;
+ }
+ // Host:
+ // Origin:
+ // Sec-WebSocket-Protocol:
+ // Sec-WebSocket-Extensions:
+ }
+ if ((check & 13) != 13) {
+ return -1;
+ }
+ n = base64_encode((void*) digest, SHA_DIGEST_SIZE, (void*) line);
+ line[n] = 0;
+ fprintf(fp,
+ "HTTP/1.1 101 Switching Protocols\r\n"
+ "Upgrade: websocket\r\n"
+ "Connection: Upgrade\r\n"
+ "Sec-Websocket-Accept: %s\r\n"
+ "\r\n", line
+ );
+ return 0;
+}
+
+int ws_process_messages(ws_server_t *ws) {
+ FILE *fp = ws->fp;
+ unsigned char hdr[2+2+4];
+ unsigned char msg[65536];
+ unsigned len;
+ for (;;) {
+ if (fread(hdr, 2, 1, fp) != 1) break;
+ // client must not set invalid flags
+ if (hdr[0] & F_INVAL) break;
+ // client must mask
+ if (!(hdr[1] & F_MASKED)) break;
+ // todo: fragments
+ if (!(hdr[0] & F_FIN)) break;
+ // todo: large packets
+ if ((hdr[1] & 0x7F) == LEN_64BIT) break;
+ if ((hdr[1] & 0x7F) == LEN_16BIT) {
+ if (fread(hdr + 2, 6, 1, fp) != 1) break;
+ len = (hdr[2] << 8) | hdr[3];
+ } else {
+ if (fread(hdr + 4, 4, 1, fp) != 1) break;
+ len = hdr[1] & 0x7F;
+ }
+#if 0
+ printf("<op %d len %d mask %02x%02x%02x%02x>\n",
+ hdr[0] & 15, len, hdr[4], hdr[5], hdr[6], hdr[7]);
+#endif
+ if (fread(msg, len, 1, fp) != 1) break;
+ // op?
+#if 0
+ unsigned i;
+ for (i = 0; i < len; i++) {
+ msg[i] ^= hdr[4 + (i & 3)];
+ printf("%02x ", msg[i]);
+ }
+ printf("\n");
+#endif
+ switch (hdr[0] & 15) {
+ case OP_PING:
+ break;
+ case OP_PONG:
+ break;
+ case OP_CLOSE:
+ // todo: reply
+ return 0;
+ case OP_TEXT:
+ ws->func(OP_TEXT, msg, len, ws->cookie);
+ break;
+ case OP_BINARY:
+ ws->func(OP_BINARY, msg, len, ws->cookie);
+ break;
+ default:
+ // invalid opcode
+ return 0;
+ }
+ }
+ return 0;
+}
+
+static int ws_send(int fd, unsigned op, const void *data, size_t len) {
+ unsigned char hdr[4];
+ struct iovec iov[2];
+ if (len > 65535) return -1;
+ hdr[0] = F_FIN | (op & 15);
+ if (len < 126) {
+ hdr[1] = len;
+ iov[0].iov_len = 2;
+ } else {
+ hdr[1] = LEN_16BIT;
+ hdr[2] = len >> 8;
+ hdr[3] = len;
+ iov[0].iov_len = 4;
+ }
+ iov[0].iov_base = hdr;
+ iov[1].iov_base = (void*) data;
+ iov[1].iov_len = len;
+ return writev(fd, iov, 2);
+}
+
+ws_server_t *ws_handshake(int fd, ws_callback_t func, void *cookie) {
+ ws_server_t *ws;
+
+ if ((ws = malloc(sizeof(ws_server_t))) == NULL) {
+ goto fail;
+ }
+ if ((ws->fp = fdopen(fd, "r+")) == NULL) {
+ goto fail;
+ }
+ if (_ws_handshake(ws)) {
+ goto fail;
+ }
+ ws->fd = fd;
+ ws->func = func;
+ ws->cookie = cookie;
+ return ws;
+fail:
+ if (ws != NULL) {
+ free(ws);
+ if (ws->fp) {
+ fclose(ws->fp);
+ return NULL;
+ }
+ }
+ close(fd);
+ return NULL;
+}
+
+void ws_close(ws_server_t *ws) {
+ fclose(ws->fp);
+ free(ws);
+}
diff --git a/tools/websocket.h b/tools/websocket.h
@@ -0,0 +1,33 @@
+/* websocket.c
+ *
+ * Copyright 2015 Brian Swetland <swetland@frotz.net>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef _WEBSOCKET_SERVER_H_
+#define _WEBSOCKET_SERVER_H_
+
+typedef struct ws_server ws_server_t;
+
+typedef void (*ws_callback_t)(unsigned op, void *msg, size_t len, void *cookie);
+
+ws_server_t *ws_handshake(int fd, ws_callback_t func, void *cookie);
+int ws_process_messages(ws_server_t *ws);
+
+int ws_send_text(ws_server_t *ws, const void *msg, size_t len);
+int ws_send_binary(ws_server_t *ws, const void *msg, size_t len);
+
+void ws_close(ws_server_t *ws);
+
+#endif
