hci: improved implementation and fixes

adapter_ninafw.go

@@ -105,6 +105,9 @@ func newBLEStack(uart *machine.UART) (*hci, *att) {
 	a := newATT(h)
 	h.att = a
 
+	l := newL2CAP(h)
+	h.l2cap = l
+
 	return h, a
 }
 
@@ -171,7 +174,7 @@ func (a *Adapter) startNotifications() {
 				}
 			}
 
-			time.Sleep(10 * time.Millisecond)
+			time.Sleep(5 * time.Millisecond)
 		}
 	}()
 
@@ -184,7 +187,7 @@ func (a *Adapter) startNotifications() {
 					println("notification received", not.connectionHandle, not.handle, not.data)
 				}
 
-				d := a.findDevice(not.connectionHandle)
+				d := a.findConnection(not.connectionHandle)
 				if d.deviceInternal == nil {
 					if debug {
 						println("no device found for handle", not.connectionHandle)
@@ -212,7 +215,23 @@ func (a *Adapter) startNotifications() {
 	}()
 }
 
-func (a *Adapter) findDevice(handle uint16) Device {
+func (a *Adapter) addConnection(d Device) {
+	a.connectedDevices = append(a.connectedDevices, d)
+}
+
+func (a *Adapter) removeConnection(d Device) {
+	for i := range a.connectedDevices {
+		if d.handle == a.connectedDevices[i].handle {
+			a.connectedDevices[i] = a.connectedDevices[len(a.connectedDevices)-1]
+			a.connectedDevices[len(a.connectedDevices)-1] = Device{}
+			a.connectedDevices = a.connectedDevices[:len(a.connectedDevices)-1]
+
+			return
+		}
+	}
+}
+
+func (a *Adapter) findConnection(handle uint16) Device {
 	for _, d := range a.connectedDevices {
 		if d.handle == handle {
 			if debug {

att_ninafw.go

@@ -12,9 +12,6 @@ import (
 )
 
 const (
-	attCID = 0x0004
-	bleCTL = 0x0008
-
 	attOpError               = 0x01
 	attOpMTUReq              = 0x02
 	attOpMTUResponse         = 0x03
@@ -261,6 +258,7 @@ type att struct {
 	lastErrorHandle uint16
 	lastErrorCode   uint8
 	mtu             uint16
+	maxMTU          uint16
 	services        []rawService
 	characteristics []rawCharacteristic
 	descriptors     []rawDescriptor
@@ -284,6 +282,7 @@ func newATT(hci *hci) *att {
 		lastHandle:      0x0001,
 		attributes:      []rawAttribute{},
 		localServices:   []rawService{},
+		maxMTU:          248,
 	}
 }
 
@@ -384,7 +383,7 @@ func (a *att) writeCmd(connectionHandle, valueHandle uint16, data []byte) error
 		return err
 	}
 
-	return a.waitUntilResponse()
+	return nil
 }
 
 func (a *att) writeReq(connectionHandle, valueHandle uint16, data []byte) error {
@@ -406,7 +405,7 @@ func (a *att) writeReq(connectionHandle, valueHandle uint16, data []byte) error
 	return a.waitUntilResponse()
 }
 
-func (a *att) mtuReq(connectionHandle, mtu uint16) error {
+func (a *att) mtuReq(connectionHandle uint16) error {
 	if debug {
 		println("att.mtuReq:", connectionHandle)
 	}
@@ -416,7 +415,7 @@ func (a *att) mtuReq(connectionHandle, mtu uint16) error {
 
 	var b [3]byte
 	b[0] = attOpMTUReq
-	binary.LittleEndian.PutUint16(b[1:], mtu)
+	binary.LittleEndian.PutUint16(b[1:], a.mtu)
 
 	if err := a.sendReq(connectionHandle, b[:]); err != nil {
 		return err
@@ -425,6 +424,12 @@ func (a *att) mtuReq(connectionHandle, mtu uint16) error {
 	return a.waitUntilResponse()
 }
 
+func (a *att) setMaxMTU(mtu uint16) error {
+	a.maxMTU = mtu
+
+	return nil
+}
+
 func (a *att) sendReq(handle uint16, data []byte) error {
 	a.clearResponse()
 
@@ -504,11 +509,21 @@ func (a *att) handleData(handle uint16, buf []byte) error {
 
 	case attOpMTUReq:
 		if debug {
-			println("att.handleData: attOpMTUReq")
+			println("att.handleData: attOpMTUReq", hex.EncodeToString(buf))
 		}
-		a.mtu = binary.LittleEndian.Uint16(buf[1:])
-		response := [3]byte{attOpMTUResponse, buf[1], buf[2]}
-		if err := a.hci.sendAclPkt(handle, attCID, response[:]); err != nil {
+		mtu := binary.LittleEndian.Uint16(buf[1:])
+		if mtu > a.maxMTU {
+			mtu = a.maxMTU
+		}
+
+		// save mtu for connection
+		a.mtu = mtu
+
+		var b [3]byte
+		b[0] = attOpMTUResponse
+		binary.LittleEndian.PutUint16(b[1:], mtu)
+
+		if err := a.hci.sendAclPkt(handle, attCID, b[:]); err != nil {
 			return err
 		}
 
@@ -1032,7 +1047,7 @@ func (a *att) waitUntilResponse() error {
 				break
 			}
 
-			time.Sleep(100 * time.Millisecond)
+			time.Sleep(5 * time.Millisecond)
 		}
 	}
 
@@ -1050,17 +1065,21 @@ func (a *att) poll() error {
 	return nil
 }
 
-func (a *att) addConnection(handle uint16) {
+func (a *att) addConnection(handle uint16) error {
 	a.connections = append(a.connections, handle)
+
+	return nil
 }
 
-func (a *att) removeConnection(handle uint16) {
+func (a *att) removeConnection(handle uint16) error {
 	for i := range a.connections {
 		if a.connections[i] == handle {
 			a.connections = append(a.connections[:i], a.connections[i+1:]...)
-			return
+			break
 		}
 	}
+
+	return nil
 }
 
 func (a *att) addLocalAttribute(typ attributeType, parent uint16, uuid UUID, permissions CharacteristicPermissions, value []byte) uint16 {

gap_ninafw.go

@@ -96,7 +96,7 @@ func (a *Adapter) Scan(callback func(*Adapter, ScanResult)) error {
 			})
 
 			a.hci.clearAdvData()
-			time.Sleep(10 * time.Millisecond)
+			time.Sleep(5 * time.Millisecond)
 
 		default:
 			if !a.scanning {
@@ -108,7 +108,7 @@ func (a *Adapter) Scan(callback func(*Adapter, ScanResult)) error {
 				lastUpdate = time.Now().UnixNano()
 			}
 
-			time.Sleep(10 * time.Millisecond)
+			time.Sleep(5 * time.Millisecond)
 		}
 	}
 
@@ -178,7 +178,7 @@ func (a *Adapter) Connect(address Address, params ConnectionParams) (Device, err
 					notificationRegistrations: make([]notificationRegistration, 0),
 				},
 			}
-			a.connectedDevices = append(a.connectedDevices, d)
+			a.addConnection(d)
 
 			return d, nil
 
@@ -188,7 +188,7 @@ func (a *Adapter) Connect(address Address, params ConnectionParams) (Device, err
 				break
 			}
 
-			time.Sleep(10 * time.Millisecond)
+			time.Sleep(5 * time.Millisecond)
 		}
 	}
 
@@ -228,7 +228,7 @@ func (d Device) Disconnect() error {
 		return err
 	}
 
-	d.adapter.connectedDevices = []Device{}
+	d.adapter.removeConnection(d)
 	return nil
 }
 
@@ -405,7 +405,7 @@ func (a *Advertisement) Start() error {
 				}
 			}
 
-			time.Sleep(10 * time.Millisecond)
+			time.Sleep(5 * time.Millisecond)
 		}
 	}()
 

gattc_ninafw.go

@@ -18,8 +18,8 @@ var (
 )
 
 const (
-	maxDefaultServicesToDiscover        = 6
-	maxDefaultCharacteristicsToDiscover = 8
+	maxDefaultServicesToDiscover        = 8
+	maxDefaultCharacteristicsToDiscover = 16
 )
 
 const (
@@ -94,6 +94,11 @@ func (d Device) DiscoverServices(uuids []UUID) ([]DeviceService, error) {
 
 		// reset raw services
 		d.adapter.att.services = []rawService{}
+
+		// did we find them all?
+		if len(foundServices) == len(uuids) {
+			break
+		}
 	}
 
 	switch {
@@ -191,6 +196,11 @@ func (s DeviceService) DiscoverCharacteristics(uuids []UUID) ([]DeviceCharacteri
 
 		// reset raw characteristics
 		s.device.adapter.att.characteristics = []rawCharacteristic{}
+
+		// did we find them all?
+		if len(foundCharacteristics) == len(uuids) {
+			break
+		}
 	}
 
 	switch {
@@ -274,7 +284,7 @@ func (c DeviceCharacteristic) EnableNotifications(callback func(buf []byte)) err
 
 // GetMTU returns the MTU for the characteristic.
 func (c DeviceCharacteristic) GetMTU() (uint16, error) {
-	err := c.service.device.adapter.att.mtuReq(c.service.device.handle, c.service.device.mtu)
+	err := c.service.device.adapter.att.mtuReq(c.service.device.handle)
 	if err != nil {
 		return 0, err
 	}