diff --git a/adapter_ninafw.go b/adapter_ninafw.go new file mode 100644 index 0000000..59115fb --- /dev/null +++ b/adapter_ninafw.go @@ -0,0 +1,209 @@ +//go:build ninafw + +package bluetooth + +import ( + "machine" + "runtime" + + "time" +) + +const maxConnections = 1 + +// Adapter represents the UART connection to the NINA fw. +type Adapter struct { + hci *hci + att *att + + isDefault bool + scanning bool + + reset func() + connectHandler func(device Address, connected bool) + + connectedDevices []*Device + notificationsStarted bool +} + +// DefaultAdapter is the default adapter on the current system. +// +// Make sure to call Enable() before using it to initialize the adapter. +var DefaultAdapter = &Adapter{ + isDefault: true, + reset: resetNINAInverted, + connectHandler: func(device Address, connected bool) { + return + }, + connectedDevices: make([]*Device, 0, maxConnections), +} + +// Enable configures the BLE stack. It must be called before any +// Bluetooth-related calls (unless otherwise indicated). +func (a *Adapter) Enable() error { + // reset the NINA in BLE mode + machine.NINA_CS.Configure(machine.PinConfig{Mode: machine.PinOutput}) + machine.NINA_RESETN.Configure(machine.PinConfig{Mode: machine.PinOutput}) + machine.NINA_CS.Low() + a.reset() + + // serial port for nina chip + uart := machine.UART1 + uart.Configure(machine.UARTConfig{ + TX: machine.NINA_TX, + RX: machine.NINA_RX, + BaudRate: 115200, + CTS: machine.NINA_CTS, + RTS: machine.NINA_RTS, + }) + + a.hci, a.att = newBLEStack(uart) + + a.hci.start() + + if err := a.hci.reset(); err != nil { + return err + } + + time.Sleep(150 * time.Millisecond) + + if err := a.hci.setEventMask(0x3FFFFFFFFFFFFFFF); err != nil { + return err + } + + if err := a.hci.setLeEventMask(0x00000000000003FF); err != nil { + return err + } + + return nil +} + +func (a *Adapter) Address() (MACAddress, error) { + if err := a.hci.readBdAddr(); err != nil { + return MACAddress{}, err + } + + return MACAddress{MAC: makeAddress(a.hci.address)}, nil +} + +func newBLEStack(uart *machine.UART) (*hci, *att) { + h := newHCI(uart) + a := newATT(h) + h.att = a + + return h, a +} + +// Convert a NINA MAC address into a Go MAC address. +func makeAddress(mac [6]uint8) MAC { + return MAC{ + uint8(mac[0]), + uint8(mac[1]), + uint8(mac[2]), + uint8(mac[3]), + uint8(mac[4]), + uint8(mac[5]), + } +} + +// Convert a Go MAC address into a NINA MAC Address. +func makeNINAAddress(mac MAC) [6]uint8 { + return [6]uint8{ + uint8(mac[0]), + uint8(mac[1]), + uint8(mac[2]), + uint8(mac[3]), + uint8(mac[4]), + uint8(mac[5]), + } +} + +func resetNINA() { + machine.NINA_RESETN.High() + time.Sleep(100 * time.Millisecond) + machine.NINA_RESETN.Low() + time.Sleep(1000 * time.Millisecond) +} + +func resetNINAInverted() { + machine.NINA_RESETN.Low() + time.Sleep(100 * time.Millisecond) + machine.NINA_RESETN.High() + time.Sleep(1000 * time.Millisecond) +} + +func (a *Adapter) startNotifications() { + if a.notificationsStarted { + return + } + + if _debug { + println("starting notifications...") + } + + a.notificationsStarted = true + + // go routine to poll for HCI events for ATT notifications + go func() { + for { + if err := a.att.poll(); err != nil { + // TODO: handle error + if _debug { + println("error polling for notifications:", err.Error()) + } + } + + time.Sleep(250 * time.Millisecond) + } + }() + + // go routine to handle characteristic notifications + go func() { + for { + select { + case not := <-a.att.notifications: + if _debug { + println("notification received", not.connectionHandle, not.handle, not.data) + } + + d := a.findDevice(not.connectionHandle) + if d == nil { + if _debug { + println("no device found for handle", not.connectionHandle) + } + continue + } + + n := d.findNotificationRegistration(not.handle) + if n == nil { + if _debug { + println("no notification registered for handle", not.handle) + } + continue + } + + if n.callback != nil { + n.callback(not.data) + } + + default: + } + + runtime.Gosched() + } + }() +} + +func (a *Adapter) findDevice(handle uint16) *Device { + for _, d := range a.connectedDevices { + if d.handle == handle { + if _debug { + println("found device", handle, d.Address.String(), "with notifications registered", len(d.notificationRegistrations)) + } + + return d + } + } + + return nil +} diff --git a/att_ninafw.go b/att_ninafw.go new file mode 100644 index 0000000..8a31bd3 --- /dev/null +++ b/att_ninafw.go @@ -0,0 +1,561 @@ +//go:build ninafw + +package bluetooth + +import ( + "encoding/binary" + "encoding/hex" + "errors" + "slices" + "sync" + "time" +) + +const ( + attCID = 0x0004 + bleCTL = 0x0008 + + attOpError = 0x01 + attOpMTUReq = 0x02 + attOpMTUResponse = 0x03 + attOpFindInfoReq = 0x04 + attOpFindInfoResponse = 0x05 + attOpFindByTypeReq = 0x06 + attOpFindByTypeResponse = 0x07 + attOpReadByTypeReq = 0x08 + attOpReadByTypeResponse = 0x09 + attOpReadReq = 0x0a + attOpReadResponse = 0x0b + attOpReadBlobReq = 0x0c + attOpReadBlobResponse = 0x0d + attOpReadMultiReq = 0x0e + attOpReadMultiResponse = 0x0f + attOpReadByGroupReq = 0x10 + attOpReadByGroupResponse = 0x11 + attOpWriteReq = 0x12 + attOpWriteResponse = 0x13 + attOpWriteCmd = 0x52 + attOpPrepWriteReq = 0x16 + attOpPrepWriteResponse = 0x17 + attOpExecWriteReq = 0x18 + attOpExecWriteResponse = 0x19 + attOpHandleNotify = 0x1b + attOpHandleInd = 0x1d + attOpHandleCNF = 0x1e + attOpSignedWriteCmd = 0xd2 + + attErrorInvalidHandle = 0x01 + attErrorReadNotPermitted = 0x02 + attErrorWriteNotPermitted = 0x03 + attErrorInvalidPDU = 0x04 + attErrorAuthentication = 0x05 + attErrorRequestNotSupported = 0x06 + attErrorInvalidOffset = 0x07 + attErrorAuthorization = 0x08 + attErrorPreQueueFull = 0x09 + attErrorAttrNotFound = 0x0a + attErrorAttrNotLong = 0x0b + attErrorInsuffEncrKeySize = 0x0c + attErrorInvalidAttrValueLength = 0x0d + attErrorUnlikely = 0x0e + attErrorInsuffEnc = 0x0f + attErrorUnsupportedGroupType = 0x10 + attErrorInsufficientResources = 0x11 + + gattUnknownUUID = 0x0000 + gattServiceUUID = 0x2800 + gattCharacteristicUUID = 0x2803 + gattDescriptorUUID = 0x2900 +) + +var ( + ErrATTTimeout = errors.New("bluetooth: ATT timeout") + ErrATTUnknownEvent = errors.New("bluetooth: ATT unknown event") + ErrATTUnknown = errors.New("bluetooth: ATT unknown error") + ErrATTOp = errors.New("bluetooth: ATT OP error") +) + +type rawService struct { + startHandle uint16 + endHandle uint16 + uuid UUID +} + +type rawCharacteristic struct { + startHandle uint16 + properties uint8 + valueHandle uint16 + uuid UUID +} + +type rawDescriptor struct { + handle uint16 + uuid UUID +} + +type rawNotification struct { + connectionHandle uint16 + handle uint16 + data []byte +} + +type att struct { + hci *hci + busy sync.Mutex + responded bool + errored bool + lastErrorOpcode uint8 + lastErrorHandle uint16 + lastErrorCode uint8 + services []rawService + characteristics []rawCharacteristic + descriptors []rawDescriptor + value []byte + notifications chan rawNotification +} + +func newATT(hci *hci) *att { + return &att{ + hci: hci, + services: []rawService{}, + characteristics: []rawCharacteristic{}, + value: []byte{}, + notifications: make(chan rawNotification, 32), + } +} + +func (a *att) readByGroupReq(connectionHandle, startHandle, endHandle uint16, uuid shortUUID) error { + if _debug { + println("att.readByGroupReq:", connectionHandle, startHandle, endHandle, uuid) + } + + a.busy.Lock() + defer a.busy.Unlock() + + var b [7]byte + b[0] = attOpReadByGroupReq + binary.LittleEndian.PutUint16(b[1:], startHandle) + binary.LittleEndian.PutUint16(b[3:], endHandle) + binary.LittleEndian.PutUint16(b[5:], uint16(uuid)) + + if err := a.sendReq(connectionHandle, b[:]); err != nil { + return err + } + + return a.waitUntilResponse() +} + +func (a *att) readByTypeReq(connectionHandle, startHandle, endHandle uint16, typ uint16) error { + if _debug { + println("att.readByTypeReq:", connectionHandle, startHandle, endHandle, typ) + } + + a.busy.Lock() + defer a.busy.Unlock() + + var b [7]byte + b[0] = attOpReadByTypeReq + binary.LittleEndian.PutUint16(b[1:], startHandle) + binary.LittleEndian.PutUint16(b[3:], endHandle) + binary.LittleEndian.PutUint16(b[5:], typ) + + if err := a.sendReq(connectionHandle, b[:]); err != nil { + return err + } + + return a.waitUntilResponse() +} + +func (a *att) findInfoReq(connectionHandle, startHandle, endHandle uint16) error { + if _debug { + println("att.findInfoReq:", connectionHandle, startHandle, endHandle) + } + + a.busy.Lock() + defer a.busy.Unlock() + + var b [5]byte + b[0] = attOpFindInfoReq + binary.LittleEndian.PutUint16(b[1:], startHandle) + binary.LittleEndian.PutUint16(b[3:], endHandle) + + if err := a.sendReq(connectionHandle, b[:]); err != nil { + return err + } + + return a.waitUntilResponse() +} + +func (a *att) readReq(connectionHandle, valueHandle uint16) error { + if _debug { + println("att.readReq:", connectionHandle, valueHandle) + } + + a.busy.Lock() + defer a.busy.Unlock() + + var b [3]byte + b[0] = attOpReadReq + binary.LittleEndian.PutUint16(b[1:], valueHandle) + + if err := a.sendReq(connectionHandle, b[:]); err != nil { + return err + } + + return a.waitUntilResponse() +} + +func (a *att) writeCmd(connectionHandle, valueHandle uint16, data []byte) error { + if _debug { + println("att.writeCmd:", connectionHandle, valueHandle, hex.EncodeToString(data)) + } + + a.busy.Lock() + defer a.busy.Unlock() + + var b [3]byte + b[0] = attOpWriteCmd + binary.LittleEndian.PutUint16(b[1:], valueHandle) + + if err := a.sendReq(connectionHandle, append(b[:], data...)); err != nil { + return err + } + + return a.waitUntilResponse() +} + +func (a *att) writeReq(connectionHandle, valueHandle uint16, data []byte) error { + if _debug { + println("att.writeReq:", connectionHandle, valueHandle, hex.EncodeToString(data)) + } + + a.busy.Lock() + defer a.busy.Unlock() + + var b [3]byte + b[0] = attOpWriteReq + binary.LittleEndian.PutUint16(b[1:], valueHandle) + + if err := a.sendReq(connectionHandle, append(b[:], data...)); err != nil { + return err + } + + return a.waitUntilResponse() +} + +func (a *att) sendReq(handle uint16, data []byte) error { + a.clearResponse() + + if _debug { + println("att.sendReq:", handle, "data:", hex.EncodeToString(data)) + } + + if err := a.hci.sendAclPkt(handle, attCID, data); err != nil { + return err + } + + return nil +} + +func (a *att) handleData(handle uint16, buf []byte) error { + if _debug { + println("att.handleData:", handle, "data:", hex.EncodeToString(buf)) + } + + switch buf[0] { + case attOpError: + a.errored = true + a.lastErrorOpcode = buf[1] + a.lastErrorHandle = binary.LittleEndian.Uint16(buf[2:]) + a.lastErrorCode = buf[4] + + if _debug { + println("att.handleData: attOpERROR", a.lastErrorOpcode, a.lastErrorCode) + } + + return ErrATTOp + + case attOpMTUReq: + if _debug { + println("att.handleData: attOpMTUReq") + } + + case attOpMTUResponse: + if _debug { + println("att.handleData: attOpMTUResponse") + } + + case attOpFindInfoReq: + if _debug { + println("att.handleData: attOpFindInfoReq") + } + + case attOpFindInfoResponse: + if _debug { + println("att.handleData: attOpFindInfoResponse") + } + a.responded = true + + lengthPerDescriptor := int(buf[1]) + var uuid [16]byte + + for i := 2; i < len(buf); i += lengthPerDescriptor { + d := rawDescriptor{ + handle: binary.LittleEndian.Uint16(buf[i:]), + } + switch lengthPerDescriptor - 2 { + case 2: + d.uuid = New16BitUUID(binary.LittleEndian.Uint16(buf[i+2:])) + case 16: + copy(uuid[:], buf[i+2:]) + slices.Reverse(uuid[:]) + d.uuid = NewUUID(uuid) + } + + if _debug { + println("att.handleData: descriptor", d.handle, d.uuid.String()) + } + + a.descriptors = append(a.descriptors, d) + } + + case attOpFindByTypeReq: + if _debug { + println("att.handleData: attOpFindByTypeReq") + } + + case attOpReadByTypeReq: + if _debug { + println("att.handleData: attOpReadByTypeReq") + } + + case attOpReadByTypeResponse: + if _debug { + println("att.handleData: attOpReadByTypeResponse") + } + a.responded = true + + lengthPerCharacteristic := int(buf[1]) + var uuid [16]byte + + for i := 2; i < len(buf); i += lengthPerCharacteristic { + c := rawCharacteristic{ + startHandle: binary.LittleEndian.Uint16(buf[i:]), + properties: buf[i+2], + valueHandle: binary.LittleEndian.Uint16(buf[i+3:]), + } + switch lengthPerCharacteristic - 5 { + case 2: + c.uuid = New16BitUUID(binary.LittleEndian.Uint16(buf[i+5:])) + case 16: + copy(uuid[:], buf[i+5:]) + slices.Reverse(uuid[:]) + c.uuid = NewUUID(uuid) + } + + if _debug { + println("att.handleData: characteristic", c.startHandle, c.properties, c.valueHandle, c.uuid.String()) + } + + a.characteristics = append(a.characteristics, c) + } + + return nil + + case attOpReadByGroupReq: + if _debug { + println("att.handleData: attOpReadByGroupReq") + } + + // return generic services + var response [14]byte + response[0] = attOpReadByGroupResponse + response[1] = 0x06 // length per service + + genericAccessService := rawService{ + startHandle: 0, + endHandle: 1, + uuid: ServiceUUIDGenericAccess, + } + binary.LittleEndian.PutUint16(response[2:], genericAccessService.startHandle) + binary.LittleEndian.PutUint16(response[4:], genericAccessService.endHandle) + binary.LittleEndian.PutUint16(response[6:], genericAccessService.uuid.Get16Bit()) + + genericAttributeService := rawService{ + startHandle: 2, + endHandle: 5, + uuid: ServiceUUIDGenericAttribute, + } + binary.LittleEndian.PutUint16(response[8:], genericAttributeService.startHandle) + binary.LittleEndian.PutUint16(response[10:], genericAttributeService.endHandle) + binary.LittleEndian.PutUint16(response[12:], genericAttributeService.uuid.Get16Bit()) + + if err := a.hci.sendAclPkt(handle, attCID, response[:]); err != nil { + return err + } + + case attOpReadByGroupResponse: + if _debug { + println("att.handleData: attOpReadByGroupResponse") + } + a.responded = true + + lengthPerService := int(buf[1]) + var uuid [16]byte + + for i := 2; i < len(buf); i += lengthPerService { + service := rawService{ + startHandle: binary.LittleEndian.Uint16(buf[i:]), + endHandle: binary.LittleEndian.Uint16(buf[i+2:]), + } + switch lengthPerService - 4 { + case 2: + service.uuid = New16BitUUID(binary.LittleEndian.Uint16(buf[i+4:])) + case 16: + copy(uuid[:], buf[i+4:]) + slices.Reverse(uuid[:]) + service.uuid = NewUUID(uuid) + } + + if _debug { + println("att.handleData: service", service.startHandle, service.endHandle, service.uuid.String()) + } + + a.services = append(a.services, service) + } + + return nil + + case attOpReadReq: + if _debug { + println("att.handleData: attOpReadReq") + } + + case attOpReadBlobReq: + if _debug { + println("att.handleData: attOpReadBlobReq") + } + + case attOpReadResponse: + if _debug { + println("att.handleData: attOpReadResponse") + } + a.responded = true + a.value = append(a.value, buf[1:]...) + + case attOpWriteReq: + if _debug { + println("att.handleData: attOpWriteReq") + } + + case attOpWriteCmd: + if _debug { + println("att.handleData: attOpWriteCmd") + } + + case attOpWriteResponse: + if _debug { + println("att.handleData: attOpWriteResponse") + } + a.responded = true + + case attOpPrepWriteReq: + if _debug { + println("att.handleData: attOpPrepWriteReq") + } + + case attOpExecWriteReq: + if _debug { + println("att.handleData: attOpExecWriteReq") + } + + case attOpHandleNotify: + if _debug { + println("att.handleData: attOpHandleNotify") + } + + not := rawNotification{ + connectionHandle: handle, + handle: binary.LittleEndian.Uint16(buf[1:]), + data: []byte{}, + } + not.data = append(not.data, buf[3:]...) + + select { + case a.notifications <- not: + default: + // out of space, drop notification :( + } + + case attOpHandleInd: + if _debug { + println("att.handleData: attOpHandleInd") + } + + case attOpHandleCNF: + if _debug { + println("att.handleData: attOpHandleCNF") + } + + case attOpReadMultiReq: + if _debug { + println("att.handleData: attOpReadMultiReq") + } + + case attOpSignedWriteCmd: + if _debug { + println("att.handleData: attOpSignedWriteCmd") + } + + default: + if _debug { + println("att.handleData: unknown") + } + } + + return nil +} + +func (a *att) clearResponse() { + a.responded = false + a.errored = false + a.lastErrorOpcode = 0 + a.lastErrorHandle = 0 + a.lastErrorCode = 0 + a.value = []byte{} +} + +func (a *att) waitUntilResponse() error { + start := time.Now().UnixNano() + for { + if err := a.hci.poll(); err != nil { + return err + } + + switch { + case a.responded: + return nil + + default: + // check for timeout + if (time.Now().UnixNano()-start)/int64(time.Second) > 3 { + break + } + + time.Sleep(100 * time.Millisecond) + } + } + + return ErrATTTimeout +} + +func (a *att) poll() error { + a.busy.Lock() + defer a.busy.Unlock() + + if err := a.hci.poll(); err != nil { + return err + } + + return nil +} diff --git a/debug.go b/debug.go new file mode 100644 index 0000000..55e8e4d --- /dev/null +++ b/debug.go @@ -0,0 +1,5 @@ +//go:build bledebug + +package bluetooth + +var _debug = true diff --git a/gap_ninafw.go b/gap_ninafw.go new file mode 100644 index 0000000..9ebae26 --- /dev/null +++ b/gap_ninafw.go @@ -0,0 +1,243 @@ +//go:build ninafw + +package bluetooth + +import ( + "errors" + "time" +) + +var ( + ErrConnect = errors.New("bluetooth: could not connect") +) + +// Scan starts a BLE scan. +func (a *Adapter) Scan(callback func(*Adapter, ScanResult)) error { + if a.scanning { + return errScanning + } + + if err := a.hci.leSetScanEnable(false, true); err != nil { + return err + } + + // passive scanning, every 40ms, for 30ms + if err := a.hci.leSetScanParameters(0x00, 0x0080, 0x0030, 0x00, 0x00); err != nil { + return err + } + + a.scanning = true + + // scan with duplicates + if err := a.hci.leSetScanEnable(true, false); err != nil { + return err + } + + lastUpdate := time.Now().UnixNano() + + for { + if err := a.hci.poll(); err != nil { + return err + } + + switch { + case a.hci.advData.reported: + adf := AdvertisementFields{} + if a.hci.advData.eirLength > 31 { + if _debug { + println("eirLength too long") + } + + a.hci.clearAdvData() + continue + } + + for i := 0; i < int(a.hci.advData.eirLength); { + l, t := int(a.hci.advData.eirData[i]), a.hci.advData.eirData[i+1] + if l < 1 { + break + } + + switch t { + case 0x02, 0x03: + // 16-bit Service Class UUID + case 0x06, 0x07: + // 128-bit Service Class UUID + case 0x08, 0x09: + if _debug { + println("local name", string(a.hci.advData.eirData[i+2:i+1+l])) + } + + adf.LocalName = string(a.hci.advData.eirData[i+2 : i+1+l]) + case 0xFF: + // Manufacturer Specific Data + } + + i += l + 1 + } + + random := a.hci.advData.peerBdaddrType == 0x01 + + callback(a, ScanResult{ + Address: Address{ + MACAddress{ + MAC: makeAddress(a.hci.advData.peerBdaddr), + isRandom: random, + }, + }, + RSSI: int16(a.hci.advData.rssi), + AdvertisementPayload: &advertisementFields{ + AdvertisementFields: adf, + }, + }) + + a.hci.clearAdvData() + time.Sleep(10 * time.Millisecond) + + default: + if !a.scanning { + return nil + } + + if _debug && (time.Now().UnixNano()-lastUpdate)/int64(time.Second) > 1 { + println("still scanning...") + lastUpdate = time.Now().UnixNano() + } + + time.Sleep(10 * time.Millisecond) + } + } + + return nil +} + +func (a *Adapter) StopScan() error { + if !a.scanning { + return errNotScanning + } + + if err := a.hci.leSetScanEnable(false, false); err != nil { + return err + } + + a.scanning = false + + return nil +} + +// Address contains a Bluetooth MAC address. +type Address struct { + MACAddress +} + +// Connect starts a connection attempt to the given peripheral device address. +func (a *Adapter) Connect(address Address, params ConnectionParams) (*Device, error) { + if _debug { + println("Connect") + } + + random := uint8(0) + if address.isRandom { + random = 1 + } + if err := a.hci.leCreateConn(0x0060, 0x0030, 0x00, + random, makeNINAAddress(address.MAC), + 0x00, 0x0006, 0x000c, 0x0000, 0x00c8, 0x0004, 0x0006); err != nil { + return nil, err + } + + // are we connected? + start := time.Now().UnixNano() + for { + if err := a.hci.poll(); err != nil { + return nil, err + } + + switch { + case a.hci.connectData.connected: + defer a.hci.clearConnectData() + + random := false + if address.isRandom { + random = true + } + + d := &Device{adapter: a, + handle: a.hci.connectData.handle, + Address: Address{ + MACAddress{ + MAC: makeAddress(a.hci.connectData.peerBdaddr), + isRandom: random}, + }, + notificationRegistrations: make([]notificationRegistration, 0), + } + a.connectedDevices = append(a.connectedDevices, d) + + return d, nil + + default: + // check for timeout + if (time.Now().UnixNano()-start)/int64(time.Second) > 5 { + break + } + + time.Sleep(10 * time.Millisecond) + } + } + + // cancel connection attempt that failed + if err := a.hci.leCancelConn(); err != nil { + return nil, err + } + + return nil, ErrConnect +} + +type notificationRegistration struct { + handle uint16 + callback func([]byte) +} + +// Device is a connection to a remote peripheral. +type Device struct { + adapter *Adapter + Address Address + handle uint16 + + notificationRegistrations []notificationRegistration +} + +// Disconnect from the BLE device. +func (d *Device) Disconnect() error { + if _debug { + println("Disconnect") + } + if err := d.adapter.hci.disconnect(d.handle); err != nil { + return err + } + + d.adapter.connectedDevices = []*Device{} + return nil +} + +func (d *Device) findNotificationRegistration(handle uint16) *notificationRegistration { + for _, n := range d.notificationRegistrations { + if n.handle == handle { + return &n + } + } + + return nil +} + +func (d *Device) addNotificationRegistration(handle uint16, callback func([]byte)) { + d.notificationRegistrations = append(d.notificationRegistrations, + notificationRegistration{ + handle: handle, + callback: callback, + }) +} + +func (d *Device) startNotifications() { + d.adapter.startNotifications() +} diff --git a/gattc_ninafw.go b/gattc_ninafw.go new file mode 100644 index 0000000..3e6b6fd --- /dev/null +++ b/gattc_ninafw.go @@ -0,0 +1,298 @@ +//go:build ninafw + +package bluetooth + +import "errors" + +var ( + errNotYetImplemented = errors.New("bluetooth: not yet implemented") + errNoWrite = errors.New("bluetooth: write not permitted") + errNoWriteWithoutResponse = errors.New("bluetooth: write without response not permitted") + errWriteFailed = errors.New("bluetooth: write failed") + errNoRead = errors.New("bluetooth: read not permitted") + errReadFailed = errors.New("bluetooth: read failed") + errNoNotify = errors.New("bluetooth: notify/indicate not permitted") + errEnableNotificationsFailed = errors.New("bluetooth: enable notifications failed") + errServiceNotFound = errors.New("bluetooth: service not found") + errCharacteristicNotFound = errors.New("bluetooth: characteristic not found") +) + +const ( + maxDefaultServicesToDiscover = 6 + maxDefaultCharacteristicsToDiscover = 8 +) + +const ( + charPropertyBroadcast = 0x01 + charPropertyRead = 0x02 + charPropertyWriteWithoutResponse = 0x04 + charPropertyWrite = 0x08 + charPropertyNotify = 0x10 + charPropertyIndicate = 0x20 +) + +// DeviceService is a BLE service on a connected peripheral device. +type DeviceService struct { + uuid UUID + + device *Device + startHandle, endHandle uint16 +} + +// UUID returns the UUID for this DeviceService. +func (s *DeviceService) UUID() UUID { + return s.uuid +} + +// DiscoverServices starts a service discovery procedure. Pass a list of service +// UUIDs you are interested in to this function. Either a slice of all services +// is returned (of the same length as the requested UUIDs and in the same +// order), or if some services could not be discovered an error is returned. +// +// Passing a nil slice of UUIDs will return a complete list of +// services. +func (d *Device) DiscoverServices(uuids []UUID) ([]DeviceService, error) { + if _debug { + println("DiscoverServices") + } + + services := make([]DeviceService, 0, maxDefaultServicesToDiscover) + foundServices := make(map[UUID]DeviceService) + + startHandle := uint16(0x0001) + endHandle := uint16(0xffff) + for endHandle == uint16(0xffff) { + err := d.adapter.att.readByGroupReq(d.handle, startHandle, endHandle, gattServiceUUID) + if err != nil { + return nil, err + } + + if _debug { + println("found d.adapter.att.services", len(d.adapter.att.services)) + } + + if len(d.adapter.att.services) == 0 { + break + } + + for _, rawService := range d.adapter.att.services { + if len(uuids) == 0 || rawService.uuid.isIn(uuids) { + foundServices[rawService.uuid] = + DeviceService{ + device: d, + uuid: rawService.uuid, + startHandle: rawService.startHandle, + endHandle: rawService.endHandle, + } + } + + startHandle = rawService.endHandle + 1 + if startHandle == 0x0000 { + endHandle = 0x0000 + } + } + + // reset raw services + d.adapter.att.services = []rawService{} + } + + switch { + case len(uuids) > 0: + // put into correct order + for _, uuid := range uuids { + s, ok := foundServices[uuid] + if !ok { + return nil, errServiceNotFound + } + + services = append(services, s) + } + default: + for _, s := range foundServices { + services = append(services, s) + } + } + + return services, nil +} + +// DeviceCharacteristic is a BLE characteristic on a connected peripheral +// device. +type DeviceCharacteristic struct { + uuid UUID + + service *DeviceService + permissions CharacteristicPermissions + handle uint16 + properties uint8 + callback func(buf []byte) +} + +// UUID returns the UUID for this DeviceCharacteristic. +func (c *DeviceCharacteristic) UUID() UUID { + return c.uuid +} + +// DiscoverCharacteristics discovers characteristics in this service. Pass a +// list of characteristic UUIDs you are interested in to this function. Either a +// list of all requested services is returned, or if some services could not be +// discovered an error is returned. If there is no error, the characteristics +// slice has the same length as the UUID slice with characteristics in the same +// order in the slice as in the requested UUID list. +// +// Passing a nil slice of UUIDs will return a complete +// list of characteristics. +func (s *DeviceService) DiscoverCharacteristics(uuids []UUID) ([]DeviceCharacteristic, error) { + if _debug { + println("DiscoverCharacteristics") + } + + characteristics := make([]DeviceCharacteristic, 0, maxDefaultCharacteristicsToDiscover) + foundCharacteristics := make(map[UUID]DeviceCharacteristic) + + startHandle := s.startHandle + endHandle := s.endHandle + for startHandle < endHandle { + err := s.device.adapter.att.readByTypeReq(s.device.handle, startHandle, endHandle, gattCharacteristicUUID) + switch { + case err == ErrATTOp && + s.device.adapter.att.lastErrorOpcode == attOpReadByTypeReq && + s.device.adapter.att.lastErrorCode == attErrorAttrNotFound: + + // no characteristics found + break + case err != nil: + return nil, err + } + + if _debug { + println("found s.device.adapter.att.characteristics", len(s.device.adapter.att.characteristics)) + } + + if len(s.device.adapter.att.characteristics) == 0 { + break + } + + for _, rawCharacteristic := range s.device.adapter.att.characteristics { + if len(uuids) == 0 || rawCharacteristic.uuid.isIn(uuids) { + dc := DeviceCharacteristic{ + service: s, + uuid: rawCharacteristic.uuid, + handle: rawCharacteristic.valueHandle, + properties: rawCharacteristic.properties, + permissions: CharacteristicPermissions(rawCharacteristic.properties), + } + + foundCharacteristics[rawCharacteristic.uuid] = dc + } + + startHandle = rawCharacteristic.valueHandle + 1 + } + + // reset raw characteristics + s.device.adapter.att.characteristics = []rawCharacteristic{} + } + + switch { + case len(uuids) > 0: + // put into correct order + for _, uuid := range uuids { + c, ok := foundCharacteristics[uuid] + if !ok { + return nil, errCharacteristicNotFound + } + characteristics = append(characteristics, c) + } + default: + for _, c := range foundCharacteristics { + characteristics = append(characteristics, c) + } + + } + + return characteristics, nil +} + +// WriteWithoutResponse replaces the characteristic value with a new value. The +// call will return before all data has been written. A limited number of such +// writes can be in flight at any given time. This call is also known as a +// "write command" (as opposed to a write request). +func (c DeviceCharacteristic) WriteWithoutResponse(p []byte) (n int, err error) { + if !c.permissions.WriteWithoutResponse() { + return 0, errNoWriteWithoutResponse + } + + err = c.service.device.adapter.att.writeCmd(c.service.device.handle, c.handle, p) + if err != nil { + return 0, err + } + + return len(p), nil +} + +// EnableNotifications enables notifications in the Client Characteristic +// Configuration Descriptor (CCCD). This means that most peripherals will send a +// notification with a new value every time the value of the characteristic +// changes. +// +// Users may call EnableNotifications with a nil callback to disable notifications. +func (c *DeviceCharacteristic) EnableNotifications(callback func(buf []byte)) error { + if !c.permissions.Notify() { + return errNoNotify + } + + switch { + case callback == nil: + // disable notifications + if _debug { + println("disabling notifications") + } + + err := c.service.device.adapter.att.writeReq(c.service.device.handle, c.handle+1, []byte{0x00, 0x00}) + if err != nil { + return err + } + default: + // enable notifications + if _debug { + println("enabling notifications") + } + + err := c.service.device.adapter.att.writeReq(c.service.device.handle, c.handle+1, []byte{0x01, 0x00}) + if err != nil { + return err + } + } + + c.callback = callback + + c.service.device.startNotifications() + c.service.device.addNotificationRegistration(c.handle, c.callback) + + return nil +} + +// GetMTU returns the MTU for the characteristic. +func (c DeviceCharacteristic) GetMTU() (uint16, error) { + return 0, errNotYetImplemented +} + +// Read reads the current characteristic value. +func (c *DeviceCharacteristic) Read(data []byte) (int, error) { + if !c.permissions.Read() { + return 0, errNoRead + } + + err := c.service.device.adapter.att.readReq(c.service.device.handle, c.handle) + if err != nil { + return 0, err + } + + if len(c.service.device.adapter.att.value) == 0 { + return 0, errReadFailed + } + + copy(data, c.service.device.adapter.att.value) + + return len(c.service.device.adapter.att.value), nil +} diff --git a/gatts.go b/gatts.go index aa9b64d..f092f51 100644 --- a/gatts.go +++ b/gatts.go @@ -56,3 +56,8 @@ func (p CharacteristicPermissions) Write() bool { func (p CharacteristicPermissions) WriteWithoutResponse() bool { return p&CharacteristicWriteWithoutResponsePermission != 0 } + +// Notify returns whether notifications are permitted. +func (p CharacteristicPermissions) Notify() bool { + return p&CharacteristicNotifyPermission != 0 +} diff --git a/gatts_ninafw.go b/gatts_ninafw.go new file mode 100644 index 0000000..904146b --- /dev/null +++ b/gatts_ninafw.go @@ -0,0 +1,6 @@ +//go:build ninafw + +package bluetooth + +type Characteristic struct { +} diff --git a/hci_ninafw.go b/hci_ninafw.go new file mode 100644 index 0000000..4b1db95 --- /dev/null +++ b/hci_ninafw.go @@ -0,0 +1,586 @@ +//go:build ninafw + +package bluetooth + +import ( + "encoding/binary" + "encoding/hex" + "errors" + "machine" + "time" +) + +const ( + ogfCommandPos = 10 + + ogfLinkCtl = 0x01 + ogfHostCtl = 0x03 + ogfInfoParam = 0x04 + ogfStatusParam = 0x05 + ogfLECtrl = 0x08 + + // ogfLinkCtl + ocfDisconnect = 0x0006 + + // ogfHostCtl + ocfSetEventMask = 0x0001 + ocfReset = 0x0003 + + // ogfInfoParam + ocfReadLocalVersion = 0x0001 + ocfReadBDAddr = 0x0009 + + // ogfStatusParam + ocfReadRSSI = 0x0005 + + // ogfLECtrl + ocfLEReadBufferSize = 0x0002 + ocfLESetRandomAddress = 0x0005 + ocfLESetAdvertisingParameters = 0x0006 + ocfLESetAdvertisingData = 0x0008 + ocfLESetScanResponseData = 0x0009 + ocfLESetAdvertiseEnable = 0x000a + ocfLESetScanParameters = 0x000b + ocfLESetScanEnable = 0x000c + ocfLECreateConn = 0x000d + ocfLECancelConn = 0x000e + ocfLEConnUpdate = 0x0013 + ocfLEParamRequestReply = 0x0020 + + leCommandEncrypt = 0x0017 + leCommandRandom = 0x0018 + leCommandLongTermKeyReply = 0x001A + leCommandLongTermKeyNegativeReply = 0x001B + leCommandReadLocalP256 = 0x0025 + leCommandGenerateDHKeyV1 = 0x0026 + leCommandGenerateDHKeyV2 = 0x005E + + leMetaEventConnComplete = 0x01 + leMetaEventAdvertisingReport = 0x02 + leMetaEventConnectionUpdateComplete = 0x03 + leMetaEventReadRemoteUsedFeaturesComplete = 0x04 + leMetaEventLongTermKeyRequest = 0x05 + leMetaEventRemoteConnParamReq = 0x06 + leMetaEventDataLengthChange = 0x07 + leMetaEventReadLocalP256Complete = 0x08 + leMetaEventGenerateDHKeyComplete = 0x09 + leMetaEventEnhancedConnectionComplete = 0x0A + leMetaEventDirectAdvertisingReport = 0x0B + + hciCommandPkt = 0x01 + hciACLDataPkt = 0x02 + hciEventPkt = 0x04 + hciSecurityPkt = 0x06 + + evtDisconnComplete = 0x05 + evtEncryptionChange = 0x08 + evtCmdComplete = 0x0e + evtCmdStatus = 0x0f + evtHardwareError = 0x10 + evtNumCompPkts = 0x13 + evtReturnLinkKeys = 0x15 + evtLEMetaEvent = 0x3e + + hciOEUserEndedConnection = 0x13 +) + +const ( + hciACLLenPos = 4 + hciEvtLenPos = 2 +) + +var ( + ErrHCITimeout = errors.New("bluetooth: HCI timeout") + ErrHCIUnknownEvent = errors.New("bluetooth: HCI unknown event") + ErrHCIUnknown = errors.New("bluetooth: HCI unknown error") + ErrHCIInvalidPacket = errors.New("bluetooth: HCI invalid packet") + ErrHCIHardware = errors.New("bluetooth: HCI hardware error") +) + +type leAdvertisingReport struct { + reported bool + numReports, typ, peerBdaddrType uint8 + peerBdaddr [6]uint8 + eirLength uint8 + eirData [31]uint8 + rssi int8 +} + +type leConnectData struct { + connected bool + status uint8 + handle uint16 + role uint8 + peerBdaddrType uint8 + peerBdaddr [6]uint8 +} + +type hci struct { + uart *machine.UART + att *att + buf []byte + address [6]byte + cmdCompleteOpcode uint16 + cmdCompleteStatus uint8 + cmdResponse []byte + scanning bool + advData leAdvertisingReport + connectData leConnectData +} + +func newHCI(uart *machine.UART) *hci { + return &hci{uart: uart, + buf: make([]byte, 256), + } +} + +func (h *hci) start() error { + for h.uart.Buffered() > 0 { + h.uart.ReadByte() + } + + return nil +} + +func (h *hci) stop() error { + return nil +} + +func (h *hci) reset() error { + return h.sendCommand(ogfHostCtl<<10 | ocfReset) +} + +func (h *hci) poll() error { + i := 0 + for h.uart.Buffered() > 0 { + data, _ := h.uart.ReadByte() + h.buf[i] = data + + done, err := h.processPacket(i) + switch { + case err == ErrHCIUnknown || err == ErrHCIInvalidPacket || err == ErrHCIUnknownEvent: + if _debug { + println("hci error:", err.Error()) + } + i = 0 + time.Sleep(5 * time.Millisecond) + case err != nil: + return err + case done: + return nil + default: + i++ + time.Sleep(1 * time.Millisecond) + } + } + + return nil +} + +func (h *hci) processPacket(i int) (bool, error) { + switch h.buf[0] { + case hciACLDataPkt: + if i > hciACLLenPos { + pktlen := int(binary.LittleEndian.Uint16(h.buf[3:5])) + switch { + case pktlen > len(h.buf): + return true, ErrHCIInvalidPacket + case i >= (hciACLLenPos + pktlen): + if _debug { + println("hci acl data:", i, hex.EncodeToString(h.buf[:1+hciACLLenPos+pktlen])) + } + return true, h.handleACLData(h.buf[1 : 1+hciACLLenPos+pktlen]) + } + } + + case hciEventPkt: + if i > hciEvtLenPos { + pktlen := int(h.buf[hciEvtLenPos]) + + switch { + case pktlen > len(h.buf): + return true, ErrHCIInvalidPacket + case i >= (hciEvtLenPos + pktlen): + if _debug { + println("hci event data:", i, hex.EncodeToString(h.buf[:1+hciEvtLenPos+pktlen])) + } + return true, h.handleEventData(h.buf[1 : 1+hciEvtLenPos+pktlen]) + } + } + + default: + if _debug { + println("unknown packet data:", h.buf[0]) + } + return true, ErrHCIUnknown + } + + return false, nil +} + +func (h *hci) readBdAddr() error { + if err := h.sendCommand(ogfInfoParam< 3 { + return ErrHCITimeout + } + } + + return nil +} + +func (h *hci) sendAclPkt(handle uint16, cid uint8, data []byte) error { + h.buf[0] = hciACLDataPkt + binary.LittleEndian.PutUint16(h.buf[1:], handle) + binary.LittleEndian.PutUint16(h.buf[3:], uint16(len(data)+4)) + binary.LittleEndian.PutUint16(h.buf[5:], uint16(len(data))) + binary.LittleEndian.PutUint16(h.buf[7:], uint16(cid)) + + copy(h.buf[9:], data) + + if _debug { + println("hci send acl data", handle, cid, hex.EncodeToString(h.buf[:9+len(data)])) + } + + if _, err := h.uart.Write(h.buf[:9+len(data)]); err != nil { + return err + } + + return nil +} + +type aclDataHeader struct { + handle uint16 + dlen uint16 + len uint16 + cid uint16 +} + +func (h *hci) handleACLData(buf []byte) error { + aclHdr := aclDataHeader{ + handle: binary.LittleEndian.Uint16(buf[0:]), + dlen: binary.LittleEndian.Uint16(buf[2:]), + len: binary.LittleEndian.Uint16(buf[4:]), + cid: binary.LittleEndian.Uint16(buf[6:]), + } + + aclFlags := (aclHdr.handle & 0xf000) >> 12 + if aclHdr.dlen-4 != aclHdr.len { + return errors.New("fragmented packet") + } + + switch aclHdr.cid { + case attCID: + if aclFlags == 0x01 { + // TODO: use buffered packet + if _debug { + println("WARNING: att.handleACLData needs buffered packet") + } + return h.att.handleData(aclHdr.handle&0x0fff, buf[8:aclHdr.len+8]) + } else { + return h.att.handleData(aclHdr.handle&0x0fff, buf[8:aclHdr.len+8]) + } + default: + if _debug { + println("unknown acl data cid", aclHdr.cid) + } + } + + return nil +} + +func (h *hci) handleEventData(buf []byte) error { + evt := buf[0] + plen := buf[1] + + switch evt { + case evtDisconnComplete: + if _debug { + println("evtDisconnComplete") + } + // TODO: something with this data? + // status := buf[2] + // handle := buf[3] | (buf[4] << 8) + // reason := buf[5] + // ATT.removeConnection(disconnComplete->handle, disconnComplete->reason); + // L2CAPSignaling.removeConnection(disconnComplete->handle, disconnComplete->reason); + + return h.leSetAdvertiseEnable(true) + + case evtEncryptionChange: + if _debug { + println("evtEncryptionChange") + } + + case evtCmdComplete: + h.cmdCompleteOpcode = binary.LittleEndian.Uint16(buf[3:]) + h.cmdCompleteStatus = buf[5] + if plen > 0 { + h.cmdResponse = buf[1 : plen+2] + } else { + h.cmdResponse = buf[:0] + } + + if _debug { + println("evtCmdComplete", h.cmdCompleteOpcode, h.cmdCompleteStatus) + } + + return nil + + case evtCmdStatus: + h.cmdCompleteStatus = buf[2] + h.cmdCompleteOpcode = binary.LittleEndian.Uint16(buf[4:]) + if _debug { + println("evtCmdStatus", h.cmdCompleteOpcode, h.cmdCompleteOpcode, h.cmdCompleteStatus) + } + + h.cmdResponse = buf[:0] + + return nil + + case evtNumCompPkts: + if _debug { + println("evtNumCompPkts") + } + case evtLEMetaEvent: + if _debug { + println("evtLEMetaEvent") + } + + switch buf[2] { + case leMetaEventConnComplete, leMetaEventEnhancedConnectionComplete: + if _debug { + println("leMetaEventConnComplete") + } + + h.connectData.connected = true + h.connectData.status = buf[3] + h.connectData.handle = binary.LittleEndian.Uint16(buf[4:]) + h.connectData.role = buf[6] + h.connectData.peerBdaddrType = buf[7] + copy(h.connectData.peerBdaddr[0:], buf[8:]) + + return nil + + case leMetaEventAdvertisingReport: + h.advData.reported = true + h.advData.numReports = buf[3] + h.advData.typ = buf[4] + h.advData.peerBdaddrType = buf[5] + copy(h.advData.peerBdaddr[0:], buf[6:]) + h.advData.eirLength = buf[12] + h.advData.rssi = 0 + if _debug { + println("leMetaEventAdvertisingReport", plen, h.advData.numReports, + h.advData.typ, h.advData.peerBdaddrType, h.advData.eirLength) + } + + if int(13+h.advData.eirLength+1) > len(buf) || h.advData.eirLength > 31 { + if _debug { + println("invalid packet length", h.advData.eirLength, len(buf)) + } + return ErrHCIInvalidPacket + } + copy(h.advData.eirData[0:h.advData.eirLength], buf[13:13+h.advData.eirLength]) + + // TODO: handle multiple reports + if h.advData.numReports == 0x01 { + h.advData.rssi = int8(buf[int(13+h.advData.eirLength)]) + } + + return nil + + case leMetaEventLongTermKeyRequest: + if _debug { + println("leMetaEventLongTermKeyRequest") + } + + case leMetaEventRemoteConnParamReq: + if _debug { + println("leMetaEventRemoteConnParamReq") + } + + connectionHandle := binary.LittleEndian.Uint16(buf[3:]) + intervalMin := binary.LittleEndian.Uint16(buf[5:]) + intervalMax := binary.LittleEndian.Uint16(buf[7:]) + latency := binary.LittleEndian.Uint16(buf[9:]) + timeOut := binary.LittleEndian.Uint16(buf[11:]) + + var b [14]byte + binary.LittleEndian.PutUint16(b[0:], connectionHandle) + binary.LittleEndian.PutUint16(b[2:], intervalMin) + binary.LittleEndian.PutUint16(b[4:], intervalMax) + binary.LittleEndian.PutUint16(b[6:], latency) + binary.LittleEndian.PutUint16(b[8:], timeOut) + binary.LittleEndian.PutUint16(b[10:], 0x000F) + binary.LittleEndian.PutUint16(b[12:], 0x0FFF) + + return h.sendCommandWithParams(ogfLECtrl<<10|ocfLEParamRequestReply, b[:]) + + case leMetaEventConnectionUpdateComplete: + if _debug { + println("leMetaEventConnectionUpdateComplete") + } + + case leMetaEventReadLocalP256Complete: + if _debug { + println("leMetaEventReadLocalP256Complete") + } + + case leMetaEventGenerateDHKeyComplete: + if _debug { + println("leMetaEventGenerateDHKeyComplete") + } + + default: + if _debug { + println("unknown metaevent", buf[2], buf[3], buf[4], buf[5]) + } + + h.clearAdvData() + return ErrHCIUnknownEvent + } + case evtHardwareError: + return ErrHCIUnknownEvent + } + + return nil +} + +func (h *hci) clearAdvData() error { + h.advData.reported = false + h.advData.numReports = 0 + h.advData.typ = 0 + h.advData.peerBdaddrType = 0 + h.advData.peerBdaddr = [6]uint8{} + h.advData.eirLength = 0 + h.advData.eirData = [31]uint8{} + h.advData.rssi = 0 + + return nil +} + +func (h *hci) clearConnectData() error { + h.connectData.connected = false + h.connectData.status = 0 + h.connectData.handle = 0 + h.connectData.role = 0 + h.connectData.peerBdaddrType = 0 + h.connectData.peerBdaddr = [6]uint8{} + + return nil +} diff --git a/nodebug.go b/nodebug.go new file mode 100644 index 0000000..78a980e --- /dev/null +++ b/nodebug.go @@ -0,0 +1,5 @@ +//go:build !bledebug + +package bluetooth + +var _debug = false diff --git a/uuid_ninafw.go b/uuid_ninafw.go new file mode 100644 index 0000000..107e11f --- /dev/null +++ b/uuid_ninafw.go @@ -0,0 +1,20 @@ +//go:build ninafw + +package bluetooth + +type shortUUID uint16 + +// UUID returns the full length UUID for this short UUID. +func (s shortUUID) UUID() UUID { + return New16BitUUID(uint16(s)) +} + +// isIn checks the passed in slice of UUIDs to see if this uuid is in it. +func (uuid UUID) isIn(uuids []UUID) bool { + for _, u := range uuids { + if u == uuid { + return true + } + } + return false +}