i = 248 - ExtractData( 248 );              //extract data, max effective size is 248/8 bytes
                            
      if ((i >= 33) && (data_buffer[0] != 0)) {  //error message found
        i = ERR_EM4035_ERROR_MSG;
        clean_data_cnt = 4;

        if (signed_crc != 0) {
          //unsign
          HWSign( (uint8_t*)&data_buffer[clean_data_cnt-2], (uint8_t*)&data_buffer[clean_data_cnt-2], 16);
        }

      } else if ((i-1) < expectedResponseLen) { //no good header found or not enough data
        clean_data_cnt = 0;
        i = ERR_EM4035_WRONG_LEN;
      } else {                                   //clamp data to bytes
        i = 0;
        clean_data_cnt = expectedResponseLen / 8;

        if (signed_crc != 0) {
          //unsign
          HWSign( (uint8_t*)&data_buffer[clean_data_cnt-2], (uint8_t*)&data_buffer[clean_data_cnt-2], 16);
        }

      }

    } else {
      i = ERR_EM4035_NO_SOF;
    }
  }
  return i;
}


//--------------------------------------------------------------
//--------------------------------------------------------------
//1TS Inventory

//#define DBG_1TS

#define TINVENTORY_MASK_LEN 0
#define TINVENTORY_VALUE    1
#define TINVENTORY_MASK     2

#define STACK_LENGTH 8                //stack size

typedef struct {
  uint8_t mask_len;
  uint8_t value;
  uint8_t mask[12];
  uint8_t vata[2];                    //to keep this structure size being 2^n
} TINVENTORY;

TINVENTORY stack[STACK_LENGTH], found[STACK_LENGTH];

int8_t stack_ptr;
int8_t found_ptr;

uint8_t dbg_counter;


//--------------------------------------------------------------
//send current inventory command and resolve the response
//
char InventoryStep(void) {

  uint8_t i, j, k, l;
  uint8_t result;
  uint16_t crc;

  expectedResponseLen = 96;

  j = stack[stack_ptr].mask_len;

  //set collision bit
  l = 2 + (j - 1) / 8;
  for(k = 0xFF, i = 0; i < j%8; i++) k <<= 1;
  stack[stack_ptr].mask[l] = (stack[stack_ptr].mask[l] & (~k)) | (stack[stack_ptr].value << ((j-1) % 8));

  cmd_message[0] = message_flags;
  cmd_message[1] = 0x01;
  cmd_message[2] = j;

  //copy uid as a mask
  if (j != 0) {
    l = (j - 1) / 8;
    for(i=0; i <= l; i++)
      cmd_message[3 + i] = stack[stack_ptr].mask[2 + i];
    j = (j + 7) / 8;
  }
  
  //compute CRC
  j += 3;
  crc = CRC( cmd_message, j );
  cmd_message[j++] = crc & 255;
  cmd_message[j++] = crc >> 8;

  //form & send command & wait
  Send( j, STD_WAIT );

  //listen for response
  maxCaptureTimeLow = (uint8_t)maxTGeneral;
  maxCaptureTimeHi = maxTGeneral >> 8;

  Capture(2);

  clean_data_cnt = 0;
  result = 0;

#ifdef DBG_1TS
  UDR = j;
  while (!(UCSRA & (1<<UDRE)))
    {} 

  for (i=2; i<j; i++) {
    UDR = cmd_message[i];
    while (!(UCSRA & (1<<UDRE)))
      {} 
  }
  UDR = sof;
  while (!(UCSRA & (1<<UDRE)))
    {} 
  UDR = capture_cnt;
  while (!(UCSRA & (1<<UDRE)))
    {} 

  if (dbg_counter == 2)
    SendCaptureData(0x83, 3);
#endif

  if ((sof == 0) || (capture_cnt < 8))
    return -2;


  j = ExtractData( expectedResponseLen );        //find first data bit

#ifdef DBG_1TS
  UDR = j;
  while (!(UCSRA & (1<<UDRE)))
    {} 
#endif

  for(i=0; i < 12; i++)                             //always copy data regardless the result
    found[found_ptr].mask[i] = data_buffer[i];

  if (j == 0) {
    //one valid response received
    clean_data_cnt = 12;

    crc = CRC(data_buffer, clean_data_cnt);
    if (crc == ~0xF0B8) {
      //switch to '1' or pop stack
      while ((stack_ptr > 0) && (stack[stack_ptr].value != 0))
        stack_ptr--;
      stack[stack_ptr].value = 1;

      //form Stay Quiet command
      cmd_message[0] = (message_flags & 0x03) | 0x20;
      cmd_message[1] = 0x02;
      for (i=2; i<10; i++)
        cmd_message[i] = found[found_ptr].mask[i];

      crc = CRC( cmd_message, 10 );
      cmd_message[10] = crc & 255;
      cmd_message[11] = crc >> 8;

      //form & send command & wait (10 = arbitrary delay)
      Send( 12, 10 );

#if 0
//#ifdef DBG_1TS
      for (i=0; i<12; i++) {
        UDR = cmd_message[i];
        while (!(UCSRA & (1<<UDRE)))
          {} 
      }
#endif

      found_ptr++;
      if (found_ptr > STACK_LENGTH-1) return ERR_EM4035_BUFFER_OVERFLOW;
      return 0;
    }
    return ERR_EM4035_BAD_CRC;
  }

  //collision
  if (stack_ptr < STACK_LENGTH-1) stack_ptr++;                 //if there is not enough room, just drop previous mask
  for(i=0; i < 12; i++)
    stack[stack_ptr].mask[i] = data_buffer[i];
  stack[stack_ptr].value = 0;
  if (j > 80)
    j = 1;
  else
    j = 81 - j;
  stack[stack_ptr].mask_len = j;
  return 1;
}

//--------------------------------------------------------------
//main inventory loop
//
uint8_t Inventory(void) {

#define TIMEOUT (16)
  int nothing = 0;
  int result;

  dbg_counter = 0;

  stack_ptr = 0;
  found_ptr = 0;
  stack[0].mask_len = 0;

  while ((stack_ptr >= 0) && (dbg_counter < TIMEOUT)) {
    result = InventoryStep();

    dbg_counter++;

    if (result == -2) nothing++;
    if (nothing >= 2) {
      if ((stack_ptr > 0) && (stack[stack_ptr].value != 0))
        stack_ptr--;
      stack[stack_ptr].value = 1;
      nothing = 0;
    }
  }

  //Send responses
  if (found_ptr > 0) {

    //some tags found
    while(--found_ptr >= 0)
      FormatResponse_Data( 0x83, 0, 12, &found[found_ptr].mask[0]);
  };
  
  return result;
}


//--------------------------------------------------------------
//--------------------------------------------------------------

  uint8_t hwa_Key;
  uint8_t hwa_ICMfg;
  uint8_t hwa_A1[7];
  uint8_t hwa_A2_db_f[11];

//--------------------------------------------------------------
//main EM4035 authentication routine
//
uint8_t HW_Authentication(void) {

  uint8_t i, j;
  uint16_t crc;

  //............................................................
  //save the parameters

  hwa_Key = cmd_message[11];
  hwa_ICMfg = cmd_message[2];

  //............................................................
  //reselect the key

  j = hwa_Key;

  for(i=0; i<7; i++) {                                 //hot one to binary conversion    
    j >>= 1;
    if(j == 0) break;
  }
  hwa_Key = (hwa_Key | (hwa_Key >> 4)) & 0x0F;

#ifdef HW_AUTH_DBG
  SendByte(i);
  SendByte(hwa_Key);
#endif

  cbi(PORTB, SDA_PIN);
  cbi(PORTC, SCK_PIN);
  cbi(PORTD, SSN_PIN);

  ResetSSN();
  HWTransaction( &i, &j, 3);

  //............................................................
  //ISO Select

  cmd_message[0] = (message_flags & 0x03) | 0x20;
  cmd_message[1] = 0x25;
  for (i=2; i<10; i++)                              //shift uid 1 byte down
    cmd_message[i] = cmd_message[i+1];

  crc = CRC( cmd_message, 10 );
  cmd_message[10] = crc & 255;
  cmd_message[11] = crc >> 8;

  expectedResponseLen = 0x20;

  //send command & wait
  Send( 12, STD_WAIT );
  maxCaptureTimeLow = (uint8_t)maxTGeneral; 
  maxCaptureTimeHi = maxTGeneral >> 8; 
  Capture(2);

  if (sof == 0)
    return ERR_EM4035_SELECT_FAILED;

  j = 248 - ExtractData( 248 );                  //find first data bit
  j /= 8;

  if (j == 3) {                                 //3 bytes data
    crc = CRC(data_buffer, j);
    if ((crc != ~0xF0B8) || (data_buffer[0] != 0))
      return ERR_EM4035_SELECT_FAILED;
  } else {
    return ERR_EM4035_SELECT_FAILED;
  }

  //............................................................
  //Auth1

  cmd_message[0] = (message_flags & 0x03) | 0x10;
  cmd_message[1] = 0xE0;
  cmd_message[2] = hwa_ICMfg;
  cmd_message[3] = hwa_Key;

  crc = CRC( cmd_message, 4 );
  cmd_message[4] = crc & 255;
  cmd_message[5] = crc >> 8;

  expectedResponseLen = 0x50;

  //send command & wait
  Send( 6, STD_WAIT );
  maxCaptureTimeLow = (uint8_t)maxTGeneral; 
  maxCaptureTimeHi = maxTGeneral >> 8; 
  
  Capture(2);

  if (sof == 0)
    return ERR_EM4035_A1_NO_SOF;

  j = 248 - ExtractData( 248 );                  //find first data bit
  clean_data_cnt = j / 8;

  if (clean_data_cnt > 0) {
    crc = CRC(data_buffer, clean_data_cnt);

    if (crc != ~0xF0B8)
      return ERR_EM4035_A1_CRC_ERROR;

    if ((clean_data_cnt != 10) || (data_buffer[0] != 0))   //10 bytes data
      return UART_MESSAGE_OK;
  } else {
    return ERR_EM4035_A1_FAILED;
  }

  for (i=0; i<7; i++) {                         //store A1
    hwa_A1[i] = data_buffer[i+1];

#ifdef HW_AUTH_DBG
    if (debug_mode == 2) {
      UDR = data_buffer[i+1];
      while (!(UCSRA & (1<<UDRE)))
        {} 
    }
#endif
  }

  //............................................................
  //HW Authentication part I
  cbi(PORTB, SDA_PIN);
  cbi(PORTC, SCK_PIN);
  cbi(PORTD, SSN_PIN);

  PORTD = PIND ^ (1<<DBG_FORWARD);

  HWTransaction( hwa_A1, hwa_A2_db_f, 56);

  for (i=0; i<11; i++)                          //clean A2_db_f
    hwa_A2_db_f[i] = 0;

  HWTransaction( hwa_A2_db_f, hwa_A2_db_f, 65);    //shift A2 and dummy byte
  HWTransaction( hwa_A2_db_f, (uint8_t*)&(hwa_A2_db_f[8]), 24); //shift result  

  PORTD = PIND ^ (1<<DBG_FORWARD);

  //............................................................
  //Auth2

  cmd_message[0] = (message_flags & 0x03) | 0x10;
  cmd_message[1] = 0xE1;
  cmd_message[2] = hwa_ICMfg;

  for(i=0; i<11; i++)
    cmd_message[3+i] = hwa_A2_db_f[i];

  crc = CRC( cmd_message, 14 );
  cmd_message[14] = crc & 255;
  cmd_message[15] = crc >> 8;

#ifdef HW_AUTH_DBG
  if (debug_mode == 2) {
    for(i=0; i<16; i++) {
      SendByte(cmd_message[i]);
    }
  }
#endif

  //wait for 6.6ms
  Wait(3300);

  expectedResponseLen = 0x30;  

  //send command & wait
  Send( 16, STD_WAIT );
  maxCaptureTimeLow = (uint8_t)maxTGeneral; 
  maxCaptureTimeHi = maxTGeneral >> 8; 
  
  Capture(2);

  if (sof == 0)
    return ERR_EM4035_A2_NO_SOF;

  j = 248 - ExtractData( 248 );                  //find first data bit

  clean_data_cnt = j / 8;

  if (clean_data_cnt > 0) {
    crc = CRC(data_buffer, clean_data_cnt);
    if (crc != ~0xF0B8)
      return ERR_EM4035_A2_CRC_ERROR;
    if (data_buffer[0] != 0)
      return UART_MESSAGE_OK;
  } else {
    return ERR_EM4035_A2_FAILED;
  }

  //............................................................
  //HW Authentication part II
  
  *(uint32_t*)&hwa_A2_db_f[0] = 0;

  HWTransaction( hwa_A2_db_f, hwa_A2_db_f, 24); //shift g'

  for (i=0; i<3; i++)                           //compare g and g'
    if (hwa_A2_db_f[i] != data_buffer[i+1])
      return ERR_EM4035_AUTH_FAILED;

  return UART_MESSAGE_OK;
}

//--------------------------------------------------------------
//--------------------------------------------------------------
//send the EM4034 login command
//
uint8_t EM4034_Login(void) {

  uint8_t i;

  //save the parameters
  hwa_ICMfg = cmd_message[2];

  i = GeneralRead();
  return i;
} 

//--------------------------------------------------------------
//--------------------------------------------------------------
//--------------------------------------------------------------

//--------------------------------------------------------------
//toggle EAS OFF
//
uint8_t ToggleEAS(void) {

  uint8_t i, j;
  uint16_t crc;

  //RF Reset
  WriteSPI( write_4094_low & 0xFFFE, write_4094_hi );
  Wait(32768);
  WriteSPI( write_4094_low | 1, write_4094_hi );
  Wait(1000);                            //wait for ~2ms

  if (cmd_message[1] == 0xE0)
    i = HW_Authentication();             //EM4035
  else
    i = EM4034_Login();                  //EM4034
     
  if (i != UART_MESSAGE_OK)
    return i;

  if (data_buffer[0] != 0)
    return UART_MESSAGE_OK;

  //form toggle EAS command
  cmd_message[0] = (message_flags & 0x03) | 0x10;
  cmd_message[1] = 0xA0;
  cmd_message[2] = hwa_ICMfg;

  crc = CRC( cmd_message, 3 );
  cmd_message[3] = crc & 255;
  cmd_message[4] = crc >> 8;

  expectedResponseLen = 0x20;

  //send command & wait
  write_tag_memory_delay = 0x1100;
  Send( 5, STD_WAIT );
  maxCaptureTimeLow = (uint8_t)maxTwa1ee; 
  maxCaptureTimeHi = maxTwa1ee >> 8; 
  Capture(2);

  if (sof == 0)
    return ERR_EM4035_NO_SOF;

  j = 248 - ExtractData( 248 );                  //find first data bit
  clean_data_cnt = j / 8;

  if (clean_data_cnt == 4) {                                  //4 bytes data
    crc = CRC(data_buffer, clean_data_cnt);
    if (crc != ~0xF0B8)
      return ERR_EM4035_BAD_CRC;
  } else {
    return ERR_EM4035_WRONG_LEN;
  }

  return i;
}