From 07a9e1d02e86b61c83bc218c684ee4b550933b39 Mon Sep 17 00:00:00 2001 From: deadprogram Date: Sun, 21 Jan 2024 13:04:26 +0100 Subject: [PATCH] hci: several improvements and fixes including: - add l2cap signaling support - implement evtNumCompPkts to count in-flight packets - correct implementation for WriteWithoutReponse - speed up time waiting for hardware - corrections to MTU exchange Signed-off-by: deadprogram --- adapter_ninafw.go | 17 ++--- att_ninafw.go | 47 +++++++++----- gap_ninafw.go | 12 ++-- gattc_ninafw.go | 16 ++++- hci_ninafw.go | 90 +++++++++++++++++++++++++- l2cap_hci.go | 156 ++++++++++++++++++++++++++++++++++++++++++++++ 6 files changed, 307 insertions(+), 31 deletions(-) create mode 100644 l2cap_hci.go diff --git a/adapter_ninafw.go b/adapter_ninafw.go index 43eebe3..739b8c8 100644 --- a/adapter_ninafw.go +++ b/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.findConnectedDevice(not.connectionHandle) + d := a.findConnection(not.connectionHandle) if d.deviceInternal == nil { if debug { println("no device found for handle", not.connectionHandle) @@ -212,15 +215,15 @@ func (a *Adapter) startNotifications() { }() } -func (a *Adapter) addDevice(d Device) { +func (a *Adapter) addConnection(d Device) { a.connectedDevices = append(a.connectedDevices, d) } -func (a *Adapter) removeDevice(d Device) { +func (a *Adapter) removeConnection(d Device) { for i := range a.connectedDevices { if d.handle == a.connectedDevices[i].handle { - copy(a.connectedDevices[i:], a.connectedDevices[i+1:]) - a.connectedDevices[len(a.connectedDevices)-1] = Device{} // the zero value of T + 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 @@ -228,7 +231,7 @@ func (a *Adapter) removeDevice(d Device) { } } -func (a *Adapter) findConnectedDevice(handle uint16) Device { +func (a *Adapter) findConnection(handle uint16) Device { for _, d := range a.connectedDevices { if d.handle == handle { if debug { diff --git a/att_ninafw.go b/att_ninafw.go index b09e228..ef34228 100644 --- a/att_ninafw.go +++ b/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 { diff --git a/gap_ninafw.go b/gap_ninafw.go index 488a07c..06f82bc 100644 --- a/gap_ninafw.go +++ b/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.addDevice(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.removeDevice(d) + 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) } }() diff --git a/gattc_ninafw.go b/gattc_ninafw.go index 03b169d..ec42e59 100644 --- a/gattc_ninafw.go +++ b/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 } diff --git a/hci_ninafw.go b/hci_ninafw.go index d0a00f8..eea27fb 100644 --- a/hci_ninafw.go +++ b/hci_ninafw.go @@ -87,6 +87,11 @@ const ( const ( hciACLLenPos = 4 hciEvtLenPos = 2 + + attCID = 0x0004 + bleCTL = 0x0008 + signalingCID = 0x0005 + securityCID = 0x0006 ) var ( @@ -113,6 +118,8 @@ type leConnectData struct { role uint8 peerBdaddrType uint8 peerBdaddr [6]uint8 + interval uint16 + timeout uint16 } type hci struct { @@ -120,6 +127,7 @@ type hci struct { softCTS machine.Pin softRTS machine.Pin att *att + l2cap *l2cap buf []byte address [6]byte cmdCompleteOpcode uint16 @@ -128,6 +136,8 @@ type hci struct { scanning bool advData leAdvertisingReport connectData leConnectData + maxPkt uint16 + pendingPkt uint16 } func newHCI(uart *machine.UART) *hci { @@ -263,6 +273,26 @@ func (h *hci) setLeEventMask(eventMask uint64) error { return h.sendCommandWithParams(ogfLECtrl< 0 && h.pendingPkt > pkts { + h.pendingPkt -= pkts + } else { + h.pendingPkt = 0 + } + + if debug { + println("evtNumCompPkts", pkts, h.pendingPkt) + } + + return nil + case evtLEMetaEvent: if debug { println("evtLEMetaEvent") @@ -569,7 +644,20 @@ func (h *hci) handleEventData(buf []byte) error { h.connectData.peerBdaddrType = buf[7] copy(h.connectData.peerBdaddr[0:], buf[8:]) + switch buf[2] { + case leMetaEventConnComplete: + h.connectData.interval = binary.LittleEndian.Uint16(buf[14:]) + h.connectData.timeout = binary.LittleEndian.Uint16(buf[16:]) + case leMetaEventEnhancedConnectionComplete: + h.connectData.interval = binary.LittleEndian.Uint16(buf[26:]) + h.connectData.timeout = binary.LittleEndian.Uint16(buf[28:]) + } + h.att.addConnection(h.connectData.handle) + if err := h.l2cap.addConnection(h.connectData.handle, h.connectData.role, + h.connectData.interval, h.connectData.timeout); err != nil { + return err + } return h.leSetAdvertiseEnable(false) diff --git a/l2cap_hci.go b/l2cap_hci.go new file mode 100644 index 0000000..84ad644 --- /dev/null +++ b/l2cap_hci.go @@ -0,0 +1,156 @@ +//go:build ninafw + +package bluetooth + +import ( + "encoding/binary" + "encoding/hex" +) + +const ( + connectionParamUpdateRequest = 0x12 + connectionParamUpdateResponse = 0x13 +) + +type l2capConnectionParamReqPkt struct { + minInterval uint16 + maxInterval uint16 + latency uint16 + timeout uint16 +} + +func (l *l2capConnectionParamReqPkt) Write(buf []byte) (int, error) { + l.minInterval = binary.LittleEndian.Uint16(buf[0:]) + l.maxInterval = binary.LittleEndian.Uint16(buf[2:]) + l.latency = binary.LittleEndian.Uint16(buf[4:]) + l.timeout = binary.LittleEndian.Uint16(buf[6:]) + + return 8, nil +} + +func (l *l2capConnectionParamReqPkt) Read(p []byte) (int, error) { + binary.LittleEndian.PutUint16(p[0:], l.minInterval) + binary.LittleEndian.PutUint16(p[2:], l.maxInterval) + binary.LittleEndian.PutUint16(p[4:], l.latency) + binary.LittleEndian.PutUint16(p[6:], l.timeout) + + return 8, nil +} + +type l2capConnectionParamResponsePkt struct { + code uint8 + identifier uint8 + length uint16 + value uint16 +} + +func (l *l2capConnectionParamResponsePkt) Read(p []byte) (int, error) { + p[0] = l.code + p[1] = l.identifier + binary.LittleEndian.PutUint16(p[2:], l.length) + binary.LittleEndian.PutUint16(p[4:], l.value) + + return 6, nil +} + +type l2cap struct { + hci *hci +} + +func newL2CAP(hci *hci) *l2cap { + return &l2cap{ + hci: hci, + } +} + +func (l *l2cap) addConnection(handle uint16, role uint8, interval, timeout uint16) error { + if role != 0x01 { + return nil + } + + var b [12]byte + b[0] = connectionParamUpdateRequest + b[1] = 0x01 + binary.LittleEndian.PutUint16(b[2:], 8) + binary.LittleEndian.PutUint16(b[4:], interval) + binary.LittleEndian.PutUint16(b[6:], interval) + binary.LittleEndian.PutUint16(b[8:], 0) + binary.LittleEndian.PutUint16(b[10:], timeout) + + return l.sendReq(handle, b[:]) +} + +func (l *l2cap) removeConnection(handle uint16) error { + return nil +} + +func (l *l2cap) handleData(handle uint16, buf []byte) error { + code := buf[0] + identifier := buf[1] + //length := binary.LittleEndian.Uint16(buf[2:4]) + + if debug { + println("l2cap.handleData:", handle, "data:", hex.EncodeToString(buf)) + } + + // TODO: check length + + switch code { + case connectionParamUpdateRequest: + return l.handleParameterUpdateRequest(handle, identifier, buf[4:]) + + case connectionParamUpdateResponse: + return l.handleParameterUpdateResponse(handle, identifier, buf[4:]) + } + + return nil +} + +func (l *l2cap) handleParameterUpdateRequest(connectionHandle uint16, identifier uint8, data []byte) error { + if debug { + println("l2cap.handleParameterUpdateRequest:", connectionHandle, "data:", hex.EncodeToString(data)) + } + + req := l2capConnectionParamReqPkt{} + req.Write(data) + + // TODO: check against min/max + + resp := l2capConnectionParamResponsePkt{ + code: connectionParamUpdateResponse, + identifier: identifier, + length: 2, + value: 0, + } + + var b [6]byte + resp.Read(b[:]) + + if err := l.sendReq(connectionHandle, b[:]); err != nil { + return err + } + + // valid so update connection parameters + if resp.value == 0 { + return l.hci.leConnUpdate(connectionHandle, req.minInterval, req.maxInterval, req.latency, req.timeout) + } + + return nil +} + +func (l *l2cap) handleParameterUpdateResponse(connectionHandle uint16, identifier uint8, data []byte) error { + if debug { + println("l2cap.handleParameterUpdateResponse:", connectionHandle, "data:", hex.EncodeToString(data)) + } + + // for now do nothing + return nil +} + +func (l *l2cap) sendReq(handle uint16, data []byte) error { + if debug { + println("l2cap.sendReq:", handle, "data:", hex.EncodeToString(data)) + } + + return l.hci.sendAclPkt(handle, signalingCID, data) +}