iarv/solarlife
1
0
Fork 0
mirror of https://github.com/majonezz/solarlife.git synced 2026-03-28 18:42:34 +01:00
Code Issues Packages Projects Releases Wiki Activity

Initial entry

Browse Source
This commit is contained in:
majonezz
2023-10-20 21:31:26 +02:00
parent ffb0757fbb
commit c75ea8ea48
40 changed files with 23910 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
README.md
View File

@@ -1,2 +1,56 @@
# solarlife
Get data from cheap solar charge controller and pass it to MQTT broker
#Solarlife
This is a simple approach to read, parse and pass data from cheap solar charge controllers like Lumiax and Volt to MQTT broker.
You can pass data to home automation software like Home Assistant, Openhab and others.
Parameters that you will be able to get are for example:
-battery voltage
-battery current
-battery power
-battery percentage capability
-solar voltage
-solar current
-solar power
-load voltage
-load current
-load power
-and many more...
Based on:
https://github.com/majki09/lumiax_solar_bt/tree/main
https://github.com/gbrault/gattclient/tree/master
https://ctlsys.com/support/how_to_compute_the_modbus_rtu_message_crc/
https://github.com/kokke/tiny-MQTT-c
https://github.com/rpz80/json
Lumiax Modbus Communication Protocol V3.9-1.pdf
Depends on:
libglib-2.0
Tested on VM running Ubuntu 21.04 and Fritz-Box 6690 Cable router.
To use this app you will need a Bluetooth adapter with low energy capability and compatible solar charger with Bluetooth.
Generally speaking all of charging controllers that uses "Solar Life" app for Android should work.
TODO:
-Fix bugs
-Pass commands to controller (load on/off, clock setup, etc.)
Example output:
root@ubuntu:/usr/src/solarlife# ./solarlife -d 04:7f:0e:51:21:4c -a test.mqtt.com
Connecting to BT device... Done
MQTT: connected to server.
Service Added - UUID: 00001801-0000-1000-8000-00805f9b34fb start: 0x0001 end: 0x0003
Service Added - UUID: 00001800-0000-1000-8000-00805f9b34fb start: 0x0004 end: 0x000e
Service Added - UUID: 0000ff00-0000-1000-8000-00805f9b34fb start: 0x000f end: 0x001a
GATT discovery procedures complete
Registering notify handler with id: 2
Registered notify handler!
{"PV_rated_voltage":50,"PV_rated_current":10,"PV_rated_power_l":130,"PV_rated_power_h":0,"battery_rated_voltage":17,"battery_rated_current":10,"battery_rated_power_l":130,"battery_rated_power_h":0,"load_rated_voltage":17,"load_rated_current":10,"load_rated_power_l":130,"load_rated_power_h":0,"slave_id":1,"running_days":6,"sys_voltage":12,"battery_status":3,"charge_status":32,"discharge_status":0,"env_temperature":13,"sys_temperature":6,"undervoltage_times":255,"fullycharged_times":1,"overvoltage_prot_times":0,"overcurrent_prot_times":0,"shortcircuit_prot_times":0,"opencircuit_prot_times":0,"hw_prot_times":0,"charge_overtemp_prot_times":0,"discharge_overtemp_prot_times":0,"battery_remaining_capacity":16,"battery_voltage":11.13,"battery_current":0,"battery_power_lo":0,"battery_power_hi":0,"load_voltage":0,"load_current":0,"load_power_l":0,"load_power_h":0,"solar_voltage":0.2,"solar_current":0,"solar_power_l":0,"solar_power_h":0,"daily_production":0.01,"total_production_l":0.82,"total_production_h":0,"daily_consumption":0.02,"total_consumption_l":0.83,"total_consumption_h":0,"lighttime_daily":695,"monthly_production_l":0,"monthly_production_h":0,"yearly_production_l":0,"yearly_production_h":0,"timestamp":"2023-10-20T19:15:58Z"}
MQTT server acknowledged data

147
att-types.h Normal file
View File

@@ -0,0 +1,147 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2014 Google Inc.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#include <stdint.h>
#ifndef __packed
#define __packed __attribute__((packed))
#endif
#define BT_ATT_SECURITY_AUTO 0
#define BT_ATT_SECURITY_LOW 1
#define BT_ATT_SECURITY_MEDIUM 2
#define BT_ATT_SECURITY_HIGH 3
#define BT_ATT_DEFAULT_LE_MTU 23
#define BT_ATT_MAX_LE_MTU 517
#define BT_ATT_MAX_VALUE_LEN 512
/* ATT protocol opcodes */
#define BT_ATT_OP_ERROR_RSP 0x01
#define BT_ATT_OP_MTU_REQ 0x02
#define BT_ATT_OP_MTU_RSP 0x03
#define BT_ATT_OP_FIND_INFO_REQ 0x04
#define BT_ATT_OP_FIND_INFO_RSP 0x05
#define BT_ATT_OP_FIND_BY_TYPE_VAL_REQ 0x06
#define BT_ATT_OP_FIND_BY_TYPE_VAL_RSP 0x07
#define BT_ATT_OP_READ_BY_TYPE_REQ 0x08
#define BT_ATT_OP_READ_BY_TYPE_RSP 0x09
#define BT_ATT_OP_READ_REQ 0x0a
#define BT_ATT_OP_READ_RSP 0x0b
#define BT_ATT_OP_READ_BLOB_REQ 0x0c
#define BT_ATT_OP_READ_BLOB_RSP 0x0d
#define BT_ATT_OP_READ_MULT_REQ 0x0e
#define BT_ATT_OP_READ_MULT_RSP 0x0f
#define BT_ATT_OP_READ_BY_GRP_TYPE_REQ 0x10
#define BT_ATT_OP_READ_BY_GRP_TYPE_RSP 0x11
#define BT_ATT_OP_WRITE_REQ 0x12
#define BT_ATT_OP_WRITE_RSP 0x13
#define BT_ATT_OP_WRITE_CMD 0x52
#define BT_ATT_OP_SIGNED_WRITE_CMD 0xD2
#define BT_ATT_OP_PREP_WRITE_REQ 0x16
#define BT_ATT_OP_PREP_WRITE_RSP 0x17
#define BT_ATT_OP_EXEC_WRITE_REQ 0x18
#define BT_ATT_OP_EXEC_WRITE_RSP 0x19
#define BT_ATT_OP_HANDLE_VAL_NOT 0x1B
#define BT_ATT_OP_HANDLE_VAL_IND 0x1D
#define BT_ATT_OP_HANDLE_VAL_CONF 0x1E
/* Packed struct definitions for ATT protocol PDUs */
/* TODO: Complete these definitions for all opcodes */
struct bt_att_pdu_error_rsp {
uint8_t opcode;
uint16_t handle;
uint8_t ecode;
} __packed;
/* Special opcode to receive all requests (legacy servers) */
#define BT_ATT_ALL_REQUESTS 0x00
/* Error codes for Error response PDU */
#define BT_ATT_ERROR_INVALID_HANDLE 0x01
#define BT_ATT_ERROR_READ_NOT_PERMITTED 0x02
#define BT_ATT_ERROR_WRITE_NOT_PERMITTED 0x03
#define BT_ATT_ERROR_INVALID_PDU 0x04
#define BT_ATT_ERROR_AUTHENTICATION 0x05
#define BT_ATT_ERROR_REQUEST_NOT_SUPPORTED 0x06
#define BT_ATT_ERROR_INVALID_OFFSET 0x07
#define BT_ATT_ERROR_AUTHORIZATION 0x08
#define BT_ATT_ERROR_PREPARE_QUEUE_FULL 0x09
#define BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND 0x0A
#define BT_ATT_ERROR_ATTRIBUTE_NOT_LONG 0x0B
#define BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION_KEY_SIZE 0x0C
#define BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN 0x0D
#define BT_ATT_ERROR_UNLIKELY 0x0E
#define BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION 0x0F
#define BT_ATT_ERROR_UNSUPPORTED_GROUP_TYPE 0x10
#define BT_ATT_ERROR_INSUFFICIENT_RESOURCES 0x11
/*
* Common Profile and Service Error Code descriptions (see Supplement to the
* Bluetooth Core Specification, sections 1.2 and 2). The error codes within
* 0xE0-0xFC are reserved for future use. The remaining 3 are defined as the
* following:
*/
#define BT_ERROR_CCC_IMPROPERLY_CONFIGURED 0xfd
#define BT_ERROR_ALREADY_IN_PROGRESS 0xfe
#define BT_ERROR_OUT_OF_RANGE 0xff
/*
* ATT attribute permission bitfield values. Permissions are grouped as
* "Access", "Encryption", "Authentication", and "Authorization". A bitmask of
* permissions is a byte that encodes a combination of these.
*/
#define BT_ATT_PERM_READ 0x01
#define BT_ATT_PERM_WRITE 0x02
#define BT_ATT_PERM_READ_ENCRYPT 0x04
#define BT_ATT_PERM_WRITE_ENCRYPT 0x08
#define BT_ATT_PERM_ENCRYPT (BT_ATT_PERM_READ_ENCRYPT | \
BT_ATT_PERM_WRITE_ENCRYPT)
#define BT_ATT_PERM_READ_AUTHEN 0x10
#define BT_ATT_PERM_WRITE_AUTHEN 0x20
#define BT_ATT_PERM_AUTHEN (BT_ATT_PERM_READ_AUTHEN | \
BT_ATT_PERM_WRITE_AUTHEN)
#define BT_ATT_PERM_AUTHOR 0x40
#define BT_ATT_PERM_NONE 0x80
/* GATT Characteristic Properties Bitfield values */
#define BT_GATT_CHRC_PROP_BROADCAST 0x01
#define BT_GATT_CHRC_PROP_READ 0x02
#define BT_GATT_CHRC_PROP_WRITE_WITHOUT_RESP 0x04
#define BT_GATT_CHRC_PROP_WRITE 0x08
#define BT_GATT_CHRC_PROP_NOTIFY 0x10
#define BT_GATT_CHRC_PROP_INDICATE 0x20
#define BT_GATT_CHRC_PROP_AUTH 0x40
#define BT_GATT_CHRC_PROP_EXT_PROP 0x80
/* GATT Characteristic Extended Properties Bitfield values */
#define BT_GATT_CHRC_EXT_PROP_RELIABLE_WRITE 0x01
#define BT_GATT_CHRC_EXT_PROP_WRITABLE_AUX 0x02
#define BT_GATT_CHRC_EXT_PROP_ENC_READ 0x04
#define BT_GATT_CHRC_EXT_PROP_ENC_WRITE 0x08
#define BT_GATT_CHRC_EXT_PROP_ENC (BT_GATT_CHRC_EXT_PROP_ENC_READ | \
BT_GATT_CHRC_EXT_PROP_ENC_WRITE)
#define BT_GATT_CHRC_EXT_PROP_AUTH_READ 0x10
#define BT_GATT_CHRC_EXT_PROP_AUTH_WRITE 0x20
#define BT_GATT_CHRC_EXT_PROP_AUTH (BT_GATT_CHRC_EXT_PROP_AUTH_READ | \
BT_GATT_CHRC_EXT_PROP_AUTH_WRITE)

1532
att.c Normal file
View File

File diff suppressed because it is too large Load Diff

93
att.h Normal file
View File

@@ -0,0 +1,93 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2014 Google Inc.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#include <stdbool.h>
#include <stdint.h>
#include "att-types.h"
struct bt_att;
struct bt_att *bt_att_new(int fd, bool ext_signed);
struct bt_att *bt_att_ref(struct bt_att *att);
void bt_att_unref(struct bt_att *att);
bool bt_att_set_close_on_unref(struct bt_att *att, bool do_close);
int bt_att_get_fd(struct bt_att *att);
typedef void (*bt_att_response_func_t)(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data);
typedef void (*bt_att_notify_func_t)(uint8_t opcode, const void *pdu,
uint16_t length, void *user_data);
typedef void (*bt_att_destroy_func_t)(void *user_data);
typedef void (*bt_att_debug_func_t)(const char *str, void *user_data);
typedef void (*bt_att_timeout_func_t)(unsigned int id, uint8_t opcode,
void *user_data);
typedef void (*bt_att_disconnect_func_t)(int err, void *user_data);
typedef bool (*bt_att_counter_func_t)(uint32_t *sign_cnt, void *user_data);
bool bt_att_set_debug(struct bt_att *att, bt_att_debug_func_t callback,
void *user_data, bt_att_destroy_func_t destroy);
uint16_t bt_att_get_mtu(struct bt_att *att);
bool bt_att_set_mtu(struct bt_att *att, uint16_t mtu);
bool bt_att_set_timeout_cb(struct bt_att *att, bt_att_timeout_func_t callback,
void *user_data,
bt_att_destroy_func_t destroy);
unsigned int bt_att_send(struct bt_att *att, uint8_t opcode,
const void *pdu, uint16_t length,
bt_att_response_func_t callback,
void *user_data,
bt_att_destroy_func_t destroy);
bool bt_att_cancel(struct bt_att *att, unsigned int id);
bool bt_att_cancel_all(struct bt_att *att);
unsigned int bt_att_send_error_rsp(struct bt_att *att, uint8_t opcode,
uint16_t handle, int error);
unsigned int bt_att_register(struct bt_att *att, uint8_t opcode,
bt_att_notify_func_t callback,
void *user_data,
bt_att_destroy_func_t destroy);
bool bt_att_unregister(struct bt_att *att, unsigned int id);
unsigned int bt_att_register_disconnect(struct bt_att *att,
bt_att_disconnect_func_t callback,
void *user_data,
bt_att_destroy_func_t destroy);
bool bt_att_unregister_disconnect(struct bt_att *att, unsigned int id);
bool bt_att_unregister_all(struct bt_att *att);
int bt_att_get_security(struct bt_att *att);
bool bt_att_set_security(struct bt_att *att, int level);
bool bt_att_set_local_key(struct bt_att *att, uint8_t sign_key[16],
bt_att_counter_func_t func, void *user_data);
bool bt_att_set_remote_key(struct bt_att *att, uint8_t sign_key[16],
bt_att_counter_func_t func, void *user_data);
bool bt_att_has_crypto(struct bt_att *att);

1456
bluetooth.c Normal file
View File

File diff suppressed because it is too large Load Diff

403
bluetooth.h Normal file
View File

@@ -0,0 +1,403 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2000-2001 Qualcomm Incorporated
* Copyright (C) 2002-2003 Maxim Krasnyansky <maxk@qualcomm.com>
* Copyright (C) 2002-2010 Marcel Holtmann <marcel@holtmann.org>
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifndef __BLUETOOTH_H
#define __BLUETOOTH_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <endian.h>
#include <byteswap.h>
#include <netinet/in.h>
#ifndef AF_BLUETOOTH
#define AF_BLUETOOTH 31
#define PF_BLUETOOTH AF_BLUETOOTH
#endif
#define BTPROTO_L2CAP 0
#define BTPROTO_HCI 1
#define BTPROTO_SCO 2
#define BTPROTO_RFCOMM 3
#define BTPROTO_BNEP 4
#define BTPROTO_CMTP 5
#define BTPROTO_HIDP 6
#define BTPROTO_AVDTP 7
#define SOL_HCI 0
#define SOL_L2CAP 6
#define SOL_SCO 17
#define SOL_RFCOMM 18
#ifndef SOL_BLUETOOTH
#define SOL_BLUETOOTH 274
#endif
#define BT_SECURITY 4
struct bt_security {
uint8_t level;
uint8_t key_size;
};
#define BT_SECURITY_SDP 0
#define BT_SECURITY_LOW 1
#define BT_SECURITY_MEDIUM 2
#define BT_SECURITY_HIGH 3
#define BT_DEFER_SETUP 7
#define BT_FLUSHABLE 8
#define BT_FLUSHABLE_OFF 0
#define BT_FLUSHABLE_ON 1
#define BT_POWER 9
struct bt_power {
uint8_t force_active;
};
#define BT_POWER_FORCE_ACTIVE_OFF 0
#define BT_POWER_FORCE_ACTIVE_ON 1
#define BT_CHANNEL_POLICY 10
/* BR/EDR only (default policy)
* AMP controllers cannot be used.
* Channel move requests from the remote device are denied.
* If the L2CAP channel is currently using AMP, move the channel to BR/EDR.
*/
#define BT_CHANNEL_POLICY_BREDR_ONLY 0
/* BR/EDR Preferred
* Allow use of AMP controllers.
* If the L2CAP channel is currently on AMP, move it to BR/EDR.
* Channel move requests from the remote device are allowed.
*/
#define BT_CHANNEL_POLICY_BREDR_PREFERRED 1
/* AMP Preferred
* Allow use of AMP controllers
* If the L2CAP channel is currently on BR/EDR and AMP controller
* resources are available, initiate a channel move to AMP.
* Channel move requests from the remote device are allowed.
* If the L2CAP socket has not been connected yet, try to create
* and configure the channel directly on an AMP controller rather
* than BR/EDR.
*/
#define BT_CHANNEL_POLICY_AMP_PREFERRED 2
#define BT_VOICE 11
struct bt_voice {
uint16_t setting;
};
#define BT_SNDMTU 12
#define BT_RCVMTU 13
#define BT_VOICE_TRANSPARENT 0x0003
#define BT_VOICE_CVSD_16BIT 0x0060
/* Connection and socket states */
enum {
BT_CONNECTED = 1, /* Equal to TCP_ESTABLISHED to make net code happy */
BT_OPEN,
BT_BOUND,
BT_LISTEN,
BT_CONNECT,
BT_CONNECT2,
BT_CONFIG,
BT_DISCONN,
BT_CLOSED
};
/* Byte order conversions */
#if __BYTE_ORDER == __LITTLE_ENDIAN
#define htobs(d) (d)
#define htobl(d) (d)
#define htobll(d) (d)
#define btohs(d) (d)
#define btohl(d) (d)
#define btohll(d) (d)
#elif __BYTE_ORDER == __BIG_ENDIAN
#define htobs(d) bswap_16(d)
#define htobl(d) bswap_32(d)
#define htobll(d) bswap_64(d)
#define btohs(d) bswap_16(d)
#define btohl(d) bswap_32(d)
#define btohll(d) bswap_64(d)
#else
#error "Unknown byte order"
#endif
/* Bluetooth unaligned access */
#define bt_get_unaligned(ptr) \
__extension__ ({ \
struct __attribute__((packed)) { \
__typeof__(*(ptr)) __v; \
} *__p = (__typeof__(__p)) (ptr); \
__p->__v; \
})
#define bt_put_unaligned(val, ptr) \
do { \
struct __attribute__((packed)) { \
__typeof__(*(ptr)) __v; \
} *__p = (__typeof__(__p)) (ptr); \
__p->__v = (val); \
} while(0)
#if __BYTE_ORDER == __LITTLE_ENDIAN
static inline uint64_t bt_get_le64(const void *ptr)
{
return bt_get_unaligned((const uint64_t *) ptr);
}
static inline uint64_t bt_get_be64(const void *ptr)
{
return bswap_64(bt_get_unaligned((const uint64_t *) ptr));
}
static inline uint32_t bt_get_le32(const void *ptr)
{
return bt_get_unaligned((const uint32_t *) ptr);
}
static inline uint32_t bt_get_be32(const void *ptr)
{
return bswap_32(bt_get_unaligned((const uint32_t *) ptr));
}
static inline uint16_t bt_get_le16(const void *ptr)
{
return bt_get_unaligned((const uint16_t *) ptr);
}
static inline uint16_t bt_get_be16(const void *ptr)
{
return bswap_16(bt_get_unaligned((const uint16_t *) ptr));
}
static inline void bt_put_le64(uint64_t val, const void *ptr)
{
bt_put_unaligned(val, (uint64_t *) ptr);
}
static inline void bt_put_be64(uint64_t val, const void *ptr)
{
bt_put_unaligned(bswap_64(val), (uint64_t *) ptr);
}
static inline void bt_put_le32(uint32_t val, const void *ptr)
{
bt_put_unaligned(val, (uint32_t *) ptr);
}
static inline void bt_put_be32(uint32_t val, const void *ptr)
{
bt_put_unaligned(bswap_32(val), (uint32_t *) ptr);
}
static inline void bt_put_le16(uint16_t val, const void *ptr)
{
bt_put_unaligned(val, (uint16_t *) ptr);
}
static inline void bt_put_be16(uint16_t val, const void *ptr)
{
bt_put_unaligned(bswap_16(val), (uint16_t *) ptr);
}
#elif __BYTE_ORDER == __BIG_ENDIAN
static inline uint64_t bt_get_le64(const void *ptr)
{
return bswap_64(bt_get_unaligned((const uint64_t *) ptr));
}
static inline uint64_t bt_get_be64(const void *ptr)
{
return bt_get_unaligned((const uint64_t *) ptr);
}
static inline uint32_t bt_get_le32(const void *ptr)
{
return bswap_32(bt_get_unaligned((const uint32_t *) ptr));
}
static inline uint32_t bt_get_be32(const void *ptr)
{
return bt_get_unaligned((const uint32_t *) ptr);
}
static inline uint16_t bt_get_le16(const void *ptr)
{
return bswap_16(bt_get_unaligned((const uint16_t *) ptr));
}
static inline uint16_t bt_get_be16(const void *ptr)
{
return bt_get_unaligned((const uint16_t *) ptr);
}
static inline void bt_put_le64(uint64_t val, const void *ptr)
{
bt_put_unaligned(bswap_64(val), (uint64_t *) ptr);
}
static inline void bt_put_be64(uint64_t val, const void *ptr)
{
bt_put_unaligned(val, (uint64_t *) ptr);
}
static inline void bt_put_le32(uint32_t val, const void *ptr)
{
bt_put_unaligned(bswap_32(val), (uint32_t *) ptr);
}
static inline void bt_put_be32(uint32_t val, const void *ptr)
{
bt_put_unaligned(val, (uint32_t *) ptr);
}
static inline void bt_put_le16(uint16_t val, const void *ptr)
{
bt_put_unaligned(bswap_16(val), (uint16_t *) ptr);
}
static inline void bt_put_be16(uint16_t val, const void *ptr)
{
bt_put_unaligned(val, (uint16_t *) ptr);
}
#else
#error "Unknown byte order"
#endif
/* BD Address */
typedef struct {
uint8_t b[6];
} __attribute__((packed)) bdaddr_t;
/* BD Address type */
#define BDADDR_BREDR 0x00
#define BDADDR_LE_PUBLIC 0x01
#define BDADDR_LE_RANDOM 0x02
#define BDADDR_ANY (&(bdaddr_t) {{0, 0, 0, 0, 0, 0}})
#define BDADDR_ALL (&(bdaddr_t) {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff}})
#define BDADDR_LOCAL (&(bdaddr_t) {{0, 0, 0, 0xff, 0xff, 0xff}})
/* Copy, swap, convert BD Address */
static inline int bacmp(const bdaddr_t *ba1, const bdaddr_t *ba2)
{
return memcmp(ba1, ba2, sizeof(bdaddr_t));
}
static inline void bacpy(bdaddr_t *dst, const bdaddr_t *src)
{
memcpy(dst, src, sizeof(bdaddr_t));
}
void baswap(bdaddr_t *dst, const bdaddr_t *src);
bdaddr_t *strtoba(const char *str);
char *batostr(const bdaddr_t *ba);
int ba2str(const bdaddr_t *ba, char *str);
int str2ba(const char *str, bdaddr_t *ba);
int ba2oui(const bdaddr_t *ba, char *oui);
int bachk(const char *str);
int baprintf(const char *format, ...);
int bafprintf(FILE *stream, const char *format, ...);
int basprintf(char *str, const char *format, ...);
int basnprintf(char *str, size_t size, const char *format, ...);
void *bt_malloc(size_t size);
void bt_free(void *ptr);
int bt_error(uint16_t code);
const char *bt_compidtostr(int id);
typedef struct {
uint8_t data[16];
} uint128_t;
static inline void bswap_128(const void *src, void *dst)
{
const uint8_t *s = (const uint8_t *) src;
uint8_t *d = (uint8_t *) dst;
int i;
for (i = 0; i < 16; i++)
d[15 - i] = s[i];
}
#if __BYTE_ORDER == __BIG_ENDIAN
#define ntoh64(x) (x)
static inline void ntoh128(const uint128_t *src, uint128_t *dst)
{
memcpy(dst, src, sizeof(uint128_t));
}
static inline void btoh128(const uint128_t *src, uint128_t *dst)
{
bswap_128(src, dst);
}
#else
static inline uint64_t ntoh64(uint64_t n)
{
uint64_t h;
uint64_t tmp = ntohl(n & 0x00000000ffffffff);
h = ntohl(n >> 32);
h |= tmp << 32;
return h;
}
static inline void ntoh128(const uint128_t *src, uint128_t *dst)
{
bswap_128(src, dst);
}
static inline void btoh128(const uint128_t *src, uint128_t *dst)
{
memcpy(dst, src, sizeof(uint128_t));
}
#endif
#define hton64(x) ntoh64(x)
#define hton128(x, y) ntoh128(x, y)
#define htob128(x, y) btoh128(x, y)
#ifdef __cplusplus
}
#endif
#endif /* __BLUETOOTH_H */

249
client.c Normal file
View File

@@ -0,0 +1,249 @@
#include "client.h"
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <netinet/in.h>
#include <netdb.h>
/* Helper functions: */
static void _dummy_connect (client_t* s, int f, const char* b) { (void) s; (void) f; (void) b; }
static void _dummy_recv_data(client_t* s, int f, char* d, int l) { (void) s; (void) f; (void) d; (void) l; }
static void _change_state(client_t* psClnt, conn_state_t new_state)
{
require(psClnt != 0);
psClnt->state = new_state;
psClnt->last_active = time(0);
}
/*
Implementation of exported interface begins here
*/
void client_init(client_t* psClnt, char* dst_addr, uint16_t dst_port, char* rxbuf, uint32_t rxbufsize)
{
require(psClnt != 0);
require(dst_addr != 0);
require(rxbuf != 0);
strcpy(psClnt->addr, dst_addr);
psClnt->port = dst_port;
psClnt->sockfd = 0;
psClnt->rxbuf = rxbuf;
psClnt->rxbufsz = rxbufsize;
psClnt->client_connected = (void*)_dummy_connect;
psClnt->client_disconnected = (void*)_dummy_connect;
psClnt->client_new_data = (void*)_dummy_recv_data;
/*
Set the socket I/O mode: In this case FIONBIO
enables or disables the blocking mode for the
socket based on the numerical value of iMode.
If iMode = 0, blocking is enabled;
If iMode != 0, non-blocking mode is enabled.
*/
int iMode = 0;
ioctl(psClnt->sockfd, FIONBIO, &iMode);
/*
// WINDOWS
DWORD timeout = SOCKET_READ_TIMEOUT_SEC * 1000;
setsockopt(socket, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout, sizeof(timeout));
*/
_change_state(psClnt, CREATED);
/* Ignore SIGPIPE - "broken pipe" / disconnected socket */
signal(SIGPIPE, SIG_IGN);
}
int client_set_callback(client_t* psClnt, cb_type eTyp, void* funcptr)
{
require(psClnt != 0);
require(funcptr != 0);
int success = 1;
switch(eTyp)
{
case CB_ON_CONNECTION: psClnt->client_connected = funcptr; break;
case CB_ON_DISCONNECT: psClnt->client_disconnected = funcptr; break;
case CB_RECEIVED_DATA: psClnt->client_new_data = funcptr; break;
default: success = 0; /* unknown callback-type */ break;
}
return success;
}
int client_send(client_t* psClnt, char* data, uint32_t nbytes)
{
require(psClnt != 0);
require(data != 0);
psClnt->last_active = time(0);
// printf("MQTT: sending %u bytes.\n", nbytes);
int success = send(psClnt->sockfd, data, nbytes, 0);
if (success < 0)
{
perror("send");
client_disconnect(psClnt);
}
return success;
}
int client_recv(client_t* psClnt, uint32_t timeout_us)
{
require(psClnt != 0);
struct timeval tv;
tv.tv_sec = 0;
while (timeout_us >= 1000000)
{
timeout_us -= 1000000;
tv.tv_sec += 1;
}
tv.tv_usec = timeout_us; /* Not init'ing this can cause strange errors */
setsockopt(psClnt->sockfd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv,sizeof(struct timeval));
int nbytes = recv(psClnt->sockfd, psClnt->rxbuf, psClnt->rxbufsz, 0);
if (nbytes <= 0)
{
/* got error or connection closed by server? */
if ( (nbytes == -1) /* nothing for us */
|| (nbytes == EAGAIN) /* try again later */
|| (nbytes == EWOULDBLOCK)) /* same as above */
{
/* do nothing */
}
else
{
/* connection lost / reset by peer */
perror("recv");
client_disconnect(psClnt);
}
}
else
{
psClnt->client_new_data(psClnt, psClnt->rxbuf, nbytes);
psClnt->last_active = time(0);
}
return nbytes;
}
int client_state(client_t* psClnt)
{
return ((psClnt != 0) ? psClnt->state : 0);
}
void client_poll(client_t* psClnt, uint32_t timeout_us)
{
/* 10usec timeout is a reasonable minimum I think */
timeout_us = ((timeout_us >= 10) ? timeout_us : 10);
require(psClnt != 0);
switch (psClnt->state)
{
case CREATED:
{
_change_state(psClnt, DISCONNECTED);
} break;
case CONNECTED:
{
client_recv(psClnt, timeout_us);
/*
static time_t timeLastMsg = 0;
if ((time(0) - timeLastMsg) > 0)
{
timeLastMsg = time(0);
client_send(psClnt, "hej mor!", 8);
}
*/
} break;
case DISCONNECTED:
{
/* ~1 second cooldown */
if (time(0) > psClnt->last_active)
{
client_connect(psClnt);
}
} break;
}
}
void client_disconnect(client_t* psClnt)
{
require(psClnt != 0);
shutdown(psClnt->sockfd, 2);
close(psClnt->sockfd);
_change_state(psClnt, DISCONNECTED);
psClnt->client_disconnected(psClnt);
}
int client_connect(client_t* psClnt)
{
require(psClnt != 0);
int success = 0;
struct sockaddr_in serv_addr;
struct hostent* server;
psClnt->sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (psClnt->sockfd < 0)
{
perror("socket");
}
else
{
server = gethostbyname(psClnt->addr);
if (server == NULL)
{
fprintf(stderr,"ERROR, no such host\n");
}
else
{
memset(&serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
memcpy(&serv_addr.sin_addr.s_addr, server->h_addr, server->h_length);
serv_addr.sin_port = htons(psClnt->port);
if (connect(psClnt->sockfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0)
{
perror("connect");
/* Clean up by calling close() on socket before allocating a new socket. */
client_disconnect(psClnt);
}
else
{
_change_state(psClnt, CONNECTED);
psClnt->client_connected(psClnt);
success = 1;
}
}
}
return success;
}

54
client.h Normal file
View File

@@ -0,0 +1,54 @@
#include <stdint.h>
#include <time.h>
#define NCONNECTIONS 1
#define BUFFER_SIZE_BYTES 8000
/* Assertion macro */
#define require(predicate) assert((predicate))
/* Socket connection state */
typedef enum
{
CREATED,
CONNECTED,
DISCONNECTED,
} conn_state_t;
/* Type definitions */
typedef enum
{
CB_ON_CONNECTION,
CB_ON_DISCONNECT,
CB_RECEIVED_DATA,
} cb_type;
typedef struct
{
int sockfd;
char* rxbuf;
uint32_t rxbufsz;
conn_state_t state;
uint16_t port;
char addr[32];
time_t last_active;
/* Callbacks */
void (*client_connected) (void* psClnt); /* new connection established */
void (*client_disconnected)(void* psClnt); /* a connection was closed */
void (*client_new_data) (void* psClnt, char* data, int nbytes); /* data has been received */
} client_t;
void client_init(client_t* psClnt, char* dst_addr, uint16_t dst_port, char* rxbuf, uint32_t rxbufsize);
int client_set_callback(client_t* psClnt, cb_type eTyp, void* funcptr);
int client_send(client_t* psClnt, char* data, uint32_t nbytes);
int client_recv(client_t* psClnt, uint32_t timeout_us);
void client_poll(client_t* psClnt, uint32_t timeout_us);
void client_disconnect(client_t* psClnt);
int client_connect(client_t* psClnt);
int client_state(client_t* psClnt);

133
config.h Normal file
View File

@@ -0,0 +1,133 @@
/* config.h. Generated from config.h.in by configure. */
/* config.h.in. Generated from configure.ac by autoheader. */
/* Directory for the Android daemon storage files */
#define ANDROID_STORAGEDIR "/var/lib/bluetooth/android"
/* Directory for the configuration files */
#define CONFIGDIR "/usr/local/etc/bluetooth"
/* Define to 1 if you have the <dlfcn.h> header file. */
#define HAVE_DLFCN_H 1
/* Define to 1 if you have the <inttypes.h> header file. */
#define HAVE_INTTYPES_H 1
/* Define to 1 if you have the <linux/if_alg.h> header file. */
#define HAVE_LINUX_IF_ALG_H 1
/* Define to 1 if you have the <linux/types.h> header file. */
#define HAVE_LINUX_TYPES_H 1
/* Define to 1 if you have the <memory.h> header file. */
#define HAVE_MEMORY_H 1
/* Define to 1 if you have the <readline/readline.h> header file. */
#define HAVE_READLINE_READLINE_H 1
/* Define to 1 if you have the <stdint.h> header file. */
#define HAVE_STDINT_H 1
/* Define to 1 if you have the <stdlib.h> header file. */
#define HAVE_STDLIB_H 1
/* Define to 1 if you have the <strings.h> header file. */
#define HAVE_STRINGS_H 1
/* Define to 1 if you have the <string.h> header file. */
#define HAVE_STRING_H 1
/* Define to 1 if you have the <sys/stat.h> header file. */
#define HAVE_SYS_STAT_H 1
/* Define to 1 if you have the <sys/types.h> header file. */
#define HAVE_SYS_TYPES_H 1
/* Define to 1 if you have the udev_hwdb_new() function. */
#define HAVE_UDEV_HWDB_NEW 1
/* Define to 1 if you have the <unistd.h> header file. */
#define HAVE_UNISTD_H 1
/* Define to the sub-directory in which libtool stores uninstalled libraries.
*/
#define LT_OBJDIR ".libs/"
/* Define if threading support is required */
/* #undef NEED_THREADS */
/* Name of package */
#define PACKAGE "bluez"
/* Define to the address where bug reports for this package should be sent. */
#define PACKAGE_BUGREPORT ""
/* Define to the full name of this package. */
#define PACKAGE_NAME "bluez"
/* Define to the full name and version of this package. */
#define PACKAGE_STRING "bluez 5.31"
/* Define to the one symbol short name of this package. */
#define PACKAGE_TARNAME "bluez"
/* Define to the home page for this package. */
#define PACKAGE_URL ""
/* Define to the version of this package. */
#define PACKAGE_VERSION "5.31"
/* Define to 1 if you have the ANSI C header files. */
#define STDC_HEADERS 1
/* Directory for the storage files */
#define STORAGEDIR "/var/lib/bluetooth"
/* Enable extensions on AIX 3, Interix. */
#ifndef _ALL_SOURCE
# define _ALL_SOURCE 1
#endif
/* Enable GNU extensions on systems that have them. */
#ifndef _GNU_SOURCE
# define _GNU_SOURCE 1
#endif
/* Enable threading extensions on Solaris. */
#ifndef _POSIX_PTHREAD_SEMANTICS
# define _POSIX_PTHREAD_SEMANTICS 1
#endif
/* Enable extensions on HP NonStop. */
#ifndef _TANDEM_SOURCE
# define _TANDEM_SOURCE 1
#endif
/* Enable general extensions on Solaris. */
#ifndef __EXTENSIONS__
# define __EXTENSIONS__ 1
#endif
/* Version number of package */
#define VERSION "5.31"
/* Define to 1 if on MINIX. */
/* #undef _MINIX */
/* Define to 2 if the system does not provide POSIX.1 features except with
this defined. */
/* #undef _POSIX_1_SOURCE */
/* Define to 1 if you need to in order for `stat' and other things to work. */
/* #undef _POSIX_SOURCE */
/* Define to the equivalent of the C99 'restrict' keyword, or to
nothing if this is not supported. Do not define if restrict is
supported directly. */
#define restrict __restrict
/* Work around a bug in Sun C++: it does not support _Restrict or
__restrict__, even though the corresponding Sun C compiler ends up with
"#define restrict _Restrict" or "#define restrict __restrict__" in the
previous line. Perhaps some future version of Sun C++ will work with
restrict; if so, hopefully it defines __RESTRICT like Sun C does. */
#if defined __SUNPRO_CC && !defined __RESTRICT
# define _Restrict
# define __restrict__
#endif

701
crypto.c Normal file
View File

@@ -0,0 +1,701 @@
/**
* @file crypto.c
* @brief crypting gatt messages
* @author Gilbert Brault
* @copyright Gilbert Brault 2015
* the original work comes from bluez v5.39
* value add: documenting main features
*
*/
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <sys/socket.h>
#include "util.h"
#include "crypto.h"
#ifndef HAVE_LINUX_IF_ALG_H
#ifndef HAVE_LINUX_TYPES_H
typedef uint8_t __u8;
typedef uint16_t __u16;
typedef uint32_t __u32;
#else
#include <linux/types.h>
#endif
struct sockaddr_alg {
__u16 salg_family;
__u8 salg_type[14];
__u32 salg_feat;
__u32 salg_mask;
__u8 salg_name[64];
};
struct af_alg_iv {
__u32 ivlen;
__u8 iv[0];
};
#define ALG_SET_KEY 1
#define ALG_SET_IV 2
#define ALG_SET_OP 3
#define ALG_OP_DECRYPT 0
#define ALG_OP_ENCRYPT 1
#define PF_ALG 38 /* Algorithm sockets. */
#define AF_ALG PF_ALG
#else
#include <linux/if_alg.h>
#endif
#ifndef SOL_ALG
#define SOL_ALG 279
#endif
/* Maximum message length that can be passed to aes_cmac */
#define CMAC_MSG_MAX 80
struct bt_crypto {
int ref_count;
int ecb_aes;
int urandom;
int cmac_aes;
};
/**
* open the pseudo random os generator, returns the associated file descriptor
*
* @return pseudo-random generator associated file descriptor
*/
static int urandom_setup(void)
{
int fd;
fd = open("/dev/urandom", O_RDONLY);
if (fd < 0)
return -1;
return fd;
}
/**
*
* @return
*/
static int ecb_aes_setup(void)
{
struct sockaddr_alg salg;
int fd;
fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
if (fd < 0)
return -1;
memset(&salg, 0, sizeof(salg));
salg.salg_family = AF_ALG;
strcpy((char *) salg.salg_type, "skcipher");
strcpy((char *) salg.salg_name, "ecb(aes)");
if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
close(fd);
return -1;
}
return fd;
}
static int cmac_aes_setup(void)
{
struct sockaddr_alg salg;
int fd;
fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
if (fd < 0)
return -1;
memset(&salg, 0, sizeof(salg));
salg.salg_family = AF_ALG;
strcpy((char *) salg.salg_type, "hash");
strcpy((char *) salg.salg_name, "cmac(aes)");
if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
close(fd);
return -1;
}
return fd;
}
struct bt_crypto *bt_crypto_new(void)
{
struct bt_crypto *crypto;
crypto = new0(struct bt_crypto, 1);
if (!crypto)
return NULL;
crypto->ecb_aes = ecb_aes_setup();
if (crypto->ecb_aes < 0) {
free(crypto);
return NULL;
}
crypto->urandom = urandom_setup();
if (crypto->urandom < 0) {
close(crypto->ecb_aes);
free(crypto);
return NULL;
}
crypto->cmac_aes = cmac_aes_setup();
if (crypto->cmac_aes < 0) {
close(crypto->urandom);
close(crypto->ecb_aes);
free(crypto);
return NULL;
}
return bt_crypto_ref(crypto);
}
struct bt_crypto *bt_crypto_ref(struct bt_crypto *crypto)
{
if (!crypto)
return NULL;
__sync_fetch_and_add(&crypto->ref_count, 1);
return crypto;
}
void bt_crypto_unref(struct bt_crypto *crypto)
{
if (!crypto)
return;
if (__sync_sub_and_fetch(&crypto->ref_count, 1))
return;
close(crypto->urandom);
close(crypto->ecb_aes);
close(crypto->cmac_aes);
free(crypto);
}
bool bt_crypto_random_bytes(struct bt_crypto *crypto,
uint8_t *buf, uint8_t num_bytes)
{
ssize_t len;
if (!crypto)
return false;
len = read(crypto->urandom, buf, num_bytes);
if (len < num_bytes)
return false;
return true;
}
static int alg_new(int fd, const void *keyval, socklen_t keylen)
{
if (setsockopt(fd, SOL_ALG, ALG_SET_KEY, keyval, keylen) < 0)
return -1;
/* FIXME: This should use accept4() with SOCK_CLOEXEC */
return accept(fd, NULL, 0);
}
static bool alg_encrypt(int fd, const void *inbuf, size_t inlen,
void *outbuf, size_t outlen)
{
__u32 alg_op = ALG_OP_ENCRYPT;
char cbuf[CMSG_SPACE(sizeof(alg_op))];
struct cmsghdr *cmsg;
struct msghdr msg;
struct iovec iov;
ssize_t len;
memset(cbuf, 0, sizeof(cbuf));
memset(&msg, 0, sizeof(msg));
msg.msg_control = cbuf;
msg.msg_controllen = sizeof(cbuf);
cmsg = CMSG_FIRSTHDR(&msg);
cmsg->cmsg_level = SOL_ALG;
cmsg->cmsg_type = ALG_SET_OP;
cmsg->cmsg_len = CMSG_LEN(sizeof(alg_op));
memcpy(CMSG_DATA(cmsg), &alg_op, sizeof(alg_op));
iov.iov_base = (void *) inbuf;
iov.iov_len = inlen;
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
len = sendmsg(fd, &msg, 0);
if (len < 0)
return false;
len = read(fd, outbuf, outlen);
if (len < 0)
return false;
return true;
}
static inline void swap_buf(const uint8_t *src, uint8_t *dst, uint16_t len)
{
int i;
for (i = 0; i < len; i++)
dst[len - 1 - i] = src[i];
}
bool bt_crypto_sign_att(struct bt_crypto *crypto, const uint8_t key[16],
const uint8_t *m, uint16_t m_len,
uint32_t sign_cnt, uint8_t signature[12])
{
int fd;
int len;
uint8_t tmp[16], out[16];
uint16_t msg_len = m_len + sizeof(uint32_t);
uint8_t msg[msg_len];
uint8_t msg_s[msg_len];
if (!crypto)
return false;
memset(msg, 0, msg_len);
memcpy(msg, m, m_len);
/* Add sign_counter to the message */
put_le32(sign_cnt, msg + m_len);
/* The most significant octet of key corresponds to key[0] */
swap_buf(key, tmp, 16);
fd = alg_new(crypto->cmac_aes, tmp, 16);
if (fd < 0)
return false;
/* Swap msg before signing */
swap_buf(msg, msg_s, msg_len);
len = send(fd, msg_s, msg_len, 0);
if (len < 0) {
close(fd);
return false;
}
len = read(fd, out, 16);
if (len < 0) {
close(fd);
return false;
}
close(fd);
/*
* As to BT spec. 4.1 Vol[3], Part C, chapter 10.4.1 sign counter should
* be placed in the signature
*/
put_be32(sign_cnt, out + 8);
/*
* The most significant octet of hash corresponds to out[0] - swap it.
* Then truncate in most significant bit first order to a length of
* 12 octets
*/
swap_buf(out, tmp, 16);
memcpy(signature, tmp + 4, 12);
return true;
}
/**
* Security function e
*
* Security function e generates 128-bit encryptedData from a 128-bit key
* and 128-bit plaintextData using the AES-128-bit block cypher:
*
* encryptedData = e(key, plaintextData)
*
* The most significant octet of key corresponds to key[0], the most
* significant octet of plaintextData corresponds to in[0] and the
* most significant octet of encryptedData corresponds to out[0].
*
*/
bool bt_crypto_e(struct bt_crypto *crypto, const uint8_t key[16],
const uint8_t plaintext[16], uint8_t encrypted[16])
{
uint8_t tmp[16], in[16], out[16];
int fd;
if (!crypto)
return false;
/* The most significant octet of key corresponds to key[0] */
swap_buf(key, tmp, 16);
fd = alg_new(crypto->ecb_aes, tmp, 16);
if (fd < 0)
return false;
/* Most significant octet of plaintextData corresponds to in[0] */
swap_buf(plaintext, in, 16);
if (!alg_encrypt(fd, in, 16, out, 16)) {
close(fd);
return false;
}
/* Most significant octet of encryptedData corresponds to out[0] */
swap_buf(out, encrypted, 16);
close(fd);
return true;
}
/**
* Random Address Hash function ah
*
* The random address hash function ah is used to generate a hash value
* that is used in resolvable private addresses.
*
* The following are inputs to the random address hash function ah:
*
* k is 128 bits
* r is 24 bits
* padding is 104 bits
*
* r is concatenated with padding to generate r' which is used as the
* 128-bit input parameter plaintextData to security function e:
*
* r' = padding || r
*
* The least significant octet of r becomes the least significant octet
* of r’ and the most significant octet of padding becomes the most
* significant octet of r'.
*
* For example, if the 24-bit value r is 0x423456 then r' is
* 0x00000000000000000000000000423456.
*
* The output of the random address function ah is:
*
* ah(k, r) = e(k, r') mod 2^24
*
* The output of the security function e is then truncated to 24 bits by
* taking the least significant 24 bits of the output of e as the result
* of ah.
*/
bool bt_crypto_ah(struct bt_crypto *crypto, const uint8_t k[16],
const uint8_t r[3], uint8_t hash[3])
{
uint8_t rp[16];
uint8_t encrypted[16];
if (!crypto)
return false;
/* r' = padding || r */
memcpy(rp, r, 3);
memset(rp + 3, 0, 13);
/* e(k, r') */
if (!bt_crypto_e(crypto, k, rp, encrypted))
return false;
/* ah(k, r) = e(k, r') mod 2^24 */
memcpy(hash, encrypted, 3);
return true;
}
typedef struct {
uint64_t a, b;
} u128;
static inline void u128_xor(const uint8_t p[16], const uint8_t q[16],
uint8_t r[16])
{
u128 pp, qq, rr;
memcpy(&pp, p, 16);
memcpy(&qq, q, 16);
rr.a = pp.a ^ qq.a;
rr.b = pp.b ^ qq.b;
memcpy(r, &rr, 16);
}
/**
* Confirm value generation function c1
*
* During the pairing process confirm values are exchanged. This confirm
* value generation function c1 is used to generate the confirm values.
*
* The following are inputs to the confirm value generation function c1:
*
* k is 128 bits
* r is 128 bits
* pres is 56 bits
* preq is 56 bits
* iat is 1 bit
* ia is 48 bits
* rat is 1 bit
* ra is 48 bits
* padding is 32 bits of 0
*
* iat is concatenated with 7-bits of 0 to create iat' which is 8 bits
* in length. iat is the least significant bit of iat'
*
* rat is concatenated with 7-bits of 0 to create rat' which is 8 bits
* in length. rat is the least significant bit of rat'
*
* pres, preq, rat' and iat' are concatenated to generate p1 which is
* XORed with r and used as 128-bit input parameter plaintextData to
* security function e:
*
* p1 = pres || preq || rat' || iat'
*
* The octet of iat' becomes the least significant octet of p1 and the
* most significant octet of pres becomes the most significant octet of
* p1.
*
* ra is concatenated with ia and padding to generate p2 which is XORed
* with the result of the security function e using p1 as the input
* paremter plaintextData and is then used as the 128-bit input
* parameter plaintextData to security function e:
*
* p2 = padding || ia || ra
*
* The least significant octet of ra becomes the least significant octet
* of p2 and the most significant octet of padding becomes the most
* significant octet of p2.
*
* The output of the confirm value generation function c1 is:
*
* c1(k, r, preq, pres, iat, rat, ia, ra) = e(k, e(k, r XOR p1) XOR p2)
*
* The 128-bit output of the security function e is used as the result
* of confirm value generation function c1.
*/
bool bt_crypto_c1(struct bt_crypto *crypto, const uint8_t k[16],
const uint8_t r[16], const uint8_t pres[7],
const uint8_t preq[7], uint8_t iat,
const uint8_t ia[6], uint8_t rat,
const uint8_t ra[6], uint8_t res[16])
{
uint8_t p1[16], p2[16];
/* p1 = pres || preq || _rat || _iat */
p1[0] = iat;
p1[1] = rat;
memcpy(p1 + 2, preq, 7);
memcpy(p1 + 9, pres, 7);
/* p2 = padding || ia || ra */
memcpy(p2, ra, 6);
memcpy(p2 + 6, ia, 6);
memset(p2 + 12, 0, 4);
/* res = r XOR p1 */
u128_xor(r, p1, res);
/* res = e(k, res) */
if (!bt_crypto_e(crypto, k, res, res))
return false;
/* res = res XOR p2 */
u128_xor(res, p2, res);
/* res = e(k, res) */
return bt_crypto_e(crypto, k, res, res);
}
/**
* Key generation function s1
*
* The key generation function s1 is used to generate the STK during the
* pairing process.
*
* The following are inputs to the key generation function s1:
*
* k is 128 bits
* r1 is 128 bits
* r2 is 128 bits
*
* The most significant 64-bits of r1 are discarded to generate r1' and
* the most significant 64-bits of r2 are discarded to generate r2'.
*
* r1' is concatenated with r2' to generate r' which is used as the
* 128-bit input parameter plaintextData to security function e:
*
* r' = r1' || r2'
*
* The least significant octet of r2' becomes the least significant
* octet of r' and the most significant octet of r1' becomes the most
* significant octet of r'.
*
* The output of the key generation function s1 is:
*
* s1(k, r1, r2) = e(k, r')
*
* The 128-bit output of the security function e is used as the result
* of key generation function s1.
*/
bool bt_crypto_s1(struct bt_crypto *crypto, const uint8_t k[16],
const uint8_t r1[16], const uint8_t r2[16],
uint8_t res[16])
{
memcpy(res, r2, 8);
memcpy(res + 8, r1, 8);
return bt_crypto_e(crypto, k, res, res);
}
static bool aes_cmac(struct bt_crypto *crypto, uint8_t key[16], uint8_t *msg,
size_t msg_len, uint8_t res[16])
{
uint8_t key_msb[16], out[16], msg_msb[CMAC_MSG_MAX];
ssize_t len;
int fd;
if (msg_len > CMAC_MSG_MAX)
return false;
swap_buf(key, key_msb, 16);
fd = alg_new(crypto->cmac_aes, key_msb, 16);
if (fd < 0)
return false;
swap_buf(msg, msg_msb, msg_len);
len = send(fd, msg_msb, msg_len, 0);
if (len < 0) {
close(fd);
return false;
}
len = read(fd, out, 16);
if (len < 0) {
close(fd);
return false;
}
swap_buf(out, res, 16);
close(fd);
return true;
}
bool bt_crypto_f4(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
uint8_t x[16], uint8_t z, uint8_t res[16])
{
uint8_t m[65];
if (!crypto)
return false;
m[0] = z;
memcpy(&m[1], v, 32);
memcpy(&m[33], u, 32);
return aes_cmac(crypto, x, m, sizeof(m), res);
}
bool bt_crypto_f5(struct bt_crypto *crypto, uint8_t w[32], uint8_t n1[16],
uint8_t n2[16], uint8_t a1[7], uint8_t a2[7],
uint8_t mackey[16], uint8_t ltk[16])
{
uint8_t btle[4] = { 0x65, 0x6c, 0x74, 0x62 };
uint8_t salt[16] = { 0xbe, 0x83, 0x60, 0x5a, 0xdb, 0x0b, 0x37, 0x60,
0x38, 0xa5, 0xf5, 0xaa, 0x91, 0x83, 0x88, 0x6c };
uint8_t length[2] = { 0x00, 0x01 };
uint8_t m[53], t[16];
if (!aes_cmac(crypto, salt, w, 32, t))
return false;
memcpy(&m[0], length, 2);
memcpy(&m[2], a2, 7);
memcpy(&m[9], a1, 7);
memcpy(&m[16], n2, 16);
memcpy(&m[32], n1, 16);
memcpy(&m[48], btle, 4);
m[52] = 0; /* Counter */
if (!aes_cmac(crypto, t, m, sizeof(m), mackey))
return false;
m[52] = 1; /* Counter */
return aes_cmac(crypto, t, m, sizeof(m), ltk);
}
bool bt_crypto_f6(struct bt_crypto *crypto, uint8_t w[16], uint8_t n1[16],
uint8_t n2[16], uint8_t r[16], uint8_t io_cap[3],
uint8_t a1[7], uint8_t a2[7], uint8_t res[16])
{
uint8_t m[65];
memcpy(&m[0], a2, 7);
memcpy(&m[7], a1, 7);
memcpy(&m[14], io_cap, 3);
memcpy(&m[17], r, 16);
memcpy(&m[33], n2, 16);
memcpy(&m[49], n1, 16);
return aes_cmac(crypto, w, m, sizeof(m), res);
}
bool bt_crypto_g2(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
uint8_t x[16], uint8_t y[16], uint32_t *val)
{
uint8_t m[80], tmp[16];
memcpy(&m[0], y, 16);
memcpy(&m[16], v, 32);
memcpy(&m[48], u, 32);
if (!aes_cmac(crypto, x, m, sizeof(m), tmp))
return false;
*val = get_le32(tmp);
*val %= 1000000;
return true;
}

61
crypto.h Normal file
View File

@@ -0,0 +1,61 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#include <stdbool.h>
#include <stdint.h>
struct bt_crypto;
struct bt_crypto *bt_crypto_new(void);
struct bt_crypto *bt_crypto_ref(struct bt_crypto *crypto);
void bt_crypto_unref(struct bt_crypto *crypto);
bool bt_crypto_random_bytes(struct bt_crypto *crypto,
uint8_t *buf, uint8_t num_bytes);
bool bt_crypto_e(struct bt_crypto *crypto, const uint8_t key[16],
const uint8_t plaintext[16], uint8_t encrypted[16]);
bool bt_crypto_ah(struct bt_crypto *crypto, const uint8_t k[16],
const uint8_t r[3], uint8_t hash[3]);
bool bt_crypto_c1(struct bt_crypto *crypto, const uint8_t k[16],
const uint8_t r[16], const uint8_t pres[7],
const uint8_t preq[7], uint8_t iat,
const uint8_t ia[6], uint8_t rat,
const uint8_t ra[6], uint8_t res[16]);
bool bt_crypto_s1(struct bt_crypto *crypto, const uint8_t k[16],
const uint8_t r1[16], const uint8_t r2[16],
uint8_t res[16]);
bool bt_crypto_f4(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
uint8_t x[16], uint8_t z, uint8_t res[16]);
bool bt_crypto_f5(struct bt_crypto *crypto, uint8_t w[32], uint8_t n1[16],
uint8_t n2[16], uint8_t a1[7], uint8_t a2[7],
uint8_t mackey[16], uint8_t ltk[16]);
bool bt_crypto_f6(struct bt_crypto *crypto, uint8_t w[16], uint8_t n1[16],
uint8_t n2[16], uint8_t r[16], uint8_t io_cap[3],
uint8_t a1[7], uint8_t a2[7], uint8_t res[16]);
bool bt_crypto_g2(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32],
uint8_t x[16], uint8_t y[16], uint32_t *val);
bool bt_crypto_sign_att(struct bt_crypto *crypto, const uint8_t key[16],
const uint8_t *m, uint16_t m_len,
uint32_t sign_cnt, uint8_t signature[12]);

112
design_notes.h Normal file
View File

@@ -0,0 +1,112 @@
/**
* @file design_notes.h
* @brief notes about btgattclient software design
*
* We discuss about various topics concerning btgattclient software design and architecture:
* - epoll: a technic, specific to Linux, to manage in a single loop various i/o events associated with files (incl. standard i/o) and sockets
* - tbc: some more details to come
*
* @author Gilbert Brault
* @copyright Gilbert Brault 2015
*
*/
/*
* design_notes.h
*
* Created on: 30 juil. 2015
* Author: gilbert
*/
#ifndef DESIGN_NOTES_H_
#define DESIGN_NOTES_H_
#include <sys/epoll.h>
/**
* controls the epoll instance associated with epfd
*
* @param epfd the epoll context structure
* @param op specifies the operation to be taken against the file associated with fd
* @param fd file descriptor can be a socket
* @param event epool event further constrains the behavior of the operation
* @return 0 success <0 error
*/
int epoll_ctl_doc (int epfd,
int op,
int fd,
struct epoll_event *event);
/**
* @brief epool event structure 4th parameter of epoll_ctl(...)
*/
struct epoll_event_doc {
/// and EPOLL_EVENT value (might be ored)
int events;
union {
void *ptr;
int fd;
int u32;
long u64;
} data;
};
/**
* @brief EPOLL_CTL_XXX possible values
*/
enum EPOLL_CTL_DOC {
/// EPOLL_CTL_ADD: Add a monitor on the file associated with the file descriptor fd to the epoll instance associated with epfd, per the events defined in event.
EPOLL_CTL_ADD_DOC=EPOLL_CTL_ADD,
/// EPOLL_CTL_DEL: Remove a monitor on the file associated with the file descriptor fd from the epoll instance associated with epfd.
EPOLL_CTL_DEL_DOC=EPOLL_CTL_DEL,
/// EPOLL_CTL_MOD: Modify an existing monitor of fd with the updated events specified by event.
EPOLL_CTL_MOD_DOC=EPOLL_CTL_MOD
};
/** @brief EPOLL_EVENTS possible values
*
<code>
The events field in the epoll_event structure lists which events to monitor on the given file descriptor.
Multiple events can be bitwise-ORed together.
Edge- Versus Level-Triggered Events
If the EPOLLET value is set in the events field of the event parameter passed to epoll_ctl( ),
the watch on fd is edge-triggered, as opposed to level-triggered.
Consider the following events between a producer and a consumer communicating over a Unix pipe:
The producer writes 1 KB of data onto a pipe.
The consumer performs an epoll_wait( ) on the pipe, waiting for the pipe to contain data, and thus be readable.
With a level-triggered watch, the call to epoll_wait( ) in step 2 will return immediately,
showing that the pipe is ready to read.
With an edge-triggered watch,this call will not return until after step 1 occurs.
That is, even if the pipe is readable at the invocation of epoll_wait( ),
the call will not return until the data is written onto the pipe.
Level-triggered is the default behavior. It is how poll( ) and select( ) behave,
and it is what most developers expect. Edge-triggered behavior requires a different approach
to programming, commonly utilizing nonblocking I/O, and careful checking for EAGAIN.
</code>
*/
enum EPOLL_EVENTS_DOC {
/// EPOLLERR: An error condition occurred on the file. This event is always monitored, even if it's not specified.
EPOLLERR_DOC=EPOLLERR,
/// EPOLLET: Enables edge-triggered behavior for the monitor of the file (see the upcoming section "Edge- Versus Level-Triggered Events"). The default behavior is level-triggered.
EPOLLET_DOC=EPOLLET,
/// EPOLLHUP: A hangup occurred on the file. This event is always monitored, even if it's not specified.
EPOLLHUP_DOC=EPOLLHUP,
/// EPOLLIN: The file is available to be read from without blocking
EPOLLIN_DOC=EPOLLIN,
/// EPOLLONESHOT: After an event is generated and read, the file is automatically no longer monitored. A new event mask must be specified via EPOLL_CTL_MOD to reenable the watch.
EPOLLONESHOT_DOC=EPOLLONESHOT,
/// EPOLLOUT: The file is available to be written to without blocking.
EPOLLOUT_DOC=EPOLLOUT,
/// EPOLLPRI: There is urgent out-of-band data available to read.
EPOLLPRI_DOC=EPOLLPRI
};
#endif /* DESIGN_NOTES_H_ */

3069
gatt-client.c Normal file
View File

File diff suppressed because it is too large Load Diff

135
gatt-client.h Normal file
View File

@@ -0,0 +1,135 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2014 Google Inc.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>
#define BT_GATT_UUID_SIZE 16
struct bt_gatt_client;
struct bt_gatt_client *bt_gatt_client_new(struct gatt_db *db,
struct bt_att *att,
uint16_t mtu);
struct bt_gatt_client *bt_gatt_client_ref(struct bt_gatt_client *client);
void bt_gatt_client_unref(struct bt_gatt_client *client);
typedef void (*bt_gatt_client_destroy_func_t)(void *user_data);
typedef void (*bt_gatt_client_callback_t)(bool success, uint8_t att_ecode,
void *user_data);
typedef void (*bt_gatt_client_debug_func_t)(const char *str, void *user_data);
typedef void (*bt_gatt_client_read_callback_t)(bool success, uint8_t att_ecode,
const uint8_t *value, uint16_t length,
void *user_data);
typedef void (*bt_gatt_client_write_long_callback_t)(bool success,
bool reliable_error, uint8_t att_ecode,
void *user_data);
typedef void (*bt_gatt_client_notify_callback_t)(uint16_t value_handle,
const uint8_t *value, uint16_t length,
void *user_data);
typedef void (*bt_gatt_client_register_callback_t)(uint16_t att_ecode,
void *user_data);
typedef void (*bt_gatt_client_service_changed_callback_t)(uint16_t start_handle,
uint16_t end_handle,
void *user_data);
bool bt_gatt_client_is_ready(struct bt_gatt_client *client);
bool bt_gatt_client_set_ready_handler(struct bt_gatt_client *client,
bt_gatt_client_callback_t callback,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
bool bt_gatt_client_set_service_changed(struct bt_gatt_client *client,
bt_gatt_client_service_changed_callback_t callback,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
bool bt_gatt_client_set_debug(struct bt_gatt_client *client,
bt_gatt_client_debug_func_t callback,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
uint16_t bt_gatt_client_get_mtu(struct bt_gatt_client *client);
struct gatt_db *bt_gatt_client_get_db(struct bt_gatt_client *client);
bool bt_gatt_client_cancel(struct bt_gatt_client *client, unsigned int id);
bool bt_gatt_client_cancel_all(struct bt_gatt_client *client);
unsigned int bt_gatt_client_read_value(struct bt_gatt_client *client,
uint16_t value_handle,
bt_gatt_client_read_callback_t callback,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
unsigned int bt_gatt_client_read_long_value(struct bt_gatt_client *client,
uint16_t value_handle, uint16_t offset,
bt_gatt_client_read_callback_t callback,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
unsigned int bt_gatt_client_read_multiple(struct bt_gatt_client *client,
uint16_t *handles, uint8_t num_handles,
bt_gatt_client_read_callback_t callback,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
unsigned int bt_gatt_client_write_without_response(
struct bt_gatt_client *client,
uint16_t value_handle,
bool signed_write,
const uint8_t *value, uint16_t length);
unsigned int bt_gatt_client_write_value(struct bt_gatt_client *client,
uint16_t value_handle,
const uint8_t *value, uint16_t length,
bt_gatt_client_callback_t callback,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
unsigned int bt_gatt_client_write_long_value(struct bt_gatt_client *client,
bool reliable,
uint16_t value_handle, uint16_t offset,
const uint8_t *value, uint16_t length,
bt_gatt_client_write_long_callback_t callback,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
unsigned int bt_gatt_client_prepare_write(struct bt_gatt_client *client,
unsigned int id,
uint16_t value_handle, uint16_t offset,
const uint8_t *value, uint16_t length,
bt_gatt_client_write_long_callback_t callback,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
unsigned int bt_gatt_client_write_execute(struct bt_gatt_client *client,
unsigned int id,
bt_gatt_client_callback_t callback,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
unsigned int bt_gatt_client_register_notify(struct bt_gatt_client *client,
uint16_t chrc_value_handle,
bt_gatt_client_register_callback_t callback,
bt_gatt_client_notify_callback_t notify,
void *user_data,
bt_gatt_client_destroy_func_t destroy);
bool bt_gatt_client_unregister_notify(struct bt_gatt_client *client,
unsigned int id);
bool bt_gatt_client_set_security(struct bt_gatt_client *client, int level);
int bt_gatt_client_get_security(struct bt_gatt_client *client);

1765
gatt-db.c Normal file
View File

File diff suppressed because it is too large Load Diff

236
gatt-db.h Normal file
View File

@@ -0,0 +1,236 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
struct gatt_db;
struct gatt_db_attribute;
struct gatt_db *gatt_db_new(void);
struct gatt_db *gatt_db_ref(struct gatt_db *db);
void gatt_db_unref(struct gatt_db *db);
bool gatt_db_isempty(struct gatt_db *db);
struct gatt_db_attribute *gatt_db_add_service(struct gatt_db *db,
const bt_uuid_t *uuid,
bool primary,
uint16_t num_handles);
bool gatt_db_remove_service(struct gatt_db *db,
struct gatt_db_attribute *attrib);
bool gatt_db_clear(struct gatt_db *db);
bool gatt_db_clear_range(struct gatt_db *db, uint16_t start_handle,
uint16_t end_handle);
struct gatt_db_attribute *gatt_db_insert_service(struct gatt_db *db,
uint16_t handle,
const bt_uuid_t *uuid,
bool primary,
uint16_t num_handles);
typedef void (*gatt_db_read_t) (struct gatt_db_attribute *attrib,
unsigned int id, uint16_t offset,
uint8_t opcode, struct bt_att *att,
void *user_data);
typedef void (*gatt_db_write_t) (struct gatt_db_attribute *attrib,
unsigned int id, uint16_t offset,
const uint8_t *value, size_t len,
uint8_t opcode, struct bt_att *att,
void *user_data);
struct gatt_db_attribute *
gatt_db_service_add_characteristic(struct gatt_db_attribute *attrib,
const bt_uuid_t *uuid,
uint32_t permissions,
uint8_t properties,
gatt_db_read_t read_func,
gatt_db_write_t write_func,
void *user_data);
struct gatt_db_attribute *
gatt_db_service_insert_characteristic(struct gatt_db_attribute *attrib,
uint16_t handle,
const bt_uuid_t *uuid,
uint32_t permissions,
uint8_t properties,
gatt_db_read_t read_func,
gatt_db_write_t write_func,
void *user_data);
struct gatt_db_attribute *
gatt_db_service_add_descriptor(struct gatt_db_attribute *attrib,
const bt_uuid_t *uuid,
uint32_t permissions,
gatt_db_read_t read_func,
gatt_db_write_t write_func,
void *user_data);
struct gatt_db_attribute *
gatt_db_service_insert_descriptor(struct gatt_db_attribute *attrib,
uint16_t handle,
const bt_uuid_t *uuid,
uint32_t permissions,
gatt_db_read_t read_func,
gatt_db_write_t write_func,
void *user_data);
struct gatt_db_attribute *
gatt_db_service_add_included(struct gatt_db_attribute *attrib,
struct gatt_db_attribute *include);
bool gatt_db_service_set_active(struct gatt_db_attribute *attrib, bool active);
bool gatt_db_service_get_active(struct gatt_db_attribute *attrib);
bool gatt_db_service_set_claimed(struct gatt_db_attribute *attrib,
bool claimed);
bool gatt_db_service_get_claimed(struct gatt_db_attribute *attrib);
typedef void (*gatt_db_attribute_cb_t)(struct gatt_db_attribute *attrib,
void *user_data);
void gatt_db_read_by_group_type(struct gatt_db *db, uint16_t start_handle,
uint16_t end_handle,
const bt_uuid_t type,
struct queue *queue);
unsigned int gatt_db_find_by_type(struct gatt_db *db, uint16_t start_handle,
uint16_t end_handle,
const bt_uuid_t *type,
gatt_db_attribute_cb_t func,
void *user_data);
unsigned int gatt_db_find_by_type_value(struct gatt_db *db,
uint16_t start_handle,
uint16_t end_handle,
const bt_uuid_t *type,
const void *value,
size_t value_len,
gatt_db_attribute_cb_t func,
void *user_data);
void gatt_db_read_by_type(struct gatt_db *db, uint16_t start_handle,
uint16_t end_handle,
const bt_uuid_t type,
struct queue *queue);
void gatt_db_find_information(struct gatt_db *db, uint16_t start_handle,
uint16_t end_handle,
struct queue *queue);
void gatt_db_foreach_service(struct gatt_db *db, const bt_uuid_t *uuid,
gatt_db_attribute_cb_t func,
void *user_data);
void gatt_db_foreach_service_in_range(struct gatt_db *db,
const bt_uuid_t *uuid,
gatt_db_attribute_cb_t func,
void *user_data,
uint16_t start_handle,
uint16_t end_handle);
void gatt_db_service_foreach(struct gatt_db_attribute *attrib,
const bt_uuid_t *uuid,
gatt_db_attribute_cb_t func,
void *user_data);
void gatt_db_service_foreach_char(struct gatt_db_attribute *attrib,
gatt_db_attribute_cb_t func,
void *user_data);
void gatt_db_service_foreach_desc(struct gatt_db_attribute *attrib,
gatt_db_attribute_cb_t func,
void *user_data);
void gatt_db_service_foreach_incl(struct gatt_db_attribute *attrib,
gatt_db_attribute_cb_t func,
void *user_data);
typedef void (*gatt_db_destroy_func_t)(void *user_data);
unsigned int gatt_db_register(struct gatt_db *db,
gatt_db_attribute_cb_t service_added,
gatt_db_attribute_cb_t service_removed,
void *user_data,
gatt_db_destroy_func_t destroy);
bool gatt_db_unregister(struct gatt_db *db, unsigned int id);
struct gatt_db_attribute *gatt_db_get_attribute(struct gatt_db *db,
uint16_t handle);
struct gatt_db_attribute *gatt_db_get_service_with_uuid(struct gatt_db *db,
const bt_uuid_t *uuid);
const bt_uuid_t *gatt_db_attribute_get_type(
const struct gatt_db_attribute *attrib);
uint16_t gatt_db_attribute_get_handle(const struct gatt_db_attribute *attrib);
bool gatt_db_attribute_get_service_uuid(const struct gatt_db_attribute *attrib,
bt_uuid_t *uuid);
bool gatt_db_attribute_get_service_handles(
const struct gatt_db_attribute *attrib,
uint16_t *start_handle,
uint16_t *end_handle);
bool gatt_db_attribute_get_service_data(const struct gatt_db_attribute *attrib,
uint16_t *start_handle,
uint16_t *end_handle,
bool *primary,
bt_uuid_t *uuid);
bool gatt_db_attribute_get_char_data(const struct gatt_db_attribute *attrib,
uint16_t *handle,
uint16_t *value_handle,
uint8_t *properties,
bt_uuid_t *uuid);
bool gatt_db_attribute_get_incl_data(const struct gatt_db_attribute *attrib,
uint16_t *handle,
uint16_t *start_handle,
uint16_t *end_handle);
uint32_t
gatt_db_attribute_get_permissions(const struct gatt_db_attribute *attrib);
typedef void (*gatt_db_attribute_read_t) (struct gatt_db_attribute *attrib,
int err, const uint8_t *value,
size_t length, void *user_data);
bool gatt_db_attribute_read(struct gatt_db_attribute *attrib, uint16_t offset,
uint8_t opcode, struct bt_att *att,
gatt_db_attribute_read_t func, void *user_data);
bool gatt_db_attribute_read_result(struct gatt_db_attribute *attrib,
unsigned int id, int err,
const uint8_t *value, size_t length);
typedef void (*gatt_db_attribute_write_t) (struct gatt_db_attribute *attrib,
int err, void *user_data);
bool gatt_db_attribute_write(struct gatt_db_attribute *attrib, uint16_t offset,
const uint8_t *value, size_t len,
uint8_t opcode, struct bt_att *att,
gatt_db_attribute_write_t func,
void *user_data);
bool gatt_db_attribute_write_result(struct gatt_db_attribute *attrib,
unsigned int id, int err);
bool gatt_db_attribute_reset(struct gatt_db_attribute *attrib);

1580
gatt-helpers.c Normal file
View File

File diff suppressed because it is too large Load Diff

116
gatt-helpers.h Normal file
View File

@@ -0,0 +1,116 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2014 Google Inc.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/* This file defines helpers for performing client-side procedures defined by
* the Generic Attribute Profile.
*/
#include <stdbool.h>
#include <stdint.h>
struct bt_gatt_result;
struct bt_gatt_iter {
struct bt_gatt_result *result;
uint16_t pos;
};
unsigned int bt_gatt_result_service_count(struct bt_gatt_result *result);
unsigned int bt_gatt_result_characteristic_count(struct bt_gatt_result *result);
unsigned int bt_gatt_result_descriptor_count(struct bt_gatt_result *result);
unsigned int bt_gatt_result_included_count(struct bt_gatt_result *result);
bool bt_gatt_iter_init(struct bt_gatt_iter *iter, struct bt_gatt_result *result);
bool bt_gatt_iter_next_service(struct bt_gatt_iter *iter,
uint16_t *start_handle, uint16_t *end_handle,
uint8_t uuid[16]);
bool bt_gatt_iter_next_characteristic(struct bt_gatt_iter *iter,
uint16_t *start_handle, uint16_t *end_handle,
uint16_t *value_handle, uint8_t *properties,
uint8_t uuid[16]);
bool bt_gatt_iter_next_descriptor(struct bt_gatt_iter *iter, uint16_t *handle,
uint8_t uuid[16]);
bool bt_gatt_iter_next_included_service(struct bt_gatt_iter *iter,
uint16_t *handle, uint16_t *start_handle,
uint16_t *end_handle, uint8_t uuid[16]);
bool bt_gatt_iter_next_read_by_type(struct bt_gatt_iter *iter,
uint16_t *handle, uint16_t *length,
const uint8_t **value);
typedef void (*bt_gatt_destroy_func_t)(void *user_data);
typedef void (*bt_gatt_result_callback_t)(bool success, uint8_t att_ecode,
void *user_data);
typedef void (*bt_gatt_request_callback_t)(bool success, uint8_t att_ecode,
struct bt_gatt_result *result,
void *user_data);
struct bt_gatt_request;
struct bt_gatt_request *bt_gatt_request_ref(struct bt_gatt_request *req);
void bt_gatt_request_unref(struct bt_gatt_request *req);
void bt_gatt_request_cancel(struct bt_gatt_request *req);
unsigned int bt_gatt_exchange_mtu(struct bt_att *att, uint16_t client_rx_mtu,
bt_gatt_result_callback_t callback,
void *user_data,
bt_gatt_destroy_func_t destroy);
struct bt_gatt_request *bt_gatt_discover_all_primary_services(
struct bt_att *att, bt_uuid_t *uuid,
bt_gatt_request_callback_t callback,
void *user_data,
bt_gatt_destroy_func_t destroy);
struct bt_gatt_request *bt_gatt_discover_primary_services(
struct bt_att *att, bt_uuid_t *uuid,
uint16_t start, uint16_t end,
bt_gatt_request_callback_t callback,
void *user_data,
bt_gatt_destroy_func_t destroy);
struct bt_gatt_request *bt_gatt_discover_secondary_services(
struct bt_att *att, bt_uuid_t *uuid,
uint16_t start, uint16_t end,
bt_gatt_request_callback_t callback,
void *user_data,
bt_gatt_destroy_func_t destroy);
struct bt_gatt_request *bt_gatt_discover_included_services(struct bt_att *att,
uint16_t start, uint16_t end,
bt_gatt_request_callback_t callback,
void *user_data,
bt_gatt_destroy_func_t destroy);
struct bt_gatt_request *bt_gatt_discover_characteristics(struct bt_att *att,
uint16_t start, uint16_t end,
bt_gatt_request_callback_t callback,
void *user_data,
bt_gatt_destroy_func_t destroy);
struct bt_gatt_request *bt_gatt_discover_descriptors(struct bt_att *att,
uint16_t start, uint16_t end,
bt_gatt_request_callback_t callback,
void *user_data,
bt_gatt_destroy_func_t destroy);
bool bt_gatt_read_by_type(struct bt_att *att, uint16_t start, uint16_t end,
const bt_uuid_t *uuid,
bt_gatt_request_callback_t callback,
void *user_data,
bt_gatt_destroy_func_t destroy);

4012
hci.c Normal file
View File

File diff suppressed because it is too large Load Diff

2449
hci.h Normal file
View File

File diff suppressed because it is too large Load Diff

242
hci_lib.h Normal file
View File

@@ -0,0 +1,242 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2000-2001 Qualcomm Incorporated
* Copyright (C) 2002-2003 Maxim Krasnyansky <maxk@qualcomm.com>
* Copyright (C) 2002-2010 Marcel Holtmann <marcel@holtmann.org>
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifndef __HCI_LIB_H
#define __HCI_LIB_H
#ifdef __cplusplus
extern "C" {
#endif
struct hci_request {
uint16_t ogf;
uint16_t ocf;
int event;
void *cparam;
int clen;
void *rparam;
int rlen;
};
struct hci_version {
uint16_t manufacturer;
uint8_t hci_ver;
uint16_t hci_rev;
uint8_t lmp_ver;
uint16_t lmp_subver;
};
int hci_open_dev(int dev_id);
int hci_close_dev(int dd);
int hci_send_cmd(int dd, uint16_t ogf, uint16_t ocf, uint8_t plen, void *param);
int hci_send_req(int dd, struct hci_request *req, int timeout);
int hci_create_connection(int dd, const bdaddr_t *bdaddr, uint16_t ptype, uint16_t clkoffset, uint8_t rswitch, uint16_t *handle, int to);
int hci_disconnect(int dd, uint16_t handle, uint8_t reason, int to);
int hci_inquiry(int dev_id, int len, int num_rsp, const uint8_t *lap, inquiry_info **ii, long flags);
int hci_devinfo(int dev_id, struct hci_dev_info *di);
int hci_devba(int dev_id, bdaddr_t *bdaddr);
int hci_devid(const char *str);
int hci_read_local_name(int dd, int len, char *name, int to);
int hci_write_local_name(int dd, const char *name, int to);
int hci_read_remote_name(int dd, const bdaddr_t *bdaddr, int len, char *name, int to);
int hci_read_remote_name_with_clock_offset(int dd, const bdaddr_t *bdaddr, uint8_t pscan_rep_mode, uint16_t clkoffset, int len, char *name, int to);
int hci_read_remote_name_cancel(int dd, const bdaddr_t *bdaddr, int to);
int hci_read_remote_version(int dd, uint16_t handle, struct hci_version *ver, int to);
int hci_read_remote_features(int dd, uint16_t handle, uint8_t *features, int to);
int hci_read_remote_ext_features(int dd, uint16_t handle, uint8_t page, uint8_t *max_page, uint8_t *features, int to);
int hci_read_clock_offset(int dd, uint16_t handle, uint16_t *clkoffset, int to);
int hci_read_local_version(int dd, struct hci_version *ver, int to);
int hci_read_local_commands(int dd, uint8_t *commands, int to);
int hci_read_local_features(int dd, uint8_t *features, int to);
int hci_read_local_ext_features(int dd, uint8_t page, uint8_t *max_page, uint8_t *features, int to);
int hci_read_bd_addr(int dd, bdaddr_t *bdaddr, int to);
int hci_read_class_of_dev(int dd, uint8_t *cls, int to);
int hci_write_class_of_dev(int dd, uint32_t cls, int to);
int hci_read_voice_setting(int dd, uint16_t *vs, int to);
int hci_write_voice_setting(int dd, uint16_t vs, int to);
int hci_read_current_iac_lap(int dd, uint8_t *num_iac, uint8_t *lap, int to);
int hci_write_current_iac_lap(int dd, uint8_t num_iac, uint8_t *lap, int to);
int hci_read_stored_link_key(int dd, bdaddr_t *bdaddr, uint8_t all, int to);
int hci_write_stored_link_key(int dd, bdaddr_t *bdaddr, uint8_t *key, int to);
int hci_delete_stored_link_key(int dd, bdaddr_t *bdaddr, uint8_t all, int to);
int hci_authenticate_link(int dd, uint16_t handle, int to);
int hci_encrypt_link(int dd, uint16_t handle, uint8_t encrypt, int to);
int hci_change_link_key(int dd, uint16_t handle, int to);
int hci_switch_role(int dd, bdaddr_t *bdaddr, uint8_t role, int to);
int hci_park_mode(int dd, uint16_t handle, uint16_t max_interval, uint16_t min_interval, int to);
int hci_exit_park_mode(int dd, uint16_t handle, int to);
int hci_read_inquiry_scan_type(int dd, uint8_t *type, int to);
int hci_write_inquiry_scan_type(int dd, uint8_t type, int to);
int hci_read_inquiry_mode(int dd, uint8_t *mode, int to);
int hci_write_inquiry_mode(int dd, uint8_t mode, int to);
int hci_read_afh_mode(int dd, uint8_t *mode, int to);
int hci_write_afh_mode(int dd, uint8_t mode, int to);
int hci_read_ext_inquiry_response(int dd, uint8_t *fec, uint8_t *data, int to);
int hci_write_ext_inquiry_response(int dd, uint8_t fec, uint8_t *data, int to);
int hci_read_simple_pairing_mode(int dd, uint8_t *mode, int to);
int hci_write_simple_pairing_mode(int dd, uint8_t mode, int to);
int hci_read_local_oob_data(int dd, uint8_t *hash, uint8_t *randomizer, int to);
int hci_read_inq_response_tx_power_level(int dd, int8_t *level, int to);
int hci_read_inquiry_transmit_power_level(int dd, int8_t *level, int to);
int hci_write_inquiry_transmit_power_level(int dd, int8_t level, int to);
int hci_read_transmit_power_level(int dd, uint16_t handle, uint8_t type, int8_t *level, int to);
int hci_read_link_policy(int dd, uint16_t handle, uint16_t *policy, int to);
int hci_write_link_policy(int dd, uint16_t handle, uint16_t policy, int to);
int hci_read_link_supervision_timeout(int dd, uint16_t handle, uint16_t *timeout, int to);
int hci_write_link_supervision_timeout(int dd, uint16_t handle, uint16_t timeout, int to);
int hci_set_afh_classification(int dd, uint8_t *map, int to);
int hci_read_link_quality(int dd, uint16_t handle, uint8_t *link_quality, int to);
int hci_read_rssi(int dd, uint16_t handle, int8_t *rssi, int to);
int hci_read_afh_map(int dd, uint16_t handle, uint8_t *mode, uint8_t *map, int to);
int hci_read_clock(int dd, uint16_t handle, uint8_t which, uint32_t *clock, uint16_t *accuracy, int to);
int hci_le_set_scan_enable(int dev_id, uint8_t enable, uint8_t filter_dup, int to);
int hci_le_set_scan_parameters(int dev_id, uint8_t type, uint16_t interval,
uint16_t window, uint8_t own_type,
uint8_t filter, int to);
int hci_le_set_advertise_enable(int dev_id, uint8_t enable, int to);
int hci_le_create_conn(int dd, uint16_t interval, uint16_t window,
uint8_t initiator_filter, uint8_t peer_bdaddr_type,
bdaddr_t peer_bdaddr, uint8_t own_bdaddr_type,
uint16_t min_interval, uint16_t max_interval,
uint16_t latency, uint16_t supervision_timeout,
uint16_t min_ce_length, uint16_t max_ce_length,
uint16_t *handle, int to);
int hci_le_conn_update(int dd, uint16_t handle, uint16_t min_interval,
uint16_t max_interval, uint16_t latency,
uint16_t supervision_timeout, int to);
int hci_le_add_white_list(int dd, const bdaddr_t *bdaddr, uint8_t type, int to);
int hci_le_rm_white_list(int dd, const bdaddr_t *bdaddr, uint8_t type, int to);
int hci_le_read_white_list_size(int dd, uint8_t *size, int to);
int hci_le_clear_white_list(int dd, int to);
int hci_le_add_resolving_list(int dd, const bdaddr_t *bdaddr, uint8_t type,
uint8_t *peer_irk, uint8_t *local_irk, int to);
int hci_le_rm_resolving_list(int dd, const bdaddr_t *bdaddr, uint8_t type, int to);
int hci_le_clear_resolving_list(int dd, int to);
int hci_le_read_resolving_list_size(int dd, uint8_t *size, int to);
int hci_le_set_address_resolution_enable(int dev_id, uint8_t enable, int to);
int hci_le_read_remote_features(int dd, uint16_t handle, uint8_t *features, int to);
int hci_for_each_dev(int flag, int(*func)(int dd, int dev_id, long arg), long arg);
int hci_get_route(bdaddr_t *bdaddr);
char *hci_bustostr(int bus);
char *hci_typetostr(int type);
char *hci_dtypetostr(int type);
char *hci_dflagstostr(uint32_t flags);
char *hci_ptypetostr(unsigned int ptype);
int hci_strtoptype(char *str, unsigned int *val);
char *hci_scoptypetostr(unsigned int ptype);
int hci_strtoscoptype(char *str, unsigned int *val);
char *hci_lptostr(unsigned int ptype);
int hci_strtolp(char *str, unsigned int *val);
char *hci_lmtostr(unsigned int ptype);
int hci_strtolm(char *str, unsigned int *val);
char *hci_cmdtostr(unsigned int cmd);
char *hci_commandstostr(uint8_t *commands, char *pref, int width);
char *hci_vertostr(unsigned int ver);
int hci_strtover(char *str, unsigned int *ver);
char *lmp_vertostr(unsigned int ver);
int lmp_strtover(char *str, unsigned int *ver);
char *pal_vertostr(unsigned int ver);
int pal_strtover(char *str, unsigned int *ver);
char *lmp_featurestostr(uint8_t *features, char *pref, int width);
static inline void hci_set_bit(int nr, void *addr)
{
*((uint32_t *) addr + (nr >> 5)) |= (1 << (nr & 31));
}
static inline void hci_clear_bit(int nr, void *addr)
{
*((uint32_t *) addr + (nr >> 5)) &= ~(1 << (nr & 31));
}
static inline int hci_test_bit(int nr, void *addr)
{
return *((uint32_t *) addr + (nr >> 5)) & (1 << (nr & 31));
}
/* HCI filter tools */
static inline void hci_filter_clear(struct hci_filter *f)
{
memset(f, 0, sizeof(*f));
}
static inline void hci_filter_set_ptype(int t, struct hci_filter *f)
{
hci_set_bit((t == HCI_VENDOR_PKT) ? 0 : (t & HCI_FLT_TYPE_BITS), &f->type_mask);
}
static inline void hci_filter_clear_ptype(int t, struct hci_filter *f)
{
hci_clear_bit((t == HCI_VENDOR_PKT) ? 0 : (t & HCI_FLT_TYPE_BITS), &f->type_mask);
}
static inline int hci_filter_test_ptype(int t, struct hci_filter *f)
{
return hci_test_bit((t == HCI_VENDOR_PKT) ? 0 : (t & HCI_FLT_TYPE_BITS), &f->type_mask);
}
static inline void hci_filter_all_ptypes(struct hci_filter *f)
{
memset((void *) &f->type_mask, 0xff, sizeof(f->type_mask));
}
static inline void hci_filter_set_event(int e, struct hci_filter *f)
{
hci_set_bit((e & HCI_FLT_EVENT_BITS), &f->event_mask);
}
static inline void hci_filter_clear_event(int e, struct hci_filter *f)
{
hci_clear_bit((e & HCI_FLT_EVENT_BITS), &f->event_mask);
}
static inline int hci_filter_test_event(int e, struct hci_filter *f)
{
return hci_test_bit((e & HCI_FLT_EVENT_BITS), &f->event_mask);
}
static inline void hci_filter_all_events(struct hci_filter *f)
{
memset((void *) f->event_mask, 0xff, sizeof(f->event_mask));
}
static inline void hci_filter_set_opcode(int opcode, struct hci_filter *f)
{
f->opcode = opcode;
}
static inline void hci_filter_clear_opcode(struct hci_filter *f)
{
f->opcode = 0;
}
static inline int hci_filter_test_opcode(int opcode, struct hci_filter *f)
{
return (f->opcode == opcode);
}
#ifdef __cplusplus
}
#endif
#endif /* __HCI_LIB_H */

423
io-mainloop.c Normal file
View File

@@ -0,0 +1,423 @@
/**
* @file io-mainloop.c
* @brief set of function to manage io part of the mainloop feature
* @see mainloop.c
* @author Gilbert Brault
* @copyright Gilbert Brault 2015
* the original work comes from bluez v5.39
* value add: documenting main features
*
*/
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <unistd.h>
#include <errno.h>
#include <sys/socket.h>
#include "mainloop.h"
#include "util.h"
#include "io.h"
/**
* @brief data structure to manage io
*/
struct io {
int ref_count; /**< number of references to the data structure */
int fd; /**< file descriptor */
uint32_t events; /**< epoll events (might be ored) */
bool close_on_destroy; /**< do you need to close the underlying socket on destroy? */
io_callback_func_t read_callback; /**< read call back */
io_destroy_func_t read_destroy; /**< data management for read */
void *read_data; /**< user pointer for read data */
io_callback_func_t write_callback; /**< write call back*/
io_destroy_func_t write_destroy; /**< data management for write */
void *write_data; /**< user pointer for write data */
io_callback_func_t disconnect_callback; /**< call back on disconnect */
io_destroy_func_t disconnect_destroy; /**< data management at disconnect */
void *disconnect_data; /**< user pointer for disconnect */
};
/**
* Increment &io->ref_count
* @param io point to io data structure
* @return
*/
static struct io *io_ref(struct io *io)
{
if (!io)
return NULL;
__sync_fetch_and_add(&io->ref_count, 1);
return io;
}
/**
* Decrement &io->ref_count and free io data structure when count reaches 0
* @param io point to io data structure
* @return
*/
static void io_unref(struct io *io)
{
if (!io)
return;
if (__sync_sub_and_fetch(&io->ref_count, 1))
return;
free(io);
}
/**
* io data structure house keeping
* @param user_data pointer to io data structure
*/
static void io_cleanup(void *user_data)
{
struct io *io = user_data;
if (io->write_destroy)
io->write_destroy(io->write_data);
if (io->read_destroy)
io->read_destroy(io->read_data);
if (io->disconnect_destroy)
io->disconnect_destroy(io->disconnect_data);
if (io->close_on_destroy)
close(io->fd);
io->fd = -1;
}
/**
* depending on events epoll event and read, write or disconnect prepare for the appropriate call back calling mainloop_modify_fd
*
* epoll event value (some 'ored' combination allowed) EPOLLRDHUP | EPOLLHUP | EPOLLERR | EPOLLIN | EPOLLOUT )
*
* @param fd file descriptor data structure (can be socket)
* @param events epoll event
* @param user_data pointer to io data structure
*/
static void io_callback(int fd, uint32_t events, void *user_data)
{
struct io *io = user_data;
io_ref(io);
if ((events & (EPOLLRDHUP | EPOLLHUP | EPOLLERR))) {
io->read_callback = NULL;
io->write_callback = NULL;
if (!io->disconnect_callback) {
mainloop_remove_fd(io->fd);
io_unref(io);
return;
}
if (!io->disconnect_callback(io, io->disconnect_data)) {
if (io->disconnect_destroy)
io->disconnect_destroy(io->disconnect_data);
io->disconnect_callback = NULL;
io->disconnect_destroy = NULL;
io->disconnect_data = NULL;
io->events &= ~EPOLLRDHUP;
mainloop_modify_fd(io->fd, io->events);
}
}
if ((events & EPOLLIN) && io->read_callback) {
if (!io->read_callback(io, io->read_data)) {
if (io->read_destroy)
io->read_destroy(io->read_data);
io->read_callback = NULL;
io->read_destroy = NULL;
io->read_data = NULL;
io->events &= ~EPOLLIN;
mainloop_modify_fd(io->fd, io->events);
}
}
if ((events & EPOLLOUT) && io->write_callback) {
if (!io->write_callback(io, io->write_data)) {
if (io->write_destroy)
io->write_destroy(io->write_data);
io->write_callback = NULL;
io->write_destroy = NULL;
io->write_data = NULL;
io->events &= ~EPOLLOUT;
mainloop_modify_fd(io->fd, io->events);
}
}
io_unref(io);
}
/**
* create a new io data structure
*
* @param fd file descriptor (includes socket)
* @return NULL if error or io data structure
*/
struct io *io_new(int fd)
{
struct io *io;
if (fd < 0)
return NULL;
io = new0(struct io, 1);
if (!io)
return NULL;
io->fd = fd;
io->events = 0;
io->close_on_destroy = false;
if (mainloop_add_fd(io->fd, io->events, io_callback,
io, io_cleanup) < 0) {
free(io);
return NULL;
}
return io_ref(io);
}
/**
* remove the fd from the 'mainloop' and reset all io call back, unref the io data structure
*
* @param io pointer to the io data structure
*/
void io_destroy(struct io *io)
{
if (!io)
return;
io->read_callback = NULL;
io->write_callback = NULL;
io->disconnect_callback = NULL;
mainloop_remove_fd(io->fd);
io_unref(io);
}
/**
* return the fd (file descriptor, can be a socket) attached to the io data structure
*
* @param io pointer to io data structure
* @return
*/
int io_get_fd(struct io *io)
{
if (!io)
return -ENOTCONN;
return io->fd;
}
/**
*
* @param io
* @param do_close
* @return
*/
bool io_set_close_on_destroy(struct io *io, bool do_close)
{
if (!io)
return false;
io->close_on_destroy = do_close;
return true;
}
/**
*
* @param io
* @param callback
* @param user_data
* @param destroy
* @return
*/
bool io_set_read_handler(struct io *io, io_callback_func_t callback,
void *user_data, io_destroy_func_t destroy)
{
uint32_t events;
if (!io || io->fd < 0)
return false;
if (io->read_destroy)
io->read_destroy(io->read_data);
if (callback)
events = io->events | EPOLLIN;
else
events = io->events & ~EPOLLIN;
io->read_callback = callback;
io->read_destroy = destroy;
io->read_data = user_data;
if (events == io->events)
return true;
if (mainloop_modify_fd(io->fd, events) < 0)
return false;
io->events = events;
return true;
}
/**
*
* @param io
* @param callback
* @param user_data
* @param destroy
* @return
*/
bool io_set_write_handler(struct io *io, io_callback_func_t callback,
void *user_data, io_destroy_func_t destroy)
{
uint32_t events;
if (!io || io->fd < 0)
return false;
if (io->write_destroy)
io->write_destroy(io->write_data);
if (callback)
events = io->events | EPOLLOUT;
else
events = io->events & ~EPOLLOUT;
io->write_callback = callback;
io->write_destroy = destroy;
io->write_data = user_data;
if (events == io->events)
return true;
if (mainloop_modify_fd(io->fd, events) < 0)
return false;
io->events = events;
return true;
}
/**
*
* @param io
* @param callback
* @param user_data
* @param destroy
* @return
*/
bool io_set_disconnect_handler(struct io *io, io_callback_func_t callback,
void *user_data, io_destroy_func_t destroy)
{
uint32_t events;
if (!io || io->fd < 0)
return false;
if (io->disconnect_destroy)
io->disconnect_destroy(io->disconnect_data);
if (callback)
events = io->events | EPOLLRDHUP;
else
events = io->events & ~EPOLLRDHUP;
io->disconnect_callback = callback;
io->disconnect_destroy = destroy;
io->disconnect_data = user_data;
if (events == io->events)
return true;
if (mainloop_modify_fd(io->fd, events) < 0)
return false;
io->events = events;
return true;
}
/**
* send data to the underlying socket (actual write)
*
* @param io io structure which describe the underlying socket and its context
* @param iov pointer to pdu, pdu length structure
* @param iovcnt number of pdu to send (iov is an array)
* @return The return value is a count of bytes written, or -errno indicating an error.
*/
ssize_t io_send(struct io *io, const struct iovec *iov, int iovcnt)
{
ssize_t ret;
if (!io || io->fd < 0)
return -ENOTCONN;
do {
ret = writev(io->fd, iov, iovcnt);
} while (ret < 0 && errno == EINTR);
if (ret < 0)
return -errno;
return ret;
}
/**
*
* @param io
* @return
*/
bool io_shutdown(struct io *io)
{
if (!io || io->fd < 0)
return false;
return shutdown(io->fd, SHUT_RDWR) == 0;
}

47
io.h Normal file
View File

@@ -0,0 +1,47 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#include <stdbool.h>
#include <sys/uio.h>
typedef void (*io_destroy_func_t)(void *data);
struct io;
struct io *io_new(int fd);
void io_destroy(struct io *io);
int io_get_fd(struct io *io);
bool io_set_close_on_destroy(struct io *io, bool do_close);
ssize_t io_send(struct io *io, const struct iovec *iov, int iovcnt);
bool io_shutdown(struct io *io);
typedef bool (*io_callback_func_t)(struct io *io, void *user_data);
bool io_set_read_handler(struct io *io, io_callback_func_t callback,
void *user_data, io_destroy_func_t destroy);
bool io_set_write_handler(struct io *io, io_callback_func_t callback,
void *user_data, io_destroy_func_t destroy);
bool io_set_disconnect_handler(struct io *io, io_callback_func_t callback,
void *user_data, io_destroy_func_t destroy);

887
json.c Normal file
View File

@@ -0,0 +1,887 @@
#include <string.h>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include "json.h"
/*
read and traverse
*/
static void skipWhiteSpaces(const char** pstr)
{
while (isspace(**pstr))
++*pstr;
}
static void fillError(const char* str, char* errorBuf, int errorBufSize)
{
static const char errorPrefix[] = "Parse error here: ";
static const int errorPrefixLen = sizeof(errorPrefix);
snprintf(errorBuf, errorBufSize, "%s", errorPrefix);
snprintf(errorBuf + errorPrefixLen - 1, errorBufSize - errorPrefixLen, "%s", str);
}
static int parseChar(const char **source, char c)
{
skipWhiteSpaces(source);
if (!**source || tolower(**source) != tolower(c))
return -1;
++*source;
return 0;
}
static int createStringCopy(const char *source, int sourceLen, char **target)
{
if (sourceLen == -1)
sourceLen = strlen(source);
*target = malloc(sourceLen + 1);
if (!*target)
return -1;
memcpy(*target, source, sourceLen);
(*target)[sourceLen] = '\0';
return 0;
}
static int parseQuotedString(const char **source, char **target)
{
const char *begin;
int strLen;
skipWhiteSpaces(source);
if (parseChar(source, '"') != 0)
return -1;
begin = *source;
while (**source != '"' && **source)
++*source;
if (!**source)
return -1;
strLen = *source - begin;
if (createStringCopy(begin, strLen, target) != 0)
return -1;
++*source;
return 0;
}
static int matchString(const char **source, const char *pattern)
{
while (*pattern)
{
if (parseChar(source, *pattern) != 0)
return -1;
++pattern;
}
return 0;
}
static int reallocObject(struct JsonVal *val)
{
if (!(val->u.object.keys = realloc(
val->u.object.keys,
(val->u.object.len + 1) * sizeof(*val->u.object.keys))))
{
return -1;
}
if (!(val->u.object.values = realloc(
val->u.object.values,
(val->u.object.len + 1) * sizeof(*val->u.object.values))))
{
return -1;
}
val->u.object.len++;
return 0;
}
static int reallocArray(struct JsonVal *val)
{
if (!(val->u.array.values = realloc(
val->u.array.values,
(val->u.array.len + 1) * sizeof(*val->u.array.values))))
{
return -1;
}
val->u.array.len++;
return 0;
}
static int parseObject(const char **source, struct JsonVal *value);
static int parseValue(const char **source, struct JsonVal *val);
static int parseArrayEntry(const char **source, struct JsonVal *val)
{
if(reallocArray(val) != 0)
return -1;
skipWhiteSpaces(source);
return parseValue(source, val->u.array.values + val->u.array.len - 1);
}
static int parseArray(const char **source, struct JsonVal *value)
{
int finished = 0;
value->type = jsonArrayT;
value->u.array.values = NULL;
value->u.array.len = 0;
if (parseChar(source, '[') != 0)
return -1;
skipWhiteSpaces(source);
if (**source == ']')
{
++*source;
return 0;
}
while (**source)
{
skipWhiteSpaces(source);
if (parseArrayEntry(source, value) != 0)
return -1;
skipWhiteSpaces(source);
if (**source == ',')
{
++*source;
continue;
}
if (**source == ']')
{
++*source;
finished = 1;
break;
}
return -1;
}
return finished ? 0 : -1;
}
static int parseString(const char **source, struct JsonVal *value)
{
value->type = jsonStringT;
return parseQuotedString(source, &value->u.string);
}
static int parseTrue(const char **source, struct JsonVal *value)
{
value->type = jsonTrueT;
return matchString(source, "true");
}
static int parseFalse(const char **source, struct JsonVal *value)
{
value->type = jsonFalseT;
return matchString(source, "false");
}
static int parseNum(const char **source, struct JsonVal *value)
{
value->type = jsonNumberT;
value->u.number = strtold(*source, (char**)source);
return 0;
}
static int parseNull(const char **source, struct JsonVal *value)
{
value->type = jsonNullT;
return matchString(source, "null");
}
static int (*getParser(char c))(const char **, struct JsonVal*)
{
switch (tolower(c))
{
case '[':
return &parseArray;
case '{':
return &parseObject;
case '"':
return &parseString;
case 'f':
return &parseFalse;
case 't':
return &parseTrue;
default:
if (isdigit(c))
return &parseNum;
else if (tolower(c) == 'n')
return &parseNull;
return NULL;
}
return NULL;
}
static int parseValue(const char **source, struct JsonVal *val)
{
int (*valueParser)(const char **str, struct JsonVal *val) = getParser(**source);
if (valueParser == NULL)
return -1;
return valueParser(source, val);
}
static int parseKeyValue(const char **source, struct JsonVal *val)
{
assert(val->type == jsonObjectT);
if(reallocObject(val) != 0)
return -1;
if (parseQuotedString(source, val->u.object.keys + val->u.object.len - 1) != 0)
return -1;
if (parseChar(source, ':') != 0)
return -1;
skipWhiteSpaces(source);
return parseValue(source, val->u.object.values + val->u.object.len - 1);
}
static int parseObject(const char **source, struct JsonVal *value)
{
int finished = 0;
value->type = jsonObjectT;
value->u.object.keys = NULL;
value->u.object.values = NULL;
value->u.object.len = 0;
skipWhiteSpaces(source);
if (parseChar(source, '{') != 0)
return -1;
skipWhiteSpaces(source);
if (**source == '}')
{
++*source;
return 0;
}
while (**source)
{
skipWhiteSpaces(source);
if (parseKeyValue(source, value) != 0)
return -1;
skipWhiteSpaces(source);
if (**source == ',')
{
++*source;
continue;
}
if (**source == '}')
{
++*source;
finished = 1;
break;
}
return -1;
}
return finished ? 0 : -1;
}
struct JsonVal jsonParseString(const char *str, char *errorBuf, int errorBufSize)
{
struct JsonVal result;
const char *_str = str;
result = (struct JsonVal){jsonNullT};
if (parseObject(&_str, &result) != 0)
fillError(_str, errorBuf, errorBufSize);
else
memset(errorBuf, 0, errorBufSize);
return result;
}
int JsonVal_isString(const struct JsonVal *val)
{
return val->type == jsonStringT;
}
int JsonVal_isNumber(const struct JsonVal *val)
{
return val->type == jsonNumberT;
}
int JsonVal_isObject(const struct JsonVal *val)
{
return val->type == jsonObjectT;
}
int JsonVal_isArray(const struct JsonVal *val)
{
return val->type == jsonArrayT;
}
int JsonVal_isTrue(const struct JsonVal *val)
{
return val->type == jsonTrueT;
}
int JsonVal_isFalse(const struct JsonVal *val)
{
return val->type == jsonFalseT;
}
int JsonVal_isNull(const struct JsonVal *val)
{
return val->type == jsonNullT;
}
int JsonVal_arrayLen(struct JsonVal *val)
{
assert(val->type == jsonArrayT);
if (val->type != jsonArrayT)
return -1;
return val->u.array.len;
}
struct JsonVal *JsonVal_arrayAt(struct JsonVal *val, int index)
{
assert(val->type == jsonArrayT);
if (val->type != jsonArrayT || index >= val->u.array.len)
return NULL;
return &val->u.array.values[index];
}
struct JsonVal *JsonVal_getObjectValueByKey(const struct JsonVal *val, const char *key)
{
int i;
assert(val->type == jsonObjectT);
if (val->type != jsonObjectT)
return NULL;
for (i = 0; i < val->u.object.len; ++i)
{
if (strcmp(key, val->u.object.keys[i]) == 0)
return &val->u.object.values[i];
}
return NULL;
}
void JsonVal_forEachArrayElement(
const struct JsonVal *val,
void *ctx,
void (*action)(void *, const struct JsonVal *))
{
int i;
assert(val->type == jsonArrayT);
if (val->type != jsonArrayT)
return;
for (i = 0; i < val->u.array.len; ++i)
action(ctx, &val->u.array.values[i]);
}
void JsonVal_forEachObjectElement(
const struct JsonVal *val,
void *ctx,
void (*action)(void *, const char *key, const struct JsonVal *))
{
int i;
assert(val->type == jsonObjectT);
if (val->type != jsonObjectT)
return;
for (i = 0; i < val->u.object.len; ++i)
action(ctx, val->u.object.keys[i], &val->u.object.values[i]);
}
struct JsonVal jsonCreateObject()
{
struct JsonVal result;
result.type = jsonObjectT;
memset(&result.u, 0, sizeof(result.u));
return result;
}
static struct JsonVal *prepareNewObjectVal(struct JsonVal *val, const char *key)
{
struct JsonVal *newValPtr;
assert(val->type == jsonObjectT);
if (val->type != jsonObjectT)
return NULL;
if (reallocObject(val) != 0)
return NULL;
if (createStringCopy(key, strlen(key), val->u.object.keys + val->u.object.len - 1) != 0)
return NULL;
newValPtr = val->u.object.values + val->u.object.len - 1;
memset(newValPtr, 0, sizeof(*newValPtr));
return newValPtr;
}
struct JsonVal *JsonVal_objectAddString(struct JsonVal *val, const char *key, const char *value)
{
struct JsonVal *newValPtr = prepareNewObjectVal(val, key);
if (newValPtr == NULL)
return NULL;
if (createStringCopy(value, strlen(value), &newValPtr->u.string) != 0)
return NULL;
newValPtr->type = jsonStringT;
return newValPtr;
}
struct JsonVal *JsonVal_objectAddNumber(struct JsonVal *val, const char *key, long double number)
{
struct JsonVal *newValPtr = prepareNewObjectVal(val, key);
if (newValPtr == NULL)
return NULL;
newValPtr->u.number = number;
newValPtr->type = jsonNumberT;
return newValPtr;
}
struct JsonVal *JsonVal_objectAddObject(struct JsonVal *val, const char *key)
{
struct JsonVal *newValPtr = prepareNewObjectVal(val, key);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonObjectT;
return newValPtr;
}
struct JsonVal *JsonVal_objectAddArray(struct JsonVal *val, const char *key)
{
struct JsonVal *newValPtr = prepareNewObjectVal(val, key);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonArrayT;
return newValPtr;
}
struct JsonVal *JsonVal_objectAddTrue(struct JsonVal *val, const char *key)
{
struct JsonVal *newValPtr = prepareNewObjectVal(val, key);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonTrueT;
return newValPtr;
}
struct JsonVal *JsonVal_objectAddFalse(struct JsonVal *val, const char *key)
{
struct JsonVal *newValPtr = prepareNewObjectVal(val, key);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonFalseT;
return newValPtr;
}
struct JsonVal *JsonVal_objectAddNull(struct JsonVal *val, const char *key)
{
struct JsonVal *newValPtr = prepareNewObjectVal(val, key);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonNullT;
return newValPtr;
}
static struct JsonVal *prepareNewArrayVal(struct JsonVal *val)
{
struct JsonVal *newValPtr;
assert(val->type == jsonArrayT);
if (val->type != jsonArrayT)
return NULL;
if (reallocArray(val) != 0)
return NULL;
newValPtr = val->u.array.values + val->u.array.len - 1;
memset(newValPtr, 0, sizeof(*newValPtr));
return newValPtr;
}
struct JsonVal *JsonVal_arrayAddString(struct JsonVal *val, const char *value)
{
struct JsonVal *newValPtr = prepareNewArrayVal(val);
if (newValPtr == NULL)
return NULL;
if (createStringCopy(value, strlen(value), &newValPtr->u.string) != 0)
return NULL;
newValPtr->type = jsonStringT;
return newValPtr;
}
struct JsonVal *JsonVal_arrayAddNumber(struct JsonVal *val, long double number)
{
struct JsonVal *newValPtr = prepareNewArrayVal(val);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonNumberT;
newValPtr->u.number = number;
return newValPtr;
}
struct JsonVal *JsonVal_arrayAddObject(struct JsonVal *val)
{
struct JsonVal *newValPtr = prepareNewArrayVal(val);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonObjectT;
return newValPtr;
}
struct JsonVal *JsonVal_arrayAddArray(struct JsonVal *val)
{
struct JsonVal *newValPtr = prepareNewArrayVal(val);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonArrayT;
return newValPtr;
}
struct JsonVal *JsonVal_arrayAddTrue(struct JsonVal *val)
{
struct JsonVal *newValPtr = prepareNewArrayVal(val);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonTrueT;
return newValPtr;
}
struct JsonVal *JsonVal_arrayAddFalse(struct JsonVal *val)
{
struct JsonVal *newValPtr = prepareNewArrayVal(val);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonFalseT;
return newValPtr;
}
struct JsonVal *JsonVal_arrayAddNull(struct JsonVal *val)
{
struct JsonVal *newValPtr = prepareNewArrayVal(val);
if (newValPtr == NULL)
return NULL;
newValPtr->type = jsonNullT;
return newValPtr;
}
/*
write
*/
struct Buf
{
char *buf;
int spaceLeft;
};
static const int kWriteBufSize = 1024;
static int writeToBuf(void *ctx, void *source, int len)
{
struct Buf *target = (struct Buf *)ctx;
int bytesToWrite = len >= target->spaceLeft ? target->spaceLeft : len;
memcpy(target->buf, source, bytesToWrite);
target->spaceLeft -= bytesToWrite;
target->buf += bytesToWrite;
return bytesToWrite;
}
static void writeChar(
char c, void *ctx,
char *writeBuf,
int (*writeFunc)(void *ctx, void *buf, int len))
{
writeBuf[0] = c;
writeFunc(ctx, writeBuf, 1);
}
static void writeString(
const char *s, void *ctx,
char *writeBuf,
int (*writeFunc)(void *ctx, void *buf, int len))
{
int writeLen = strlen(s);
int bytesToWrite;
while (writeLen > 0)
{
bytesToWrite = writeLen < kWriteBufSize ? writeLen : kWriteBufSize;
writeFunc(ctx, (void *)s, bytesToWrite);
writeLen -= bytesToWrite;
s += bytesToWrite;
}
}
static void writeObject(
const struct JsonVal *val,
void *ctx, char *writeBuf,
int (*writeFunc)(void *ctx, void *buf, int len));
static void writeValue(
const struct JsonVal *val,
void *ctx, char *writeBuf,
int (*writeFunc)(void *ctx, void *buf, int len));
static void writeQuotedString(
const char *s, void *ctx,
char *writeBuf,
int (*writeFunc)(void *ctx, void *buf, int len))
{
writeBuf[0] = '"';
writeFunc(ctx, writeBuf, 1);
writeString(s, ctx, writeBuf, writeFunc);
writeBuf[0] = '"';
writeFunc(ctx, writeBuf, 1);
}
static void writeNumber(
long double num,
void *ctx, char *writeBuf,
int (*writeFunc)(void *ctx, void *buf, int len))
{
char *dot;
char *end;
sprintf(writeBuf, "%.5Lf", num);
dot = strchr(writeBuf, '.');
end = writeBuf + strlen(writeBuf) - 1;
for (; end != dot; --end)
if (*end != '0')
break;
if (end == dot)
*end = '\0';
else
*(++end) ='\0';
writeString(writeBuf, ctx, NULL, writeFunc);
}
static void writeArray(
const struct JsonVal *val,
void *ctx, char *writeBuf,
int (*writeFunc)(void *ctx, void *buf, int len))
{
int i;
writeChar('[', ctx, writeBuf, writeFunc);
for (i = 0; i < val->u.array.len; ++i)
{
writeValue(&val->u.array.values[i], ctx, writeBuf, writeFunc);
if (i != val->u.array.len - 1)
writeChar(',', ctx, writeBuf, writeFunc);
}
writeChar(']', ctx, writeBuf, writeFunc);
}
static void writeTrue(void *ctx, char *writeBuf, int (*writeFunc)(void *ctx, void *buf, int len))
{
writeString("true", ctx, writeBuf, writeFunc);
}
static void writeFalse(void *ctx, char *writeBuf, int (*writeFunc)(void *ctx, void *buf, int len))
{
writeString("false", ctx, writeBuf, writeFunc);
}
static void writeNull(void *ctx, char *writeBuf, int (*writeFunc)(void *ctx, void *buf, int len))
{
writeString("null", ctx, writeBuf, writeFunc);
}
static void writeValue(
const struct JsonVal *val,
void *ctx, char *writeBuf,
int (*writeFunc)(void *ctx, void *buf, int len))
{
switch (val->type)
{
case jsonStringT:
writeQuotedString(val->u.string, ctx, writeBuf, writeFunc);
break;
case jsonNumberT:
writeNumber(val->u.number, ctx, writeBuf, writeFunc);
break;
case jsonObjectT:
writeObject(val, ctx, writeBuf, writeFunc);
break;
case jsonArrayT:
writeArray(val, ctx, writeBuf, writeFunc);
break;
case jsonTrueT:
writeTrue(ctx, writeBuf, writeFunc);
break;
case jsonFalseT:
writeFalse(ctx, writeBuf, writeFunc);
break;
case jsonNullT:
writeNull(ctx, writeBuf, writeFunc);
break;
}
}
static void writeObject(
const struct JsonVal *val,
void *ctx, char *writeBuf,
int (*writeFunc)(void *ctx, void *buf, int len))
{
int i;
writeChar('{', ctx, writeBuf, writeFunc);
for (i = 0; i < val->u.object.len; ++i)
{
writeQuotedString(val->u.object.keys[i], ctx, writeBuf, writeFunc);
writeChar(':', ctx, writeBuf, writeFunc);
writeValue(&val->u.object.values[i], ctx, writeBuf, writeFunc);
if (i != val->u.object.len - 1)
writeChar(',', ctx, writeBuf, writeFunc);
}
writeChar('}', ctx, writeBuf, writeFunc);
}
struct CountingContext
{
void *userCtx;
int (*userWriteFunc)(void *ctx, void *buf, int len);
int totalWrittenBytes;
};
static int countingWriteWrapper(void *ctx, void *buf, int len)
{
struct CountingContext *countingCtx = (struct CountingContext *)ctx;
countingCtx->totalWrittenBytes += len;
return countingCtx->userWriteFunc(countingCtx->userCtx, buf, len);
}
int JsonVal_write(
const struct JsonVal *val, void *ctx,
int (*writeFunc)(void *ctx, void *buf, int len))
{
char writeBuf[kWriteBufSize];
struct CountingContext countingCtx = {ctx, writeFunc, 0};
writeObject(val, &countingCtx, writeBuf, &countingWriteWrapper);
writeBuf[0] = '\0';
countingWriteWrapper(&countingCtx, writeBuf, 1);
return countingCtx.totalWrittenBytes;
}
int JsonVal_writeString(const struct JsonVal *val, char *buf, int len)
{
struct Buf ctx = {buf, len};
return JsonVal_write(val, &ctx, &writeToBuf);
}
void JsonVal_destroy(struct JsonVal *val)
{
int i;
switch (val->type)
{
case jsonStringT:
free(val->u.string);
break;
case jsonObjectT:
for (i = 0; i < val->u.object.len; ++i)
{
JsonVal_destroy(&val->u.object.values[i]);
free(val->u.object.keys[i]);
}
free(val->u.object.values);
free(val->u.object.keys);
break;
case jsonArrayT:
for (i = 0; i < val->u.array.len; ++i)
JsonVal_destroy(&val->u.array.values[i]);
free(val->u.array.values);
break;
default:
break;
}
}

94
json.h Normal file
View File

@@ -0,0 +1,94 @@
#pragma once
#include <stddef.h>
enum JsonType
{
jsonStringT,
jsonNumberT,
jsonObjectT,
jsonArrayT,
jsonTrueT,
jsonFalseT,
jsonNullT,
};
struct JsonVal
{
enum JsonType type;
union
{
char *string;
long double number;
struct {
char **keys;
struct JsonVal *values;
int len;
} object;
struct {
struct JsonVal *values;
int len;
} array;
} u;
};
struct JsonVal jsonParseString(const char *str, char *errorBuf, int errorBufSize);
/*creates empty (top-level) object*/
struct JsonVal jsonCreateObject();
/*check type functions*/
int JsonVal_isString(const struct JsonVal *val);
int JsonVal_isNumber(const struct JsonVal *val);
int JsonVal_isObject(const struct JsonVal *val);
int JsonVal_isArray(const struct JsonVal *val);
int JsonVal_isTrue(const struct JsonVal *val);
int JsonVal_isFalse(const struct JsonVal *val);
int JsonVal_isNull(const struct JsonVal *val);
/*add subvalues to existing values. return pointers to the newly created values.*/
struct JsonVal *JsonVal_objectAddString(struct JsonVal *val, const char *key, const char *value);
struct JsonVal *JsonVal_objectAddNumber(struct JsonVal *val, const char *key, long double number);
struct JsonVal *JsonVal_objectAddObject(struct JsonVal *val, const char *key);
struct JsonVal *JsonVal_objectAddArray(struct JsonVal *val, const char *key);
struct JsonVal *JsonVal_objectAddTrue(struct JsonVal *val, const char *key);
struct JsonVal *JsonVal_objectAddFalse(struct JsonVal *val, const char *key);
struct JsonVal *JsonVal_objectAddNull(struct JsonVal *val, const char *key);
struct JsonVal *JsonVal_arrayAddString(struct JsonVal *val, const char *value);
struct JsonVal *JsonVal_arrayAddNumber(struct JsonVal *val, long double number);
struct JsonVal *JsonVal_arrayAddObject(struct JsonVal *val);
struct JsonVal *JsonVal_arrayAddArray(struct JsonVal *val);
struct JsonVal *JsonVal_arrayAddTrue(struct JsonVal *val);
struct JsonVal *JsonVal_arrayAddFalse(struct JsonVal *val);
struct JsonVal *JsonVal_arrayAddNull(struct JsonVal *val);
/*access functions*/
int JsonVal_arrayLen(struct JsonVal *val);
struct JsonVal *JsonVal_arrayAt(struct JsonVal* val, int index);
struct JsonVal *JsonVal_getObjectValueByKey(const struct JsonVal *val, const char *key);
void JsonVal_forEachArrayElement(
const struct JsonVal *val,
void *ctx,
void (*action)(void *, const struct JsonVal *));
void JsonVal_forEachObjectElement(
const struct JsonVal *val,
void *ctx,
void (*action)(void *, const char *key, const struct JsonVal *));
/*write functions*/
int JsonVal_write(
const struct JsonVal *val,
/*User context. Will be passed to the writeFunc callback.*/
void *ctx,
/*user supplied write callback function.*/
int (*writeFunc)(void *ctx, void *buf, int len));
/*returns total bytes required*/
int JsonVal_writeString(const struct JsonVal *val, char *buf, int len);
void JsonVal_destroy(struct JsonVal *val);

279
l2cap.h Normal file
View File

@@ -0,0 +1,279 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2000-2001 Qualcomm Incorporated
* Copyright (C) 2002-2003 Maxim Krasnyansky <maxk@qualcomm.com>
* Copyright (C) 2002-2010 Marcel Holtmann <marcel@holtmann.org>
* Copyright (c) 2012 Code Aurora Forum. All rights reserved.
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifndef __L2CAP_H
#define __L2CAP_H
#ifdef __cplusplus
extern "C" {
#endif
#include <sys/socket.h>
/* L2CAP defaults */
#define L2CAP_DEFAULT_MTU 672
#define L2CAP_DEFAULT_FLUSH_TO 0xFFFF
/* L2CAP socket address */
struct sockaddr_l2 {
sa_family_t l2_family;
unsigned short l2_psm;
bdaddr_t l2_bdaddr;
unsigned short l2_cid;
uint8_t l2_bdaddr_type;
};
/* L2CAP socket options */
#define L2CAP_OPTIONS 0x01
struct l2cap_options {
uint16_t omtu;
uint16_t imtu;
uint16_t flush_to;
uint8_t mode;
uint8_t fcs;
uint8_t max_tx;
uint16_t txwin_size;
};
#define L2CAP_CONNINFO 0x02
struct l2cap_conninfo {
uint16_t hci_handle;
uint8_t dev_class[3];
};
#define L2CAP_LM 0x03
#define L2CAP_LM_MASTER 0x0001
#define L2CAP_LM_AUTH 0x0002
#define L2CAP_LM_ENCRYPT 0x0004
#define L2CAP_LM_TRUSTED 0x0008
#define L2CAP_LM_RELIABLE 0x0010
#define L2CAP_LM_SECURE 0x0020
/* L2CAP command codes */
#define L2CAP_COMMAND_REJ 0x01
#define L2CAP_CONN_REQ 0x02
#define L2CAP_CONN_RSP 0x03
#define L2CAP_CONF_REQ 0x04
#define L2CAP_CONF_RSP 0x05
#define L2CAP_DISCONN_REQ 0x06
#define L2CAP_DISCONN_RSP 0x07
#define L2CAP_ECHO_REQ 0x08
#define L2CAP_ECHO_RSP 0x09
#define L2CAP_INFO_REQ 0x0a
#define L2CAP_INFO_RSP 0x0b
#define L2CAP_CREATE_REQ 0x0c
#define L2CAP_CREATE_RSP 0x0d
#define L2CAP_MOVE_REQ 0x0e
#define L2CAP_MOVE_RSP 0x0f
#define L2CAP_MOVE_CFM 0x10
#define L2CAP_MOVE_CFM_RSP 0x11
/* L2CAP extended feature mask */
#define L2CAP_FEAT_FLOWCTL 0x00000001
#define L2CAP_FEAT_RETRANS 0x00000002
#define L2CAP_FEAT_BIDIR_QOS 0x00000004
#define L2CAP_FEAT_ERTM 0x00000008
#define L2CAP_FEAT_STREAMING 0x00000010
#define L2CAP_FEAT_FCS 0x00000020
#define L2CAP_FEAT_EXT_FLOW 0x00000040
#define L2CAP_FEAT_FIXED_CHAN 0x00000080
#define L2CAP_FEAT_EXT_WINDOW 0x00000100
#define L2CAP_FEAT_UCD 0x00000200
/* L2CAP fixed channels */
#define L2CAP_FC_L2CAP 0x02
#define L2CAP_FC_CONNLESS 0x04
#define L2CAP_FC_A2MP 0x08
/* L2CAP structures */
typedef struct {
uint16_t len;
uint16_t cid;
} __attribute__ ((packed)) l2cap_hdr;
#define L2CAP_HDR_SIZE 4
typedef struct {
uint8_t code;
uint8_t ident;
uint16_t len;
} __attribute__ ((packed)) l2cap_cmd_hdr;
#define L2CAP_CMD_HDR_SIZE 4
typedef struct {
uint16_t reason;
} __attribute__ ((packed)) l2cap_cmd_rej;
#define L2CAP_CMD_REJ_SIZE 2
typedef struct {
uint16_t psm;
uint16_t scid;
} __attribute__ ((packed)) l2cap_conn_req;
#define L2CAP_CONN_REQ_SIZE 4
typedef struct {
uint16_t dcid;
uint16_t scid;
uint16_t result;
uint16_t status;
} __attribute__ ((packed)) l2cap_conn_rsp;
#define L2CAP_CONN_RSP_SIZE 8
/* connect result */
#define L2CAP_CR_SUCCESS 0x0000
#define L2CAP_CR_PEND 0x0001
#define L2CAP_CR_BAD_PSM 0x0002
#define L2CAP_CR_SEC_BLOCK 0x0003
#define L2CAP_CR_NO_MEM 0x0004
/* connect status */
#define L2CAP_CS_NO_INFO 0x0000
#define L2CAP_CS_AUTHEN_PEND 0x0001
#define L2CAP_CS_AUTHOR_PEND 0x0002
typedef struct {
uint16_t dcid;
uint16_t flags;
uint8_t data[0];
} __attribute__ ((packed)) l2cap_conf_req;
#define L2CAP_CONF_REQ_SIZE 4
typedef struct {
uint16_t scid;
uint16_t flags;
uint16_t result;
uint8_t data[0];
} __attribute__ ((packed)) l2cap_conf_rsp;
#define L2CAP_CONF_RSP_SIZE 6
#define L2CAP_CONF_SUCCESS 0x0000
#define L2CAP_CONF_UNACCEPT 0x0001
#define L2CAP_CONF_REJECT 0x0002
#define L2CAP_CONF_UNKNOWN 0x0003
#define L2CAP_CONF_PENDING 0x0004
#define L2CAP_CONF_EFS_REJECT 0x0005
typedef struct {
uint8_t type;
uint8_t len;
uint8_t val[0];
} __attribute__ ((packed)) l2cap_conf_opt;
#define L2CAP_CONF_OPT_SIZE 2
#define L2CAP_CONF_MTU 0x01
#define L2CAP_CONF_FLUSH_TO 0x02
#define L2CAP_CONF_QOS 0x03
#define L2CAP_CONF_RFC 0x04
#define L2CAP_CONF_FCS 0x05
#define L2CAP_CONF_EFS 0x06
#define L2CAP_CONF_EWS 0x07
#define L2CAP_CONF_MAX_SIZE 22
#define L2CAP_MODE_BASIC 0x00
#define L2CAP_MODE_RETRANS 0x01
#define L2CAP_MODE_FLOWCTL 0x02
#define L2CAP_MODE_ERTM 0x03
#define L2CAP_MODE_STREAMING 0x04
#define L2CAP_SERVTYPE_NOTRAFFIC 0x00
#define L2CAP_SERVTYPE_BESTEFFORT 0x01
#define L2CAP_SERVTYPE_GUARANTEED 0x02
typedef struct {
uint16_t dcid;
uint16_t scid;
} __attribute__ ((packed)) l2cap_disconn_req;
#define L2CAP_DISCONN_REQ_SIZE 4
typedef struct {
uint16_t dcid;
uint16_t scid;
} __attribute__ ((packed)) l2cap_disconn_rsp;
#define L2CAP_DISCONN_RSP_SIZE 4
typedef struct {
uint16_t type;
} __attribute__ ((packed)) l2cap_info_req;
#define L2CAP_INFO_REQ_SIZE 2
typedef struct {
uint16_t type;
uint16_t result;
uint8_t data[0];
} __attribute__ ((packed)) l2cap_info_rsp;
#define L2CAP_INFO_RSP_SIZE 4
/* info type */
#define L2CAP_IT_CL_MTU 0x0001
#define L2CAP_IT_FEAT_MASK 0x0002
/* info result */
#define L2CAP_IR_SUCCESS 0x0000
#define L2CAP_IR_NOTSUPP 0x0001
typedef struct {
uint16_t psm;
uint16_t scid;
uint8_t id;
} __attribute__ ((packed)) l2cap_create_req;
#define L2CAP_CREATE_REQ_SIZE 5
typedef struct {
uint16_t dcid;
uint16_t scid;
uint16_t result;
uint16_t status;
} __attribute__ ((packed)) l2cap_create_rsp;
#define L2CAP_CREATE_RSP_SIZE 8
typedef struct {
uint16_t icid;
uint8_t id;
} __attribute__ ((packed)) l2cap_move_req;
#define L2CAP_MOVE_REQ_SIZE 3
typedef struct {
uint16_t icid;
uint16_t result;
} __attribute__ ((packed)) l2cap_move_rsp;
#define L2CAP_MOVE_RSP_SIZE 4
typedef struct {
uint16_t icid;
uint16_t result;
} __attribute__ ((packed)) l2cap_move_cfm;
#define L2CAP_MOVE_CFM_SIZE 4
typedef struct {
uint16_t icid;
} __attribute__ ((packed)) l2cap_move_cfm_rsp;
#define L2CAP_MOVE_CFM_RSP_SIZE 2
#ifdef __cplusplus
}
#endif
#endif /* __L2CAP_H */

936
main.c Normal file
View File

@@ -0,0 +1,936 @@
#include <stdio.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>
#include <getopt.h>
#include <limits.h>
#include <errno.h>
#include "mqtt.h"
#include "client.h"
#include <assert.h>
#include "bluetooth.h"
#include "hci.h"
#include "hci_lib.h"
#include "l2cap.h"
#include "uuid.h"
#include <sys/ioctl.h>
#include "mainloop.h"
#include "util.h"
#include "att.h"
#include "queue.h"
#include "gatt-db.h"
#include "gatt-client.h"
#include "json.h"
#define ATT_CID 4
#define PRLOG(...) \
printf(__VA_ARGS__);
#define JSONOUTLEN 4096
#define MQTT_SEND_PERIOD_S 10
static uint16_t mqtt_port = 1883;
static uint16_t send_interval = MQTT_SEND_PERIOD_S;
static bool verbose = false;
static int handler_registered=0;
struct client *cli;
uint8_t rcv_buf[256];
char jsonout[JSONOUTLEN];
struct JsonVal top;
uint8_t rcv_ptr=0, frame_len=0;
uint16_t addr_base=0;
char mqtt_buffer[BUFFER_SIZE_BYTES];
client_t c;
//int is_subscriber = 1;
int keepalive_sec = 4;
int nbytes;
struct client {
/// socket
int fd;
/// pointer to a bt_att structure
struct bt_att *att;
/// pointer to a gatt_db structure
struct gatt_db *db;
/// pointer to a bt_gatt_client structure
struct bt_gatt_client *gatt;
/// session id
unsigned int reliable_session_id;
};
static void got_connection(client_t* psClnt)
{
uint8_t buf[128];
require(psClnt != 0);
if(!psClnt->sockfd) return;
printf("MQTT: connected to server.\n");
nbytes = mqtt_encode_connect_msg2(buf, 0x02, keepalive_sec, (uint8_t*)"Solarlife", 9);
client_send(&c, (char*)buf, nbytes);
}
static void lost_connection(client_t* psClnt)
{
require(psClnt != 0);
printf("MQTT: disconnected from server.\n");
mainloop_quit();
}
static void got_data(client_t* psClnt, unsigned char* data, int nbytes)
{
require(psClnt != 0);
uint8_t ctrl = data[0] >> 4;
if (ctrl == CTRL_PUBACK) {
printf("MQTT server acknowledged data\n");
}
}
int16_t b2int (uint8_t *ptr) {
return ptr[1] | ptr[0] << 8;
}
int b2int2 (uint8_t *ptr) {
return ptr[0] | ptr[1] << 8;
}
uint16_t ModRTU_CRC(uint8_t* buf, int len)
{
uint16_t crc = 0xFFFF;
for (int pos = 0; pos < len; pos++) {
crc ^= (uint16_t)buf[pos]; // XOR byte into least sig. byte of crc
for (int i = 8; i != 0; i--) { // Loop over each bit
if ((crc & 0x0001) != 0) { // If the LSB is set
crc >>= 1; // Shift right and XOR 0xA001
crc ^= 0xA001;
}
else // Else LSB is not set
crc >>= 1; // Just shift right
}
}
// Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes)
return crc;
}
static void write_cb(bool success, uint8_t att_ecode, void *user_data)
{
/*
if (success) {
PRLOG("\nWrite successful\n");
} else {
PRLOG("\nWrite failed: %s (0x%02x)\n",
ecode_to_string(att_ecode), att_ecode);
}
*/
}
int send_modbus_req(uint8_t eq_id, uint8_t fct_code,uint16_t addr, uint16_t len) {
uint8_t buf[10];
buf[0]=eq_id;
buf[1]=fct_code;
buf[2]=addr>>8;
buf[3]=addr&0xff;
buf[4]=len>>8;
buf[5]=len&0xff;
uint16_t crc = ModRTU_CRC(buf,6);
buf[7]=crc>>8;
buf[6]=crc&0xff;
addr_base = addr;
rcv_ptr = 0; //reset pointer
top = jsonCreateObject();
if (!bt_gatt_client_write_value(cli->gatt, 0x14, buf, 8, write_cb, NULL, NULL)) {
printf("Failed to initiate write procedure\n");
return 1;
}
return 0;
}
static void log_service_event(struct gatt_db_attribute *attr, const char *str)
{
char uuid_str[MAX_LEN_UUID_STR];
bt_uuid_t uuid;
uint16_t start, end;
gatt_db_attribute_get_service_uuid(attr, &uuid);
bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str));
gatt_db_attribute_get_service_handles(attr, &start, &end);
PRLOG("%s - UUID: %s start: 0x%04x end: 0x%04x\n", str, uuid_str,
start, end);
}
static void register_notify_cb(uint16_t att_ecode, void *user_data)
{
if (att_ecode) {
PRLOG("Failed to register notify handler "
"- error code: 0x%02x\n", att_ecode);
return;
}
PRLOG("Registered notify handler!\n");
handler_registered = 1;
}
void parse_val(uint8_t *buf, uint16_t addr) {
//printf("Parsing value at address %04x\n",addr);
uint8_t ptr = addr*2+3;
addr+=addr_base;
switch (addr) {
case 0x3000:
JsonVal_objectAddNumber(&top, "PV_rated_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x3001:
JsonVal_objectAddNumber(&top, "PV_rated_current", (float)b2int(buf+ptr)/100);
break;
case 0x3002:
JsonVal_objectAddNumber(&top, "PV_rated_power_l", (float)b2int(buf+ptr)/100);
break;
case 0x3003:
JsonVal_objectAddNumber(&top, "PV_rated_power_h", (float)b2int(buf+ptr)/100);
break;
case 0x3004:
JsonVal_objectAddNumber(&top, "battery_rated_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x3005:
JsonVal_objectAddNumber(&top, "battery_rated_current", (float)b2int(buf+ptr)/100);
break;
case 0x3006:
JsonVal_objectAddNumber(&top, "battery_rated_power_l", (float)b2int(buf+ptr)/100);
break;
case 0x3007:
JsonVal_objectAddNumber(&top, "battery_rated_power_h", (float)b2int(buf+ptr)/100);
break;
case 0x3008:
JsonVal_objectAddNumber(&top, "load_rated_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x3009:
JsonVal_objectAddNumber(&top, "load_rated_current", (float)b2int(buf+ptr)/100);
break;
case 0x300a:
JsonVal_objectAddNumber(&top, "load_rated_power_l", (float)b2int(buf+ptr)/100);
break;
case 0x300b:
JsonVal_objectAddNumber(&top, "load_rated_power_h", (float)b2int(buf+ptr)/100);
break;
case 0x3030:
JsonVal_objectAddNumber(&top, "slave_id", (int)b2int(buf+ptr));
break;
case 0x3031:
JsonVal_objectAddNumber(&top, "running_days", (int)b2int(buf+ptr));
break;
case 0x3032:
JsonVal_objectAddNumber(&top, "sys_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x3033:
JsonVal_objectAddNumber(&top, "battery_status", b2int(buf+ptr));
break;
case 0x3034:
JsonVal_objectAddNumber(&top, "charge_status", b2int(buf+ptr));
break;
case 0x3035:
JsonVal_objectAddNumber(&top, "discharge_status", b2int(buf+ptr));
break;
case 0x3036:
JsonVal_objectAddNumber(&top, "env_temperature", (float)b2int(buf+ptr)/100);
break;
case 0x3037:
JsonVal_objectAddNumber(&top, "sys_temperature", (float)b2int(buf+ptr)/100);
break;
case 0x3038:
JsonVal_objectAddNumber(&top, "undervoltage_times", b2int(buf+ptr));
break;
case 0x3039:
JsonVal_objectAddNumber(&top, "fullycharged_times", b2int(buf+ptr));
break;
case 0x303a:
JsonVal_objectAddNumber(&top, "overvoltage_prot_times", b2int(buf+ptr));
break;
case 0x303b:
JsonVal_objectAddNumber(&top, "overcurrent_prot_times", b2int(buf+ptr));
break;
case 0x303c:
JsonVal_objectAddNumber(&top, "shortcircuit_prot_times", b2int(buf+ptr));
break;
case 0x303d:
JsonVal_objectAddNumber(&top, "opencircuit_prot_times", b2int(buf+ptr));
break;
case 0x303e:
JsonVal_objectAddNumber(&top, "hw_prot_times", b2int(buf+ptr));
break;
case 0x303f:
JsonVal_objectAddNumber(&top, "charge_overtemp_prot_times", b2int(buf+ptr));
break;
case 0x3040:
JsonVal_objectAddNumber(&top, "discharge_overtemp_prot_times", b2int(buf+ptr));
break;
case 0x3045:
JsonVal_objectAddNumber(&top, "battery_remaining_capacity", b2int(buf+ptr));
break;
case 0x3046:
JsonVal_objectAddNumber(&top, "battery_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x3047:
JsonVal_objectAddNumber(&top, "battery_current", (float)b2int(buf+ptr)/100);
//JsonVal_objectAddNumber(&top, "battery_current", b2int(buf+ptr)/100);
//printf("current: %d\n",b2int(buf+ptr));
break;
case 0x3048:
JsonVal_objectAddNumber(&top, "battery_power_lo", (float)b2int(buf+ptr)/100);
break;
case 0x3049:
JsonVal_objectAddNumber(&top, "battery_power_hi", (float)b2int(buf+ptr)/100);
break;
case 0x304a:
JsonVal_objectAddNumber(&top, "load_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x304b:
JsonVal_objectAddNumber(&top, "load_current", (float)b2int(buf+ptr)/100);
break;
case 0x304c:
JsonVal_objectAddNumber(&top, "load_power_l", (float)b2int(buf+ptr)/100);
break;
case 0x304d:
JsonVal_objectAddNumber(&top, "load_power_h", (float)b2int(buf+ptr)/100);
break;
case 0x304e:
JsonVal_objectAddNumber(&top, "solar_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x304f:
JsonVal_objectAddNumber(&top, "solar_current", (float)b2int(buf+ptr)/100);
break;
case 0x3050:
JsonVal_objectAddNumber(&top, "solar_power_l", (float)b2int(buf+ptr)/100);
break;
case 0x3051:
JsonVal_objectAddNumber(&top, "solar_power_h", (float)b2int(buf+ptr)/100);
break;
case 0x3052:
JsonVal_objectAddNumber(&top, "daily_production", (float)b2int(buf+ptr)/100);
break;
case 0x3053:
JsonVal_objectAddNumber(&top, "total_production_l", (float)b2int(buf+ptr)/100);
break;
case 0x3054:
JsonVal_objectAddNumber(&top, "total_production_h", (float)b2int(buf+ptr)/100);
break;
case 0x3055:
JsonVal_objectAddNumber(&top, "daily_consumption", (float)b2int(buf+ptr)/100);
break;
case 0x3056:
JsonVal_objectAddNumber(&top, "total_consumption_l", (float)b2int(buf+ptr)/100);
break;
case 0x3057:
JsonVal_objectAddNumber(&top, "total_consumption_h", (float)b2int(buf+ptr)/100);
break;
case 0x3058:
JsonVal_objectAddNumber(&top, "lighttime_daily", b2int(buf+ptr));
break;
case 0x305d:
JsonVal_objectAddNumber(&top, "monthly_production_l", (float)b2int(buf+ptr)/100);
break;
case 0x305e:
JsonVal_objectAddNumber(&top, "monthly_production_h", (float)b2int(buf+ptr)/100);
break;
case 0x305f:
JsonVal_objectAddNumber(&top, "yearly_production_l", (float)b2int(buf+ptr)/100);
break;
case 0x3060:
JsonVal_objectAddNumber(&top, "yearly_production_h", (float)b2int(buf+ptr)/100);
break;
case 0x9017:
JsonVal_objectAddNumber(&top, "rtc_second", b2int(buf+ptr));
break;
case 0x9018:
JsonVal_objectAddNumber(&top, "rtc_minute", b2int(buf+ptr));
break;
case 0x9019:
JsonVal_objectAddNumber(&top, "rtc_hour", b2int(buf+ptr));
break;
case 0x901a:
JsonVal_objectAddNumber(&top, "rtc_day", b2int(buf+ptr));
break;
case 0x901b:
JsonVal_objectAddNumber(&top, "rtc_month", b2int(buf+ptr));
break;
case 0x901c:
JsonVal_objectAddNumber(&top, "rtc_year", b2int(buf+ptr));
break;
case 0x901d:
JsonVal_objectAddNumber(&top, "baud_rate", b2int(buf+ptr));
break;
case 0x901f:
JsonVal_objectAddNumber(&top, "password", b2int(buf+ptr));
break;
case 0x9021:
JsonVal_objectAddNumber(&top, "battery_type", b2int(buf+ptr));
break;
case 0x9022:
JsonVal_objectAddNumber(&top, "lvp_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x9023:
JsonVal_objectAddNumber(&top, "lvr_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x9024:
JsonVal_objectAddNumber(&top, "boost_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x9025:
JsonVal_objectAddNumber(&top, "equalizing_voltage", (float)b2int(buf+ptr)/100);
break;
case 0x9026:
JsonVal_objectAddNumber(&top, "floating_voltage", (float)b2int(buf+ptr)/100);
break;
default:
break;
}
}
static void notify_cb(uint16_t value_handle, const uint8_t *value,
uint16_t length, void *user_data)
{
memcpy(rcv_buf+rcv_ptr, value, length);
rcv_ptr+=length;
if (rcv_ptr==rcv_buf[2]+5) {
uint16_t crc_r = b2int2(rcv_buf+rcv_ptr-2);
uint16_t crc = ModRTU_CRC(rcv_buf, rcv_ptr-2);
if((crc==crc_r) && addr_base) { //CRC check passed
for (int ii = 0; ii<(rcv_buf[2]-2)/2; ii++) {
parse_val(rcv_buf,ii);
}
time_t now;
time(&now);
char buf[sizeof "2011-10-08T07:07:09Z"];
strftime(buf, sizeof buf, "%FT%TZ", gmtime(&now));
JsonVal_objectAddString(&top, "timestamp", buf);
JsonVal_writeString(&top, jsonout, JSONOUTLEN);
puts(jsonout);
char buf2[8000];
nbytes = mqtt_encode_publish_msg((uint8_t*)buf2, (uint8_t*)"Solarlife",9 , 1, 10, (uint8_t*)jsonout, strlen(jsonout));
if(c.sockfd) {
client_send(&c, buf2, nbytes);
client_poll(&c, 1000000);
}
JsonVal_destroy(&top);
//if(addr_base<0x9000) {
// send_modbus_req(0x01, 0x03, 0x9000, 0x40);
// addr_base = 0;
//}
}
}
}
static void ready_cb(bool success, uint8_t att_ecode, void *user_data)
{
struct client *cli = user_data;
if (!success) {
PRLOG("GATT discovery procedures failed - error code: 0x%02x\n",
att_ecode);
return;
}
PRLOG("GATT discovery procedures complete\n");
unsigned char buf[] = {01,00};
if (!bt_gatt_client_write_value(cli->gatt, 0x12, buf, sizeof(buf),
write_cb,
NULL, NULL))
printf("Failed to initiate write procedure\n");
unsigned int id = bt_gatt_client_register_notify(cli->gatt, 0x11,
register_notify_cb,
notify_cb, NULL, NULL);
if (!id) {
printf("Failed to register notify handler\n");
}
PRLOG("Registering notify handler with id: %u\n", id);
}
static void service_removed_cb(struct gatt_db_attribute *attr, void *user_data)
{
log_service_event(attr, "Service Removed");
}
static void service_added_cb(struct gatt_db_attribute *attr, void *user_data)
{
log_service_event(attr, "Service Added");
}
static void att_disconnect_cb(int err, void *user_data)
{
printf("Device disconnected: %s\n", strerror(err));
mainloop_quit();
}
void send_mqtt_ping(void) {
unsigned char buf[100];
nbytes = mqtt_encode_ping_msg(buf);
if ((nbytes != 0) && (c.sockfd))
{
client_send(&c, (char*)buf, nbytes);
client_poll(&c, 0);
}
}
static void signal_cb(int signum, void *user_data)
{
static int sec_counter;
switch (signum) {
case SIGINT:
case SIGTERM:
mainloop_quit();
break;
case SIGALRM:
{
//if(addr_base) send_modbus_req(0x01, 0x03, 0x9000, 0x40);
//else {
// send_modbus_req(0x01, 0x04, 0x3000, 0x7c);
// addr_base=0;
//}
//uint16_t len = form_modbus_msg(buff,0x01, 0x03, 0x9000, 0x40);
//send_modbus_req(0x01, 0x04, 0x3000, 0x7c);
//send_modbus_req(0x01, 0x04, 0x3047, 0x20);
send_mqtt_ping();
if(handler_registered) {
if (++sec_counter>send_interval) {
send_modbus_req(0x01, 0x04, 0x3000, 0x7c);
sec_counter=0;
}
}
alarm(1);
break;
}
default:
break;
}
}
static void client_destroy(struct client *cli)
{
bt_gatt_client_unref(cli->gatt);
bt_att_unref(cli->att);
free(cli);
}
static struct client *client_create(int fd, uint16_t mtu)
{
struct client *cli;
cli = new0(struct client, 1);
if (!cli) {
fprintf(stderr, "Failed to allocate memory for client\n");
return NULL;
}
cli->att = bt_att_new(fd, false);
if (!cli->att) {
fprintf(stderr, "Failed to initialze ATT transport layer\n");
bt_att_unref(cli->att);
free(cli);
return NULL;
}
if (!bt_att_set_close_on_unref(cli->att, true)) {
fprintf(stderr, "Failed to set up ATT transport layer\n");
bt_att_unref(cli->att);
free(cli);
return NULL;
}
if (!bt_att_register_disconnect(cli->att, att_disconnect_cb, NULL,
NULL)) {
fprintf(stderr, "Failed to set ATT disconnect handler\n");
bt_att_unref(cli->att);
free(cli);
return NULL;
}
cli->fd = fd;
cli->db = gatt_db_new();
if (!cli->db) {
fprintf(stderr, "Failed to create GATT database\n");
bt_att_unref(cli->att);
free(cli);
return NULL;
}
cli->gatt = bt_gatt_client_new(cli->db, cli->att, mtu);
if (!cli->gatt) {
fprintf(stderr, "Failed to create GATT client\n");
gatt_db_unref(cli->db);
bt_att_unref(cli->att);
free(cli);
return NULL;
}
gatt_db_register(cli->db, service_added_cb, service_removed_cb,
NULL, NULL);
bt_gatt_client_set_ready_handler(cli->gatt, ready_cb, cli, NULL);
/* bt_gatt_client already holds a reference */
gatt_db_unref(cli->db);
return cli;
}
static int l2cap_le_att_connect(bdaddr_t *src, bdaddr_t *dst, uint8_t dst_type,
int sec)
{
int sock;
struct sockaddr_l2 srcaddr, dstaddr;
struct bt_security btsec;
sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
if (sock < 0) {
perror("Failed to create L2CAP socket");
return -1;
}
/* Set up source address */
memset(&srcaddr, 0, sizeof(srcaddr));
srcaddr.l2_family = AF_BLUETOOTH;
srcaddr.l2_cid = htobs(ATT_CID);
srcaddr.l2_bdaddr_type = 0;
bacpy(&srcaddr.l2_bdaddr, src);
if (bind(sock, (struct sockaddr *)&srcaddr, sizeof(srcaddr)) < 0) {
perror("Failed to bind L2CAP socket");
close(sock);
return -1;
}
/* Set the security level */
memset(&btsec, 0, sizeof(btsec));
btsec.level = sec;
if (setsockopt(sock, SOL_BLUETOOTH, BT_SECURITY, &btsec,
sizeof(btsec)) != 0) {
fprintf(stderr, "Failed to set L2CAP security level\n");
close(sock);
return -1;
}
/* Set up destination address */
memset(&dstaddr, 0, sizeof(dstaddr));
dstaddr.l2_family = AF_BLUETOOTH;
dstaddr.l2_cid = htobs(ATT_CID);
dstaddr.l2_bdaddr_type = dst_type;
bacpy(&dstaddr.l2_bdaddr, dst);
printf("Connecting to BT device...");
fflush(stdout);
if (connect(sock, (struct sockaddr *) &dstaddr, sizeof(dstaddr)) < 0) {
perror(" Failed to connect to BT device");
close(sock);
return -1;
}
printf(" Done\n");
return sock;
}
void inthandler(int dummy)
{
(void)dummy;
uint8_t buf[128];
int nbytes = mqtt_encode_disconnect_msg(buf);
if (nbytes != 0)
{
client_send(&c, (char*)buf, nbytes);
}
client_poll(&c, 10000000);
exit(0);
}
static void usage(char *argv[])
{
printf("%s\n",argv[0]);
printf("Usage:\n\t%s [options]\n",argv[0]);
printf("Options:\n"
"\t-i, --index <id>\t\tSpecify adapter index, e.g. hci0\n"
"\t-d, --dest <addr>\t\tSpecify the destination mac address of the Solarlife device\n"
"\t-a, --host <ipaddr>\t\tSpecify the MQTT broker address\n"
"\t-p, --port \t\t\tMQTT broker port, default: %d\n"
"\t-t, --interval \t\t\tSet data sending interval in seconds, default: %d s\n"
"\t-v, --verbose\t\t\tEnable extra logging\n"
"\t-h, --help\t\t\tDisplay help\n",mqtt_port, send_interval);
printf("Example:\n"
"%s -d C4:BE:84:70:29:04 -a test.mosquitto.org -t 10\n",argv[0]);
}
static struct option main_options[] = {
{ "index", 1, 0, 'i' },
{ "dest", 1, 0, 'd' },
{ "host", 1, 0, 'a' },
{ "port", 1, 0, 'p' },
{ "interval", 1, 0, 't' },
{ "verbose", 0, 0, 'v' },
{ "help", 0, 0, 'h' },
{ }
};
int main(int argc, char *argv[])
{
int opt;
int sec = BT_SECURITY_LOW;
uint16_t mtu = 0;
uint8_t dst_type = BDADDR_LE_PUBLIC;
bool dst_addr_given = false;
bdaddr_t src_addr, dst_addr;
int dev_id = -1;
int fd;
sigset_t mask;
char mqtt_host[100];
//str2ba("04:7f:0e:54:61:0c", &dst_addr);
while ((opt = getopt_long(argc, argv, "+hva:t:d:i:p:",
main_options, NULL)) != -1) {
switch (opt) {
case 'h':
usage(argv);
return EXIT_SUCCESS;
case 'v':
verbose = true;
break;
case 'a':
strncpy(mqtt_host,optarg,sizeof(mqtt_host));
break;
case 'p': {
int arg;
arg = atoi(optarg);
if (arg <= 0) {
fprintf(stderr, "Invalid MQTT port: %d\n", arg);
return EXIT_FAILURE;
}
if (arg > UINT16_MAX) {
fprintf(stderr, "MQTT port too large: %d\n", arg);
return EXIT_FAILURE;
}
mqtt_port = (uint16_t)arg;
break;
}
case 't': {
int arg;
arg = atoi(optarg);
if (arg <= 0) {
fprintf(stderr, "Invalid interval: %d s\n", arg);
return EXIT_FAILURE;
}
if (arg > UINT16_MAX) {
fprintf(stderr, "Interval too large: %d s\n", arg);
return EXIT_FAILURE;
}
if (arg < 1) {
fprintf(stderr, "Interval too small: %d s\n", arg);
return EXIT_FAILURE;
}
send_interval = (uint16_t)arg;
break;
}
case 'd':
if (str2ba(optarg, &dst_addr) < 0) {
fprintf(stderr, "Invalid Solarlife address: %s\n",
optarg);
return EXIT_FAILURE;
}
dst_addr_given = true;
break;
case 'i':
dev_id = hci_devid(optarg);
if (dev_id < 0) {
perror("Invalid adapter");
return EXIT_FAILURE;
}
break;
default:
//fprintf(stderr, "Invalid option: %c\n", opt);
return EXIT_FAILURE;
}
}
if (!argc) {
usage(argv);
return EXIT_SUCCESS;
}
argc -= optind;
argv += optind;
optind = 0;
if (argc) {
usage(argv);
return EXIT_SUCCESS;
}
if (dev_id == -1)
bacpy(&src_addr, BDADDR_ANY);
else if (hci_devba(dev_id, &src_addr) < 0) {
perror("Adapter not available");
return EXIT_FAILURE;
}
if (!dst_addr_given) {
fprintf(stderr, "Destination BT mac address required! Use -h option to print usage.\n");
return EXIT_FAILURE;
}
/* create the mainloop resources */
mainloop_init();
fd = l2cap_le_att_connect(&src_addr, &dst_addr, dst_type, sec);
if (fd < 0)
return EXIT_FAILURE;
cli = client_create(fd, mtu);
if (!cli) {
close(fd);
return EXIT_FAILURE;
}
if(strlen(mqtt_host)) {
client_init(&c, mqtt_host, mqtt_port, mqtt_buffer, BUFFER_SIZE_BYTES);
assert(client_set_callback(&c, CB_RECEIVED_DATA, got_data) == 1);
assert(client_set_callback(&c, CB_ON_CONNECTION, got_connection) == 1);
assert(client_set_callback(&c, CB_ON_DISCONNECT, lost_connection) == 1);
signal(SIGINT, inthandler);
client_connect(&c);
} else {
printf("MQTT Broker address not given, will not send data.\n");
}
alarm(1);
sigemptyset(&mask);
sigaddset(&mask, SIGINT);
sigaddset(&mask, SIGTERM);
sigaddset(&mask, SIGALRM);
/* add handler for process interrupted (SIGINT) or terminated (SIGTERM)*/
mainloop_set_signal(&mask, signal_cb, NULL, NULL);
mainloop_run();
printf("\n\nShutting down...\n");
if(c.sockfd) client_disconnect(&c);
client_destroy(cli);
return EXIT_SUCCESS;
}

481
mainloop.c Normal file
View File

@@ -0,0 +1,481 @@
/**
* @file mainloop.c
* @brief set of function to manage the epool loop
* @author Gilbert Brault
* @copyright Gilbert Brault 2015
* the original work comes from bluez v5.39
* value add: documenting main features
*
*/
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2011-2014 Intel Corporation
* Copyright (C) 2002-2010 Marcel Holtmann <marcel@holtmann.org>
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <stdio.h>
#include <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <sys/signalfd.h>
#include <sys/timerfd.h>
#include <sys/epoll.h>
#include "mainloop.h"
#define MAX_EPOLL_EVENTS 10
static int epoll_fd;
static int epoll_terminate;
static int exit_status;
/**
* @brief mainloop file descriptor event data structure
*/
struct mainloop_data {
/// socket or file descriptor, incl. standard i/o
int fd;
/// epoll event @see EPOLL_EVENTS_DOC
uint32_t events;
/// call back function(int fd, uint32_t events, void *user_data);
mainloop_event_func callback;
/// data management call back function(void *user_data);
mainloop_destroy_func destroy;
/// pointer to a user specific data structure
void *user_data;
};
#define MAX_MAINLOOP_ENTRIES 128
/**
* @brief array of file descriptor event stub
*/
static struct mainloop_data *mainloop_list[MAX_MAINLOOP_ENTRIES];
struct timeout_data {
int fd;
mainloop_timeout_func callback;
mainloop_destroy_func destroy;
void *user_data;
};
struct signal_data {
int fd;
sigset_t mask;
mainloop_signal_func callback;
mainloop_destroy_func destroy;
void *user_data;
};
static struct signal_data *signal_data;
/**
* create the epoll resource (epoll_fd global variable)
* initialize mainloop_list (global variable) event table
* set epoll_terminate to 0 (mainloop_run looping)
*/
void mainloop_init(void)
{
unsigned int i;
epoll_fd = epoll_create1(EPOLL_CLOEXEC);
for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++)
mainloop_list[i] = NULL;
epoll_terminate = 0;
}
/**
* set epoll_terminate to 1 (mainloop_run exit looping)
*/
void mainloop_quit(void)
{
epoll_terminate = 1;
}
/**
* set exit_status to EXIT_SUCCESS
* set epoll_terminate to 1 (mainloop_run exit looping)
*/
void mainloop_exit_success(void)
{
exit_status = EXIT_SUCCESS;
epoll_terminate = 1;
}
/**
* set exit_status to EXIT_FAILURE
* set epoll_terminate to 1 (mainloop_run exit looping)
*/
void mainloop_exit_failure(void)
{
exit_status = EXIT_FAILURE;
epoll_terminate = 1;
}
/**
*
* @param fd
* @param events
* @param user_data
*/
static void signal_callback(int fd, uint32_t events, void *user_data)
{
struct signal_data *data = user_data;
struct signalfd_siginfo si;
ssize_t result;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_quit();
return;
}
result = read(fd, &si, sizeof(si));
if (result != sizeof(si))
return;
if (data->callback)
data->callback(si.ssi_signo, data->user_data);
}
/**
* main loop wait for epoll events
* to exit the loop, set epoll_terminate to a <>0 value
* @see mainloop_exit_failure
* @see mainloop_exit_success
*
* @return exit_status EXIT_SUCCESS or EXIT_FAILURE
*/
int mainloop_run(void)
{
unsigned int i;
if (signal_data) {
if (sigprocmask(SIG_BLOCK, &signal_data->mask, NULL) < 0)
return EXIT_FAILURE;
signal_data->fd = signalfd(-1, &signal_data->mask,
SFD_NONBLOCK | SFD_CLOEXEC);
if (signal_data->fd < 0)
return EXIT_FAILURE;
if (mainloop_add_fd(signal_data->fd, EPOLLIN,
signal_callback, signal_data, NULL) < 0) {
close(signal_data->fd);
return EXIT_FAILURE;
}
}
exit_status = EXIT_SUCCESS;
while (!epoll_terminate) {
struct epoll_event events[MAX_EPOLL_EVENTS];
int n, nfds;
nfds = epoll_wait(epoll_fd, events, MAX_EPOLL_EVENTS, -1);
if (nfds < 0)
continue;
for (n = 0; n < nfds; n++) {
struct mainloop_data *data = events[n].data.ptr;
data->callback(data->fd, events[n].events,
data->user_data);
}
}
if (signal_data) {
mainloop_remove_fd(signal_data->fd);
close(signal_data->fd);
if (signal_data->destroy)
signal_data->destroy(signal_data->user_data);
}
for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++) {
struct mainloop_data *data = mainloop_list[i];
mainloop_list[i] = NULL;
if (data) {
epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL);
if (data->destroy)
data->destroy(data->user_data);
free(data);
}
}
close(epoll_fd);
epoll_fd = 0;
return exit_status;
}
/**
* trigger an event to be processed by the mainloop_run function
* and create a mainloop_list entry @see EPOLL_EVENTS_DOC for events description
*
* @param fd "file descriptor" source of the event (socket)
* @param events event flags EPOOL type like EPOLLIN, EPOLLOUT...
* @param callback function to call back by the event processor
* @param user_data associated data
* @param destroy management function to unallocate user_data
* @return 0 success else <0 error
*/
int mainloop_add_fd(int fd, uint32_t events, mainloop_event_func callback,
void *user_data, mainloop_destroy_func destroy)
{
struct mainloop_data *data;
struct epoll_event ev;
int err;
if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1 || !callback)
return -EINVAL;
data = malloc(sizeof(*data));
if (!data)
return -ENOMEM;
memset(data, 0, sizeof(*data));
data->fd = fd;
data->events = events;
data->callback = callback;
data->destroy = destroy;
data->user_data = user_data;
memset(&ev, 0, sizeof(ev));
ev.events = events;
ev.data.ptr = data;
err = epoll_ctl(epoll_fd, EPOLL_CTL_ADD, data->fd, &ev);
if (err < 0) {
free(data);
return err;
}
mainloop_list[fd] = data;
return 0;
}
/**
* trigger an epoll event for an existing mainloop socket (exisiting mainloop_list[fd])
* epool event "events" = events, "data.ptr" = mainloop_list[fd]
*
* @param fd socket
* @param events EPOLL event like EPOLLIN, EPOLLOUT...
* @return 0==Success <0 error
*/
int mainloop_modify_fd(int fd, uint32_t events)
{
struct mainloop_data *data;
struct epoll_event ev;
int err;
if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1)
return -EINVAL;
data = mainloop_list[fd];
if (!data)
return -ENXIO;
memset(&ev, 0, sizeof(ev));
ev.events = events;
ev.data.ptr = data;
err = epoll_ctl(epoll_fd, EPOLL_CTL_MOD, data->fd, &ev);
if (err < 0)
return err;
data->events = events;
return 0;
}
int mainloop_remove_fd(int fd)
{
struct mainloop_data *data;
int err;
if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1)
return -EINVAL;
data = mainloop_list[fd];
if (!data)
return -ENXIO;
mainloop_list[fd] = NULL;
err = epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL);
if (data->destroy)
data->destroy(data->user_data);
free(data);
return err;
}
static void timeout_destroy(void *user_data)
{
struct timeout_data *data = user_data;
close(data->fd);
data->fd = -1;
if (data->destroy)
data->destroy(data->user_data);
}
static void timeout_callback(int fd, uint32_t events, void *user_data)
{
struct timeout_data *data = user_data;
uint64_t expired;
ssize_t result;
if (events & (EPOLLERR | EPOLLHUP))
return;
result = read(data->fd, &expired, sizeof(expired));
if (result != sizeof(expired))
return;
if (data->callback)
data->callback(data->fd, data->user_data);
}
static inline int timeout_set(int fd, unsigned int msec)
{
struct itimerspec itimer;
unsigned int sec = msec / 1000;
memset(&itimer, 0, sizeof(itimer));
itimer.it_interval.tv_sec = 0;
itimer.it_interval.tv_nsec = 0;
itimer.it_value.tv_sec = sec;
itimer.it_value.tv_nsec = (msec - (sec * 1000)) * 1000;
return timerfd_settime(fd, 0, &itimer, NULL);
}
int mainloop_add_timeout(unsigned int msec, mainloop_timeout_func callback,
void *user_data, mainloop_destroy_func destroy)
{
struct timeout_data *data;
if (!callback)
return -EINVAL;
data = malloc(sizeof(*data));
if (!data)
return -ENOMEM;
memset(data, 0, sizeof(*data));
data->callback = callback;
data->destroy = destroy;
data->user_data = user_data;
data->fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK | TFD_CLOEXEC);
if (data->fd < 0) {
free(data);
return -EIO;
}
if (msec > 0) {
if (timeout_set(data->fd, msec) < 0) {
close(data->fd);
free(data);
return -EIO;
}
}
if (mainloop_add_fd(data->fd, EPOLLIN | EPOLLONESHOT,
timeout_callback, data, timeout_destroy) < 0) {
close(data->fd);
free(data);
return -EIO;
}
return data->fd;
}
int mainloop_modify_timeout(int id, unsigned int msec)
{
if (msec > 0) {
if (timeout_set(id, msec) < 0)
return -EIO;
}
if (mainloop_modify_fd(id, EPOLLIN | EPOLLONESHOT) < 0)
return -EIO;
return 0;
}
int mainloop_remove_timeout(int id)
{
return mainloop_remove_fd(id);
}
/**
* set mainloop signal handler (signal_data) usally SIGINT and SIGTERM handler
* signal_data is a global variable
*
* @param mask events to filter
* @param callback function call triggered by filtered events prototype callback(int signum, void *user_data)
* @param user_data user data to pass to callback
* @param destroy user data storage management
* @return 0 success else <0 error
*/
int mainloop_set_signal(sigset_t *mask, mainloop_signal_func callback,
void *user_data, mainloop_destroy_func destroy)
{
struct signal_data *data;
if (!mask || !callback)
return -EINVAL;
data = malloc(sizeof(*data));
if (!data)
return -ENOMEM;
memset(data, 0, sizeof(*data));
data->callback = callback;
data->destroy = destroy;
data->user_data = user_data;
data->fd = -1;
memcpy(&data->mask, mask, sizeof(sigset_t));
free(signal_data);
signal_data = data;
return 0;
}

51
mainloop.h Normal file
View File

@@ -0,0 +1,51 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2011-2014 Intel Corporation
* Copyright (C) 2002-2010 Marcel Holtmann <marcel@holtmann.org>
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#include <signal.h>
#include <sys/epoll.h>
typedef void (*mainloop_destroy_func) (void *user_data);
typedef void (*mainloop_event_func) (int fd, uint32_t events, void *user_data);
typedef void (*mainloop_timeout_func) (int id, void *user_data);
typedef void (*mainloop_signal_func) (int signum, void *user_data);
void mainloop_init(void);
void mainloop_quit(void);
void mainloop_exit_success(void);
void mainloop_exit_failure(void);
int mainloop_run(void);
int mainloop_add_fd(int fd, uint32_t events, mainloop_event_func callback,
void *user_data, mainloop_destroy_func destroy);
int mainloop_modify_fd(int fd, uint32_t events);
int mainloop_remove_fd(int fd);
int mainloop_add_timeout(unsigned int msec, mainloop_timeout_func callback,
void *user_data, mainloop_destroy_func destroy);
int mainloop_modify_timeout(int fd, unsigned int msec);
int mainloop_remove_timeout(int id);
int mainloop_set_signal(sigset_t *mask, mainloop_signal_func callback,
void *user_data, mainloop_destroy_func destroy);

21
makefile Normal file
View File

@@ -0,0 +1,21 @@
CC=gcc
CFLAGS=-I/usr/include/glib-2.0 -I/usr/lib/x86_64-linux-gnu/glib-2.0/include -Wall
LIBS=-lglib-2.0
CC_FRITZ=/usr/src/freetz-ng/toolchain/target/bin/i686-linux-uclibc-gcc-5.5.0
CFLAGS_FRITZ=-I/usr/src/freetz-ng/toolchain/target/i686-linux-uclibc/include/glib-2.0 -I/usr/src/freetz-ng/toolchain/target/i686-linux-uclibc/lib/glib-2.0/include -Wall --static -s
LIBS_FRITZ=-L/usr/src/freetz-ng/toolchain/target/i686-linux-uclibc/lib -lglib-2.0
APPNAME=solarlife
all:
$(CC) $(CFLAGS) *.c -o $(APPNAME) $(LIBS)
fritz:
$(CC_FRITZ) $(CFLAGS_FRITZ) *.c -o $(APPNAME) $(LIBS_FRITZ)
.PHONY: clean
clean:
rm -f $(APPNAME)

441
mqtt.c Normal file
View File

@@ -0,0 +1,441 @@
#include "mqtt.h"
#include <assert.h>
#include <stdint.h>
static int mqtt_encode_length(unsigned int nbytes, uint8_t* au8data)
{
int nbytes_encoded = 0;
if (nbytes < 0x10000000) /* max MQTT packet size */
{
unsigned int x = nbytes;
do
{
uint8_t u8digit = x % 128;
x /= 128; /* >> 7 */
if (x != 0)
u8digit |= 0x80;
au8data[nbytes_encoded++] = u8digit;
}
while (x > 0);
}
return nbytes_encoded;
}
static int mqtt_decode_length(uint8_t* au8data)
{
int nbytes = 0;
int multiplier = 1;
unsigned int array_idx = 0;
uint8_t u8digit;
do
{
u8digit = au8data[array_idx++];
nbytes += (u8digit & 127) * multiplier;
multiplier *= 128;
}
while ( (++array_idx < 4)
&& ((u8digit & 128) != 0));
return nbytes;
}
static int mqtt_encode_msg(uint8_t* pu8dst, uint8_t u8ctrl_type, uint8_t u8flgs, uint8_t** apu8data_in, uint32_t* au32input_len, uint32_t u32nargs, uint32_t u32input_len)
{
int nbytes_encoded = 0;
if ( (pu8dst != 0)
&& (u8ctrl_type > 0) /* cmd type = [1:14] .. 0 + 15 are reserved */
&& (u8ctrl_type < 15)
&& (u8flgs < 16) /* flag are 4 bits wide, [0:15] */
&& ( (u32nargs == 0) /* Allow for msg's without payload */
|| ( (apu8data_in != 0) /* ... otherwise, check the args */
&& (au32input_len != 0))))
{
pu8dst[0] = (u8ctrl_type << 4) | (u8flgs);
int idx = mqtt_encode_length(u32input_len, &pu8dst[1]);
uint32_t i, j;
for (i = 0; i < u32nargs; ++i)
{
for (j = 0; j < au32input_len[i]; ++j, ++nbytes_encoded)
{
pu8dst[idx + nbytes_encoded + 1] = apu8data_in[i][j];
}
}
nbytes_encoded += (1 + idx);
}
return nbytes_encoded;
}
int mqtt_decode_msg(uint8_t* pu8src, uint8_t* pu8ctrl_type, uint8_t* pu8flgs, uint8_t* pu8data_out, uint32_t* pu32output_len)
{
int nbytes_decoded = 0;
if ( (pu8src != 0)
&& (pu8ctrl_type != 0)
&& (pu8flgs != 0)
&& (pu8data_out != 0)
&& (pu32output_len != 0))
{
*pu8ctrl_type = ((*pu8src) >> 4);
*pu8flgs = ((*pu8src) & 0x0F);
int idx = 1;
int nbytes = mqtt_decode_length(&pu8src[idx]);
//printf("nbytes = %d\n", nbytes);
if (nbytes < 0x80) /* 0 <= nbytes < 128 */
{
idx += 1;
}
else if (nbytes < 0x4000) /* 128 <= nbytes < 16384 */
{
idx += 2;
}
else if (nbytes < 0x200000) /* 16384 <= nbytes < 2097152 */
{
idx += 3;
}
else
{
idx += 4;
assert(nbytes < 0x10000000);
}
/* Copy decoded data to output-buffer */
int i;
for (i = 0; i < nbytes; ++i)
{
pu8data_out[i] = pu8src[idx + i];
}
//memcpy(pu8data_out, &pu8src[idx], nbytes);
*pu32output_len = nbytes;
nbytes_decoded = nbytes + idx;
}
return nbytes_decoded;
}
/* Advanced connect: More options available */
int mqtt_encode_connect_msg2(uint8_t* pu8dst, uint8_t u8conn_flgs, uint16_t u16keepalive, uint8_t* pu8clientid, uint16_t u16clientid_len)
{
int nbytes_encoded = 0;
if ( (pu8dst != 0)
&& (pu8clientid != 0))
{
const uint32_t u32hdr_len = 12;
uint8_t u8keepalive_msb = (u16keepalive & 0xFF00) >> 8; /* Bug if on Big-Endian machine */
uint8_t u8keepalive_lsb = (u16keepalive & 0x00FF);
uint8_t u8clientid_len_msb = (u16clientid_len & 0xFF00) >> 8; /* Bug if on Big-Endian machine */
uint8_t u8clientid_len_lsb = (u16clientid_len & 0x00FF);
uint8_t au8conn_buf[u32hdr_len];
/* Variable part of header for CONNECT msg */
int idx = 0;
au8conn_buf[idx++] = 0x00; /* protocol version length U16 */
au8conn_buf[idx++] = 0x04; /* 4 bytes long (MQTT) */
au8conn_buf[idx++] = 'M';
au8conn_buf[idx++] = 'Q';
au8conn_buf[idx++] = 'T';
au8conn_buf[idx++] = 'T';
au8conn_buf[idx++] = 0x04; /* 4 == MQTT version 3.1.1 */
au8conn_buf[idx++] = u8conn_flgs;
au8conn_buf[idx++] = u8keepalive_msb;
au8conn_buf[idx++] = u8keepalive_lsb;
au8conn_buf[idx++] = u8clientid_len_msb;
au8conn_buf[idx++] = u8clientid_len_lsb;
uint8_t* buffers[] = { au8conn_buf, pu8clientid };
uint32_t sizes[] = { u32hdr_len, u16clientid_len };
nbytes_encoded = mqtt_encode_msg(pu8dst, CTRL_CONNECT, 0, buffers, sizes, 2, u32hdr_len + u16clientid_len);
}
return nbytes_encoded;
}
/* Simple connect: No username/password, no QoS etc. */
int mqtt_encode_connect_msg(uint8_t* pu8dst, uint8_t* pu8clientid, uint16_t u16clientid_len) /* u8conn_flgs = 2, u16keepalive = 60 */
{
return mqtt_encode_connect_msg2(pu8dst, 0x02, 60, pu8clientid, u16clientid_len);
}
int mqtt_encode_disconnect_msg(uint8_t* pu8dst)
{
return mqtt_encode_msg(pu8dst, CTRL_DISCONNECT, 0x02, 0, 0, 0, 0);
}
int mqtt_encode_ping_msg(uint8_t* pu8dst)
{
return mqtt_encode_msg(pu8dst, CTRL_PINGREQ, 0x02, 0, 0, 0, 0);
}
int mqtt_encode_publish_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id, uint8_t* pu8payload, uint32_t u32data_len)
{
int nbytes_encoded = 0;
if (pu8topic != 0)
{
uint32_t u32msg_len = sizeof(uint16_t) + u16topic_len + sizeof(uint16_t) + u32data_len;
uint8_t u8topic_len_msb = (u16topic_len & 0xFF00) >> 8; /* Bug if on Big-Endian machine */
uint8_t u8topic_len_lsb = (u16topic_len & 0x00FF);
uint8_t u8msg_id_msb = (u16msg_id & 0xFF00) >> 8; /* Bug if on Big-Endian machine */
uint8_t u8msg_id_lsb = (u16msg_id & 0x00FF);
uint8_t au8topic_len_buf[sizeof(uint16_t)] = { u8topic_len_msb, u8topic_len_lsb };
uint8_t au8msg_id_buf[sizeof(uint16_t)] = { u8msg_id_msb, u8msg_id_lsb };
uint8_t* buffers[] = { au8topic_len_buf, pu8topic, au8msg_id_buf, pu8payload };
uint32_t sizes[] = { sizeof(uint16_t), u16topic_len, sizeof(uint16_t), u32data_len };
nbytes_encoded = mqtt_encode_msg(pu8dst, CTRL_PUBLISH, u8qos << 1, buffers, sizes, 4, u32msg_len);
}
return nbytes_encoded;
}
static int encode_pubsub_msg2(uint8_t* pu8dst, uint8_t u8ctrl, uint8_t** apu8topic, uint16_t* au16topic_len, uint8_t* au8qos, uint32_t u32nargs, uint16_t u16msg_id)
{
int nbytes_encoded = 0;
if ( (apu8topic != 0)
&& (u32nargs < MSG_SUB_MAXNTOPICS))
{
uint8_t topicsizes[sizeof(uint16_t)][MSG_SUB_MAXNTOPICS];
uint32_t sizes[1 + (3 * MSG_SUB_MAXNTOPICS)]; /* for each topic a topic-len, the topic itself, and the qos -- hence 3 * max(subs_pr_msg) */
uint8_t* buffers[1 + (3 * MSG_SUB_MAXNTOPICS)]; /* msgid, [topic-len + topic + qos]+ */
uint8_t u8msg_id_msb = (u16msg_id & 0xFF00) >> 8; /* Bug if on Big-Endian machine */
uint8_t u8msg_id_lsb = (u16msg_id & 0x00FF);
uint8_t au8msg_id_buf[sizeof(uint16_t)] = { u8msg_id_msb, u8msg_id_lsb };
int bufidx = 0;
buffers[bufidx] = au8msg_id_buf;
sizes[bufidx++] = sizeof(uint16_t);
uint32_t u32msg_len = sizeof(uint16_t); /* msgid */
uint8_t u8highest_qos = 0;
uint32_t i;
for (i = 0; i < u32nargs; ++i)
{
u32msg_len += sizeof(uint16_t) + au16topic_len[i] + sizeof(uint8_t); /* topic-len + topic + qos */
uint8_t u8topic_len_msb = (au16topic_len[i] & 0xFF00) >> 8; /* Bug if on Big-Endian machine */
uint8_t u8topic_len_lsb = (au16topic_len[i] & 0x00FF);
topicsizes[i][0] = u8topic_len_msb;
topicsizes[i][1] = u8topic_len_lsb;
buffers[bufidx] = topicsizes[i];
sizes[bufidx++] = sizeof(uint16_t);
buffers[bufidx] = apu8topic[i];
sizes[bufidx++] = au16topic_len[i];
buffers[bufidx] = &au8qos[i];
sizes[bufidx++] = sizeof(uint8_t);
/* Send MQTT SUBSCRIBE msg with highest QoS of all subscribtions */
if (au8qos[i] > u8highest_qos)
{
u8highest_qos = au8qos[i];
}
}
nbytes_encoded = mqtt_encode_msg(pu8dst, u8ctrl, u8highest_qos << 1, buffers, sizes, bufidx, u32msg_len);
}
return nbytes_encoded;
}
int mqtt_encode_subscribe_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id)
{
return encode_pubsub_msg2(pu8dst, CTRL_SUBSCRIBE, &pu8topic, &u16topic_len, &u8qos, 1, u16msg_id);
}
int mqtt_encode_unsubscribe_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id)
{
return encode_pubsub_msg2(pu8dst, CTRL_UNSUBSCRIBE, &pu8topic, &u16topic_len, &u8qos, 1, u16msg_id);
}
int mqtt_encode_subscribe_msg2(uint8_t* pu8dst, uint8_t** apu8topic, uint16_t* au16topic_len, uint8_t* au8qos, uint32_t u32nargs, uint16_t u16msg_id)
{
return encode_pubsub_msg2(pu8dst, CTRL_SUBSCRIBE, apu8topic, au16topic_len, au8qos, u32nargs, u16msg_id);
}
int mqtt_encode_unsubscribe_msg2(uint8_t* pu8dst, uint8_t** apu8topic, uint16_t* au16topic_len, uint8_t* au8qos, uint32_t u32nargs, uint16_t u16msg_id)
{
return encode_pubsub_msg2(pu8dst, CTRL_UNSUBSCRIBE, apu8topic, au16topic_len, au8qos, u32nargs, u16msg_id);
}
int mqtt_decode_connack_msg(uint8_t* pu8src, uint32_t u32nbytes)
{
return ( (pu8src != 0)
&& (u32nbytes >= 4)
&& (pu8src[0] == 0x20) /* 0x20 : CTRL_CONNACK << 4 */
&& (pu8src[1] == 0x02) /* 0x02 : bytes after fixed header */
// && (pu8src[3] == 0x00) /* 0x00 : 3rd byte is reserved */
&& (pu8src[3] == 0x00)); /* 0x00 : Connection Accepted */
}
int mqtt_decode_pingresp_msg(uint8_t* pu8src, uint32_t u32nbytes)
{
return ( (pu8src != 0)
&& (u32nbytes >= 2)
&& (pu8src[0] == 0xd0) /* 0xD0 : CTRL_PINGRESP << 4 */
&& (pu8src[1] == 0x00)); /* 0x00 : bytes after fixed header */
}
int mqtt_decode_puback_msg(uint8_t* pu8src, uint32_t u32nbytes, uint16_t* pu16msg_id_out)
{
int success = 0;
if ( (pu8src != 0)
&& (u32nbytes >= 4)
&& (pu8src[0] == 0x40) /* 0x40 : CTRL_PUBACK << 4 */
&& (pu8src[1] == 0x02) /* 0x02 : bytes after fixed header */
&& (pu16msg_id_out != 0))
{
*pu16msg_id_out = (pu8src[2] << 8) | pu8src[3];
success = 1;
}
return success;
}
int mqtt_decode_suback_msg(uint8_t* pu8src, uint32_t u32nbytes, uint16_t* pu16msg_id_out)
{
int success = 0;
if ( (pu8src != 0)
&& (u32nbytes >= 4)
&& (pu8src[0] == 0x90) /* 0x90 : CTRL_SUBACK << 4 */
&& (pu8src[1] == 0x02) /* 0x02 : bytes after fixed header */
&& (pu16msg_id_out != 0))
{
*pu16msg_id_out = (pu8src[2] << 8) | pu8src[3];
success = 1;
}
return success;
}
int mqtt_decode_publish_msg(uint8_t* pu8src, uint32_t u32nbytes, uint8_t* pu8qos, uint16_t* pu16msg_id_out, uint16_t* pu16topic_len, uint8_t** ppu8topic, uint8_t** ppu8payload)
{
int success = 0;
if ( (pu8src != 0)
&& (u32nbytes >= 6)
&& (pu8src[0] >> 4 == CTRL_PUBLISH)
&& (pu16msg_id_out != 0)
&& (pu16topic_len != 0)
&& (ppu8topic != 0)
&& (ppu8payload != 0) )
{
*pu8qos = (pu8src[0] >> 1) & 3;
uint16_t u16topic_len = (pu8src[2] << 8) | pu8src[3];
*pu16topic_len = u16topic_len;
*ppu8topic = &pu8src[4];
*pu16msg_id_out = (pu8src[4 + u16topic_len] << 8) | pu8src[5 + u16topic_len];
*ppu8payload = &pu8src[6 + u16topic_len];
success = 1;
}
return success;
}
#if defined(TEST) && (TEST == 1)
int main(void)
{
// int mqtt_encode_connect_msg2(uint8_t* pu8dst, uint8_t u8conn_flgs, uint16_t u16keepalive, uint8_t* pu8clientid, uint16_t u16clientid_len)
uint8_t buf[128];
int nbytes;
int i;
nbytes = mqtt_encode_connect_msg2(buf, 0x02, 60, (uint8_t*)"DIGI", 4);
printf("con: ");
for (i = 0; i < nbytes; ++i)
printf("0x%.02x ", buf[i]);
printf("\n");
nbytes = mqtt_encode_connect_msg(buf, (uint8_t*)"DIGI", 4);
printf("con: ");
for (i = 0; i < nbytes; ++i)
printf("0x%.02x ", buf[i]);
printf("\n");
nbytes = mqtt_encode_disconnect_msg(buf);
for (i = 0; i < nbytes; ++i)
printf("0x%.02x ", buf[i]);
printf("\n");
nbytes = mqtt_encode_ping_msg(buf);
for (i = 0; i < nbytes; ++i)
printf("0x%.02x ", buf[i]);
printf("\n");
nbytes = mqtt_encode_publish_msg(buf, (uint8_t*)"a/b", 3, 1, 32767, (uint8_t*)"payload", 8);
//nbytes = mqtt_encode_publish_msg(buf, (uint8_t*)"a/b", 3, 1, 10, 0, 0);
printf("pub: ");
for (i = 0; i < nbytes; ++i)
{
if (buf[i] >= 'A' && buf[i] <= 'z')
printf("%c ", buf[i]);
else
printf("0x%.02x ", buf[i]);
}
printf("\n");
{
uint16_t msg_id, topic_len;
uint8_t qos;
uint8_t* topic;
uint8_t* payload;
int rc = mqtt_decode_publish_msg(buf, nbytes, &qos, &msg_id, &topic_len, &topic, &payload);
printf("nbytes = %u\n", nbytes);
printf("decode pub msg = %d:\n", rc);
printf(" qos = %u\n", qos);
printf(" msg id = %u\n", msg_id);
printf(" topic-len = %u\n", topic_len);
printf(" topic = '%s'\n", topic);
printf(" payload = '%s'\n", payload);
}
nbytes = mqtt_encode_subscribe_msg(buf, (uint8_t*)"a/b", 3, 1, 32767);
printf("sub: ");
for (i = 0; i < nbytes; ++i)
printf("0x%.02x ", buf[i]);
printf("\n");
uint8_t* tpcs[] = { (uint8_t*)"a/b" };
uint16_t tpclens[] = { 3 };
uint8_t tpcqos[] = { 1 };
nbytes = mqtt_encode_subscribe_msg2(buf, tpcs, tpclens, tpcqos, 1, 32767);
printf("sub: ");
for (i = 0; i < nbytes; ++i)
printf("0x%.02x ", buf[i]);
printf("\n");
nbytes = mqtt_encode_unsubscribe_msg(buf, (uint8_t*)"a/b", 3, 1, 32767);
printf("unsub: ");
for (i = 0; i < nbytes; ++i)
printf("0x%.02x ", buf[i]);
printf("\n");
nbytes = mqtt_encode_unsubscribe_msg2(buf, tpcs, tpclens, tpcqos, 1, 32767);
printf("unsub: ");
for (i = 0; i < nbytes; ++i)
printf("0x%.02x ", buf[i]);
printf("\n");
uint8_t au8connack_msg[] = { 0x20, 0x02, 0x00, 0x00 };
printf("connack(msg) = %d \n", mqtt_decode_connack_msg(au8connack_msg, sizeof(au8connack_msg)));
uint8_t au8pingresp_msg[] = { 0xd0, 0x00 };
printf("pingresp(msg) = %d \n", mqtt_decode_pingresp_msg(au8pingresp_msg, sizeof(au8pingresp_msg)));
uint16_t u16msg_id;
uint8_t au8puback_msg[] = { 0x40, 0x02, 0x7f, 0xff };
printf("puback(msg) = %d \n", mqtt_decode_puback_msg(au8puback_msg, sizeof(au8puback_msg), &u16msg_id));
return 0;
}
#endif

55
mqtt.h Normal file
View File

@@ -0,0 +1,55 @@
#include <stdint.h>
/* Max number of topics one can subscribe to in a single SUBSCRIBE message */
#define MSG_SUB_MAXNTOPICS 8
/* Control Command Types */
enum
{
CTRL_RESERVED, /* 0 */
CTRL_CONNECT, /* 1 */
CTRL_CONNACK, /* 2 */
CTRL_PUBLISH, /* 3 */
CTRL_PUBACK, /* 4 */
CTRL_PUBREC, /* 5 */
CTRL_PUBREL, /* 6 */
CTRL_PUBCOMP, /* 7 */
CTRL_SUBSCRIBE, /* 8 */
CTRL_SUBACK, /* 9 */
CTRL_UNSUBSCRIBE, /* 10 */
CTRL_UNSUBACK, /* 11 */
CTRL_PINGREQ, /* 12 */
CTRL_PINGRESP, /* 13 */
CTRL_DISCONNECT, /* 14 */
};
enum
{
QOS_AT_MOST_ONCE, /* 0 - "Fire and Forget" */
QOS_AT_LEAST_ONCE, /* 1 - ARQ until ACK */
QOS_EXACTLY_ONCE, /* 2 - Transactional delivery */
};
/* Simple connect: No username/password, no QoS etc. */
int mqtt_encode_connect_msg(uint8_t* pu8dst, uint8_t* pu8clientid, uint16_t u16clientid_len); /* u8conn_flgs = 2, u16keepalive = 60 */
/* Advanced connect: More options available */
int mqtt_encode_connect_msg2(uint8_t* pu8dst, uint8_t u8conn_flgs, uint16_t u16keepalive, uint8_t* pu8clientid, uint16_t u16clientid_len);
int mqtt_encode_disconnect_msg(uint8_t* pu8dst);
int mqtt_encode_ping_msg(uint8_t* pu8dst);
int mqtt_encode_publish_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id, uint8_t* pu8payload, uint32_t u32data_len);
int mqtt_encode_subscribe_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id);
int mqtt_encode_subscribe_msg2(uint8_t* pu8dst, uint8_t** apu8topic, uint16_t* au16topic_len, uint8_t* au8qos, uint32_t u32nargs, uint16_t u16msg_id);
int mqtt_encode_unsubscribe_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id);
int mqtt_encode_unsubscribe_msg2(uint8_t* pu8dst, uint8_t** apu8topic, uint16_t* au16topic_len, uint8_t* au8qos, uint32_t u32nargs, uint16_t u16msg_id);
int mqtt_decode_connack_msg(uint8_t* pu8src, uint32_t u32nbytes);
int mqtt_decode_pingresp_msg(uint8_t* pu8src, uint32_t u32nbytes);
int mqtt_decode_puback_msg(uint8_t* pu8src, uint32_t u32nbytes, uint16_t* pu16msg_id);
int mqtt_decode_suback_msg(uint8_t* pu8src, uint32_t u32nbytes, uint16_t* pu16msg_id_out);
int mqtt_decode_msg(uint8_t* pu8src, uint8_t* pu8ctrl_type, uint8_t* pu8flgs, uint8_t* pu8data_out, uint32_t* pu32output_len);
int mqtt_decode_publish_msg(uint8_t* pu8src, uint32_t u32nbytes, uint8_t* pu8qos, uint16_t* pu16msg_id_out, uint16_t* pu16topic_len, uint8_t** ppu8topic, uint8_t** ppu8payload);

575
queue.c Normal file
View File

@@ -0,0 +1,575 @@
/**
* @file queue.c
* @brief set of functions to manage queues
* @author Gilbert Brault
* @copyright Gilbert Brault 2015
* the original work comes from bluez v5.39
* value add: documenting main features
*
*/
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "util.h"
#include "queue.h"
struct queue {
int ref_count;
struct queue_entry *head;
struct queue_entry *tail;
unsigned int entries;
};
/**
* increment &queue->ref_count
*
* @param queue pointer to the queue structure
* @return
*/
static struct queue *queue_ref(struct queue *queue)
{
if (!queue)
return NULL;
__sync_fetch_and_add(&queue->ref_count, 1);
return queue;
}
/**
* decrement &queue->ref_count and free queue structure if ref_count == 0
*
* @param queue pointer to the queue structure
*/
static void queue_unref(struct queue *queue)
{
if (__sync_sub_and_fetch(&queue->ref_count, 1))
return;
free(queue);
}
/**
* create a new queue structure
*
* @return queue reference
*/
struct queue *queue_new(void)
{
struct queue *queue;
queue = new0(struct queue, 1);
if (!queue)
return NULL;
queue->head = NULL;
queue->tail = NULL;
queue->entries = 0;
return queue_ref(queue);
}
/**
* destroy all queue structure elements
*
* @param queue pointer to the queue structure
* @param destroy function making data deallocation
*/
void queue_destroy(struct queue *queue, queue_destroy_func_t destroy)
{
if (!queue)
return;
queue_remove_all(queue, NULL, NULL, destroy);
queue_unref(queue);
}
/**
* increment &entry->ref_count
*
* @param entry
* @return entry
*/
static struct queue_entry *queue_entry_ref(struct queue_entry *entry)
{
if (!entry)
return NULL;
__sync_fetch_and_add(&entry->ref_count, 1);
return entry;
}
/**
* decrement &entry->ref_count and free entry structure if ref_count == 0
*
* @param entry
*/
static void queue_entry_unref(struct queue_entry *entry)
{
if (__sync_sub_and_fetch(&entry->ref_count, 1))
return;
free(entry);
}
/**
* create a new queue, set queue->data to data, increment queue->ref_count
*
* @param data
* @return new queue pointer
*/
static struct queue_entry *queue_entry_new(void *data)
{
struct queue_entry *entry;
entry = new0(struct queue_entry, 1);
if (!entry)
return NULL;
entry->data = data;
return queue_entry_ref(entry);
}
/**
* push a queue entry allocated with data and set it at the tail of queue
*
* @param queue queue pointer where to allocate data
* @param data data to queue
* @return
*/
bool queue_push_tail(struct queue *queue, void *data)
{
struct queue_entry *entry;
if (!queue)
return false;
entry = queue_entry_new(data);
if (!entry)
return false;
if (queue->tail)
queue->tail->next = entry;
queue->tail = entry;
if (!queue->head)
queue->head = entry;
queue->entries++;
return true;
}
/**
* push a queue entry allocated with data and set it at the head of queue
*
* @param queue queue pointer where to allocate data
* @param data data to queue
* @return
*/
bool queue_push_head(struct queue *queue, void *data)
{
struct queue_entry *entry;
if (!queue)
return false;
entry = queue_entry_new(data);
if (!entry)
return false;
entry->next = queue->head;
queue->head = entry;
if (!queue->tail)
queue->tail = entry;
queue->entries++;
return true;
}
/**
* push a queue entry allocated with data and set it after the entry
*
* @param queue queue pointer where to allocate data
* @param entry element queue where to put the data after
* @param data data to queue
* @return
*/
bool queue_push_after(struct queue *queue, void *entry, void *data)
{
struct queue_entry *qentry, *tmp, *new_entry;
qentry = NULL;
if (!queue)
return false;
for (tmp = queue->head; tmp; tmp = tmp->next) {
if (tmp->data == entry) {
qentry = tmp;
break;
}
}
if (!qentry)
return false;
new_entry = queue_entry_new(data);
if (!new_entry)
return false;
new_entry->next = qentry->next;
if (!qentry->next)
queue->tail = new_entry;
qentry->next = new_entry;
queue->entries++;
return true;
}
/**
* pop data from the head of queue
*
* @param queue queue pointer
*/
void *queue_pop_head(struct queue *queue)
{
struct queue_entry *entry;
void *data;
if (!queue || !queue->head)
return NULL;
entry = queue->head;
if (!queue->head->next) {
queue->head = NULL;
queue->tail = NULL;
} else
queue->head = queue->head->next;
data = entry->data;
queue_entry_unref(entry);
queue->entries--;
return data;
}
/**
* peek data from the head of the queue
*
* @param queue queue pointer
*/
void *queue_peek_head(struct queue *queue)
{
if (!queue || !queue->head)
return NULL;
return queue->head->data;
}
/**
* peek data from the tail of the queue
*
* @param queue queue pointer
*/
void *queue_peek_tail(struct queue *queue)
{
if (!queue || !queue->tail)
return NULL;
return queue->tail->data;
}
/**
* iterator for the queue
*
* @param queue queue pointer
* @param function function(void *data, void *user_data) to call for each element
* @param user_data user pointer to pass to function
*/
void queue_foreach(struct queue *queue, queue_foreach_func_t function,
void *user_data)
{
struct queue_entry *entry;
if (!queue || !function)
return;
entry = queue->head;
if (!entry)
return;
queue_ref(queue);
while (entry && queue->head && queue->ref_count > 1) {
struct queue_entry *next;
queue_entry_ref(entry);
function(entry->data, user_data);
next = entry->next;
queue_entry_unref(entry);
entry = next;
}
queue_unref(queue);
}
/**
* compare a with b
*
* @param a first parameter
* @param b second parameter
*
* @return true if both element match
*/
static bool direct_match(const void *a, const void *b)
{
return a == b;
}
/**
*
*
* @param queue queue pointer
* @param function function call to make comparison
* @param match_data data to match with queue entry
*
* @return found entry and return pointer to user data (queued data) or NULL if not found
*/
void *queue_find(struct queue *queue, queue_match_func_t function,
const void *match_data)
{
struct queue_entry *entry;
if (!queue)
return NULL;
if (!function)
function = direct_match;
for (entry = queue->head; entry; entry = entry->next)
if (function(entry->data, match_data))
return entry->data;
return NULL;
}
/**
* remove queue element holding data
*
* @param queue queue pointer
* @param data data pointer
*
* @return false if not found, true if element removed
*/
bool queue_remove(struct queue *queue, void *data)
{
struct queue_entry *entry, *prev;
if (!queue)
return false;
for (entry = queue->head, prev = NULL; entry;
prev = entry, entry = entry->next) {
if (entry->data != data)
continue;
if (prev)
prev->next = entry->next;
else
queue->head = entry->next;
if (!entry->next)
queue->tail = prev;
queue_entry_unref(entry);
queue->entries--;
return true;
}
return false;
}
/**
* remove element match by function(void *data, void *match_data) where match_data = user_data
*
* @param queue queue pointer
* @param function function call to make comparison
* @param user_data data to match queue element in function
*
* @return NULL if not found or queue entry matching criteria
*/
void *queue_remove_if(struct queue *queue, queue_match_func_t function,
void *user_data)
{
struct queue_entry *entry, *prev = NULL;
if (!queue || !function)
return NULL;
entry = queue->head;
while (entry) {
if (function(entry->data, user_data)) {
void *data;
if (prev)
prev->next = entry->next;
else
queue->head = entry->next;
if (!entry->next)
queue->tail = prev;
data = entry->data;
queue_entry_unref(entry);
queue->entries--;
return data;
} else {
prev = entry;
entry = entry->next;
}
}
return NULL;
}
/**
* remove all queue element
*
* @param queue queue pointer
* @param function function used to match data deallocation
* @param user_data data pointer used in deallocation function
* @param destroy function for data deallocation
*
* @return count of element destroyed (0 if none)
*/
unsigned int queue_remove_all(struct queue *queue, queue_match_func_t function,
void *user_data, queue_destroy_func_t destroy)
{
struct queue_entry *entry;
unsigned int count = 0;
if (!queue)
return 0;
entry = queue->head;
if (function) {
while (entry) {
void *data;
unsigned int entries = queue->entries;
data = queue_remove_if(queue, function, user_data);
if (entries == queue->entries)
break;
if (destroy)
destroy(data);
count++;
}
} else {
queue->head = NULL;
queue->tail = NULL;
queue->entries = 0;
while (entry) {
struct queue_entry *tmp = entry;
entry = entry->next;
if (destroy)
destroy(tmp->data);
queue_entry_unref(tmp);
count++;
}
}
return count;
}
/**
* return queue head pointer
*
* @param queue queue pointer
*
* @return queue head
*/
const struct queue_entry *queue_get_entries(struct queue *queue)
{
if (!queue)
return NULL;
return queue->head;
}
/**
* return queue length
*
* @param queue queue pointer
*
* @return return 0 or queue elements count
*/
unsigned int queue_length(struct queue *queue)
{
if (!queue)
return 0;
return queue->entries;
}
/**
* test if queue is empty
*
* @param queue queue pointer
* @return
*/
bool queue_isempty(struct queue *queue)
{
if (!queue)
return true;
return queue->entries == 0;
}

65
queue.h Normal file
View File

@@ -0,0 +1,65 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#include <stdbool.h>
typedef void (*queue_destroy_func_t)(void *data);
struct queue;
struct queue_entry {
int ref_count;
void *data;
struct queue_entry *next;
};
struct queue *queue_new(void);
void queue_destroy(struct queue *queue, queue_destroy_func_t destroy);
bool queue_push_tail(struct queue *queue, void *data);
bool queue_push_head(struct queue *queue, void *data);
bool queue_push_after(struct queue *queue, void *entry, void *data);
void *queue_pop_head(struct queue *queue);
void *queue_peek_head(struct queue *queue);
void *queue_peek_tail(struct queue *queue);
typedef void (*queue_foreach_func_t)(void *data, void *user_data);
void queue_foreach(struct queue *queue, queue_foreach_func_t function,
void *user_data);
typedef bool (*queue_match_func_t)(const void *data, const void *match_data);
void *queue_find(struct queue *queue, queue_match_func_t function,
const void *match_data);
bool queue_remove(struct queue *queue, void *data);
void *queue_remove_if(struct queue *queue, queue_match_func_t function,
void *user_data);
unsigned int queue_remove_all(struct queue *queue, queue_match_func_t function,
void *user_data, queue_destroy_func_t destroy);
const struct queue_entry *queue_get_entries(struct queue *queue);
unsigned int queue_length(struct queue *queue);
bool queue_isempty(struct queue *queue);

86
timeout-glib.c Normal file
View File

@@ -0,0 +1,86 @@
/**
* @file timeout-glib.c
* @author Gilbert Brault
* @copyright Gilbert Brault 2015
* the original work comes from bluez v5.39
* value add: documenting main features
*
*/
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
*/
#include "timeout.h"
#include <glib.h>
struct timeout_data {
timeout_func_t func;
timeout_destroy_func_t destroy;
void *user_data;
};
static gboolean timeout_callback(gpointer user_data)
{
struct timeout_data *data = user_data;
if (data->func(data->user_data))
return TRUE;
return FALSE;
}
static void timeout_destroy(gpointer user_data)
{
struct timeout_data *data = user_data;
if (data->destroy)
data->destroy(data->user_data);
g_free(data);
}
unsigned int timeout_add(unsigned int timeout, timeout_func_t func,
void *user_data, timeout_destroy_func_t destroy)
{
struct timeout_data *data;
guint id;
data = g_try_new0(struct timeout_data, 1);
if (!data)
return 0;
data->func = func;
data->destroy = destroy;
data->user_data = user_data;
id = g_timeout_add_full(G_PRIORITY_DEFAULT, timeout, timeout_callback,
data, timeout_destroy);
if (!id)
g_free(data);
return id;
}
void timeout_remove(unsigned int id)
{
GSource *source = g_main_context_find_source_by_id(NULL, id);
if (source)
g_source_destroy(source);
}

27
timeout.h Normal file
View File

@@ -0,0 +1,27 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
*/
#include <stdbool.h>
typedef bool (*timeout_func_t)(void *user_data);
typedef void (*timeout_destroy_func_t)(void *user_data);
unsigned int timeout_add(unsigned int timeout, timeout_func_t func,
void *user_data, timeout_destroy_func_t destroy);
void timeout_remove(unsigned int id);

178
util.c Normal file
View File

@@ -0,0 +1,178 @@
/**
* @file util.c
* @brief set of utility functions
* @author Gilbert Brault
* @copyright Gilbert Brault 2015
* the original work comes from bluez v5.39
* value add: documenting main features
*
*/
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <stdio.h>
#include <ctype.h>
#include <stdarg.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <dirent.h>
#include <limits.h>
#include <string.h>
#include "util.h"
/**
* create a str debug message using format and then call function(str,user_data)
*
* @param function function to call
* @param user_data data for the "function"
* @param format format string template of str string
*/
void util_debug(util_debug_func_t function, void *user_data,
const char *format, ...)
{
char str[78];
va_list ap;
if (!function || !format)
return;
va_start(ap, format);
vsnprintf(str, sizeof(str), format, ap);
va_end(ap);
function(str, user_data);
}
/**
* hexadecimal dump utility: create the str hex string and then call function(str,user_data)
*
* @param dir first char of str
* @param buf buffer to convert to hex (str)
* @param len size of buffer (should be less than or equal to 16)
* @param function function to call with (str,user_data)
* @param user_data pointer to pass to function
*/
void util_hexdump(const char dir, const unsigned char *buf, size_t len,
util_debug_func_t function, void *user_data)
{
static const char hexdigits[] = "0123456789abcdef";
char str[68];
size_t i;
if (!function || !len)
return;
str[0] = dir;
for (i = 0; i < len; i++) {
str[((i % 16) * 3) + 1] = ' ';
str[((i % 16) * 3) + 2] = hexdigits[buf[i] >> 4];
str[((i % 16) * 3) + 3] = hexdigits[buf[i] & 0xf];
str[(i % 16) + 51] = isprint(buf[i]) ? buf[i] : '.';
if ((i + 1) % 16 == 0) {
str[49] = ' ';
str[50] = ' ';
str[67] = '\0';
function(str, user_data);
str[0] = ' ';
}
}
if (i % 16 > 0) {
size_t j;
for (j = (i % 16); j < 16; j++) {
str[(j * 3) + 1] = ' ';
str[(j * 3) + 2] = ' ';
str[(j * 3) + 3] = ' ';
str[j + 51] = ' ';
}
str[49] = ' ';
str[50] = ' ';
str[67] = '\0';
function(str, user_data);
}
}
/**
* Helper for getting the dirent type in case readdir
*
* @param parent prefix to build full path
* @param name suffix to builld full path
* @return DT_UNKNOWN or DT_DIR if parent/name is a DIR
*/
unsigned char util_get_dt(const char *parent, const char *name)
{
char filename[PATH_MAX];
struct stat st;
snprintf(filename, sizeof(filename), "%s/%s", parent, name);
if (lstat(filename, &st) == 0 && S_ISDIR(st.st_mode))
return DT_DIR;
return DT_UNKNOWN;
}
/**
* Helpers for bitfield operations
*
* Find unique id in range from 1 to max but no bigger then
* sizeof(int) * 8. ffs() is used since it is POSIX standard
*
* @param bitmap map of bits
* @param max max bit position
* @return
*/
uint8_t util_get_uid(unsigned int *bitmap, uint8_t max)
{
uint8_t id;
id = ffs(~*bitmap);
if (!id || id > max)
return 0;
*bitmap |= 1 << (id - 1);
return id;
}
/**
* Clear id bit in bitmap
*
* @param bitmap map of bits
* @param id bit rank to clear
*/
void util_clear_uid(unsigned int *bitmap, uint8_t id)
{
if (!id)
return;
*bitmap &= ~(1 << (id - 1));
}

158
util.h Normal file
View File

@@ -0,0 +1,158 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
*
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#include <stdint.h>
#include <stdlib.h>
#include <alloca.h>
#include <byteswap.h>
#include <string.h>
#if __BYTE_ORDER == __LITTLE_ENDIAN
#define le16_to_cpu(val) (val)
#define le32_to_cpu(val) (val)
#define le64_to_cpu(val) (val)
#define cpu_to_le16(val) (val)
#define cpu_to_le32(val) (val)
#define cpu_to_le64(val) (val)
#define be16_to_cpu(val) bswap_16(val)
#define be32_to_cpu(val) bswap_32(val)
#define be64_to_cpu(val) bswap_64(val)
#define cpu_to_be16(val) bswap_16(val)
#define cpu_to_be32(val) bswap_32(val)
#define cpu_to_be64(val) bswap_64(val)
#elif __BYTE_ORDER == __BIG_ENDIAN
#define le16_to_cpu(val) bswap_16(val)
#define le32_to_cpu(val) bswap_32(val)
#define le64_to_cpu(val) bswap_64(val)
#define cpu_to_le16(val) bswap_16(val)
#define cpu_to_le32(val) bswap_32(val)
#define cpu_to_le64(val) bswap_64(val)
#define be16_to_cpu(val) (val)
#define be32_to_cpu(val) (val)
#define be64_to_cpu(val) (val)
#define cpu_to_be16(val) (val)
#define cpu_to_be32(val) (val)
#define cpu_to_be64(val) (val)
#else
#error "Unknown byte order"
#endif
#define get_unaligned(ptr) \
__extension__ ({ \
struct __attribute__((packed)) { \
__typeof__(*(ptr)) __v; \
} *__p = (__typeof__(__p)) (ptr); \
__p->__v; \
})
#define put_unaligned(val, ptr) \
do { \
struct __attribute__((packed)) { \
__typeof__(*(ptr)) __v; \
} *__p = (__typeof__(__p)) (ptr); \
__p->__v = (val); \
} while (0)
#define PTR_TO_UINT(p) ((unsigned int) ((uintptr_t) (p)))
#define UINT_TO_PTR(u) ((void *) ((uintptr_t) (u)))
#define PTR_TO_INT(p) ((int) ((intptr_t) (p)))
#define INT_TO_PTR(u) ((void *) ((intptr_t) (u)))
#define new0(t, n) ((t*) calloc((n), sizeof(t)))
#define newa(t, n) ((t*) alloca(sizeof(t)*(n)))
#define malloc0(n) (calloc((n), 1))
typedef void (*util_debug_func_t)(const char *str, void *user_data);
void util_debug(util_debug_func_t function, void *user_data,
const char *format, ...)
__attribute__((format(printf, 3, 4)));
void util_hexdump(const char dir, const unsigned char *buf, size_t len,
util_debug_func_t function, void *user_data);
unsigned char util_get_dt(const char *parent, const char *name);
uint8_t util_get_uid(unsigned int *bitmap, uint8_t max);
void util_clear_uid(unsigned int *bitmap, uint8_t id);
static inline uint16_t get_le16(const void *ptr)
{
return le16_to_cpu(get_unaligned((const uint16_t *) ptr));
}
static inline uint16_t get_be16(const void *ptr)
{
return be16_to_cpu(get_unaligned((const uint16_t *) ptr));
}
static inline uint32_t get_le32(const void *ptr)
{
return le32_to_cpu(get_unaligned((const uint32_t *) ptr));
}
static inline uint32_t get_be32(const void *ptr)
{
return be32_to_cpu(get_unaligned((const uint32_t *) ptr));
}
static inline uint64_t get_le64(const void *ptr)
{
return le64_to_cpu(get_unaligned((const uint64_t *) ptr));
}
static inline uint64_t get_be64(const void *ptr)
{
return be64_to_cpu(get_unaligned((const uint64_t *) ptr));
}
static inline void put_le16(uint16_t val, void *dst)
{
put_unaligned(cpu_to_le16(val), (uint16_t *) dst);
}
static inline void put_be16(uint16_t val, const void *ptr)
{
put_unaligned(cpu_to_be16(val), (uint16_t *) ptr);
}
static inline void put_le32(uint32_t val, void *dst)
{
put_unaligned(cpu_to_le32(val), (uint32_t *) dst);
}
static inline void put_be32(uint32_t val, void *dst)
{
put_unaligned(cpu_to_be32(val), (uint32_t *) dst);
}
static inline void put_le64(uint64_t val, void *dst)
{
put_unaligned(cpu_to_le64(val), (uint64_t *) dst);
}
static inline void put_be64(uint64_t val, void *dst)
{
put_unaligned(cpu_to_be64(val), (uint64_t *) dst);
}

324
uuid.c Normal file
View File

@@ -0,0 +1,324 @@
/**
* @file uuid.c
* @brief uuid collection of functions
* @author Gilbert Brault
* @copyright Gilbert Brault 2015
* the original work comes from bluez v5.39
* value add: documenting main features
*
*/
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2011 Nokia Corporation
* Copyright (C) 2011 Marcel Holtmann <marcel@holtmann.org>
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include "bluetooth.h"
#include "uuid.h"
static uint128_t bluetooth_base_uuid = {
.data = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
0x80, 0x00, 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB }
};
#define BASE_UUID16_OFFSET 2
#define BASE_UUID32_OFFSET 0
static void bt_uuid16_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst)
{
uint16_t be16;
dst->value.u128 = bluetooth_base_uuid;
dst->type = BT_UUID128;
/*
* No matter the system: 128-bit UUIDs should be stored
* as big-endian. 16-bit UUIDs are stored on host order.
*/
be16 = htons(src->value.u16);
memcpy(&dst->value.u128.data[BASE_UUID16_OFFSET], &be16, sizeof(be16));
}
static void bt_uuid32_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst)
{
uint32_t be32;
dst->value.u128 = bluetooth_base_uuid;
dst->type = BT_UUID128;
/*
* No matter the system: 128-bit UUIDs should be stored
* as big-endian. 32-bit UUIDs are stored on host order.
*/
be32 = htonl(src->value.u32);
memcpy(&dst->value.u128.data[BASE_UUID32_OFFSET], &be32, sizeof(be32));
}
void bt_uuid_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst)
{
switch (src->type) {
case BT_UUID128:
*dst = *src;
break;
case BT_UUID32:
bt_uuid32_to_uuid128(src, dst);
break;
case BT_UUID16:
bt_uuid16_to_uuid128(src, dst);
break;
case BT_UUID_UNSPEC:
default:
break;
}
}
static int bt_uuid128_cmp(const bt_uuid_t *u1, const bt_uuid_t *u2)
{
return memcmp(&u1->value.u128, &u2->value.u128, sizeof(uint128_t));
}
int bt_uuid16_create(bt_uuid_t *btuuid, uint16_t value)
{
memset(btuuid, 0, sizeof(bt_uuid_t));
btuuid->type = BT_UUID16;
btuuid->value.u16 = value;
return 0;
}
int bt_uuid32_create(bt_uuid_t *btuuid, uint32_t value)
{
memset(btuuid, 0, sizeof(bt_uuid_t));
btuuid->type = BT_UUID32;
btuuid->value.u32 = value;
return 0;
}
int bt_uuid128_create(bt_uuid_t *btuuid, uint128_t value)
{
memset(btuuid, 0, sizeof(bt_uuid_t));
btuuid->type = BT_UUID128;
btuuid->value.u128 = value;
return 0;
}
int bt_uuid_cmp(const bt_uuid_t *uuid1, const bt_uuid_t *uuid2)
{
bt_uuid_t u1, u2;
bt_uuid_to_uuid128(uuid1, &u1);
bt_uuid_to_uuid128(uuid2, &u2);
return bt_uuid128_cmp(&u1, &u2);
}
/*
* convert the UUID to string, copying a maximum of n characters.
*/
int bt_uuid_to_string(const bt_uuid_t *uuid, char *str, size_t n)
{
if (!uuid) {
snprintf(str, n, "NULL");
return -EINVAL;
}
switch (uuid->type) {
case BT_UUID16:
snprintf(str, n, "%.4x", uuid->value.u16);
break;
case BT_UUID32:
snprintf(str, n, "%.8x", uuid->value.u32);
break;
case BT_UUID128: {
unsigned int data0;
unsigned short data1;
unsigned short data2;
unsigned short data3;
unsigned int data4;
unsigned short data5;
const uint8_t *data = (uint8_t *) &uuid->value.u128;
memcpy(&data0, &data[0], 4);
memcpy(&data1, &data[4], 2);
memcpy(&data2, &data[6], 2);
memcpy(&data3, &data[8], 2);
memcpy(&data4, &data[10], 4);
memcpy(&data5, &data[14], 2);
snprintf(str, n, "%.8x-%.4x-%.4x-%.4x-%.8x%.4x",
ntohl(data0), ntohs(data1),
ntohs(data2), ntohs(data3),
ntohl(data4), ntohs(data5));
}
break;
case BT_UUID_UNSPEC:
default:
snprintf(str, n, "Type of UUID (%x) unknown.", uuid->type);
return -EINVAL; /* Enum type of UUID not set */
}
return 0;
}
static inline int is_uuid128(const char *string)
{
return (strlen(string) == 36 &&
string[8] == '-' &&
string[13] == '-' &&
string[18] == '-' &&
string[23] == '-');
}
static inline int is_base_uuid128(const char *string)
{
uint16_t uuid;
char dummy;
if (!is_uuid128(string))
return 0;
return sscanf(string,
"0000%04hx-0000-1000-8000-00805%1[fF]9%1[bB]34%1[fF]%1[bB]",
&uuid, &dummy, &dummy, &dummy, &dummy) == 5;
}
static inline int is_uuid32(const char *string)
{
return (strlen(string) == 8 || strlen(string) == 10);
}
static inline int is_uuid16(const char *string)
{
return (strlen(string) == 4 || strlen(string) == 6);
}
static int bt_string_to_uuid16(bt_uuid_t *uuid, const char *string)
{
uint16_t u16;
char *endptr = NULL;
u16 = strtol(string, &endptr, 16);
if (endptr && (*endptr == '\0' || *endptr == '-')) {
bt_uuid16_create(uuid, u16);
return 0;
}
return -EINVAL;
}
static int bt_string_to_uuid32(bt_uuid_t *uuid, const char *string)
{
uint32_t u32;
char *endptr = NULL;
u32 = strtol(string, &endptr, 16);
if (endptr && *endptr == '\0') {
bt_uuid32_create(uuid, u32);
return 0;
}
return -EINVAL;
}
static int bt_string_to_uuid128(bt_uuid_t *uuid, const char *string)
{
uint32_t data0, data4;
uint16_t data1, data2, data3, data5;
uint128_t u128;
uint8_t *val = (uint8_t *) &u128;
if (sscanf(string, "%08x-%04hx-%04hx-%04hx-%08x%04hx",
&data0, &data1, &data2,
&data3, &data4, &data5) != 6)
return -EINVAL;
data0 = htonl(data0);
data1 = htons(data1);
data2 = htons(data2);
data3 = htons(data3);
data4 = htonl(data4);
data5 = htons(data5);
memcpy(&val[0], &data0, 4);
memcpy(&val[4], &data1, 2);
memcpy(&val[6], &data2, 2);
memcpy(&val[8], &data3, 2);
memcpy(&val[10], &data4, 4);
memcpy(&val[14], &data5, 2);
bt_uuid128_create(uuid, u128);
return 0;
}
int bt_string_to_uuid(bt_uuid_t *uuid, const char *string)
{
if (is_base_uuid128(string))
return bt_string_to_uuid16(uuid, string + 4);
else if (is_uuid128(string))
return bt_string_to_uuid128(uuid, string);
else if (is_uuid32(string))
return bt_string_to_uuid32(uuid, string);
else if (is_uuid16(string))
return bt_string_to_uuid16(uuid, string);
return -EINVAL;
}
int bt_uuid_strcmp(const void *a, const void *b)
{
return strcasecmp(a, b);
}
int bt_uuid_to_le(const bt_uuid_t *src, void *dst)
{
bt_uuid_t uuid;
switch (src->type) {
case BT_UUID16:
bt_put_le16(src->value.u16, dst);
return 0;
case BT_UUID32:
bt_uuid32_to_uuid128(src, &uuid);
src = &uuid;
/* Fallthrough */
case BT_UUID128:
/* Convert from 128-bit BE to LE */
bswap_128(&src->value.u128, dst);
return 0;
case BT_UUID_UNSPEC:
default:
return -EINVAL;
}
}

181
uuid.h Normal file
View File

@@ -0,0 +1,181 @@
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2011 Nokia Corporation
* Copyright (C) 2011 Marcel Holtmann <marcel@holtmann.org>
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifndef __BLUETOOTH_UUID_H
#define __BLUETOOTH_UUID_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#define GENERIC_AUDIO_UUID "00001203-0000-1000-8000-00805f9b34fb"
#define HSP_HS_UUID "00001108-0000-1000-8000-00805f9b34fb"
#define HSP_AG_UUID "00001112-0000-1000-8000-00805f9b34fb"
#define HFP_HS_UUID "0000111e-0000-1000-8000-00805f9b34fb"
#define HFP_AG_UUID "0000111f-0000-1000-8000-00805f9b34fb"
#define ADVANCED_AUDIO_UUID "0000110d-0000-1000-8000-00805f9b34fb"
#define A2DP_SOURCE_UUID "0000110a-0000-1000-8000-00805f9b34fb"
#define A2DP_SINK_UUID "0000110b-0000-1000-8000-00805f9b34fb"
#define AVRCP_REMOTE_UUID "0000110e-0000-1000-8000-00805f9b34fb"
#define AVRCP_TARGET_UUID "0000110c-0000-1000-8000-00805f9b34fb"
#define PANU_UUID "00001115-0000-1000-8000-00805f9b34fb"
#define NAP_UUID "00001116-0000-1000-8000-00805f9b34fb"
#define GN_UUID "00001117-0000-1000-8000-00805f9b34fb"
#define BNEP_SVC_UUID "0000000f-0000-1000-8000-00805f9b34fb"
#define PNPID_UUID "00002a50-0000-1000-8000-00805f9b34fb"
#define DEVICE_INFORMATION_UUID "0000180a-0000-1000-8000-00805f9b34fb"
#define GATT_UUID "00001801-0000-1000-8000-00805f9b34fb"
#define IMMEDIATE_ALERT_UUID "00001802-0000-1000-8000-00805f9b34fb"
#define LINK_LOSS_UUID "00001803-0000-1000-8000-00805f9b34fb"
#define TX_POWER_UUID "00001804-0000-1000-8000-00805f9b34fb"
#define BATTERY_UUID "0000180f-0000-1000-8000-00805f9b34fb"
#define SCAN_PARAMETERS_UUID "00001813-0000-1000-8000-00805f9b34fb"
#define SAP_UUID "0000112D-0000-1000-8000-00805f9b34fb"
#define HEART_RATE_UUID "0000180d-0000-1000-8000-00805f9b34fb"
#define HEART_RATE_MEASUREMENT_UUID "00002a37-0000-1000-8000-00805f9b34fb"
#define BODY_SENSOR_LOCATION_UUID "00002a38-0000-1000-8000-00805f9b34fb"
#define HEART_RATE_CONTROL_POINT_UUID "00002a39-0000-1000-8000-00805f9b34fb"
#define HEALTH_THERMOMETER_UUID "00001809-0000-1000-8000-00805f9b34fb"
#define TEMPERATURE_MEASUREMENT_UUID "00002a1c-0000-1000-8000-00805f9b34fb"
#define TEMPERATURE_TYPE_UUID "00002a1d-0000-1000-8000-00805f9b34fb"
#define INTERMEDIATE_TEMPERATURE_UUID "00002a1e-0000-1000-8000-00805f9b34fb"
#define MEASUREMENT_INTERVAL_UUID "00002a21-0000-1000-8000-00805f9b34fb"
#define CYCLING_SC_UUID "00001816-0000-1000-8000-00805f9b34fb"
#define CSC_MEASUREMENT_UUID "00002a5b-0000-1000-8000-00805f9b34fb"
#define CSC_FEATURE_UUID "00002a5c-0000-1000-8000-00805f9b34fb"
#define SENSOR_LOCATION_UUID "00002a5d-0000-1000-8000-00805f9b34fb"
#define SC_CONTROL_POINT_UUID "00002a55-0000-1000-8000-00805f9b34fb"
#define RFCOMM_UUID_STR "00000003-0000-1000-8000-00805f9b34fb"
#define HDP_UUID "00001400-0000-1000-8000-00805f9b34fb"
#define HDP_SOURCE_UUID "00001401-0000-1000-8000-00805f9b34fb"
#define HDP_SINK_UUID "00001402-0000-1000-8000-00805f9b34fb"
#define HID_UUID "00001124-0000-1000-8000-00805f9b34fb"
#define DUN_GW_UUID "00001103-0000-1000-8000-00805f9b34fb"
#define GAP_UUID "00001800-0000-1000-8000-00805f9b34fb"
#define PNP_UUID "00001200-0000-1000-8000-00805f9b34fb"
#define SPP_UUID "00001101-0000-1000-8000-00805f9b34fb"
#define OBEX_SYNC_UUID "00001104-0000-1000-8000-00805f9b34fb"
#define OBEX_OPP_UUID "00001105-0000-1000-8000-00805f9b34fb"
#define OBEX_FTP_UUID "00001106-0000-1000-8000-00805f9b34fb"
#define OBEX_PCE_UUID "0000112e-0000-1000-8000-00805f9b34fb"
#define OBEX_PSE_UUID "0000112f-0000-1000-8000-00805f9b34fb"
#define OBEX_PBAP_UUID "00001130-0000-1000-8000-00805f9b34fb"
#define OBEX_MAS_UUID "00001132-0000-1000-8000-00805f9b34fb"
#define OBEX_MNS_UUID "00001133-0000-1000-8000-00805f9b34fb"
#define OBEX_MAP_UUID "00001134-0000-1000-8000-00805f9b34fb"
/* GATT UUIDs section */
#define GATT_PRIM_SVC_UUID 0x2800
#define GATT_SND_SVC_UUID 0x2801
#define GATT_INCLUDE_UUID 0x2802
#define GATT_CHARAC_UUID 0x2803
/* GATT Characteristic Types */
#define GATT_CHARAC_DEVICE_NAME 0x2A00
#define GATT_CHARAC_APPEARANCE 0x2A01
#define GATT_CHARAC_PERIPHERAL_PRIV_FLAG 0x2A02
#define GATT_CHARAC_RECONNECTION_ADDRESS 0x2A03
#define GATT_CHARAC_PERIPHERAL_PREF_CONN 0x2A04
#define GATT_CHARAC_SERVICE_CHANGED 0x2A05
#define GATT_CHARAC_SYSTEM_ID 0x2A23
#define GATT_CHARAC_MODEL_NUMBER_STRING 0x2A24
#define GATT_CHARAC_SERIAL_NUMBER_STRING 0x2A25
#define GATT_CHARAC_FIRMWARE_REVISION_STRING 0x2A26
#define GATT_CHARAC_HARDWARE_REVISION_STRING 0x2A27
#define GATT_CHARAC_SOFTWARE_REVISION_STRING 0x2A28
#define GATT_CHARAC_MANUFACTURER_NAME_STRING 0x2A29
#define GATT_CHARAC_PNP_ID 0x2A50
/* GATT Characteristic Descriptors */
#define GATT_CHARAC_EXT_PROPER_UUID 0x2900
#define GATT_CHARAC_USER_DESC_UUID 0x2901
#define GATT_CLIENT_CHARAC_CFG_UUID 0x2902
#define GATT_SERVER_CHARAC_CFG_UUID 0x2903
#define GATT_CHARAC_FMT_UUID 0x2904
#define GATT_CHARAC_AGREG_FMT_UUID 0x2905
#define GATT_CHARAC_VALID_RANGE_UUID 0x2906
#define GATT_EXTERNAL_REPORT_REFERENCE 0x2907
#define GATT_REPORT_REFERENCE 0x2908
typedef struct {
enum {
BT_UUID_UNSPEC = 0,
BT_UUID16 = 16,
BT_UUID32 = 32,
BT_UUID128 = 128,
} type;
union {
uint16_t u16;
uint32_t u32;
uint128_t u128;
} value;
} bt_uuid_t;
int bt_uuid_strcmp(const void *a, const void *b);
int bt_uuid16_create(bt_uuid_t *btuuid, uint16_t value);
int bt_uuid32_create(bt_uuid_t *btuuid, uint32_t value);
int bt_uuid128_create(bt_uuid_t *btuuid, uint128_t value);
int bt_uuid_cmp(const bt_uuid_t *uuid1, const bt_uuid_t *uuid2);
void bt_uuid_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst);
#define MAX_LEN_UUID_STR 37
int bt_uuid_to_string(const bt_uuid_t *uuid, char *str, size_t n);
int bt_string_to_uuid(bt_uuid_t *uuid, const char *string);
int bt_uuid_to_le(const bt_uuid_t *uuid, void *dst);
static inline int bt_uuid_len(const bt_uuid_t *uuid)
{
return uuid->type / 8;
}
#ifdef __cplusplus
}
#endif
#endif /* __BLUETOOTH_UUID_H */