core: Add Manufacturer Specific Data EIR field

Add data structure and parsing support.

1 files changed, 22 insertions, 0 deletions

diff --git a/src/eir.c b/src/eir.c
index d22ad9193..2ea8731bd 100644
--- a/src/eir.c
+++ b/src/eir.c
@@ -53,6 +53,8 @@ void eir_data_free(struct eir_data *eir)
 	eir->hash = NULL;
 	g_free(eir->randomizer);
 	eir->randomizer = NULL;
+	g_slist_free_full(eir->msd_list, g_free);
+	eir->msd_list = NULL;
 }
 
 static void eir_parse_uuid16(struct eir_data *eir, const void *data,
@@ -137,6 +139,22 @@ static char *name2utf8(const uint8_t *name, uint8_t len)
 	return g_strdup(utf8_name);
 }
 
+static void eir_parse_msd(struct eir_data *eir, const uint8_t *data,
+								uint8_t len)
+{
+	struct eir_msd *msd;
+
+	if (len < 2 || len > 2 + sizeof(msd->data))
+		return;
+
+	msd = g_malloc(sizeof(*msd));
+	msd->company = get_le16(data);
+	msd->data_len = len - 2;
+	memcpy(&msd->data, data + 2, msd->data_len);
+
+	eir->msd_list = g_slist_append(eir->msd_list, msd);
+}
+
 void eir_parse(struct eir_data *eir, const uint8_t *eir_data, uint8_t eir_len)
 {
 	uint16_t len = 0;
@@ -240,6 +258,10 @@ void eir_parse(struct eir_data *eir, const uint8_t *eir_data, uint8_t eir_len)
 			eir->did_product = data[4] | (data[5] << 8);
 			eir->did_version = data[6] | (data[7] << 8);
 			break;
+
+		case EIR_MANUFACTURER_DATA:
+			eir_parse_msd(eir, data, data_len);
+			break;
 		}
 
 		eir_data += field_len + 1;