Hayato Hiratori / Mbed 2 deprecated WebSocketServerTest

Simple WebSocket server to control a tank.

Dependencies:   SNICInterface_mod WebSocketServer mbed-rtos mbed PowerControl C12832

au Firefox OS WoTハッカソン on ホワイトデーで使用した、タンクを動かすプログラムです。 ゲームパッドでタンクを操縦します。

ゲームパッドは PC に接続し、ブラウザ上の Web アプリから Gamepad API で入力を取得します。 取得した入力データは WebSocket で mbed 上の WebSocket サーバに送信します。

WebSocket サーバのコードはライブラリ化したので、他のプログラムでもインポートして使えます。

使用した機材

  • LPC1768
  • Murata TypeYD
  • LPC1768 用アプリケーションボード
  • TAMIYA トラック&ホイールセット
  • TAMIYA ダブルキヤボックス(左右独立４速タイプ)
  • TAMIYA ユニバーサルプレート
  • TOSHIBA TA7291P x 2
  • その他、モバイルバッテリー、電池ボックス等

左右のモータードライバにそれぞれ LPC1768 の p12, p13 と p14, p15 のピンを割り当てていますが、必要に応じてコードを変更してください。

コントローラー側（Webアプリ）

https://github.com/chikoski/wotxwot-control

Firefox ブラウザで動作確認しています（他のブラウザでは動かないかも）。 ゲームパッドの左右のスティックの前後の操作が左右それぞれのモータの前転・後転に対応しています。

動いているところの動画

https://www.facebook.com/video.php?v=456620974491805

ハッカソンでは ARM 賞をいただきました！

(参考) au Firefox OS WoTハッカソン on ホワイトデー

http://au-fx.kddi.com/event/20150314/wot_hackathon0314.html

WebSocketServer.cpp@8:6635ca3b5a5c, 2015-03-14 (annotated)

Committer:
flatbird
Date:
Sat Mar 14 07:52:50 2015 +0000
Revision:
8:6635ca3b5a5c
Parent:
7:6cfe0638b957
Child:
9:774f408b9740
fixed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
flatbird 0:f7596ed7ab5c 1 #include "WebSocketServer.h"
flatbird 6:79ecd4e53456 2 #include "sha1.h"
flatbird 0:f7596ed7ab5c 3
flatbird 0:f7596ed7ab5c 4 WebSocketServer::WebSocketServer() {
flatbird 0:f7596ed7ab5c 5 }
flatbird 0:f7596ed7ab5c 6
flatbird 0:f7596ed7ab5c 7 WebSocketServer::~WebSocketServer() {
flatbird 0:f7596ed7ab5c 8 }
flatbird 0:f7596ed7ab5c 9
flatbird 1:84af7a219830 10 bool WebSocketServer::init(int port) {
flatbird 1:84af7a219830 11 mTCPSocketServer.set_blocking(true);
flatbird 1:84af7a219830 12
flatbird 1:84af7a219830 13 int ret = mTCPSocketServer.bind(port);
flatbird 1:84af7a219830 14 if (ret != 0) {
flatbird 1:84af7a219830 15 printf("ERROR: Failed to bind %d\r\n", ret);
flatbird 1:84af7a219830 16 return false;
flatbird 1:84af7a219830 17 }
flatbird 1:84af7a219830 18 ret = mTCPSocketServer.listen();
flatbird 1:84af7a219830 19 if (ret != 0) {
flatbird 1:84af7a219830 20 printf("ERROR: Failed to listen %d\r\n", ret);
flatbird 1:84af7a219830 21 return false;
flatbird 1:84af7a219830 22 }
flatbird 1:84af7a219830 23
flatbird 1:84af7a219830 24 return true;
flatbird 1:84af7a219830 25 }
flatbird 1:84af7a219830 26
flatbird 1:84af7a219830 27 void WebSocketServer::run() {
flatbird 7:6cfe0638b957 28 // TCPSocketConnection connection;
flatbird 1:84af7a219830 29 char buf[1024];
flatbird 1:84af7a219830 30
flatbird 1:84af7a219830 31 while (true) {
flatbird 1:84af7a219830 32 bool isWebSocket = false;
flatbird 8:6635ca3b5a5c 33 printf("accepting\r\n");
flatbird 7:6cfe0638b957 34 int ret = mTCPSocketServer.accept(mConnection);
flatbird 1:84af7a219830 35 if (ret != 0) {
flatbird 1:84af7a219830 36 continue;
flatbird 1:84af7a219830 37 }
flatbird 1:84af7a219830 38 printf("New connection\r\n");
flatbird 7:6cfe0638b957 39 mConnection.set_blocking(true);
flatbird 5:ce3f1dd90068 40
flatbird 7:6cfe0638b957 41 while (mConnection.is_connected()) {
flatbird 7:6cfe0638b957 42 ret = mConnection.receive(buf, sizeof(buf) - 1);
flatbird 5:ce3f1dd90068 43 if (ret == 0) {
flatbird 6:79ecd4e53456 44 // printf("No data to receive\r\n");
flatbird 5:ce3f1dd90068 45 continue;
flatbird 5:ce3f1dd90068 46 }
flatbird 5:ce3f1dd90068 47 if (ret < 0) {
flatbird 5:ce3f1dd90068 48 printf("ERROR: Failed to receive %d\r\n", ret);
flatbird 1:84af7a219830 49 break;
flatbird 1:84af7a219830 50 }
flatbird 1:84af7a219830 51 if (!isWebSocket) {
flatbird 7:6cfe0638b957 52 if (this->handleHTTP(buf, ret)) {
flatbird 1:84af7a219830 53 isWebSocket = true;
flatbird 1:84af7a219830 54 } else {
flatbird 1:84af7a219830 55 printf("ERROR: Non websocket\r\n");
flatbird 1:84af7a219830 56 break;
flatbird 1:84af7a219830 57 }
flatbird 1:84af7a219830 58 } else {
flatbird 1:84af7a219830 59 if (!this->handleWebSocket(buf, ret)) {
flatbird 8:6635ca3b5a5c 60 printf("websocket closed\r\n");
flatbird 1:84af7a219830 61 break;
flatbird 1:84af7a219830 62 }
flatbird 1:84af7a219830 63 }
flatbird 1:84af7a219830 64 }
flatbird 5:ce3f1dd90068 65 printf("closed\r\n");
flatbird 7:6cfe0638b957 66 mConnection.close();
flatbird 1:84af7a219830 67 }
flatbird 1:84af7a219830 68 }
flatbird 1:84af7a219830 69
flatbird 1:84af7a219830 70 bool WebSocketServer::setHandler(const char* path, WebSocketHandler* handler) {
flatbird 7:6cfe0638b957 71 mHandler = handler; // support only one handler now
flatbird 1:84af7a219830 72 return true;
flatbird 1:84af7a219830 73 }
flatbird 1:84af7a219830 74
flatbird 5:ce3f1dd90068 75 #define UPGRADE_WEBSOCKET "Upgrade: websocket"
flatbird 5:ce3f1dd90068 76 #define SEC_WEBSOCKET_KEY "Sec-WebSocket-Key:"
flatbird 5:ce3f1dd90068 77 #define MAGIC_NUMBER "258EAFA5-E914-47DA-95CA-C5AB0DC85B11"
flatbird 5:ce3f1dd90068 78
flatbird 7:6cfe0638b957 79 bool WebSocketServer::handleHTTP(char* buf, int size) {
flatbird 1:84af7a219830 80 char* line = &buf[0];
flatbird 5:ce3f1dd90068 81 char key[128];
flatbird 5:ce3f1dd90068 82 bool isUpgradeWebSocket = false;
flatbird 5:ce3f1dd90068 83 bool isSecWebSocketKeyFound = false;
flatbird 1:84af7a219830 84
flatbird 1:84af7a219830 85 for (int i = 0; i < size; i++) {
flatbird 1:84af7a219830 86 if (buf[i] == '\r' && i+1 < size && buf[i+1] == '\n') {
flatbird 1:84af7a219830 87 buf[i] = '\0';
flatbird 1:84af7a219830 88 if (strlen(buf) <= 0) {
flatbird 1:84af7a219830 89 break;
flatbird 1:84af7a219830 90 }
flatbird 8:6635ca3b5a5c 91 printf("[%s]\r\n", line);
flatbird 5:ce3f1dd90068 92 if (line == &buf[0]) {
flatbird 6:79ecd4e53456 93 char* method = strtok(buf, " ");
flatbird 6:79ecd4e53456 94 char* path = strtok(NULL, " ");
flatbird 6:79ecd4e53456 95 char* version = strtok(NULL, " ");
flatbird 8:6635ca3b5a5c 96 // printf("[%s] [%s] [%s]\r\n", method, path, version);
flatbird 5:ce3f1dd90068 97
flatbird 5:ce3f1dd90068 98 } else if (strncmp(line, UPGRADE_WEBSOCKET, strlen(UPGRADE_WEBSOCKET)) == 0) {
flatbird 5:ce3f1dd90068 99 isUpgradeWebSocket = true;
flatbird 8:6635ca3b5a5c 100 printf("*** %s found\r\n", UPGRADE_WEBSOCKET);
flatbird 5:ce3f1dd90068 101 } else if (strncmp(line, SEC_WEBSOCKET_KEY, strlen(SEC_WEBSOCKET_KEY)) == 0) {
flatbird 8:6635ca3b5a5c 102 printf("*** %s found\r\n", SEC_WEBSOCKET_KEY);
flatbird 5:ce3f1dd90068 103 isSecWebSocketKeyFound = true;
flatbird 5:ce3f1dd90068 104 char* ptr = line + strlen(SEC_WEBSOCKET_KEY);
flatbird 5:ce3f1dd90068 105 while (*ptr == ' ') ++ptr;
flatbird 6:79ecd4e53456 106 strcpy(key, ptr);
flatbird 8:6635ca3b5a5c 107 printf("*** key=%s\r\n", key);
flatbird 5:ce3f1dd90068 108 }
flatbird 1:84af7a219830 109 i += 2;
flatbird 1:84af7a219830 110 line = &buf[i];
flatbird 1:84af7a219830 111 }
flatbird 1:84af7a219830 112 }
flatbird 1:84af7a219830 113
flatbird 5:ce3f1dd90068 114 if (isUpgradeWebSocket && isSecWebSocketKeyFound) {
flatbird 7:6cfe0638b957 115 this->sendUpgradeResponse(key);
flatbird 8:6635ca3b5a5c 116 mPrevFin = true;
flatbird 5:ce3f1dd90068 117 return true;
flatbird 5:ce3f1dd90068 118 }
flatbird 5:ce3f1dd90068 119
flatbird 5:ce3f1dd90068 120 return false;
flatbird 5:ce3f1dd90068 121 }
flatbird 5:ce3f1dd90068 122
flatbird 7:6cfe0638b957 123 #define OP_CONT 0x0
flatbird 7:6cfe0638b957 124 #define OP_TEXT 0x1
flatbird 7:6cfe0638b957 125 #define OP_BINARY 0x2
flatbird 7:6cfe0638b957 126 #define OP_CLOSE 0x8
flatbird 7:6cfe0638b957 127 #define OP_PING 0x9
flatbird 7:6cfe0638b957 128 #define OP_PONG 0xA
flatbird 7:6cfe0638b957 129
flatbird 5:ce3f1dd90068 130 bool WebSocketServer::handleWebSocket(char* buf, int size) {
flatbird 7:6cfe0638b957 131 uint8_t* ptr = (uint8_t*)buf;
flatbird 7:6cfe0638b957 132
flatbird 7:6cfe0638b957 133 bool fin = (*ptr & 0x80) == 0x80;
flatbird 7:6cfe0638b957 134 uint8_t opcode = *ptr & 0xF;
flatbird 8:6635ca3b5a5c 135 // printf("Byte1: %02X\r\n", *ptr);
flatbird 7:6cfe0638b957 136
flatbird 8:6635ca3b5a5c 137 // printf("fin=%d\r\n", fin);
flatbird 7:6cfe0638b957 138 printf("opcode=%d\r\n", opcode);
flatbird 8:6635ca3b5a5c 139
flatbird 7:6cfe0638b957 140 if (opcode == OP_PING) {
flatbird 7:6cfe0638b957 141 *ptr = ((*ptr & 0xF0) | OP_PONG);
flatbird 7:6cfe0638b957 142 mConnection.send_all(buf, size);
flatbird 7:6cfe0638b957 143 return true;
flatbird 7:6cfe0638b957 144 }
flatbird 7:6cfe0638b957 145 if (opcode == OP_CLOSE) {
flatbird 7:6cfe0638b957 146 return false;
flatbird 7:6cfe0638b957 147 }
flatbird 7:6cfe0638b957 148 ptr++;
flatbird 8:6635ca3b5a5c 149 // printf("Byte2: %02X\r\n", *ptr);
flatbird 7:6cfe0638b957 150
flatbird 8:6635ca3b5a5c 151 if (!fin || !mPrevFin) {
flatbird 7:6cfe0638b957 152 printf("WARN: Data consists of multiple frame not supported\r\n");
flatbird 8:6635ca3b5a5c 153 mPrevFin = fin;
flatbird 7:6cfe0638b957 154 return true; // not an error, just discard it
flatbird 7:6cfe0638b957 155 }
flatbird 8:6635ca3b5a5c 156 mPrevFin = fin;
flatbird 7:6cfe0638b957 157
flatbird 8:6635ca3b5a5c 158 bool mask = (*ptr & 0x80) == 0x80;
flatbird 7:6cfe0638b957 159 uint8_t len = *ptr & 0x7F;
flatbird 7:6cfe0638b957 160 ptr++;
flatbird 8:6635ca3b5a5c 161
flatbird 8:6635ca3b5a5c 162 // printf("mask=%d\r\n", mask);
flatbird 8:6635ca3b5a5c 163 // printf("length=%d\r\n", len);
flatbird 8:6635ca3b5a5c 164
flatbird 7:6cfe0638b957 165 if (len > 125) {
flatbird 7:6cfe0638b957 166 printf("WARN: Extended payload lenght not supported\r\n");
flatbird 7:6cfe0638b957 167 return true; // not an error, just discard it
flatbird 7:6cfe0638b957 168 }
flatbird 7:6cfe0638b957 169
flatbird 7:6cfe0638b957 170 char* data;
flatbird 7:6cfe0638b957 171 if (mask) {
flatbird 7:6cfe0638b957 172 char* maskingKey = (char*)ptr;
flatbird 7:6cfe0638b957 173 data = (char*)(ptr + 4);
flatbird 7:6cfe0638b957 174 for (int i = 0; i < len; i++) {
flatbird 7:6cfe0638b957 175 data[i] = data[i] ^ maskingKey[(i % 4)];
flatbird 7:6cfe0638b957 176 }
flatbird 7:6cfe0638b957 177 } else {
flatbird 7:6cfe0638b957 178 data = (char*)ptr;
flatbird 7:6cfe0638b957 179 }
flatbird 7:6cfe0638b957 180 if (mHandler) {
flatbird 7:6cfe0638b957 181 if (opcode == OP_TEXT) {
flatbird 8:6635ca3b5a5c 182 // printf("OP_TEXT\r\n");
flatbird 7:6cfe0638b957 183 data[len] = '\0';
flatbird 7:6cfe0638b957 184 mHandler->onMessage(data);
flatbird 7:6cfe0638b957 185 } else if (opcode == OP_BINARY) {
flatbird 8:6635ca3b5a5c 186 // printf("OP_BINARY %d\r\n", len);
flatbird 7:6cfe0638b957 187 mHandler->onMessage(data, len);
flatbird 7:6cfe0638b957 188 }
flatbird 7:6cfe0638b957 189 }
flatbird 8:6635ca3b5a5c 190 // printf("return true\r\n");
flatbird 1:84af7a219830 191 return true;
flatbird 1:84af7a219830 192 }
flatbird 1:84af7a219830 193
flatbird 5:ce3f1dd90068 194 static char encoding_table[] = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H',
flatbird 5:ce3f1dd90068 195 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P',
flatbird 5:ce3f1dd90068 196 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X',
flatbird 5:ce3f1dd90068 197 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f',
flatbird 5:ce3f1dd90068 198 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n',
flatbird 5:ce3f1dd90068 199 'o', 'p', 'q', 'r', 's', 't', 'u', 'v',
flatbird 5:ce3f1dd90068 200 'w', 'x', 'y', 'z', '0', '1', '2', '3',
flatbird 5:ce3f1dd90068 201 '4', '5', '6', '7', '8', '9', '+', '/'};
flatbird 5:ce3f1dd90068 202 static int mod_table[] = {0, 2, 1};
flatbird 5:ce3f1dd90068 203
flatbird 5:ce3f1dd90068 204 char *base64_encode(const unsigned char *data,
flatbird 5:ce3f1dd90068 205 size_t input_length,
flatbird 5:ce3f1dd90068 206 // size_t *output_length
flatbird 5:ce3f1dd90068 207 char* encoded_data,
flatbird 5:ce3f1dd90068 208 size_t buffer_length) {
flatbird 5:ce3f1dd90068 209
flatbird 5:ce3f1dd90068 210 // *output_length = 4 * ((input_length + 2) / 3);
flatbird 5:ce3f1dd90068 211 size_t output_length = 4 * ((input_length + 2) / 3);
flatbird 5:ce3f1dd90068 212 if (buffer_length - 1 < output_length) {
flatbird 5:ce3f1dd90068 213 return NULL;
flatbird 5:ce3f1dd90068 214 }
flatbird 5:ce3f1dd90068 215
flatbird 5:ce3f1dd90068 216 // char *encoded_data = malloc(*output_length);
flatbird 5:ce3f1dd90068 217 // if (encoded_data == NULL) return NULL;
flatbird 5:ce3f1dd90068 218
flatbird 5:ce3f1dd90068 219 for (int i = 0, j = 0; i < input_length;) {
flatbird 5:ce3f1dd90068 220
flatbird 5:ce3f1dd90068 221 uint32_t octet_a = i < input_length ? (unsigned char)data[i++] : 0;
flatbird 5:ce3f1dd90068 222 uint32_t octet_b = i < input_length ? (unsigned char)data[i++] : 0;
flatbird 5:ce3f1dd90068 223 uint32_t octet_c = i < input_length ? (unsigned char)data[i++] : 0;
flatbird 5:ce3f1dd90068 224
flatbird 5:ce3f1dd90068 225 uint32_t triple = (octet_a << 0x10) + (octet_b << 0x08) + octet_c;
flatbird 5:ce3f1dd90068 226
flatbird 5:ce3f1dd90068 227 encoded_data[j++] = encoding_table[(triple >> 3 * 6) & 0x3F];
flatbird 5:ce3f1dd90068 228 encoded_data[j++] = encoding_table[(triple >> 2 * 6) & 0x3F];
flatbird 5:ce3f1dd90068 229 encoded_data[j++] = encoding_table[(triple >> 1 * 6) & 0x3F];
flatbird 5:ce3f1dd90068 230 encoded_data[j++] = encoding_table[(triple >> 0 * 6) & 0x3F];
flatbird 5:ce3f1dd90068 231 }
flatbird 5:ce3f1dd90068 232
flatbird 5:ce3f1dd90068 233 for (int i = 0; i < mod_table[input_length % 3]; i++)
flatbird 5:ce3f1dd90068 234 // encoded_data[*output_length - 1 - i] = '=';
flatbird 5:ce3f1dd90068 235 encoded_data[output_length - 1 - i] = '=';
flatbird 5:ce3f1dd90068 236 encoded_data[output_length] = '\0';
flatbird 5:ce3f1dd90068 237
flatbird 5:ce3f1dd90068 238 return encoded_data;
flatbird 1:84af7a219830 239 }
flatbird 5:ce3f1dd90068 240
flatbird 7:6cfe0638b957 241 bool WebSocketServer::sendUpgradeResponse(char* key) {
flatbird 5:ce3f1dd90068 242 char buf[128];
flatbird 5:ce3f1dd90068 243
flatbird 5:ce3f1dd90068 244 if (strlen(key) + sizeof(MAGIC_NUMBER) > sizeof(buf)) {
flatbird 5:ce3f1dd90068 245 return false;
flatbird 5:ce3f1dd90068 246 }
flatbird 5:ce3f1dd90068 247 strcpy(buf, key);
flatbird 5:ce3f1dd90068 248 strcat(buf, MAGIC_NUMBER);
flatbird 5:ce3f1dd90068 249
flatbird 5:ce3f1dd90068 250 unsigned char hash[20];
flatbird 5:ce3f1dd90068 251 char encoded[30];
flatbird 6:79ecd4e53456 252 sha1(buf, strlen(buf), (char*)hash);
flatbird 5:ce3f1dd90068 253 base64_encode(hash, 20, encoded, sizeof(encoded));
flatbird 5:ce3f1dd90068 254
flatbird 5:ce3f1dd90068 255 char resp[] = "HTTP/1.1 101 Switching Protocols\r\n" \
flatbird 5:ce3f1dd90068 256 "Upgrade: websocket\r\n" \
flatbird 5:ce3f1dd90068 257 "Connection: Upgrade\r\n" \
flatbird 5:ce3f1dd90068 258 "Sec-WebSocket-Accept: XXXXXXXXXXXXXXXXXXXXXXXXXXXXX\r\n\r\n";
flatbird 5:ce3f1dd90068 259 char* ptr = strstr(resp, "XXXXX");
flatbird 5:ce3f1dd90068 260 strcpy(ptr, encoded);
flatbird 5:ce3f1dd90068 261 strcpy(ptr+strlen(encoded), "\r\n\r\n");
flatbird 5:ce3f1dd90068 262
flatbird 7:6cfe0638b957 263 int ret = mConnection.send_all(resp, strlen(resp));
flatbird 5:ce3f1dd90068 264 if (ret < 0) {
flatbird 5:ce3f1dd90068 265 printf("ERROR: Failed to send response\r\n");
flatbird 5:ce3f1dd90068 266 return false;
flatbird 5:ce3f1dd90068 267 }
flatbird 5:ce3f1dd90068 268
flatbird 5:ce3f1dd90068 269 return true;
flatbird 5:ce3f1dd90068 270 }