ninafw: handle calls to DiscoverServices/DiscoerCharacteristics that …

…include a specfic list of UUIDs Signed-off-by: deadprogram <[email protected]>
tinygo-org · Jan 2, 2024 · 624a9f9 · 624a9f9
1 parent 27d629d
commit 624a9f9
Showing 1 changed file with 73 additions and 33 deletions.
diff --git a/gattc_ninafw.go b/gattc_ninafw.go
@@ -13,6 +13,8 @@ var (
 	errReadFailed                = errors.New("bluetooth: read failed")
 	errNoNotify                  = errors.New("bluetooth: notify/indicate not permitted")
 	errEnableNotificationsFailed = errors.New("bluetooth: enable notifications failed")
+	errServiceNotFound           = errors.New("bluetooth: service not found")
+	errCharacteristicNotFound    = errors.New("bluetooth: characteristic not found")
 )
 
 const (
@@ -52,6 +54,7 @@ func (d *Device) DiscoverServices(uuids []UUID) ([]DeviceService, error) {
 	}
 
 	d.services = []DeviceService{}
+	foundServices := make(map[UUID]DeviceService)
 
 	startHandle := uint16(0x0001)
 	endHandle := uint16(0xffff)
@@ -67,13 +70,16 @@ func (d *Device) DiscoverServices(uuids []UUID) ([]DeviceService, error) {
 
 		if len(d.adapter.att.services) > 0 {
 			for _, rawService := range d.adapter.att.services {
-				d.services = append(d.services,
-					DeviceService{
-						device:      d,
-						uuid:        rawService.uuid,
-						startHandle: rawService.startHandle,
-						endHandle:   rawService.endHandle,
-					})
+				if len(uuids) == 0 || rawService.uuid.IsIn(uuids) {
+					foundServices[rawService.uuid] =
+						DeviceService{
+							device:      d,
+							uuid:        rawService.uuid,
+							startHandle: rawService.startHandle,
+							endHandle:   rawService.endHandle,
+						}
+				}
+
 				startHandle = rawService.endHandle + 1
 				if startHandle == 0x0000 {
 					endHandle = 0x0000
@@ -87,6 +93,21 @@ func (d *Device) DiscoverServices(uuids []UUID) ([]DeviceService, error) {
 		}
 	}
 
+	// put into correct order if needed
+	if len(uuids) > 0 {
+		for _, uuid := range uuids {
+			s, ok := foundServices[uuid]
+			if !ok {
+				return nil, errServiceNotFound
+			}
+			d.services = append(d.services, s)
+		}
+	} else {
+		for _, s := range foundServices {
+			d.services = append(d.services, s)
+		}
+	}
+
 	return d.services, nil
 }
 
@@ -121,6 +142,7 @@ func (s *DeviceService) DiscoverCharacteristics(uuids []UUID) ([]DeviceCharacter
 	}
 
 	s.characteristics = []DeviceCharacteristic{}
+	foundCharacteristics := make(map[UUID]DeviceCharacteristic)
 
 	startHandle := s.startHandle
 	endHandle := s.endHandle
@@ -143,33 +165,36 @@ func (s *DeviceService) DiscoverCharacteristics(uuids []UUID) ([]DeviceCharacter
 
 		if len(s.device.adapter.att.characteristics) > 0 {
 			for _, rawCharacteristic := range s.device.adapter.att.characteristics {
-				permissions := CharacteristicPermissions(0)
-				if rawCharacteristic.properties&charPropertyBroadcast != 0 {
-					permissions |= CharacteristicBroadcastPermission
-				}
-				if rawCharacteristic.properties&charPropertyRead != 0 {
-					permissions |= CharacteristicReadPermission
-				}
-				if rawCharacteristic.properties&charPropertyWriteWithoutResponse != 0 {
-					permissions |= CharacteristicWriteWithoutResponsePermission
+				if len(uuids) == 0 || rawCharacteristic.uuid.IsIn(uuids) {
+					permissions := CharacteristicPermissions(0)
+					if rawCharacteristic.properties&charPropertyBroadcast != 0 {
+						permissions |= CharacteristicBroadcastPermission
+					}
+					if rawCharacteristic.properties&charPropertyRead != 0 {
+						permissions |= CharacteristicReadPermission
+					}
+					if rawCharacteristic.properties&charPropertyWriteWithoutResponse != 0 {
+						permissions |= CharacteristicWriteWithoutResponsePermission
+					}
+					if rawCharacteristic.properties&charPropertyWrite != 0 {
+						permissions |= CharacteristicWritePermission
+					}
+					if rawCharacteristic.properties&charPropertyNotify != 0 {
+						permissions |= CharacteristicNotifyPermission
+					}
+					if rawCharacteristic.properties&charPropertyIndicate != 0 {
+						permissions |= CharacteristicIndicatePermission
+					}
+					foundCharacteristics[rawCharacteristic.uuid] =
+						DeviceCharacteristic{
+							service:     s,
+							uuid:        rawCharacteristic.uuid,
+							handle:      rawCharacteristic.valueHandle,
+							properties:  rawCharacteristic.properties,
+							permissions: permissions,
+						}
 				}
-				if rawCharacteristic.properties&charPropertyWrite != 0 {
-					permissions |= CharacteristicWritePermission
-				}
-				if rawCharacteristic.properties&charPropertyNotify != 0 {
-					permissions |= CharacteristicNotifyPermission
-				}
-				if rawCharacteristic.properties&charPropertyIndicate != 0 {
-					permissions |= CharacteristicIndicatePermission
-				}
-				s.characteristics = append(s.characteristics,
-					DeviceCharacteristic{
-						service:     s,
-						uuid:        rawCharacteristic.uuid,
-						handle:      rawCharacteristic.valueHandle,
-						properties:  rawCharacteristic.properties,
-						permissions: permissions,
-					})
+
 				startHandle = rawCharacteristic.valueHandle + 1
 			}
 
@@ -180,6 +205,21 @@ func (s *DeviceService) DiscoverCharacteristics(uuids []UUID) ([]DeviceCharacter
 		}
 	}
 
+	// put into correct order if needed
+	if len(uuids) > 0 {
+		for _, uuid := range uuids {
+			c, ok := foundCharacteristics[uuid]
+			if !ok {
+				return nil, errCharacteristicNotFound
+			}
+			s.characteristics = append(s.characteristics, c)
+		}
+	} else {
+		for _, c := range foundCharacteristics {
+			s.characteristics = append(s.characteristics, c)
+		}
+	}
+
 	return s.characteristics, nil
 }