fermentord/vendor/github.com/JuulLabs-OSS/ble/linux/att/server.go

674 lines
18 KiB
Go
Raw Normal View History

package att
import (
"bytes"
"encoding/binary"
"fmt"
"io"
"time"
"github.com/JuulLabs-OSS/ble"
)
type conn struct {
ble.Conn
svr *Server
cccs map[uint16]uint16
nn map[uint16]ble.Notifier
in map[uint16]ble.Notifier
}
// Server implements an ATT (Attribute Protocol) server.
type Server struct {
conn *conn
db *DB
// Refer to [Vol 3, Part F, 3.3.2 & 3.3.3] for the requirement of
// sequential request-response protocol, and transactions.
rxMTU int
txBuf []byte
chNotBuf chan []byte
chIndBuf chan []byte
chConfirm chan bool
dummyRspWriter ble.ResponseWriter
// Store a write handler for defer execute once receiving ExecuteWriteRequest
prepareWriteRequestAttr *attr
prepareWriteRequestData bytes.Buffer
}
// NewServer returns an ATT (Attribute Protocol) server.
func NewServer(db *DB, l2c ble.Conn) (*Server, error) {
mtu := l2c.RxMTU()
if mtu < ble.DefaultMTU || mtu > ble.MaxMTU {
return nil, fmt.Errorf("invalid MTU")
}
// Although the rxBuf is initialized with the capacity of rxMTU, it is
// not discovered, and only the default ATT_MTU (23 bytes) of it shall
// be used until remote central request ExchangeMTU.
s := &Server{
conn: &conn{
Conn: l2c,
cccs: make(map[uint16]uint16),
in: make(map[uint16]ble.Notifier),
nn: make(map[uint16]ble.Notifier),
},
db: db,
rxMTU: mtu,
txBuf: make([]byte, ble.DefaultMTU, ble.DefaultMTU),
chNotBuf: make(chan []byte, 1),
chIndBuf: make(chan []byte, 1),
chConfirm: make(chan bool),
dummyRspWriter: ble.NewResponseWriter(nil),
}
s.conn.svr = s
s.chNotBuf <- make([]byte, ble.DefaultMTU, ble.DefaultMTU)
s.chIndBuf <- make([]byte, ble.DefaultMTU, ble.DefaultMTU)
return s, nil
}
// notify sends notification to remote central.
func (s *Server) notify(h uint16, data []byte) (int, error) {
// Acquire and reuse notifyBuffer. Release it after usage.
nBuf := <-s.chNotBuf
defer func() { s.chNotBuf <- nBuf }()
rsp := HandleValueNotification(nBuf)
rsp.SetAttributeOpcode()
rsp.SetAttributeHandle(h)
buf := bytes.NewBuffer(rsp.AttributeValue())
buf.Reset()
if len(data) > buf.Cap() {
data = data[:buf.Cap()]
}
buf.Write(data)
return s.conn.Write(rsp[:3+buf.Len()])
}
// indicate sends indication to remote central.
func (s *Server) indicate(h uint16, data []byte) (int, error) {
// Acquire and reuse indicateBuffer. Release it after usage.
iBuf := <-s.chIndBuf
defer func() { s.chIndBuf <- iBuf }()
rsp := HandleValueIndication(iBuf)
rsp.SetAttributeOpcode()
rsp.SetAttributeHandle(h)
buf := bytes.NewBuffer(rsp.AttributeValue())
buf.Reset()
if len(data) > buf.Cap() {
data = data[:buf.Cap()]
}
buf.Write(data)
n, err := s.conn.Write(rsp[:3+buf.Len()])
if err != nil {
return n, err
}
select {
case _, ok := <-s.chConfirm:
if !ok {
return 0, io.ErrClosedPipe
}
return n, nil
case <-time.After(time.Second * 30):
return 0, ErrSeqProtoTimeout
}
}
// Loop accepts incoming ATT request, and respond response.
func (s *Server) Loop() {
type sbuf struct {
buf []byte
len int
}
pool := make(chan *sbuf, 2)
pool <- &sbuf{buf: make([]byte, s.rxMTU)}
pool <- &sbuf{buf: make([]byte, s.rxMTU)}
seq := make(chan *sbuf)
go func() {
b := <-pool
for {
n, err := s.conn.Read(b.buf)
if n == 0 || err != nil {
close(seq)
close(s.chConfirm)
_ = s.conn.Close()
return
}
if b.buf[0] == HandleValueConfirmationCode {
select {
case s.chConfirm <- true:
default:
logger.Error("server", "received a spurious confirmation", nil)
}
continue
}
b.len = n
seq <- b // Send the current request for handling
b = <-pool // Swap the buffer for next incoming request.
}
}()
for req := range seq {
if rsp := s.handleRequest(req.buf[:req.len]); rsp != nil {
if len(rsp) != 0 {
s.conn.Write(rsp)
}
}
pool <- req
}
for h, ccc := range s.conn.cccs {
if ccc != 0 {
logger.Info("cleanup", ble.ContextKeyCCC, fmt.Sprintf("0x%02X", ccc))
}
if ccc&cccIndicate != 0 {
s.conn.in[h].Close()
}
if ccc&cccNotify != 0 {
s.conn.nn[h].Close()
}
}
}
func (s *Server) handleRequest(b []byte) []byte {
var resp []byte
logger.Debug("server", "req", fmt.Sprintf("% X", b))
switch reqType := b[0]; reqType {
case ExchangeMTURequestCode:
resp = s.handleExchangeMTURequest(b)
case FindInformationRequestCode:
resp = s.handleFindInformationRequest(b)
case FindByTypeValueRequestCode:
resp = s.handleFindByTypeValueRequest(b)
case ReadByTypeRequestCode:
resp = s.handleReadByTypeRequest(b)
case ReadRequestCode:
resp = s.handleReadRequest(b)
case ReadBlobRequestCode:
resp = s.handleReadBlobRequest(b)
case ReadByGroupTypeRequestCode:
resp = s.handleReadByGroupRequest(b)
case WriteRequestCode:
resp = s.handleWriteRequest(b)
case WriteCommandCode:
s.handleWriteCommand(b)
case PrepareWriteRequestCode:
resp = s.handlePrepareWriteRequest(b)
case ExecuteWriteRequestCode:
resp = s.handleExecuteWriteRequest(b)
case ReadMultipleRequestCode,
SignedWriteCommandCode:
fallthrough
default:
resp = newErrorResponse(reqType, 0x0000, ble.ErrReqNotSupp)
}
logger.Debug("server", "rsp", fmt.Sprintf("% X", resp))
return resp
}
// handle MTU Exchange request. [Vol 3, Part F, 3.4.2]
func (s *Server) handleExchangeMTURequest(r ExchangeMTURequest) []byte {
// Validate the request.
switch {
case len(r) != 3:
fallthrough
case r.ClientRxMTU() < 23:
return newErrorResponse(r.AttributeOpcode(), 0x0000, ble.ErrInvalidPDU)
}
txMTU := int(r.ClientRxMTU())
s.conn.SetTxMTU(txMTU)
if txMTU != len(s.txBuf) {
// Apply the txMTU afer this response has been sent and before
// any other attribute protocol PDU is sent.
defer func() {
s.txBuf = make([]byte, txMTU, txMTU)
<-s.chNotBuf
s.chNotBuf <- make([]byte, txMTU, txMTU)
<-s.chIndBuf
s.chIndBuf <- make([]byte, txMTU, txMTU)
}()
}
rsp := ExchangeMTUResponse(s.txBuf)
rsp.SetAttributeOpcode()
rsp.SetServerRxMTU(uint16(s.rxMTU))
return rsp[:3]
}
// handle Find Information request. [Vol 3, Part F, 3.4.3.1 & 3.4.3.2]
func (s *Server) handleFindInformationRequest(r FindInformationRequest) []byte {
// Validate the request.
switch {
case len(r) != 5:
return newErrorResponse(r.AttributeOpcode(), 0x0000, ble.ErrInvalidPDU)
case r.StartingHandle() == 0 || r.StartingHandle() > r.EndingHandle():
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), ble.ErrInvalidHandle)
}
rsp := FindInformationResponse(s.txBuf)
rsp.SetAttributeOpcode()
rsp.SetFormat(0x00)
buf := bytes.NewBuffer(rsp.InformationData())
buf.Reset()
// Each response shall contain Types of the same format.
for _, a := range s.db.subrange(r.StartingHandle(), r.EndingHandle()) {
if rsp.Format() == 0 {
rsp.SetFormat(0x01)
if a.typ.Len() == 16 {
rsp.SetFormat(0x02)
}
}
if rsp.Format() == 0x01 && a.typ.Len() != 2 {
break
}
if rsp.Format() == 0x02 && a.typ.Len() != 16 {
break
}
if buf.Len()+2+a.typ.Len() > buf.Cap() {
break
}
binary.Write(buf, binary.LittleEndian, a.h)
binary.Write(buf, binary.LittleEndian, a.typ)
}
// Nothing has been found.
if rsp.Format() == 0 {
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), ble.ErrAttrNotFound)
}
return rsp[:2+buf.Len()]
}
// handle Find By Type Value request. [Vol 3, Part F, 3.4.3.3 & 3.4.3.4]
func (s *Server) handleFindByTypeValueRequest(r FindByTypeValueRequest) []byte {
// Validate the request.
switch {
case len(r) < 7:
return newErrorResponse(r.AttributeOpcode(), 0x0000, ble.ErrInvalidPDU)
case r.StartingHandle() == 0 || r.StartingHandle() > r.EndingHandle():
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), ble.ErrInvalidHandle)
}
rsp := FindByTypeValueResponse(s.txBuf)
rsp.SetAttributeOpcode()
buf := bytes.NewBuffer(rsp.HandleInformationList())
buf.Reset()
for _, a := range s.db.subrange(r.StartingHandle(), r.EndingHandle()) {
v, starth, endh := a.v, a.h, a.endh
if !a.typ.Equal(ble.UUID16(r.AttributeType())) {
continue
}
if v == nil {
// The value shall not exceed ATT_MTU - 7 bytes.
// Since ResponseWriter caps the value at the capacity,
// we allocate one extra byte, and the written length.
buf2 := bytes.NewBuffer(make([]byte, 0, len(s.txBuf)-7+1))
e := handleATT(a, s, r, ble.NewResponseWriter(buf2))
if e != ble.ErrSuccess || buf2.Len() > len(s.txBuf)-7 {
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), ble.ErrInvalidHandle)
}
endh = a.h
}
if !(ble.UUID(v).Equal(ble.UUID(r.AttributeValue()))) {
continue
}
if buf.Len()+4 > buf.Cap() {
break
}
binary.Write(buf, binary.LittleEndian, starth)
binary.Write(buf, binary.LittleEndian, endh)
}
if buf.Len() == 0 {
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), ble.ErrAttrNotFound)
}
return rsp[:1+buf.Len()]
}
// handle Read By Type request. [Vol 3, Part F, 3.4.4.1 & 3.4.4.2]
func (s *Server) handleReadByTypeRequest(r ReadByTypeRequest) []byte {
// Validate the request.
switch {
case len(r) != 7 && len(r) != 21:
return newErrorResponse(r.AttributeOpcode(), 0x0000, ble.ErrInvalidPDU)
case r.StartingHandle() == 0 || r.StartingHandle() > r.EndingHandle():
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), ble.ErrInvalidHandle)
}
rsp := ReadByTypeResponse(s.txBuf)
rsp.SetAttributeOpcode()
buf := bytes.NewBuffer(rsp.AttributeDataList())
buf.Reset()
// handle length (2 bytes) + value length.
// Each response shall only contains values with the same size.
dlen := 0
for _, a := range s.db.subrange(r.StartingHandle(), r.EndingHandle()) {
if !a.typ.Equal(ble.UUID(r.AttributeType())) {
continue
}
v := a.v
if v == nil {
buf2 := bytes.NewBuffer(make([]byte, 0, len(s.txBuf)-2))
if e := handleATT(a, s, r, ble.NewResponseWriter(buf2)); e != ble.ErrSuccess {
// Return if the first value read cause an error.
if dlen == 0 {
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), e)
}
// Otherwise, skip to the next one.
break
}
v = buf2.Bytes()
}
if dlen == 0 {
// Found the first value.
dlen = 2 + len(v)
if dlen > 255 {
dlen = 255
}
if dlen > buf.Cap() {
dlen = buf.Cap()
}
rsp.SetLength(uint8(dlen))
} else if 2+len(v) != dlen {
break
}
if buf.Len()+dlen > buf.Cap() {
break
}
binary.Write(buf, binary.LittleEndian, a.h)
binary.Write(buf, binary.LittleEndian, v[:dlen-2])
}
if dlen == 0 {
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), ble.ErrAttrNotFound)
}
return rsp[:2+buf.Len()]
}
// handle Read request. [Vol 3, Part F, 3.4.4.3 & 3.4.4.4]
func (s *Server) handleReadRequest(r ReadRequest) []byte {
// Validate the request.
switch {
case len(r) != 3:
return newErrorResponse(r.AttributeOpcode(), 0x0000, ble.ErrInvalidPDU)
}
rsp := ReadResponse(s.txBuf)
rsp.SetAttributeOpcode()
buf := bytes.NewBuffer(rsp.AttributeValue())
buf.Reset()
a, ok := s.db.at(r.AttributeHandle())
if !ok {
return newErrorResponse(r.AttributeOpcode(), r.AttributeHandle(), ble.ErrInvalidHandle)
}
// Simple case. Read-only, no-authorization, no-authentication.
if a.v != nil {
binary.Write(buf, binary.LittleEndian, a.v)
return rsp[:1+buf.Len()]
}
// Pass the request to upper layer with the ResponseWriter, which caps
// the buffer to a valid length of payload.
if e := handleATT(a, s, r, ble.NewResponseWriter(buf)); e != ble.ErrSuccess {
return newErrorResponse(r.AttributeOpcode(), r.AttributeHandle(), e)
}
return rsp[:1+buf.Len()]
}
// handle Read Blob request. [Vol 3, Part F, 3.4.4.5 & 3.4.4.6]
func (s *Server) handleReadBlobRequest(r ReadBlobRequest) []byte {
// Validate the request.
switch {
case len(r) != 5:
return newErrorResponse(r.AttributeOpcode(), 0x0000, ble.ErrInvalidPDU)
}
a, ok := s.db.at(r.AttributeHandle())
if !ok {
return newErrorResponse(r.AttributeOpcode(), r.AttributeHandle(), ble.ErrInvalidHandle)
}
rsp := ReadBlobResponse(s.txBuf)
rsp.SetAttributeOpcode()
buf := bytes.NewBuffer(rsp.PartAttributeValue())
buf.Reset()
// Simple case. Read-only, no-authorization, no-authentication.
if a.v != nil {
binary.Write(buf, binary.LittleEndian, a.v)
return rsp[:1+buf.Len()]
}
// Pass the request to upper layer with the ResponseWriter, which caps
// the buffer to a valid length of payload.
if e := handleATT(a, s, r, ble.NewResponseWriter(buf)); e != ble.ErrSuccess {
return newErrorResponse(r.AttributeOpcode(), r.AttributeHandle(), e)
}
return rsp[:1+buf.Len()]
}
// handle Read Blob request. [Vol 3, Part F, 3.4.4.9 & 3.4.4.10]
func (s *Server) handleReadByGroupRequest(r ReadByGroupTypeRequest) []byte {
// Validate the request.
switch {
case len(r) != 7 && len(r) != 21:
return newErrorResponse(r.AttributeOpcode(), 0x0000, ble.ErrInvalidPDU)
case r.StartingHandle() == 0 || r.StartingHandle() > r.EndingHandle():
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), ble.ErrInvalidHandle)
}
rsp := ReadByGroupTypeResponse(s.txBuf)
rsp.SetAttributeOpcode()
buf := bytes.NewBuffer(rsp.AttributeDataList())
buf.Reset()
dlen := 0
for _, a := range s.db.subrange(r.StartingHandle(), r.EndingHandle()) {
v := a.v
if v == nil {
buf2 := bytes.NewBuffer(make([]byte, buf.Cap()-buf.Len()-4))
if e := handleATT(a, s, r, ble.NewResponseWriter(buf2)); e != ble.ErrSuccess {
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), e)
}
v = buf2.Bytes()
}
if dlen == 0 {
dlen = 4 + len(v)
if dlen > 255 {
dlen = 255
}
if dlen > buf.Cap() {
dlen = buf.Cap()
}
rsp.SetLength(uint8(dlen))
} else if 4+len(v) != dlen {
break
}
if buf.Len()+dlen > buf.Cap() {
break
}
binary.Write(buf, binary.LittleEndian, a.h)
binary.Write(buf, binary.LittleEndian, a.endh)
binary.Write(buf, binary.LittleEndian, v[:dlen-4])
}
if dlen == 0 {
return newErrorResponse(r.AttributeOpcode(), r.StartingHandle(), ble.ErrAttrNotFound)
}
return rsp[:2+buf.Len()]
}
// handle Write request. [Vol 3, Part F, 3.4.5.1 & 3.4.5.2]
func (s *Server) handleWriteRequest(r WriteRequest) []byte {
// Validate the request.
switch {
case len(r) < 3:
return newErrorResponse(r.AttributeOpcode(), 0x0000, ble.ErrInvalidPDU)
}
a, ok := s.db.at(r.AttributeHandle())
if !ok {
return newErrorResponse(r.AttributeOpcode(), r.AttributeHandle(), ble.ErrInvalidHandle)
}
// We don't support write to static value. Pass the request to upper layer.
if a == nil {
return newErrorResponse(r.AttributeOpcode(), r.AttributeHandle(), ble.ErrWriteNotPerm)
}
if e := handleATT(a, s, r, ble.NewResponseWriter(nil)); e != ble.ErrSuccess {
return newErrorResponse(r.AttributeOpcode(), r.AttributeHandle(), e)
}
return []byte{WriteResponseCode}
}
func (s *Server) handlePrepareWriteRequest(r PrepareWriteRequest) []byte {
logger.Debug("handlePrepareWriteRequest ->", "r.AttributeHandle", r.AttributeHandle())
// Validate the request.
switch {
case len(r) < 3:
return newErrorResponse(r.AttributeOpcode(), 0x0000, ble.ErrInvalidPDU)
}
a, ok := s.db.at(r.AttributeHandle())
if !ok {
return newErrorResponse(r.AttributeOpcode(), r.AttributeHandle(), ble.ErrInvalidHandle)
}
// We don't support write to static value. Pass the request to upper layer.
if a == nil {
return newErrorResponse(r.AttributeOpcode(), r.AttributeHandle(), ble.ErrWriteNotPerm)
}
if e := handleATT(a, s, r, ble.NewResponseWriter(nil)); e != ble.ErrSuccess {
return newErrorResponse(r.AttributeOpcode(), r.AttributeHandle(), e)
}
// Convert and validate the response.
rsp := PrepareWriteResponse(r)
rsp.SetAttributeOpcode()
return rsp
}
func (s *Server) handleExecuteWriteRequest(r ExecuteWriteRequest) []byte {
// Validate the request.
switch {
case len(r) < 2:
return newErrorResponse(r.AttributeOpcode(), 0x0000, ble.ErrInvalidPDU)
}
switch r.Flags() {
case 0:
// 0x00 Cancel all prepared writes
s.prepareWriteRequestAttr = nil
case 1:
// 0x01 Immediately write all pending prepared values
a := s.prepareWriteRequestAttr
if e := handleATT(a, s, r, ble.NewResponseWriter(nil)); e != ble.ErrSuccess {
return newErrorResponse(r.AttributeOpcode(), 0, e)
}
}
return []byte{ExecuteWriteResponseCode}
}
// handle Write command. [Vol 3, Part F, 3.4.5.3]
func (s *Server) handleWriteCommand(r WriteCommand) []byte {
// Validate the request.
switch {
case len(r) <= 3:
return nil
}
a, ok := s.db.at(r.AttributeHandle())
if !ok {
return nil
}
// We don't support write to static value. Pass the request to upper layer.
if a == nil {
return nil
}
if e := handleATT(a, s, r, s.dummyRspWriter); e != ble.ErrSuccess {
return nil
}
return nil
}
func newErrorResponse(op byte, h uint16, s ble.ATTError) []byte {
r := ErrorResponse(make([]byte, 5))
r.SetAttributeOpcode()
r.SetRequestOpcodeInError(op)
r.SetAttributeInError(h)
r.SetErrorCode(uint8(s))
return r
}
func handleATT(a *attr, s *Server, req []byte, rsp ble.ResponseWriter) ble.ATTError {
rsp.SetStatus(ble.ErrSuccess)
var offset int
var data []byte
conn := s.conn
switch req[0] {
case ReadByTypeRequestCode:
fallthrough
case ReadRequestCode:
if a.rh == nil {
return ble.ErrReadNotPerm
}
a.rh.ServeRead(ble.NewRequest(conn, data, offset), rsp)
case ReadBlobRequestCode:
if a.rh == nil {
return ble.ErrReadNotPerm
}
offset = int(ReadBlobRequest(req).ValueOffset())
a.rh.ServeRead(ble.NewRequest(conn, data, offset), rsp)
case PrepareWriteRequestCode:
if a.wh == nil {
return ble.ErrWriteNotPerm
}
data = PrepareWriteRequest(req).PartAttributeValue()
logger.Debug("handleATT", "PartAttributeValue",
fmt.Sprintf("data: %x, offset: %d, %p\n", data, int(PrepareWriteRequest(req).ValueOffset()), s.prepareWriteRequestAttr))
if s.prepareWriteRequestAttr == nil {
s.prepareWriteRequestAttr = a
s.prepareWriteRequestData.Reset()
}
s.prepareWriteRequestData.Write(data)
case ExecuteWriteRequestCode:
if a.wh == nil {
return ble.ErrWriteNotPerm
}
data = s.prepareWriteRequestData.Bytes()
a.wh.ServeWrite(ble.NewRequest(conn, data, offset), rsp)
s.prepareWriteRequestAttr = nil
case WriteRequestCode:
fallthrough
case WriteCommandCode:
if a.wh == nil {
return ble.ErrWriteNotPerm
}
data = WriteRequest(req).AttributeValue()
a.wh.ServeWrite(ble.NewRequest(conn, data, offset), rsp)
// case SignedWriteCommandCode:
// case ReadByGroupTypeRequestCode:
// case ReadMultipleRequestCode:
default:
return ble.ErrReqNotSupp
}
return rsp.Status()
}