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 <ron@hybridgroup.com>
This commit is contained in:
deadprogram 2024-01-21 13:04:26 +01:00 committed by BCG
parent 0a9bffe397
commit 07a9e1d02e
6 changed files with 307 additions and 31 deletions

View file

@ -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 {

View file

@ -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 {

View file

@ -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)
}
}()

View file

@ -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
}

View file

@ -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<<ogfCommandPos|0x01, b[:])
}
func (h *hci) readLeBufferSize() error {
if err := h.sendCommand(ogfLECtrl<<ogfCommandPos | ocfLEReadBufferSize); err != nil {
return err
}
pktLen := binary.LittleEndian.Uint16(h.buf[0:])
h.maxPkt = uint16(h.buf[2])
// pkt len must be at least 27 bytes
if pktLen < 27 {
pktLen = 27
}
if err := h.att.setMaxMTU(pktLen); err != nil {
return err
}
return nil
}
func (h *hci) leSetScanEnable(enabled, duplicates bool) error {
h.scanning = enabled
@ -357,6 +387,21 @@ func (h *hci) leCancelConn() error {
return h.sendCommand(ogfLECtrl<<ogfCommandPos | ocfLECancelConn)
}
func (h *hci) leConnUpdate(handle uint16, minInterval, maxInterval,
latency, supervisionTimeout uint16) error {
var b [14]byte
binary.LittleEndian.PutUint16(b[0:], handle)
binary.LittleEndian.PutUint16(b[2:], minInterval)
binary.LittleEndian.PutUint16(b[4:], maxInterval)
binary.LittleEndian.PutUint16(b[6:], latency)
binary.LittleEndian.PutUint16(b[8:], supervisionTimeout)
binary.LittleEndian.PutUint16(b[10:], 0x0004)
binary.LittleEndian.PutUint16(b[12:], 0x0006)
return h.sendCommandWithParams(ogfLECtrl<<ogfCommandPos|ocfLEConnUpdate, b[:])
}
func (h *hci) disconnect(handle uint16) error {
var b [3]byte
binary.LittleEndian.PutUint16(b[0:], handle)
@ -437,6 +482,8 @@ func (h *hci) sendAclPkt(handle uint16, cid uint8, data []byte) error {
return err
}
h.pendingPkt++
return nil
}
@ -492,6 +539,13 @@ func (h *hci) handleACLData(buf []byte) error {
} else {
return h.att.handleData(aclHdr.handle&0x0fff, buf[8:aclHdr.len+8])
}
case signalingCID:
if debug {
println("signaling cid", aclHdr.cid, hex.EncodeToString(buf))
}
return h.l2cap.handleData(aclHdr.handle&0x0fff, buf[8:aclHdr.len+8])
default:
if debug {
println("unknown acl data cid", aclHdr.cid)
@ -513,6 +567,7 @@ func (h *hci) handleEventData(buf []byte) error {
handle := binary.LittleEndian.Uint16(buf[3:])
h.att.removeConnection(handle)
h.l2cap.removeConnection(handle)
return h.leSetAdvertiseEnable(true)
@ -549,8 +604,28 @@ func (h *hci) handleEventData(buf []byte) error {
case evtNumCompPkts:
if debug {
println("evtNumCompPkts")
println("evtNumCompPkts", hex.EncodeToString(buf))
}
// count of handles
c := buf[2]
pkts := uint16(0)
for i := byte(0); i < c; i++ {
pkts += binary.LittleEndian.Uint16(buf[5+i*4:])
}
if pkts > 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)

156
l2cap_hci.go Normal file
View file

@ -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)
}