Func to search BER-TLV (paycard) records for given tag, returning data ptr & length

gistfile1.txt

bool searchTLV(uint8_t* inbuf, uint16_t tag_to_find, uint8_t** foundbuf, uint8_t* foundlen)
{
  // Tag, Length, Value buffer parser/lookup
  // TAG & 0x20 = 0 - primitive (pure data in V)
  // TAG & 0x20 = 1 - constructer (another TLVs in V)

  // template code is first byte in buf
  bool constructed;
  bool multibytetag;
  uint16_t tag = *inbuf;
  //DEBUG("SEARCHTLV: \n");
  while (tag) {
    constructed = (tag & 0x20)==0x20;
    multibytetag = (tag & 0x1f)==0x1f;
    if (multibytetag) {
        tag <<= 8;
        tag += *++inbuf;
    }
    //DEBUG(" -> TAG: %04x con=[%d] mbt=[%d] LEN=%d bytes \n", tag, constructed, multibytetag, *++inbuf);

    if (tag == tag_to_find) {
      // we've found tag
      *foundlen = *inbuf;
      *foundbuf = ++inbuf;
      return true;
    } else 
    {
      // skip to next tag
      if (constructed) {
          // skip length & enter constructed V
          ++inbuf;
      } else 
      {
        // primitive tag - skip data
        uint8_t len = *inbuf; // get length
        //DEBUG("--> VAL: %03d bytes \n", len);
        inbuf+=len+1; // skip length      
      }
    }    
    tag = *inbuf; // get next tag
  }
  foundbuf=NULL;
  foundlen=0;
  return false;  // tag not found
}