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
 }
 

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 {

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 {
@@ -263,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
 
@@ -357,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)
@@ -492,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)
@@ -513,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)
 
@@ -569,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
+}