mbedでWebSocketサーバー

mbed_ws_server.cpp

#include <string>
#include <sstream>
#include "mbed.h"
#include "EthernetInterface.h"
#include "sha1config.h"
#include "sha1.h"

//#define TEXT_LCD

#ifdef TEXT_LCD
    #include "TextLCD.h"
#else
    #include "C12832_lcd.h"
#endif

const int WS_HANDSHAKE = 1;
const int WS_CONNECT = 2;
int wsState = 0;

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
#ifdef TEXT_LCD
    TextLCD lcd(p24, p26, p27, p28, p29, p30);
#else
    C12832_LCD lcd;
#endif
PwmOut servo1(p21);
PwmOut servo2(p22);

string encode64(char *Buf,int Length) {
    char *Codes64 = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
    int Byte3=0;
    string Result="";
    for (int i=0; i<Length; i++) {
        Byte3=(Byte3<<8)+(int)Buf[i];
        if ((i+1)%3==0) {
            Result=Result+Codes64[(Byte3>>18)&0x3F];
            Result=Result+Codes64[(Byte3>>12)&0x3F];
            Result=Result+Codes64[(Byte3>>6)&0x3F];
            Result=Result+Codes64[(Byte3)&0x3F];
            Byte3=0;
        }
    }

    int Rest=Length%3;
    switch (Rest) {
        case 1:
            Byte3=Byte3<<4;
            Result=Result+Codes64[(Byte3>>6)&0x3F];
            Result=Result+Codes64[(Byte3)&0x3F];
            Result=Result+"==";
            break;
        case 2:
            Byte3=Byte3<<2;
            Result=Result+Codes64[(Byte3>>12)&0x3F];
            Result=Result+Codes64[(Byte3>>6 )&0x3F];
            Result=Result+Codes64[(Byte3)&0x3F];
            Result=Result+"=";
            break;
    }
    return Result;
}

int main() {
    EthernetInterface eth;
    //eth.init("192.168.0.2", "255.255.255.0", "192.168.0.2");
    //eth.init("192.168.1.2","255.255.255.0","192.168.1.1");
    eth.init(); // DHCP
    eth.connect();
    lcd.cls();
    lcd.locate(0, 0);
    lcd.printf("%s", eth.getIPAddress());
    servo1.period_ms(20);// pulse cycle = 20ms
    servo2.period_ms(20);// pulse cycle = 20ms
    
    TCPSocketServer server;
    
    server.bind(8080);
    server.listen();
    printf("\nWait for new connection...\n");
    while(true){
        TCPSocketConnection sock;
        server.accept(sock);
        wsState = WS_HANDSHAKE;
        char buff[600];
        char strServo[8] = "  servo";
        strServo[0] = 0x81;
        strServo[1] = 5;

        int len;
        while (true) {
            len = sock.receive(buff, sizeof(buff)-1);
            if (len <= 0){
                printf("close by 0 length\n");
                break;
            }
            buff[len] = '\0';
            switch(wsState){
                case WS_HANDSHAKE:{
                        printf("Received %d chars from server:\n%s\n", len, buff);
                        for (int i = 0; i < len; i++) {
                            if (buff[i] == 'K' && buff[i + 1] == 'e' && buff[i + 2] == 'y') {
                                for (int j = i + 1; j < len; j++) {
                                    if (buff[j] == '\r') {
                                        i += 5;
                                        int keyLen = j - i;
                                        char strKey[keyLen + 1];
                                        strKey[keyLen] = 0;
                                        strncpy(strKey, buff + i, keyLen);
                                        char guid[] = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11";
                                        strcat(strKey, guid);
                                        unsigned char hash[20];
                                        sha1((unsigned char*)strKey,strlen((char*)strKey),hash);
                                        string accept = encode64((char*)hash, 20);
                                        string hsRes = "HTTP/1.1 101 Switching Protocols\r\nUpgrade: websocket\r\nConnection: Upgrade\r\nSec-WebSocket-Accept: ";
                                        hsRes += accept;
                                        hsRes += "\r\n\r\n";
                                        printf("%s", hsRes.c_str());
                                        sock.send_all((char *)hsRes.c_str(), hsRes.size());
                                        printf("change to WS_CONNECT state\n");
                                        #ifdef TEXT_LCD
                                            lcd.locate(0, 1);
                                        #else
                                            lcd.locate(0, 8);
                                        #endif
                                        lcd.printf("connect");
                                        sock.send(strServo, 7);
                                        wsState = WS_CONNECT;
                                        break;
                                    }
                                }
                            }
                        }
                    }
                    break;
                case WS_CONNECT:{
                        bool fin = (buff[0] & 0x80) == 0x80;
                        int opcode = buff[0] & 0x0f;
                        if (opcode == 0x8) {
                            printf("close by close opcode\n");
                            wsState = WS_HANDSHAKE;
                            sock.close();
                            #ifdef TEXT_LCD
                                lcd.locate(0, 1);
                            #else
                                lcd.locate(0, 8);
                            #endif
                            lcd.printf("close  ");
                            break;
                        }
                        if (opcode == 0x9) {
                            buff[0] += 1;
                            sock.send_all(buff, len);
                            break;
                        }
                        int dataLen = buff[1] & 0x7f;
                        if (!fin || dataLen > 125) {
                            sock.close();
                            printf("close by illegal data length \n");
                            #ifdef TEXT_LCD
                                lcd.locate(0, 1);
                            #else
                                lcd.locate(0, 8);
                            #endif
                            lcd.printf("close  ");
                            wsState = WS_HANDSHAKE;
                            break;
                        }
                        int i = 0;
                        for (i = 0; i < dataLen; i++) {
                            buff[6 + i] = buff[6 + i] ^ buff[2 + (i % 4)];
                        }
                        printf("data length:%d\n", dataLen);
                        buff[6 + dataLen] = 0;
                        if (opcode == 1) {
                            // TEXT DATA
                            printf("received data:%s\n", buff + 6);
                            char sendData[2 + dataLen + 1];
                            sendData[0] = buff[0];
                            sendData[1] = buff[1] & 0x7f;
                            for (i = 0; i < dataLen; i++) {
                                sendData[2 + i] = buff[6 + i];
                            }
                            sendData[2 + dataLen] = 0;
                            sock.send_all(sendData, 2 + dataLen);
                        } else if (opcode == 2) {
                            // BINARY DATA
                        }
                    }
                    break;
            }
        }
        sock.close();
    }
}