Improbe ADV Manufacturer Data

gap.go

@@ -1,4 +1,4 @@
-package bluetooth
+package bluetooth
 
 import (
 	"errors"
@@ -46,7 +46,7 @@ type AdvertisementOptions struct {
 	// this is a zero-length string.
 	LocalName string
 
-	ManufacturerData []byte
+	ManufacturerData map[uint16][]byte
 
 	// ServiceUUIDs are the services (16-bit or 128-bit) that are broadcast as
 	// part of the advertisement packet, in data types such as "complete list of
@@ -191,6 +191,8 @@ func (p *advertisementFields) ManufacturerData() map[uint16][]byte {
 type rawAdvertisementPayload struct {
 	data [31]byte
 	len  uint8
+	scandata [31]byte
+	scanlen uint8
 }
 
 // Bytes returns the raw advertisement packet as a byte slice.
@@ -198,6 +200,11 @@ func (buf *rawAdvertisementPayload) Bytes() []byte {
 	return buf.data[:buf.len]
 }
 
+// Bytes returns the raw advertisement packet as a byte slice.
+func (buf *rawAdvertisementPayload) scanBytes() []byte {
+	return buf.scandata[:buf.scanlen]
+}
+
 // findField returns the data of a specific field in the advertisement packet.
 //
 // See this list of field types:
@@ -215,6 +222,18 @@ func (buf *rawAdvertisementPayload) findField(fieldType byte) []byte {
 		}
 		data = data[fieldLength+1:]
 	}
+	scandata := buf.scanBytes()
+	for len(scandata) >= 2 {
+		fieldLength := scandata[0]
+		if int(fieldLength)+1 > len(scandata) {
+			// Invalid field length.
+			return nil
+		}
+		if fieldType == scandata[1] {
+			return data[2 : fieldLength+1]
+		}
+		scandata = scandata[fieldLength+1:]
+	}
 	return nil
 }
 
@@ -296,6 +315,7 @@ func (buf *rawAdvertisementPayload) reset() {
 	// The data is not reset (only the length), because with a zero length the
 	// data is undefined.
 	buf.len = 0
+	buf.scanlen = 0
 }
 
 // addFromOptions constructs a new advertisement payload (assumed to be empty
@@ -303,10 +323,10 @@ func (buf *rawAdvertisementPayload) reset() {
 // false otherwise.
 func (buf *rawAdvertisementPayload) addFromOptions(options AdvertisementOptions) (ok bool) {
 	buf.addFlags(0x06)
-	if len(options.ManufacturerData) != 0 {
+	/*if len(options.ManufacturerData) != 0 {
 		copy(buf.data[buf.len:len(options.ManufacturerData)+int(buf.len)], options.ManufacturerData[0:len(options.ManufacturerData)])
 		buf.len += uint8(len(options.ManufacturerData))
-	}
+	}*/
 	if options.LocalName != "" {
 		if !buf.addCompleteLocalName(options.LocalName) {
 			return false
@@ -322,6 +342,13 @@ func (buf *rawAdvertisementPayload) addFromOptions(options AdvertisementOptions)
 			return false
 		}
 	}
+	
+	for _, data := range options.ManufacturerData {
+		if !buf.addManufacturerData(data) {
+			return false
+		}
+	}
+	
 	return true
 }
 
@@ -342,14 +369,14 @@ func (buf *rawAdvertisementPayload) addFlags(flags byte) (ok bool) {
 // addCompleteLocalName adds the Complete Local Name field to the advertisement
 // buffer. It returns true on success (the name fits) and false on failure.
 func (buf *rawAdvertisementPayload) addCompleteLocalName(name string) (ok bool) {
-	if int(buf.len)+len(name)+2 > len(buf.data) {
+	if int(buf.scanlen)+len(name)+2 > len(buf.scandata) {
 		return false // name doesn't fit
 	}
 
-	buf.data[buf.len] = byte(len(name) + 1) // length of field (including type)
-	buf.data[buf.len+1] = 9                 // type, 0x09 means Complete Local name
-	copy(buf.data[buf.len+2:], name)        // copy the name into the buffer
-	buf.len += byte(len(name) + 2)
+	buf.scandata[buf.scanlen] = byte(len(name) + 1) // length of field (including type)
+	buf.scandata[buf.scanlen+1] = 9                 // type, 0x09 means Complete Local name
+	copy(buf.scandata[buf.scanlen+2:], name)        // copy the name into the buffer
+	buf.scanlen += byte(len(name) + 2)
 	return true
 }
 
@@ -364,25 +391,42 @@ func (buf *rawAdvertisementPayload) addServiceUUID(uuid UUID) (ok bool) {
 			return false // UUID doesn't fit.
 		}
 		shortUUID := uuid.Get16Bit()
-		buf.data[buf.len+0] = 3    // length of field, including type
-		buf.data[buf.len+1] = 0x03 // type, 0x03 means "Complete List of 16-bit Service Class UUIDs"
-		buf.data[buf.len+2] = byte(shortUUID)
-		buf.data[buf.len+3] = byte(shortUUID >> 8)
+		buf.scandata[buf.scanlen+0] = 3    // length of field, including type
+		buf.scandata[buf.scanlen+1] = 0x03 // type, 0x03 means "Complete List of 16-bit Service Class UUIDs"
+		buf.scandata[buf.scanlen+2] = byte(shortUUID)
+		buf.scandata[buf.scanlen+3] = byte(shortUUID >> 8)
 		buf.len += 4
 		return true
 	} else {
 		if int(buf.len)+18 > len(buf.data) {
 			return false // UUID doesn't fit.
 		}
-		buf.data[buf.len+0] = 17   // length of field, including type
-		buf.data[buf.len+1] = 0x07 // type, 0x07 means "Complete List of 128-bit Service Class UUIDs"
+		buf.scandata[buf.scanlen+0] = 17   // length of field, including type
+		buf.scandata[buf.scanlen+1] = 0x07 // type, 0x07 means "Complete List of 128-bit Service Class UUIDs"
 		rawUUID := uuid.Bytes()
-		copy(buf.data[buf.len+2:], rawUUID[:])
+		copy(buf.scandata[buf.scanlen+2:], rawUUID[:])
 		buf.len += 18
 		return true
 	}
 }
 
+func (buf *rawAdvertisementPayload) addManufacturerData(mdid uint16, []byte data) (ok bool) {
+    /*
+    var adv AdvertisementOptions
+	adv.ManufacturerData = make(map[uint16][]byte)
+	adv.ManufacturerData[0x1545] = []byte{0x12, 0x15, 0x19}
+	*/
+	if int(buf.len)+len(data)+4 > len(buf.data) {
+		return false // flags don't fit
+	}
+
+	buf.data[buf.len] = len(data)+3       // length of field (including type)
+	buf.data[buf.len+1] = 0x99  // type, 0x01 means Flags
+	buf.data[buf.len+2] = copy(buf.data[buf.len+4:], data[:]) // the flags
+	buf.len += 3
+	return true
+}
+
 // ConnectionParams are used when connecting to a peripherals.
 type ConnectionParams struct {
 	// The timeout for the connection attempt. Not used during the rest of the

gap_nrf51.go

@@ -52,7 +52,7 @@ func (a *Advertisement) Configure(options AdvertisementOptions) error {
 		return errAdvertisementPacketTooBig
 	}
 
-	errCode := C.sd_ble_gap_adv_data_set(&payload.data[0], payload.len, nil, 0)
+	errCode := C.sd_ble_gap_adv_data_set(&payload.data[0], payload.len, &payload.scandata[0], payload.scanlen)
 	a.interval = options.Interval
 	return makeError(errCode)
 }