From 0c3a4d0c01a7d56ff84bd50b0ae26e532e002216 Mon Sep 17 00:00:00 2001
From: Andrea Franz <andrea@gravityblast.com>
Date: Tue, 25 Sep 2018 15:26:06 +0200
Subject: [PATCH] add FindTag utils function to parse TLV data

---
 apdu/command_test.go |  2 ++
 apdu/utils.go        | 61 ++++++++++++++++++++++++++++++++++++++++++++
 apdu/utils_test.go   | 42 ++++++++++++++++++++++++++++++
 3 files changed, 105 insertions(+)
 create mode 100644 apdu/utils.go
 create mode 100644 apdu/utils_test.go

diff --git a/apdu/command_test.go b/apdu/command_test.go
index 6bfd5ef..8f0f197 100644
--- a/apdu/command_test.go
+++ b/apdu/command_test.go
@@ -4,12 +4,14 @@ import (
 	"encoding/hex"
 	"fmt"
 	"log"
+	"regexp"
 	"testing"
 
 	"github.com/stretchr/testify/assert"
 )
 
 func hexToBytes(s string) []byte {
+	s = regexp.MustCompile(" ").ReplaceAllString(s, "")
 	b := make([]byte, hex.DecodedLen(len(s)))
 	_, err := hex.Decode(b, []byte(s))
 	if err != nil {
diff --git a/apdu/utils.go b/apdu/utils.go
new file mode 100644
index 0000000..b1acd16
--- /dev/null
+++ b/apdu/utils.go
@@ -0,0 +1,61 @@
+package apdu
+
+import (
+	"bytes"
+	"fmt"
+	"io"
+)
+
+type ErrTagNotFound struct {
+	tag uint8
+}
+
+func (e *ErrTagNotFound) Error() string {
+	return fmt.Sprintf("tag %x not found", e.tag)
+}
+
+func FindTag(raw []byte, tags ...uint8) ([]byte, error) {
+	if len(tags) == 0 {
+		return raw, nil
+	}
+
+	target := tags[0]
+	buf := bytes.NewBuffer(raw)
+
+	var (
+		tag    uint8
+		length uint8
+		err    error
+	)
+
+	for {
+		tag, err = buf.ReadByte()
+		switch {
+		case err == io.EOF:
+			return []byte{}, &ErrTagNotFound{target}
+		case err != nil:
+			return nil, err
+		}
+
+		length, err = buf.ReadByte()
+		if err != nil {
+			return nil, err
+		}
+
+		data := make([]byte, length)
+		if length != 0 {
+			_, err = buf.Read(data)
+			if err != nil {
+				return nil, err
+			}
+		}
+
+		if tag == target {
+			if len(tags) == 1 {
+				return data, nil
+			}
+
+			return FindTag(data, tags[1:]...)
+		}
+	}
+}
diff --git a/apdu/utils_test.go b/apdu/utils_test.go
new file mode 100644
index 0000000..284353d
--- /dev/null
+++ b/apdu/utils_test.go
@@ -0,0 +1,42 @@
+package apdu
+
+import (
+	"testing"
+
+	"github.com/stretchr/testify/assert"
+)
+
+func TestFindTag(t *testing.T) {
+	var (
+		tagData []byte
+		err     error
+	)
+
+	data := hexToBytes("C1 02 BB CC C2 04 C3 02 11 22 C3 02 88 99")
+
+	tagData, err = FindTag(data, uint8(0xC1))
+	assert.NoError(t, err)
+	assert.Equal(t, "BB CC", bytesToHexWithSpaces(tagData))
+
+	tagData, err = FindTag(data, uint8(0xC2))
+	assert.NoError(t, err)
+	assert.Equal(t, "C3 02 11 22", bytesToHexWithSpaces(tagData))
+
+	tagData, err = FindTag(data, uint8(0xC3))
+	assert.NoError(t, err)
+	assert.Equal(t, "88 99", bytesToHexWithSpaces(tagData))
+
+	tagData, err = FindTag(data, uint8(0xC2), uint8(0xC3))
+	assert.NoError(t, err)
+	assert.Equal(t, "11 22", bytesToHexWithSpaces(tagData))
+
+	// tag not found
+	badData := hexToBytes("C1 00")
+	_, err = FindTag(badData, uint8(0xC2))
+	assert.Equal(t, &ErrTagNotFound{uint8(0xC2)}, err)
+
+	// sub-tag not found
+	badData = hexToBytes("C1 02 C2 00")
+	_, err = FindTag(badData, uint8(0xC1), uint8(0xC3))
+	assert.Equal(t, &ErrTagNotFound{uint8(0xC3)}, err)
+}