Zero Touch Provisioning Based on TLS 1.3

#include <MKRGSM.h>
#include <ArduinoUniqueID.h>
#include <ArduinoJson.h>

// LED (Config)
#define PIN_LED  7

// number of loops to wait for the end of initialization 
#define TECHNOLOGY_INIT_WAITING  7

#define LEN_MODEM_AT  4
const unsigned char MODEM_AT[LEN_MODEM_AT] = {'a', 't', '\r', '\n'};


#define LEN_MODEM_AT_CREG  10
const unsigned char MODEM_AT_CREG[LEN_MODEM_AT_CREG] = {'a', 't', '+', 'c', 'r', 'e', 'g', '?', '\r', '\n'};

#define LEN_MODEM_CREG  8
const char MODEM_CREG[LEN_MODEM_CREG] = {'+', 'C', 'R', 'E', 'G', ':', ' ', '\0'};

#define LEN_MODEM_AT_CSIM  8
const unsigned char MODEM_AT_CSIM[LEN_MODEM_AT_CSIM] = {'a', 't', '+', 'c', 's', 'i', 'm', '='};

#define LEN_MODEM_CSIM  8
const char MODEM_CSIM[LEN_MODEM_CSIM] = {'+', 'C', 'S', 'I', 'M', ':', ' ', '\0'};

#define LEN_APDU_HEADER  5

const char APDU_MANAGE_CHANNEL[LEN_APDU_HEADER] = { 0x00, 0x70, 0x00, 0x00, 0x00 };

const char APDU_SELECT[LEN_APDU_HEADER] = { 0x00, (char)0xA4, 0x04, 0x0C, 0x00 };

#define APDU_ISO_CLA  0x00

#define APDU_PUT_DATA_INS  0xda
#define APDU_PUT_DATA_P1  0x02

#define APDU_GET_DATA_INS  0xca
#define APDU_GET_DATA_MODE_REQUEST  0x00
#define APDU_GET_DATA_MODE_DATA  0x01

#define APDU_GET_STATUS_INS  0xcc
#define APDU_GET_STATUS_MODE_GET  0x02
#define APDU_GET_STATUS_LEN_GET  0x04

#define APDU_GET_RESPONSE_INS  0xc0


#define STATUS_RECEIVE_DONE  0x0003
#define STATUS_RECEIVE_DATA  0x0007
#define STATUS_RECEIVE_ERROR  0x000f
#define LEN_AID_SAFE2  12
const char AID_SAFE2[LEN_AID_SAFE2] = {(char)0xf0, 0x70, 0x6F, 0x64, 0x67, 0x73, 0x61, 0x66, 0x65, 0x32, 0x01, 0x01 };

#define LEN_MODEM_OK  4
const char MODEM_OK[LEN_MODEM_OK] = {'O', 'K', '\r', '\0'};

#define LEN_MODEM_ERROR  7
const char MODEM_ERROR[LEN_MODEM_ERROR] = {'E', 'R', 'R', 'O', 'R', '\r', '\0'};

#define LEN_1_CHAR_MAX  10
#define LEN_2_CHARS_MAX  100

#define CR   '\r'
#define EOL  '\n'

// IO buffer used for AT commands proceeding, size = header + 256 * 2
#define LEN_IO_BUFFER  600
char buf[LEN_IO_BUFFER];

#define LEN_DATA_BUF  4
unsigned char dataBuf[LEN_DATA_BUF];

unsigned char apdu_header[LEN_APDU_HEADER];

unsigned char regFlag;

#define STATE_READY  0x00
#define STATE_REQUEST_SENT  0x01
#define STATE_REQUEST_DATA  0x02
#define STATE_DONE  0x03

#define LEN_DEVICE_ID_MAX  16
unsigned char id[LEN_DEVICE_ID_MAX];

char gState;
char gChan;

#define JSON_CONFIG_CAPACITY  384


//DynamicJsonDocument cfg(JSON_CONFIG_CAPACITY);
StaticJsonDocument<192> cfg;


char registered(char * resp) {
  char b = *resp;
  b &= 0x0f;
  resp++;
  if (*resp != CR) {
    // 2-chars <stat> value
    b *= 10;
    b += *resp - 0x30;
  }
  switch (b) {
    case 1: // 1: registered, home network
    case 5: // 5: registered, roaming
      return true;
    
    default:
      return false;
  }
}

void logData(char * dataBuf, char len) {
  int dataLen = (unsigned char)len;
  int blockLen;
  int ofs = 0;
  while (dataLen > 0) {
    blockLen = Serial.availableForWrite();
    if (blockLen < 1) {
      delay(1);
      continue;
    }
    if (blockLen > dataLen) {
      blockLen = dataLen;
    }
    Serial.write(&dataBuf[ofs], blockLen);
    ofs += blockLen;
    dataLen -= blockLen;
  }
  Serial.flush();
}


void atCommand(char * cmdBuf, short len) {
  short cmdLen = len;
  short blockLen;
  short ofs = 0;
  while (cmdLen > 0) {
    blockLen = SerialGSM.availableForWrite();
    if (blockLen < 1) {
      delay(1);
      continue;
    }
    if (blockLen > cmdLen) {
      blockLen = cmdLen;
    }
    SerialGSM.write(&cmdBuf[ofs], blockLen);
    ofs += blockLen;
    cmdLen -= blockLen;
  }
  SerialGSM.flush();
}

#define CNTR_MAX  10000

short atResponse(char * respBuf) {
  // read AT response
  short cntr = 0;
  char * ptr;
  short len = 0;
  short blockLen;

  // initialize for timeout
  respBuf[0] = 0;
  do {
    blockLen = SerialGSM.readBytesUntil(EOL, &respBuf[len], (int)(LEN_IO_BUFFER - len));
    if (blockLen > 0) {
      len += blockLen;
      respBuf[len++] = EOL;
      respBuf[len] = 0;
    }

    ptr = strstr(respBuf, MODEM_OK);
    if (ptr == NULL) {
      ptr = strstr(respBuf, MODEM_ERROR);
    }
    
  } while (ptr == NULL);
  return len;
}

char decimalPut(int v, char * dst) {
  char ofs = 0;
  // number of chars populated
  char cnt = 1;
  char b;
  // safety check - shall be redesigned for negative numbers
  if (v < 0) {
    v = 0;
  }
  if (v >= LEN_1_CHAR_MAX) {
    ofs++;
    cnt++;
  }
  if (v >= LEN_2_CHARS_MAX) {
    ofs++;
    cnt++;
  }
  // at least 1 digit shall be populated
  do {
    b = (char)(v % 10);
    dst[ofs--] = '0' + b;
    v -= b;
    v /= 10;
  } while (v > 0);
  return cnt;
}

#define LEN_HEX  16
const char HEX_CONVERSION[LEN_HEX] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};

void hexPut(char b, char * dst) {
  dst[0] = HEX_CONVERSION[(b >> 4) & 0x0f];
  dst[1] = HEX_CONVERSION[b & 0x0f];
}

#define LEN_BYTE_HEX  2

short buildAtCsim(char * cmdBuf, char cla, char * apduBuf, char * dataBuf, short dataLen) {
  short len = LEN_APDU_HEADER; 
  short ofs = 0;
  if (dataBuf != NULL) {
    len += dataLen;
  }
  memcpy(cmdBuf, MODEM_AT_CSIM, LEN_MODEM_AT_CSIM);
  ofs = LEN_MODEM_AT_CSIM;
  ofs += decimalPut(len << 1, &cmdBuf[ofs]);
  cmdBuf[ofs++] = ',';
  cmdBuf[ofs++] = '"';
  
  // put CLA
  hexPut(apduBuf[0] | cla, &cmdBuf[ofs]);
  ofs += LEN_BYTE_HEX;
  // put INS, P1, P2
  for (char i=1; i<4; i++) {
    hexPut(apduBuf[i], &cmdBuf[ofs]);
    ofs += LEN_BYTE_HEX;
  }
  // put LEN
  hexPut(dataLen, &cmdBuf[ofs]);
  ofs += LEN_BYTE_HEX;
  if ((dataLen > 0) && (dataBuf != NULL)) {
    // put Data
    for (short i=0; i<dataLen; i++) {
      hexPut(dataBuf[i], &cmdBuf[ofs]);
      ofs += LEN_BYTE_HEX;
    }
  }
  
  cmdBuf[ofs++] = '"';
  cmdBuf[ofs++] = '\r';
  cmdBuf[ofs++] = '\n';
  cmdBuf[ofs] = 0;
  return ofs;
}

#define RES_OK  0x00
#define RES_INVALID_CSIM_RESPONSE  0x01
#define RES_INVALID_OPEN_CHANNEL_RESPONSE  0x02
#define RES_INVALID_CHANNEL_ID  0x03
#define RES_UNEXPECTED_GET_STATUS_RESPONSE  0x04
#define RES_INVALID_GET_STATUS_RESPONSE  0x05
#define RES_UNEXPECTED_GET_DATA_RESPONSE  0x06
#define RES_INVALID_GET_DATA_RESPONSE  0x07
#define RES_JSON_DESERIALIZATION_FAILED  0x08

#define OFS_APDU_CLA  0x00
#define OFS_APDU_INS  0x01
#define OFS_APDU_P1  0x02
#define OFS_APDU_P2  0x03
#define OFS_APDU_LEN  0x05

#define MODE_CHANNEL_CLOSE  0x80

#define TAG_DEVICE_ID  0xc0
#define TAG_DEVICE_DATA 0xc1

char simChannelOpen(char * chan) {
  // put AT SIM command: Open Supplementary Logical Channel
  short cmdLen = buildAtCsim(buf, 0, (char *)APDU_MANAGE_CHANNEL, (char *)NULL, 1);
  atCommand(buf, cmdLen);
  short respLen = atResponse(buf);
  logData(buf, respLen);
  // analyze response
  char * ptr;
  ptr = strstr(buf, MODEM_CSIM);
  if (ptr == NULL) {
    Serial.println("CSIM not detected in the response");
    return RES_INVALID_CSIM_RESPONSE;
  }

  if ((ptr[LEN_MODEM_CSIM - 1] != '6') ||
      (ptr[LEN_MODEM_CSIM + 0] != ',') || 
      (ptr[LEN_MODEM_CSIM + 1] != '"') ||
      (ptr[LEN_MODEM_CSIM + 2] != '0')) {

    Serial.println("Unexpected response for MANAGE CHANNEL");
    return RES_INVALID_OPEN_CHANNEL_RESPONSE;
  }

  // extract channel ID
  char ch = ptr[LEN_MODEM_CSIM + 3] - '0';
  if ((ch <= 0) || (ch >= 4)) {
    Serial.print("Unexpected response (chan ID) for MANAGE CHANNEL: ");
    Serial.println(ch);
    return RES_INVALID_CHANNEL_ID;
  }
  *chan = ch;
  return RES_OK;
}

char simChannelClose(char chan) {
  // close supplementary logical channel
  memcpy(apdu_header, APDU_MANAGE_CHANNEL, LEN_APDU_HEADER);
  apdu_header[OFS_APDU_P1] = MODE_CHANNEL_CLOSE;
  apdu_header[OFS_APDU_P2] = chan;
  short cmdLen = buildAtCsim(buf, 0, (char *)apdu_header, (char *)NULL, 0);
  atCommand(buf, cmdLen);
  short respLen = atResponse(buf);
  logData(buf, respLen);
  return RES_OK;
}

char simSelectSafe2(char chan) {
  // SELECT (by AID) SAFE2
  short cmdLen = buildAtCsim(buf, chan, (char *)APDU_SELECT, (char *)AID_SAFE2, LEN_AID_SAFE2);
  atCommand(buf, cmdLen);
  short respLen = atResponse(buf);
  logData(buf, respLen);
  return RES_OK;
}

char simGetData(char chan, char mode, unsigned char * buf, short * len) {
  // PUT DATA (from dataBuffer)

  char bh, bl;
  
  apdu_header[OFS_APDU_CLA] = APDU_ISO_CLA;
  apdu_header[OFS_APDU_INS] = APDU_GET_DATA_INS;
  apdu_header[OFS_APDU_P1] = mode;
  apdu_header[OFS_APDU_P2] = 0x00;
  apdu_header[OFS_APDU_LEN] = 0x00;
  short cmdLen = buildAtCsim((char *)buf, chan, (char *)apdu_header, NULL, 0);
  atCommand((char *)buf, cmdLen);
  short respLen = atResponse((char *)buf);

  if (mode == APDU_GET_DATA_MODE_DATA) {
    // analyze response
    char * ptr;
    ptr = strstr((const char *)buf, MODEM_CSIM);
    if (ptr == NULL) {
      Serial.println("CSIM not detected in the response");
      return RES_INVALID_CSIM_RESPONSE;
    }
  
    // expected response: 4,"61xx"
    if ((ptr[LEN_MODEM_CSIM - 1] == '4') &&
        (ptr[LEN_MODEM_CSIM + 0] == ',') && 
        (ptr[LEN_MODEM_CSIM + 1] == '"') &&
        (ptr[LEN_MODEM_CSIM + 2] == '6') && 
        (ptr[LEN_MODEM_CSIM + 3] == '1')) {
  
      bh = hex2val(ptr[LEN_MODEM_CSIM + 4]);
      bl = hex2val(ptr[LEN_MODEM_CSIM + 5]);
      if ((bh == -1) || (bl == -1)) {
        Serial.println("Invalid response for GET DATA (Data)");
        return RES_INVALID_GET_DATA_RESPONSE;
      }
      unsigned char len = (unsigned char)((bh << 4) | bl);
      Serial.print("expected data length: ");
      Serial.println(len, HEX);
    
      apdu_header[OFS_APDU_CLA] = APDU_ISO_CLA;
      apdu_header[OFS_APDU_INS] = APDU_GET_RESPONSE_INS;
      apdu_header[OFS_APDU_P1] = 0x00;
      apdu_header[OFS_APDU_P2] = 0x00;
      apdu_header[OFS_APDU_LEN] = len;
      cmdLen = buildAtCsim((char *)buf, chan, (char *)apdu_header, NULL, len);
      atCommand((char *)buf, cmdLen);
      respLen = atResponse((char *)buf);
      logData((char *)buf, respLen);
    
      ptr = strstr((const char *)buf, MODEM_CSIM);
      if (ptr == NULL) {
        Serial.println("CSIM not detected in the response (GR)");
        return RES_INVALID_CSIM_RESPONSE;
      }
    }    
    short hlen = 0;
    short ofs = LEN_MODEM_CSIM - 1;
    char b;
    do {
      b = ptr[ofs++] - 0x30;
      if ((b < 0) || (b > 9)) {
        Serial.println("CSIM not detected in the response (GR)");
        return RES_INVALID_CSIM_RESPONSE;
      }
      hlen *= 10; // shift left for 1 tetrade
      hlen += b;
    } while (ptr[ofs] != ',');
    // skip ',' and '"'
    ofs += 2;
    hlen >>= 1; // number of bytes
    hlen -= 2; // exclude SW
    Serial.print("received:");
    Serial.println(hlen);
    for (short ii=0; ii<hlen; ii++) {
      bh = hex2val(ptr[ofs]);
      bl = hex2val(ptr[ofs + 1]);
      if ((bh == -1) || (bl == -1)) {
        Serial.print("Invalid charcters in the response (GR): ");
        return RES_INVALID_CSIM_RESPONSE;
      }
      ofs += 2;
      buf[ii] = (unsigned char)((bh << 4) | bl);
    }
    buf[hlen] = 0;
    // re-assign response length
    respLen = hlen;
  }
  logData((char *)buf, respLen);
  Serial.println();
  *len = respLen;
  return RES_OK;
}

char hex2val(char h) {
  for (char i=0; i<LEN_HEX; i++) {
    if (HEX_CONVERSION[i] == h) {
      return i;
    }
  }
  return -1;
}

unsigned short chars2short(char * buf) {
  unsigned char b; 
  unsigned short w = 0;
  for (char i=0; i<4; i++) {
    b = hex2val(buf[i]);
    //Serial.print(b);
    if (b < 0) {
      return -1;
    }
    w <<= 4;
    w += b;
  }
  return w;
}

char simGetStatus(char chan, char mode, unsigned char * buf, short * stat, short * data_len) {
  // PUT DATA (from dataBuffer)
  
  apdu_header[OFS_APDU_CLA] = APDU_ISO_CLA;
  apdu_header[OFS_APDU_INS] = APDU_GET_STATUS_INS;
  apdu_header[OFS_APDU_P1] = mode;
  apdu_header[OFS_APDU_P2] = 0x00;
  apdu_header[OFS_APDU_LEN] = 0x00;
  short cmdLen = buildAtCsim((char *)buf, chan, (char *)apdu_header, NULL, 0);
  atCommand((char *)buf, cmdLen);
  short respLen = atResponse((char *)buf);
  logData((char *)buf, respLen);

  // analyze response
  char * ptr;
  ptr = strstr((const char *)buf, MODEM_CSIM);
  if (ptr == NULL) {
    Serial.println("CSIM not detected in the response (GS)");
    return RES_INVALID_CSIM_RESPONSE;
  }

  // expected response: 4,"6104"
  if ((ptr[LEN_MODEM_CSIM - 1] == '4') &&
      (ptr[LEN_MODEM_CSIM + 0] == ',') && 
      (ptr[LEN_MODEM_CSIM + 1] == '"') &&
      (ptr[LEN_MODEM_CSIM + 2] == '6') && 
      (ptr[LEN_MODEM_CSIM + 3] == '1') &&
      (ptr[LEN_MODEM_CSIM + 4] == '0') && 
      (ptr[LEN_MODEM_CSIM + 5] == '4')) {

    apdu_header[OFS_APDU_CLA] = APDU_ISO_CLA;
    apdu_header[OFS_APDU_INS] = APDU_GET_RESPONSE_INS;
    apdu_header[OFS_APDU_P1] = 0x00;
    apdu_header[OFS_APDU_P2] = 0x00;
    apdu_header[OFS_APDU_LEN] = APDU_GET_STATUS_LEN_GET;
    cmdLen = buildAtCsim((char *)buf, chan, (char *)apdu_header, NULL, APDU_GET_STATUS_LEN_GET);
    atCommand((char *)buf, cmdLen);
    respLen = atResponse((char *)buf);
    logData((char *)buf, respLen);
  
    ptr = strstr((const char *)buf, MODEM_CSIM);
    if (ptr == NULL) {
      Serial.println("CSIM not detected in the response (GR)");
      return RES_INVALID_CSIM_RESPONSE;
    }
  }
  
  // expected response: [receiveStatus:2][receiveLength:2][SW:2] => 12 chars
  if ((ptr[LEN_MODEM_CSIM - 1] != '1') ||
      (ptr[LEN_MODEM_CSIM + 0] != '2') || 
      (ptr[LEN_MODEM_CSIM + 1] != ',') ||
      (ptr[LEN_MODEM_CSIM + 2] != '"')) {

    Serial.println("Unexpected response for GET STATUS (Receive)");
    return RES_UNEXPECTED_GET_STATUS_RESPONSE;
  }

  // extract status & length
  //Serial.println("status");
  short wStatus = chars2short(&ptr[LEN_MODEM_CSIM + 3]);
  if (wStatus < 0) {
    Serial.println("Invalid response for GET STATUS (Receive)");
    return RES_INVALID_GET_STATUS_RESPONSE;
  }
  //Serial.println("length");
  short wLength = chars2short(&ptr[LEN_MODEM_CSIM + 3 + 4]);
  if (wLength < 0) {
    Serial.println("Invalid response for GET STATUS (Receive)");
    return RES_INVALID_GET_STATUS_RESPONSE;
  }
  //Serial.println();
  *stat = wStatus;
  *data_len = wLength;
  return RES_OK;
}


char send(unsigned char tag, unsigned char * dataPtr, short dataLen) {

  short cmdLen = 0;
  short respLen = 0;
  char chan = 0;
  // put AT SIM command: Open Supplementary Logical Channel
  cmdLen = buildAtCsim((char *)buf, 0, (char *)APDU_MANAGE_CHANNEL, NULL, 1);
  atCommand((char *)buf, cmdLen);
  respLen = atResponse((char *)buf);
  logData((char *)buf, respLen);
  // analyze response
  char * ptr;
  ptr = strstr((const char *)buf, MODEM_CSIM);
  if (ptr == NULL) {
    Serial.println("CSIM not detected in the response");
    return RES_INVALID_CSIM_RESPONSE;
  }

  if ((ptr[LEN_MODEM_CSIM - 1] != '6') ||
      (ptr[LEN_MODEM_CSIM + 0] != ',') || 
      (ptr[LEN_MODEM_CSIM + 1] != '"') ||
      (ptr[LEN_MODEM_CSIM + 2] != '0')) {

    Serial.println("Unexpected response for MANAGE CHANNEL");
    return RES_INVALID_OPEN_CHANNEL_RESPONSE;
  }

  // extract channel ID
  chan = ptr[LEN_MODEM_CSIM + 3] - '0';
  if ((chan <= 0) || (chan >= 4)) {
    Serial.print("Unexpected response (chan ID) for MANAGE CHANNEL: ");
    Serial.println(chan);
    return RES_INVALID_CHANNEL_ID;
  }
  // SELECT (by AID) SAFE2
  cmdLen = buildAtCsim((char *)buf, chan, (char *)APDU_SELECT, (char *)AID_SAFE2, LEN_AID_SAFE2);
  atCommand((char *)buf, cmdLen);
  respLen = atResponse((char *)buf);
  logData((char *)buf, respLen);
  
  // PUT DATA (from dataBuffer)
  char apdu_header[LEN_APDU_HEADER];
  apdu_header[OFS_APDU_CLA] = APDU_ISO_CLA;
  apdu_header[OFS_APDU_INS] = APDU_PUT_DATA_INS;
  apdu_header[OFS_APDU_P1] = APDU_PUT_DATA_P1;
  apdu_header[OFS_APDU_P2] = tag;
  apdu_header[OFS_APDU_LEN] = 0x00;
  cmdLen = buildAtCsim(buf, chan, (char *)apdu_header, (char *)dataPtr, dataLen);
  atCommand(buf, cmdLen);
  respLen = atResponse(buf);
  logData(buf, respLen);
  
  // close supplementary logical channel
  memcpy(apdu_header, APDU_MANAGE_CHANNEL, LEN_APDU_HEADER);
  apdu_header[OFS_APDU_P1] = MODE_CHANNEL_CLOSE;
  apdu_header[OFS_APDU_P2] = chan;
  cmdLen = buildAtCsim(buf, 0, apdu_header, NULL, 0);
  atCommand(buf, cmdLen);
  respLen = atResponse(buf);
  logData(buf, respLen);
  return RES_OK;
}

#define LEN_NAME_CONTROL_LED  9
const char NAME_CONTROL_LED[LEN_NAME_CONTROL_LED] = { 'i', 'o', 't', ':', 'A', 'l', 'a', 'r', 'm' };

#define LEN_NAME_EFFECT_ON  2
const char NAME_EFFECT_ON[LEN_NAME_EFFECT_ON] = { 'O', 'n' };

char configParseApply(const char * buf, short dataLen) {
  
  DeserializationError error = deserializeJson(cfg, buf, dataLen);
  
  if (error) {
    Serial.print(F("deserializeJson() failed: "));
    Serial.println(error.f_str());
    return RES_JSON_DESERIALIZATION_FAILED;
  }
  
  const char* cfg_version = cfg["configuration"]["version"]; // "2020-10-13"
  Serial.print("configuration version: ");
  Serial.println(cfg_version);
  
  const char* cfg_config_0_action = cfg["configuration"]["config"][0]["action"]; // "iot:Alarm"
  const char* cfg_config_0_effect = cfg["configuration"]["config"][0]["effect"]; // "On"

  if (memcmp(cfg_config_0_action, NAME_CONTROL_LED, LEN_NAME_CONTROL_LED) == 0) {
    if (memcmp(cfg_config_0_effect, NAME_EFFECT_ON, LEN_NAME_EFFECT_ON) == 0) {
      Serial.println("iot:Alarm configured as ON");
      digitalWrite(PIN_LED, HIGH);
    } else {
      Serial.println("iot:Alarm configured as OFF");
      digitalWrite(PIN_LED, LOW);
    }
  }
  
}

void setup() {
  // initialize digital pin LED (#7) as an output
  pinMode(PIN_LED, OUTPUT);
  
  // reset the ublox module
  pinMode(GSM_RESETN, OUTPUT);
  digitalWrite(GSM_RESETN, HIGH);
  delay(100);
  digitalWrite(GSM_RESETN, LOW);
  delay(300);

  // Open serial communications and wait for port to open:
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect
  }
  Serial.println("MKR AT");
  // put Device ID
  short sz = 0;
  for (short i=0; i<UniqueIDsize; i++) {
    hexPut(UniqueID[i], &buf[sz]);
    sz += 2;
  }
  logData(buf, sz);
  Serial.println();
  // initialize modem AT interface
  SerialGSM.begin(9600);

  regFlag = false;
  char ready = 0;
  unsigned char b = 0;
  unsigned char pos = 0;
  do {
    // put AT command
    logData((char *)MODEM_AT, LEN_MODEM_AT);
    SerialGSM.write(MODEM_AT, LEN_MODEM_AT);
    delay(100);
    // modem could not respond during initialization
    if (SerialGSM.available()) {
      ready = 1;
      while (SerialGSM.available()) {
        b = SerialGSM.read();
        Serial.write(b);
        delay(10);
      }
    }
    delay(300);
  } while (ready == 0);

  unsigned char cntr = 0;
  while (cntr < TECHNOLOGY_INIT_WAITING) {
    // wait for a half of a second  
    delay(500);                       

    // put AT command
    logData((char *)MODEM_AT_CREG, LEN_MODEM_AT_CREG);
    atCommand((char *)MODEM_AT_CREG, LEN_MODEM_AT_CREG);
    // read AT response
    pos = atResponse(buf);
    
    // analyze response
    char * ptr;
    ptr = strstr(buf, MODEM_CREG);
    if (ptr != NULL) {
      regFlag = registered(&ptr[LEN_MODEM_CREG + 1]);
      Serial.write(ptr[LEN_MODEM_CREG - 1]);
      Serial.write(':');
      Serial.write(ptr[LEN_MODEM_CREG + 1]);
      Serial.println();
    } else {
      Serial.println("not found `+CREG`");
    }
    if (regFlag) {
      cntr++;
    }
    Serial.print("cntr: ");
    Serial.println(cntr);
    // wait 1 sec before repeat
    delay(1000);
  }
  Serial.println("setup finished");

  
  // put Device ID
  short idLen = UniqueIDsize;
  if (idLen > LEN_DEVICE_ID_MAX) {
    idLen = LEN_DEVICE_ID_MAX;
  }
  memcpy(id, (void *)UniqueID, idLen);
  char res = send(TAG_DEVICE_ID, &id[0], idLen);
  if (res == RES_OK) {
    Serial.println("Set Device ID: OK");
  } else {
    Serial.println("Set Device ID: ERROR");
  }
  
  gState = STATE_READY;
}

void loop() {

  unsigned char secs;
  short dataLen = 0;
  short receiveState, receiveRes;
  char rv;


  if (gState == STATE_READY) {
    rv = simChannelOpen(&gChan);
    if (rv == RES_OK) {
      rv = simSelectSafe2(gChan);
    }
    if (rv == RES_OK) {
      rv = simGetData(gChan, (char)APDU_GET_DATA_MODE_REQUEST, (unsigned char *)buf, &dataLen);
    }
    if (rv == RES_OK) {
      gState = STATE_REQUEST_SENT;
    } else {
      gState = STATE_DONE;
    }
  }
  if (gState == STATE_REQUEST_SENT) {
    
    for (secs=0; secs < 15; secs++) {
      delay(1000);
    }
    rv = simGetStatus(gChan, APDU_GET_STATUS_MODE_GET, (unsigned char *)buf, &receiveState, &receiveRes);
    Serial.print("receiving state: ");
    Serial.println(receiveState, HEX);
    if (receiveState == STATUS_RECEIVE_DATA) {
      gState = STATE_REQUEST_DATA;
      return;
    }
    if (receiveState == STATUS_RECEIVE_DONE) {
      if (receiveRes != 0) {
        Serial.print("receiving error: ");
        Serial.println(receiveRes, HEX);
        gState = STATE_DONE;  
        return;
      }
    }
    if (rv != RES_OK) {
      gState = STATE_DONE;
    }
    return;
  }
  if (gState == STATE_REQUEST_DATA) {
    rv = simGetData(gChan, APDU_GET_DATA_MODE_DATA, (unsigned char *)buf, &dataLen);
    if (rv == RES_OK) {
      rv = configParseApply(buf, dataLen);
    }
    gState = STATE_DONE;
  }
  if (gState == STATE_DONE) {
    simChannelClose(gChan);
    gState = STATE_READY;
  
    Serial.print("waiting");
    unsigned char minutes = 5;
    
    while (minutes > 0) {
      for (secs=0; secs < 60; secs++) {
        delay(1000);
      }
      minutes -= 1;
      Serial.print('.');
    }
    Serial.println();
    return;
  }
}