hci: add l2cap for signaling

Signed-off-by: deadprogram <ron@hybridgroup.com>

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
 }
 
@@ -184,7 +187,7 @@ func (a *Adapter) startNotifications() {
 					println("notification received", not.connectionHandle, not.handle, not.data)
 				}
 
-				d := a.findDevice(not.connectionHandle)
+				d := a.findConnectedDevice(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) addDevice(d Device) {
+	a.connectedDevices = append(a.connectedDevices, d)
+}
+
+func (a *Adapter) removeDevice(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 = a.connectedDevices[:len(a.connectedDevices)-1]
+
+			return
+		}
+	}
+}
+
+func (a *Adapter) findConnectedDevice(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:          23,
 	}
 }
 
@@ -408,12 +407,19 @@ func (a *att) writeReq(connectionHandle, valueHandle uint16, data []byte) error
 
 func (a *att) mtuReq(connectionHandle, mtu uint16) error {
 	if debug {
-		println("att.mtuReq:", connectionHandle)
+		println("att.mtuReq:", connectionHandle, mtu)
 	}
 
 	a.busy.Lock()
 	defer a.busy.Unlock()
 
+	if mtu > a.maxMTU {
+		mtu = a.maxMTU
+	}
+
+	// save mtu for connection
+	a.mtu = mtu
+
 	var b [3]byte
 	b[0] = attOpMTUReq
 	binary.LittleEndian.PutUint16(b[1:], mtu)
@@ -425,6 +431,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()
 
@@ -1050,17 +1062,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 {

examples/multiples/main.go

@@ -0,0 +1,146 @@
+// This example scans and then connects to multiple Bluetooth peripherals
+// that provide the Heart Rate Service (HRS).
+//
+// Once connected to all the desired devices, it subscribes to notifications.
+//
+// To run on bare metal microcontroller:
+// tinygo flash -target metro-m4-airlift -ldflags="-X main.wanted=D9:2A:A1:5C:ED:56,4D:A1:3C:24:F0:46" -monitor ./examples/multiples/
+//
+// To run on OS:
+// go run ./examples/multiples/ D9:2A:A1:5C:ED:56,64:0B:1D:46:D8:1D
+package main
+
+import (
+	"context"
+	"os"
+	"slices"
+	"time"
+
+	"tinygo.org/x/bluetooth"
+)
+
+var (
+	adapter = bluetooth.DefaultAdapter
+
+	heartRateServiceUUID        = bluetooth.ServiceUUIDHeartRate
+	heartRateCharacteristicUUID = bluetooth.CharacteristicUUIDHeartRateMeasurement
+
+	exitCtx context.Context
+)
+
+func main() {
+	exitCtx = initExitHandler()
+
+	println("enabling")
+
+	// Enable BLE interface.
+	must("enable BLE stack", adapter.Enable())
+
+	scanResults := make(map[string]bluetooth.ScanResult)
+	finished := make(chan bool, 1)
+
+	searchList, _ := connectAddresses()
+
+	// Start scanning.
+	println("scanning...")
+	err := adapter.Scan(func(adapter *bluetooth.Adapter, result bluetooth.ScanResult) {
+		print(".")
+		// is the scanned device one of the ones we want?
+		if slices.Contains(searchList, result.Address.String()) {
+			if _, ok := scanResults[result.Address.String()]; !ok {
+				println(".")
+				println("found device:", result.Address.String(), result.RSSI, result.LocalName())
+				scanResults[result.Address.String()] = result
+			}
+
+			if len(scanResults) == len(searchList) {
+				println(".")
+				adapter.StopScan()
+				finished <- true
+			}
+		}
+		select {
+		case <-exitCtx.Done():
+			println("exiting.")
+			os.Exit(0)
+		default:
+		}
+	})
+	must("scan", err)
+
+	devices := []bluetooth.Device{}
+	select {
+	case <-time.After(5 * time.Second):
+		failMessage("timed out")
+		return
+	case <-exitCtx.Done():
+		println("exiting.")
+		return
+	case <-finished:
+	}
+
+	defer func() {
+		for _, device := range devices {
+			device.Disconnect()
+		}
+	}()
+
+	// now connect to all devices
+	for _, result := range scanResults {
+		device, err := adapter.Connect(result.Address, bluetooth.ConnectionParams{})
+		if err != nil {
+			failMessage(err.Error())
+			return
+		}
+
+		println("connected to", result.Address.String())
+		devices = append(devices, device)
+	}
+
+	// get services
+	println("discovering services/characteristics")
+
+	for _, device := range devices {
+		srvcs, err := device.DiscoverServices([]bluetooth.UUID{heartRateServiceUUID})
+		must("discover services", err)
+
+		if len(srvcs) == 0 {
+			failMessage("could not find heart rate service")
+			return
+		}
+
+		srvc := srvcs[0]
+
+		println("found service", srvc.UUID().String(), "for device", device.Address.String())
+
+		chars, err := srvc.DiscoverCharacteristics([]bluetooth.UUID{heartRateCharacteristicUUID})
+		if err != nil {
+			failMessage(err.Error())
+			return
+		}
+
+		if len(chars) == 0 {
+			failMessage("could not find heart rate characteristic")
+			return
+		}
+
+		char := chars[0]
+		addr := device.Address.String()
+		println("found characteristic", char.UUID().String(), "for device", addr)
+
+		char.EnableNotifications(func(buf []byte) {
+			println(addr, "data:", uint8(buf[1]))
+		})
+	}
+
+	// wait for exit
+	<-exitCtx.Done()
+	println("exiting.")
+}
+
+func must(action string, err error) {
+	if err != nil {
+		failMessage("failed to " + action + ": " + err.Error())
+		return
+	}
+}

examples/multiples/mcu.go

@@ -0,0 +1,37 @@
+//go:build baremetal
+
+package main
+
+import (
+	"context"
+	"errors"
+	"strings"
+	"time"
+)
+
+// Devices are the MAC addresses of the Bluetooth peripherals you want to connect to.
+// Replace this by using -ldflags="-X main.Devices='[MAC ADDRESS],[MAC ADDRESS]'"
+// where [MAC ADDRESS] is the actual MAC address of the peripheral.
+// For example:
+// tinygo flash -target nano-rp2040 -ldflags="-X main.Devices='7B:36:98:8C:41:1C,7B:36:98:8C:41:1D" ./examples/heartrate-monitor/
+var Devices string
+
+func initExitHandler() context.Context {
+	return context.Background()
+}
+
+func connectAddresses() ([]string, error) {
+	addrs := strings.Split(Devices, ",")
+	if len(addrs) == 0 {
+		return nil, errors.New("no devices specified")
+	}
+
+	return addrs, nil
+}
+
+func failMessage(msg string) {
+	for {
+		println(msg)
+		time.Sleep(1 * time.Second)
+	}
+}

examples/multiples/os.go

@@ -0,0 +1,49 @@
+//go:build !baremetal
+
+package main
+
+import (
+	"context"
+	"errors"
+	"os"
+	"os/signal"
+	"strings"
+	"syscall"
+)
+
+func initExitHandler() context.Context {
+	return contextWithSignal(context.Background())
+}
+
+// ContextWithSignal creates a context canceled when SIGINT or SIGTERM are notified
+func contextWithSignal(ctx context.Context) context.Context {
+	newCtx, cancel := context.WithCancel(ctx)
+	signals := make(chan os.Signal)
+	signal.Notify(signals, syscall.SIGINT, syscall.SIGTERM)
+	go func() {
+		select {
+		case <-signals:
+			cancel()
+		}
+	}()
+	return newCtx
+}
+
+func connectAddresses() ([]string, error) {
+	if len(os.Args) < 2 {
+		println("usage: multiples [address],[address]")
+		os.Exit(1)
+	}
+
+	addrs := strings.Split(os.Args[1], ",")
+	if len(addrs) == 0 {
+		return nil, errors.New("no devices specified")
+	}
+
+	return addrs, nil
+}
+
+func failMessage(msg string) {
+	println(msg)
+	exitCtx.Done()
+}

gap_ninafw.go

@@ -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.addDevice(d)
 
 			return d, nil
 
@@ -228,7 +228,7 @@ func (d Device) Disconnect() error {
 		return err
 	}
 
-	d.adapter.connectedDevices = []Device{}
+	d.adapter.removeDevice(d)
 	return nil
 }
 

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,7 @@ type hci struct {
 	scanning          bool
 	advData           leAdvertisingReport
 	connectData       leConnectData
+	maxPkt            uint8
 }
 
 func newHCI(uart *machine.UART) *hci {
@@ -185,6 +194,12 @@ func (h *hci) poll() error {
 			return err
 		case done:
 			return nil
+		case i+1 >= len(h.buf):
+			if debug {
+				println("hci error: buffer overflow")
+			}
+			i = 0
+			time.Sleep(5 * time.Millisecond)
 		default:
 			i++
 			time.Sleep(1 * time.Millisecond)
@@ -257,6 +272,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 = 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
 
@@ -351,6 +386,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)
@@ -486,6 +536,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)
@@ -507,6 +564,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)
 
@@ -563,7 +621,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)
 

l2cap_hci.go

@@ -0,0 +1,164 @@
+//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)
+
+	if err := l.sendReq(handle, b[:]); err != nil {
+		return err
+	}
+
+	return nil
+}
+
+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))
+	}
+
+	if err := l.hci.sendAclPkt(handle, signalingCID, data); err != nil {
+		return err
+	}
+
+	return nil
+}