static int gxf_write_umf_user_data(ByteIOContext *pb, GXFContext *ctx)
{
offset_t pos = url_ftell(pb);
ctx->umf_user_data_offset = pos - ctx->umf_start_offset;
put_le32(pb, 20);
put_le32(pb, 0);
put_le16(pb, 0);
put_le16(pb, 0);
put_le32(pb, 0);
put_byte(pb, 0);
put_byte(pb, 0);
put_byte(pb, 0);
put_byte(pb, 0);
return 20;
}
uint16_t enc_signed_write_cmd(uint16_t handle, const uint8_t *value,
size_t vlen, struct bt_crypto *crypto,
const uint8_t csrk[16],
uint32_t sign_cnt,
uint8_t *pdu, size_t len)
{
const uint16_t hdr_len = sizeof(pdu[0]) + sizeof(handle);
const uint16_t min_len = hdr_len + ATT_SIGNATURE_LEN;
if (pdu == NULL)
return 0;
if (vlen > len - min_len)
vlen = len - min_len;
pdu[0] = ATT_OP_SIGNED_WRITE_CMD;
put_le16(handle, &pdu[1]);
if (vlen > 0)
memcpy(&pdu[hdr_len], value, vlen);
if (!bt_crypto_sign_att(crypto, csrk, pdu, hdr_len + vlen, sign_cnt,
&pdu[hdr_len + vlen]))
return 0;
return min_len + vlen;
}
static void enable_report_notifications(struct report *report,
bool enable_on_device)
{
struct hog_device *hogdev = report->hogdev;
uint8_t value[2];
if (!hogdev->uhid_created)
return;
if (!report->ccc_handle)
return;
/* Register callback for HoG report notifications */
report->notifyid = g_attrib_register(hogdev->attrib,
ATT_OP_HANDLE_NOTIFY,
report->decl->value_handle,
report_value_cb, report, NULL);
if (!enable_on_device)
return;
/* Enable HoG report notifications on the HoG device */
put_le16(GATT_CLIENT_CHARAC_CFG_NOTIF_BIT, value);
gatt_write_char(hogdev->attrib, report->ccc_handle, value,
sizeof(value), report_ccc_written_cb, report);
}
static int gif_write_video(AVFormatContext *s,
AVCodecContext *enc, const uint8_t *buf, int size)
{
ByteIOContext *pb = s->pb;
GIFContext *gif = s->priv_data;
int jiffies;
int64_t delay;
/* graphic control extension block */
put_byte(pb, 0x21);
put_byte(pb, 0xf9);
put_byte(pb, 0x04); /* block size */
put_byte(pb, 0x04); /* flags */
/* 1 jiffy is 1/70 s */
/* the delay_time field indicates the number of jiffies - 1 */
delay = gif->file_time - gif->time;
/* XXX: should use delay, in order to be more accurate */
/* instead of using the same rounded value each time */
/* XXX: don't even remember if I really use it for now */
jiffies = (70*enc->time_base.num/enc->time_base.den) - 1;
put_le16(pb, jiffies);
put_byte(pb, 0x1f); /* transparent color index */
put_byte(pb, 0x00);
gif_image_write_image(pb, 0, 0, enc->width, enc->height,
buf, enc->width * 3, PIX_FMT_RGB24);
put_flush_packet(s->pb);
return 0;
}
int
bletest_hci_le_rd_chanmap(uint16_t handle)
{
int rc;
uint8_t *dst;
uint8_t buf[BLE_HCI_CMD_HDR_LEN + BLE_HCI_RD_CHANMAP_LEN];
uint8_t rspbuf[BLE_HCI_RD_CHANMAP_RSP_LEN];
uint8_t rsplen;
dst = buf;
ble_hs_hci_cmd_write_hdr(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_RD_CHAN_MAP,
BLE_HCI_RD_CHANMAP_LEN, dst);
dst += BLE_HCI_CMD_HDR_LEN;
put_le16(dst, handle);
rc = ble_hs_hci_cmd_tx(buf, rspbuf, BLE_HCI_RD_CHANMAP_RSP_LEN, &rsplen);
if (rc != 0) {
return rc;
}
if (rsplen != BLE_HCI_RD_CHANMAP_RSP_LEN) {
return BLE_HS_ECONTROLLER;
}
return rc;
}
static void exchange_mtu_cb(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data)
{
struct bt_gatt_server *server = user_data;
uint16_t client_rx_mtu;
uint16_t final_mtu;
uint8_t rsp_pdu[2];
if (length != 2) {
bt_att_send_error_rsp(server->att, opcode, 0,
BT_ATT_ERROR_INVALID_PDU);
return;
}
client_rx_mtu = get_le16(pdu);
final_mtu = MAX(MIN(client_rx_mtu, server->mtu), BT_ATT_DEFAULT_LE_MTU);
/* Respond with the server MTU */
put_le16(server->mtu, rsp_pdu);
bt_att_send(server->att, BT_ATT_OP_MTU_RSP, rsp_pdu, 2, NULL, NULL,
NULL);
/* Set MTU to be the minimum */
server->mtu = final_mtu;
bt_att_set_mtu(server->att, final_mtu);
util_debug(server->debug_callback, server->debug_data,
"MTU exchange complete, with MTU: %u", final_mtu);
}
static void discover_descriptor_cb(guint8 status, const guint8 *pdu,
guint16 len, gpointer user_data)
{
struct scan *scan = user_data;
struct att_data_list *list;
uint8_t *ptr;
uint16_t uuid16, handle;
uint8_t value[2];
uint8_t format;
list = dec_find_info_resp(pdu, len, &format);
if (list == NULL)
return;
if (format != ATT_FIND_INFO_RESP_FMT_16BIT)
goto done;
ptr = list->data[0];
handle = get_le16(ptr);
uuid16 = get_le16(&ptr[2]);
if (uuid16 != GATT_CLIENT_CHARAC_CFG_UUID)
goto done;
put_le16(GATT_CLIENT_CHARAC_CFG_NOTIF_BIT, value);
gatt_write_char(scan->attrib, handle, value, sizeof(value),
ccc_written_cb, user_data);
done:
att_data_list_free(list);
}
bool bt_gatt_server_send_notification(struct bt_gatt_server *server,
uint16_t handle, const uint8_t *value,
uint16_t length)
{
uint16_t pdu_len;
uint8_t *pdu;
bool result;
if (!server || (length && !value))
return false;
pdu_len = MIN(bt_att_get_mtu(server->att) - 1, length + 2);
pdu = malloc(pdu_len);
if (!pdu)
return false;
put_le16(handle, pdu);
memcpy(pdu + 2, value, pdu_len - 2);
result = !!bt_att_send(server->att, BT_ATT_OP_HANDLE_VAL_NOT, pdu,
pdu_len, NULL, NULL, NULL);
free(pdu);
return result;
}
static bool hr_msrmt_cb(void *user_data)
{
struct server *server = user_data;
bool expended_present = !(server->hr_ee_count % 10);
uint16_t len = 2;
uint8_t pdu[4];
uint32_t cur_ee;
pdu[0] = 0x06;
pdu[1] = 90 + (rand() % 40);
if (expended_present) {
pdu[0] |= 0x08;
put_le16(server->hr_energy_expended, pdu + 2);
len += 2;
}
bt_gatt_server_send_notification(server->gatt,
server->hr_msrmt_handle,
pdu, len);
cur_ee = server->hr_energy_expended;
server->hr_energy_expended = MIN(UINT16_MAX, cur_ee + 10);
server->hr_ee_count++;
return true;
}
static int put_payload_parsing_info(
AVFormatContext *s,
unsigned int sendtime,
unsigned int duration,
int nb_payloads,
int padsize
)
{
ASFContext *asf = s->priv_data;
ByteIOContext *pb = &s->pb;
int ppi_size, i;
unsigned char *start_ppi_ptr = pb->buf_ptr;
int iLengthTypeFlags = ASF_PPI_LENGTH_TYPE_FLAGS;
put_byte(pb, ASF_PACKET_ERROR_CORRECTION_FLAGS);
for (i = 0; i < ASF_PACKET_ERROR_CORRECTION_DATA_SIZE; i++){
put_byte(pb, 0x0);
}
if (asf->multi_payloads_present)
iLengthTypeFlags |= ASF_PPI_FLAG_MULTIPLE_PAYLOADS_PRESENT;
if (padsize > 0) {
if (padsize < 256)
iLengthTypeFlags |= ASF_PPI_FLAG_PADDING_LENGTH_FIELD_IS_BYTE;
else
iLengthTypeFlags |= ASF_PPI_FLAG_PADDING_LENGTH_FIELD_IS_WORD;
}
put_byte(pb, iLengthTypeFlags);
put_byte(pb, ASF_PPI_PROPERTY_FLAGS);
if (iLengthTypeFlags & ASF_PPI_FLAG_PADDING_LENGTH_FIELD_IS_WORD)
put_le16(pb, padsize - 2);
if (iLengthTypeFlags & ASF_PPI_FLAG_PADDING_LENGTH_FIELD_IS_BYTE)
put_byte(pb, padsize - 1);
put_le32(pb, sendtime);
put_le16(pb, duration);
if (asf->multi_payloads_present)
put_byte(pb, nb_payloads | ASF_PAYLOAD_FLAGS);
ppi_size = pb->buf_ptr - start_ppi_ptr;
return ppi_size;
}
static int voc_write_header(AVFormatContext *s)
{
ByteIOContext *pb = s->pb;
const int header_size = 26;
const int version = 0x0114;
if (s->nb_streams != 1
|| s->streams[0]->codec->codec_type != CODEC_TYPE_AUDIO)
return AVERROR_PATCHWELCOME;
put_buffer(pb, ff_voc_magic, sizeof(ff_voc_magic) - 1);
put_le16(pb, header_size);
put_le16(pb, version);
put_le16(pb, ~version + 0x1234);
return 0;
}
static void put_uuid_le(const bt_uuid_t *uuid, void *dst)
{
if (uuid->type == BT_UUID16)
put_le16(uuid->value.u16, dst);
else
/* Convert from 128-bit BE to LE */
bswap_128(&uuid->value.u128, dst);
}
static inline void put_uuid_le(const bt_uuid_t *src, void *dst)
{
if (src->type == BT_UUID16)
put_le16(src->value.u16, dst);
else
/* Convert from 128-bit BE to LE */
bswap_128(&src->value.u128, dst);
}
static void put_str16_nolen(ByteIOContext *s, const char *tag)
{
int c;
do{
c = (uint8_t)*tag++;
put_le16(s, c);
}while(c);
}
static int avi_write_ix(AVFormatContext *s)
{
ByteIOContext *pb = s->pb;
AVIContext *avi = s->priv_data;
char tag[5];
char ix_tag[] = "ix00";
int i, j;
assert(!url_is_streamed(pb));
if (avi->riff_id > AVI_MASTER_INDEX_SIZE)
return -1;
for (i=0;i<s->nb_streams;i++) {
AVIStream *avist= s->streams[i]->priv_data;
int64_t ix, pos;
avi_stream2fourcc(&tag[0], i, s->streams[i]->codec->codec_type);
ix_tag[3] = '0' + i;
/* Writing AVI OpenDML leaf index chunk */
ix = url_ftell(pb);
put_tag(pb, &ix_tag[0]); /* ix?? */
put_le32(pb, avist->indexes.entry * 8 + 24);
/* chunk size */
put_le16(pb, 2); /* wLongsPerEntry */
put_byte(pb, 0); /* bIndexSubType (0 == frame index) */
put_byte(pb, 1); /* bIndexType (1 == AVI_INDEX_OF_CHUNKS) */
put_le32(pb, avist->indexes.entry);
/* nEntriesInUse */
put_tag(pb, &tag[0]); /* dwChunkId */
put_le64(pb, avi->movi_list);/* qwBaseOffset */
put_le32(pb, 0); /* dwReserved_3 (must be 0) */
for (j=0; j<avist->indexes.entry; j++) {
AVIIentry* ie = avi_get_ientry(&avist->indexes, j);
put_le32(pb, ie->pos + 8);
put_le32(pb, ((uint32_t)ie->len & ~0x80000000) |
(ie->flags & 0x10 ? 0 : 0x80000000));
}
put_flush_packet(pb);
pos = url_ftell(pb);
/* Updating one entry in the AVI OpenDML master index */
url_fseek(pb, avist->indexes.indx_start - 8, SEEK_SET);
put_tag(pb, "indx"); /* enabling this entry */
url_fskip(pb, 8);
put_le32(pb, avi->riff_id); /* nEntriesInUse */
url_fskip(pb, 16*avi->riff_id);
put_le64(pb, ix); /* qwOffset */
put_le32(pb, pos - ix); /* dwSize */
put_le32(pb, avist->indexes.entry); /* dwDuration */
url_fseek(pb, pos, SEEK_SET);
}
return 0;
}
static void write_ccc(GAttrib *attrib, uint16_t handle, void *user_data)
{
uint8_t value[2];
put_le16(GATT_CLIENT_CHARAC_CFG_NOTIF_BIT, value);
gatt_write_char(attrib, handle, value, sizeof(value),
report_ccc_written_cb, user_data);
}
/**
* HCI read buffer size command. Returns the ACL data packet length and
* num data packets.
*
* @param rspbuf Pointer to response buffer
* @param rsplen Length of response buffer
*
* @return int BLE error code
*/
static int
ble_ll_hci_le_read_bufsize(uint8_t *rspbuf, uint8_t *rsplen)
{
/* Place the data packet length and number of packets in the buffer */
put_le16(rspbuf, g_ble_ll_data.ll_acl_pkt_size);
rspbuf[2] = g_ble_ll_data.ll_num_acl_pkts;
*rsplen = BLE_HCI_RD_BUF_SIZE_RSPLEN;
return BLE_ERR_SUCCESS;
}
static void cmd_heart_rate(struct server *server, char *cmd_str)
{
bool enable;
uint8_t pdu[4];
struct gatt_db_attribute *attr;
if (!cmd_str) {
heart_rate_usage();
return;
}
if (strcmp(cmd_str, "on") == 0)
enable = true;
else if (strcmp(cmd_str, "off") == 0)
enable = false;
else {
heart_rate_usage();
return;
}
if (enable == server->hr_visible) {
printf("Heart Rate Service already %s\n",
enable ? "visible" : "hidden");
return;
}
server->hr_visible = enable;
attr = gatt_db_get_attribute(server->db, server->hr_handle);
gatt_db_service_set_active(attr, server->hr_visible);
update_hr_msrmt_simulation(server);
if (!server->svc_chngd_enabled)
return;
put_le16(server->hr_handle, pdu);
put_le16(server->hr_handle + 7, pdu + 2);
server->hr_msrmt_enabled = false;
update_hr_msrmt_simulation(server);
bt_gatt_server_send_indication(server->gatt,
server->gatt_svc_chngd_handle,
pdu, 4, conf_cb, NULL, NULL);
}
static struct os_mbuf *
bletest_send_packet(uint16_t handle)
{
int j;
uint8_t val;
struct os_mbuf *om;
uint16_t pktlen;
om = bletest_get_packet();
if (om) {
/* set payload length */
#if BLETEST_THROUGHPUT_TEST
pktlen = BLETEST_PKT_SIZE;
#else
#if (BLETEST_CFG_RAND_PKT_SIZE == 1)
pktlen = rand() % (BLETEST_MAX_PKT_SIZE + 1);
#else
pktlen = BLETEST_PKT_SIZE;
#endif
#endif
/* Put the HCI header in the mbuf */
put_le16(om->om_data, handle);
put_le16(om->om_data + 2, pktlen + 4);
/* Place L2CAP header in packet */
put_le16(om->om_data + 4, pktlen);
om->om_data[6] = 0;
om->om_data[7] = 0;
om->om_len = 8;
OS_MBUF_PKTHDR(om)->omp_len = 8;
/* Fill with incrementing pattern (starting from 1) */
for (j = 0; j < pktlen; ++j) {
val = j + 1;
os_mbuf_append(om, &val, 1);
}
/* Transmit it */
ble_hci_trans_hs_acl_tx(om);
}
return om;
}
static int gxf_write_umf_track_description(AVFormatContext *s)
{
ByteIOContext *pb = s->pb;
GXFContext *gxf = s->priv_data;
int64_t pos = url_ftell(pb);
int i;
gxf->umf_track_offset = pos - gxf->umf_start_offset;
for (i = 0; i < s->nb_streams; ++i) {
GXFStreamContext *sc = s->streams[i]->priv_data;
put_le16(pb, sc->media_info);
put_le16(pb, 1);
}
put_le16(pb, gxf->timecode_track.media_info);
put_le16(pb, 1);
return url_ftell(pb) - pos;
}
static void register_tx_power(struct btd_adapter *adapter)
{
uint16_t start_handle, h;
const int svc_size = 4;
uint8_t atval[256];
bt_uuid_t uuid;
bt_uuid16_create(&uuid, TX_POWER_SVC_UUID);
start_handle = attrib_db_find_avail(adapter, &uuid, svc_size);
if (start_handle == 0) {
error("Not enough free handles to register service");
return;
}
DBG("start_handle=0x%04x", start_handle);
h = start_handle;
/* Primary service definition */
bt_uuid16_create(&uuid, GATT_PRIM_SVC_UUID);
put_le16(TX_POWER_SVC_UUID, &atval[0]);
attrib_db_add(adapter, h++, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 2);
/* Power level characteristic */
bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
atval[0] = GATT_CHR_PROP_READ | GATT_CHR_PROP_NOTIFY;
put_le16(h + 1, &atval[1]);
put_le16(POWER_LEVEL_CHR_UUID, &atval[3]);
attrib_db_add(adapter, h++, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 5);
/* Power level value */
bt_uuid16_create(&uuid, POWER_LEVEL_CHR_UUID);
atval[0] = 0x00;
attrib_db_add(adapter, h++, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 1);
/* Client characteristic configuration */
bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID);
atval[0] = 0x00;
atval[1] = 0x00;
attrib_db_add(adapter, h++, &uuid, ATT_NONE, ATT_NONE, atval, 2);
g_assert(h - start_handle == svc_size);
}
static void write_ccc(struct bt_bas *bas, GAttrib *attrib, uint16_t handle,
void *user_data)
{
uint8_t value[2];
put_le16(GATT_CLIENT_CHARAC_CFG_NOTIF_BIT, value);
write_char(bas, attrib, handle, value, sizeof(value), ccc_written_cb,
user_data);
}
/* GIF header */
static int gif_image_write_header(ByteIOContext *pb,
int width, int height, uint32_t *palette)
{
int i;
unsigned int v;
put_tag(pb, "GIF");
put_tag(pb, "89a");
put_le16(pb, width);
put_le16(pb, height);
put_byte(pb, 0xf7); /* flags: global clut, 256 entries */
put_byte(pb, 0x1f); /* background color index */
put_byte(pb, 0); /* aspect ratio */
/* the global palette */
if (!palette) {
put_buffer(pb, (unsigned char *)gif_clut, 216*3);
for(i=0;i<((256-216)*3);i++)
put_byte(pb, 0);
} else {
for(i=0;i<256;i++) {
v = palette[i];
put_byte(pb, (v >> 16) & 0xff);
put_byte(pb, (v >> 8) & 0xff);
put_byte(pb, (v) & 0xff);
}
}
/* application extension header */
/* XXX: not really sure what to put in here... */
#ifdef GIF_ADD_APP_HEADER
put_byte(pb, 0x21);
put_byte(pb, 0xff);
put_byte(pb, 0x0b);
put_tag(pb, "NETSCAPE2.0");
put_byte(pb, 0x03);
put_byte(pb, 0x01);
put_byte(pb, 0x00);
put_byte(pb, 0x00);
#endif
return 0;
}
/**
* Read local version
*
* @param rspbuf
* @param rsplen
*
* @return int
*/
static int
ble_ll_hci_rd_local_version(uint8_t *rspbuf, uint8_t *rsplen)
{
uint16_t hci_rev;
uint16_t lmp_subver;
uint16_t mfrg;
hci_rev = 0;
lmp_subver = 0;
mfrg = MYNEWT_VAL(BLE_LL_MFRG_ID);
/* Place the data packet length and number of packets in the buffer */
rspbuf[0] = BLE_HCI_VER_BCS_4_2;
put_le16(rspbuf + 1, hci_rev);
rspbuf[3] = BLE_LMP_VER_BCS_4_2;
put_le16(rspbuf + 4, mfrg);
put_le16(rspbuf + 6, lmp_subver);
*rsplen = BLE_HCI_RD_LOC_VER_INFO_RSPLEN;
return BLE_ERR_SUCCESS;
}
static void put_str16_nolen(ByteIOContext *s, const char *tag)
{
int c;
for(;;) {
c = (uint8_t)*tag++;
put_le16(s, c);
if (c == '\0')
break;
}
}
static void put_swf_end_tag(AVFormatContext *s)
{
SWFContext *swf = s->priv_data;
ByteIOContext *pb = s->pb;
int64_t pos;
int tag_len, tag;
pos = url_ftell(pb);
tag_len = pos - swf->tag_pos - 2;
tag = swf->tag;
url_fseek(pb, swf->tag_pos, SEEK_SET);
if (tag & TAG_LONG) {
tag &= ~TAG_LONG;
put_le16(pb, (tag << 6) | 0x3f);
put_le32(pb, tag_len - 4);
} else {
assert(tag_len < 0x3f);
put_le16(pb, (tag << 6) | tag_len);
}
url_fseek(pb, pos, SEEK_SET);
}
static bool encode_find_info_rsp(struct gatt_db *db, struct queue *q,
uint16_t mtu,
uint8_t *pdu, uint16_t *len)
{
uint16_t handle;
struct gatt_db_attribute *attr;
const bt_uuid_t *type;
int uuid_len, cur_uuid_len;
int iter = 0;
*len = 0;
while (queue_peek_head(q)) {
attr = queue_pop_head(q);
handle = gatt_db_attribute_get_handle(attr);
type = gatt_db_attribute_get_type(attr);
if (!handle || !type)
return false;
cur_uuid_len = bt_uuid_len(type);
if (iter == 0) {
switch (cur_uuid_len) {
case 2:
uuid_len = 2;
pdu[0] = 0x01;
break;
case 4:
case 16:
uuid_len = 16;
pdu[0] = 0x02;
break;
default:
return false;
}
iter++;
} else if (cur_uuid_len != uuid_len)
break;
if (iter + uuid_len + 2 > mtu - 1)
break;
put_le16(handle, pdu + iter);
bt_uuid_to_le(type, pdu + iter + 2);
iter += uuid_len + 2;
}
*len = iter;
return true;
}
int ff_put_str16_nolen(ByteIOContext *s, const char *tag)
{
const uint8_t *q = tag;
int ret = 0;
while (*q) {
uint32_t ch;
uint16_t tmp;
GET_UTF8(ch, *q++, break;)
PUT_UTF16(ch, tmp, put_le16(s, tmp);ret += 2;)
}
/* BITMAPINFOHEADER header */
void put_bmp_header(ByteIOContext *pb, AVCodecContext *enc, const CodecTag *tags, int for_asf)
{
put_le32(pb, 40 + enc->extradata_size); /* size */
put_le32(pb, enc->width);
put_le32(pb, enc->height);
put_le16(pb, 1); /* planes */
put_le16(pb, enc->bits_per_sample ? enc->bits_per_sample : 24); /* depth */
/* compression type */
put_le32(pb, for_asf ? codec_get_asf_tag(tags, enc->codec_id) : enc->codec_tag);
put_le32(pb, enc->width * enc->height * 3);
put_le32(pb, 0);
put_le32(pb, 0);
put_le32(pb, 0);
put_le32(pb, 0);
put_buffer(pb, enc->extradata, enc->extradata_size);
if (enc->extradata_size & 1)
put_byte(pb, 0);
}
uint16_t enc_read_req(uint16_t handle, uint8_t *pdu, size_t len)
{
if (pdu == NULL)
return 0;
/* Attribute Opcode (1 octet) */
pdu[0] = ATT_OP_READ_REQ;
/* Attribute Handle (2 octets) */
put_le16(handle, &pdu[1]);
return 3;
}