add ServiceData advertising element (#243)

* gap: fix comment

* gap: expose ServiceData() in AdvertisementFields

* macos: include ServiceData in AdvertisementFields

* gap/linux: include ServiceData in AdvertisementFields

* gap: add unimplemented ServiceData() to raw advertisement

* added ServiceData advertising element also to the sending pieces

* more explicitly use the ad element type ids

* added a test case for ServiceData

* linux: added ServiceData advertising element

* sd: fix: handle no servicedata present

* linux: bluez uses string uuids for service data

* linux: fix: correct datatype for advertise with ServiceData

* uuid: add 32-Bit functions

* ServiceData now also uses a slice instead of a map as in #244

* Revert unnessesary changes

* formatting

* remove extra check

---------

Co-authored-by: William Johansson <[email protected]>

Author: dnlwgnd

adapter_darwin.go

@@ -156,6 +156,19 @@ func makeScanResult(prph cbgo.Peripheral, advFields cbgo.AdvFields, rssi int) Sc
 		})
 	}
 
+	var serviceData []ServiceDataElement
+	for _, svcData := range advFields.ServiceData {
+		cbgoUUID := svcData.UUID
+		uuid, err := ParseUUID(cbgoUUID.String())
+		if err != nil {
+			continue
+		}
+		serviceData = append(serviceData, ServiceDataElement{
+			UUID: uuid,
+			Data: svcData.Data,
+		})
+	}
+
 	// Peripheral UUID is randomized on macOS, which means to
 	// different centrals it will appear to have a different UUID.
 	return ScanResult{
@@ -168,6 +181,7 @@ func makeScanResult(prph cbgo.Peripheral, advFields cbgo.AdvFields, rssi int) Sc
 				LocalName:        advFields.LocalName,
 				ServiceUUIDs:     serviceUUIDs,
 				ManufacturerData: manufacturerData,
+				ServiceData:      serviceData,
 			},
 		},
 	}

gap.go

@@ -55,8 +55,10 @@ type AdvertisementOptions struct {
 	Interval Duration
 
 	// ManufacturerData stores Advertising Data.
-	// Keys are the Manufacturer ID to associate with the data.
 	ManufacturerData []ManufacturerDataElement
+
+	// ServiceData stores Advertising Data.
+	ServiceData []ServiceDataElement
 }
 
 // Manufacturer data that's part of an advertisement packet.
@@ -73,6 +75,17 @@ type ManufacturerDataElement struct {
 	Data []byte
 }
 
+// ServiceDataElement strores a uuid/byte-array pair used as ServiceData advertisment elements
+type ServiceDataElement struct {
+	// service uuid or company uuid
+	// The list can also be viewed here:
+	// https://bitbucket.org/bluetooth-SIG/public/src/main/assigned_numbers/company_identifiers/company_identifiers.yaml
+	// https://bitbucket.org/bluetooth-SIG/public/src/main/assigned_numbers/uuids/service_uuids.yaml
+	UUID UUID
+	// the data byte array
+	Data []byte
+}
+
 // Duration is the unit of time used in BLE, in 0.625µs units. This unit of time
 // is used throughout the BLE stack.
 type Duration uint16
@@ -124,9 +137,13 @@ type AdvertisementPayload interface {
 	// if this data is not available.
 	Bytes() []byte
 
-	// ManufacturerData returns a map with all the manufacturer data present in the
-	//advertising. IT may be empty.
+	// ManufacturerData returns a slice with all the manufacturer data present in the
+	// advertising. It may be empty.
 	ManufacturerData() []ManufacturerDataElement
+
+	// ServiceData returns a slice with all the service data present in the
+	// advertising. It may be empty.
+	ServiceData() []ServiceDataElement
 }
 
 // AdvertisementFields contains advertisement fields in structured form.
@@ -142,6 +159,9 @@ type AdvertisementFields struct {
 
 	// ManufacturerData is the manufacturer data of the advertisement.
 	ManufacturerData []ManufacturerDataElement
+
+	// ServiceData is the service data of the advertisement.
+	ServiceData []ServiceDataElement
 }
 
 // advertisementFields wraps AdvertisementFields to implement the
@@ -179,6 +199,11 @@ func (p *advertisementFields) ManufacturerData() []ManufacturerDataElement {
 	return p.AdvertisementFields.ManufacturerData
 }
 
+// ServiceData returns the underlying ServiceData field.
+func (p *advertisementFields) ServiceData() []ServiceDataElement {
+	return p.AdvertisementFields.ServiceData
+}
+
 // rawAdvertisementPayload encapsulates a raw advertisement packet. Methods to
 // get the data (such as LocalName()) will parse just the needed field. Scanning
 // the data should be fast as most advertisement packets only have a very small
@@ -288,6 +313,40 @@ func (buf *rawAdvertisementPayload) ManufacturerData() []ManufacturerDataElement
 	return manufacturerData
 }
 
+// ServiceData returns the service data in the advertisment payload
+func (buf *rawAdvertisementPayload) ServiceData() []ServiceDataElement {
+	var serviceData []ServiceDataElement
+	for index := 0; index < int(buf.len)+4; index += int(buf.data[index]) + 1 {
+		fieldLength := int(buf.data[index+0])
+		if fieldLength < 3 { // field has only length and type and no data
+			continue
+		}
+		fieldType := buf.data[index+1]
+		switch fieldType {
+		case 0x16: // 16-bit uuid
+			serviceData = append(serviceData, ServiceDataElement{
+				UUID: New16BitUUID(uint16(buf.data[index+2]) + (uint16(buf.data[index+3]) << 8)),
+				Data: buf.data[index+4 : index+fieldLength+1],
+			})
+		case 0x20: // 32-bit uuid
+			serviceData = append(serviceData, ServiceDataElement{
+				UUID: New32BitUUID(uint32(buf.data[index+2]) + (uint32(buf.data[index+3]) << 8) + (uint32(buf.data[index+4]) << 16) + (uint32(buf.data[index+5]) << 24)),
+				Data: buf.data[index+6 : index+fieldLength+1],
+			})
+		case 0x21: // 128-bit uuid
+			var uuidArray [16]byte
+			copy(uuidArray[:], buf.data[index+2:index+18])
+			serviceData = append(serviceData, ServiceDataElement{
+				UUID: NewUUID(uuidArray),
+				Data: buf.data[index+18 : index+fieldLength+1],
+			})
+		default:
+			continue
+		}
+	}
+	return serviceData
+}
+
 // reset restores this buffer to the original state.
 func (buf *rawAdvertisementPayload) reset() {
 	// The data is not reset (only the length), because with a zero length the
@@ -322,6 +381,12 @@ func (buf *rawAdvertisementPayload) addFromOptions(options AdvertisementOptions)
 		}
 	}
 
+	for _, element := range options.ServiceData {
+		if !buf.addServiceData(element.UUID, element.Data) {
+			return false
+		}
+	}
+
 	return true
 }
 
@@ -344,6 +409,57 @@ func (buf *rawAdvertisementPayload) addManufacturerData(key uint16, value []byte
 	return true
 }
 
+// addServiceData adds service data ([]byte) entries to the advertisement payload.
+func (buf *rawAdvertisementPayload) addServiceData(uuid UUID, data []byte) (ok bool) {
+	switch {
+	case uuid.Is16Bit():
+		// check if it fits
+		fieldLength := 1 + 1 + 2 + len(data) // 1 byte length, 1 byte ad type, 2 bytes uuid, actual service data
+		if int(buf.len)+fieldLength > len(buf.data) {
+			return false
+		}
+		// Add the data.
+		buf.data[buf.len+0] = byte(fieldLength - 1)
+		buf.data[buf.len+1] = 0x16
+		buf.data[buf.len+2] = byte(uuid.Get16Bit())
+		buf.data[buf.len+3] = byte(uuid.Get16Bit() >> 8)
+		copy(buf.data[buf.len+4:], data)
+		buf.len += uint8(fieldLength)
+
+	case uuid.Is32Bit():
+		// check if it fits
+		fieldLength := 1 + 1 + 4 + len(data) // 1 byte length, 1 byte ad type, 4 bytes uuid, actual service data
+		if int(buf.len)+fieldLength > len(buf.data) {
+			return false
+		}
+		// Add the data.
+		buf.data[buf.len+0] = byte(fieldLength - 1)
+		buf.data[buf.len+1] = 0x20
+		buf.data[buf.len+2] = byte(uuid.Get32Bit())
+		buf.data[buf.len+3] = byte(uuid.Get32Bit() >> 8)
+		buf.data[buf.len+4] = byte(uuid.Get32Bit() >> 16)
+		buf.data[buf.len+5] = byte(uuid.Get32Bit() >> 24)
+		copy(buf.data[buf.len+6:], data)
+		buf.len += uint8(fieldLength)
+
+	default: // must be 128-bit uuid
+		// check if it fits
+		fieldLength := 1 + 1 + 16 + len(data) // 1 byte length, 1 byte ad type, 16 bytes uuid, actual service data
+		if int(buf.len)+fieldLength > len(buf.data) {
+			return false
+		}
+		// Add the data.
+		buf.data[buf.len+0] = byte(fieldLength - 1)
+		buf.data[buf.len+1] = 0x21
+		uuid_bytes := uuid.Bytes()
+		copy(buf.data[buf.len+2:], uuid_bytes[:])
+		copy(buf.data[buf.len+2+16:], data)
+		buf.len += uint8(fieldLength)
+
+	}
+	return true
+}
+
 // addFlags adds a flags field to the advertisement buffer. It returns true on
 // success (the flags can be added) and false on failure.
 func (buf *rawAdvertisementPayload) addFlags(flags byte) (ok bool) {

gap_linux.go

@@ -53,6 +53,10 @@ func (a *Advertisement) Configure(options AdvertisementOptions) error {
 	for _, uuid := range options.ServiceUUIDs {
 		serviceUUIDs = append(serviceUUIDs, uuid.String())
 	}
+	var serviceData = make(map[string]interface{})
+	for _, element := range options.ServiceData {
+		serviceData[element.UUID.String()] = element.Data
+	}
 
 	// Convert map[uint16][]byte to map[uint16]any because that's what BlueZ needs.
 	manufacturerData := map[uint16]any{}
@@ -71,6 +75,7 @@ func (a *Advertisement) Configure(options AdvertisementOptions) error {
 			"ServiceUUIDs":     {Value: serviceUUIDs},
 			"ManufacturerData": {Value: manufacturerData},
 			"LocalName":        {Value: options.LocalName},
+			"ServiceData":      {Value: serviceData},
 			// The documentation states:
 			// > Timeout of the advertisement in seconds. This defines the
 			// > lifetime of the advertisement.
@@ -288,6 +293,20 @@ func makeScanResult(props map[string]dbus.Variant) ScanResult {
 	localName, _ := props["Name"].Value().(string)
 	rssi, _ := props["RSSI"].Value().(int16)
 
+	var serviceData []ServiceDataElement
+	if sdata, ok := props["ServiceData"].Value().(map[string]dbus.Variant); ok {
+		for k, v := range sdata {
+			uuid, err := ParseUUID(k)
+			if err != nil {
+				continue
+			}
+			serviceData = append(serviceData, ServiceDataElement{
+				UUID: uuid,
+				Data: v.Value().([]byte),
+			})
+		}
+	}
+
 	return ScanResult{
 		RSSI:    rssi,
 		Address: a,
@@ -296,6 +315,7 @@ func makeScanResult(props map[string]dbus.Variant) ScanResult {
 				LocalName:        localName,
 				ServiceUUIDs:     serviceUUIDs,
 				ManufacturerData: manufacturerData,
+				ServiceData:      serviceData,
 			},
 		},
 	}

gap_test.go

@@ -78,6 +78,42 @@ func TestCreateAdvertisementPayload(t *testing.T) {
 				},
 			},
 		},
+		{
+			raw: "\x02\x01\x06" + // flags
+				"\x05\x16\xD2\xFC\x40\x02" + // service data 16-Bit UUID
+				"\x06\x20\xD2\xFC\x40\x02\xC4", // service data 32-Bit UUID
+			parsed: AdvertisementOptions{
+				ServiceData: []ServiceDataElement{
+					{UUID: New16BitUUID(0xFCD2), Data: []byte{0x40, 0x02}},
+					{UUID: New32BitUUID(0x0240FCD2), Data: []byte{0xC4}},
+				},
+			},
+		},
+		{
+			raw: "\x02\x01\x06" + // flags
+				"\x05\x16\xD2\xFC\x40\x02" + // service data 16-Bit UUID
+				"\x05\x16\xD3\xFC\x40\x02", // service data 16-Bit UUID
+			parsed: AdvertisementOptions{
+				ServiceData: []ServiceDataElement{
+					{UUID: New16BitUUID(0xFCD2), Data: []byte{0x40, 0x02}},
+					{UUID: New16BitUUID(0xFCD3), Data: []byte{0x40, 0x02}},
+				},
+			},
+		},
+		{
+			raw: "\x02\x01\x06" + // flags
+				"\x04\x16\xD2\xFC\x40" + // service data 16-Bit UUID
+				"\x12\x21\xB8\x6C\x75\x05\xE9\x25\xBD\x93\xA8\x42\x32\xC3\x00\x01\xAF\xAD\x09", // service data 128-Bit UUID
+			parsed: AdvertisementOptions{
+				ServiceData: []ServiceDataElement{
+					{UUID: New16BitUUID(0xFCD2), Data: []byte{0x40}},
+					{
+						UUID: NewUUID([16]byte{0xad, 0xaf, 0x01, 0x00, 0xc3, 0x32, 0x42, 0xa8, 0x93, 0xbd, 0x25, 0xe9, 0x05, 0x75, 0x6c, 0xb8}),
+						Data: []byte{0x09},
+					},
+				},
+			},
+		},
 	}
 	for _, tc := range tests {
 		var expectedRaw rawAdvertisementPayload

uuid.go

@@ -38,6 +38,20 @@ func New16BitUUID(shortUUID uint16) UUID {
 	return uuid
 }
 
+// New32BitUUID returns a new 128-bit UUID based on a 32-bit UUID.
+//
+// Note: only use registered UUIDs. See
+// https://www.bluetooth.com/specifications/gatt/services/ for a list.
+func New32BitUUID(shortUUID uint32) UUID {
+	// https://stackoverflow.com/questions/36212020/how-can-i-convert-a-bluetooth-16-bit-service-uuid-into-a-128-bit-uuid
+	var uuid UUID
+	uuid[0] = 0x5F9B34FB
+	uuid[1] = 0x80000080
+	uuid[2] = 0x00001000
+	uuid[3] = shortUUID
+	return uuid
+}
+
 // Replace16BitComponent returns a new UUID where bits 16..32 have been replaced
 // with the bits given in the argument. These bits are the same bits that vary
 // in the 16-bit compressed UUID form.
@@ -68,6 +82,14 @@ func (uuid UUID) Get16Bit() uint16 {
 	return uint16(uuid[3])
 }
 
+// Get32Bit returns the 32-bit version of this UUID. This is only valid if it
+// actually is a 32-bit UUID, see Is32Bit.
+func (uuid UUID) Get32Bit() uint32 {
+	// Note: using a Get* function as a getter because method names can't start
+	// with a number.
+	return uuid[3]
+}
+
 // Bytes returns a 16-byte array containing the raw UUID.
 func (uuid UUID) Bytes() [16]byte {
 	buf := [16]byte{}