esptest.ino

/*
ESPTest for ZMTP + ZRE + ZOCP
Original author: Ronald Hof <ronald@k-n-p.org>
 
license: MPLv2
 */

#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include <string.h>

WiFiUDP udp;

const char* ssid     = "PerformanceEngine";
const char* password = "";
IPAddress broadcast;
IPAddress client_address;
word client_port=0;
word server_port=43434;
char client_state=0;
char server_connected=0;
char server_state=0;
char zocp_state=0;
char zocp_ready=0;
unsigned char server_flags=0,client_flags=0;
unsigned char server_len=0,client_len=0;
uint16_t client_seq=0;
const uint8_t zre_whisper[]={0x01,0x06,0xaa,0xa1,0x02,0x02};
const char uuid[]={0x7C,0x56,0xF6,0xDC,0xF8,0x2A,0x11,0xE4,0x88,0x9A,0x0C,0x32,0xB6,0x64,0xA5,0xDC};
uint8_t b[256];



WiFiServer server(server_port);
unsigned long beacontime=millis();
WiFiClient server_client;
WiFiClient client_client;

void send_beacon()
{
  udp.beginPacket(broadcast,5670);
  udp.write("ZRE");
  udp.write(0x01);
  for(char i=0;i<16;i++)
    udp.write(uuid[i]);
  udp.write(server_port>>8);
  udp.write(server_port&0x00ff);
  udp.endPacket();
}


int i;

void setup() {
  Serial.begin(115200);
  delay(10);

  // We start by connecting to a WiFi network

  Serial.println();
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  
  WiFi.begin(ssid, password);
  
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");  
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
  uint32_t localip=WiFi.localIP();
  uint32_t netmask=WiFi.subnetMask();
  uint32_t bcast=(localip&netmask)|(netmask^0xffffffff);
  broadcast=bcast;
  Serial.println(broadcast);
  //udp.begin(5670);
  server.begin();
 
}

int receive_data(WiFiClient cl,int len,uint8_t *buf)
{  
  if (cl.available()<len)
    return 0;
  for(char c=0;c<len;c++)
    buf[c]=cl.read();
  return 1;
}


char str[256];

    
void do_server()
{
  uint32_t ofs=0,ol=0;
  uint32_t sc=0,sl=0;

  char *st;
  switch(server_state)
  {
    case 0: // wait for someone to connect
      server_client=server.available();
      if(server_client) server_state=1;
      break;
      
    case 1: // got a connection, read signature
      if(receive_data(server_client,10,b))
        if((b[0]==0xff) && (b[9]==0x7f))
        {
          Serial.println("Server: valid signature found");
          b[10]=3;
          server_client.write((uint8_t *)b,11);
          server_state=2;
        }
        else
        {
          Serial.println("Server: invalid signature ,disconnecting");
          server_client.stop();server_state=0;
        }
      break;
      
    case 2: // version
      if(receive_data(server_client,1,b))
      {
        Serial.print("Server: got version:");
        Serial.println(b[0],HEX);
        if(b[0]==3)
        {
          //server_client.write((byte)3);
          server_state=3;
        }
        else
        {
          Serial.println("Server: version not 3, disconnecting");
          server_client.stop();server_state=0;
        }
      }
      break;
    case 3: // NULL handshake
      if(receive_data(server_client,53,b))
      {
        if(memcmp(&b[1],"NULL",4)==0)
        {
          Serial.print("Server: NULL handshake, minor version:");
          Serial.println(b[0],HEX);
          b[0]=0;b[5]=0;
          server_client.write((uint8_t *)b,53);
          server_state=4;
        }
        else
        {
          Serial.println("Server: unknown handshake, disconnecting");
          server_client.stop();server_state=0;
        }
      }
      break;
    case 4: // ready to receive messages, first get message flag
      if(receive_data(server_client,1,b))
      {
        server_flags=b[0];
        Serial.print("Server: Frame start - Flags:");
        if(server_flags & 1) Serial.print("MORE ");
        if(server_flags & 2) Serial.print("LONG ");
        if(server_flags & 4) Serial.print("COMMAND ");
        server_state=5;
      }
      break;
    case 5: // get message size
      if(receive_data(server_client,1,b))
      {
        server_len=b[0];
        Serial.print(" Len:");
        Serial.println(server_len,DEC);
        server_state=6;
      }
      break;
    case 6: // handle message
        if(receive_data(server_client,server_len,b))
        {
          if(server_flags & 4)
          {
            ofs=0;
            ol=b[ofs++];
            if(memcmp(&b[ofs],"READY",5)==0)
            {
              ofs+=ol;
              while(ofs<server_len)
              {
                ol=b[ofs++];
                Serial.print("        property: ");
                memcpy(str,&b[ofs],ol);
                str[ol]=0;
                Serial.print(str);
                ofs+=ol+3;
                ol=b[ofs++];
                Serial.print(" value: ");
                memcpy(str,&b[ofs],ol);
                str[ol]=0;
                Serial.println(str);
                ofs+=ol;
              }
              Serial.println(server_len,DEC);
              Serial.println(ofs,DEC);
              Serial.println("done");
              ofs=2;
              str[ofs++]=5;
              memcpy(&str[ofs],"READY",5);ofs+=5;
              str[ofs++]=11;
              memcpy(&str[ofs],"Socket-type",11);ofs+=11;
              memset(&str[ofs],0,3);ofs+=3;
              str[ofs++]=6;
              memcpy(&str[ofs],"ROUTER",6);ofs+=6;
              str[ofs++]=8;
              memcpy(&str[ofs],"Identity",8);ofs+=8;
              memset(&str[ofs],0,4);ofs+=4;
              str[0]=4;
              str[1]=ofs-2;
              server_client.write((uint8_t*)str,ofs);
              Serial.println(ofs,DEC);
            }
          }
          else
          {
            if(zocp_state==1)
            {
              Serial.print("ZOCP:");
              b[server_len]=0;
              if(strncmp((char *)&b[2],"GET",3)==0)
              {
                Serial.print("GET");
                memcpy(str,zre_whisper,6);
                str[6]=(client_seq & 0xff00) >> 8;
                str[7]=(client_seq & 0x00ff);
                client_seq++;             
                ofs=10;
                str[8]=0;
                ol=sprintf(&str[10],"{\"GET\": null}");
                ofs+=ol;
                str[9]=ofs-10;
                client_client.write((uint8_t *)str,ofs);
                
                memcpy(str,zre_whisper,6);
                str[6]=(client_seq & 0xff00) >> 8;
                str[7]=(client_seq & 0x00ff);
                client_seq++;             
                ofs=10;
                str[8]=0;
                ol=sprintf(&str[10],"{\"MOD\": {\"Output\": {\"typeHint\": \"string\", \"value\": \"\", \"subscribers\": [], \"access\": \"re\"}}}");
                ofs+=ol;
                str[9]=ofs-10;
                client_client.write((uint8_t *)str,ofs);
                zocp_ready=1;
              }
              Serial.println((char *)b);
              //handle_zocp((char *)b);
              zocp_state=0; 
            }
            if(b[0]==0xaa && b[1]==0xa1)
            {
              Serial.print("Server: ZRE:");
              int o=6;              
              switch(b[2])
              {
                case 1:
                  Serial.print("hello v:");
                  Serial.print(b[3],DEC);Serial.print(" seq:");Serial.print((int)b[4]<<8+b[5],DEC);Serial.print(" endpoint:");
                  ofs=6;
                  ol=b[ofs++];
                  memcpy(str,&b[ofs],ol);str[ol]=0;ofs+=ol;                 
                  Serial.print(str);
                  uint32_t ip[4];
                  char *c;
                 
                  //sscanf(str,"tcp://%hhu.%hhu.%hhu.%hhu:%hu",&ip[0],&ip[1],&ip[2],&ip[3],&client_port);
                  i=0;while(str[o+i]!='.') i++;str[o+i]=0;
                  ip[0]=atoi(&str[o]);o+=i+1;;
                  i=0;while(str[o+i]!='.') i++;str[o+i]=0;
                  ip[1]=atoi(&str[o]);o+=i+1;
                  i=0;while(str[o+i]!='.') i++;str[o+i]=0;
                  ip[2]=atoi(&str[o]);o+=i+1;
                  i=0;while(str[o+i]!=':') i++;str[o+i]=0;
                  ip[3]=atoi(&str[o]);o+=i+1;
                  client_port=atoi(&str[o]);
                  client_address=(ip[3]<<24)+(ip[2]<<16)+(ip[1]<<8)+ip[0];
                  client_state=1;
                  Serial.print(" groups (");
                  sc=(b[ofs]<<24)+(b[ofs+1]<<16)+(b[ofs+2]<<8)+(b[ofs+3]);ofs+=4;
                  Serial.print(sc,DEC);Serial.print("):");
                  i=0;
                  while(i<sc)
                  {
                    sl=(b[ofs]<<24)+(b[ofs+1]<<16)+(b[ofs+2]<<8)+(b[ofs+3]);ofs+=4;
                    memcpy(str,&b[ofs],sl);str[sl]=0;ofs+=sl;
                    Serial.print(str); 
                    i++;                 
                  }
                  Serial.print(" status:");
                  Serial.print(b[ofs++],DEC);
                  Serial.print(" name:");
                  ol=b[ofs++];
                  memcpy(str,&b[ofs],ol);ofs+=ol;str[ol]=0;
                  Serial.print(str);
                  Serial.print(" Headers (");
                  sc=(b[ofs]<<24)+(b[ofs+1]<<16)+(b[ofs+2]<<8)+(b[ofs+3]);ofs+=4;
                  Serial.print(sc,DEC);Serial.print("):");
                  i=0;
                  while(i<sc)
                  {
                    ol=b[ofs++];
                    memcpy(str,&b[ofs],ol);str[ol]=0;ofs+=ol;
                    Serial.print(str);Serial.print(",");
                    sc=(b[ofs]<<24)+(b[ofs+1]<<16)+(b[ofs+2]<<8)+(b[ofs+3]);ofs+=4;
                    memcpy(str,&b[ofs],sc);str[sc]=0;ofs+=sc;
                    Serial.print(str);Serial.print(" ");
                    i++;
                  }
                  Serial.print(server_len,DEC);
                  Serial.print(" ");Serial.println(ofs,DEC);
                  break;
                case 2:
                  Serial.println("whisper");
                  zocp_state=1; // next message is ZOCP;
                  break;                
              }
            }
            Serial.print("Server msg:");
            for(i=0;i<server_len;i++)
              {
                if(b[i]>=32 && b[i]<=126) Serial.write(b[i]); else {Serial.print(" 0x");Serial.print(b[i],HEX);Serial.print(" ");}
              }
          }
          Serial.println("");
          server_state=4; // ready for the next message
        }
      break;      
  }         
}

void do_client()
{
  char c;
  int i;
  int ofs,ol;
  char s[32];
  switch(client_state)
  {
    case 0:
      break;
    case 1: // connect and send signature
      if(client_client.connect(client_address,client_port))
      {
        Serial.print("Client: Connected to ");
        Serial.print(client_address);
        Serial.print(":");
        Serial.println(client_port);
        memset(str,0,10);
        str[0]=0xff;str[9]=0x7f;
        client_client.write((uint8_t*)str,10);
        client_state=2;
      }
      break;
    case 2: // receive signature, send version
      if(receive_data(client_client,10,b))
        if((b[0]==0xff) && (b[9]==0x7f))
        {
          Serial.println("Client: valid signature found");
          client_client.write(0x03);
          client_state=3;
        }
        else
        {
          Serial.println("Client: invalid signature ,disconnecting");
          client_client.stop();client_state=0;
        }
      break;
    case 3: // check version, send NULL handshake;
      if(receive_data(client_client,1,b))
        if(b[0]==3)
        {
          memset(str,0,53);
          memcpy(&str[1],"NULL",4);
          client_client.write((uint8_t*)str,53);
          client_state=4;
        }
        else
        {
          Serial.println("Client: version not 3, disconnecting");
          client_client.stop();client_state=0;
        }
      break;
    case 4://receive handshake, send READY message
      if(receive_data(client_client,53,b))
        if(memcmp(&b[1],"NULL",4)==0)
        {
          Serial.print("Client: NULL handshake, minor version:");
          Serial.println(b[0],HEX);
          ofs=2;
          ol=0;
          str[0]=0x04;
          str[ofs++]=5;
          memcpy(&str[ofs],"READY",5);ofs+=5;
          str[ofs++]=0x0b;
          memcpy(&str[ofs],"Socket-Type",11);ofs+=11;
          memset(&str[ofs],0,3);ofs+=3;
          str[ofs++]=6;
          memcpy(&str[ofs],"DEALER",6);ofs+=6;
          str[ofs++]=8;
          memcpy(&str[ofs],"Identity",8);ofs+=8;
          memset(&str[ofs],0,3);ofs+=3;
          str[ofs++]=17;
          str[ofs++]='1';
          memcpy(&str[ofs],uuid,16);ofs+=16;
          str[1]=ofs-2;
          client_client.write((uint8_t*)str,ofs);          
          client_state=5;          
        }
        else
        {
          Serial.println("Client: unknown handshake, disconnecting");
          client_client.stop();client_state=0;
        }
      break;
    case 5: // ready to receive messages
      if(receive_data(client_client,1,b))
      {
        client_flags=b[0];
        Serial.print("Client: Frame start - Flags:");
        if(client_flags & 1) Serial.print("MORE ");
        if(client_flags & 2) Serial.print("LONG ");
        if(client_flags & 4) Serial.print("COMMAND ");
        client_state=6;
      }
      break;
    case 6:
      if(receive_data(client_client,1,b))
      {
        client_len=b[0];
        Serial.print(" Len:");
        Serial.println(server_len,DEC);
        client_state=7;
      }
      break;
    case 7:
      if(receive_data(client_client,client_len,b))
      {
        if(client_flags & 4)
        {
          ofs=0;
          ol=b[ofs++];
          if(memcmp(&b[ofs],"READY",5)==0)
          {
            ofs+=ol;
            while(ofs<client_len)
            {
              ol=b[ofs++];
              Serial.print("        property: ");
              memcpy(str,&b[ofs],ol);
              str[ol]=0;
              Serial.print(str);
              ofs+=ol+3;
              ol=b[ofs++];
              Serial.print(" value: ");
              memcpy(str,&b[ofs],ol);
              str[ol]=0;
              Serial.println(str);
              ofs+=ol;
            }
            // send ZRE hello
            ofs=2;
            str[0]=0;
            str[ofs++]=0xaa;str[ofs++]=0xa1; // ZRE signature
            str[ofs++]=1; // hello
            str[ofs++]=2; // version      
            str[ofs++]=0;str[ofs++]=1;client_seq=1; // seq
            uint32_t ip=client_address;
            ol=sprintf(s,"tcp://%d.%d.%d.%d,%d",(ip & 0x000000ff),(ip & 0x0000ff00)>>8,(ip & 0x00ff0000)>>16,(ip & 0xff000000)>>24,client_port);
            str[ofs++]=ol;
            memcpy(&str[ofs],s,ol);ofs+=ol; // endpoint;
            memset(&str[ofs],0,3);ofs+=3;
            str[ofs++]=1; // 1 group
            memset(&str[ofs],0,3);ofs+=3;
            str[ofs++]=4;
            memcpy(&str[ofs],"ZOCP",4);ofs+=4; // group name
            str[ofs++]=1; // group status;
            ol=sprintf(s,"ESP output"); // sender public name
            str[ofs++]=ol;
            memcpy(&str[ofs],s,ol);ofs+=ol;
            memset(&str[ofs],0,3);ofs+=3;
            str[ofs++]=1;
            str[ofs++]=6;
            memcpy(&str[ofs],"X-ZOCP",6);ofs+=6;
            memset(&str[ofs],0,3);ofs+=3;
            str[ofs++]=1;
            str[ofs++]='1';
            str[1]=ofs-2;
            client_client.write((uint8_t*)str,ofs);
            client_seq++;        
          }
        }
       client_state=5;
       Serial.print("Client msg: ");
       for(i=0;i<client_len;i++)
       {
         if(b[i]>=32 && b[i]<=126) Serial.write(b[i]); else {Serial.print(" 0x");Serial.print(b[i],HEX);Serial.print(" ");}
       }
       Serial.println("");

      }
      break;

  }
  
}


void loop() {

  if(millis()>beacontime)
  {
    send_beacon();
    beacontime=millis()+1000;

    if(zocp_ready!=0)
    {
      
      memcpy(str,zre_whisper,6);
      str[6]=(client_seq & 0xff00) >> 8;
      str[7]=(client_seq & 0x00ff);
      client_seq++;             
      int ofs=10;
      str[8]=0;
      ofs+=sprintf(&str[10],"{\"SIG\": [\"Output\", \"T%d\"]}",millis()/1000);
      str[9]=ofs-10;
      client_client.write((uint8_t *)str,ofs);
    }
  }

    
  do_server();
  do_client();
}

The serial console of the ESP8266, on the other side a ZOCP server is running:

Connecting to ZeroMQ
.......
WiFi connected
IP address: 192.168.1.36
192.168.1.255
Server: valid signature found
Server: got version:3
Server: NULL handshake, minor version:0
Server: Frame start - Flags:COMMAND Len:58
IP address: 192.168.1.36
192.168.1.255
Server: valid signature found
Server: got version:3
Server: NULL handshake, minor version:0
Server: Frame start - Flags:COMMAND Len:58
property: Socket-Type value: DEALER
property: Identity value: 1��mH��N��U�]:]�
58
58
done
43