diff --git a/adapter_ninafw.go b/adapter_ninafw.go index 8d1ecc51..59115fb5 100644 --- a/adapter_ninafw.go +++ b/adapter_ninafw.go @@ -9,6 +9,8 @@ import ( "time" ) +const maxConnections = 1 + // Adapter represents the UART connection to the NINA fw. type Adapter struct { hci *hci @@ -20,7 +22,7 @@ type Adapter struct { reset func() connectHandler func(device Address, connected bool) - connectedDevices map[uint16]*Device + connectedDevices []*Device notificationsStarted bool } @@ -33,7 +35,7 @@ var DefaultAdapter = &Adapter{ connectHandler: func(device Address, connected bool) { return }, - connectedDevices: make(map[uint16]*Device), + connectedDevices: make([]*Device, 0, maxConnections), } // Enable configures the BLE stack. It must be called before any @@ -135,12 +137,21 @@ func (a *Adapter) startNotifications() { return } + if _debug { + println("starting notifications...") + } + a.notificationsStarted = true // go routine to poll for HCI events for ATT notifications go func() { for { - a.att.poll() + if err := a.att.poll(); err != nil { + // TODO: handle error + if _debug { + println("error polling for notifications:", err.Error()) + } + } time.Sleep(250 * time.Millisecond) } @@ -151,14 +162,48 @@ func (a *Adapter) startNotifications() { for { select { case not := <-a.att.notifications: - // TODO: find and call the handler if _debug { - println("notification received", not.handle, not.data) + println("notification received", not.connectionHandle, not.handle, not.data) + } + + d := a.findDevice(not.connectionHandle) + if d == nil { + if _debug { + println("no device found for handle", not.connectionHandle) + } + continue + } + + n := d.findNotificationRegistration(not.handle) + if n == nil { + if _debug { + println("no notification registered for handle", not.handle) + } + continue + } + + if n.callback != nil { + n.callback(not.data) } default: } + runtime.Gosched() } }() } + +func (a *Adapter) findDevice(handle uint16) *Device { + for _, d := range a.connectedDevices { + if d.handle == handle { + if _debug { + println("found device", handle, d.Address.String(), "with notifications registered", len(d.notificationRegistrations)) + } + + return d + } + } + + return nil +} diff --git a/att_ninafw.go b/att_ninafw.go index 49036821..8a31bd38 100644 --- a/att_ninafw.go +++ b/att_ninafw.go @@ -69,10 +69,10 @@ const ( ) var ( - ErrATTTimeout = errors.New("ATT timeout") - ErrATTUnknownEvent = errors.New("ATT unknown event") - ErrATTUnknown = errors.New("ATT unknown error") - ErrATTOp = errors.New("ATT OP error") + ErrATTTimeout = errors.New("bluetooth: ATT timeout") + ErrATTUnknownEvent = errors.New("bluetooth: ATT unknown event") + ErrATTUnknown = errors.New("bluetooth: ATT unknown error") + ErrATTOp = errors.New("bluetooth: ATT OP error") ) type rawService struct { @@ -94,8 +94,9 @@ type rawDescriptor struct { } type rawNotification struct { - handle uint16 - data []byte + connectionHandle uint16 + handle uint16 + data []byte } type att struct { @@ -108,6 +109,7 @@ type att struct { lastErrorCode uint8 services []rawService characteristics []rawCharacteristic + descriptors []rawDescriptor value []byte notifications chan rawNotification } @@ -164,6 +166,26 @@ func (a *att) readByTypeReq(connectionHandle, startHandle, endHandle uint16, typ return a.waitUntilResponse() } +func (a *att) findInfoReq(connectionHandle, startHandle, endHandle uint16) error { + if _debug { + println("att.findInfoReq:", connectionHandle, startHandle, endHandle) + } + + a.busy.Lock() + defer a.busy.Unlock() + + var b [5]byte + b[0] = attOpFindInfoReq + binary.LittleEndian.PutUint16(b[1:], startHandle) + binary.LittleEndian.PutUint16(b[3:], endHandle) + + if err := a.sendReq(connectionHandle, b[:]); err != nil { + return err + } + + return a.waitUntilResponse() +} + func (a *att) readReq(connectionHandle, valueHandle uint16) error { if _debug { println("att.readReq:", connectionHandle, valueHandle) @@ -185,7 +207,7 @@ func (a *att) readReq(connectionHandle, valueHandle uint16) error { func (a *att) writeCmd(connectionHandle, valueHandle uint16, data []byte) error { if _debug { - println("att.writeCmd:", connectionHandle, valueHandle) + println("att.writeCmd:", connectionHandle, valueHandle, hex.EncodeToString(data)) } a.busy.Lock() @@ -202,6 +224,25 @@ func (a *att) writeCmd(connectionHandle, valueHandle uint16, data []byte) error return a.waitUntilResponse() } +func (a *att) writeReq(connectionHandle, valueHandle uint16, data []byte) error { + if _debug { + println("att.writeReq:", connectionHandle, valueHandle, hex.EncodeToString(data)) + } + + a.busy.Lock() + defer a.busy.Unlock() + + var b [3]byte + b[0] = attOpWriteReq + binary.LittleEndian.PutUint16(b[1:], valueHandle) + + if err := a.sendReq(connectionHandle, append(b[:], data...)); err != nil { + return err + } + + return a.waitUntilResponse() +} + func (a *att) sendReq(handle uint16, data []byte) error { a.clearResponse() @@ -253,6 +294,30 @@ func (a *att) handleData(handle uint16, buf []byte) error { if _debug { println("att.handleData: attOpFindInfoResponse") } + a.responded = true + + lengthPerDescriptor := int(buf[1]) + var uuid [16]byte + + for i := 2; i < len(buf); i += lengthPerDescriptor { + d := rawDescriptor{ + handle: binary.LittleEndian.Uint16(buf[i:]), + } + switch lengthPerDescriptor - 2 { + case 2: + d.uuid = New16BitUUID(binary.LittleEndian.Uint16(buf[i+2:])) + case 16: + copy(uuid[:], buf[i+2:]) + slices.Reverse(uuid[:]) + d.uuid = NewUUID(uuid) + } + + if _debug { + println("att.handleData: descriptor", d.handle, d.uuid.String()) + } + + a.descriptors = append(a.descriptors, d) + } case attOpFindByTypeReq: if _debug { @@ -302,6 +367,33 @@ func (a *att) handleData(handle uint16, buf []byte) error { println("att.handleData: attOpReadByGroupReq") } + // return generic services + var response [14]byte + response[0] = attOpReadByGroupResponse + response[1] = 0x06 // length per service + + genericAccessService := rawService{ + startHandle: 0, + endHandle: 1, + uuid: ServiceUUIDGenericAccess, + } + binary.LittleEndian.PutUint16(response[2:], genericAccessService.startHandle) + binary.LittleEndian.PutUint16(response[4:], genericAccessService.endHandle) + binary.LittleEndian.PutUint16(response[6:], genericAccessService.uuid.Get16Bit()) + + genericAttributeService := rawService{ + startHandle: 2, + endHandle: 5, + uuid: ServiceUUIDGenericAttribute, + } + binary.LittleEndian.PutUint16(response[8:], genericAttributeService.startHandle) + binary.LittleEndian.PutUint16(response[10:], genericAttributeService.endHandle) + binary.LittleEndian.PutUint16(response[12:], genericAttributeService.uuid.Get16Bit()) + + if err := a.hci.sendAclPkt(handle, attCID, response[:]); err != nil { + return err + } + case attOpReadByGroupResponse: if _debug { println("att.handleData: attOpReadByGroupResponse") @@ -383,8 +475,9 @@ func (a *att) handleData(handle uint16, buf []byte) error { } not := rawNotification{ - handle: binary.LittleEndian.Uint16(buf[1:]), - data: []byte{}, + connectionHandle: handle, + handle: binary.LittleEndian.Uint16(buf[1:]), + data: []byte{}, } not.data = append(not.data, buf[3:]...) @@ -460,7 +553,9 @@ func (a *att) poll() error { a.busy.Lock() defer a.busy.Unlock() - a.hci.poll() + if err := a.hci.poll(); err != nil { + return err + } return nil } diff --git a/gap_ninafw.go b/gap_ninafw.go index ff907ed8..9ebae262 100644 --- a/gap_ninafw.go +++ b/gap_ninafw.go @@ -8,7 +8,7 @@ import ( ) var ( - ErrConnect = errors.New("could not connect") + ErrConnect = errors.New("bluetooth: could not connect") ) // Scan starts a BLE scan. @@ -76,9 +76,15 @@ func (a *Adapter) Scan(callback func(*Adapter, ScanResult)) error { i += l + 1 } + random := a.hci.advData.peerBdaddrType == 0x01 + callback(a, ScanResult{ - Address: Address{MACAddress{MAC: makeAddress(a.hci.advData.peerBdaddr)}, - a.hci.advData.peerBdaddrType}, + Address: Address{ + MACAddress{ + MAC: makeAddress(a.hci.advData.peerBdaddr), + isRandom: random, + }, + }, RSSI: int16(a.hci.advData.rssi), AdvertisementPayload: &advertisementFields{ AdvertisementFields: adf, @@ -122,8 +128,6 @@ func (a *Adapter) StopScan() error { // Address contains a Bluetooth MAC address. type Address struct { MACAddress - - random uint8 } // Connect starts a connection attempt to the given peripheral device address. @@ -132,8 +136,12 @@ func (a *Adapter) Connect(address Address, params ConnectionParams) (*Device, er println("Connect") } + random := uint8(0) + if address.isRandom { + random = 1 + } if err := a.hci.leCreateConn(0x0060, 0x0030, 0x00, - address.random, makeNINAAddress(address.MAC), + random, makeNINAAddress(address.MAC), 0x00, 0x0006, 0x000c, 0x0000, 0x00c8, 0x0004, 0x0006); err != nil { return nil, err } @@ -148,13 +156,22 @@ func (a *Adapter) Connect(address Address, params ConnectionParams) (*Device, er switch { case a.hci.connectData.connected: defer a.hci.clearConnectData() + + random := false + if address.isRandom { + random = true + } + d := &Device{adapter: a, handle: a.hci.connectData.handle, - Address: Address{MACAddress{MAC: makeAddress(a.hci.connectData.peerBdaddr)}, - a.hci.connectData.peerBdaddrType}, - services: []DeviceService{}, + Address: Address{ + MACAddress{ + MAC: makeAddress(a.hci.connectData.peerBdaddr), + isRandom: random}, + }, + notificationRegistrations: make([]notificationRegistration, 0), } - a.connectedDevices[d.handle] = d + a.connectedDevices = append(a.connectedDevices, d) return d, nil @@ -176,13 +193,18 @@ func (a *Adapter) Connect(address Address, params ConnectionParams) (*Device, er return nil, ErrConnect } +type notificationRegistration struct { + handle uint16 + callback func([]byte) +} + // Device is a connection to a remote peripheral. type Device struct { adapter *Adapter Address Address handle uint16 - services []DeviceService + notificationRegistrations []notificationRegistration } // Disconnect from the BLE device. @@ -194,6 +216,28 @@ func (d *Device) Disconnect() error { return err } - delete(d.adapter.connectedDevices, d.handle) + d.adapter.connectedDevices = []*Device{} return nil } + +func (d *Device) findNotificationRegistration(handle uint16) *notificationRegistration { + for _, n := range d.notificationRegistrations { + if n.handle == handle { + return &n + } + } + + return nil +} + +func (d *Device) addNotificationRegistration(handle uint16, callback func([]byte)) { + d.notificationRegistrations = append(d.notificationRegistrations, + notificationRegistration{ + handle: handle, + callback: callback, + }) +} + +func (d *Device) startNotifications() { + d.adapter.startNotifications() +} diff --git a/gattc_ninafw.go b/gattc_ninafw.go index 176ebea0..3e6b6fd2 100644 --- a/gattc_ninafw.go +++ b/gattc_ninafw.go @@ -17,6 +17,11 @@ var ( errCharacteristicNotFound = errors.New("bluetooth: characteristic not found") ) +const ( + maxDefaultServicesToDiscover = 6 + maxDefaultCharacteristicsToDiscover = 8 +) + const ( charPropertyBroadcast = 0x01 charPropertyRead = 0x02 @@ -30,9 +35,7 @@ const ( type DeviceService struct { uuid UUID - device *Device - characteristics []DeviceCharacteristic - + device *Device startHandle, endHandle uint16 } @@ -53,7 +56,7 @@ func (d *Device) DiscoverServices(uuids []UUID) ([]DeviceService, error) { println("DiscoverServices") } - d.services = []DeviceService{} + services := make([]DeviceService, 0, maxDefaultServicesToDiscover) foundServices := make(map[UUID]DeviceService) startHandle := uint16(0x0001) @@ -68,47 +71,49 @@ func (d *Device) DiscoverServices(uuids []UUID) ([]DeviceService, error) { println("found d.adapter.att.services", len(d.adapter.att.services)) } - if len(d.adapter.att.services) > 0 { - for _, rawService := range d.adapter.att.services { - if len(uuids) == 0 || rawService.uuid.IsIn(uuids) { - foundServices[rawService.uuid] = - DeviceService{ - device: d, - uuid: rawService.uuid, - startHandle: rawService.startHandle, - endHandle: rawService.endHandle, - } - } + if len(d.adapter.att.services) == 0 { + break + } - startHandle = rawService.endHandle + 1 - if startHandle == 0x0000 { - endHandle = 0x0000 - } + for _, rawService := range d.adapter.att.services { + if len(uuids) == 0 || rawService.uuid.isIn(uuids) { + foundServices[rawService.uuid] = + DeviceService{ + device: d, + uuid: rawService.uuid, + startHandle: rawService.startHandle, + endHandle: rawService.endHandle, + } } - // reset raw services - d.adapter.att.services = []rawService{} - } else { - break + startHandle = rawService.endHandle + 1 + if startHandle == 0x0000 { + endHandle = 0x0000 + } } + + // reset raw services + d.adapter.att.services = []rawService{} } - // put into correct order if needed - if len(uuids) > 0 { + switch { + case len(uuids) > 0: + // put into correct order for _, uuid := range uuids { s, ok := foundServices[uuid] if !ok { return nil, errServiceNotFound } - d.services = append(d.services, s) + + services = append(services, s) } - } else { + default: for _, s := range foundServices { - d.services = append(d.services, s) + services = append(services, s) } } - return d.services, nil + return services, nil } // DeviceCharacteristic is a BLE characteristic on a connected peripheral @@ -120,6 +125,7 @@ type DeviceCharacteristic struct { permissions CharacteristicPermissions handle uint16 properties uint8 + callback func(buf []byte) } // UUID returns the UUID for this DeviceCharacteristic. @@ -141,7 +147,7 @@ func (s *DeviceService) DiscoverCharacteristics(uuids []UUID) ([]DeviceCharacter println("DiscoverCharacteristics") } - s.characteristics = []DeviceCharacteristic{} + characteristics := make([]DeviceCharacteristic, 0, maxDefaultCharacteristicsToDiscover) foundCharacteristics := make(map[UUID]DeviceCharacteristic) startHandle := s.startHandle @@ -163,64 +169,48 @@ func (s *DeviceService) DiscoverCharacteristics(uuids []UUID) ([]DeviceCharacter println("found s.device.adapter.att.characteristics", len(s.device.adapter.att.characteristics)) } - if len(s.device.adapter.att.characteristics) > 0 { - for _, rawCharacteristic := range s.device.adapter.att.characteristics { - 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 len(s.device.adapter.att.characteristics) == 0 { + break + } + + for _, rawCharacteristic := range s.device.adapter.att.characteristics { + if len(uuids) == 0 || rawCharacteristic.uuid.isIn(uuids) { + dc := DeviceCharacteristic{ + service: s, + uuid: rawCharacteristic.uuid, + handle: rawCharacteristic.valueHandle, + properties: rawCharacteristic.properties, + permissions: CharacteristicPermissions(rawCharacteristic.properties), } - startHandle = rawCharacteristic.valueHandle + 1 + foundCharacteristics[rawCharacteristic.uuid] = dc } - // reset raw characteristics - s.device.adapter.att.characteristics = []rawCharacteristic{} - } else { - break + startHandle = rawCharacteristic.valueHandle + 1 } + + // reset raw characteristics + s.device.adapter.att.characteristics = []rawCharacteristic{} } - // put into correct order if needed - if len(uuids) > 0 { + switch { + case len(uuids) > 0: + // put into correct order for _, uuid := range uuids { c, ok := foundCharacteristics[uuid] if !ok { return nil, errCharacteristicNotFound } - s.characteristics = append(s.characteristics, c) + characteristics = append(characteristics, c) } - } else { + default: for _, c := range foundCharacteristics { - s.characteristics = append(s.characteristics, c) + characteristics = append(characteristics, c) } + } - return s.characteristics, nil + return characteristics, nil } // WriteWithoutResponse replaces the characteristic value with a new value. The @@ -247,7 +237,39 @@ func (c DeviceCharacteristic) WriteWithoutResponse(p []byte) (n int, err error) // // Users may call EnableNotifications with a nil callback to disable notifications. func (c *DeviceCharacteristic) EnableNotifications(callback func(buf []byte)) error { - return errNotYetImplemented + if !c.permissions.Notify() { + return errNoNotify + } + + switch { + case callback == nil: + // disable notifications + if _debug { + println("disabling notifications") + } + + err := c.service.device.adapter.att.writeReq(c.service.device.handle, c.handle+1, []byte{0x00, 0x00}) + if err != nil { + return err + } + default: + // enable notifications + if _debug { + println("enabling notifications") + } + + err := c.service.device.adapter.att.writeReq(c.service.device.handle, c.handle+1, []byte{0x01, 0x00}) + if err != nil { + return err + } + } + + c.callback = callback + + c.service.device.startNotifications() + c.service.device.addNotificationRegistration(c.handle, c.callback) + + return nil } // GetMTU returns the MTU for the characteristic. diff --git a/gatts.go b/gatts.go index aa9b64d6..f092f514 100644 --- a/gatts.go +++ b/gatts.go @@ -56,3 +56,8 @@ func (p CharacteristicPermissions) Write() bool { func (p CharacteristicPermissions) WriteWithoutResponse() bool { return p&CharacteristicWriteWithoutResponsePermission != 0 } + +// Notify returns whether notifications are permitted. +func (p CharacteristicPermissions) Notify() bool { + return p&CharacteristicNotifyPermission != 0 +} diff --git a/hci_ninafw.go b/hci_ninafw.go index 77d4e3ed..4b1db956 100644 --- a/hci_ninafw.go +++ b/hci_ninafw.go @@ -90,11 +90,11 @@ const ( ) var ( - ErrHCITimeout = errors.New("HCI timeout") - ErrHCIUnknownEvent = errors.New("HCI unknown event") - ErrHCIUnknown = errors.New("HCI unknown error") - ErrHCIInvalidPacket = errors.New("HCI invalid packet") - ErrHCIHardware = errors.New("HCI hardware error") + ErrHCITimeout = errors.New("bluetooth: HCI timeout") + ErrHCIUnknownEvent = errors.New("bluetooth: HCI unknown event") + ErrHCIUnknown = errors.New("bluetooth: HCI unknown error") + ErrHCIInvalidPacket = errors.New("bluetooth: HCI invalid packet") + ErrHCIHardware = errors.New("bluetooth: HCI hardware error") ) type leAdvertisingReport struct { @@ -159,6 +159,9 @@ func (h *hci) poll() error { done, err := h.processPacket(i) switch { case err == ErrHCIUnknown || err == ErrHCIInvalidPacket || err == ErrHCIUnknownEvent: + if _debug { + println("hci error:", err.Error()) + } i = 0 time.Sleep(5 * time.Millisecond) case err != nil: diff --git a/uuid_ninafw.go b/uuid_ninafw.go index d35f81fe..107e11f5 100644 --- a/uuid_ninafw.go +++ b/uuid_ninafw.go @@ -8,3 +8,13 @@ type shortUUID uint16 func (s shortUUID) UUID() UUID { return New16BitUUID(uint16(s)) } + +// isIn checks the passed in slice of UUIDs to see if this uuid is in it. +func (uuid UUID) isIn(uuids []UUID) bool { + for _, u := range uuids { + if u == uuid { + return true + } + } + return false +}