components/vnc_server/vnc-server.c

/**
 * @file vnc-server.c
 * @brief VNC server original written for eCos.
 *
 * The VNC server runs on the target platform and waits for a client 
 * (vncviewer program running on a PC) to connect via an ethernet connection
 * (port 5900). Once a client has connected, the target platform has a 
 * virtual screen, keyboard and mouse.
 *
 * This port of the VNC server has been designed with some limitations in 
 * order to keep memory and processor power requirements to a minimum.
 *
 * The VNC client must accept the settings that the VNC server suggests (bits-per-pixel, 
 * number-of-colours and so on as specified at build time with the eCos configuration 
 * tool) or the VNC server will just disconnect.
 *
 * The VNC server only supports CoRRE encoding and RAW encoding for sending 
 * display data to the client.
 *
 * The VNC server does not support password authentication, the client is able 
 * to connect without the user typing in a password.
 *
 * Only one VNC client may connect to the VNC server at a time.
 *
 * In reality these limitations are not a problem.
 *
 * @author Chris Garry <cgarry@sweeneydesign.co.uk>
 * @author Michiel Broek.
 */

#include "vnc-server.h"
#include "websocket_server.h"

static const char *TAG = "vnc-server";

#define BACKLOG             5				///< Number of pending connections queue will hold 
#define MESSAGE_BUFFER_SIZE 60				///< Was 50, but I have seen 52 bytes length
#define TILE_SIZE           16				///< Tile dimension
#define TRUE_COLOUR_FLAG    1				///< True colour is set
#define BIG_ENDIAN_FLAG     1				///< Always send colour data big endian

/* Default display parameters */
#define BITS_PER_PIXEL      8				///< Bits per pixel
#define PIXEL_DEPTH         8				///< Usefull bits per pixel
#define RED_MAX             7				///< Red maximum
#define GREEN_MAX           7				///< Green maximum
#define BLUE_MAX            3				///< Blue maximum, together 8 bits.
#define RED_SHIFT           5				///< Red shift in color byte
#define GREEN_SHIFT         2				///< Green shift in color byte
#define BLUE_SHIFT          0				///< Blue shift in color byte


/* Initial default RGB332 */
uint8_t			Bits_Per_Pixel = 8;		///< Current number of bits per pixel

uint16_t		Red_Max     = RED_MAX;		///< Current Red maxmimum
uint16_t		Green_Max   = GREEN_MAX;	///< Current Green maximum
uint16_t		Blue_Max    = BLUE_MAX;		///< Current Blue maximum
uint8_t			Red_Shift   = RED_SHIFT;	///< Current Red bits shift
uint8_t			Green_Shift = GREEN_SHIFT;	///< Current Green bits shift
uint8_t			Blue_Shift  = BLUE_SHIFT;	///< Current Blue bits shift
bool			AltPixels   = false;		///< Alternate the pixels

/* Client to Server message types */
#define SET_PIXEL_FORMAT         0			///< Set pixel format
#define FIX_COLOUR_MAP_ENTRIES   1			///< Fix color map entries (not used)
#define SET_ENCODINGS            2			///< Set encodings
#define FRAME_BUFFER_UPDATE_REQ  3			///< Request frame buffer update
#define KEY_EVENT                4			///< Keyboard event (not used)
#define POINTER_EVENT            5			///< Pointer event, translated to touch events
#define CLIENT_CUT_TEXT          6			///< Text editing, not used.

/* Macros to split colour to bytes */
#define COLOUR2BYTE1(col) ((col>>8)&0xFF)		///< High part
#define COLOUR2BYTE0(col) (col&0xFF)			///< Low part


/* Thread function prototypes */
static TaskHandle_t	xTaskClientHandler = NULL;	///< Task for VNC clients
static TaskHandle_t	xTaskFrameUpdate   = NULL;	///< Task for framebuffer updates
static TaskHandle_t	xTaskWSclient      = NULL;	///< Task for websocket clients

SemaphoreHandle_t	SoundBell_lock     = NULL;	///< Lock for the bell sound.

static QueueHandle_t	message_queue;			///< Websockets message queue
const static int	message_queue_size = 5;		///< Message queue size

/**
 * @brief VNC messages.
 */
struct strMessage {
    int			length;				///< Length of the message
    uint8_t		message[MESSAGE_BUFFER_SIZE];	///< The message
};


uint8_t			VNC_pointer_button = 0;		///< Button mask for the mouse pointer
uint16_t		VNC_pointer_x = 0;		///< Mouse position X
uint16_t		VNC_pointer_y = 0;		///< Mouse position Y


/* Define size of each thread's stack */
#define MIN_STACK_SIZE	3072				///< Minimal task stack size


/**
 * @brief  Messages
 */
static char		server_ProtocolVersion[] = "RFB 003.003\n";
static char		bad_protocol[] = "Unsupported ProtocolVersion";
static char		server_busy[] = "Server is Busy";
static char		sound_bell[] = "\2";
static char		desktop_name[] = "MBSE BrewBoard";	// Hardcoded, don't change or the VNC webclient breaks.

/**
 * @brief  Frame Buffer
 */
vnc_color_t		frame_buffer[CONFIG_VNC_SERVER_FRAME_HEIGHT+1][CONFIG_VNC_SERVER_FRAME_WIDTH+1];

/* Calculate the number of tiles in the X and Y directions */
#if (CONFIG_VNC_SERVER_FRAME_HEIGHT % TILE_SIZE) != 0
#define NUM_TILES_Y_AXIS (CONFIG_VNC_SERVER_FRAME_HEIGHT/TILE_SIZE + 1)
#define LAST_TILE_HEIGHT (CONFIG_VNC_SERVER_FRAME_HEIGHT % TILE_SIZE)
#else
#define NUM_TILES_Y_AXIS (CONFIG_VNC_SERVER_FRAME_HEIGHT/TILE_SIZE)	///< Nr of tiles on the Y axis.
#define LAST_TILE_HEIGHT TILE_SIZE					///< Height of the last tile.
#endif

#if (CONFIG_VNC_SERVER_FRAME_WIDTH % TILE_SIZE) != 0
#define NUM_TILES_X_AXIS (CONFIG_VNC_SERVER_FRAME_WIDTH/TILE_SIZE + 1)
#define LAST_TILE_WIDTH (CONFIG_VNC_SERVER_FRAME_WIDTH % TILE_SIZE)
#else
#define NUM_TILES_X_AXIS (CONFIG_VNC_SERVER_FRAME_WIDTH/TILE_SIZE)	///< Nr of tiles on the X axis.
#define LAST_TILE_WIDTH TILE_SIZE					///< Width of the last tile.
#endif

/**
 * @brief  Array for marking tiles that have been updated
 */
int			tile_updated[NUM_TILES_Y_AXIS+1][NUM_TILES_X_AXIS+1];

EventGroupHandle_t	xEventGroupVNC;			///< Variable to signal that a client is connected and initialised
const int 		VNC_CLIENT_UPDATE_REQ = BIT0;	///< Client update request event
int			vnc_client_sock = -1;		///< Client network socket
bool			vnc_client_connected = false;	///< Client connected?
int			SoundBellCount;			///< Count the client's bell
bool			VNC_WS_run = false;		///< Websocket running
int			VNC_WS_num = -1;		///< Websocket connection number


/**
 * @brief Variable to hold the frame format details
 */
vnc_frame_format_t	frame_format = {CONFIG_VNC_SERVER_FRAME_WIDTH, CONFIG_VNC_SERVER_FRAME_HEIGHT, frame_buffer,
                                   1,			// RGB332 server native.
                                   0,			// RGB555
                                   0,			// RGB565
                                   0,			// BGR233
                                   0,			// TRUECOLOR0888
};


#if 0
void dump_msg(char *buf, uint16_t buflen, char *mode)
{   
    int i, l = 0;

    printf("%s %d", mode, buflen);
    for (i = 0; i < buflen; i++) {
	if ((i % 16) == 0) {
	    printf("\n%02d: ", l);
	    l++;
	}
	printf("%02x ", buf[i]);
    }
    printf("\n");
}
#endif


/**
 * @brief Structure to hold the encoding type details
 */
volatile struct encoding_type_struct
{
    uint8_t		raw;
    uint8_t		copy_rectangle;
    uint8_t		rre;
    uint8_t		corre;
    uint8_t		hextile;
} encoding_type;


static int GetMessageData(int, char *, int);
static int GenTileUpdateData(uint8_t *);
void task_VNCserver(void *);
void task_frame(void *);
void task_WS(void *);


/**
 * @brief Serve a new VNC client.
 */
void vnc_netconn_serve(void);


/* VNC startup. */
void VncStartup(void)
{
    /* Initialise mutex & cond vars */
    xEventGroupVNC = xEventGroupCreate();
    SoundBell_lock = xSemaphoreCreateMutex();

    xTaskCreate(&task_VNCserver, "VNCserver",    MIN_STACK_SIZE, NULL, 5, &xTaskClientHandler);
    xTaskCreate(&task_frame,     "frame_update", MIN_STACK_SIZE, NULL, 6, &xTaskFrameUpdate);
}


/** 
 * @brief Client Handler Task.
 *
 * This task handles the client initialisation sequence. Once the client
 * is initialised this task handles all received messages from the client,
 * but does not send any data to the client.
 */
void task_VNCserver(void *pvParameter)
{
    int			server_sock; 
    struct sockaddr_in	server_addr;
    struct sockaddr_in	client_addr;
    socklen_t		client_addr_size;

    ESP_LOGI(TAG, "Starting VNC server");

    /* Clear the encoding type structure */
    encoding_type.raw = 0;
    encoding_type.copy_rectangle = 0;
    encoding_type.rre = 0;
    encoding_type.corre = 0;
    encoding_type.hextile = 0;

    /* Clear the sound bell counter */
    SoundBellCount = 0;
    VNC_pointer_button = 0;
    VNC_pointer_x = VNC_pointer_y = 0;

    /* Create socket for incomming connections */
    if ((server_sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0) {
        ESP_LOGE(TAG, "socket() VNC function failed");
        exit(1);
    }

    /* Construct the server address structure */
    memset(&server_addr, 0, sizeof(server_addr));  /* Fill entire structure with 0's */
    server_addr.sin_family      = AF_INET;         /* Internet address family */
    server_addr.sin_addr.s_addr = INADDR_ANY;      /* Autofill with my IP address */
    server_addr.sin_port        = htons(CONFIG_VNC_SERVER_PORT);

    /* Bind socket to local address */
    if (bind(server_sock, (struct sockaddr *) &server_addr, sizeof(server_addr)) < 0) {
        ESP_LOGE(TAG, "bind() VNC function failed");
        exit(1);
    }

    /* Set the socket to listen for incoming connections */
    if (listen(server_sock, BACKLOG) < 0) {
        ESP_LOGE(TAG, "listen() VNC function failed");
        exit(1);
    }

    do {
	vnc_client_sock = (accept(server_sock, (struct sockaddr *) &client_addr, &client_addr_size));
	if (vnc_client_sock >= 0) {
	    vnc_client_connected = true;
	    vnc_netconn_serve();
	    vnc_client_connected = false;
	}
	vTaskDelay((TickType_t)10); /* allows the freeRTOS scheduler to take over if needed */
    } while (vnc_client_sock >= 0);
}


/**
 * @brief Serve a new VNC client and handle the whole session.
 */
void vnc_netconn_serve(void)
{
    long int			temp_long;
    char 			protocol_ver[8], message_buffer[MESSAGE_BUFFER_SIZE];
    int				i, j, message_len, ProtocolOkay;
    uint32_t			*ptr_to_uint32;
    uint16_t			*ptr_to_uint16;
    struct sockaddr_storage	addr;
    socklen_t			len = sizeof addr;
    char			ipstr[INET6_ADDRSTRLEN];

    getpeername(vnc_client_sock, (struct sockaddr*)&addr, &len);
    if (addr.ss_family == AF_INET) {
	struct sockaddr_in *s = (struct sockaddr_in *)&addr;
	inet_ntop(AF_INET, &s->sin_addr, ipstr, sizeof ipstr);
    }
    ESP_LOGI(TAG, "VNC new client %s socket %d", ipstr, vnc_client_sock);

    /* ProtocolVersion Handshake - begin */
    /* Send ProtocolVersion we want to use to client */
    message_len = sprintf(message_buffer, "RFB 003.003\n");
    if (send(vnc_client_sock, message_buffer, message_len, 0) != message_len) {
	goto close_connection;
    }

    /* Receive ProtocolVersion the client wants to use */
    if (GetMessageData(vnc_client_sock, &(message_buffer[0]), 12) == 0) {
	goto close_connection;
    }

    /* Check this is acceptable (RFB 003.xxx is okay) */
    ProtocolOkay = 1;
    for (i = 0; i < 8; i++) {
	if (message_buffer[i] != server_ProtocolVersion[i]) {
	    ProtocolOkay = 0;
	}

        /* Store the protocol version - ignoring thr 'RFB ' part */
        protocol_ver[i] = message_buffer[i + 4];
    }
    protocol_ver[7] = 0;
    /* ProtocolVersion Handshake - end */

    /* Authentication - begin */
    /* Send Authentication scheme to be used to client */
    if (ProtocolOkay == 0) {
	/* ProtocolVerion is not okay */

	/* Generate the Bad ProtocolVerion message */
	ptr_to_uint32 = (uint32_t *) &(message_buffer[0]);
	*ptr_to_uint32 = htonl(0);
	ptr_to_uint32 = (uint32_t *) &(message_buffer[4]);
	*ptr_to_uint32 = htonl(strlen(bad_protocol));
	strcpy(&(message_buffer[8]), bad_protocol);

	if (send(vnc_client_sock, message_buffer, 8 + strlen(bad_protocol), 0) != (8 + strlen(bad_protocol))) {
	    printf("Call to send() 1 failed\n");
	}
	goto close_connection;

    } else if (VNC_WS_num != -1) {
	/* Busy with a websocket client */
	ptr_to_uint32 = (uint32_t *) &(message_buffer[0]);
	*ptr_to_uint32 = htonl(0);
	ptr_to_uint32 = (uint32_t *) &(message_buffer[4]);
	*ptr_to_uint32 = htonl(strlen(server_busy));
	strcpy(&(message_buffer[8]), server_busy);

	if (send(vnc_client_sock, message_buffer, 8 + strlen(server_busy), 0) != (8 + strlen(server_busy))) {
	    printf("Call to send() 1 failed\n");
	}
	goto close_connection;

    } else {
	/* ProtocolVerion is okay - connect with no authentication*/

	/* Generate the No Authentication message */
	ptr_to_uint32 = (uint32_t *) &(message_buffer[0]);
	*ptr_to_uint32 = htonl((uint32_t)1);

	if (send(vnc_client_sock, message_buffer, 4, 0) != 4) {
	    goto close_connection;
	}
    }
    /* Authentication - end */

    /* ClientInitialisation - begin */
    /* Receive initialisation message from client (1 byte) */
    if (GetMessageData(vnc_client_sock, &(message_buffer[0]), 1) == 0) {
	goto close_connection;
    }
    /* Do nothing with this as we only support 1 Client at a time */
    /* ClientInitialisation - end */

    /* ServerInitialisation - begin */

    /* Initial default RGB332 */
    Red_Max     = RED_MAX;
    Green_Max   = GREEN_MAX;
    Blue_Max    = BLUE_MAX;
    Red_Shift   = RED_SHIFT;
    Green_Shift = GREEN_SHIFT;
    Blue_Shift  = BLUE_SHIFT;
    AltPixels   = false;

    /* Create Initialisation message for client */
    ptr_to_uint16 = (uint16_t *) &(message_buffer[0]);
    *ptr_to_uint16 = htons((uint16_t)CONFIG_VNC_SERVER_FRAME_WIDTH);

    ptr_to_uint16 = (uint16_t *) &(message_buffer[2]);
    *ptr_to_uint16 = htons((uint16_t)CONFIG_VNC_SERVER_FRAME_HEIGHT);

    message_buffer[4] = (uint8_t)BITS_PER_PIXEL;
    message_buffer[5] = (uint8_t)PIXEL_DEPTH;
    message_buffer[6] = (uint8_t)BIG_ENDIAN_FLAG;
    message_buffer[7] = (uint8_t)TRUE_COLOUR_FLAG;

    ptr_to_uint16 = (uint16_t *) &(message_buffer[8]);
    *ptr_to_uint16 = htons(Red_Max);

    ptr_to_uint16 = (uint16_t *) &(message_buffer[10]);
    *ptr_to_uint16 = htons(Green_Max);

    ptr_to_uint16 = (uint16_t *) &(message_buffer[12]);
    *ptr_to_uint16 = htons(Blue_Max);

    message_buffer[14] = Red_Shift;
    message_buffer[15] = Green_Shift;
    message_buffer[16] = Blue_Shift;

    ptr_to_uint32 = (uint32_t *) &(message_buffer[20]);
    *ptr_to_uint32 = htonl(strlen(desktop_name));
    strcpy(&(message_buffer[24]), desktop_name);

    if (send(vnc_client_sock, message_buffer, 24 + strlen(desktop_name), 0) != (24 + strlen(desktop_name))) {
	printf("Call to send() 3 failed\n");
    }
    /* ServerInitialisation - end */

    /* Cancel any outstanding Sound Bell requests */
    if (xSemaphoreTake(SoundBell_lock, 10) == pdTRUE) {
	SoundBellCount = 0;
	xSemaphoreGive(SoundBell_lock);
    }

    ESP_LOGI(TAG, "VNC client connected (RFB Protocol Ver: %s)", protocol_ver);

    /* Main message handling loop */
    while(1) {
	int num_of_encodings;

	/* Receive 1st byte of message from client */
	if (GetMessageData(vnc_client_sock, &(message_buffer[0]), 1) == 0) {
	    goto close_connection;	/* Connection lost */
	}

	switch(message_buffer[0]) {
            case SET_PIXEL_FORMAT:

                /* Get the remainder (19 bytes) of message */
                if (GetMessageData(vnc_client_sock, &(message_buffer[1]), 19) == 0) {
                    goto close_connection;
                }

                /* Check pixel format is as expected */
                i = 0;
                if (message_buffer[4] != BITS_PER_PIXEL) {
		    ESP_LOGI(TAG, "SetPixelFormat client wants %d bits-per-pixel", message_buffer[4]);
                    i++;
                }

                if (message_buffer[5] != PIXEL_DEPTH) {
		    ESP_LOGI(TAG, "SetPixelFormat client wants %d pixel-depth", message_buffer[5]);
                    i++;
                }

                if ((message_buffer[7] & 0x01) != TRUE_COLOUR_FLAG) {
		    ESP_LOGI(TAG, "SetPixelFormat client wants %d true-colour-flag", message_buffer[7]);
                    i++;
                }

                ptr_to_uint16 = (uint16_t *)&(message_buffer[8]);
                if (htons(*ptr_to_uint16) != Red_Max) {
		    Red_Max = htons(*ptr_to_uint16);
		    AltPixels = true;
		    ESP_LOGI(TAG, "SetPixelFormat client granted %d red-max", Red_Max);
                }

                ptr_to_uint16 = (uint16_t *)&(message_buffer[10]);
                if (htons(*ptr_to_uint16) != Green_Max) {
		    Green_Max = htons(*ptr_to_uint16);
		    AltPixels = true;
		    ESP_LOGI(TAG, "SetPixelFormat client granted %d green-max", Green_Max);
                }

                ptr_to_uint16 = (uint16_t *)&(message_buffer[12]);
                if (htons(*ptr_to_uint16) != Blue_Max) {
		    Blue_Max = htons(*ptr_to_uint16);
		    AltPixels = true;
		    ESP_LOGI(TAG, "SetPixelFormat client granted %d blue-max", Blue_Max);
                }

                if (message_buffer[14] != Red_Shift) {
		    Red_Shift = message_buffer[14];
		    AltPixels = true;
		    ESP_LOGI(TAG, "SetPixelFormat client granted %d red-shift", Red_Shift);
                }

                if (message_buffer[15] != Green_Shift) {
		    Green_Shift = message_buffer[15];
		    AltPixels = true;
		    ESP_LOGI(TAG, "SetPixelFormat client granted %d green-shift", Green_Shift);
                }

                if (message_buffer[16] != Blue_Shift) {
		    Blue_Shift = message_buffer[16];
		    AltPixels = true;
		    ESP_LOGI(TAG, "SetPixelFormat client granted %d blue-shift", Blue_Shift);
                }

                if (i) {
		    ESP_LOGI(TAG, "SetPixelFormat %d errors, disconnect client", i);
                     ESP_LOGI(TAG, "Ensure the 'Auto select' is not enabled in your vncviewer options,");
                    goto close_connection_quietly;
                }

                break;

            case FIX_COLOUR_MAP_ENTRIES:
printf("fix colormap entries\n");
                /* Not supported, just get the data from the buffer and ignore it */

                /* Get the next 5 bytes of message */
                if (GetMessageData(vnc_client_sock, &(message_buffer[1]), 5) == 0) {
                    goto close_connection;
                }

                /* Calculate how many colour entries are in the buffer */
                i = message_buffer[4]*255 + message_buffer[5];
                i *= 6;

                /* Get this amount of data from the buffer */
                for (j = 0; j < i; j++) {
                    if (GetMessageData(vnc_client_sock, &(message_buffer[6]), 6) == 0) {
                        goto close_connection;
                    }
                }

                break;

            case SET_ENCODINGS:

                /* Get the next 3 bytes of message */
                if (GetMessageData(vnc_client_sock, &(message_buffer[1]), 3) == 0) {
                    goto close_connection;
                }

                num_of_encodings = message_buffer[2]*255 + message_buffer[3];

                /* Get the remainder of message */
                if (GetMessageData(vnc_client_sock, &(message_buffer[0]), 4 * num_of_encodings) == 0) {
                    goto close_connection;
                }

                /* Clear the encoding type structure */
                encoding_type.raw = 0;
                encoding_type.copy_rectangle = 0;
                encoding_type.rre = 0;
                encoding_type.corre = 0;
                encoding_type.hextile = 0;

                for (i = 0; i < num_of_encodings; i++) {
                    switch(message_buffer[3 + (i*4)]) {
                    case 0:  /* Raw encoding */
                        encoding_type.raw = i + 1;
                        break;
                    case 1:  /* Copy rectangle encoding */
                        encoding_type.copy_rectangle = i + 1;
                        break;
                    case 2:  /* RRE encoding */
                        encoding_type.rre = i + 1;
                        break;
                    case 4:  /* CoRRE encoding */
                        encoding_type.corre = i + 1;
                        break;
                    case 5:  /* Hextile encoding */
                        encoding_type.hextile = i + 1;
                        break;
                    default:  /* Unknown coding type - do nothing */
                        break;
                    }
                }
                break;

            case FRAME_BUFFER_UPDATE_REQ:
                /* Get the remainder of message (9 bytes) */
                if (GetMessageData(vnc_client_sock, &(message_buffer[1]), 9) == 0) {
                    goto close_connection;
                }

                if (!message_buffer[1]) {
                    /* Non-incremental mode - mark the squares that need to be updated */
                    for (i = (message_buffer[2]*255 + message_buffer[3])/TILE_SIZE;
                         i <= (message_buffer[2]*255 + message_buffer[3] + message_buffer[6]*255 + message_buffer[7])/TILE_SIZE;
                         i++) {
                        for (j = (message_buffer[4]*255 + message_buffer[5])/TILE_SIZE;
                             j <= (message_buffer[4]*255 + message_buffer[5] + message_buffer[8]*255 + message_buffer[9])/TILE_SIZE;
                             j++) {
                            tile_updated[j][i] = 1;
                        }
                    }
                }

                /* Signal that there is now a pending update request */
                xEventGroupSetBits(xEventGroupVNC, VNC_CLIENT_UPDATE_REQ);
		break;

            case KEY_EVENT:
                /* Handle the key event, ignored for brewboard */
                /* Get the remainder of message (7 bytes) */
                if (GetMessageData(vnc_client_sock, &(message_buffer[1]), 7) == 0) {
                    goto close_connection;
                }
                break;

            case POINTER_EVENT:
                /* Handle the pointer event, simulate the touch screen. */
                /* Get the remainder of message (5 bytes) */
                if (GetMessageData(vnc_client_sock, &(message_buffer[1]), 5) == 0) {
                    goto close_connection;
                }
		/* Set global variables that will be read by another task. */
		VNC_pointer_button = message_buffer[1];
		VNC_pointer_x = message_buffer[2]*255 + message_buffer[3];
		VNC_pointer_y = message_buffer[4]*255 + message_buffer[5];
                break;

            case CLIENT_CUT_TEXT:
                /* Handle the client has cut text event */
                /* Current we just get and discard the data */
                /* Get the next 7 bytes of the message */
                if (GetMessageData(vnc_client_sock, &(message_buffer[1]), 7) == 0) {
                    goto close_connection;
                }

                ptr_to_uint32 = (uint32_t *)&(message_buffer[4]);
                temp_long = htonl(*ptr_to_uint32);

                while (temp_long > 0) {
                    /* Get the text in chunks MESSAGE_BUFFER_SIZE-1 characters */
                    if (temp_long > MESSAGE_BUFFER_SIZE-2) {
                        if (GetMessageData(vnc_client_sock, &(message_buffer[0]), MESSAGE_BUFFER_SIZE-1) == 0) {
                            goto close_connection;
                        }

                        message_buffer[MESSAGE_BUFFER_SIZE-1] = 0;
                        temp_long -= (MESSAGE_BUFFER_SIZE-1);
                    } else {
                        if (GetMessageData(vnc_client_sock, &(message_buffer[0]), temp_long) == 0) {
                            goto close_connection;
                        }

                        message_buffer[temp_long] = 0;
                        temp_long = 0;
                    }
                }

                break;

            default:
		ESP_LOGI(TAG, "Unknown message %d from client", message_buffer[0]);
        }

	vTaskDelay( (TickType_t)1);
    }

close_connection:
    ESP_LOGI(TAG, "VNC client disconnected");

close_connection_quietly:

    /* Cancel any outstanding update requests */
    xEventGroupClearBits(xEventGroupVNC, VNC_CLIENT_UPDATE_REQ);
    close(vnc_client_sock);
}


/**
 * @brief Frame update task. This thread handles the sending of all frame
 *        update data to the client and sends the 'ring bell' message to
 *        the client when required.
 *        Works for VNC port connected clients and Websocket clients.
 * @param pvParameter Ignored
 */
void task_frame(void *pvParameter)
{
    int			i, j, x_pos, y_pos, packet_length, num_updated_tiles;
    uint16_t		*ptr_to_uint16;
    /* These are declared static so they don't use thread stack memory */
    static uint8_t	FramebufferUpdate_msg[4 + 12 + TILE_SIZE*TILE_SIZE*BITS_PER_PIXEL/8 + 1460];
    static int		FrameBufferPtr;  /* Pointer to next space in buffer */
    static int		tile_updated_local[NUM_TILES_Y_AXIS][NUM_TILES_X_AXIS];

    ESP_LOGI(TAG, "Starting VNC frame updater");

    while(1) {
        /* Wait until client sends a frame update request */
wait_for_client:
	xEventGroupWaitBits(xEventGroupVNC, VNC_CLIENT_UPDATE_REQ, pdFALSE, pdFALSE, portMAX_DELAY);

        /* Copy tile_updated array to local version and clear copied tiles */
	vTaskSuspendAll();
        num_updated_tiles = 0;
        for (i = 0; i < NUM_TILES_Y_AXIS; i++) {
            for (j = 0; j < NUM_TILES_X_AXIS; j++) {
                if (tile_updated[i][j]) {
                    tile_updated_local[i][j] = 1;
                    tile_updated[i][j] = 0;
                    num_updated_tiles++;  /* Keep count of the updated tiles */
                }
            }
        }
	xTaskResumeAll();

        if (num_updated_tiles) {
	    /* Cancel update request */
	    xEventGroupClearBits(xEventGroupVNC, VNC_CLIENT_UPDATE_REQ);
            /* Fill in constant parts of FramebufferUpdate Message */
            FramebufferUpdate_msg[0] = 0;  /* Message-type */
            FramebufferUpdate_msg[1] = 0;  /* Padding */
            ptr_to_uint16 = (uint16_t *) &(FramebufferUpdate_msg[2]);
            *ptr_to_uint16 = htons(num_updated_tiles);  /* Number-of-rectangles */
            FrameBufferPtr = 4;

            for (y_pos = 0; y_pos < NUM_TILES_Y_AXIS; y_pos++) {
                for (x_pos = 0; x_pos < NUM_TILES_X_AXIS; x_pos++) {
                    if (tile_updated_local[y_pos][x_pos]) {
                        /* Send current square data to client */

                        /* x-position */
                        FramebufferUpdate_msg[FrameBufferPtr+0] = (x_pos * TILE_SIZE) / 256;
                        FramebufferUpdate_msg[FrameBufferPtr+1] = (x_pos * TILE_SIZE) % 256;

                        /* y-position */
                        FramebufferUpdate_msg[FrameBufferPtr+2] = (y_pos * TILE_SIZE) / 256;
                        FramebufferUpdate_msg[FrameBufferPtr+3] = (y_pos * TILE_SIZE) % 256;


                        /* Set width of tile in packet */
                        if (x_pos == (NUM_TILES_X_AXIS -1)) {
                            /* Last tile in X-axis */
                            FramebufferUpdate_msg[FrameBufferPtr+4] = LAST_TILE_WIDTH / 256;
                            FramebufferUpdate_msg[FrameBufferPtr+5] = LAST_TILE_WIDTH % 256;
                        } else {
                            FramebufferUpdate_msg[FrameBufferPtr+4] = TILE_SIZE / 256;
                            FramebufferUpdate_msg[FrameBufferPtr+5] = TILE_SIZE % 256;
                        }

                        if (y_pos == (NUM_TILES_Y_AXIS -1)) {
                            /* Last tile in Y-axis */
                            FramebufferUpdate_msg[FrameBufferPtr+6] = LAST_TILE_HEIGHT / 256;
                            FramebufferUpdate_msg[FrameBufferPtr+7] = LAST_TILE_HEIGHT % 256;
                        } else {
                            FramebufferUpdate_msg[FrameBufferPtr+6] = TILE_SIZE / 256;
                            FramebufferUpdate_msg[FrameBufferPtr+7] = TILE_SIZE % 256;
                        }

                        /* Generate the packet data for this tile */
                        packet_length = GenTileUpdateData(&(FramebufferUpdate_msg[FrameBufferPtr]));

                        /* Send the packet data for this tile to the client */
                        FrameBufferPtr += packet_length;

                        if (FrameBufferPtr > 1460) {
                            /* Send the data to the client */
			    if (VNC_WS_num != -1) {
				ws_server_send_bin_client(VNC_WS_num, (char *)FramebufferUpdate_msg, FrameBufferPtr);
			    }
			    if (vnc_client_connected) {
                            	if (send(vnc_client_sock, FramebufferUpdate_msg, FrameBufferPtr, 0) != FrameBufferPtr) {
                            	    goto wait_for_client;
                            	}
			    }
                            FrameBufferPtr = 0;
                        }

                        tile_updated_local[y_pos][x_pos] = 0;  /* Clear the update bit for this square */
                    }
                }
            }

            if (FrameBufferPtr > 0) {
                /* Last data for this update, send it to the client */
		if (VNC_WS_num != -1) {
		    ws_server_send_bin_client(VNC_WS_num, (char *)FramebufferUpdate_msg, FrameBufferPtr);
		}
		if (vnc_client_connected) {
                    if (send(vnc_client_sock, FramebufferUpdate_msg, FrameBufferPtr, 0) != FrameBufferPtr) {
                    	goto wait_for_client;
                    }
		}

                FrameBufferPtr = 0;
            }

        } else { /* if (num_updated_tiles) */
            /* There was no new display data to send to the client */
            /* Sleep for 1/20th second before checking again */
	    vTaskDelay(20 / portTICK_PERIOD_MS);
	}

        /* Check for sound bell event */
	if (xSemaphoreTake(SoundBell_lock, 10) == pdTRUE) {
            if (SoundBellCount) {
            	--SoundBellCount;
		xSemaphoreGive(SoundBell_lock);

		if (vnc_client_connected) {
            	    if (send(vnc_client_sock, sound_bell, 1, 0) != 1) {
                    	goto wait_for_client;
            	    }
		}
		if (VNC_WS_num != -1) {
		    ws_server_send_bin_client(VNC_WS_num, sound_bell, 1);
		}
            } else {
	    	xSemaphoreGive(SoundBell_lock);
	    }
	}
    }
}


/**
 * Convert a framebuffer pixel to BGR233 if needed.
 * @param pixel The 8 bit pixel value
 * @return Then unchanged or changed pixel.
 */
vnc_color_t IRAM_ATTR PixelConvert(vnc_color_t pixel)
{
    if (!AltPixels)
    	return pixel;

    return (((pixel >> RED_SHIFT) & RED_MAX) << Red_Shift) | 
	   (((pixel >> GREEN_SHIFT) & GREEN_MAX) << Green_Shift) | 
	   (((pixel >> BLUE_SHIFT) & BLUE_MAX) << Blue_Shift);
}


/** 
 * @brief Generate tile update data function
 *
 *        This function is called by the frame_update thread to generate the message
 *        data for a tile to send to the client.  This function expects the
 *        x-position, y-position, width and height fields of the buffer to be filled
 *        in before it is called.
 *
 *        The format of the buffer is:
 *        packet_buffer[0:1] - x-position of tile
 *        packet_buffer[2:3] - y-position of tile
 *        packet_buffer[4:5] - width of tile
 *        packet_buffer[6:7] - height of tile
 *        packet_buffer[8 onwards] - Pixel data for the tile
 *
 *        The pixel data will be encoded with CoRRE encoding (if the CDL option is
 *        enabled and the client can handle it) or RAW encoding if that is smaller
 *        than CoRRE encoding for that particular tile.
 *
 * @param  packet_buffer  Buffer to store tile data
 * @return Length of generated data in bytes
 */
static int GenTileUpdateData(uint8_t *packet_buffer)
{
    uint16_t		x_pos, y_pos;
    int			i, j;
    int			tile_width, tile_height;

    /* Get the X and Y positions of this tile from the packet buffer */
    x_pos = packet_buffer[0] * 256 + packet_buffer[1];
    y_pos = packet_buffer[2] * 256 + packet_buffer[3];

    /* Get the tile width and height from the packet buffer */
    tile_width = packet_buffer[4] * 256 + packet_buffer[5];
    tile_height = packet_buffer[6] * 256 + packet_buffer[7];

    /* Create packet data using RAW encoding */
    for (i = 0; i < tile_height; i++) {
        for (j = 0; j < tile_width; j++) {
	    if (Bits_Per_Pixel == 8) {
             	packet_buffer[12 + tile_width * i + j] = PixelConvert(frame_buffer[y_pos + i][x_pos + j]);
	    } else {
		packet_buffer[12 + 2 * tile_width * i + 2*j]    = COLOUR2BYTE0(frame_buffer[y_pos + i][x_pos + j]);
		packet_buffer[12 + 2 * tile_width * i + 2*j+ 1] = COLOUR2BYTE1(frame_buffer[y_pos + i][x_pos + j]);
	    }
        }
    }

    /* Set the encoding type to raw */
    packet_buffer[8+0] = 0;
    packet_buffer[8+1] = 0;
    packet_buffer[8+2] = 0;
    packet_buffer[8+3] = 0;

    return (12 + tile_width*tile_height*(Bits_Per_Pixel/8));
}


/**
 * @brief Get message data function
 *        This function is called by the client_handler thread to get data
 *        from the client's socket.
 *
 * @param socket_fd    File descriptor of the socket to get the data from.
 * @param *buffer      Buffer to store received data in.
 * @param num_bytes    Number of bytes to attempt to get.
 *
 * @return  1 on sucessfull completion - 0 on error.
 */
static int GetMessageData(int socket_fd, char *buffer, int num_bytes)
{
    int bytes_rxd;
    int message_len = 0;

    while (message_len < num_bytes) {
        if ((bytes_rxd = recv(socket_fd, buffer, num_bytes, 0)) <= 0) {
            return 0;
        }
        message_len += bytes_rxd;
    }

    return 1;
}


void VncCls(vnc_color_t color)
{
    /* Clear the frame buffer */
    int i, j;

    for (i = 0; i < CONFIG_VNC_SERVER_FRAME_HEIGHT; i++) {
        for (j = 0; j < CONFIG_VNC_SERVER_FRAME_WIDTH; j++) {
	    VncDrawPixel(j, i, color);
        }
    }
}


void IRAM_ATTR VncDrawPixel(uint16_t x, uint16_t y, vnc_color_t color)
{
    if (x >= CONFIG_VNC_SERVER_FRAME_WIDTH || y >= CONFIG_VNC_SERVER_FRAME_HEIGHT) {
	printf("write_frame(%d, %d) tile %d/%d\n", x, y, x/TILE_SIZE, y/TILE_SIZE);
	return;
    }

    /* Set that pixel to 'colour' */
    frame_buffer[y][x] = color;

    /* Mark the tile for update */
    tile_updated[y/TILE_SIZE][x/TILE_SIZE] = 1;
}


void IRAM_ATTR VncDrawHorzLine(uint16_t x1, uint16_t x2, uint16_t y, vnc_color_t color)
{
    int i;

    /* Draw the line */
    for (i = x1; i <= x2; i++) {
	VncDrawPixel(i, y, color);
    }
}


void IRAM_ATTR VncDrawVertLine(uint16_t x, uint16_t y1, uint16_t y2, vnc_color_t color)
{
    int i;

    /* Draw the line */
    for (i = y1; i <= y2; i++) {
	VncDrawPixel(x, i, color);
    }
}


void IRAM_ATTR VncFillRect(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, vnc_color_t color)
{
    /* Draw a solid rectangle */
    int i, j;

    for (i = y1; i <= y2; i++) {
        for (j = x1; j <= x2; j++) {
	    VncDrawPixel(j, i, color);
        }
    }
}


void VncSoundBell(void)
{
    if (xSemaphoreTake(SoundBell_lock, 10) == pdTRUE) {
    	SoundBellCount++;
	xSemaphoreGive(SoundBell_lock);
    }
}


/**
 * @brief Websocket task. Created on a websocket connection.
 */
void task_WS(void *pvParameter)
{
    int			i, j, num_of_encodings;
    uint16_t		*ptr_to_uint16;
    struct strMessage   _msg;

    VNC_WS_run = true;

    while (VNC_WS_run) {

	/*
	 * Wait until something is received from the client. Exit the queue receiver
	 * each 5 milliseconds to allow to evaluate the VNC_WS_run flag and to allow
	 * a clean exit of this task.
	 */
	if (xQueueReceive(message_queue, &_msg, 5 / portTICK_PERIOD_MS) == pdTRUE) {

	    switch (_msg.message[0]) {

	    	case SET_PIXEL_FORMAT:
		    /* Check total message length */
		    if (_msg.length >= 20) {
			/* Check pixel format is as expected */
			i = 0;
			if (_msg.message[4] != BITS_PER_PIXEL) {
			    ESP_LOGI(TAG, "SetPixelFormat client wants %d bits-per-pixel", _msg.message[4]);
			    i++;
			}

			if (_msg.message[5] != PIXEL_DEPTH) {
			    ESP_LOGI(TAG, "SetPixelFormat client wants %d pixel-depth", _msg.message[5]);
			    i++;
			}

			if ((_msg.message[7] & 0x01) != TRUE_COLOUR_FLAG) {
			    ESP_LOGI(TAG, "SetPixelFormat client wants %d true-colour-flag", _msg.message[7]);
			    i++;
			}

			ptr_to_uint16 = (uint16_t *)&(_msg.message[8]);
			if (htons(*ptr_to_uint16) != Red_Max) {
			    Red_Max = htons(*ptr_to_uint16);
			    AltPixels = true;
			    ESP_LOGI(TAG, "SetPixelFormat client granted %d red-max", Red_Max);
			}

			ptr_to_uint16 = (uint16_t *)&(_msg.message[10]);
			if (htons(*ptr_to_uint16) != Green_Max) {
			    Green_Max = htons(*ptr_to_uint16);
			    AltPixels = true;
			    ESP_LOGI(TAG, "SetPixelFormat client granted %d green-max", Green_Max);
			}

			ptr_to_uint16 = (uint16_t *)&(_msg.message[12]);
			if (htons(*ptr_to_uint16) != Blue_Max) {
			    Blue_Max = htons(*ptr_to_uint16);
			    AltPixels = true;
			    ESP_LOGI(TAG, "SetPixelFormat client granted %d blue-max", Blue_Max);
			}

			if (_msg.message[14] != Red_Shift) {
			    Red_Shift = _msg.message[14];
			    AltPixels = true;
			    ESP_LOGI(TAG, "SetPixelFormat client granted %d red-shift", Red_Shift);
			}

			if (_msg.message[15] != Green_Shift) {
			    Green_Shift = _msg.message[15];
			    AltPixels = true;
			    ESP_LOGI(TAG, "SetPixelFormat client granted %d green-shift", Green_Shift);
			}

			if (_msg.message[16] != Blue_Shift) {
			    Blue_Shift = _msg.message[16];
			    AltPixels = true;
			    ESP_LOGI(TAG, "SetPixelFormat client granted %d blue-shift", Blue_Shift);
			}

			if (i) {
			    ESP_LOGI(TAG, "SetPixelFormat %d errors, disconnect client", i);
			    ESP_LOGI(TAG, "Ensure the 'Auto select' is not enabled in your vncviewer options,");
			    _msg.length = 0;
			    ws_server_remove_client(VNC_WS_num);
			}
		    }
		    break;

	    	case FIX_COLOUR_MAP_ENTRIES:
		    printf("FIX_COLOUR_MAP_ENTRIES\n");
		    break;

	    	case SET_ENCODINGS:
		    if (_msg.length >= 3) {
			// Get the total encodings
			num_of_encodings = _msg.message[2]*255 + _msg.message[3];

			/* Clear the encoding type structure */
			encoding_type.raw = 0;
			encoding_type.copy_rectangle = 0;
			encoding_type.rre = 0;
			encoding_type.corre = 0;
			encoding_type.hextile = 0;

			for (i = 0; i < num_of_encodings; i++) {
			    switch(_msg.message[3 + (i*4)]) {
				case 0:  /* Raw encoding */
					encoding_type.raw = i + 1;
					break;
				case 1:  /* Copy rectangle encoding */
					encoding_type.copy_rectangle = i + 1;
					break;
				case 2:  /* RRE encoding */
					encoding_type.rre = i + 1;
					break;
				case 4:  /* CoRRE encoding */
					encoding_type.corre = i + 1;
					break;
				case 5:  /* Hextile encoding */
					encoding_type.hextile = i + 1;
					break;
				default:  /* Unknown coding type - do nothing */
					break;
			    }
			}
		    }
		    break;

	    	case FRAME_BUFFER_UPDATE_REQ:
		    if (_msg.length == 10) {
			if (!_msg.message[1]) {
			    /* Non-incremental mode - mark the squares that need to be updated */
			    for (i = (_msg.message[2]*255 + _msg.message[3])/TILE_SIZE;
				i <= (_msg.message[2]*255 + _msg.message[3] + _msg.message[6]*255 + _msg.message[7])/TILE_SIZE;
				i++) {
				for (j = (_msg.message[4]*255 + _msg.message[5])/TILE_SIZE;
				    j <= (_msg.message[4]*255 + _msg.message[5] + _msg.message[8]*255 + _msg.message[9])/TILE_SIZE;
				    j++) {
					tile_updated[j][i] = 1;
				}
			    }
			}

			/* Signal that there is now a pending update request */
			xEventGroupSetBits(xEventGroupVNC, VNC_CLIENT_UPDATE_REQ);
		    }
		    break;

	    	case KEY_EVENT:
		    printf("KEY_EVENT\n");
		    /* Handle the key event, ignored for brewboard */
		    /* Get the remainder of message (7 bytes) */
		    break;

	    	case POINTER_EVENT:
		    /* Handle the pointer event, simulate the touch screen. */
		    if (_msg.length == 6) {
			/* Set global variables that will be read by another task. */
			VNC_pointer_button = _msg.message[1];
			VNC_pointer_x = _msg.message[2]*255 + _msg.message[3];
			VNC_pointer_y = _msg.message[4]*255 + _msg.message[5];
		    }
		    _msg.length = 0;
		    break;

	    	case CLIENT_CUT_TEXT:
		    printf("CLIENT_CUT_TEXT\n");
		    break;

	    	default:
		    ESP_LOGI(TAG, "Unknown message %d from client", _msg.message[0]);
	    }
	}
    }

    VNC_WS_num = -1;
    xEventGroupClearBits(xEventGroupVNC, VNC_CLIENT_UPDATE_REQ);
    ESP_LOGI(TAG, "task_WS finished");
    vTaskDelete(NULL);
}


int VncStartWS(int num)
{
    char		protocol_ver[8], message_buffer[MESSAGE_BUFFER_SIZE];
    int			i, ProtocolOkay;
    uint32_t		*ptr_to_uint32;
    uint16_t		*ptr_to_uint16;
    struct strMessage	_msg;

    ESP_LOGI(TAG, "Start VNC WebSocket connection %d", num);
    message_queue = xQueueCreate(message_queue_size, sizeof(struct strMessage));

    /*
     * Initial handshake
     */
    ws_server_send_bin_client(num, "RFB 003.003\n", 12);
    ProtocolOkay = 1;
    if (xQueueReceive(message_queue, &_msg, 5000 / portTICK_PERIOD_MS) == pdTRUE) {
//	dump_msg((char *)_msg.message, _msg.length, "pull");

	for (i = 0; i < 8; i++) {
	    if (_msg.message[i] != server_ProtocolVersion[i]) {
		ProtocolOkay = 0;
	    }
	    /* Store the protocol version - ignoring thr 'RFB ' part */
	    protocol_ver[i] = _msg.message[i + 4];
	}
	protocol_ver[7] = 0;
    } else {
	ESP_LOGE(TAG, "Client timeout after initial message");
	return -5;
    }
    
    if (ProtocolOkay == 0) {
	/* Generate the Bad ProtocolVerion message */
	ESP_LOGI(TAG, "Start VNC WebSocket task failed, bad protocol.");
	ptr_to_uint32 = (uint32_t *) &(message_buffer[0]);
	*ptr_to_uint32 = htonl(0);
	ptr_to_uint32 = (uint32_t *) &(message_buffer[4]);
	*ptr_to_uint32 = htonl(strlen(bad_protocol));
	strcpy(&(message_buffer[8]), bad_protocol);
	ws_server_send_bin_client(num, message_buffer, 8 + strlen(bad_protocol));
	vTaskDelay(500 / portTICK_PERIOD_MS);
	ws_server_remove_client(num);
	return -2;
    }
    /* ProtocolVerion is okay - connect with no authentication*/

    /* Server is busy with another client */
    if (VNC_WS_run || vnc_client_connected) {
	ESP_LOGI(TAG, "Start VNC WebSocket task failed, server busy.");
	ptr_to_uint32 = (uint32_t *) &(message_buffer[0]);
	*ptr_to_uint32 = htonl(0);
	ptr_to_uint32 = (uint32_t *) &(message_buffer[4]);
	*ptr_to_uint32 = htonl(strlen(server_busy));
	strcpy(&(message_buffer[8]), server_busy);
	ws_server_send_bin_client(num, message_buffer, 8 + strlen(server_busy));
	vTaskDelay(500 / portTICK_PERIOD_MS);
	ws_server_remove_client(num);
	return -1;
    }

    /* Generate the No Authentication message */
    ptr_to_uint32 = (uint32_t *) &(message_buffer[0]);
    *ptr_to_uint32 = htonl((uint32_t)1);
    ws_server_send_bin_client(num, message_buffer, 4);
    /* Authentication - end */

    /* ClientInitialisation - begin */
    /* Receive initialisation message from client (1 byte) */
    if (xQueueReceive(message_queue, &_msg, 5000 / portTICK_PERIOD_MS) == pdFALSE) {
	ESP_LOGE(TAG, "Client timeout after auth message");
	return -5;
    }
    /* Do nothing with this as we only support 1 Client at a time */
    /* ClientInitialisation - end */

    /* Initial default RGB332 */
    Red_Max     = RED_MAX;
    Green_Max   = GREEN_MAX;
    Blue_Max    = BLUE_MAX;
    Red_Shift   = RED_SHIFT;
    Green_Shift = GREEN_SHIFT;
    Blue_Shift  = BLUE_SHIFT;
    AltPixels   = false;

    /* ServerInitialisation - begin */
    /* Create Initialisation message for client */
    ptr_to_uint16 = (uint16_t *) &(message_buffer[0]);
    *ptr_to_uint16 = htons((uint16_t)CONFIG_VNC_SERVER_FRAME_WIDTH);

    ptr_to_uint16 = (uint16_t *) &(message_buffer[2]);
    *ptr_to_uint16 = htons((uint16_t)CONFIG_VNC_SERVER_FRAME_HEIGHT);

    message_buffer[4] = (uint8_t)BITS_PER_PIXEL;
    message_buffer[5] = (uint8_t)PIXEL_DEPTH;
    message_buffer[6] = (uint8_t)BIG_ENDIAN_FLAG;
    message_buffer[7] = (uint8_t)TRUE_COLOUR_FLAG;

    ptr_to_uint16 = (uint16_t *) &(message_buffer[8]);
    *ptr_to_uint16 = htons(Red_Max);

    ptr_to_uint16 = (uint16_t *) &(message_buffer[10]);
    *ptr_to_uint16 = htons(Green_Max);

    ptr_to_uint16 = (uint16_t *) &(message_buffer[12]);
    *ptr_to_uint16 = htons(Blue_Max);

    message_buffer[14] = Red_Shift;
    message_buffer[15] = Green_Shift;
    message_buffer[16] = Blue_Shift;

    ptr_to_uint32 = (uint32_t *) &(message_buffer[20]);
    *ptr_to_uint32 = htonl(strlen(desktop_name));
    strcpy(&(message_buffer[24]), desktop_name);

//  dump_msg(message_buffer, 24 + strlen(desktop_name), (char *)"send");
    ws_server_send_bin_client(num, message_buffer, 24 + strlen(desktop_name));

    /* ServerInitialisation - end */
    ESP_LOGI(TAG, "VNC WebSocket client connected (RFB Protocol Ver: %s)", protocol_ver);

    VNC_WS_num = num;
    xTaskCreate(&task_WS, "WSclient",    MIN_STACK_SIZE, NULL, 5, &xTaskWSclient);
    return 0;
}


void VncStopWS(int num)
{
    if (! VNC_WS_run) {
	ESP_LOGI(TAG, "Stop VNC WebSocket, not running.");
	return;
    }

    if (num != VNC_WS_num) {
	ESP_LOGI(TAG, "Stop VNC WebSocket, %d is running, requested %d", VNC_WS_num, num);
	return;
    }

    ESP_LOGI(TAG, "Stop VNC WebSocket task connection %d", num);
    VNC_WS_run = false;
    xQueueReset(message_queue);
}


void VncGetWSmessage(char *msg, uint16_t len)
{
    int			max;
    struct strMessage	_msg;
 
    if (len == 0)
	return;

    max = len;
    if (max > MESSAGE_BUFFER_SIZE) {
	    ESP_LOGE(TAG, "VncGetWSmessage need %d bytes in a %d length buffer", len, MESSAGE_BUFFER_SIZE);
	    max = MESSAGE_BUFFER_SIZE;
    }

    for (int i = 0; i < max; i++) {
	    _msg.message[i] = msg[i];
    }
    _msg.length = max;
    if (xQueueSendToBack(message_queue, &_msg, 200 / portTICK_PERIOD_MS) == pdFALSE) {
	ESP_LOGE(TAG, "VncGetWSmessage() Queue full, message lost");
    }
}